CN111761582B - 一种基于随机采样的移动机械臂避障规划方法 - Google Patents

Abstract

本发明公开了一种基于随机采样的移动机械臂避障规划方法。本发明首先将移动平台建模成一个由x、y方向的移动关节上接一个旋转关节组成的特殊的机械臂，并将其与原有的机械臂模型组合，建立n自由度移动机械臂的正运动学模型。然后对运动规划过程中的约束进行分析，然后设计移动机械臂车‑臂一体化路径规划算法流程。最后设计用于路径平滑处理的双向删除冗余路径方法，对上一步得到的路径做平滑处理。本发明将整个移动机械臂建模成含有移动关节的特殊机械臂，极大简化了运动学求解复杂度。同时设计了双向删除冗余路径算法，用较低的计算复杂度很大程度地缩减和平滑了RRT算法规划出的路径。

Description

一种基于随机采样的移动机械臂避障规划方法

技术领域

本发明属于路径规划领域，具体来说是一种面向移动机械臂的基于随机采样算法的避障路径规划方法，可以在指定大小和障碍物的场地上为移动机械臂快速规划出障碍物避让的可行路径。

背景技术

移动机械臂既具有移动平台的移动性，又具有机械手的操作性，这类平台广泛应用于不同的工业和生产领域。相比于传统的机器人主要集中于特定研究方向，例如感知、规划、控制及人工智能等，移动机械臂系统更倾向于能够在实际环境中完成更大范围任务的综合系统。

由于没有考虑移动平台和机械臂这两个系统间的交互作用，且移动平台和机械臂的结合会引入高度的运动学冗余度，现有的移动平台或机械臂的规划方法并不能直接复制于移动机械臂之中。因此移动平台和机械臂的协调规划问题不可忽视，需要针对移动机械臂的特征，进行车-臂一体化的运动规划以进行有效的避障和任务执行。

目前针对机器人的运动规划方法大致可分为基于图的运动规划方法和基于随机采样的运动规划方法。A*算法是最常见的基于图的运动规划方法，这类方法通常在高维复杂空间计算效率很低，因此并不适用于移动机械臂的运动规划。快速拓展随机树(RRT)算法是最常见的一类基于随机采样的运动规划方法，这类方法的特点在于速度快，且运算速度不会因维度提升而显著下降，因此常用于冗余机械臂的规划，然而将其运用于移动机械臂的车-臂一体化运动规划却较少涉及。为此，本发明提出了一种基于RRT-GoalBias算法的移动机械臂运动规划方法，能够快速规划出障碍物避让的可行路径。

发明内容

本发明的目的在于提出一种面向移动机械臂的基于随机采样算法的避障路径规划，用以解决移动机械臂在给定场地下快速避障规划的问题。

本发明包括以下步骤：

步骤1：将移动平台建模成一个由x、y方向的移动关节上接一个旋转关节组成的特殊的机械臂，并将其与原有的机械臂模型组合，建立n自由度移动机械臂的正运动学模型为：

其中，移动平台的位姿表示为q_b＝[x,y,θ₀]^T，机械臂的关节转角向量为q_m＝[θ₁,θ₂,…,θ_n-3]^T，移动机械臂位姿表示为q_i＝[q_b ^T,q_m ^T]^T。

为移动机械臂的车-臂一体化变换矩阵，

为世界坐标系到移动平台坐标系的状态转换矩阵，

为第i-1个关节到第i个关节的状态转换矩阵。

步骤2：对运动规划过程中的约束进行分析，然后设计移动机械臂车-臂一体化路径规划算法流程，具体为：

1)目标约束

移动机械臂规划路径的初始和终止位姿须等于规定的起始位姿和目标位姿，表示为：

q_init＝q_start

q_end＝q_goal

2)位姿约束

针对每一个关节转角以及移动平台的方位角，都有：

θ_jmin≤θ_j≤θ_jmax,j＝0,2,3,...n-3

其中，θ_jmin和θ_jmax分别表示第j个关节的角度上确界和下确界；

针对移动平台x、y方向上的坐标，有：

l_min≤x_i,y_i≤l_max

对于移动机械臂的整个路径规划过程，位姿约束为：

