基于自适应步长RRT的双机器人协同路径规划

李 洋 徐 达 周 诚

(陆军装甲兵学院兵器与控制系， 北京 100072)

0 引言

RRT作为具有概率完备性的规划方法[1-4]广泛应用于机器人的路径规划。该方法省略了在C-space(构型空间)中建立障碍物模型的过程，只需在节点处进行碰撞检测，其运行速度快，对于高维空间的路径规划优势更加明显。目前，国内外学者针对RRT提出了多种改进方法，LIN等[5]提出了一种自适应步长RRT，该方法可根据障碍物自动调整随机树的生长方向，可快速生成避障路径。WANG等[6]提出了变步长RRT方法，在随机树的生长过程中，加入了贪婪思想，使随机树以固定步长增量尽可能生长，加快了RRT算法的收敛速度，但无法控制在W-space(工作空间)中的步长大小。文献[7-10]提出了自适应维度RRT，通过降低C-space的局部维度，提高算法的运行速度，但该方法没有考虑节点之间的碰撞问题，无法保证碰撞检测的有效性。刘成菊等[11]将势函数引入到RRT算法中，减少了RRT算法的随机性，在局部搜索过程中具有良好的避障效果。

目前，RRT及其改进方法的相关研究主要集中在移动机器人的路径规划，在双机器人的协同路径规划方面，RRT相关应用较少。对于高维空间的运动规划，RRT方法虽然省去了在C-space中建立障碍物模型的过程，但只在节点处进行碰撞检测难以保证其有效性[12-15]；利用RRT进行双机器人协同路径规划时，在每个节点处两个机器人末端执行器的相对位置必须保持固定[16-17]，而双机器人的随机树却在各自的C-space中随机生长，若要保持双机器人之间的协调，需在不同的C-space中对机器人随机树的生长进行协调控制。

本文提出一种自适应步长RRT方法，通过构建C-space和W-space中步长的范数不等式，把随机树每一次生长产生的位移约束在给定范围内，确保碰撞检测的有效性，通过被动生长算法控制两个机器人随机树的协调生长，完成双机器人在W-space中的协调运动，以实现双机器人协同路径规划。

1 双机器人运动学模型建立与求解

1.1 双机器人旋量模型

建立的双机器人模型如图1所示，双机器人系统由机器人R1、R2组成，其基座坐标系分别为S1和S2，取S1为惯性坐标系，S2在惯性坐标系下的描述为

1S2=Rz(π)Tx(lx)Ty(ly)S2

(1)

其中

图1 双机器人模型Fig.1 Models of dual-robot

通过1S2可以将机器人R1和R2的运动学统一在惯性坐标系S1中表达，故只需在机器人R1、R2各自的基座坐标系下建立运动学模型即可。建立机器人R1的运动旋量模型，如图2所示，R1的惯性坐标系S与基座坐标系重合，T为工具坐标系，关节1和5绕z轴方向旋转，其余关节绕y轴方向旋转，ri为各关节轴线所在位置，建立关节的运动旋量坐标

(2)

图2 机器人R1的运动旋量模型Fig.2 Twist model of robot R1

将关节的旋量坐标映射为指数形式

1.2 正向运动学

根据串联机器人的POE公式[18]，计算R1的正向运动学方程

(3)

其中r11=-c6(s1s5-c1c5c234-c1s6s234)

r21=-c6(c1s5-s1c5c234-s1s6s234)
r31=-s6c234-c5c6s234
r12=-c5s1-c1s5c234r22=c1c5-s1c5c234
r32=s5s234r13=c1c6s234-s6(s1s5-c1c5c234)
r23=c6s1s234-s6(c1s5+s1c5c234)
r33=c6c234-c5s6s234
p1=l5s1+l2c1s2+l3(c1c2c3+c1s2c3)+
l4(c1c2c3s4+c1c2s3c4+c1s2c3c4-c1s2s3s4)
p2=-l5c1+l2s1s2+l3(s1c2s3+s1s2c3)+
l4(s1c2c3s4+s1c2s3c4+s1s2c3c4-s1s2s3s4)

