CN112356033B - 一种融合低差异序列与rrt算法的机械臂路径规划方法 - Google Patents

Abstract

本发明公开一种融合了低差异序列与RRT算法的机械臂路径规划方法，包括初始化工作环境、关节角空间采样、生成新节点及路径优化处理。本发明使用低差异的序列代替RRT算法中的伪随机序列，从而生成均匀差异的采样点，避免了采样点重复；同时为了进一步加快搜索速度引入目标偏向策略使得该算法在搜索过程中具有一定的目标性减少了对无效区域的搜索；接着在采样过程中建立采样池，对采样点进行优选提高采样点质量；将本发明应用在机械臂关节空间中进行路径规划避免了逆运动学求解减小了计算量。

Description

一种融合低差异序列与RRT算法的机械臂路径规划方法

技术领域

本发明涉及机器人路径规划领域，特别涉及一种融合低差异序列与RRT算法的机械臂路径规划方法。

背景技术

废弃矿区土地复垦是缓解农业用地，改善矿区环境的重要途经，但其环境恶劣可能存在一些重金属、未知塌陷、环境污染等潜在危险，采用移动机器人进行无人化土壤采样是矿区修复的首要环节。机械臂作为移动机器人的执行机构承担着土壤采样的重要任务，由于矿区环境复杂存在未知障碍物，因此需要对机械臂进行路径规划以避开障碍物到达目标点。随着机器人学的发展，国内外学者已经提出许多路径规划算法，例如人工势场法、A*算法以及RRT算法等，人工势场法和A*算法在路径规划是虽然能够满足最优性和实时性要求，但人工势场法极易陷入局部极小值，A*算法存在内存开销大，运行时间长，累计转折角度大等问题。RRT算法是一种基于随机采样的路径规划方法，其不需要对状态空间进行预处理，具有搜索速度快，便于处理约束等特性，能有效解决复杂环境下路径规划的问题，近年来被广泛应用于机器人路径规划领域。但标准RRT也存在一些固有的缺陷：(1)全局的随机采样导致算法无谓地耗费较大代价，使得收敛速度变慢；(2)算法的随机性会导致生成的路径不平滑，难以被机器人直接执行。

发明内容

本发明的目的在提供一种融合低差异序列与RRT算法的机械臂路径规划方法，用于提高机械臂的路径规划效率，并优化规划出的路径。

为了实现上述目的，本发明提供的融合低差异序列与RRT算法的机械臂路径规划方法，包括以下步骤：

步骤一：初始化工作环境

建立机械臂的运动学模型并确定机械臂的运动空间和障碍物Obstacle；获取机械臂状态空间中的路径起点(起始点)x_start和路径终点(目标点)x_goal，初始化随机树T_node，并确定RRT算法参数，主要包括：

(1)目标偏置概率p，使采样点以一定的概率p等于目标点，降低算法的盲目性。

(2)采样池容量n，每次生成n个采样点放入采样池中选取采样池中最优的采样点生成新节点x_new。

(3)步长step，算法根据最近节点、采样点的位置以及步长step确定新节点。

(4)允许误差error，当新节点与目标节点的欧式距离小于允许误差error时认为路径搜索完成。

步骤二：关节角空间采样

设定目标偏置概率p，并在区间范围(0，1)之间生成一个随机数rand；比较该随机数与偏置概率p的大小，如果rand≤p不再进行采样直接令采样点x_sample等于目标点(路径终点)x_goal，反之使用低差异Sobol序列生成n个具有均匀差异的采样点按与目标点(路径终点)x_goal的欧式距离远近，从小到大排列进入采样池。

步骤三：生成新节点

3.1寻找随机树T_node中距离采样点x_sample最近的一个点作为最近节点x_nearest；

3.2在最近节点x_nearest到采样点x_sample的连线方向上以步长step前进生成新节点x_new；

3.3进行碰撞检测，若检测到碰撞则重新选取采样点生成新节点，若没有检测到碰撞则将新节点x_new存储到随机树T_node，且最近节点x_nearest视为新节点x_new的父节点；

3.4根据更新后的随机树T_node，重复步骤二～步骤三，直到得到的最新的节点x_new满足|x_new-x_goal|≤error，视为找到目标点，error是指允许误差；

3.5在随机树T_node中根据各节点的父子关系，反向搜索找到规划路径。

步骤四：路径优化处理

将步骤三得到的路径使用基于最小二乘法的五次多项式拟合方法拟合出六条光滑曲线。

作为改进，在步骤二中，Sobol序列是一种比伪随机序列分布更加均匀的序列，是一组基于“直接数”{v_i}所形成的序列，

m_i＜2ⁱ且为正奇数；v_i，m_i的生成依赖于系数为a、阶数为p的简单多项式f(z)，多项式系数只为0或1，形式为：

f(z)＝z^p+a₁z^p-1+…+a_p-1z+a_p

当i＜p时需要指定m_i的值，而当i＞p时，m_i递推公式为：

其中

表示向下取整，

表示二进制按位异或，同样对于v_i的递归公式有：

从而可以得到Sobol序列的第i个数：

其中…b₃b₂b₁是i的二进制表示。

