CN116117822A - 基于非障碍物空间概率势场采样的rrt机械臂轨迹规划方法 - Google Patents

Abstract

本发明提出基于非障碍物空间概率势场采样的RRT机械臂轨迹规划方法，包括基于非障碍物空间生成采样点，即通过采样点是否落入障碍物空间来对采样点进行筛选，避免冗余路径的生成以及非必要路径碰撞检测；所述规划方法还包括概率势场贪婪采样策略，即在保证算法概率完备的前提下赋予算法预设概率的目标导向性以加快路径生成，且通过引力场与斥力场作用来减少路径碰撞检测且生成路径方向相对更优；所述规划方法还包括冗余节点删除策略，即对算法回溯生成的路径进行优化，删除路径中冗余节点，使得生成路径更优；并通过插值多项式对优化后的路径进行平滑处理，使得路径更符合实际机械臂运动；本发明能更高效地生成平滑路径，具有非常高的工业应用价值。

Description

基于非障碍物空间概率势场采样的RRT机械臂轨迹规划方法

技术领域

本发明涉及机械臂技术领域，尤其是基于非障碍物空间概率势场采样的RRT机械臂轨迹规划方法。

背景技术

随着人工智能与智能制造技术的不断发展，未来的工业制造必定是高度智能化、信息化且人机协作共融的，机械臂所面临的工作场景也必定是复杂且不确定的。由于工业机械臂的应用环境越来越复杂，机械臂避障路径规划的作用也就越来越重要，机械臂避障路径规划是指在工作空间存在障碍物的情况下，机械臂能在有限的时间内规划出一条无碰撞路径。

现有技术以及所在问题如下：

1、基于随机采样的轨迹规划算法--快速扩展随机树(Rapidly-exploring RandomTrees，RRT)是一种在二维以及高维空间都能广泛适用且概率完备的轨迹规划方法，被广泛用于机械臂避障路径规划。

2、RRT-Connect算法同时从初始点与目标点生长出两棵随机树，具有良好的搜索特性，相较于RRT算法，其在搜索速度、效率上有了显著提高。渐进最优的RRT*算法保证计算复杂度和渐进最优解。Informed-RRT*算法能够更快地返回渐近最优解。因RRT算法其生成采样点的随机性，会导致其在工作空间中盲目采样以至于算法规划可行路径时间成本过高，且会生成大量冗余节点，空间成本高，随机性大。

3、人工势场法(Artificial potential field method，APF)在无障碍物或障碍物较少的机器人工作空间内表现优异，但人工势场在复杂障碍物工作空间内机器人所受到的引力与斥力极易相互抵消导致机器人陷入局部极小值。

4、发明专利CN202210671495.5中使用目标偏置的方法让RRT在保持概率完备的情况下具有了一定目的性，减短了搜索时间，并使用三角剪枝方法对路径进行了优化，但其采样时是对于整个工作空间采样，且其目标偏置过程中也并未考虑障碍物信息，因此会产生大量无效采样点以及消耗大量时间进行碰撞检测。

5、论文《RRT与人工势场法结合的机械臂避障规划》将RRT算法与人工势场相结合，当人工势场陷入极小值时以RRT采样生成虚拟目标点来跳出局部极小值。

6、发明专利CN201810008810.X对于人工势场法容易陷入局部极小值问题通过添加虚拟障碍物并改进势场函数使得机械臂能跳出局部极小值完成路径规划。

7、发明专利CN202210973726.8针对局部极小值问题通过设立虚拟目标点以完成路径规划，但其所寻找路径并非最优,且在陷入局部极小值后，机械臂会在此区域产生高震荡，对机械臂造成强烈的物理冲击。

发明内容

本发明提出基于非障碍物空间概率势场采样的RRT机械臂轨迹规划方法，能更高效地生成平滑路径，具有非常高的工业应用价值。

本发明采用以下技术方案。

