基于ROS的双机器人路径规划*

张 宁，张彩霞，高 萌，林 壮,，郭 静,3，余 伟,3

(1.佛山科学技术学院机电工程与自动化学院，佛山 528000；2.佛山智能装备技术研究院，佛山 528000；3.广东省智慧城市基础设施健康监测与评估工程技术研究中心，佛山 528000)

0 引言

近年来，随着机器人行业的不断发展，双机械臂因其能搬运大负载、灵巧、敏捷、适应更加复杂的工作环境的优势逐渐受到人们的关注。而路径规划即是双机械臂协同的基础，又是双机械臂运动控制的前提。所以路径规划问题被大量专家学者探索改进，并产生了许多优秀的算法。

基于采样的路径规划算法[1]在机械臂运动规划中应用最为广泛。此类型算法在工作空间任意采样，经过某种要求，将采样点通过局部规划器添加到已建立的搜索树中，并判断是否发生碰撞。若不产生碰撞则将采样点加入搜索范围中，然后通过搜索算法在工作空间中选择无碰撞路径。此类型的算法主要包括RRT[2]算法和PRM[3]算法。通过上述两种算法得到的路径是不确定的且不是最优的。为了改善采样法路径规划存在的路径非最优的缺点，RRT在原来的基础上衍生出了RRT*[4]、RRT-Connect[5]、RRG[6]和S-RRT[7]等改进算法。而PRM算法也衍生出一些性能良好的算法，如LD-PRM[8]、CPRM[9]和PRM*[10]等改进算法。

本文在上述文献的基础上，结合RRT算法和PRM算法的优点，提出一种混合的改进算法，并将改进后的算法应用到双机械臂运动规划中。最终经仿真验证和实际双机械臂搬运测试，本文所提方案效果更高。

1 结构与模型

在协同作业过程中，两台机械臂为各自独立的系统，且研究的双机械臂协同需要两台机械臂在同一空间坐标系下完成协调作业。双机械臂协同搬运模型如图1所示。其中将左机械臂看作主机械臂，右机械臂看作从机械臂。

图1 双机械臂协同搬运模型

1.1 约束关系建立

根据王美玲[11]所提出的3种操作模式，本文双臂协同搬运主要运用独立模式和全约束模式，而在独立模式中，首要任务是避免双机械臂之间，机械臂与障碍物之间出现碰撞，而全约束模式下两台机械臂需满足位姿，速度和加速度之间的约束关系。

下面给出双臂间的位姿约束关系：

(1)

(2)

所以：

(3)

故由式(3)可推导出双机械臂之间的位姿约束关系：

(4)

下面给出双机械臂之间的速度约束关系：

被搬运工件的运动速度与第m(m=L或R)个机械臂末端的运行速度满足如下关系：

(5)

当被搬运工件与双机械臂形成闭链时，它们之间无相对运动，因此满足下式：

(6)

结合式(6)、式(7)可以得到：

(7)

根据式(7)可以进一步推导出双臂间的速度与加速度约束关系：

(8)

在双臂自身的参考坐标系下，满足如下的微分运动关系：

(9)

结合式(8)、式(9)可以推导得到双臂的关节速度约束关系：

(10)

1.2 双机械臂基座标标定

为了得到精确的双机械臂基座标齐次变换关系，本文提出一种简单精确的标定方法。一般情况下，将左机械臂的坐标系和世界坐标系重合，故本文只需要进行两机械臂基座之间的标定。使用机械臂自带的示教器在双机械臂可达空间中标定一个坐标系相对世界坐标系方向相同且固定不变的工件。示教器进行工件标定后分别获得工件相对于双机械臂基座系的笛卡尔空间坐标，通过坐标转换获得双机械臂基座标位姿关系。具体步骤如下：

步骤1：固定工件位置，在标定期间保持位置不变。对机械臂分别安装标定针，具体如图2所示；

图2 标定针

步骤2：使用主机械臂的示教器进行工件标定，通过三点法获得标定结果并记录如下：

M1=(x1,y1,z1,R1,P1,Y1)

(11)

步骤3：同理按照上述步骤操作，从机械臂获得标定结果并记录如下：

M2=(x2,y2,z2,R2,P2,Y2)

(12)

步骤4：将(R1,P1,Y1)和(R2,P2,Y2)根据外旋方式，推导出旋转矩阵：