采样点由机械臂各个关节角组成，每一个关节角需要一个Sobol序列生成。通过改变多项式f(z)的系数，可以得到不同的Sobol序列。得到的Sobol序列均匀分布于(0,1)空间，采样点中的一个关节角可以表示为min+(max-min)*x_i其中min和max表示关节角上下限。

作为改进，所述步骤3.3，具体为：判断x_new对应的机械臂姿态是否与障碍物碰撞，在发生碰撞的情况下若当前采样点等于路径终点则转到步骤二重新采样，若当前采样点不等于路径终点则舍弃该采样点按顺序选择采样池中下一个采样点并转到步骤三生成新节点x_new，除非采样池中采样点生成的新节点都不满足要求再重新进行采样。在不发生碰撞的情况下将新节点x_new加入随机树T_node中并定义x_nearest为x_new的父节点。

有益效果：本发明使用低差异的序列代替RRT算法中的伪随机序列，从而生成均匀差异的采样点，避免了采样点重复；同时为了进一步加快搜索速度引入目标偏向策略使得该算法在搜索过程中具有一定的目标性减少了对无效区域的搜索；接着在采样过程中建立采样池，对采样点进行优选提高采样点质量；将本发明应用在机械臂关节空间中进行路径规划避免了逆运动学求解减小了计算量。

附图说明

图1为本发明提供的融合低差异序列与RRT算法的机械臂路径规划方法的搜索扩展示意图。

图2为本发明实例使用的AUBO-i5机械臂。

图3为本发明提供的机械臂仿真环境示意图。

具体实施方式

下面将结合附图对本发明作详细说明。

如图1所示，一种融合低差异序列与RRT算法的机械臂路径规划方法，包括以下步骤：

1、对于如图2所示的AUBO-i5机械臂，通过D-H法描述其位姿，建立机械臂的连杆坐标系，确定机械臂D-H参数，然后计算得到机械臂的D-H模型。连杆坐标系是用来描述连杆之间、杆件与关节之间的位移关系，得出末端坐标系相对于基坐标系的齐次变换矩阵。齐次变换矩阵是各关节变量的函数，用于从机械臂的关节空间到笛卡尔空间的求运动学正解。运动学正解是机械臂的关节空间到笛卡尔空间的映射，由此可求出在给定机械臂姿态和关节角下，机械臂的末端位姿，从而实现路径规划。如图3所示，连杆坐标系的建立规则为：

(1)z_i为关节i的运动轴正方向。

(2)x_i轴垂直于z_i-1轴并指向z_i-1z_i轴的方向。

(3)y_i轴根据右手定则建立。

建立相应的D-H参数表，如表1所示。其中参数α和d表示机械臂连杆长度参数。表中：α_i-1是从Z_i-1到Z_i沿着x_i-1的距离；d_i是从x_i-1到x_i沿着Z_i的距离；a_i-1是从Z_i-1到Z_i绕着x_i-1旋转的角度；θ_i是从x_i-1到x_i绕着Z_i旋转的角度。

表1 D-H参数表

2、机械臂路径规划是在其关节空间进行规划，AUBO-i5各关节角范围均为[-π,π]，初始关节角(路径起点)x_start和目标关节角(路径终点)x_goal分别为[0,0,0,0,0,0]，[-135,-90,55,60,100,40]，目标偏置概率p为0.3，步长step为6，采样池容量大小为10个采样点、允许误差error＝6；误差定义为当前关节角与与目标关节角的欧式距离。障碍物和机械臂连杆使用规则的几何包络体包裹，用以简化碰撞判断条件。

3、在(0,1)空间生成随机数rand。

4、若随机数rand≤0.3，则不进行采样，直接令采样点x_sample等于目标关节角x_goal。

5、若随机数rand＞0.3，使用低差异Sobol序列生成10个采样点x_sample，按与目标点欧式距离远近从小到大放入采样池中。本实施例使用的AUBO-i5机械臂具有6个自由度，因此需要生成6列Sobol序列。Sobol序列是一组基于“直接数”{v_i}所形成的序列，

m_i＜2ⁱ且为正奇数；v_i，m_i的生成依赖于系数为a、阶数为p的简单多项式f(z)，多项式系数只为0或1，形式为：

f(z)＝z^p+a₁z^p-1+…+a_p-1z+a_p

当i＜p时需要指定m_i的值；当i＞p时，m_i递推公式为：

其中

表示向下取整，

表示二进制按位异或，同样对于v_i的递归公式有：

从而可以得到Sobol序列的第i个数：

其中…b₃b₂b₁是i的二进制表示。

改变多项式f(z)的系数，可以得到不同的Sobol序列。得到的Sobol序列均匀分布于(0,1)空间，采样点中的一个关节角可以表示为min+(max-min)*x_i其中min＝-π和max＝π表示关节角上下限。

6、在随机树中选择距离采样点x_sample最近的点作为最近节点x_nearest。

7、以一定的步长step＝6在最近节点x_nearest与采样点x_sample连线方向上扩展新节点

