CN113352319A - 基于改进快速扩展随机树的冗余机械臂避障轨迹规划方法 - Google Patents

Abstract

本发明属于自动控制的技术领域，公开了一种基于改进快速扩展随机树的冗余机械臂避障轨迹规划方法，包括步骤一、建立冗余机械臂的D‑H模型以及障碍物的简化包络模型；步骤二、根据冗余机械臂末端执行器的目标位姿，利用冗余机械臂逆运动学解析法求解满足需求的目标关节角，并设为q_goal；步骤三、将障碍物的简化包络模型向冗余机械臂的关节空间映射，进而得到冗余机械臂在关节空间内的无碰撞自由空间；步骤四、以目标关节角q_goal对应的节点为目标节点，在无碰撞自由空间内建立搜索树，根据选取规则有条件地选取随机采样节点，并引入搜索树节点筛选机制，最终在无碰撞自由空间内规划出一条无碰撞路径。

Description

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

技术领域

本发明属于自动控制的技术领域，具体涉及一种基于改进快速扩展随机树的冗余机械臂避障轨迹规划方法。

背景技术

轨迹规划是机械臂轨迹控制的基础，其性能对机械臂工作效率、运动平稳性和能量消耗具有非常重要的意义。随着机械臂应用领域的发展，工作环境愈为复杂，冗余机械臂运动时可能会与环境中的障碍物发生碰撞，导致工作任务无法完成，极大限制了机械臂的应用，因此开展冗余机械臂避障轨迹规划研究是十分必要的。

冗余机械臂运动范围大，灵活度高，能够适应多种工作环境，并完成各种复杂任务。在执行工作任务时，需要为冗余机械臂规划在给定约束条件下从起始位姿运动到目标位姿的路径，但其高自由度的特点也导致了传统路径规划算法在对于冗余机械臂的应用中存在计算耗时长，收敛速度慢，避障效率低，生成路径难以在实际工作中满足要求的缺点。

目前针对机械臂避障路径规划方法，有自由空间法，人工势场法，基于采样的路径搜索算法等，如Lazona-Perze提出的一种基于C空间法的自由空间法，先根据机械臂的各个关节约束范围建立机械臂所有可行C空间，再将障碍物映射到C空间内，进而求解C空间内除去障碍物的补集空间，即机械臂不会发生碰撞的自由空间，在得到的自由空间内，利用启发式搜索算法(A*算法，蚁群算法等)搜寻机械臂的运动路径；贾庆轩,陈钢等基于A*算法在机械臂无碰撞的自由C空间内搜索避障路径；齐志刚等根据空间冗余机械臂臂型角参数化完善逆运动学算法，提出了基于臂型角逆运动学的优化A*路径规划算法等等，上述几种算法对于冗余机械臂都存在路径求解计算速率低的问题，基于采样的路径搜索算法的提出，解决了冗余机械臂自由度过高而难以搜索到关节空间解的问题，其代表算法有快速扩展随机树法(rapidly-exploring random trees,RRT)和随机路图法(probabilistic roadmapmethod,PRM)，由于RRT算法和PRM算法都是直接在机械臂关节空间搜索，并不需要构造求解可***的C空间，因此其适合高维空间的规划，但其采样过于平均，搜索效率低且耗时长，并且在复杂环境下可能搜索不到解。

发明内容

本发明提供了一种基于改进快速扩展随机树的冗余机械臂避障轨迹规划方法，解决了传统路径规划算法在对于冗余机械臂的应用中存在计算耗时长，生成路径难以在实际工作中满足要求等问题。

本发明可通过以下技术方案实现：

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

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

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

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

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

进一步，在笛卡尔空间内，通过冗余机械臂与障碍物在空间几何上的关系，将障碍物的简化包络模型向冗余机械臂的关节空间映射，求得障碍物与冗余机械臂发生碰撞的碰撞关节空间，进而以所述碰撞关节空间的补集作为障碍物在冗余机械臂的关节空间内不会发生碰撞的无碰撞自由空间。

进一步，采用在冗余机械臂的关节空间内的分级遍历搜索法，以关节空间中各段连杆与障碍物的简化包络模型碰撞与否为判断条件来求解所有可能发生碰撞的关节角，从而获得障碍物与冗余机械臂发生碰撞的碰撞关节空间。