基于非障碍物空间概率势场采样的RRT机械臂轨迹规划方法，用于含多关节的多连杆机械臂，在非障碍物空间内利用RRT与人工势场相结合进行全局路径规划，所述规划方法包括基于非障碍物空间生成采样点，即通过采样点是否落入障碍物空间来对采样点进行筛选，避免冗余路径的生成以及非必要路径碰撞检测；

所述规划方法还包括概率势场贪婪采样策略，即在保证算法概率完备的前提下赋予算法预设概率的目标导向性以加快路径生成，且通过引力场与斥力场作用来减少路径碰撞检测且生成路径方向相对更优；

所述规划方法还包括冗余节点删除策略，即对算法回溯生成的路径进行优化，删除路径中冗余节点，使得生成路径更优；并通过插值多项式对优化后的路径进行平滑处理，使得路径更符合实际机械臂运动以减少剧烈运动摩擦损耗。

所述规划方法包括以下步骤；

步骤S1：机械臂的数学建模：对机械臂位姿进行描述并建立机械臂连杆坐标系，得到机械臂的D-H模型；

步骤S2：初始化以执行全局路径规划，具体为：在工作空间内初始化树T，给定初始节点q_start＝[x_start，y_start，z_start]、目标节点q_goal＝[x_goal，y_goal，z_goal]、扩展固定步长step、最大迭代次数max、障碍物空间Ω_obs以及目标点误差允许阈值thr；

步骤S3：执行概率势场贪婪采样策略，即采用非障碍物空间概率势场采样生成采样点：设随机概率阈值为P，当产生的随机概率p小于P时，算法进行随机采样，获得随机采样点q_rand＝[x_rand，y_rand，z_rand]；

步骤S4：碰撞检测：将q_near到q_new所生成的路径进行碰撞检测；

步骤S5：冗余节点删除策略：将所得到的路径使用冗余节点删除策略进行路径重规划；

步骤S6：对机械臂的运动轨迹进行优化。

所述步骤S3包括以下步骤；

步骤S3-1：若随机采样点ｑ_rand落入障碍物空间Ω_obs内，则舍弃此点，再次随机采样；

步骤S3-2：若q_rand落在非障碍物空间Ω内，则根据遍历整颗生成树的节点q_T＝[x_T，y_T，z_T]，找到q_T与随机采样点q_rand的欧式距离最近的节点q_near＝[x_near，y_nｅar，z_near]，即：

步骤S3-3：以

为方向按固定步长step生成新的节点，以公式表述为

q_new＝[x_nｅw，Y_new，z_nｅw] 公式二；

步骤S3-4：若采样点随机概率p大于P时，算法进行人工势场导向的贪婪采样；则根据遍历整颗生成树的节点q_T＝[x_T，y_T，z_T]，找到q_T与目标节点q_goal＝[x_goal，y_goal，z_goal]的欧式距离最近节点q_near＝[x_near，y_near，z_near]，即：

步骤S3-5：计算节点q_near所受到的合力势能U：

U＝U_att+U_rep 公式四；

式中，机械臂受到的总引力以公式表述为：

U_rep为机械臂受到的总的斥力：

其中，U_att(i)为机械臂各连杆受到空间内障碍物所产生的引力；U_rep(i)为机械臂各连杆受到空间内障碍物所产生的斥力；n为机械臂连杆受到障碍物斥力的数目；d_i为机械臂对应连杆关节点的实际位置与障碍物的欧式距离；d_o为每个连杆的斥力场作用范围；d_s为每个连杆的安全距离，k_a为引力比例常数，k_r为斥力比例常数；

步骤S3-6：得到合力势能U后，以向量U的方向扩展固定步长step生成新节点q_new＝[x_new，y_new，z_new]，若势场陷入局部极小值，则舍弃此次采样点生成，再次随机采样。

所述步骤S4包括以下步骤；

步骤S4-1：将路径q_nearq_new等步长分为n个点，若其中有一点落入障碍物空间范围内则代表发生碰撞，若未发生碰撞则将q_new存储到随机树T中，将q_new视为q_near的父节点；若检测到发生碰撞则舍弃q_new，回到步骤S3重新生成q_new；

