基于多自由度机械臂的改进式RRT*的避障运动规划方法技术领域
本发明涉及到机械臂避障环节中运动规划方法，具体是一种改进式快速随机搜索
的方法应用到全局空间，属于机械臂运动规划领域
背景技术
机器人的运动规划问题早在上世纪六十年代提出，早期的运动规划只是涉及到路
径规划，机器人被看成一点，运动规划就被看成在位姿空间中寻找一条从起始位姿点到目
标位姿点的连续路径，路径规划经过一个搜索的过程，根据世界环境的建模和搜索方法的
不同主要分成自由空间几何构造的规划，智能法规划和基于随机采样的规划方法。然而早
期的路径规划不能适应于当前机械臂的高自由度和障碍物空间复杂的环境，早期的规划方
法只能增加大量的计算，降低搜索的效率。
机械臂的避障规划目标是规划处一条满足各类指标的最优路径，针对这一问题，
已采用的C空间法、A*搜索法、人工试场法、遗传算法都存在一定的局限性，C空间法的计算
时间要比机械臂的响应时间长，限制其在避障中的应用，蚁群算法是本质上并行的算法，它
在问题空间多点同时开始进行多线独立的解搜索，但改算法搜索时间较长，易陷入停滞问
题。
考虑到现在搜索算法存在的诸多效率低下的问题，本发明旨在提出一种改进的
RRT*(rapidly exploring random tree)算法的多自由度机械臂避障的运动规划方法，对
解决了搜索时花费时间长，迭代次数多，易于限于局部无穷小，收敛率低的问题，使机械臂
能够快速有效地避开障碍物。这种概率规划尤其对于高维空间中的规划问题更是有效。
发明内容
为了实现上述目的，本发明采用的技术方案为基于多自由度机械臂的改进式RRT*
的避障运动规划方法，该方法的实现过程如下：
步骤一：搭建七连杆和六旋转关节的六自由度机械臂模型，设定目标到达点位姿
值，逆解求其关节角度，确定逆解是否有解，如果有解，设定为目标状态xgoal，目标状态xgoal
包含位置和姿态信息；如果逆解无解，重新采样空间区域内目标位姿点，直到选中可达的目
标状态为止；
步骤二：确定待搜索的空间内参数初始化，初始化参数为：起始位型xinit，目标位
型xgoal，扩展步长λ，当前的迭代次数n；
步骤三：在位型空间内随机采样选取xrand,选取xrand附近临近节点区域，根据节点
区域公式得到xnear的集合Xnear，其中d是空间
维度，γ是选择的常数，V是已构成搜索树节点的集合，x′是搜索域中某一节点。
步骤四：为了选定xmin,σmin作为暂定的父节点和父路径，遍历集合Xnear，定义最优
路径minCost，将xmin,σmin赋Null值，从随机点xrand向集合内子节点xnear生成各自路径σ；
步骤五：判断Cost(xnear)+Cost(σ)<minCost是否成立，如果小于最小花费路径距
离，则将该集合内的附近节点到初始点和该节点到随机点的距离暂定为最小路径，相应地，
xnear和σ就暂定为各自的xmin,σmin，如果大于最小花费路径，则遍历其他附近节点，直到找到
最小路径为止；
步骤六：对新生成的σ做碰撞检测，如果新生成的路径没有碰撞到障碍物区间，则
将节点xrand和路径σ添加到树中；
步骤七：将现有的树即节点和边和Xnear集合剩余的xnear，xrand添加到Rewire函数进
行重新焊接路径，将冗余路径剔除；
步骤八：遍历剩余的xnear，从新生成的节点xrand调用Steer(xrand,xnear)步长函数生
成路径σ；
步骤九：判断Cost(xrand)+Cost(σ)<Cost(xnear)是否成立，如果初始点到随机点路
径加上新生成路径的距离小于原始的初始点到相应附近节点路径，那么对其σ进行碰撞检
测，如果没有碰撞到障碍物，则定义该xnear作为父节点，去除所有的父节点和其余xnear的边E
←E\{xparent,xnear}，E代表所构成的边的集合，并将随机点和父节点的边重新添加到树中E
←E∪{xrand,xnear}；
步骤十：重新采样下一个随机点，根据正态分布的规律改变随机采样的特性
μ代表平均值，σ是标准差。将均值设置为xgoal，通过改变标准差的
值来改变搜索的随机特性，设置标准差σ的参数值；
步骤十一：判断搜索空间的大小以及对称程度，将正态分布的采样区域重新更改
规划，采用-φ(xlower)是采样可
达最大区域，将概率密度函数更改以适应不同的实际环境，f′(x|μ,σ)是修正后的采样区域
概率密度；
步骤十二：重复步骤三到步骤十一，一直到最优路径搜索到；
将生成的轨迹添加到轨迹规划器中，本发明与现有技术相比具有以下优点，通过
添加正态分布的方式改变随机搜索的特性，这种带有启发式的搜索可以提高算法的收敛
率，RRT*算法具有渐进优化的路径，不用通过大量的计算；加入启发点在目标点附近的高斯
分布后，提高了收敛速率，节约了搜索时间。
附图说明
图1为搭建的六自由度机械臂
图2为发明流程图
图3为RRT*搜索示意图
图4为RRT*在三维环境下的搜索路径图
图5目标位姿点添加高斯分布密度函数图
图6为加入高斯分布后的改进式RRT*最优搜索路径图
图7为在ROS仿真环境下六自由度机械臂添加改进式RRT*运动规划的仿真路径图
具体实施方式
下面结合说明附图和具体实施方式对本发明进一步说明，本发明的实施例是按照
本发明的技术方案前提下实施的。给出了详细的实施方式和具体的操作流程，但本发明的
保护范围不限于以下实例范围。
首先建立七连杆六旋转关节的机械臂，选择二指夹持器作为末端执行器，如图1一
样(x0，y0，z0)作为基坐标系，其余关节围绕旋转的轴作为z轴，根据DH参数值计算出末端的
位姿矩阵其中A1到A6为各关节旋转矩阵，通
过逆运动学分析求解目标状态下的各个关节角度
根据图2的流程图完成算法路径的搜索，具体为
在100*100*100的三维立体空间内，起始位型xinit设定为(5,5,5),在不考虑机械臂
姿态，只考虑空间位置的情况下，目标位型xgoal(95,95,95),扩展步长λ设为1.5,当前迭代次
数为0次，在搜索空间内随机采样选取xrand,根据公式
得到xnear的集合Xnear,其中d设定为3，γ选择0.618，在该集合里查看是否有搜索树上的节
点，如果没有进一步扩展迭代次数增加搜索域直到集合Xnear包含附近节点；选定xmin,σmin作
为暂定的父节点和父路径，遍历集合Xnear，定义最优路径minCost，将xmin,σmin赋Null值，从
随机点xrand向集合内子节点xnear生成各自路径σ；
判断Cost(xnear)+Cost(σ)<minCost是否成立，如果小于最小花费路径距离，则将
该集合内的附近节点到初始点和该节点到随机点的距离暂定为最小路径，相应地，xnear和σ
就暂定为各自的xmin,σmin，如果大于最小花费路径，则遍历其他附近节点，直到找到最小路
径为止；对新生成的σ做碰撞检测，如果新生成的路径没有碰撞到障碍物区间，则将节点
xrand和路径σ添加到树中；将现有的树(节点和边)和Xnear集合剩余的xnear，xrand添加到
Rewire函数进行重新焊接路径，将冗余路径剔除；遍历剩余的xnear，从新生成的节点xrand调
用Steer(xrand,xnear)步长函数生成路径σ，判断Cost(xrand)+Cost(σ)<Cost(xnear)是否成立，
如果初始点到随机点路径加上新生成路径的距离小于原始的初始点到相应附近节点路径，
那么对其σ进行碰撞检测，如果没有碰撞到障碍物，则定义该xnear作为父节点，祛除所有的
父节点和其余xnear的边E←E\{xparent,xnear}，并将随机点和父节点的边重新添加到树中E←E
∪{xrand,xnear}，如图3示意图显示，拓展节点的方式和重新焊接已有节点和边是RRT*不同于
原始RRT的一种搜索方法，重新采样下一个随机点，根据正态分布的规律改变随机采样的特
性将均值设置为xgoal(95,95,95)，通过改变标准差的值来改变搜
索的随机特性，设置标准差σ＝1；判断搜索空间的大小以及对称程度，将正态分布的采样区
域重新更改规划，如图5所示，采用将概率
密度函数更改以适应不同的实际环境，重复如说明图的步骤，直到找到一条合适的搜索路
径为止。
图4是RRT*的搜索路径图，包含了8个球形的障碍物，起始点是(5,5,5)终点是(95,
95,95)只考虑末端的位置，不考虑姿态的情况下按照如上所述搜索策略的搜索路径图，其
中迭代次数为792次，路径规划的时间是17.89s，黑线是最终搜索到的路径。
图6是通过改变σ的值来调试适合实际情况的修正过后的高斯分布采样，通过将μ
值设置为目标点，改变σ值来改变随机点的分布情况，图6所示为σ值分别为20,50,80情况下
的RRT*路径图，通过这种改变，搜索的时间递减，搜索采样的迭代次数递减，可以看出改方
法的改进可以提高算法的收敛性。最后通过目标点反解得到一系列关节角集合，得到最终
机械臂各关节位姿。最后在ros仿真环境中使用moveit和rviz显现六自由度机械臂应用改
进的RRT*算法的抓取实验图。