进一步，所述障碍物设置为静止的，其简化包络模型设置为球体。

进一步，假设L_n为冗余机械臂第n段连杆的长度，Joint_n-1、Joint_n分别为冗余机械臂第n段连杆两端关节在笛卡尔坐标系的位置坐标，rr为安全距离，其值应大于冗余机械臂连杆截面半径与障碍物的简化包络模型半径之和，

冗余机械臂与障碍物的简化包络模型几何中心

之间的最近距离h为：

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

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

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

进一步，所述选取规则设置为将冗余机械臂各个关节的当前关节角设为根节点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最近的节点，△为步长。

进一步，所述搜索树节点筛选机制包括以下步骤：

步骤Ⅰ、计算新节点q_new与障碍物的简化包络模型在关节空间内的最小距离d，设置阈值η，

当d<＝η时，认为冗余机械臂与障碍物有碰撞趋势，引入第一评价函数H(q)，分别计算新节点q_new与节点q_near对应的评价函数数值，若H(q_new)<H(q_near)，舍弃新节点q_new，否则执行步骤Ⅱ；

所述第一评价函数设置为：H(q)＝min(||q_obs1-q||,||q_obs2-q||,…,||q_obsn-q||)

其中，q_obs1,q_obs2,…,q_obsn为碰撞关节空间内部的所有节点；

当d>η时，引入第二评价函数

当cosθ<阈值γ时，舍弃新节点q_new，否则执行步骤Ⅱ，其中，Q_new、Q_goal分别表示在笛卡尔坐标系下，节点q_near对应冗余机械臂末端执行器的位置分别指向新节点q_new、目标节点q_goal对应冗余机械臂末端执行器的位置的矢量，θ为矢量Q_new、Q_new之间夹角；

步骤Ⅱ、判断新节点q_new与q_near的连线是否与障碍物碰撞，若不碰撞，将新节点q_new加入扩展树中，否则舍弃新节点q_new，重新根据选取规则有条件地选取随机采样节点，并执行步骤Ⅰ-Ⅱ，直至找到符合要求的新节点q_new。

进一步，采用多项式插值法，根据步骤四规划出的无碰撞路径计算出冗余机械臂运动到目的地所需的参数，将其发送给冗余机械臂执行运动，并实时监测障碍物位置，若障碍物与规划出的无碰撞轨迹发生碰撞时，重新构建搜索树，以冗余机械臂当前的关节角状态为搜索树新的根节点，重复步骤四，规划出新的无碰撞轨迹。

本发明有益的技术效果在于：

首先简化冗余机械臂与障碍物的模型，并将障碍物模型映射到冗余机械臂的关节空间，然后结合冗余机械臂工作环境特点改进标准RRT算法中随机采样点选取机制，针对冗余机械臂自由度高而导致的在标准RRT算法中收敛速度慢的缺点，优化随机采样点选取机制，避免随机扩展树密集分布；通过引入评价函数对扩展树生成的新节点进行筛选，减少对避障任务无效的新节点生成，从而达到加快算法速率，收敛速度更快，避障效率更高的效果。

附图说明

图1是本发明的总体流程示意图；

图2是本发明的Kinova公司的Gen3七自由度冗余机械臂的结构示意图；

图3是本发明的采用球体包络模型来对障碍物近似建模的示意图；

图4是本发明的冗余机械臂各个关节角度变化图以及相对障碍物的位置示意图；

图5是本发明的障碍物向冗余机械臂关节空间映射的原理示意图；

图6是本发明的随机采样节点后的变化状态示意图；

图7是本发明的搜索树节点筛选机制的原理示意图；

图8是采用本发明的规划方法和标准RRT算法进行路径规划的所生成路径的对比示意图；

图9是采用本发明的规划方法生成的路径中冗余机械臂与障碍物之间距离变化示意图。

具体实施方式

下面结合附图及较佳实施例详细说明本发明的具体实施方式。

