[发明专利]一种机械臂运动规划方法

权利要求书

1.一种机械臂运动规划方法，其特征在于，具体按照以下步骤实施：

步骤1、利用常见的空间规则物体近似包络障碍物进行障碍物建模；

步骤2、利用步骤1建立的模型，给定机械臂起始位置与目标位置，通过逆运动学方法获得目标位置的逆解；

步骤3、利用步骤2得到的逆解的值通过p-rrt算法计算出一条轨迹；

步骤4、导入七自由度机械臂模型并给定机械臂目标位置，通过逆运动学得到目标位置逆解；将步骤3得到的轨迹的路径节点作为输入并用B样条曲线对该路径进行平滑优化处理；

步骤5，根据步骤4可以得到机械臂平滑后的路径曲线，在保证对机械臂运动不与障碍物发生碰撞的前提下，在Matlab中进行仿真实验，加入B样条曲线优化后的仿真对比，对比了平滑前后的路径曲线，验证有效性，完成械臂运动规划。

2.根据权利要求1所述的一种机械臂运动规划方法，其特征在于，所述步骤1具体按照以下实施：获取障碍物的大小、位置信息，设置碰撞检测方案；为了避免机械臂各连杆与障碍物发生碰撞，在将障碍物包络成球体后，需计算球形障碍物和机械臂连杆之间的空间位置关系，若机械臂连杆与球形障碍物圆心的距离小于半径之和即机械臂连杆和障碍物有交集部分，则会发生碰撞，其余情况则表示不会发生碰撞。

3.根据权利要求1所述的一种机械臂运动规划方法，其特征在于，所述步骤3具体按照以下步骤实施：

步骤3.1，在地图环境中进行随机采样并确定采样点x_rand，找到距离x_rand最近的节点x_near；

步骤3.2，确定随机点边界并根据概率函数P的值来选择树的生长方向是朝向目标点方向还是随机方向；此处设置的概率为有0.5概率直行；

步骤3.3，沿着x_near到x_rand方向前进stepsize的距离得到x_new；

步骤3.4，采用collisionFree(M,E_i)方法检测Edge(x_new,x_near)是否与地图环境中的障碍物有碰撞，如果没有碰撞则可以完成一次成功的空间拓展搜索；

步骤3.5，计算新点x_new与终点的距离，如果小于目标点阈值则结束寻迹，并生成轨迹。

4.根据权利要求3所述的一种机械臂运动规划方法，其特征在于，所述步骤3中，初始化地图环境并确定起始点x_init和目标点x_goal并设置目标点阈值，当到达这个范围内是则认为已到达目标点，此处阈值设置为：

Thr＝0.03。

5.根据权利要求1所述的一种机械臂运动规划方法，其特征在于，所述步骤4具体按照以下步骤实施：

步骤4.1，采用三次非均匀B样条插值优化步骤3得到的轨迹：

K次非均匀B样条曲线定义：

其中d_i,i＝0,1,2,...,n为控制点，顺序连接成为B样条控制多边形，N_i,k(u)是定义在非均匀节点矢量U上的k次规范B样条基函数；并有

K＝0时的N_i,k(u)如果次数为零(即K＝0)，这时基函数都是阶梯函数，如果是在第i个节点区间[u_i,u_i+1]上，那么基函数的N_i,0(u)＝1；

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

其中k为偶数次时：

其中k为奇数次时：

基于P-RRT得到的路径点q_i，对非均匀B样条反解得到控制点d_i，然后通过控制点生成平滑的轨迹，完成对步骤3得到的轨迹优化，完成机械臂运动规划。