R=Rz(γ)*Ry(β)*Rx(α)

(13)

最终得到主从机械臂基座标相对于虚拟工件基座标的齐次变换矩阵：

(14)

(15)

由式(14)、式(15)最终得出双机械臂基坐标之间的齐次变换矩阵：

(16)

通常情况下在实际系统中很难避免，这也将导致机械臂轨迹存在偏差。

2 路径规划算法改进

2.1 传统RRT算法

RRT算法描述如下：随机树T的根节点为xinit，集合V存储树的节点xi，边集E存储节点与节点的连接，且每个节点存在于Xfree中，在运行该算法时，通过选取节点来确定树的扩散方向。当节点到达目标节点后，停止运行，返回起始点到目标点的树干路径连线。其伪代码如表1所示。

表1 RRT算法伪代码

2.2 改进RRT算法

经过对RRT算法的分析，本文对该算法进行改进，其主要思想：在RRT算法基础上加入偏置采样策略后，产生搜索树并将叶节点存入到程序数组中，获取规划的路径后融合PRM算法，将得到的路径节点代替PRM算法的采样点，以此来减小传统PRM算法随机采样的分散性，最后利用A*[12]算法得到最优路径。

2.2.1 目标偏向采样策略

RRT算法采用均匀采样的方式满足概率的完备性，但同时也增加了数据的冗余性，其结果将导致算法规划时间较长，不能快速到达目标节点qgoal。针对这一问题，本文在传统RRT算法的基础上加入了偏置采样，步骤为：先设定目标偏向阈值pgoal，在采样的过程中，由均匀分布得到一个概率值p。若p

2.2.2 融合PRM算法

PRM算法由离线学习和在线查询两部分组成。离线学习部分主要是在工作空间中采取一定量的坐标点。在线查询部分是在已知起始点和终点的前提下，由离线部分构建的地图，发现一条连接起始点到终点的无碰撞轨迹。

所以对目前所获得的路径进行处理，获取每个路径点坐标，此时将获取的坐标充当PRM算法中的随机采样点，然后基于所获得的轨迹点建立可行区域。算法伪代码如表2所示。

通过上述方式减少了采样点在工作空间中的分散性，提高了路径搜索的明确性，完成PRM算法中的离线学习阶段，然后再利用A*算法找到最佳的路径，使得改进后的算法能够减小规划时间并获得最优路径。

3 方案验证

为了验证所提方案效果更好，本文进行了实验仿真和物理测试。实验仿真主要有两部分组成，第一部分先是在MATLAB环境下，验证改进算法的性能，主要对传统RRT算法，RRT-Connect算法、PRM算法和改进后的算法在时间和路径上的表现进行对比，然后再ROS上模拟双机械臂独立操作模式下的运动规划。第二部分验证双机械臂基座标关系的精确度。经过实验仿真验证之后，对本文所提的方案在实际机器人上进行物理验证。

3.1 数值仿真

为了证明改进算法的的效率更高，首先在MATLAB上测试算法的可行性，然后在ROS上验证机械臂路径规划的效果。

3.1.1 MATLAB仿真验证

设置[10,10]为起始点坐标，[490,490]为目标点坐标，0.6为目标偏置概率，1000为迭代次数，20为扩散步长。经实验操作我们获得改进后的算法图像，具体如图3所示。

图3 二维平面改进前后的RRT算法对比图

表3 RRT算法改进前后对比

起点和目标点分别在图中的左上角和右下角，标有“-”的细曲线表示随机树，图3f中的红色线条为经改进后的最终规划路径。

为了客观验证改进算法的效果，重复进行50次路径规划。引入PRM算法后，相比传统RRT算法时间缩短了60.6%，路径缩短了20.3%相比于上一步虽然牺牲了一些时间，但仍远远优于传统RRT算法。

3.1.2 在ROS平台仿真

ROS操作系统作为机械臂的控制软件，它提供了操作系统应有的服务，包括硬件抽象，底层设备控制，常用函数的实现，进程间消息传递，以及功能包管理等服务。

具体的验证双机械臂路径规划算法的操作如下：首先从配置Movelt生成的config文件里启动Rviz。其次将改进的路径算法当作一个可执行函数模块，当执行该函数后获得一条无碰撞的轨迹点并存储在轨迹下发接口中。最后将Rviz和轨迹下发接口通信连接，轨迹下发接口以设定的频率将轨迹点发送给Rviz节点，Rviz中的机械臂在获得轨迹下发接口发送的轨迹点后立即做出响应。