步骤S4-2：重复步骤S3～步骤S4，直到所生成的新节点q_new与q_goal之间的欧氏距离小于阈值thr，即代表路径规划已找到目标点；在随机树T中根据各节点父子关系回溯随机树T找到所规划的路径。

所述步骤S5具体为：

将所得到的路径使用冗余节点删除策略进行路径重规划；把初始路径节点存储于数组nums中，nums＝[q_start,q₂,q₃,…,q_goal]，以nums为基础从初始点q_start到q_goal进行路径重规划，即计算数组中根节点q_root到下一节点q_next欧式距离：

d＝||q_root-q_next|| 公式八；

若其欧式距离d小于等于原本路径长度l：

l＝step*(n-m) 公式九；

n代表在数组nums中q_next此时的下标，m代表在数组nums中q_root此时的下标。若其欧式距离小于原本路径长度且根节点q_root到下一节点q_next的路径不与障碍物发生碰撞，则删除q_root与q_next之间的冗余节点；并将q_next更新为q_next的下一节点；若欧式距离d大于原本路径长度l或根节点q_root到下一节点q_next的路径与障碍物发生碰撞，则将q_root更新为q_next的上一节点,重复上述步骤，直到q_next为q_goal则代表路径重规划完成。

所述步骤S6具体为：

将笛卡尔空间坐标中删除冗余节点后的离散节点通过逆运动学求解，得到机械臂在各离散点的关节角度[θ₁,θ₂,θ₃,θ₄,θ₅,θ₆]；使用多项式插值处理路径节点，机械臂轨迹关节角多项式规划函数如下：

θ_i(t)＝a₀+...+a_itⁱ+...+a_ntⁿ 公式十；

函数式中求出a₀,…,a_i,…,a_n则可确定该函数式；将该函数式对时间求导，得到关节角速度表达式为：

将上式再次对时间求导得到关节角加速度表达式为：

假设机械臂在开始运动时关节角度为θ₀，该关节运动停止时角度为θ_f；

设整段运动时间为t_f，初始关节角速度为

结束关节角速度为

初始角加速度为

结束关节角加速度为

求得多项式系数a₀,…,a_i,…,a_n，代入多项式方程得到平滑的机械臂关节变化角度，完成最终机械臂路径规划。

所述轨迹规划是机械臂避障路径规划，指在工作空间存在障碍物的情况下，机械臂在限定的时间范围内规划出一条无碰撞路径。

本发明的有益效果在于：

(1)本发明提出了一种基于非障碍物空间概率势场采样的方法，即保证了RRT采样的概率完备性，又使用人工势场导向的贪婪采样为算法赋予了一定概率的目标偏置。减少了碰撞检测以及冗余采样路径的生成，缩短了路径生成的时间。

(2)提出了冗余节点删除策略，通过对冗余节点的删除，可以使得路径在长度上更短，更优。

(3)提出了多项式插值轨迹优化，使得生成的最终轨迹更加平滑，适于机械臂运行。

附图说明

下面结合附图和具体实施方式对本发明进一步详细的说明：

附图1是本发明所述机械臂的示意图(以ABB-1200机械臂为例，图中表格为本发明所述机械臂D-H参数表)；

附图2是本发明所述整体工作空间采样示意图；

附图3是本发明所述非障碍物工作空间采样示意图；

附图4是本发明所述删除冗余节点后轨迹示意图；

附图5是本发明所述五次多项式处理后轨迹示意图；

图中：1-连杆；2-关节。

具体实施方式

如图所示，基于非障碍物空间概率势场采样的RRT机械臂轨迹规划方法，用于含多关节2的多连杆1机械臂，在非障碍物空间内利用RRT与人工势场相结合进行全局路径规划，所述规划方法包括基于非障碍物空间生成采样点，即通过采样点是否落入障碍物空间来对采样点进行筛选，避免冗余路径的生成以及非必要路径碰撞检测；

所述规划方法还包括概率势场贪婪采样策略，即在保证算法概率完备的前提下赋予算法预设概率的目标导向性以加快路径生成，且通过引力场与斥力场作用来减少路径碰撞检测且生成路径方向相对更优；

