CN113771035B - 基于rrt*算法的冗余多自由度机械臂避障路径优化方法 - Google Patents

Abstract

本发明公开了一种基于RRT*算法的冗余多自由度机械臂避障路径优化方法。本方法针对采用RRT*算法规划出的冗余多自由度机械臂的避障路径存在的路径折角过多和相邻路径折角过小等不利于机械臂运动的因素，首先通过判断两段相邻路径的夹角是否小于角度阈值来判断两段路径的连接点是否为折角过小的路径点；若该路径点属于折角过小点，则通过分别在相邻两段路径上距离折角点三分之一路径长度的位置各新增一个节点，去掉原来的折角点，然后直接连接新增的两个节点的方法，通过此方法可以去除避障路径上所有的折角过小的折角点，有效地解决避障路径上折角过小问题，实现了对避障路径的平滑优化，使得机械臂末端在运行时平稳、顺滑。

Description

基于RRT*算法的冗余多自由度机械臂避障路径优化方法

技术领域

本发明涉及机械臂路径规划领域，具体是一种基于RRT*算法的冗余多自由度机械臂避障路径优化方法。

背景技术

RRT*算法是一种基于RRT算法改进的渐进最优算法，其所找到的路径是被理论验证的正确的算法。采用RRT*(即改进RRT，改进的快速扩展随机树)及基于RRT*改进的相关算法，由于此类算法本身随机性大的问题，其规划出的避障路径通常存在折角过小问题。同时对于冗余机械臂而言，由于其逆解有无数组的特点，将算法应用于冗余自由度机械臂避障规划时，规划出的路径通常会存在路径曲折，即相邻两段路径存在折角过小问题，这非常不利于机械臂末端的实际运行。

发明内容

针对现有技术的不足，本发明拟解决的技术问题是，提供一种基于RRT*算法的冗余多自由度机械臂避障路径优化方法。

本发明解决所述技术问题的技术方案是，提供一种基于RRT*算法的冗余多自由度机械臂避障路径优化方法，其特征在于，该方法包括以下步骤：

步骤1、利用RRT*算法生成冗余多自由度机械臂的原始避障路径X(x₁、x₂、…、x_i…、x_n)，其中x_i为原始避障路径X在三维空间中的路径点；

步骤2、在原始避障路径X中，由依次的x_i-1、x_i和x_i+1三个路径点形成两段相邻路径，判断两段相邻路径所形成的折角点x_i处的折角角度α_xi与角度阈值α_limt的关系；

当折角角度α_xi不小于角度阈值α_limt时，将路径点x_i-1和x_i保存到初步平滑避障路径X_new中；下一次判断中，第一段路径由x_i和x_i+1组成；

当折角角度α_xi小于角度阈值α_limt时，设向量

的模长为l₁，向量

的模长为l₂；在

路径的距离折角点x_i的1/3l₁长度的位置新增一个路径点x_i-1,i，在

路径的距离折角点x_i的1/3l₂长度的位置新增一个路径点x_i,i+1；用两个新增路径点代替原来的折角点x_i，并将x_i-1、x_i-1,i和x_i,i+1三个路径点保存到初步平滑避障路径X_new中；下一次判断中，第一段路径由x_i,i+1和x_i+1组成；

所述角度阈值α_limt由实际情况决定；

步骤3、判断路径点x_i+1是否为原始避障路径X的倒数第二个路径点x_n-1；如果是，则得到初步平滑避障路径X_new；如果不是，则在下一个判断中，对折角点x_i+1重复步骤2，直至对原始避障路径X中的倒数第二个路径点x_n-1重复步骤2，进而得到初步平滑避障路径X_new；

步骤4、将初步平滑避障路径X_new进行改进B样条曲线拟合，得到平滑避障路径。

与现有技术相比，本发明有益效果在于：