q_min≤q_i≤q_max,i＝1,2,3,...N

其中，q_min＝[l_min,l_min,θ_0min,...,θ_(n-3)min]^T为位姿向量的下限，

q_max＝[l_max,l_max,θ_0max,...,θ_(n-3)max]^T为位姿向量的上限。

3)碰撞约束

将移动机械臂进行碰撞检测建模，用若干个球体建模移动机械臂，其中机械臂上球体的球心分别位于各个关节处。由此，在碰撞检测中，给定与障碍物表面上点的最小距离d_min，需要保证在每一个节点上，对于移动机械臂上的每个点M_i，都有：

d(M_i,B_j)≥d_min+R

其中，B_j为障碍物表面上的标记点，d(M_i,B_j)表示移动机械臂上的每个点到障碍物表面上的标记点的距离，R为球体的半径。

由此，设计移动机械臂车-臂一体化路径规划算法流程如下：

随机树的扩展和构建：

Step 1初始化随机树，定义关节空间C₁关节约束范围和任务空间C₂场地约束范围。

Step 2从初始位姿q_init出发，将其添加至随机树T中。

Step 3生成(0,1)上的随机数r，并将其与规定的概率值P_goal做比较，若r>P_goal，则在关节空间C₁中进行随机采样，得到状态点q_rand，反之则令q_rand＝q_goal。

Step 4在随机树T上找到距离q_rand最近的点q_near。

Step 5将q_near向q_rand前进一个步长，得到新节点q_new。

Step 6将新节点q_new正运动学求解投影到任务空间C₂中，并解算出各个关节和移动平台上圆球的球心坐标，在任务空间C₂中对其进行碰撞检测，若无碰撞，则在关节空间C₁中将对应的新节点q_new添加至随机树T中，反之则舍弃该节点。

Step 7重复Step 3～6，迭代扩展随机树T，直至有节点接近或到达目标位姿q_goal或者超出规定的迭代次数，则扩展停止。

路径查询：

通过回溯法从q_goal到q_init找到一条完整的路径，该算法结束。

步骤3：设计用于路径平滑处理的双向删除冗余路径方法，对上一步得到的路径做平滑处理，算法的思想是在避障的前提下，最大程度地用直线替换冗余路径，正向平滑一次的基础上，再反向平滑一次，算法流程如下：

Step 1从初始位姿开始查找，定义其为出发点q₀。

Step 2依次将后续的路径点与出发点q₀连接。

Step 3对所连直线路径进行碰撞检测，若无障碍，则将下一个路径点与出发点q₀连接并做碰撞检测，直至所连的直线路径上有障碍。

Step 4连接上一路径点q_last与出发点q₀形成直线路径，替换冗余路径。

Step 5将上一路径点q_last设置为新的出发点q₀。

Step 6循环Step 2～5，直至完成所有路径点的遍历。

Step 7在上述生成路径的基础上，将终止位姿q_end设置为新出发点，反向遍历路径点，重复Step 2～5，直至完成所有路径点的遍历。

本发明的有益效果主要表现在：本发明给出了一种移动机械臂车-臂一体化建模的方式，将整个移动机械臂建模成含有移动关节的特殊机械臂，极大简化了运动学求解复杂度。拓展了传统的RRT-GoalBias算法，将其应用于移动机械臂的运动规划，在关节空间拓展随机树，运动学求解投影到任务空间，并解算出移动机械臂上每个点M_i，对它们分别做碰撞检测。并设计了双向删除冗余路径算法，用较低的计算复杂度很大程度地缩减和平滑了RRT算法规划出的路径。

附图说明

图1是本发明提出的针对移动机械臂的车-臂一体化规划方法的流程图；

图2是双向删除冗余路径算法的示意图；

图3是移动机械臂的仿真图；

图4是在给定大小和障碍物的场地下对移动机械臂的路径规划的仿真结果。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

现结合实施例、图1～图4对本发明进行进一步描述：

本发明的实施技术方案为：

1)建立移动机械臂运动学模型：

移动机械臂移动平台参数如下：

底盘参数表

将移动平台建模成一个由x、y方向的移动关节上接一个旋转关节组成的特殊的机械臂，并将其与原有的机械臂模型组合后，移动机械臂D-H表如下：

