CN110744543B - 基于ur3机械臂的改进式prm避障运动规划方法 - Google Patents

Abstract

本发明涉及基于UR3机械臂的改进式PRM避障运动规划方法，该方法首先根据UR3机械臂运动学特性建模，进行逆运动学的求解；确定搜索空间的各种参数，构造一个无向图的路径网格存储采样点集和路径集；每一次迭代过程得到一个采样点，根据动态邻域半径来选择邻域内的若干点作为当前采样点的连接对象；通过构建局部规划器来规划点与点之间的简单路径，并检测该路径与空间障碍物的碰撞情况，若无碰撞则将路径添加到路径网格中；迭代完成最后，再利用A*算法，从得到的路径网格RoadMap中获取最佳的连接起始点和终点位姿的路径。能够使机械臂更快速有效的避开障碍物，且这种概率式的规划在高维空间中更是有效，精确度更高，具有渐进最优的优点。

Description

基于UR3机械臂的改进式PRM避障运动规划方法

技术领域

本发明涉及机器人技术领域，尤其涉及一种基于UR3机械臂的改进式PRM避障运动规划方法。

背景技术

自从机器人概念从上世纪五十年代提出，经历了半个多世纪的演变，机器人经历了从简单功能到复杂功能，从单一行为到复杂行为以及从单场景到多场景的转变，而今天已经出现集成多功能的复杂型机器人。从Willow Garag公司开发的可以自动打台球的双臂机器人PR2，到可以自主潜到水深3000米的水下机器人OCEAN ONE，再到ABB公司2014年上市的人机协作机械臂Yumi，机器人的概念从简单工业生产线上的机械臂，转入到服务于人类的服务型机器人，转型到精加工工厂流水线的轻型协作机械臂。

目前市场导向追逐更加灵活的任务层面的操纵，例如实现从A点抓取，在B点停止并进行作业这样的任务编程，不需要考虑中间的状态。这就要求其有精准的自主快速规划路径的能力，以及针对复杂障碍物环境高效的避障功能，机器人如何能无碰撞地在关节空间中完成初始关节角度到末端关节角度是机器人运动规划需要考虑的重要问题，其本质上是约束条件下的路径优化问题，随着规划方法的不断智能化，如何快速高效的选出优化的路径是衡量机器人智能化的重要指标。

发明内容

针对现有技术中的问题，本发明的首要目的是提供一种基于UR3机械臂的改进式PRM避障运动规划方法。基于该目的，本发明至少提供如下技术方案：

基于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维欧氏空间中单位球的体积。

进一步的，所述步骤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是动态邻域半径。

进一步的，所述步骤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。

进一步的，所述步骤S5具体包括如下步骤：在区间[0,1]采样，对τ_s(0,1)路径中的采样点分别使用碰撞检测算法，设置空间内的采样步长λ，若所有采样点均属于空间C_free，则该路径为有效路径，添加至路径集中；反之为无效路径，不予添加。

进一步的，所述步骤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。

进一步的，所述步骤S1中包括如下步骤：

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

与现有技术相比，本发明至少具有如下有益效果：

本发明的机械臂避障运动规划方法中，对每次迭代产生的采样点设置动态邻域半径来选择其邻域内的若干点作为当前采样点的连接对象，邻域半径随着样本的增加而减少，使得点与点之间尝试连接的次数大大减少，避免了大量的连接，提高了效率，节约了搜索时间，能够使机械臂更快速有效的避开障碍物，且这种概率式的规划在高维空间中更是有效；另外，本发明的避障运动规划方法中，通过给采样点添加权重的方式来重新扩展区域采样，这种带有启发式的方法，可以提高采样的精确度，具有渐进最优的优点。

附图说明

图1为本发明避障运动规划方法流程图。

图2为相邻连杆的参数示意图。

图3为UR3机械臂连杆坐标示意图。

图4为本发明避障运动规划方法中的采样点示意图。

图5为本发明避障运动规划方法中的邻域半径示意图。

图6为本发明避障运动规划方法中的碰撞检测采样密度示意图。

图7为本发明避障运动规划方法中的搜索最优路径示意图。

具体实施方式

为了使本发明的目的、技术方案以及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步的详细说明。应当理解，此处所描述的具体实施例仅用于解释本发明，并不限于本发明。

本发明的基于UR3机械臂的改进式PRM避障运动规划方法流程图如图1所示。下面将结合图1对本发明的避障运动规划方法作以详细介绍。该方法包括以下步骤：

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