(1)本发明针对采用RRT*算法规划出的冗余多自由度机械臂的避障路径存在的路径折角过多和相邻路径折角过小等不利于机械臂运动的因素，首先通过判断两段相邻路径的夹角是否小于角度阈值来判断两段路径的连接点是否为折角过小的路径点；若该路径点属于折角过小点，则通过分别在相邻两段路径上距离折角点三分之一路径长度的位置各新增一个节点，去掉原来的折角点，然后直接连接新增的两个节点的方法，通过此方法可以去除避障路径上所有的折角过小的折角点，有效地解决避障路径上折角过小问题，实现了对避障路径的平滑优化，使得机械臂末端在运行时平稳、顺滑，同时为后续的路径进一步优化提供了良好的基础。

(2)本发明新增路径点的位置是距离折角点三分之一路径长度，主要考虑均匀性问题，如果相邻的三段路径形成的两个折角都不满足角度阈值条件，那么中间的路径取其原长的三分之一不至于使得该段路径过长或者过短，过短优化效果不明显，过长可能会使得机械臂在运行时与障碍物碰撞。

附图说明

图1为本发明的优化方法流程图；

图2为本发明的路径平滑原理图；

图3为本发明实施例1的空间障碍物模型及基于RRT*算法生成的无碰撞原始避障路径图；

图4为本发明实施例1的原始避障路径X图；图中，三角形代表的点为需要去除的折角点；

图5为本发明实施例1的初步平滑避障路径X_new图；图中，圆形代表的点为新增的路径点；

图6为本发明实施例1的原始避障路径X和初步平滑避障路径X_new经过改进B样条曲线拟合后生成的路径的对比图。

具体实施方式

下面给出本发明的一个具体实例。具体实施例仅用于进一步详细说明本发明，不限制本申请权利要求的保护范围。

本发明提供了一种基于RRT*算法的冗余多自由度机械臂避障路径优化方法(简称方法)，其特征在于，该方法包括以下步骤：

步骤1、利用RRT*算法生成冗余多自由度机械臂的从起点到终点的无碰撞原始避障路径X(x₁、x₂、…、x_i…、x_n)，其中x_i为原始避障路径X在三维空间中的路径点；

步骤2、在原始避障路径X中，由依次的x_i-1、x_i和x_i+1三个路径点形成两段相邻路径，判断两段相邻路径所形成的折角点x_i处的折角角度α_xi与角度阈值α_limt的关系；

当折角角度α_xi不小于角度阈值α_limt时，保留两段路径中包含的路径点x_i-1和x_i并将路径点x_i-1和x_i保存到初步平滑避障路径X_new中；下一次判断中，第一段路径由x_i和x_i+1组成；

当折角角度α_xi小于角度阈值α_limt时，设向量

的模长为l₁，向量

的模长为l₂；在

路径的距离折角点x_i的1/3l₁长度的位置新增一个路径点x_i-1,i，在

路径的距离折角点x_i的1/3l₂长度的位置新增一个路径点x_i,i+1；用两个新增路径点代替原来的折角点x_i，并将x_i-1、x_i-1,i和x_i,i+1三个路径点保存到初步平滑避障路径X_new中；下一次判断中，第一段路径由x_i,i+1和x_i+1组成；

所述角度阈值α_limt由实际情况决定；

优选地，步骤2中，所述角度阈值α_limt如此确定：原始避障路径X中存在的n个路径点形成的n-1段路径，n-1段路径形成n-2个折角，则角度阈值α_limt取n-2个折角角度的平均值。

优选地，步骤2中，折角角度

优选地，步骤2中，新增路径点x_i-1i的坐标的计算公式是

新增路径点x_i,i+1的坐标的计算公式是

步骤3、判断路径点x_i+1是否为原始避障路径X的倒数第二个路径点x_n-1；如果是，则得到初步平滑避障路径X_new；如果不是，则在下一个判断中，对折角点x_i+1重复步骤2，直至对原始避障路径X中的倒数第二个路径点x_n-1重复步骤2，进而得到初步平滑避障路径X_new；

步骤4、将初步平滑避障路径X_new进行改进B样条曲线拟合，得到平滑避障路径。

优选地，步骤4中，改进B样条曲线拟合采用文献《董甲甲,王太勇,董靖川,张永宾,陶浩.改进B样条曲线应用于6R机器人轨迹优化[J].中国机械工程,2018,29(02):193-200.》中所述的方法。

