[发明专利]基于UR3机械臂的改进式PRM避障运动规划方法

权利要求书

1.基于UR3机械臂的改进式PRM避障运动规划方法，其特征在于，包括如下步骤：

步骤S1、搭建七连杆和六旋转关节的六自由度机械臂运动学模型及逆运动学求解各关节的参数；

步骤S2、确定所述机械臂所在空间的参数，设位型空间为C，自由空间为C_free，障碍物空间为C_obs，构造一个初始化后的无向图的路径网格存储采样的随机点集以及路径集；

步骤S3、进行当前迭代，每次迭代获得一个采样点，设置采样点的动态邻域半径，根据动态邻域半径来选择邻域内的若干随机点作为当前采样点的连接对象，其中所述动态邻域半径是指邻域半径随采样点数动态变化；

步骤S4、构建局部规划器，当前采样点调用局部规划器分别与其邻域内的连接对象进行连接形成新的路径；

步骤S5、对所述新的路径做碰撞检测，若所述新的路径与障碍物空间无碰撞，则将所述新的路径添加至路径集中，反之，则不添加；

步骤S6、为每个采样点引入权重系数ω，扩充区域点；

步骤S7、重复步骤S3至步骤S6，直到迭代完成，获得路径网格；

步骤S8、将路径的起始位姿以及终点位姿导入所述步骤S7中的路径网格，根据A*算法，从所述路径网格中获取最佳的连接起始位姿和终点位姿的路径；

所述动态邻域半径随采样点数动态变化如下：

其中，r是动态邻域半径，d是空间维度，n是当前已采样的点数，μ(C_free)是自由空间的体积，ζ(C_d)是d维欧氏空间中单位球的体积。

2.根据权利要求1的所述避障运动规划方法，其特征在于，所述步骤S3 中，每次迭代获得一个采样点x_rand，若x_rand不属于C_free，重新获得采样点；若x_rand∈C_free，则将该采样点添加至随机点集V中，选取采样点x_rand邻域，根据节点邻域公式：Near(V,x_rand)＝{x'∈V:||x_rand-x'||≤r}获得采样点x_rand的邻域点集合x_near，其中x’是V中的某一节点，r是动态邻域半径。

3.根据权利要求1-2之一的所述避障运动规划方法，其特征在于，所述步骤S4具体包括如下步骤：

对于当前的采样点x_rand(x_rand∈C_free)，遍历邻域点集合x_near，分别构造路径τ_s：[0,1]→C_free，满足τ(0)＝x_cur，τ(1)＝x_rand，x_cur∈x_near，路径τ_s即为新的路径e。

4.根据权利要求3的所述避障运动规划方法，其特征在于，所述步骤S5具体包括如下步骤：在区间[0,1]采样，对τ_s(0,1)路径中的采样点分别使用碰撞检测算法，设置空间内的采样步长λ，若所有采样点均属于空间C_free，则该路径为有效路径，添加至路径集中；反之为无效路径，不予添加。

5.根据权利要求1的所述避障运动规划方法，其特征在于，所述步骤S6中，权重系数ω的选取方法为：与局部规划器为采样点x_rand尝试连接的失败率成正比；

其中，r_f(X_rand)表示局部规划器为采样点x_rand尝试连接的失败概率，k(x_rand)是试图连接采样点x_rand的总次数，f(x_rand)是失败的总次数；ω(X_rand)表示采样点x_rand的权重，x_cur属于采样点x_rand的领域点集合x_near。

6.根据权利要求3的所述避障运动规划方法，其特征在于，所述步骤S1中包括如下步骤：

搭建七连杆和六旋转关节的六自由度机械臂模型，设定目标到达点位姿势，逆解求所述目标到达点位姿势的关节角度，确定逆解是否有解，若有解，将该目标到达 点位姿势设定为目标位姿x_goal，目标位姿x_goal包含位置和姿态信息；如果逆解无解，重新采样目标到达点位姿。