UR3机器人可以用四个参数来描述：连杆长度a_i，连杆扭角α_i，连杆距离d_i和关节转角θ_i来描述。如图2所示，连杆长度a_i以及连杆扭角α_i表示连杆的特征，连杆距离d_i和关节转角θ_i表示连杆和连杆之间的关系。图3为UR3连杆建立坐标系，根据连杆之间的关系和各连杆的尺寸，得出UR3机器人的D-H参数，如表1所示。

表1

以(x,y,z)作为基坐标系，其余关节围绕旋转的轴作为z轴，根据UR3机器人的各连杆坐标系，已知两相邻连杆间位置关系，按从左到右的原则得到各连杆的变换矩阵：

式(1)中i＝1,2,3,…6。

根据UR3机器人的D-H参数值得到

到

各个连杆的变换矩阵，将各个连杆变换矩阵相乘就能得到UR3的变换矩阵

即末端执行器的位姿矩阵：

式(2)中各简式如下：

式(3)中各简式如下：

上式中，i＝1,2,3,…6，j＝1,2,3,…6，k＝1,2,3,…6。

通过逆运动学分析求解目标状态下的各个关节角度，以确定逆解是否存在。若不存在该逆解，重新确定终点位姿直至逆解存在；反之则存在逆解，设定起始点位姿为x_init，设定终点位姿状态为x_goal，其中x_goal包含机械臂末端执行器的空间位置以及各关节的角度姿态信息。逆运动学求解如下：

θ₅＝arccos(d₆,p_xs₁-p_yc₁-d₄)

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

当前迭代次数为n，总迭代次数为N。构造一个无向图的路径网格RoadMap，即R＝(V，E)，其中V代表随机点集，E代表存在于C_free所有可能的两点中间的路径集。初始化V和E为空集。确定待搜索空间(自由空间)内参数初始化，初始化参数为：起始位姿x_init，终点位姿x_goal。

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

进行当前迭代，首先在位型空间C中随机产生一个采样点x_rand(如图4)，该采样点x_rand即为当前采样点x_rand，若x_rand∈C_free，则将该当前采样点x_rand添加到随机点集V中，从随机点集V中选取当前采样点x_rand附近临近节点区域，根据节点邻域公式：

Near(V,x_rand)＝{x'∈V:||x_rand-x'||≤r}

得到当前采样点x_rand的领域点集合x_near，其中d是空间维度，x’是V中的某一节点，r是邻域半径。为了使获得采样点x_rand邻域点集合x_near更具有效率，此处的邻域半径r为动态邻域半径，即动态邻域半径随着采样点数动态变化，如下：

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

步骤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。

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

检测碰撞的方法如下所述：检测τ(0,1)的方法是在区间[0,1]采样，对[0,1]区间内的采样点分别使用碰撞检测算法，如图6。设置恰当的采样密度(可根据具体情况设置)，即空间内的采样步长λ。若所有采样点均属于空间C_free，则该规划的路径为有效路径，便可添加到路径网格中；反之为无效路径，不予添加。

步骤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。

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

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

至此已得到的路径网格RoadMap。首先将路径的起始和终点位姿x_init和x_goal分别调用局部规划器将其接入路径网格。

接下来详细介绍如何根据A*算法从路径网格中获取最佳的连接起始位姿和终点位姿的路径。

根据F(i)值构建随机点集V的优先队列，其中F(i)＝dist(i)+h(i)，i为顶点，即为随机点集中的某一个，属于V，dist(i)表示从起点到这个顶点的欧氏距离，h(i)表示这个顶点到终点路径的估值函数，用曼哈顿距离计算。所谓曼哈顿距离是空间两点之间xyz轴坐标的距离之和。

构建dist数组，表示顶点到起始位姿x_init的距离。初始化顶点值。将顶点为初始位姿x_init的F值和dist初始化为0，随机点集中的其余顶点的F值和dist初始化为无穷大。然后将所有顶点加入到优先队列中。

从优先队列中取出F最小的顶点minVertex，然后考察这个顶点的所有可达顶点nextVertex(即路径网格RoadMap中存在路径的两个顶点)，设minVertex与nextVertex之间的欧式距离为w，若minVertex的dist值加上w小于nextVertex当前的dist值，则将nextVertex的dist值更新为minVertex的dist值加上w。同时更新nextVertex的F(nextVertex)＝dist(nextVertex)+h(nextVertex)。直至遍历到终点位姿x_goal，获得最优运动规划路径，如图1所示。

上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims (6)

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包含位置和姿态信息；如果逆解无解，重新采样目标到达点位姿。

Images