[发明专利]一种基于随机采样的移动机械臂避障规划方法

权利要求书

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做比较，若rP_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，直至完成所有路径点的遍历。