基于RRT搜索算法的六自由度机械臂避障路径规划*

1  引言
路径规划是机器人学要研究的重要课题,主要解决在障碍物环境下的机器人自主寻找一条从给定起点到终点的路径。传统的路径规划规划算法有栅格法,人工势场法,遗传算法,蚁群算法优化算法,启发式搜索法等。三维空间中,栅格法的栅格划分对路径规划影响很大,复杂度明显提高,以上方法需要建立模型在二维空间比较实用,对于三维空间的路径规划则难度加大,不适合解决多自由度机器人在三维环境中的路径规划。
基于快速扩展的随机树的路径规划算法,通过对状态空间中的采样点进行碰撞检测,避免了三维空间建模建立难度大的问题。该方法的优点是能够快速有效地搜索高维空间,在笛卡尔空间中,通过对未知空间的随机采样,使路径导向无障碍区域。从而能够有效地规划出一条从起始点到目标点的路径。
由于RRT算法采样的随机性,可能使得新生成的节点远离目标节点。因此,本文在确定随机节点时,通过选择基准向量,确保采用多点采样选取,确保事件的概率完备性。通过赌轮法决定朝着那一个随机节点的方向发展。本文采用通用型六自由度鉴定机械臂为研究对象,通过matlab可视化界面研究对象。
2  六自由度机械臂的运动学正反解

文中采用通用型六自由度机械臂机械结构如图1所示,通常情况下,为了方便进行运动学的求解,采用D-H法就可以确定第i-1坐标系和第i坐标系之间变换矩阵,模型如图 2所示,x0-y0-z0为基坐标系,xt-zt-yt为机械臂末端执行器坐标系。依据基坐标变换矩阵到各个关节的坐标关系,可以得到执行器的位姿矩阵

根据机械臂的物理参数以及图 2所示的坐标定义,可得到D-H参数如表1所示。依据(1)式和表1的参数,可以依次得到各个关节在基坐标下的位姿矩阵。运动学正解算法比较简单也比较成熟,本文不在阐述。

2.1  运动学反解
依据DH原理,依次右乘第i个关节角的坐标系变换矩阵,依次类推,可得到末端执行器坐标系在世界坐标系中的坐标变换矩阵为由根据图 2和表1,机械臂末端执行器的初始位置姿态为其中已知,从而可得到





根据式(5)和(6),可计算出剩余的三个角度值。

当机器人处于奇异位型,连杆5和连杆6成一直线。实际使用时,
在保持不变的前提下,令朝相反方向转动相同大小的角度,从而避免奇异位置。

3  基于RRT的避障路径搜索
3.1  长方体包络碰撞检测法
为了解决机械臂整体与障碍之间的碰撞问题,本文提出了长方体包络检测算法。机械臂为链式结构。为研究方便,我们将其骨架化,从而转化为空间中的直线段。通过控制搜索步长来补偿骨架化后机械臂模型带来的失真。障碍物一般具有不规则的几何形状,长方体包络更接近实际的障碍物模型,因此提高了精度,这样一来我们把问题转化为空间线段与空间四边形相交的问题,空间线面的位置关系有以下五种,交点在面内,交点在某条边上,线段在面内部,线段与面平行,线段与面相离。 如图3所示。

给定空间平面上的一点 P0 和 法向量N,满足N.(P-P0)=0的点P的集合平面的方程一般可以表示为
Ax+By+Cz+D=0                          (7)                                                                                                                                    
其中A,B,和C为法向量N的x,y,z分量,D=-N.P0。
设P(t)=Q+tV表示包含点Q并沿着V方向延伸的直线,对于法向量为N并且距原点带符号距离为D的平面,可以通过解下面的方程来确定直线与平面的交点,N.P(t)+D=0                                  (8)
用Q +tV代替P(t),可以得到
N.Q+(N.V)t+D=0                         (9)                    
解关于t的方程,可得

