[发明专利]基于多自由度机械臂的改进式RRT*的避障运动规划方法

摘要

本发明公开了基于多自由度机械臂的改进式RRT*的避障运动规划方法，属于机械臂运动规划领域。搭建七连杆和六旋转关节的六自由度机械臂模型；确定待搜索的空间内参数；如果小于最小花费路径距离，则将该集合内的附近节点到初始点和该节点到随机点的距离暂定为最小路径；对新生成的σ做碰撞检测，如果新生成的路径没有碰撞到障碍物区间；直到最优路径搜索到；将生成的轨迹添加到轨迹规划器中，本发明与现有技术相比具有以下优点，通过添加正态分布的方式改变随机搜索的特性，这种带有启发式的搜索可以提高算法的收敛率，RRT*算法具有渐进优化的路径，不用通过大量的计算；加入启发点在目标点附近的高斯分布后，提高了收敛速率，节约了搜索时间。

主权项

基于多自由度机械臂的改进式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|μ,σ)是修正后的采样区域概率密度；步骤十二：重复步骤三到步骤十一，一直到最优路径搜索到。