p3=l1+l2c2+l3c23+l4c234

式中ci=cosθi，si=sinθi，cij=cos(θi+θj)，sij=sin(θi+θj)，下文同。

为实现双机器人协同路径规划，须将机器人R1和R2的正向运动学统一在同一个坐标系下描述，由于机器人R1和R2具有相同的结构，所以机器人R2的正向运动学g2(θ)在S2中描述与g1(θ)相同，通过式(1)可将g2(θ)在惯性坐标系S下表述为

g2(θ)=1S2g1(θ)

(4)

1.3 逆向运动学

机器人R1和R2后3个关节的轴线不相交于一点，因此无法求解逆向运动学的解析解[19]。本文采用Newton-Raphson迭代法[20]，首先给出一组估计解θ0和误差Δ，通过多次迭代找到一组接近θ0且误差小于Δ的数值解θt，θ0的选取在下文中会进行详细的说明。

1.4 雅可比矩阵

雅可比矩阵定义了机器人单个关节速度对末端执行器速度的贡献度，将机器人C-space中的关节速度映射到W-space的矩阵函数，是构建C-space和W-space中步长关系的关键。机器人R1末端执行器速度与关节速度的映射关系为

V=J(θ)θ′

(5)

(6)

式中θ′——关节速度

J(θ)——雅可比矩阵

机器人R1的6个关节皆为转动副，因此有

(7)

2 传统RRT算法

采用固定步长RRT方法对机器人路径进行规划，需要在C-space中确定步长，对于多自由度的关节型机器人，C-space通常为6维，因此无法在C-space中进行碰撞检测，碰撞检测只能在W-space中的节点处进行，虽然在C-space中RRT的步长为固定的Δθ，但是通过正向运动学将Δθ映射到W-space中所产生的位移Δp却无法固定。

如图3所示，在C-space中随机树以固定步长Δθ从节点1生长到节点2所引起的位移Δp大于障碍物W-obs的尺寸，而在W-space中节点1和节点2并没有与障碍物发生碰撞，RRT算法会通过这次碰撞检测，但在机器人从节点1向节点2运动的过程中却发生了碰撞，由此可见，利用固定步长RRT方法对多自由度关节型机器人进行路径规划，无法确保生成一条避障路径。

图3 C-space和W-space中的步长关系Fig.3 Relationship of stepsize in C-space and W-space

3 自适应步长RRT

3.1 C-space和W-space的范数不等式构建

RRT方法以固定步长Δθ在C-space中进行随机树的生长无法控制在W-space中所产生的位移Δp，为了解决这一问题引入算子范数，构建C-space和W-space中步长的范数不等式，实现在C-space中控制W-space中Δp的目的。给定两个向量范数‖·‖a∈Rm，‖·‖b∈Rn和矩阵A∈Rm×n，从属于两个向量范数的矩阵算子范数为

(8)

可得到范数不等式

‖Ax‖a≤‖A‖a,b·‖x‖b

(9)

机器人R1的C-space中步长Δθ∈R6，W-space中所对应的位移Δp∈R3，因此只需构造从属于向量Δp和Δθ的矩阵算子范数，就可建立C-space和W-space的范数不等式，即构造一个矩阵A∈R3×6，满足Δp=AΔθ即可通过不等式约束Δp的最大值，达到在C-space中控制步长的目的。矩阵A的构造过程如下：

(10)

其中

根据雅可比矩阵式(10)可改写为

(11)

式(11)两边同乘以Δt可得

(12)

‖Δp‖w≤‖A‖w,c‖Δθ‖c

(13)

式中 ‖Δp‖w——W-space中的范数

‖Δθ‖c——C-space中的范数

3.2 C-space和W-space范数相容不等式改进