如果N.V=0,则说明线段与平面平行。则需要判断线段的点是否在平面上如果N.Q+D=0,则线段在平面上。如何判断直线与平面的交点就是线段与四边形的交点。必须满足以下两个条件 
(I) 交点必须在线段上
设空间线段的两个端点pt1(x1,y1,z1)和pt2(x2,y2,z2)交点 point(x,y,z),如果min(x1,x2) =< x <=max(x1,x2)&& min(y1,y2) =< y <=max(y1,y2)&& min(z1,z2) =<z<= max(z1,z2) ,那末交点必定在线段上。
(II) 交点必须在四边形内部
把四边形沿对角线分成两个三角形,假设点P位于三角形内,当沿着ABCA的方向在三条边上移动时,点P始终位于边AB,BC和CA的右侧。利用这一特点,当选定线段AB时,点C位于AB的右侧,同理选定BC时,点A位于BC的右侧,最后选定CA时,点B位于CA的右侧,当选择某一条边时,只需检测P与该边所对的点在同一侧即可。通过向量叉积来实现,连接PA,将PA和AB做叉积,再将CA和AB做叉积,如果两个叉积的结果的方向一致,则两个点在同一侧。
3.2  RRT算法规划路径

基本RRT算法描述:C是机械臂的可行笛卡尔空间,且可用空间中的点(x,y,z)表示机器人空间位置坐标。快速随机树的构建过程如图4所示。为一个拥有k个节点的机器人工作空间中随机树。为初始状态,为目标状态,即检定台的表位。为在C空间随机选取的状态点 。遍历随机树找到上离距离最近的叶子节点代表空间的两个位姿的几何距离。如果的连线上求为搜索步长,如果存在则随机树增加一个新的节点,否则重新选择一组作为备选节点,重复以上过程;直到达到目标点位置。扩展公式如下(11)所示:

但在RRT算法扩展树生成时,新增的叶子节点应该距离目标节点越来越近,由于扩展节点的方向是随机产生的,因而会引入局部极小问题,我们通过判断与之间的距离与与其他叶子节点的距离确定是否增加节点。如果前者较小则,增加该节点,否则取消增加。
3.2.1  算法改进
由于选取的随机性,从而导致从树的根节点到叶子节点规划的路径有时接近目标路径,有时远离目标,随机性很大。下图为基本RRT随机树的扩展过程。
针对RRT扩展过程中的这个特点,为了减少路径规划的随机性,使扩展树朝向最优目标点方向生长,由于随机树是按固定步长扩展节点,因此扩展的方向决定了路径规划性能。我们通过引入方向向量使得随机节点不至于偏离理想中的最优路径。即随机节点与目标节点的方向向量决定了路径的长短。
方向向量以随机树的当前父节点为基准点,为目标节点确定方向向量为基准向量,随机选取20个随机节点,然后计算与与确定的方向向量与基准向量之间的夹角。然后从小到大排序,选择前四个作为备选节点。通过赌轮法选择这四个随机节点扩展路径。详细过程参见步骤3.3中介绍。通过算法改进改进使得RRT能够有效地缩短规划时间,并且能够减少随机数的节点数,从而提高了效率。
3.2.2  路径平滑
RRT搜索之后的曲线折段,机械臂在运行中会发生抖动,我们采用三次Hermite曲线平滑处理,确保机械臂的速度连续性。Hermite曲线是由r0,r1,r2,r3四个控制点组成的曲线,其中中间过渡点为r1和r2。它以曲线的两个端点r0、r3端点处的切向矢量R0、R1为边界条件的三次参数曲线,其中起点切线R0=r1-r0,终点切线R1=r3-r2。
插值描述:1.当起点为r0=p0时,终点r3=p2,中间过渡点r1=r2=p1;记m=p0-p2;n=p5-p2;q=(m+n)/2 ;
f=cross(m ,n);则p1-p2=cross(q , f);  ||p1-p2||=0.5*|| m|| ; p3-p2= -(p1-p2);
2.当r0≠p0时,若r0=p2,则中间过渡点r1=p3,r2=p4,r3=p5;
规划曲线如(12)式所示:
 