本发明提供了一种基于改进快速扩展随机树的冗余机械臂避障轨迹规划方法，首先简化冗余机械臂与障碍物的模型，并将障碍物模型映射到冗余机械臂的关节空间，然后结合冗余机械臂工作环境特点改进标准RRT算法中随机采样点选取机制，针对冗余机械臂自由度高而导致的在标准RRT算法中收敛速度慢的缺点，优化随机采样点选取机制，避免随机扩展树密集分布；通过引入评价函数对扩展树生成的新节点进行筛选，减少对避障任务无效的新节点生成，从而达到加快算法速率，收敛速度更快，避障效率更高的效果。

本发明提供了一种基于改进快速扩展随机树的冗余机械臂避障轨迹规划方法，如图1所示，具体包括以下步骤：

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

以Kinova公司的Gen3七自由度冗余机械臂如图2所示为研究对象，建立冗余机械臂的D-H运动学模型，D-H参数如表1所示。

表1机械臂D-H模型参数

Tab.1 The D-H parameter of the manipulator

在标准D-H描述法中，假设各关节轴向为z轴，与下一关节两轴轴向公法线方向为x轴，再根据右手定则确定y轴。表1中，j为关节序号；θ_j为第j个关节的关节变量，因为KinovaGen3型机械臂七个关节都是转动关节，故θ_j所表示的关节变量都是关节角度，即为绕着z_j轴从x_j-1到x_j的角度；d_j为沿着z_j轴从x_j到x_j+1的距离；a_j为沿着x_j轴从z_j到z_j+1的距离；α_j为绕着x_j轴从z_j转到z_j+1的距离。

机械臂第j-1关节坐标系到第j关节坐标系的齐次变换矩阵

为：

式中，^j-1T_j为第j-1关节到第j关节坐标轴变换矩阵，R(q_j)为第j关节坐标系在关节角q_j下的旋转矩阵。

本发明研究对象Kinova Gen3型机械臂的第1，3，5，7关节轴向与连杆延伸方向平行，第2，4，6关节轴向与连杆延伸方向垂直，第1，3，5，7关节所连接的两段连杆不会在空间结构上发生变化，故在碰撞检测时可将机械臂视作4段串联机械臂分别计算，绕杆轴向转动作用可等效至杆件两端。当基座不发生运动时，第1段连杆在空间内不会发生位姿上的变化，该段机械臂与障碍物发生碰撞的情况不做考虑。

现实中的各种障碍物通常没有规则的形状，无法通过数学的方式描述，本发明采用球体包络模型来对障碍物近似建模如图3所示，同时本发明的规划方法主要针对静止的障碍物进行，这种建模方式简化了碰撞检测的计算难度，并在一定程度上提高了算法所规划出的避障路径安全性。图3中，点O(x_o,y_o,z_o)为包络障碍物的球体包络模型的几何中心在基座的笛卡尔坐标系下坐标，r_o为障碍物球体包络模型的半径。

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

如图4所示，冗余机械臂的初始位姿即各个连杆关节对应的初始关节角[30°,15°,30°,15°,15°,15°,15°]，利用冗余机械臂逆运动学解析法求解满足需求的目标关节角q_goal即目标位姿[30°,60°,-60°,60°,-60°,-60°,0°]。关节1，3，5，7角度约束范围为[-180°，180°]，关节2，4，6角度约束范围为[-120°，120°]。设置三个障碍物在基系笛卡尔空间内位置分别为[0.5m,-0.3m,0.75m]，[0.5m,-0.3m,0.65m],[0.6m,0.25m,0.55m]，半径分别为0.1m，0.1m，0.07m。

步骤三、将障碍物的简化包络模型向冗余机械臂的关节空间映射，进而得到冗余机械臂在关节空间内的无碰撞自由空间；即在笛卡尔坐标系空间内，通过冗余机械臂与障碍物在空间几何上的关系，将障碍物的简化包络模型向冗余机械臂的关节空间映射，求得障碍物与冗余机械臂发生碰撞的碰撞关节空间，进而以所述碰撞关节空间的补集作为障碍物在冗余机械臂的关节空间内不会发生碰撞的无碰撞自由空间。

