CN113459086B - 一种超冗余机械臂路径规划方法 - Google Patents

Abstract

本发明涉及一种超冗余机械臂路径规划方法，步骤1：针对超冗余机械臂建立运动学模型，确定绳长、偏角、坐标之间的转换关系；步骤2：建立障碍物模型，确定与障碍物对应的不可抵达区域；步骤3：在步骤1的基础上，通过正弦定理及两个极值点，理论分析两相邻直线路径之间的最大夹角Ψ_max与第i个臂杆端点沿其中一段直线路径运动过程中的臂杆偏角变化最大值ψ_max、臂杆长度L_i、路径步长l_w之间的关系；步骤4：根据步骤2确定的不可抵达区域以及步骤3中的约束关系，提出改进RRT算法进行路径规划。本发明的可行性与优化性优于传统算法。

Description

一种超冗余机械臂路径规划方法

技术领域

本发明涉及机械臂路径规划技术领域，具体涉及一种基于改进RRT算法的超冗余机械臂路径规划方法。

背景技术

在很多领域，路径规划技术都得到了广泛的应用。但是随着科技的发展及需求的提高，传统的路径规划算法难以满足复杂环境下的规划条件。基于节点采样的快速扩展随机树(Rapidly-Exploring Random Tree，RRT)算法，具有既能用于完整***的路径规划，也能添加约束条件用于非完整***的自主寻路等优点，为高维复杂环境下的路径规划开辟了一种解决方案。

RRT算法在路径规划领域取得较大的进展，但算法仍存在计算量偏大、路径不稳定、节点利用率低、不容易获得满足条件的路径等缺点。并且很少有算法在有关节偏角限制的条件下，分析如何规划路径，使得机械臂从狭窄空间中穿过。目前，可以借助一些软件、程序、工具箱(例如RVC、ROS)等工具可以规划出满足关节角度限制的路径，但在这些规划中都需要在规划过程中实时验算是否满足约束。因此，需要提出一种方法，可直接规划出符合关节角度限制的路径，可以保证机械臂在沿着规划的路径运动时，不仅可以穿过狭窄空间抵达目标点，并且还可以保证机械臂关节偏角始终在限制范围内。

发明内容

本发明解决的技术问题是：提出一种超冗余机械臂路径规划方法。该方法可与概率路线图(PRM)算法、A*算法、人工势能(APF)算法等路径规划算法相结合，可直接规划出符合关节角度限制的路径，大大提高了可行性与优化性。

本发明解决技术的方案是：一种超冗余机械臂路径规划方法，步骤如下：

步骤1：针对超冗余机械臂建立机械臂运动学模型，确定驱动绳长、臂杆偏转角度、臂杆末端点坐标之间的转换关系；

步骤2：针对环境中的障碍物建立障碍物模型，从障碍物模型中获得障碍物的位置、形状、大小信息，进而确定与障碍物对应的不可抵达区域；

步骤3：假设相邻臂杆长度相同且与路径步长l_w成线性关系，根据步骤1中确定的机械臂运动学模型，理论分析两相邻直线路径之间的最大夹角Ψ_max与第i个臂杆初始点沿其中一段直线路径运动过程中的臂杆偏角变化最大值ψmax、臂杆长度L_i、路径步长l_w之间的关系；

步骤4：根据步骤2确定的与障碍物对应的不可抵达区域以及步骤3中的关系约束进行路径规划。

优选的，所述的步骤2中，将路径与障碍物之间的最近距离设置为臂杆表面上的所有点距离该臂杆轴线的最远距离加上臂杆轴线上的点与直线路径之间的最远距离，与障碍物的距离小于等于上述最近距离的范围视为不可抵达区域。

优选的，步骤4采用MDA+RRT算法进行路径规划，具体通过下述方式实现：

步骤41：定义步长δ即l_w，以路径规划的起始点xinit作为随机树T的根节点；

步骤42：随机选择一个节点x_rand，选择一个离x_rand最近的节点x_near，根据节点x_near与目标点x_goal之间的距离d_goal确定Ψ_max的取值；然后根据步骤3中的关系与x_rand、x_near、T分析x_new的选取范围，进而计算出x_new的位置；