设置主机械臂的目标点位置为(0.37,-0.41,1.36)、姿态四元数为(0.99,0,0,-0.11)，从机械臂的目标点位置为(0.77,-0.405,1.36)、姿态四元数为(0,0.99,0.11,0)。最终在ROS上双机械臂的运行路径如图4所示。

(a) RRT算法(b) 改进算法

由图4可知，机械臂之间的木板为障碍物，蓝色线段为机械臂末端运行轨迹，经实验对比，所改进的算法效果更好。

3.2 轨迹误差验证

通过双机械臂示教器在双机械臂可达空间按照上述第一部分的标定步骤进行工件标定，标定后双机械臂的数值分别为(0.65,0,0.37,0.18,0,0)和(0.65,0,0.43,0,0,0)，带入式(16)，最终得到双机械臂基座标的齐次矩阵。

(17)

由上述位姿关系可得，双机械臂基座标之间在x方向距离为1.28 m，y、z保持在同一平面上。

根据所求得的基座标齐次变换关系，更改ROS环境下双机械臂URDF模型下base_link的参数，调整双机械臂的仿真位姿和实际机器人位姿完全一致。

为了验证上述所求双机械臂基座标的正确性，在Rviz界面读取主机械臂末端安装的夹爪的位置为(0.4,0,1.35)，姿态为(0.08,0.7,-0.08,0.70)，根据式(3)求得从机械臂末端夹爪的位姿。并将求得的位姿和从机械臂示教器所显示的位姿数据进行对比，为了实验的客观性，不断变换双机械臂的位姿，经过20次实验，最终得出推导的从机械臂位姿和示教器显示的位姿在各方向的误差小于1 mm，故可证明所提双机械臂标定算法的正确性。

3.3 物理环境验证

本研究基于华数机器人HSR_CO605本体搭建系统平台，在主机械臂末端安装有六维力矩传感器Onrobot-hex，并对两台机械臂安装气动夹爪。ROS下发机械臂轨迹点周期设置为40 ms，具体实物系统如图5所示。

图5 物理环境

由图5所示，以3个刚体框架为障碍物，在Rviz环境下对障碍物三维建模。在独立操作模式下，对机械臂进行运动规划，设置主机械臂的抓取点位置为(0.55,-0.405,1.36)、姿态四元数为(0.99,0,0,-0.11)，从机械臂的抓取点位置为(0.65,-0.405,1.36)、姿态四元数为(0,0.99,0.11,0)。当通过改进算法对机械臂进行运动规划之后对双机械臂进行五次多项式轨迹优化，实时订阅/joint_state话题获得机械臂轨迹值。最终获得优化后轨迹如图6和图7所示。

图6 主机械臂位置、速度、加速度数据

在全约束模式下，对主机械臂进行笛卡尔空间运动规划，设置起始点为主机械臂的抓取点，实时采集关节轴位置、速度、加速度数据。然后设机械臂末端间距相差0.1 m，对所获得的主机械臂轨迹点代入式(3)推导出对应从机械臂的轨迹点。然后通过主从机械臂之间的笛卡尔空间中的位置误差来验证约束关系的正确性，设计误差评判公式为：

ess=XFollow-XMain-0.1

(18)

主从机械臂x轴、y轴、z轴方向误差大小如图8所示。

图8 轨迹误差

经过上述轨迹点的误差对比，x、y、z三个方向的误差均在±0.003 m内，这相对于机器人本体结构来说，误差可忽略不计，故可说明约束关系推理完全正确。

总之，由于各种校准误差和外部干扰，双臂协作系统中总是存在未知的轨迹偏差。这些轨迹偏差是非线性的，具有不确定性，易导致系统损坏和执行器过载。

4 结论

为了使双机械臂能够在复杂的环境中完成协同运动任务，提出了一种改进的路径规划算法。通过改进的双机械臂基座标标定算法，获得精确的双机械臂模型的前提下，对传统的RRT算法偏置采样，然后结合PRM算法对路径二次优化，最后经过五次多项式优化路径以获得最优轨迹。经过实验仿真，改进后的路径规划算法比传统的RRT算法在路径上缩短了20.3%，时间上减小了60.6%。最后通过ROS控制实际双机械臂，进一步验证了改进后的算法效率更高。