所述规划方法还包括冗余节点删除策略，即对算法回溯生成的路径进行优化，删除路径中冗余节点，使得生成路径更优；并通过插值多项式对优化后的路径进行平滑处理，使得路径更符合实际机械臂运动以减少剧烈运动摩擦损耗。

所述规划方法包括以下步骤；

步骤S1：机械臂的数学建模：对机械臂位姿进行描述并建立机械臂连杆坐标系，得到机械臂的D-H模型；

步骤S2：初始化以执行全局路径规划，具体为：在工作空间内初始化树T，给定初始节点q_start＝[x_start，y_start，z_start]、目标节点q_goal＝[x_goal，y_goal，z_goal]、扩展固定步长step、最大迭代次数max、障碍物空间Ω_obs以及目标点误差允许阈值thr；

步骤S3：执行概率势场贪婪采样策略，即采用非障碍物空间概率势场采样生成采样点：设随机概率阈值为P，当产生的随机概率p小于P时，算法进行随机采样,获得随机采样点q_rand＝[x_rand，y_rand，z_rand]；

步骤S4：碰撞检测：将q_near到q_new所生成的路径进行碰撞检测；

步骤S5：冗余节点删除策略：将所得到的路径使用冗余节点删除策略进行路径重规划；

步骤S6：对机械臂的运动轨迹进行优化。

所述步骤S3包括以下步骤；

步骤S3-1：若随机采样点q_rand落入障碍物空间Ω_obs内，则舍弃此点，再次随机采样；

步骤S3-2：若q_rand落在非障碍物空间Ω内，则根据遍历整颗生成树的节点q_T＝[x_T，y_T，z_T]，找到q_T与随机采样点q_rand的欧式距离最近的节点q_near＝[x_near，y_near，z_near]，即：

步骤S3-3：以

为方向按固定步长step生成新的节点，以公式表述为

q_new＝[x_new，y_new，z_new] 公式二；

步骤S3-4：若采样点随机概率p大于P时，算法进行人工势场导向的贪婪采样；则根据遍历整颗生成树的节点q_T＝[x_T，y_T，z_T]，找到q_T与目标节点q_goal＝[x_goal，y_goal，z_goal]的欧式距离最近节点q_near＝[x_near，y_near，z_near]，即：

步骤S3-5：计算节点q_near所受到的合力势能U：

U＝U_att+U_rep 公式四；

式中，机械臂受到的总引力以公式表述为：

U_rep为机械臂受到的总的斥力：

其中，U_att(i)为机械臂各连杆受到空间内障碍物所产生的引力；U_rep(i)为机械臂各连杆受到空间内障碍物所产生的斥力；n为机械臂连杆受到障碍物斥力的数目；d_i为机械臂对应连杆关节点的实际位置与障碍物的欧式距离；d_o为每个连杆的斥力场作用范围；d_s为每个连杆的安全距离，k_a为引力比例常数，k_r为斥力比例常数；

步骤S3-6：得到合力势能U后，以向量U的方向扩展固定步长step生成新节点q_new＝[x_new，y_new，z_new]，若势场陷入局部极小值，则舍弃此次采样点生成，再次随机采样。

所述步骤S4包括以下步骤；

步骤S4-1：将路径q_nearq_new等步长分为n个点，若其中有一点落入障碍物空间范围内则代表发生碰撞，若未发生碰撞则将q_new存储到随机树T中，将q_new视为q_near的父节点；若检测到发生碰撞则舍弃q_new，回到步骤S3重新生成q_new；

步骤S4-2：重复步骤S3～步骤S4，直到所生成的新节点q_new与q_goal之间的欧氏距离小于阈值thr，即代表路径规划已找到目标点；在随机树T中根据各节点父子关系回溯随机树T找到所规划的路径。

所述步骤S5具体为：

