[发明专利]基于改进快速扩展随机树的冗余机械臂避障轨迹规划方法

权利要求书

1.一种基于改进快速扩展随机树的冗余机械臂避障轨迹规划方法，其特征在于包括以下步骤：

步骤一、建立冗余机械臂的D-H模型以及障碍物的简化包络模型；

步骤二、根据冗余机械臂末端执行器的目标位姿，利用冗余机械臂逆运动学解析法求解满足需求的目标关节角，并设为q_goal；

步骤三、将障碍物的简化包络模型向冗余机械臂的关节空间映射，进而得到冗余机械臂在关节空间内的无碰撞自由空间；

步骤四、以目标关节角q_goal对应的节点为目标节点，在无碰撞自由空间内建立搜索树，根据选取规则有条件地选取随机采样节点，并引入搜索树节点筛选机制，最终在无碰撞自由空间内规划出一条无碰撞路径；

所述选取规则设置为将冗余机械臂各个关节的当前关节角设为根节点q_root，在冗余机械臂的关节空间随机采样一个节点q_rand，然后在扩展树中搜索与q_rand最近的节点q_near，当q_rand与q_near两个节点在关节空间的欧式距离小于等于步长△时，向节点q_rand的方向延伸有可能导致搜索树密集分别，舍弃采样节点q_rand；若q_rand与q_near两个节点距离大于步长△，则在两节点连线上由q_near向外延伸一个步长△得到新节点q_new；

扩展树新节点q_new由如下公式计算:

其中，q_near为扩展树中与q_rand最近的节点，△为步长。

2.根据权利要求1所述的基于改进快速扩展随机树的冗余机械臂避障轨迹规划方法，其特征在于：在笛卡尔坐标系空间内，通过冗余机械臂与障碍物在空间几何上的关系，将障碍物的简化包络模型向冗余机械臂的关节空间映射，求得障碍物与冗余机械臂发生碰撞的碰撞关节空间，进而以所述碰撞关节空间的补集作为障碍物在冗余机械臂的关节空间内不会发生碰撞的无碰撞自由空间。

3.根据权利要求2所述的基于改进快速扩展随机树的冗余机械臂避障轨迹规划方法，其特征在于：采用在冗余机械臂的关节空间内的分级遍历搜索法，以关节空间中各段连杆与障碍物的简化包络模型碰撞与否为判断条件来求解所有可能发生碰撞的关节角，从而获得障碍物与冗余机械臂发生碰撞的碰撞关节空间。

4.根据权利要求3所述的基于改进快速扩展随机树的冗余机械臂避障轨迹规划方法，其特征在于：所述障碍物设置为静止的，其简化包络模型设置为球体。

5.根据权利要求3所述的基于改进快速扩展随机树的冗余机械臂避障轨迹规划方法，其特征在于：假设L_n为冗余机械臂第n段连杆的长度，Joint_n-1、Joint_n分别为冗余机械臂第n段连杆两端关节在笛卡尔坐标系的位置坐标，rr为安全距离，其值应大于冗余机械臂连杆截面半径与障碍物的简化包络模型半径之和，

冗余机械臂与障碍物的简化包络模型几何中心之间的最近距离h为：

障碍物的简化包络模型在连杆J_n-1J_n轴向方向的投影长度l_h：

障碍物的简化包络模型与冗余机械臂发生碰撞的条件为：

在整个遍历过程中，满足该条件所有关节角集合q_obs即为障碍物的简化包络模型在冗余机械臂关节空间内的映射，也就是碰撞关节空间。