3.3  机械臂路径规划算法
Step1  初始化机械臂的起点和目标点在三维空间内建立障碍物模型。
Step2  通过extendTree(point)函数扩展叶子节点。
(1) 判断新曾节点与目标节点的距离。如果到达目标节点即则随机树扩展结束,退出循环。
(2) 如果未达到目标节点则扩展节点,遍历随机树,找到随机树距离目标最近的节点选择随机节点扩展叶子节点,为了减小随机性,我们选择20个空间点作为备选随机节并计算这20个随机节点与目标节点之间的距离并保存到数组array[20],并通过轮盘赌算法选取其中的四个随机数按照欧式距离将其从小到大排序。
(3) 进入4次循环,如果新节点与随机节点的距离小于扩展步长L,本次不扩展,继续选择当4次循环结束时,未找到随机节点转(1)。判断新节点与其父节点组成的线段是否与障碍物发生碰撞,如果没有碰撞则按(6)式扩展节点,转(1)。当循环结束时,认为找到扩展的叶子节点,转(1)。
Step3  平滑搜索到的节点,即用折线段(最小二乘法求得)逼近搜索路径,并将其保存在node 数组中,通过Hermite插值平滑曲线。
Step4  机械臂运动学反解,执行器的空间某点位姿矩阵pos_goal=[2*(rand(1,3)-0.5),[new_node(:,i)]抅; 代入backKineticsXPcore函数进行运动学反解,如果反解失败,则在该点相邻的两点为对称轴,找到其对称点坐标,继续反解。
Step5  反解之后我们得到一系列关节角的集合,然后通过运动学正解。得到六个关节的空间坐标,再次进行碰撞检测。
Step6  如果反解失败,则在该位姿点的相邻的两点的六个角之间随机取值,至到得到安全的六个关节的角度。

4  仿真结果分析
图5所示的仿真实验是在1.5×1.3×0.7/(m3)的空间下,无障碍物,取工作空间中的点(-0.2,0.022,0.05)为起点,点(0.9,0.7,0.3)为目标点,图6左边的RRT的搜索步长为0.03,图6右边的RRT的搜索步长为0.05。图5左右两边的仿真图展现了一棵RRT的生长过程。

图5所示仿真实验是在1.5×1.3×0.7的空间环境中,在无障碍物时机械臂沿着直线的方向行进,这也说明该算法接近最短路径。
下面为设障碍的仿真效果图,碍物障位于起始位置与目标位置之间,的连线刚好穿越障碍物,实验环境与无障碍时相同,障碍物是一长方体,仿真结果如图7和图8所示。

5  结束语
由以上规划算法仿真图可以看出,RRT算法具有随机性,基本RRT算法搜索的路径距离理想路径偏差较大,而且搜索速度表较慢。改进后的算法减少了路径规划时间,修正了偏离目标点路径规划的方向。仿真结果表明:在无障碍物基本上是沿着直线,在有障碍时随机扩展,能够顺利避开障碍物,并且仍然以较近的路径到达目标位置。而且降低了运动学正反解的难度,提高了规划效率。

参考文献:
[1] 王滨,金明河,谢宗武,刘宏.基于启发式的快速随机树路径规划算法[J].机械制造,2007-12,45(520):2-3.  
[2] 王华,赵臣,王红宝等.基于快速扫描随机树方法的路径规划器[J].哈尔滨工业大学学报,2004,36(7):964-965.
[3] 郝利波,侯媛彬.基于一种改进RRT算法的足球机器人路径规划 [J].西安科技大学学报,2011-01,31(1):81-82.
[4] 美Eric Lengyel 著,詹海生,李广鑫译,王鸿利译.3D游戏与计算机图形学中的数学方法[M],清华大学出版社,2004.4.
[5] Steven M.Lavalle,James J.Kuffner,Jr.Rapidly-Exploring Random Trees:Progress and Prospects[J].Algorithmic and Computational Robotics.2000:3-4.
[6] ZhaXF.optimalPoseTrajeetory Planning for Robot Manipulators[J].Meehanismand Machine Theory,2002:1063-1086.
[7] 王红娟.三维实体建模及布尔运算造型技术[D].山东济南:山东科技大学 2007.
[8] 庄慧忠,杜树新,吴铁军,机器人路径规划及相关算法与仿真研究[J].科技通报,2004-5,20(3):211-212.
[9] 美Steve Rabin著.庄越挺,吴飞译.人工智能游戏编程真言[M].清华大学出版社,2005.1:79-81.

作者简介:代彦辉(1985-),男,在读硕士,研究方向:机器人视觉与智能算法。