将所得到的路径使用冗余节点删除策略进行路径重规划；把初始路径节点存储于数组nums中，nums＝[q_start,q₂,q₃,…,q_goal]，以nums为基础从初始点q_start到q_goal进行路径重规划，即计算数组中根节点q_root到下一节点q_next欧式距离：

d＝||q_root-q_next|| 公式八；

若其欧式距离d小于等于原本路径长度l：

l＝step*(n-m) 公式九；

n代表在数组nums中q_next此时的下标，m代表在数组nums中q_root此时的下标。若其欧式距离小于原本路径长度且根节点q_root到下一节点q_next的路径不与障碍物发生碰撞，则删除q_root与q_next之间的冗余节点；并将q_next更新为q_next的下一节点；若欧式距离d大于原本路径长度l或根节点q_root到下一节点q_next的路径与障碍物发生碰撞，则将q_root更新为q_next的上一节点,重复上述步骤，直到q_next为q_goal则代表路径重规划完成。

所述步骤S6具体为：

将笛卡尔空间坐标中删除冗余节点后的离散节点通过逆运动学求解，得到机械臂在各离散点的关节角度[θ₁,θ₂,θ₃,θ₄,θ₅,θ₆]；使用多项式插值处理路径节点，机械臂轨迹关节角多项式规划函数如下：

θ_i(t)＝a₀+...+a_itⁱ+...+a_ntⁿ 公式十；

函数式中求出a₀,…,a_i,…,a_n则可确定该函数式；将该函数式对时间求导，得到关节角速度表达式为：

将上式再次对时间求导得到关节角加速度表达式为：

假设机械臂在开始运动时关节角度为θ₀，该关节运动停止时角度为θ_f；

设整段运动时间为t_f，初始关节角速度为

结束关节角速度为

初始角加速度为

结束关节角加速度为

求得多项式系数a₀,…,a_i,…,a_n，代入多项式方程得到平滑的机械臂关节变化角度，完成最终机械臂路径规划。

所述轨迹规划是机械臂避障路径规划，指在工作空间存在障碍物的情况下，机械臂在限定的时间范围内规划出一条无碰撞路径。

实施例：

本例中的步骤S1针对ABB-1200机械臂，如图1所示，由D-H法对其位姿进行描述并建立机械臂连杆坐标系，得到机械臂的D-H模型，如图中的表格所示。

本例在步骤S4的碰撞检测中，将q_near到q_new所生成的路径进行碰撞检测。其中碰撞检测为将路径q_nearq_new等步长分为100个点，若其中有一点落入障碍物空间范围内则代表发生碰撞，若未发生碰撞则将q_new存储到随机树T中，将q_new视为q_near的父节点；若检测到发生碰撞则舍弃q_new，回到步骤S3重新生成q_new。重复步骤S3～步骤S4，直到所生成的新节点q_new与q_goal之间的欧氏距离小于阈值thr，则代表路径规划已找到目标点。在随机树T中根据各节点父子关系回溯随机树T找到所规划的路径。

本例在步骤S5的冗余节点删除策略中，将所得到的路径使用冗余节点删除策略进行路径重规划。初始路径节点存储于数组nums中，nums＝[q_start,q₂,q₃,…,q_goal]，以nums为基础从初始点q_start到q_goal进行路径重规划，即计算数组中根节点q_root到下一节点q_next欧式距离，若其欧式距离小于原本路径长度且根节点q_root到下一节点q_next的路径不与障碍物发生碰撞，则删除q_root与q_next之间的冗余节点。其具体过程为以初始化q_root为q_start,q_next为q₂，若q_root与q_next之间距离小于或等于原本路径长度且不发生碰撞则更新q_next为q₃，删除q_root到q₂之间的节点，若q_root与q_next之间距离大于原本路径长度或发生碰撞则更新q_root为q₂,重复上述步骤，直到q_next为q_goal则代表路径重规划完成。

本例在步骤S6的多项式轨迹优化中，将笛卡尔空间坐标中删除冗余节点后的离散节点通过逆运动学求解，得到机械臂在各离散点的关节角度[θ₁,θ₂,θ₃,θ₄,θ₅,θ₆]。使用多项式插值处理路径节点。机械臂轨迹关节角多项式规划函数如下：