在式(13)中Δp特指双机器人末端执行器的位移，当在C-space中给定一个步长Δθ机器人发生位移的最大位置不一定为末端执行器，也有可能发生在关节处，所以仅对机器人末端执行器的位移进行约束无法保证碰撞检测的有效性，因此需改进范数不等式。

选用长方体包围盒对双机器人的连杆进行模型等效，设第i个连杆的顶点集合Li={pi1,pi2,…,pi8}，i=1,2,…,6，则产生最大位移的位置必然在集合Li中，所以有

‖Δpmax‖w=max‖Δpij‖w(Δpij∈Li)

(14)

将式(14)代入式(13)可得

max‖Δpij‖w≤max‖Aij‖w,c‖Δθ‖c

(15)

max‖Aij‖w,c∈R3×6

由此得到改进C-space和W-space范数不等式为

‖Δpij‖w≤max‖Aij‖w,c‖Δθ‖c

(16)

式中w、c的取值参考3.1节。

通过式(16)可以控制机器人任意位置由Δθ所引起的最大位移Δpij，完成双机器人的全局性避障。

3.3 随机树被动生长

为了保持机器人之间的协调，在W-space中，随机树每一个节点的机器人末端执行器相对位置必须保持不变，因此两个机器人在W-space中必须共享一颗随机树S_tree，但是在C-space中，机器人R1和R2的随机树却是各自生长，故RRT算法无法控制双机器人随机树的生长产生相同的S_tree。引入随机树被动生长方法，使机器人R2的随机树跟随机器人R1随机树的生长，在机器人R2的随机树生长的同时，完成对S_tree的复制，如图4所示，假设机器人R1的随机树为主动生长树A_tree，机器人R2的随机树为被动生长树P_tree，当A_tree在C-space中生长出一个新的节点A_node时，通过正向运动学将该节点映射到W-space中，完成共享随机树S_tree的生长并产生新的共享节点S_node，而P_tree新节点P_node的正向运动学须满足

g1(A_node)=1S2g2(P_node)=S_node

(17)

因此机器人R2随机树P_tree只能根据式(17)进行生长，通过机器人R2的逆向运动学反解出P_node，为了避免机器人R2的关节位移产生跳动，估计解θ0应取在P_tree中A_node的父节点Anear_node所对应的节点Pnear_node。此时P_node为P_tree的新节点，若P_node通过碰撞检测，则把P_node加入到机器人R2随机树P_tree中，完成一次双机器人随机树的生长。

图4 随机树被动生长Fig.4 Passive growing of random tree

3.4 双机器人自适应步长RRT算法

基于3.2节所建立的范数不等式和3.3节提出的随机树被动生长方法，建立双机器人自适应步长RRT算法，首先确定机器人在W-space中所允许的最大位移，令max‖Aij‖w,c‖Δθ‖c=Δpmax，代入式(16)可得

‖Δpijmax‖w≤max‖Aij‖w,c‖Δθ‖c=Δpmax

(18)

在构型空间中的步长为

(19)

式中‖Δpmax‖w为算法的初始设定值，随机树每一次生长都会更新‖Aij‖w,c，从而使步长Δθ自动满足式(19)，此时Δθ的取值即控制了机器人工作空间步长Δpijmax又可以保证随机树的生长速率。为了提高算法的搜索效率，采用双向搜索方法[21]，双机器人自适应步长RRT算法的伪代码为

F_WTree=pint,S_WTree=pgoal

Whilen

Tree_Add(pnew,F_WTree)

return Path_J(S_JTree, F_JTree)

return Path_W(S_WTree, F_WTree)

else

ExchangeTrees(S_JTree1, S_JTree2, F_JTree1,F_JTree2)

end if

end if

end while

Return Null

在C-space中分别定义以机器人R1、R2起始点和目标点为根的随机树F_JTree1、S_JTree1、F_JTree2、S_JTree2，在W-space中定义机器人R1、R2的共享树F_WTree、S_WTree，在对随机树F_JTree1进行生长时，引入范数不等式，通过Stepsize( )计算并且约束步长Δθ，Stepsize( )的伪代码为

