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

说明书

技术领域

本发明涉及到机械臂避障环节中运动规划方法，具体是一种改进式快速随机搜索的方法应用到全局空间，属于机械臂运动规划领域

背景技术

机器人的运动规划问题早在上世纪六十年代提出，早期的运动规划只是涉及到路径规划，机器人被看成一点，运动规划就被看成在位姿空间中寻找一条从起始位姿点到目标位姿点的连续路径，路径规划经过一个搜索的过程，根据世界环境的建模和搜索方法的不同主要分成自由空间几何构造的规划，智能法规划和基于随机采样的规划方法。然而早期的路径规划不能适应于当前机械臂的高自由度和障碍物空间复杂的环境，早期的规划方法只能增加大量的计算，降低搜索的效率。

机械臂的避障规划目标是规划处一条满足各类指标的最优路径，针对这一问题，已采用的C空间法、A*搜索法、人工试场法、遗传算法都存在一定的局限性，C空间法的计算时间要比机械臂的响应时间长，限制其在避障中的应用，蚁群算法是本质上并行的算法，它在问题空间多点同时开始进行多线独立的解搜索，但改算法搜索时间较长，易陷入停滞问题。

考虑到现在搜索算法存在的诸多效率低下的问题，本发明旨在提出一种改进的RRT*(rapidly exploring random tree)算法的多自由度机械臂避障的运动规划方法，对解决了搜索时花费时间长，迭代次数多，易于限于局部无穷小，收敛率低的问题，使机械臂能够快速有效地避开障碍物。这种概率规划尤其对于高维空间中的规划问题更是有效。

发明内容

为了实现上述目的，本发明采用的技术方案为基于多自由度机械臂的改进式RRT*的避障运动规划方法，该方法的实现过程如下：

步骤一：搭建七连杆和六旋转关节的六自由度机械臂模型，设定目标到达点位姿值，逆解求其关节角度，确定逆解是否有解，如果有解，设定为目标状态x_goal，目标状态x_goal包含位置和姿态信息；如果逆解无解，重新采样空间区域内目标位姿点，直到选中可达的目标状态为止；

步骤二：确定待搜索的空间内参数初始化，初始化参数为：起始位型x_init，目标位型x_goal，扩展步长λ，当前的迭代次数n；

步骤三：在位型空间内随机采样选取x_rand,选取x_rand附近临近节点区域，根据节点区域公式得到x_near的集合X_near，其中d是空间维度，γ是选择的常数，V是已构成搜索树节点的集合，x′是搜索域中某一节点。

步骤四：为了选定x_min,σ_min作为暂定的父节点和父路径，遍历集合X_near，定义最优路径minCost，将x_min,σ_min赋Null值，从随机点x_rand向集合内子节点x_near生成各自路径σ；

步骤五：判断Cost(x_near)+Cost(σ)<minCost是否成立，如果小于最小花费路径距离，则将该集合内的附近节点到初始点和该节点到随机点的距离暂定为最小路径，相应地，x_near和σ就暂定为各自的x_min,σ_min，如果大于最小花费路径，则遍历其他附近节点，直到找到最小路径为止；

步骤六：对新生成的σ做碰撞检测，如果新生成的路径没有碰撞到障碍物区间，则将节点x_rand和路径σ添加到树中；

步骤七：将现有的树即节点和边和X_near集合剩余的x_near，x_rand添加到Rewire函数进行重新焊接路径，将冗余路径剔除；

步骤八：遍历剩余的x_near，从新生成的节点x_rand调用Steer(x_rand,x_near)步长函数生成路径σ；