θ_i(t)＝a₀+...+a_itⁱ+...+a_ntⁿ

函数式中求出a₀,…,a_i,…,a_n则可确定该函数式。将该函数式对时间求导，得到关节角速度表达式为：

将上式再次对时间求导得到关节角加速度表达式为：

假设机械臂在开始运动时关节角度为θ₀，该关节运动停止时角度为θ_f。设整段运动时间为t_f，初始关节角速度为

结束关节角速度为

初始角加速度为

结束关节角加速度为

便可求得多项式系数a₀,…,a_i,…,a_n，代入多项式方程便可得到平滑的机械臂关节变化角度，完成最终机械臂路径规划。

Claims (7)

1.基于非障碍物空间概率势场采样的RRT机械臂轨迹规划方法，用于含多关节的多连杆机械臂，在非障碍物空间内利用RRT与人工势场相结合进行全局路径规划，其特征在于：所述规划方法包括基于非障碍物空间生成采样点，即通过采样点是否落入障碍物空间来对采样点进行筛选，避免冗余路径的生成以及非必要路径碰撞检测；

所述规划方法还包括概率势场贪婪采样策略，即在保证算法概率完备的前提下赋予算法预设概率的目标导向性以加快路径生成，且通过引力场与斥力场作用来减少路径碰撞检测且生成路径方向相对更优；

所述规划方法还包括冗余节点删除策略，即对算法回溯生成的路径进行优化，删除路径中冗余节点，使得生成路径更优；并通过插值多项式对优化后的路径进行平滑处理，使得路径更符合实际机械臂运动。

2.根据权利要求1所述的基于非障碍物空间概率势场采样的RRT机械臂轨迹规划方法，其特征在于：所述规划方法包括以下步骤；

步骤S1：机械臂的数学建模：对机械臂位姿进行描述并建立机械臂连杆坐标系，得到机械臂的D-H模型；

步骤S2：初始化以执行全局路径规划，具体为：在工作空间内初始化树T，给定初始节点q_start＝[x_start，y_start，z_start]、目标节点q_goal＝[x_goal，y_goal，z_goal]、扩展固定步长step、最大迭代次数max、障碍物空间Ω_abs以及目标点误差允许阈值thr；

步骤S3：执行概率势场贪婪采样策略，即采用非障碍物空间概率势场采样生成采样点：设随机概率阈值为P，当产生的随机概率p小于P时，算法进行随机采样，获得随机采样点q_rand＝[x_rand，y_rand，z_rand]；

步骤S4：碰撞检测：将q_near到q_new所生成的路径进行碰撞检测；

步骤S5：冗余节点删除策略：将所得到的路径使用冗余节点删除策略进行路径重规划；

步骤S6：对机械臂的运动轨迹进行优化。

3.根据权利要求2所述的基于非障碍物空间概率势场采样的RRT机械臂轨迹规划方法，其特征在于：所述步骤S3包括以下步骤；

步骤S3-1：若随机采样点q_rand落入障碍物空间Ω_obs内，则舍弃此点，再次随机采样；

步骤S3-2：若q_rand落在非障碍物空间Ω内，则根据遍历整颗生成树的节点q_T＝[x_T，y_T，z_T]，找到q_T与随机采样点q_rand的欧式距离最近的节点q_near＝[x_near，y_near，z_near]，即：

步骤S3-3：以

为方向按固定步长step生成新的节点，以公式表述为

q_new＝[x_new，y_new，z_new] 公式二；

步骤S3-4：若采样点随机概率p大于P时，算法进行人工势场导向的贪婪采样；则根据遍历整颗生成树的节点q_T＝[x_T，y_T，z_T]，找到q_T与目标节点q_goal＝[x_goal，y_goal，z_goal]的欧式距离最近节点q_near＝[x_near，y_near，z_near]，即：

步骤S3-5：计算节点q_near所受到的合力势能U：

U＝U_att+U_rep 公式四；

式中，机械臂受到的总引力以公式表述为：

