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

权利要求书

1.基于多自由度机械臂的改进式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)步长函数生成路径σ；

步骤九：判断Cost(x_rand)+Cost(σ)<Cost(x_near)是否成立，如果初始点到随机点路径加上新生成路径的距离小于原始的初始点到相应附近节点路径，那么对其σ进行碰撞检测，如果没有碰撞到障碍物，则定义该x_near作为父节点，去除所有的父节点和其余x_near的边E←E/{x_parent,x_near}，E代表所构成的边的集合，并将随机点和父节点的边重新添加到树中E←E∪{x_rand,x_near}；

步骤十：重新采样下一个随机点，根据正态分布的规律改变随机采样的特性μ代表平均值，σ是标准差；将均值设置为x_go_al，通过改变标准差的值来改变搜索的随机特性，设置标准差σ的参数值；

步骤十一：判断搜索空间的大小以及对称程度，将正态分布的采样区域重新更改规划，采用-φ(x_lower)是采样可达最大区域，将概率密度函数更改以适应不同的实际环境，f′(x|μ,σ)是修正后的采样区域概率密度；

步骤十二：重复步骤三到步骤十一，一直到最优路径搜索到。

2.根据权利要求1所述的基于多自由度机械臂的改进式RRT*的避障运动规划方法，其特征在于：

首先建立七连杆六旋转关节的机械臂，选择二指夹持器作为末端执行器，(x0，y0，z0)作为基坐标系，其余关节围绕旋转的轴作为z轴，根据DH参数值计算出末端的位姿矩阵其中A1到A6为各关节旋转矩阵，通过逆运动学分析求解目标状态下的各个关节角度；完成算法路径的搜索，具体为：在100*100*100的三维立体空间内，起始位型x_init设定为(5,5,5),在不考虑机械臂姿态，只考虑空间位置的情况下，目标位型x_goal(95,95,95),扩展步长λ设为1.5,当前迭代次数为0次，在搜索空间内随机采样选取x_rand,根据公式得到x_near的集合X_near,其中d设定为3，_γ选择0.618，在该集合里查看是否有搜索树上的节点，如果没有进一步扩展迭代次数增加搜索域直到集合X_near包含附近节点；选定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)步长函数生成路径σ，判断Cost(x_rand)+Cost(σ)<Cost(x_near)是否成立，如果初始点到随机点路径加上新生成路径的距离小于原始的初始点到相应附近节点路径，那么对其σ进行碰撞检测，如果没有碰撞到障碍物，则定义该x_near作为父节点，祛除所有的父节点和其余x_near的边E←E/{x_parent,x_near}，并将随机点和父节点的边重新添加到树中E←E∪{x_rand,x_near}，拓展节点的方式和重新焊接已有节点和边是RRT*不同于原始RRT的一种搜索方法，重新采样下一个随机点，根据正态分布的规律改变随机采样的特性将均值设置为x_go_al(95,95,95)，通过改变标准差的值来改变搜索的随机特性，设置标准差σ＝1；判断搜索空间的大小以及对称程度，将正态分布的采样区域重新更改规划，采用将概率密度函数更改以适应不同的实际环境，重复如说明图的步骤，直到找到一条合适的搜索路径为止。