步骤43：若x_new符合条件要求即未进入步骤2中确定的不可抵达区域，则将x_new附近的节点存放至x_neighbor中，并将这些附近节点的祖先存放至x_parent中，进而得到一个由x_new整理出的Xin集合；

步骤44：在Xin中找到使x_new成本最小且满足偏角要求的新父节点x_par，删除x_new与原父节点x_near的连线，连接x_new与x_par，并在T中进行修剪；

步骤45：若x_neighbor内一个节点x_from的权值大于当前x_new的权值与到该节点的距离之和，且满足偏角要求，则以当前x_new节点作为x_from的父节点；

步骤46：重复寻找x_new及修剪x_new与附近节点的连接关系，直到到达目标点。

优选的，所述的步骤42通过下述方式实现：

S1、若x_near是起始点x_init，则在-60°到60°之间随机选取x_new的方向，执行步骤S2，否则，在-min(Ψ_max)到min(Ψ_max)之间随机选取x_new的方向，执行步骤S2；min(Ψ_max)为节点x_near与目标点x_goal之间不同距离下确定的Ψ_max的最小值；

S2、判断是否满足下列不等式Abs(ang_rand-ang_near)≤Ψ_max，若满足，则保持x_new方向；若不满足，则令Ψ_i为可选取范围中的边界值；

其中，x_rand与x_near两点连成的线段相对于水平线的夹角为ang_rand，在T中读取x_near与其父节点连成的线段相对于水平线的夹角为ang_near。

优选的，步骤42中通过下述方式确定Ψ_max的取值：

d_goal<L，则令Ψ_max取值为根据倒数第二个臂杆结合步骤3中关系计算的两相邻直线路径之间的最大夹角；

d_goal≥L，则令Ψ_max取值为根据最长臂杆结合步骤3中关系计算的两相邻直线路径之间的最大夹角；

L为最后两个臂杆的长度和。

优选的，Ψ_i为可选取范围中的边界值，在二维情况下Ψ_i＝ang_near+Sign(ang_rand-ang_near)·Ψ_max，x_new为边界上的一个点；在三维情况下，保证两相邻直线路径之间的夹角为Ψmax，xnew的选取空间为一个半径为δ·sin(Ψ_max)的圆，x_new为该圆上的一个点。

优选的，步骤3中关系通过下述方式确定：

假设l_w＝k·L_i＝k·L_i-1，k∈(0,1]，让第i-1个臂杆的起始点沿着第w段直线路径运动，其运动过程中第i个臂杆的偏角ψ_i变化的极大值出现的位置有两处；第一处为第i-1个臂杆末端点即第i个臂杆初始点运动到一段直线路径的起始点或终止点时，此时ψ_i变化的极大值记为ψ_{i_end}；第二处为第i个臂杆初始点运动到一段直线路径的中点时，该点处ψ_i变化的极大值记为ψ_{i_mid}；

求出这两个极大值后即求得臂杆偏角变化最大值max(ψ_{i_end},ψ_{i_mid})。

优选的，k＝1时通过下述方式确定步骤3中的关系：

将相邻两臂杆沿轴线简化为一条直线，相邻两臂杆与路径上连续的三条直线构成两个三角形，根据三角形的正弦定理及角度变换确定关系如下：

9、根据权利要求7所述的一种超冗余机械臂路径规划方法，其特征在于：

N_end为第i个臂杆端点运动到一段直线路径的端点时，臂杆两个端点之间有几个路径转折点；

N_mid为第i个臂杆端点运动到一段直线路径的中点时，臂杆两个端点之间有几个路径转折点。

优选的，所述的步骤44中，在寻找新父节点x_par时，除了满足降低代价，且无碰撞的条件外，还需满足从xin的父节点到xin的路径与xin到x_new的路径之间的夹角不大于Ψ_max；xin为Xin集合中的节点。

优选的，所述的步骤45中，若以当前x_new节点作为x_from的父节点，则不仅需要满足降低代价、无碰撞的条件外，还需要满足从x_new的父节点到x_new的路径与x_new到x_from的路径之间的夹角不大于Ψ_max。

优选的，所述的超冗余机械臂具有2N+1个自由度，N为机械臂的数量，不少于3。

优选的，在无人汽车的自动驾驶、遥控飞机的自主规划、机器人的自主无碰行动中的应用。