实施例1

本实施例中，利用本方法优化基于RRT*算法规划出的七自由度机械臂避障路径，包括以下步骤：

步骤1、初始化参数和生成路径；

(1)根据D-H法建立连杆坐标系，进而建立七自由度机械臂的运动学模型；

表1D-H参数表

(2)由D-H法建立的连杆坐标系的齐次变换矩阵为：

上式中，R_i-1为旋转矩阵，T_i-1为平移矩阵，cθ_i为cosθ_i，cα_i-1为cosα_i-1，sθ_i为sinθ_i，sα_i-1为sinα_i-1，α_i-1为连杆转角，a_i-1为连杆长度，d_i为连杆偏距，θ_i为关节角；

由连杆变换关系可得，机械臂底座上的坐标系到末端执行器坐标系的正运动学解：

由于七自由度机械臂的冗余性，使得其逆解有无数组；由于几何法求逆解的精度高并且计算效率高，所以本发明采用改变臂型角的求逆解方法，选取其中角度变化最小的一组，同时需要满足关节角约束；

优选地，所述改变臂型角的求逆解方法采用文献《高春艳,唐家豪,吕晓玲,张明路.一种冗余机械臂的简化运动学求解方法[J/OL].机械科学与技术:1-5[2021-09-23].》中公开的方法。

(3)初始化环境参数：机械臂末端在工作空间中的起点位姿是start[1 0 0 100；01 0 300；0 0 1 200；0 0 0 1]，终点位姿是goal[1 0 0 551；0 1 0 563；0 0 1 500；0 0 01]；四个障碍物的模型和位置分别是obs1(391,376,300)、obs2(300,450,550)、obs3(300,600,400)和obs4(300,450,400)，边长分别为120mm、100mm、100mm和100mm；碰撞检测方法采用基于OBB包围盒分离轴的碰撞检测；

(4)利用RRT*算法生成七自由度机械臂的从起点到终点的无碰撞原始避障路径X(x₁、x₂、…、x_i…、x_n)，如图3所示，路径由一系列路径点组成，其中x_i为原始避障路径X在三维空间中的路径点，具体过程是：首先，以起点start为随机树的根节点，初始化随机树。然后在采样空间中生成随机采样点n_rand，遍历随机树，找到距离n_rand最近的点n_near。以n_near为起点，沿着n_rand方向生成新节点n_new。新节点n_new是否无碰撞，若无碰撞则继续进行以下过程，若碰撞则返回重新采样。当n_new无碰撞时，检测是否存在逆解，若存在则继续进行以下过程，若不存在则返回重新采样。当n_new无碰撞且有逆解时，检测机械臂的连杆与障碍物是否发生碰撞，若无碰撞则继续进行以下过程，若碰撞则返回重新采样。当满足以上条件时，遍历随机树，找到n_new的邻域内代价最小节点n_nearest作为n_new的父节点。判断n_new是否到达目标点邻域，若到达则按照父节点的顺序，找到一条从起点到终点的原始避障路径X，若未到达则返回重新采样，执行以上过程。

步骤2、在原始避障路径X中，由依次的x_i-1、x_i和x_i+1三个路径点形成两段相邻路径，向量

代表第一段路径，向量

代表第二段路径，x_i为两段相邻路径所形成的折角点；向量

与向量

之间的夹角α_xi代表两段相邻路径所形成的折角点x_i处的折角角度；

当折角角度α_xi不小于角度阈值α_limt时，保留两段路径中包含的路径点x_i-1和x_i并将路径点x_i-1和x_i保存到初步平滑避障路径X_new中；下一段路径由x_i和x_i+1组成；

当折角角度α_xi小于角度阈值α_limt时，设向量

的模长为l₁，向量

的模长为l₂；在

路径的距离折角点x_i的1/3l₁长度的位置新增一个路径点x_i-1,i，在

路径的距离折角点x_i的1/3l₂长度的位置新增一个路径点x_i,i+1；用两个新增路径点代替原来的折角点x_i，并将x_i-1、x_i-1,i和x_i,i+1三个路径点保存到初步平滑避障路径X_new中；下一段路径由x_i,i+1和x_i+1组成；