移动机械臂D-H表

根据D-H表建立的移动机械臂仿真模型如图3所示。

移动平台的位姿表示为q_b＝[x,y,θ₀]^T，机械臂的关节转角向量为q_m＝[θ₁,θ₂,…,θ₅]^T，移动机械臂位姿表示为q_i＝[q_b ^T,q_m ^T]^T。

为移动机械臂的车-臂一体化变换矩阵

为世界坐标系到移动平台坐标系的状态转换矩阵，

为第i-1个关节到第i个关节的状态转换矩阵。

其中，

其中，s0～s5代表sinθ₀～sinθ₅，c0～c5代表cosθ₀～cosθ₅，s23代表sin(θ₂+θ₃)，c23代表cos(θ₂+θ₃)，s234代表sin(θ₂+θ₃+θ₄)，c234代表cos(θ₂+θ₃+θ₄)。2)基于RRT-GoalBias算法，设计移动机械臂车-臂一体化路径规划方法。

目标约束上，设置移动机械臂起始位姿q_start＝[0,0,0,1.57,0.8,-0.7,-2.335,0]^T,目标位姿q_goal＝[4000,4000,0,0,0.8,-0.7,-2.335,0,]^T移动机械臂规划路径的初始和终止位姿须等于规定的起始位姿和目标位姿，表示为：

q_init＝q_start

q_end＝q_goal

关节约束上，关节空间起始点为

q_min＝[0,0,-3.14,-2.95,0.436,-2.635,-3.36,-2.923]^T，终止点为q_max＝[5000,5000,3.14,2.95,3.14,2.548,2.18,-2.923]^T，

对于移动机械臂的整个路径规划过程，位姿约束为：

q_min≤q_i≤q_max,i＝1,2,3,...N

其中，N为规划出路径上路径点的数目；

障碍约束上，设置三个长方体障碍物和两个球体障碍物如图4中所示，如图2所示，给定与障碍物表面上点的最小距离d_min，需要保证在每一个节点上，对于移动机械臂上的每个点M_i，都有：

d(M_i,B_j)≥d_min+R

其中，R为球体的半径，B_j为障碍物表面上的标记点。

设计基于RRT-GoalBias的移动机械臂路径规划算法流程，该算法的流程图如图1所示，具体流程如下：

随机树的扩展和构建：

Step 1初始化随机树，定义关节空间C₁关节约束范围和任务空间C₂场地约束范围。

Step 2从初始位姿q_init出发，将其添加至随机树T中。

Step 3生成(0,1)上的随机数r，并将其与规定的概率值P_goal做比较，若r>P_goal，则在关节空间C₁中进行随机采样，得到状态点q_rand，反之则令q_rand＝q_goal。

Step 4在随机树T上找到距离q_rand最近的点q_near。

Step 5将q_near向q_rand前进一个步长，得到新节点q_new。

Step 6将新节点q_new正运动学求解投影到任务空间C₂中，并解算出各个关节和移动平台上圆球的球心坐标，在任务空间C₂中对其进行碰撞检测，若无碰撞，则在关节空间C₁中将对应的新节点q_new添加至随机树T中，反之则舍弃该节点。

Step 7重复Step 3～6，迭代扩展随机树T，直至有节点接近或到达目标位姿q_goal或者超出规定的迭代次数，则扩展停止。

路径查询：

通过回溯法从q_goal到q_init找到一条完整的路径，该算法结束。

3)算法的本质是在关节空间拓展随机树，将新节点通过运动学求解投影到任务空间，并解算出移动机械臂上每个点M_i，并对它们分别做碰撞检测，若无碰撞则在关节空间的随机树上添加该新节点。

对上一步得到的路径执行用于平滑的双向删除冗余路径算法，算法的基本原理如图2所示，即正向平滑的基础上，再进行一次反向平滑，路径1和路径2’构成最终输出的平滑后的路径，具体流程为：

Step 1从初始位姿开始查找，定义其为出发点q₀，

Step 2依次将后续的路径点与出发点q₀连接；

Step 3对所连直线路径进行碰撞检测，若无障碍，则将下一个路径点与出发点q₀连接并做碰撞检测，直至所连的直线路径上有障碍；