本发明与现有技术相比的有益效果是：

(1)本发明方法可保证在二维、三维(2D、3D)环境中，机械臂的关节偏转角度始终在一定范围内。(2)MDA+RRT算法相对于RRT算法未增加计算量，规划出的路径的可行性与优化性也都优于对应的RRT算法。(3)本文中的方法可以与任何一种RRT算法或其他路径规划算法相结合，可为各种运动物体，规划出满足角度约束条件的路径，具有通用性。

附图说明

图1 MDA+RRT算法流程图；

图2超冗余机械臂结构图；

图3规划路径时的不可抵达区域示意图；

图4机械臂在直线路径上的示意图及等效图；

图5机械臂运动示意图及关节角度变化图；

图6 x_new选取范围的示意图；

图7六种算法在第一种障碍物中的树状分布图；

图8六种算法在第二种障碍物中的树状分布图；

图9六种算法在第三种障碍物中的树状分布图；

图10六种算法针对三种障碍物规划出的路径的性能参数图；

图11超冗余机械臂的运动过程图。

具体实施方式

下面结合实施例对本发明作进一步阐述。

本发明提供的一种基于改进RRT算法的超冗余机械臂路径规划方法，该方法除了可用于超冗余机械臂的路径规划外，还可以将该算法应用于其他运动物体(如：无人汽车的自动驾驶、遥控飞机的自主规划、机器人的自主无碰行动等)的路径规划，从而规划出指定偏角范围内的路径。算法流程图如图1所示，首先建立机械臂运动学模型和障碍物模型，然后理论分析各参数之间的关系，最后根据对应的RRT算法进行改进，得到对应的MDA+RRT算法。

在本实施例中，设三个规划地图的范围分别为2000×1665、3000×2500、1700×2000×1400。机械臂的数量N＝8。

如图1所示，本发明具体实施步骤如下：

步骤1：建立机械臂运动学模型，即针对对应的超冗余机械臂建立的D-H坐标系，并推导出机械臂模型中个参数之间的转换关系。

建立的D-H坐标系如图2所示，其中包括相对于地面固定不变的世界坐标系{O_xyz}、基座的本体坐标系{O₀}以及八个臂杆的本体坐标系{O₁}-{O₈}。各坐标系之间可以通过转换矩阵进行坐标系转换，两个相邻臂杆的本体坐标系之间的转换矩阵与第i个臂杆的长度L_i、第i个臂杆的偏航角α_i与第i个臂杆的俯仰角θ_i相关。驱动绳穿过相对于臂杆本体坐标系{O_i}固定不变的孔去驱动机械臂，所以可以根据驱动绳长度求出臂杆的偏转角度进而求出臂杆末端点坐标，也可以通过臂杆末端点坐标求出臂杆的偏转角度进而求出驱动绳长度。若已知机械臂的重复运动路径即可求出个臂杆末端点的位置，从而也可求出两相邻臂杆之间的偏转角度ψ_i。

步骤2：建立障碍物模型：针对环境中的障碍物建立对应的模型，可以从该模型中获得障碍物的位置、形状、大小等信息。

以第一个障碍物为例，分析为了保证机械臂不碰到障碍物，应当保证路径与障碍物之间的距离保持在多少之上。每个臂杆表面上的所有点距离该臂杆轴线的最远距离为75，考虑到臂杆沿着直线路径运动时，只是臂杆两端点沿着路径运动，并且在机械臂运动过程中还存在控制误差、定位误差等误差，所以臂杆轴线上的点与路径之间存在一定的距离(认为最大距离为Δ_max)，故令规划出的路径与障碍物之间的最近距离为75+Δ_max。将与障碍物的距离小于等于75+Δ_max的地方都视为不可抵达区域，如图3所示的黑色线框以内的区域都为不可抵达区域。因此，可以根据空间点与障碍物之间的最近距离来判断该空间点是否可抵达，在后面的路径规划中将障碍物模型变为该障碍物对应的不可抵达区域模型。

步骤3：理论分析臂杆长度L_i、关节最大夹角ψ_max、路径步长l_w、路径中两相邻直线之间的最大夹角Ψ_max这四个参数之间的关系，从而将关节偏角限制的问题转换到路径偏角限制的问题。