本实施例中，将原始避障路径X所形成的28个路径转角取平均值，得到角度阈值α_limt为48.3°；

步骤3、判断路径点x_i+1是否为倒数第二个路径点x_n-1；如果是，则得到初步平滑避障路径X_new；如果不是，则对原始避障路径X中的下一个折角点x_i+1重复步骤2，直至对原始避障路径X中的倒数第二个路径点x_n-1重复步骤2，进而得到初步平滑避障路径X_new；

步骤4、将初步平滑避障路径X_new进行改进B样条曲线拟合，得到平滑避障路径。

由图4和图5对比可以看出，图5去掉了图4中折角过小的折角点，并用两个新增路径点代替需要去掉的对应的一个折角点，通过新增的路径点使得路径变得顺滑。同时由于基于RRT*算法生成的路径步长本身较小，所以本发明方法取距离折角点1/3路径长度的位置处增加路径点不会导致机械臂与障碍物发生碰撞，所以平滑后的路径也是无碰撞路径。

图6中，点划线表示将原始避障路径X进行改进B样条曲线拟合后生成的路径，实线表示将初步平滑避障路径X_new进行改进B样条曲线拟合后得到平滑避障路径。由图6可以看出，本发明得到的平滑避障路径去掉了由RRT*算法规划出来的路径中的折角过小的节点，使得路径更加平滑，符合机械臂实际的实际运动路径，保证机械臂末端的平稳运行。

本发明未述及之处适用于现有技术。

Claims (4)

1.一种基于RRT*算法的冗余多自由度机械臂避障路径优化方法，其特征在于，该方法包括以下步骤：

步骤1、利用RRT*算法生成冗余多自由度机械臂的原始避障路径X(x₁、x₂、…、x_i…、x_n)，其中x_i为原始避障路径X在三维空间中的路径点；

步骤2、在原始避障路径X中，由依次的x_i-1、x_i和x_i+1三个路径点形成两段相邻路径，判断两段相邻路径所形成的折角点x_i处的折角角度α_xi与角度阈值α_limt的关系；

当折角角度α_xi不小于角度阈值α_limt时，将路径点x_i-1和x_i保存到初步平滑避障路径X_new中；下一次判断中，第一段路径由x_i和x_i+1组成；

当折角角度α_xi小于角度阈值α_limt时，设向量

的模长为l₁，向量

的模长为l₂；在

路径的距离折角点x_i的1/3l₁长度的位置新增一个路径点x_i-1,i，在

路径的距离折角点x_i的1/3l₂长度的位置新增一个路径点x_i,i+1；用两个新增路径点代替原来的折角点x_i，并将x_i-1、x_i-1,i和x_i,i+1三个路径点保存到初步平滑避障路径X_new中；下一次判断中，第一段路径由x_i,i+1和x_i+1组成；

所述角度阈值α_limt由实际情况决定；

步骤3、判断路径点x_i+1是否为原始避障路径X的倒数第二个路径点x_n-1；如果是，则得到初步平滑避障路径X_new；如果不是，则在下一个判断中，对折角点x_i+1重复步骤2，直至对原始避障路径X中的倒数第二个路径点x_n-1重复步骤2，进而得到初步平滑避障路径X_new；

步骤4、将初步平滑避障路径X_new进行改进B样条曲线拟合，得到平滑避障路径。

2.根据权利要求1所述的基于RRT*算法的冗余多自由度机械臂避障路径优化方法，其特征在于，步骤2中，原始避障路径X中存在的n个路径点形成的n-1段路径，n-1段路径形成n-2个折角，则角度阈值α_limt取n-2个折角角度的平均值。

3.根据权利要求1所述的基于RRT*算法的冗余多自由度机械臂避障路径优化方法，其特征在于，步骤2中，折角角度

4.根据权利要求1所述的基于RRT*算法的冗余多自由度机械臂避障路径优化方法，其特征在于，步骤2中，新增路径点x_i-1,i的坐标的计算公式是

新增路径点x_i,i+1的坐标的计算公式是

Images