算法实质上为式(19)的计算过程，其中涉及到w、c的取值，前文已有w=2，c=1，则Aij的算子范数定义如下

(20)

Aij的算子范数求解伪代码为

max=0

J=Jacc(θnew)

fori= 1 to 6

forj= 1 to 8

fork= 1 toi

temp=‖ωi×pij+νi‖2

if temp>max

max=temp

end if

end for

end for

end for

Return max

算法中的第2行是雅可比矩阵的求解，ωi和νi已经在前文中定义。

flag = true

pidx= GetpointOnF_WTree1(idx, F_WTree1)

d= GetDirection(pidx,pnew)

dist= GetDistance(pidx,pnew)

R= GetPose(pnew)

while flag = true

temp=pnew+s

if CollisionFreeAndCheckJointLimits

pnew=temp

continue

else

break

end if

else

flag = false

end if

end while

图5 随机树的融合Fig.5 Merging of random tree

算法首先找出随机树S_WTree1中距离pnew1最近的点pidx，并确定随机树的融合方向d，算法的9～12行是以自适应步长的方式，在pnew和pidx之间插入n个节点，而后检测这些节点能否通过关节限制检测和碰撞检测，若通过检测则随机树F_WTree、S_WTree融合，机器人R1和R2在工作空间中走出以pnew1、pidx为端点的直线轨迹；若其中有节点发生碰撞或者超出关节限制，则随机树F_WTree、S_WTree无法融合，继续进行随机树的生长。

当随机树F_WTree、S_WTree完成融合时，自适应步长RRT算法结束，得到双机器人的协同运动路径。

4 仿真

4.1 单次仿真

在Matlab中搭建双机器人模型，如图6所示，在双机器人的W-space中分别设置4个球形障碍物，半径分别为0.1、0.1、0.15、0.2 m，和一个长方体障碍物，尺寸为1.6 m×0.4 m×0.4 m，最小半径为0.1 m，故设置Δpmax为0.1 m，即机器人R1和R2在W-space中的步长不能超过0.1 m，否则碰撞检测将会失效，无法保证机器人有效避障，起点的坐标为(0.15，0.6，0)，终点的坐标为(-0.55，0.6，0.6)。在模型中分别利用自适应步长RRT算法和RRT算法进行路径搜索并进行对比。

图6 双机器人仿真环境Fig.6 Simulation environment of dual-robot

图7为自适应步长RRT算法和步长为0.5 rad的RRT算法进行一次路径搜索的结果，其中以起始点为根的随机树为F_WTree，以目标点为根的随机树为S_WTree，当两棵树达到融合条件时，随机树停止生长，经过圆滑处理产生一条距离最短的最终路径。由于RRT算法的步长为0.5 rad，因此在工作空间中产生的步长也相对较大，所用的迭代次数较少，虽然RRT算法成功地生成了一条路径，但从图7a可发现，RRT算法在工作空间中所产生的步长具有明显的跳动，在W-space中的步长最大值为0.155 7 m，大于Δpmax，无法保证碰撞检测的有效性。自适应步长RRT所产生的步长具有很强的稳定性，且工作空间中最大步长为0.023 6 m，小于Δpmax，实现了双机器人的避障路径规划。

进一步减小RRT算法的步长并与自适应步长RRT算法进行对比，其结果如图8所示，图8a为步长为0.3 rad的RRT的路径搜索结果，相对于步长为0.5 rad的RRT，步长为0.3 rad的RRT在W-space中产生的步长平均值大幅减小，但仍然存在较大的步长跳动，步长最大值为0.112 9 m，大于Δpmax，仍然无法保证碰撞检测的有效性，图8b为自适应步长RRT算法第2次的路径搜索结果，保持了在W-space中步长的稳定性，最大步长为0.032 7 m，小于Δpmax。

图8 步长为0.3 rad的RRT与自适应步长RRT路径规划对比Fig.8 Comparison of path planning by RRT with stepsize of 0.3 rad and self-adaptive stepsize RRT