分析当l_w＝L_i＝L_i-1(w＝1,2,3…)时，各参数之间应满足的关系。如图4所示的两臂杆轴线与路径上的三条直线构成了两个三角形：△ABC与△CDE。根据三角形的正弦定理及角度转换可得出max(ψ_i)的数学模型如下：

进而可求出：

其次分析当l_w＝k·L_i＝k·L_i-1(其中的k∈(0,1],w＝1,2,3…)时，各参数之间应满足的关系。让第i-1个臂杆的起始点沿着第w段直线路径运动，其运动过程及第i个关节处的偏角变化如图5所示。

ψ_i的极大值出现的位置有两处。第一处为第i-1个臂杆末端点即第i个臂杆初始点运动到一段直线路径的起始点或终止点时(图3中的a＝0或b＝0)，经推导得出该点处的ψ_{i_end}如下所示：

第二处为第i个臂杆初始点运动到一段直线路径的中点时(图3中的a＝b)，该点处的ψ_{i_mid}如下所示：

求出这两个极大值后即可求得关节最大偏角ψ_max＝max(ψ_{i_end},ψ_{i_mid})。

公式中的N_end为臂杆端点运动到一段直线路径的端点时，臂杆两个端点之间有几个路径转折点，例如图5中第一幅图中的N_end＝1。其中的N_mid为臂杆端点运动到一段直线路径的中点时，臂杆两个端点之间有几个路径转折点，例如图5中第二幅图中的N_mid＝2。令l_w＝300、Ψ_max＝22.40°、L_i＝L_i-1＝486，带入公式可求得ψ_{i_end}＝39.99°、ψ_{i_mid}＝35.77°，即让臂杆长度为486的机械臂沿着步长为300、两相邻直线之间的最大夹角为22.40°的路径运动时，关节偏角ψi始终不超过40°的限制范围。

根据公式可确定当l_w＝k·L_i＝k·L_i-1、ψ_max＝40°时，Ψ_max应取值多少，如表1所示。

表1不同步长下Ψ_max的取值(k＝0.1、0.2…1)

从表中可以看出k越小Ψ_max越小，即l_w越小Ψ_max越小，并且l_w与Ψ_max之间是非线性关系。

下面结合具体的RRT算法进行改进，这里以Q-RRT*算法改进后的MDA-QRRT*算法为例，介绍如何规划出满足机械臂结构要求的运动路线。

步骤4：定义及步长δ(即路径补偿l_w)，以路径规划的起始点x_init作为随机树T的根节点。MDA-QRRT*算法以路径起点x_init作为随机树T的根节点，树中节点x_i用集合V存储，V在n维空间中具有n列，每一行对应一个节点的位置数据，节点间的连接用边集E存储，E中的第一列表示节点x_i的父节点是第几个节点，第2列为节点x_i与其父节点这两点连成线段与水平线之间的角度，第3列为节点x_i到其父节点再到父父节点一直到x_init处的叠加欧氏距离。始终保证V和E中的所有数据都在X_free中。

步骤5：利用随机函数选择一个x_rand，选择一个离x_rand最近的节点x_near，然后在Angle-MDA函数中根据步骤3中的结论，根据x_rand、x_near与T分析x_new的选取范围。在确定好Ψ_i即x_new的方向之后，根据下列公式可计算出x_new的位置。

x_new＝x_near+δ·Ψ_i

其中的Angle-MDA函数根据x_rand、x_near与T分析x_new的选取范围，进而确定下一个离散点x_new的具***置。

在该函数中首先求出x_rand与x_near这两点连成的线段相对于水平线的夹角ang_rand，并在T中读取x_near与其父节点连成的线段相对于水平线的夹角ang_near。本算法的目的是保证关节偏角始终在限定范围内，因为机械臂臂杆长度不恒定，所以首先需要根据节点x_near与目标点x_goal之间的距离d_goal来分析Ψ_max的取值。由于本发明选择的机械臂的臂杆长度不相等，所以在前面的路径规划中把L_i当成486即最长臂杆的长度，当路径上的离散点与目标物的距离小于L₇+L₈时，把L_i当成206计倒数第二根臂杆的长度。根据步骤三中的结论，如果d_goal<L₇+L₈，则令Ψ_max＝40°，如果d_goal≥L₇+L₈，则令Ψ_max＝22.40°。