U_rep为机械臂受到的总的斥力：

其中，U_att(i)为机械臂各连杆受到空间内障碍物所产生的引力；U_rep(i)为机械臂各连杆受到空间内障碍物所产生的斥力；n为机械臂连杆受到障碍物斥力的数目；d_i为机械臂对应连杆关节点的实际位置与障碍物的欧式距离；d_o为每个连杆的斥力场作用范围；d_s为每个连杆的安全距离，k_a为引力比例常数，k_r为斥力比例常数；

步骤S3-6：得到合力势能U后，以向量U的方向扩展固定步长step生成新节点q_new＝[x_new，y_new，z_new]，若势场陷入局部极小值，则舍弃此次采样点生成，再次随机采样。

4.根据权利要求3所述的基于非障碍物空间概率势场采样的RRT机械臂轨迹规划方法，其特征在于：所述步骤S4包括以下步骤；

步骤S4-1：将路径q_nearq_new等步长分为n个点，若其中有一点落入障碍物空间范围内则代表发生碰撞，若未发生碰撞则将q_new存储到随机树T中，将q_new视为q_near的父节点；若检测到发生碰撞则舍弃q_new，回到步骤S3重新生成g_new；

步骤S4-2：重复步骤S3～步骤S4，直到所生成的新节点q_new与q_goal之间的欧氏距离小于阈值thr，即代表路径规划已找到目标点；在随机树T中根据各节点父子关系回溯随机树T找到所规划的路径。

5.根据权利要求4所述的基于非障碍物空间概率势场采样的RRT机械臂轨迹规划方法，其特征在于：所述步骤S5具体为：

将所得到的路径使用冗余节点删除策略进行路径重规划；把初始路径节点存储于数组nums中，nums＝[q_start，q₂，g₃，…，q_goal]，以nums为基础从初始点q_start到q_goal进行路径重规划，即计算数组中根节点q_root到下一节点q_next欧式距离：

d＝||q_root-q_next|| 公式八；

若其欧式距离d小于等于原本路径长度l：

l＝step*(n-m) 公式九；

n代表在数组nums中q_next此时的下标，m代表在数组nums中q_root此时的下标。若其欧式距离小于原本路径长度且根节点q_root到下一节点q_next的路径不与障碍物发生碰撞，则删除q_root与q_next之间的冗余节点；并将q_next更新为q_next的下一节点；若欧式距离d大于原本路径长度l或根节点q_root到下一节点q_next的路径与障碍物发生碰撞，则将q_root更新为q_next的上一节点，重复上述步骤，直到q_next为q_goal则代表路径重规划完成。

6.根据权利要求5所述的基于非障碍物空间概率势场采样的RRT机械臂轨迹规划方法，其特征在于：所述步骤S6具体为：

将笛卡尔空间坐标中删除冗余节点后的离散节点通过逆运动学求解，得到机械臂在各离散点的关节角度[θ₁，θ₂，θ₃，θ₄，θ₅，θ₆]；使用多项式插值处理路径节点，机械臂轨迹关节角多项式规划函数如下：

θ_i(t)＝a₀+...+a_itⁱ+...+a_ntⁿ 公式十；

函数式中求出α₀，…，a_i，…，a_n则可确定该函数式；将该函数式对时间求导，得到关节角速度表达式为：

将上式再次对时间求导得到关节角加速度表达式为：

假设机械臂在开始运动时关节角度为θ₀，该关节运动停止时角度为θ_f；设整段运动时间为t_f，初始关节角速度为

结束关节角速度为

初始角加速度为

结束关节角加速度为

求得多项式系数α₀，…，a_i，…，a_n，代入多项式方程得到平滑的机械臂关节变化角度，完成最终机械臂路径规划。

7.根据权利要求1所述的基于非障碍物空间概率势场采样的RRT机械臂轨迹规划方法，其特征在于：所述轨迹规划是机械臂避障路径规划，指在工作空间存在障碍物的情况下，机械臂在限定的时间范围内规划出一条无碰撞路径。

Images