采用在冗余机械臂的关节空间内的分级遍历搜索法，以关节空间中各段连杆与障碍物的简化包络模型碰撞与否为判断条件来求解所有可能发生碰撞的关节角，从而获得障碍物与冗余机械臂发生碰撞的碰撞关节空间。本发明将笛卡尔空间下的障碍物通过冗余机械臂与障碍物在空间几何上的关系向冗余机械臂关节空间映射，利用障碍物在关节空间内的模型，将基系笛卡尔空间内各段连杆与障碍物之间的碰撞检测简化为关节空间内点与点之间距离的计算。

具体地，映射原理如图5所示，首先将所有关节置于最小极限角度，由基座向末端连杆依次遍历，每当前一段连杆的关节角增加一个步长，后一段关节都要重新遍历一次。假设每关节的运动范围都被分为n步，则对于有七自由度的冗余机械臂，整个遍历过程共有n⁷组关节角。

假设L_n为冗余机械臂第n段连杆的长度，Joint_n-1、Joint_n分别为冗余机械臂第n段连杆两端关节在笛卡尔坐标系的位置坐标，rr为安全距离，其值应大于冗余机械臂连杆截面半径与障碍物的简化包络模型半径之和，

冗余机械臂与障碍物的简化包络模型几何中心

之间的最近距离h为：

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

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

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

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

RRT算法是一种基于随机采样的算法，以初始位置为根节点，通过在空间内随机采样并以一定步长生成新的无碰撞节点的方式来延伸随机扩展树。当有任意子节点到达目标点附近时则认为寻得路径，通过从目标点向根节点回溯最终获得一条无碰撞的路径。

在标准RRT算法中，新节点是沿空间中一随机采样点与扩展树中最近节点的连线方向延伸步长△获得，扩展树中节点数量的增加将导致算法的计算速度下降。由于冗余机械臂的工作环境相对固定，工业应用场景相对简单，扩展树无需在整个关节空间密集分布，故可以通过舍弃会导致扩展树在局部区域聚集的随机采样点来加快扩展速度。

当随机采样点q_rand与扩展树中最近节点q_near距离小于步长△时，新节点q_new与扩展树中其他节点的最小距离有可能小于步长△，如图6所示。由于随机采样点只是确定新节点生成的扩展方向，即使满足要求的无碰撞路径在某一区域存在唯一节点可行，生成该节点对应的随机采样点也不唯一，为了不增加算法计算量，本发明提出了一种新的选取规则即为在搜索与随机采样最近节点步骤中，若最小距离小于步长△，则舍弃该采样点重新采样。

具体地，将冗余机械臂各个关节的当前关节角设为根节点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最近的节点，△为步长。

冗余机械臂由于自由度高，相较于在二维空间搜索路径的移动机器人，在关节空间内采用RRT搜索路径存在收敛速度慢的问题。因此，本发明提供了一种新的搜索树节点筛选机制，在对冗余机械臂进行避障路径规划时，需要同时完成运动至目标点任务以及避障任务，在障碍物一定范围内设定阈值，当节点在阈值范围内可认为机械臂继续运动有碰撞风险，此时主要任务为避障任务，在机械臂关节空间引入评价函数H(q)，分别计算新节点q_new与扩展树上最近节点q_near处评价函数数值，如图7(a)所示筛去不利于避障任务的节点。

同时，由于随机采样是在整个空间中均匀采样，不具备方向性，针对冗余机械臂的工作环境，可以利用基系下末端执行器向目标位置运动引导扩展树延伸方向，使得新节点皆使得末端执行器向目标位置靠近。当节点在阈值范围之外可认为碰撞风险低，此时可视为在无障碍物的自由空间内运动，主要任务为运动至目标点，通过添加扩展树延伸方向与直接向终点延伸方向夹角约束，如图7(b)所示筛去在阈值外远离终点方向的节点。

具体如下：

步骤Ⅰ、计算新节点q_new与障碍物的简化包络模型在关节空间内的最小距离d，设置阈值η，

当d<＝η时，认为冗余机械臂与障碍物有碰撞趋势，引入第一评价函数H(q)，分别计算新节点q_new与节点q_near对应的评价函数数值，若H(q_new)<H(q_near)，舍弃新节点q_new，否则执行步骤Ⅱ；

所述第一评价函数设置为：H(q)＝min(||q_obs1-q||,||q_obs2-q||,…,||q_obsn-q||)