在确定好路径中两相邻直线之间的最大偏角Ψ_max后，分析x_new是否超出可选取范围。如果x_near是起始点x_init，则在-60°到60°之间随机选取x_new的方向。否则，在-22.40°到22.40°之间随机选取x_new的方向；

如果两相邻直线之间的夹角Abs(ang_rand-ang_near)≤Ψ_max，则保持x_new的方向Ψ_i＝ang_rand。如果两相邻直线之间的夹角Abs(ang_rand-ang_near)>Ψ_max，则令Ψ_i变为可选取范围中的边界值，图6列举了ang超过选取范围的情况。在二维情况下令Ψ_i＝ang_near+Sign(ang_rand-ang_near)·Ψ_max，x_new的选取点唯一，为边界上的一个点；在三维情况下，保证两相邻直线之间的夹角为Ψ_max，x_new的选取空间为一个半径为δ·sin(Ψ_max)的圆，x_new为该圆上的一个点。

步骤6：若x_new复合条件要求，即x_new与x_near之间无障碍物，则将x_new附近的节点存放至X_neighbor中，并将这些附近节点的祖先存放至X_parent中，进而得到一个由x_new整理出的X_in集合。

步骤7：利用ChooseParent-MDA函数在X_in中找到使x_new成本最小且满足偏角要求的新父节点x_par，删除x_new与原父节点x_near的连线，连接x_new与x_par，并在T中进行修剪。

ChooseParent-MDA函数内容如下，在初始化最小父节点x_min、最小路径σ_min、最小代价c_min后，若x_new把在X_in中的节点x_in当作新的父节点会降低代价，且x_in与x_new形成的路径σ满足无碰撞，且满足从x_in的父节点到x_in的路径与x_in到x_new的路径之间的夹角不大于Ψ_max，则将节点x_in设为x_new新的父节点。其中的X_in不仅包括x_new附近的节点还包括这些节点的祖先，从而提高了算法的优化速度。

步骤8：若X_neighbor内一个节点x_from的权值大于当前x_new的权值与到该节点的距离之和，且满足偏角要求，则以当前x_new节点作为x_from的父节点，具体过程如下。

在MDA-QRRT*算法中不仅需要修剪与x_new相关的路径，还会探寻X_neighbor中所有节点的更优父节点。在该函数中分析X_neighbor中所有节点x_nei与x_new及x_new的父节点形成的新路径(即分析x_nei与x_from形成的新路径)，是否降低代价且满足无碰撞及角度限制的条件(即满足从x_new的父节点到x_new的路径与x_new到x_from的路径之间的夹角不大于Ψ_max。)。如果满足，则将x_nei的父节点更新为x_from，修剪路径。

步骤9：重复寻找x_new及修剪x_new与附近节点的连接关系，修改随机树T中的数据，直到到达目标点。

仿真结果1：针对三种障碍物，采用每个算法运行1次，产生500个根节点，分别得到如图7、8、9所示的树状分布图。在产生500个根节点的运算过程中，MDA-RRT*算法、MDA-QRRT*算法的运行时间和RRT*算法、Q-RRT*算法的运行时间较为接近，这说明了改进算法未增加算法的计算量，只是限制了下个节点的选取范围，利用该约束来保证规划出的路径一定满足关节偏角限制的要求。从仿真结果中可以看出，在产生500个根节点后，采用三种MDA+RRT算法无论是在obs1中的简单的路径规划，还是在obs2中的复杂的路径规划，还是在obs3中的三维路径规划，都可规划出满足关节偏角限制要求的路径。由于RRT算法中存在随机性，在与RRT算法相关的路径规划中，快速得到一个高质量的初始解尤为重要。

仿真结果2：将终止条件由500个节点样本修改为规划出一条可抵达终点的路径，采用这六种算法在每个障碍物中运行50次，分析采用这六种算法规划出初始路径的所需时间与质量。仿真结果如图10所示，从图中可以看出三种MDA+RRT算法的可行性与优化性都优于对应的RRT算法。在这六种算法中，MDA-QRRT*算法的可行性与优化性均为最好。