8、判断x_new对应的机械臂姿态是否与障碍物碰撞，在发生碰撞的情况下若采样点x_sample等于目标点x_goal则舍弃采样点x_sample转到步骤3，若采样点x_sample是从采样池中获取的则舍弃该采样点并按顺序选择下一个采样点转到步骤6生成新节点x_new，除非采样池中采样点耗尽再重新进行采样。在不发生碰撞的情况下将新节点x_new加入随机树T_node中并定义最近节点x_nearest为新节点x_new的父节点。

9、根据更新后的随机树T_node，重复步骤3-8，直到得到的最新的节点x_new满足|x_new-x_goal|≤error，视为找到目标点，error是指允许误差。

10、在随机树T_node中根据各节点的父子关系，反向搜索找到规划路径。

11、由于算法的随机性导致规划出的路径并不平滑无法直接应用于机械臂，首先将规划出的关节角轨迹使用基于最小二乘法的五次多项式拟合方法拟合出光滑曲线，其中一次拟合结果为：

y₁＝-0.003x⁴-0.02x³+0.5719x²-7.3594x+10.2835 (1)

y₂＝-0.004x⁴-0.0256x³+0.6914x²-7.5403x+12.4140 (2)

y₃＝-0.001x⁴-0.0022x³+0.0326x²+0.7793x-3.8404 (3)

y₄＝-0.0004x³+0.0401x²+1.1348x-4.8081 (4)

y₅＝-0.0001x⁴+0.0078x³-0.1672x²+3.1304x-7.3039 (5)

y₆＝0.0002x⁴-0.0118x³+0.3291x²-2.4139x+8.1399 (6)

从给定初始关节角(初始点)x_start到目标关节角(目标点)x_goal按拟合出的五次多项式y₁…y₆均匀采样得到能使机械臂平滑运行的关节角采样点。将关节角采样点按机械臂控制器的通讯协议通过串口发送控制指令。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下还可以做出若干改进，这些改进也应视为本发明的保护范围。

Claims (2)

1.一种融合低差异序列与RRT算法的机械臂路径规划方法，其特征在于，包括以下步骤：

步骤一：初始化工作环境

建立机械臂的运动学模型并确定机械臂的运动空间和障碍物Obstacle；获取机械臂状态空间中的起始点x_start和目标点x_goal；初始化随机树T_node；并确定RRT算法参数：目标偏置概率p、采样池容量n、步长step及允许误差error；

步骤二：关节角空间采样

2.1设定目标偏置概率p，并在区间范围(0，1)之间生成随机数rand；

2.2若rand≤p，不再进行采样，令采样点x_sample等于目标点x_goal；

2.3若rand>p使用Sobol序列生成n个采样点x_sample按与目标点x_goal欧式距离的远近，从小到大排列进入采样池；

步骤三：生成新节点

3.1寻找随机树T_node中距离采样点x_sample最近的一个点作为最近节点x_nearest；

3.2在最近节点x_nearest到采样点x_sample的连线方向上以步长step前进生成新节点x_new；

3.3进行碰撞检测，判断生成的新节点x_new对应的机械臂位姿是否与障碍物发生碰撞；在与障碍物发生碰撞的情况下，若当前采样点x_sample是由步骤2.2得到的，转到步骤二重新采样；若当前采样点x_sample是由步骤2.3得到的，舍弃该采样点按顺序选择采样池中下一个采样点x_sample并转到步骤三生成新节点x_new，除非采样池中采样点x_sample生成的新节点x_nearest都不满足要求再重新进行采样；若没有检测到碰撞则将新节点x_new存储到随机树T_node，且最近节点x_nearest视为新节点x_new的父节点；

3.4根据更新后的随机树T_node，重复步骤二～步骤三，直到得到的最新的节点x_new满足|x_new-x_goal|<error，视为找到目标点；

3.5在随机树T_node中根据各节点的父子关系，反向搜索找到规划路径；

步骤四：路径优化处理

将步骤三得到的路径使用基于最小二乘法的五次多项式拟合方法拟合出光滑曲线，采样光滑曲线上的点并按机械臂控制器的通讯协议通过串口发送控制指令。

2.根据权利要求1所述的融合低差异序列与RRT算法的机械臂路径规划方法，其特征在于：在所述步骤二中，所述Sobol序列是一种比伪随机序列分布更加均匀的序列，是一组基于{v_i}所形成的序列，

m_i<2ⁱ且为正奇数；v_i，m_i的生成依赖于系数为a、阶数为p的简单多项式f(z)，多项式系数只为0或1，形式为：

f(z)＝z^p+a₁z^p-1+…+a_p-1z+a_p

当i<p时需要指定m_i的值，而当i>p时，m_i递推公式为：

其中

表示二进制按位异或，对于v_i的递归公式为：

其中

表示向下取整。

得到Sobol序列的第i个数：

其中…b₃b₂b₁是i的二进制表示；

所述采样点x_sample由机械臂各个关节角组成，每一个关节角由一个Sobol序列生成；通过改变多项式f(z)的系数，得到不同的Sobol序列；得到的Sobol序列均匀分布于(0,1)空间；采样点x_sample中的一个关节角表示为min+(max-min)*x_i，其中min和max表示关节角上下限。

Images