其中，q_obs1,q_obs2,…,q_obsn为碰撞关节空间内部的所有节点；

当d>η时，引入第二评价函数

当cosθ<阈值γ时，舍弃新节点q_new，否则执行步骤Ⅱ，其中，Q_new、Q_goal分别表示在笛卡尔坐标系下，节点q_near对应冗余机械臂末端执行器的位置分别指向新节点q_new、目标节点q_goal对应冗余机械臂末端执行器的位置的矢量，θ为矢量Q_new、Q_new之间夹角；

步骤Ⅱ、判断新节点q_new与q_near的连线是否与障碍物碰撞，若不碰撞，将新节点q_new加入扩展树中，否则舍弃新节点q_new，重新根据选取规则有条件地选取随机采样节点，并执行步骤Ⅰ-Ⅱ，直至找到符合要求的新节点q_new。

综上，相较于标准RRT，本发明主要在以下两处加以改进：

1)在得到与随机采样点q_rand最近节点q_near后计算||q_rand-q_near||，只有当||q_rand-q_near||>△时才会执行后续步骤；

2)当q_near相对障碍物距离在设置的阈值范围之内时，生成节点q_new后计算q_new与q_near两点处评价函数H(q)，当H(q_new)≥H(q_near)时，生成节点满足需求，将q_new加入扩展树；当q_near在阈值范围之外时，计算q_new与q_goal节点对应末端执行器位置与q_near节点对应的末端执行器位置连线的矢量夹角θ，当θ小于约束值时，生成节点满足要求，将q_new加入扩展树。

为了方便后续冗余机械臂的运动执行，可采用多项式插值法，根据步骤四规划出的无碰撞路径计算出冗余机械臂运动到目的地所需的参数，将其发送给冗余机械臂执行运动，并实时监测障碍物位置，若障碍物与规划出的无碰撞轨迹发生碰撞时，重新构建搜索树，以冗余机械臂当前的关节角状态为搜索树新的根节点，重复步骤四，规划出新的无碰撞轨迹。

为了验证本发明轨迹规划方法的效果，本发明基于上文所述的Kinova公司Gen3型冗余机械臂，在Matlab R2020b环境下进行仿真试验。

试验对比改进RRT与标准RRT算法在相同条件下路径规划效率。机械臂初始位姿[30°,15°,30°,15°,15°,15°,15°]，目标位姿即目标关节角为[30°,60°,-60°,60°,-60°,-60°,0°]。关节1，3，5，7角度约束范围为(-180°，180°)，关节2，4，6角度约束范围为(-120°，120°)。

设置三个障碍物在基系笛卡尔空间内位置为[0.5m,-0.3m,0.75m]，[0.5m,-0.3m,0.65m],[0.6m,0.25m,0.55m]，半径分别为0.1m，0.1m，0.07m。

由于试验中对比的两种算法都是在相同的关节空间内均匀采样，故可以通过对比两种算法生成路径所需要的总随机采样点个数判断路径搜索速度，采样点数越少速度越快，通过比较路径节点数与随机扩展树节点数的比值判断算法收敛速度，比值越大收敛速度越快。

试验分别采用标准RRT与本发明提出的改进RRT算法进行若干次仿真，记录每次生成的关节空间路径，末端执行器在笛卡尔坐标系下路径，总采样次数以及生成无碰撞路径所需的扩展树节点数。生成无碰撞路径后采用多项式插值法获得轨迹，在Matlab中绘制机械臂运动过程。

对比两种算法生成的路径，标准RRT在笛卡尔空间内的路径如图8(a)所示，关节角度变化如图8(b)所示。标准RRT生成的路径过程完全随机，所以存在部分不利于完成运动至目标位置任务和避障任务路段，关节角度在相对远离障碍物时也存在变化率较大的情况，当空间中无障碍物时，关节空间中初末位置之间的最优路径应当为两点间直线段，故在远离障碍物时，关节角变化越接近线性，生成路径越好。本发明提出的改进RRT在笛卡尔空间内的路径如图8(c)所示，关节角度变化如图8(d)所示。对比标准RRT，改进后的RRT算法生成的路径点不会在局部聚集，关节角也接近线性变化。