如图11所示，展示了机械臂在运动过程中的5个位置对应的静态图以及最后机械臂抓取到目标物时的静态图。图中的红色线段为采用MDA-QRRT*算法规划出的长度为3085mm的路径，图中还包括障碍物、超冗余机械臂、当前位置对应的第i-1个臂杆与第i个臂杆之间的偏角ψi的大小及一些注释。

从图中可以看出机械臂在运动过程中始终未碰到障碍物，且可以抓取到目标物，并根据前面的仿真结果可知关节偏角始终小于40°，从而验证了采用本发明中的改进算法规划出的路径的可行性。

本发明针对超冗余机械臂结构中存在关节偏角限制的问题，在RRT算法的基础上，提出了MDA+RRT算法进行路径规划。在该发明中理论分析了各参数之间的关系，将关节偏角限制的问题转换到路径偏角限制的问题，在理论分析的基础上，对几种RRT算法进行了改进，发现改进后的MDA+RRT算法相对于改进前的RRT算法未增加计算量，只是限制了节点的选取范围，利用该约束来保证规划出的路径一定满足关节偏角限制的要求。

本发明还可以在本算法的基础上进行变步长RRT算法、双向RRT算法或与其他智能RRT算法相结合，从而规划出满足约束的路径。除了RRT还有：PRM算法、A*算法、APF算法。

本发明除了将MDA+RRT算法用于超冗余机械臂的路径规划外，还可以将该算法应用于其他运动物体(如：无人汽车的自动驾驶、遥控飞机的自主规划、巡航导弹躲避雷达搜索、机器人的自主无碰行动等)的路径规划，从而规划出指定偏角范围内的路径。

本发明未详细说明部分属于本领域技术领域的公知常识。

Claims (12)

1.一种超冗余机械臂路径规划方法，其特征在于步骤如下：

步骤1：针对超冗余机械臂建立机械臂运动学模型，确定驱动绳长、臂杆偏转角度、臂杆末端点坐标之间的转换关系；

步骤2：针对环境中的障碍物建立障碍物模型，从障碍物模型中获得障碍物的位置、形状、大小信息，进而确定与障碍物对应的不可抵达区域；

步骤3：假设相邻臂杆长度相同且与路径步长l_w成线性关系，根据步骤1中确定的机械臂运动学模型，理论分析两相邻直线路径之间的最大夹角Ψ_max与第i个臂杆初始点沿其中一段直线路径运动过程中的臂杆偏角变化最大值ψ_max、臂杆长度L_i、路径步长l_w之间的关系；

步骤4：根据步骤2确定的与障碍物对应的不可抵达区域以及步骤3中的关系约束进行路径规划；

步骤4采用MDA+RRT算法进行路径规划，具体通过下述方式实现：

步骤41：定义步长δ即l_w，以路径规划的起始点x_init作为随机树T的根节点；

步骤42：随机选择一个节点x_rand，选择一个离x_rand最近的节点x_near，根据节点x_near与目标点x_goal之间的距离d_goal确定Ψ_max的取值；然后根据步骤3中的关系与x_rand、x_near、T分析x_new的选取范围，进而计算出x_new的位置；

步骤43：若x_new符合条件要求即未进入步骤2中确定的不可抵达区域，则将x_new附近的节点存放至x_neighbor中，并将这些附近节点的祖先存放至x_parent中，进而得到一个由x_new整理出的Xin集合；

步骤44：在Xin中找到使x_new成本最小且满足偏角要求的新父节点x_par，删除x_new与原父节点x_near的连线，连接x_new与x_par，并在T中进行修剪；

步骤45：若x_neighbor内一个节点x_from的权值大于当前x_new的权值与到该节点的距离之和，且满足偏角要求，则以当前x_new节点作为x_from的父节点；

步骤46：重复寻找x_new及修剪x_new与附近节点的连接关系，直到到达目标点。

2.根据权利要求1所述的一种超冗余机械臂路径规划方法，其特征在于，所述的步骤2中，将路径与障碍物之间的最近距离设置为臂杆表面上的所有点距离该臂杆轴线的最远距离加上臂杆轴线上的点与直线路径之间的最远距离，与障碍物的距离小于等于上述最近距离的范围视为不可抵达区域。

