[发明专利]基于目标偏向寻优的启发式RRT机械臂运动规划方法

权利要求书

1.基于目标偏向寻优的启发式RRT机械臂运动规划方法，其特征在于，包括如下步骤：

步骤一、基于目标偏向寻优的RRT路径节点确定的步骤；包括：

A1、确定起始点q_start,将q_start存储到q_nodes中；

A2、在空间中全地图随机采样点q_rand，寻找q_nodes到达q_rand中最近的一个点作为q_near；

A3、在q_near到q_rand的方向上以步长δ前进，δ∈C，利用目标偏向因子控制随机点产生方向以及距离前进至节点q_new，在前进过程中进行碰撞检测，如果检测到碰撞，则返回A2；如果没有检测到碰撞，则将q_new存储到q_nodes中并转至A4；其中：

A4、根据更新后的q_nodes，重复A2～A3，直至得到最新的节点q_goal满足|q_new-q_goal|Error，视为找到到目标点，将q_new存入到q_nodes中，Error是指允许误差；

A5、在q_nodes根据各节点的父子关系，反向搜索找到规划路径；

步骤二、基于Douglas-Peucker算法的初始路径节点抽稀处理的步骤；包括：

B1、根据A5得到的规划路径，在路径首尾两点AG之间连一条线段；

B2、确定其他路径点到该线段AG距离最大的点C，获取C点到线段AG的距离d；

B3、给定γ与d比较，若d小，删除C点，若d大，保留C点作为关键点，用C点将原有路径分为两段AC和CG；其中，γ∈(0,δ)；

B4、对于分割后形成的分段，分别重复B1～B3；直至初始路径上所有的点均已经过遍历后被删除或者作为关键点保留；

B5、所得到的关键点q_i，即为抽稀后的路径点。

2.根据权利 要求1 所述的一种基于目标偏向寻优的启发式RRT机械臂运动规划方法，其特征在于，还包括

步骤三、基于非均匀B样条拟合的轨迹优化的步骤，包括：

C1、采用三次非均匀B样条插值优化B5中得到的抽稀后的路径；

B样条曲线方程式：

其中d_i(i＝1,2,…,n)为控制点，N_i,k为k次规范B样条基函数，u为节点向量；且

其中l_i＝|d_i-d_i-1|(i＝1,2,...,n)；

其中k为偶数次时：

其中k为奇数次时：

基于B5得到的路径点q_i，对非均匀B样条反解得到控制点d_i(i∈q_curve)，然后通过控制点生成平滑的轨迹，实现在原路径规划的基础上实现轨迹优化。

3.根据权利 要求1所述的一种基于目标偏向寻优的启发式RRT机械臂运动规划方法，其特征在于，所述步骤A3中，在没有障碍影响、β＝π时，控制避免由于对目标点指向性太强造成局部最优解；其中β为与之间的夹角，δ∈C，C是任意常数。