改进RRT算法所生成路径的每一节点对应的机械臂相对障碍物距离变化如图9所示，设置距离障碍物0.22m为阈值η，当机械臂与障碍物距离小于阈值η时，主要任务为避障任务，扩展树向远离障碍物方向延伸。

基于随机采样的路径搜索算法结构具有随机性，多次试验记录数据如表2所示。由表2中数据可得，在相同环境下，本发明提出的改进RRT算法生成路径所需的总采样次数、扩展树中节点数以及最终路径节点数更少，说明改进后的算法寻得无碰撞路径速度更快。改进RRT算法的路径节点数与扩展树节点数的比值相较于标准RRT算法更大，说明改进后的算法收敛速度更快。试验结果符合算法设计要求。

表2试验结果

Tab.2 Experimental results

针对冗余机械臂高自由度而导致的路径搜索算法收敛速度慢，避障效率低的问题，本发明在机械臂关节空间内搜索的基础上提出一种改进RRT算法，首先，优化随机采样点选取机制，避免随机扩展树密集分布；进而，在障碍物一定范围设定阈值，根据节点所在位置判断节点主从任务，并引入评价函数筛选节点。由仿真试验结果可得出结论，本发明提出的改进RRT算法不仅能得到一条顺滑的避障路径，而且相较于标准RRT算法，收敛速度更快，避障效率更高。

虽然以上描述了本发明的具体实施方式，但是本领域的技术人员应当理解，这些仅是举例说明，在不背离本发明的原理和实质的前提下，可以对这些实施方式做出多种变更或修改，因此，本发明的保护范围由所附权利要求书限定。

Claims (8)

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

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

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

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

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

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即为障碍物的简化包络模型在冗余机械臂关节空间内的映射，也就是碰撞关节空间。

6.根据权利要求2所述的基于改进快速扩展随机树的冗余机械臂避障轨迹规划方法，其特征在于：所述选取规则设置为将冗余机械臂各个关节的当前关节角设为根节点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最近的节点，△为步长。

7.根据权利要求6所述的基于改进快速扩展随机树的冗余机械臂避障轨迹规划方法，其特征在于所述搜索树节点筛选机制包括以下步骤：

步骤Ⅰ、计算新节点q_new与障碍物的简化包络模型在关节空间内的最小距离d，设置阈值η，

当d<＝η时，认为冗余机械臂与障碍物有碰撞趋势，引入第一评价函数H(q)，分别计算新节点q_new与节点q_near对应的评价函数数值，若H(q_new)<H(q_near)，舍弃新节点q_new，否则执行步骤Ⅱ；

所述第一评价函数设置为：H(q)＝min(||q_obs1-q||,||q_obs2-q||,…,||q_obsn-q||)

其中，q_obs1,q_obs2,…,q_obsn为碰撞关节空间内部的所有节点；

当d>η时，引入第二评价函数

当cosθ<阈值γ时，舍弃新节点q_new，否则执行步骤Ⅱ，其中，Q_new、Q_goal分别表示在笛卡尔坐标系下，节点q_near对应冗余机械臂末端执行器的位置分别指向新节点q_new、目标节点q_goal对应冗余机械臂末端执行器的位置的矢量，θ为矢量Q_new、Q_new之间夹角；

步骤Ⅱ、判断新节点q_new与q_near的连线是否与障碍物碰撞，若不碰撞，将新节点q_new加入扩展树中，否则舍弃新节点q_new，重新根据选取规则有条件地选取随机采样节点，并执行步骤Ⅰ-Ⅱ，直至找到符合要求的新节点q_new。

8.根据权利要求1所述的基于改进快速扩展随机树的冗余机械臂避障轨迹规划方法，其特征在于：采用多项式插值法，根据步骤四规划出的无碰撞路径计算出冗余机械臂运动到目的地所需的参数，将其发送给冗余机械臂执行运动，并实时监测障碍物位置，若障碍物与规划出的无碰撞轨迹发生碰撞时，重新构建搜索树，以冗余机械臂当前的关节角状态为搜索树新的根节点，重复步骤四，规划出新的无碰撞轨迹。

Images