3.根据权利要求2所述的一种超冗余机械臂路径规划方法，其特征在于，所述的步骤42通过下述方式实现：

S1、若x_near是起始点x_init，则在-60°到60°之间随机选取x_new的方向，执行步骤S2，否则，在-min(Ψ_max)到min(Ψ_max)之间随机选取x_new的方向，执行步骤S2；min(Ψ_max)为节点x_near与目标点x_goal之间不同距离下确定的Ψ_max的最小值；

S2、判断是否满足下列不等式Abs(ang_rand-ang_near)≤Ψ_max，若满足，则保持x_new方向；若不满足，则令Ψ_i为可选取范围中的边界值；

其中，x_rand与x_near两点连成的线段相对于水平线的夹角为ang_rand，在T中读取x_near与其父节点连成的线段相对于水平线的夹角为ang_near。

4.根据权利要求2所述的一种超冗余机械臂路径规划方法，其特征在于步骤42中通过下述方式确定Ψ_max的取值：

d_goal<L，则令Ψ_max取值为根据倒数第二个臂杆结合步骤3中关系计算的两相邻直线路径之间的最大夹角；

d_goal≥L，则令Ψ_max取值为根据最长臂杆结合步骤3中关系计算的两相邻直线路径之间的最大夹角；

L为最后两个臂杆的长度和。

5.根据权利要求3所述的一种超冗余机械臂路径规划方法，其特征在于当Ψ_i为可选取范围中的边界值时，在二维情况下Ψ_i＝ang_near+Sign(ang_rand-ang_near)·Ψ_max，x_new为边界上的一个点；在三维情况下，保证两相邻直线路径之间的夹角为

的选取空间为一个半径为δ·sin(Ψ_max) 的圆，x_new为该圆上的一个点。

6.根据权利要求1或4所述的一种超冗余机械臂路径规划方法，其特征在于步骤3中关系通过下述方式确定：

假设l_w＝k·L_i＝k·L_i-1，k∈(0，1]，让第i-1个臂杆的起始点沿着第w段直线路径运动，其运动过程中第i个臂杆的偏角ψ_i变化的极大值出现的位置有两处；第一处为第i-1个臂杆末端点即第i个臂杆初始点运动到一段直线路径的起始点或终止点时，此时ψ_i变化的极大值记为ψ_{i_end}；第二处为第i个臂杆初始点运动到一段直线路径的中点时，该点处ψ_i变化的极大值记为ψ_{i_mid}；

求出这两个极大值后即求得臂杆偏角变化最大值max(ψ_{i_end},ψ_{i_mid})。

7.根据权利要求6所述的一种超冗余机械臂路径规划方法，其特征在于：k＝1时通过下述方式确定步骤3中的关系：

将相邻两臂杆沿轴线简化为一条直线，相邻两臂杆与路径上连续的三条直线构成两个三角形，根据三角形的正弦定理及角度变换确定关系如下：

8.根据权利要求6所述的一种超冗余机械臂路径规划方法，其特征在于：

N_end为第i个臂杆端点运动到一段直线路径的端点时，臂杆两个端点之间有几个路径转折点；

N_mid为第i个臂杆端点运动到一段直线路径的中点时，臂杆两个端点之间有几个路径转折点。

9.根据权利要求1所述的一种超冗余机械臂路径规划方法，其特征在于，所述的步骤44中，在寻找新父节点x_par时，除了满足降低代价，且无碰撞的条件外，还需满足从xin的父节点到xin的路径与xin到x_new的路径之间的夹角不大于Ψ_max；xin为Xin集合中的节点。

10.根据权利要求1所述的一种超冗余机械臂路径规划方法，其特征在于，所述的步骤45中，若以当前x_new节点作为x_from的父节点，则不仅需要满足降低代价、无碰撞的条件外，还需要满足从x_new的父节点到x_new的路径与x_new到x_from的路径之间的夹角不大于Ψ_max。

11.根据权利要求1所述的一种超冗余机械臂路径规划方法，其特征在于，所述的超冗余机械臂具有2N+1个自由度，N为机械臂的数量，不少于3。

12.根据权利要求1所述的一种超冗余机械臂路径规划方法，其特征在于，在无人汽车的自动驾驶、遥控飞机的自主规划、机器人的自主无碰行动中的应用。

Images