Step 4连接上一路径点q_last与出发点q₀形成直线路径，替换冗余路径；

Step 5将上一路径点q_last设置为新的出发点q₀；

Step 6循环Step 2～5，直至完成所有路径点的遍历。

Step 7在上述生成路径的基础上，将终止位姿q_end设置为新出发点，反向遍历路径点，重复Step 2～5，直至完成所有路径点的遍历。

4)将得到的路径点写入移动机械臂的控制代码，控制移动机械臂在给定场地下运动。

Claims (1)

1.一种基于随机采样的移动机械臂避障规划方法，其特征在于，包括以下步骤：

步骤1：将移动平台建模成一个由x、y方向的移动关节上接一个旋转关节组成的机械臂，并将其与原有的机械臂模型组合，建立n自由度移动机械臂的正运动学模型；

步骤2：对运动规划过程中的约束进行分析，设计移动机械臂车-臂一体化路径规划算法流程，具体为：

1)目标约束

移动机械臂规划路径的初始位姿q_init和终止位姿q_end等于规定的起始位姿q_start和目标位姿q_goal；

2)位姿约束

针对每一个关节转角以及移动平台的方位角，都有：

θ_jmin≤θ_j≤θ_jmax

其中，θ_jmin和θ_jmax分别表示第j个关节的角度上确界和下确界；

针对移动平台x、y方向上的坐标，都有：

l_min≤x_i,y_i≤l_max

其中，l_min和l_max分别表示给定场地在x、y方向上的坐标上确界和下确界；

对于移动机械臂的整个路径规划过程，位姿约束为：

q_min≤q_i≤q_max,i＝1,2,3,...N

其中，q_min为位姿的下限，q_max为位姿的上限；

3)碰撞约束

将移动机械臂进行碰撞检测建模，用若干个球体建模移动机械臂，其中机械臂上球体的球心分别位于各个关节处；

在碰撞检测中，给定与障碍物表面上点的最小距离d_min，保证在每一个节点上，对于移动机械臂上的每个点M_i，都有：

d(M_i,B_j)≥d_min+R

其中，B_j为障碍物表面上的标记点，d(M_i,B_j)表示移动机械臂上的每个点到障碍物表面上的标记点的距离，R为球体的半径；

设计移动机械臂车-臂一体化路径规划算法流程如下：

2-1、初始化随机树，定义关节空间C₁关节约束范围和任务空间C₂场地约束范围；

2-2、从初始位姿q_init出发，将其添加至随机树T中；

2-3、生成(0,1)上的随机数r，并将其与规定的概率值P_goal做比较，若r>P_goal，则在关节空间C₁中进行随机采样，得到状态点q_rand，反之则令q_rand＝q_goal；

2-4、在随机树T上找到距离q_rand最近的点q_near；

2-5、将q_near向q_rand前进一个步长，得到新节点q_new；

2-6、将新节点q_new正运动学求解投影到任务空间C₂中，并解算出各个关节和移动平台上圆球的球心坐标，在任务空间C₂中对其进行碰撞检测，若无碰撞，则在关节空间C₁中将对应的新节点q_new添加至随机树T中，反之则舍弃该节点；

2-7重复2-3～2-6，迭代扩展随机树T，直至有节点接近或到达目标位姿q_goal或者超出规定的迭代次数，则扩展停止；

路径查询：

通过回溯法从q_goal到q_init找到一条完整的路径，该算法结束；

步骤3：对步骤2得到的路径做平滑处理，具体如下：

3-1从初始位姿开始查找，定义其为出发点q₀，

3-2依次将后续的路径点与出发点q₀连接；

3-3对所连直线路径进行碰撞检测，若无障碍，则将下一个路径点与出发点q₀连接并做碰撞检测，直至所连的直线路径上有障碍；

3-4连接上一路径点q_last与出发点q₀形成直线路径，替换冗余路径；

3-5将上一路径点q_last设置为新的出发点q₀；

3-6循环3-2～3-5，直至完成所有路径点的遍历；

3-7在上述生成路径的基础上，将终止位姿q_end设置为新出发点，反向遍历路径点，重复3-2～3-5，直至完成所有路径点的遍历。

Images