将RRT算法的步长缩小到0.1 rad并再次与自适应步长RRT算法进行对比，结果如图9所示，在C-space中0.1 rad是一个非常小的步长，意味着机器人6个关节所产生的角度变化的总和为0.1 rad，因此随机树的每一次生长在W-space中所产生的步长会非常小，如图9a所示，步长最大值为0.001 9 m，虽然远小于Δpmax，但过小的步长使随机树的生长速度变慢，导致迭代次数剧增，是步长为0.3 rad的12倍。在相对严格的融合条件下，两棵随机树难以融合，在可接受的时间内无法搜索出一条避障路径。自适应步长RRT算法的第3次路径搜索仍然保持了步长稳定的特性，且迭代次数约为步长0.1 rad RRT算法的1/2，同时兼顾了随机树生长速度和步长大小的控制。

图9 步长为0.1 rad的RRT与自适应步长RRT路径规划对比Fig.9 Comparison of path planning by RRT with stepsize of 0.1 rad and self-adaptive stepsize RRT

通过对RRT算法步长的多次调整，确定0.2 rad为RRT算法在当前仿真环境下的步长匹配值，进行一次路径搜索，其结果如图10所示。

图10 步长为0.2 rad的RRT路径规划Fig.10 Path planning by RRT with stepsize of 0.2 rad

算法的最大步长为0.042 3 m，迭代次数为30次，基本与自适应步长RRT算法相近，步长仍有轻微跳动，最大步长已经控制在Δpmax的范围内，可以满足碰撞检测的要求。使用RRT算法进行路径搜索时，从上述的仿真过程可以发现，找到合适的步长通常需要多次调试才能够确定，而自适应步长RRT只需要指定Δpmax，就可自动调整C-space中的步长来控制W-space中产生步长，当机器人的工作环境发生变化时，只需根据环境模型指定新的Δpmax就可再次进行路径搜索，而固定步长RRT算法需再次对步长进行多次的调试。另外，传统的RRT算法存在步长过大所导致的碰撞检测失效问题，而自适应步长RRT算法，通过式(16)把机器人的关节、连杆和末端执行器的位移全部约束在Δpmax内，可得到机器人全局性的无碰撞路径。

4.2 多次仿真

分别对自适应步长RRT和步长0.5、0.3、0.1、0.2 rad的RRT进行50次仿真，其结果如表1所示。表中pmax为W-space中步长的最大值，当pmax大于Δpmax时，算法失效。从表1可以看出，RRT算法随着步长的减小，pmax的最大值和平均值也随之减小，但迭代次数会不断增加。当步长为0.5 rad时，RRT算法的步长较大，失效次数为37次，失效概率为74%，当进一步减小步长至0.3 rad时，虽然pmax的平均值降到可接受的范围内，但仍有14次失效，失效概率为28%。当步长为0.1 rad时，失效次数虽然为0，但过小的步长导致迭代次数的剧增，而自适应步长RRT既保证了算法的失效概率为0，同时又控制了算法的迭代次数。当取RRT的步长为0.2 rad时，可得到与自适应步长RRT相接近的最大步长与迭代次数。

表1 不同步长RRT与自适应步长RRT步长、迭代次数和失效次数Tab.1 Maximum stepsize, iterations and invalid times of RRT and self-adaptive stepsize RRT

5 结束语

针对RRT算法无法控制双机器人在W-space中步长的问题，提出了自适应步长RRT算法。通过构建C-space和W-space的范数不等式，把随机树每一次生长所产生的机器人位移约束在给定范围内，在利用自适应步长RRT进行路径规划时，只需根据机器人的工作环境指定Δpmax，省去了RRT算法为确定步长而进行多次调试的耗时过程；提出了随机树被动生长方法，实现了双机器人末端执行器同位置不同构型的路径规划，结合自适应步长RRT算法完成了双机器人的协同路径规划。