CN113561186B

CN113561186B - 一种机械手路径规划结果的评价方法

Info

Publication number: CN113561186B
Application number: CN202111117966.XA
Authority: CN
Inventors: 张校志; 钟泽邦; 林才纺
Original assignee: Ji Hua Laboratory
Current assignee: Ji Hua Laboratory
Priority date: 2021-09-24
Filing date: 2021-09-24
Publication date: 2021-12-07
Anticipated expiration: 2041-09-24
Also published as: CN113561186A

Abstract

本发明属于机器人技术领域，具体公开了一种机械手路径规划结果的评价方法，所述评价方法用于对机械手的路径规划结果进行评价，且在对机械手的路径规划结果进行评价时可以从避障效果、能耗、路径平滑度、运动范围、机械手运行稳定性、路径规划精细程度六个方面进行综合评价，相对于现有的路径规划方法而言，考虑更加全面，这样不仅有利于在诸多路径中选出最优路径，而且本发明的机械手路径规划结果的评价方法可以为路径规划提供技术指导，以便于规划出最优路径。

Description

一种机械手路径规划结果的评价方法

技术领域

本发明属于机器人技术领域，具体涉及一种机械手路径规划结果的评价方法。

背景技术

机械手在完成任务前，通常需要提前规划好路径，路径规划的目标不仅仅是为了实现避障以及缩短路程，而且还可能需要满足某些约束条件，例如在空间有限的情况下限定机械手的工作范围等；现有的路径规划方法在对机械手进行路径规划的时候只考虑了少数的约束条件，未能考虑全面，使得现有的路径规划方法规划得到的路径往往是局部最优的，例如专利申请号为2016110111955的中国发明专利申请公开了“一种有限空间约束的注塑机械手动力学协同仿真方法”，所述注塑机械手动力学协同仿真方法在路径规划时也只是考虑了有限空间和路径平滑度的两个约束条件；再例如专利申请号为2017105581156的中国发明专利申请公开了“一种避障任务无关人工势场引导的避障路径规划方法”，所述避障路径规划方法能够使空间机械手在完成避障路径规划问题并保证路径规划效率的前提下降低机械手的使用代价（即可对机械手能耗、末端速度、摩擦磨损进行优化），提升运行过程中与局部路径规划算法结合的能力。

然而上述两份专利文献所公开的路径规划方法均未能同时考虑避障效果、能耗、路径平滑度、运动范围、机械手运行稳定性和路径规划精细程度这六个约束条件，使得现有的机械手路径规划方法还有待改进。

发明内容

本发明在于克服现有技术的不足，提供一种机械手路径规划结果的评价方法，所述评价方法可以从避障效果、能耗、路径平滑度、运动范围、机械手运行稳定性、路径规划精细程度六个方面对规划后的路径进行评价，从而选出最优路径；并且该评价方法可以为路径规划提供技术指导，以便于规划出最优路径。

本发明解决上述技术问题的技术方案是：

一种机械手路径规划结果的评价方法，包括以下步骤：

（1）、对待评价路径进行处理，分别得到关节空间下和笛卡尔空间下的路径点序列；

（2）、对避障效果进行评价，在笛卡尔空间中通过构建包围盒模型，并围绕包围盒建立缓冲区，计算得到避障效果表征量A1；

（3）、对能耗进行评价，在获得关节能耗权重后，在关节空间下对待评价路径计算加权平均的累积运动角度，并对计算得到的加权平均的累积运动角度进行归一化处理，最终得到能耗表征量A2；

（4）、对路径平滑度进行评价，在笛卡尔空间中计算每个路径点的斜率，从斜率的角度计算路径平滑度的表征量A3；

（5）、对运动范围的评价，从关节空间中统计待评价路径的所有路径点在每个关节上的运动幅度，用来综合计算运动范围的表征量A4：

；

其中，n代表机械手的关节个数，k代表机械手中的第k个关节，

分别代表待评价路径中的所有路径点在第k个关节处能达到的最小关节角度位置和最大关节角度位置，两者的差值代表第k个关节在待评价路径上的运动幅度，

为第k个关节的额定最大运动幅度，由该关节的额定的运动范围确定；

（6）、对机械手运行稳定性进行评价，根据机械手的运动范围按照阈值T3设定边界缓冲区，统计未落在边界缓冲区的路径点的比例，以此求得机械手运行稳定性的表征量A5：

A5=未落在边界缓冲区的路径点的个数/总路径点的个数；

（7）、对路径规划精细程度进行评价，当路径规划结果满足无碰撞要求后，计算路径点密度

，所述

的计算公式为：

其中，L为待评价路径在笛卡尔空间的长度，由相邻路径点连线的长度累加得到；m为待评价路径中的路径点的个数；在确定好机械手和任务目标后，只考虑基础约束，所述基础约束指的是机械手能够从起点无碰撞地抵达终点，采用基于随机搜索的路径生成方法大量生成满足任务目标的随机路径，然后计算每条随机路径的

，接着计算得到所有随机路径的

的平均值

和方差

后，对待评价路径的

进行归一化，得到路径规划精细程度的表征量A6：

（8）、对以上求得的表征量A1-A6进行处理，最终生成对路径规划结果的评价，即对待评价路径的综合评价

：

其中，N为约束条件的个数，且N=6；h代表第h个约束条件；通过比较多条待评价路径的

的大小，当值越大时，代表该路径规划结果越优。

优选的，在步骤（2）中，所述避障效果表征量A1的求取步骤为：

（2-1）、在仿真环境中建立机械手的模型，并建立机械手各个关节的包围盒，判断当机械手处于待评价路径的路径点时，是否会导致机械手的非相邻关节之间发生碰撞，若发生碰撞，则路径规划结果不合格；其中，判断机械手的非相邻关节之间是否发生碰撞，通过判断机械手的非相邻关节的包围盒是否发生接触，若发生接触，则表明机械手的非相邻关节之间发生碰撞；

（2-2）、建立环境障碍物的模型，并建立障碍物包围盒，围绕障碍物包围盒，若机械手处于待评价路径的路径点时，机械手各个关节的包围盒的任意部分落入障碍物包围盒内，判断机械手与障碍物发生碰撞，则路径规划结果不合格；

（2-3）、若不存在路径点使得当机械手处于该路径点时，机械手的非相邻关节之间发生碰撞或者使得机械手与障碍物发生碰撞，则在障碍物包围盒外按照阈值T1扩大范围后建立障碍物包围盒缓冲区，统计落在障碍物包围盒缓冲区内的路径点的个数，并计算避障效果表征量A1：

其中，在未发生碰撞的情况下，A1取值范围为[0,1]，且A1的值越大则代表避障效果越好；当发生碰撞时，A1取值为-100，即路径规划结果不合格。

优选的，在步骤（2-3）中，阈值T1为50mm以上；若机械手因任务需求需要经过孔状障碍物，T1为孔状障碍物的通孔半径的1/4-1/2。

优选的，在步骤（3）中，所述能耗表征量A2的求取步骤为：

（3-1）、计算关节能耗权重，对于多轴机械手，根据每个关节的额定功率P获取每个关节的关节能耗权重；

；

其中，n为机械手的关节个数，P_k为第k个关节的额定功率，W_k为第k个关节的关节能耗权重；

（3-2）、取待评价路径在关节空间的表示：

Path={[angle1_1,angle2_1,…,anglen_1],[angle1_2,angle2_2,…,anglen_2],…,[angle1_m,angle2_m,…,anglen_m]}；

其中，Path代表待评价路径；angle1_m…anglen_m分别代表待评价路径中第m个路径点在关节空间下的第1个关节…第n个关节的关节角度；

将相邻的路径点做差，对每一个做差后的数据取绝对值，得到长度为m-1的差值序列Path_difference，其中，

Path_difference={[|angle1_2-angle1_1|,|angle2_2-angle2_1|,…,|anglen_2-anglen_1|],…,[|angle1_m-angle1_m-1|,|angle2_m-angle2_m-1|,…,|anglen_m-anglen_m-1|]}；

结合差值序列Path_difference和步骤（3-1）得到的关节能耗权重，得到加权平均累积运动角度

：

；

其中，m为待评价路径Path中的路径点的个数，n为机械手的关节个数；i代表第i个路径点，而k则代表第k个关节；Path_difference[i][k]代表在关节空间中，待评价路径的第i个路径点与第i-1个路径点在第k个关节处的关节角度的差值；

（3-3）、在确定机械手的型号和起点与终点任务后，只考虑基础约束，所述基础约束指的是机械手能够从起点无碰撞地抵达终点，采用基于随机搜索的路径生成方法大量生成满足任务目标的随机路径；通过采用步骤（3-1）和步骤（3-2）计算每条随机路径的

值，得到所有随机路径的

值，以此构成加权能耗参考集合，所述加权能耗参考集合包含待评价路径的

值，统计加权能耗参考集合的平均值

和方差

，最终能耗表征量A2用下述公式计算：

其中，A2取值在[-1,1]之间，A2的值越大代表能耗越小，反之能耗越大。

优选的，在步骤（4）中，所述路径平滑度的表征量A3的求取步骤为：

（4-1）、在笛卡尔空间下，待评价路径表示为：

Path={[x_1,y_1,z_1,α_1,β_1,γ_1],…,[x_m,y_m,z_m,α_m,β_m,γ_m]}；

其中x,y,z代表机械手末端在笛卡尔坐标系的空间位置，α,β,γ代表机械手末端绕笛卡尔坐标系的三个坐标轴的旋转角度；m表示待评价路径的路径点的个数；

（4-2）、对待评价路径的开始点和结束点两端进行填充，两边各填充q个路径点；

（4-3）、对填充的待评价路径上原本就存在的路径点，获取其前后各q个路径点，总共2q+1个点拟合一条直线，得到该路径点处的直线方程，并计算直线方程的方向向量，以此类推得到待评价路径中所有路径点的拟合直线的方向向量；

（4-4）、计算所有相邻的路径点的方向向量的夹角，已知两条拟合直线的方向向量为v1和v2，两者的夹角θ的计算公式如下：

其中，

表示方向向量v1和v2的点积，

和

分别表示方向向量v1和v2的模；

（4-5）、统计夹角中小于阈值T2的路径点的比例，计算路径平滑度的表征量A3：

式中，

为夹角中小于阈值T2的路径点的个数；m为待评价路径的路径点的个数；A3值越大代表路径越平滑。

优选的，阈值T2的取值区间为[0°，180°]，其中，阈值T2的取值越小，代表对路径平滑度的要求越高。

优选的，在步骤（4-3）中，采用最小二乘法或者SVD分解法来进行直线拟合。

优选的，在步骤（6）中，阀值T3为对应关节额定最大运动范围的5%。

本发明与现有技术相比具有以下的有益效果：

1、本发明的机械手路径规划结果的评价方法可以从避障效果、能耗、路径平滑度、运动范围、机械手运行稳定性和路径规划精细程度六个方面对规划后的路径进行评价，从而选出最优路径。

2、本发明的机械手路径规划结果的评价方法考虑因素更加全面，可以为改进局部最优的路径规划结果提供指导，帮助机械手找到更好的路径规划结果。

3、本发明的机械手路径规划结果的评价方法可以路径规划提供技术指导，以便于规划出最优路径。

附图说明

图1为本发明的机械手路径规划结果的评价方法的流程图。

图2为对避障效果进行评价时所采用的障碍物包围盒缓冲区示意图，其中，标号A为障碍物包围盒，标号B为缓冲区，标号C为障碍物，“*”表示落在缓冲区的路径点，“·”表示缓冲区外的路径点。

图3为对能耗进行评价时的流程框图。

图4为对待评价路径一端进行扩充的原理示意图。

图5为具体案例中对待评价路径的首尾扩充路径点的示意图，其中，扩充的路径点用“x”表示，对应图中的H点和I点，原路径点用圆点表示，对应图中的E点、F点、G点。

图6-图8分别为对扩充后的路径点进行直线拟合得到的直线示意图，其中，x代表扩充后的路径点，其中，位于左下方的H点为扩充后路径的起点，位于右上方的I点为扩充后路径的终点；E点、F点、G点三个圆点则代表待评价路径的三个原路径点；且图6、图7和图8中的直线分别代表HEF、EFG和FGI三条拟合直线。

具体实施方式

下面结合实施例及附图对本发明作进一步详细的描述，但本发明的实施方式不限于此。

参见图1-图4，本发明的机械手路径规划结果的评价方法用于评价机械手路径规划的结果，可作为一个衡量标准，比较在同一个场景中两个路径规划结果的好坏，帮助选择最优路径或者改进路径，待评价的机械手路径规划结果（即待评价路径）应该是一系列关节角度的位置或者末端位姿的集合,假设待评价路径点的个数（即路径点的总个数）为m，机械手的关节数为n，则以关节角度位置为例，路径规划结果可表示为：

Path={[angle1_1,angle2_1,…,anglen_1],[angle1_2,angle2_2,…,anglen_2]，…,[angle1_m,angle2_m,…,anglen_m]}；

其中，Path代表待评价路径；angle1_m…anglen_m分别代表待评价路径中第m个路径点在关节空间下第1个关节…第n个关节的关节角度；

每一个角度位置能够根据机械手正运动学解算成为笛卡尔空间的一个机械手末端位姿（本发明采用空间的三维位置和欧拉角表示，其它同类表示方法有三维位置+四元素/旋转矩阵），实现关节空间到笛卡尔空间的转换，转换关系如下：

F([angle1_i,angle2_i,…,anglen_i])=[x_i,y_i,z_i,α_i,β_i,γ_i]；

其中，[angle1_i,angle2_i,…,anglen_i]为第i个路径点在关节空间下的第1个关节…第n个关节的关节角度；而[x_i,y_i,z_i,α_i,β_i,γ_i]则为第i个路径点在笛卡尔空间下的末端位姿。

其中，F(X)代表正运动学计算函数，可由DH参数建模获得；同样每一个末端位姿能够根据机械手逆运动学解算成为关节空间的一个角度位置，实现笛卡尔空间到关节空间的转换，G(X)代表逆运动学计算函数（可能存在无解，或者多解情况，这里为说明过程，先假设逆解算能够得到唯一解），转换关系如下：

G([x_i,y_i,z_i,α_i,β_i,γ_i])=[angle1_i,angle2_i,…,anglen_i]；

在评估一个路径序列前，首先对待评价路径序列进行预处理，得到关节空间和笛卡尔空间的路径点序列，然后从以下六个方面来分析路径规划结果的优劣。

（1）、避障效果评价

首先基本要求是当机械手处于待评价路径上的每一个路径点的时，机械手不与周边环境（例如周边环境中的障碍物）发生碰撞，机械手自身也不发生碰撞（即机械手的非相邻关节之间不发生碰撞），其次要求容错率最高（考虑机械手抖动和定位误差），路径规划结果沿线环境复杂度越低越好，能够避免发生碰撞的范围越大越好。

对每个障碍物，围绕其周边建立两个区域，一个是基于最小包围原则建立的包围盒，用于判断是否发生碰撞；一个是在包围盒的基础上再扩充了一个缓冲区，所述缓冲区不包含包围盒这部分空间，该缓冲区表示靠近障碍物但未与障碍物发生碰撞（可参见图2，其中，标号A为障碍物包围盒，标号B为缓冲区，标号C为障碍物，“*”表示落在缓冲区的路径点，“·”表示缓冲区外的点）；具体步骤为：

（1）、在仿真环境中建立机械手的模型，并建立机械手各个关节的包围盒，判断当机械手处于待评价路径的路径点时，是否会导致机械手的非相邻关节之间发生碰撞，若发生碰撞，则路径规划结果不合格；其中，判断机械手的非相邻关节之间是否发生碰撞，通过判断机械手的非相邻关节的包围盒是否发生接触，若发生接触，则表明机械手的非相邻关节之间发生碰撞；

（2）、建立环境障碍物的模型，围绕障碍物包围盒（球形包围盒或者AABB包围盒）,如果机械手处于待评价路径的路径点时，机械手各个关节的包围盒的任意部分落入障碍物包围盒内，则判断机械手与障碍物发生碰撞，路径规划结果不合格；

（3）、若不存在路径点使得当机械手处于该路径点时，机械手自身发生碰撞或者使得机械手与障碍物发生碰撞，按照阈值T1扩大一定范围后建立障碍物包围盒缓冲区，阈值T1一般设置为50mm以上，如果机械手因任务需求必须需要经过孔状障碍物，阈值T1则不受限制，可以根据孔径大小设置，例如为孔半径的1/4-1/2；其中，阈值T1越小则代表对规避障碍物的要求越高，因此阈值T1不宜过大，不然会导致在障碍物较多的时候障碍物包围盒缓冲区占据过多空间，影响对路径规划结果的避障效果的判断；统计落在障碍物包围盒缓冲区内的路径点的个数；所述避障效果表征量A1的计算如下：

；

其中，在未发生碰撞的情况下，A1取值范围为[0,1]，且A1的值越大则代表避障效果越好；当发生碰撞时，A1取值为-100，即路径结果不合格。

2、能耗评价

机械手所有关节需要运动的角度累积最小，可以用以下的统计量表征：关节空间多个关节累积欧式距离（加权，例如因为负载的差异，不同的关节转动同样的角度能耗不同，那么能耗大的关节尽可能需要减少累积转动角度）；其中，能耗表征量A2的计算步骤如下：

第一步，计算关节能耗权重，对于多轴机械手，根据每个关节的额定功率获取每个关节的关节能耗权重；

；

第二步，根据关节能耗权重，对待评价路径计算加权平均的累积运动角度：

取待评价路径中的路径点在关节空间的表示；

Path={[angle1_1,angle2_1,…,anglen_1],…,[angle1_m,angle2_m,…,anglen_m]}；

根据差值序列Path_difference，并结合关节能耗权重可以得到加权平均累积运动角度

：

；

其中，m为待评价路径Path中的路径点的个数，n为机械手的关节个数；i代表第i个路径点，而k则代表第k个关节；Path_difference[i][k]代表在关节空间下，待评价路径的第i个路径点与第i-1个路径点在第k个关节处关节角度的差值。

第三步，归一化，由于计算得到的

值区间范围较大，受到具体机械手型号、路径的起点和终点的影响，为保证能耗表征量A2可量化且有意义，可以在确定机械手型号与路径的起点和终点任务后，只考虑基础约束，所述基础约束指的是机械手能够从起点无碰撞地抵达终点，采用基于随机搜索的路径生成方法如快速扩展随机树算法等大量生成满足任务目标的随机路径；并用第一步和第二步的方法和参数计算每条随机路径的

值，得到所有随机路径的

值后，构建加权能耗参考集合（所述加权能耗参考集合包含待评价路径的

值），统计加权能耗参考集合的平均值

和方差

，最终能耗表征量A2可用下述公式计算：

其中，A2取值大部分情况在-1到1之间，A2值越大代表能耗越小，反之能耗越大。

3、路径平滑程度评价

路径平滑程度越大越有利于保护机械手的电机，减少大角度的转弯动作：在笛卡尔空间中，计算每个路径点的斜率，从斜率的角度出发计算路径平滑的表征量，具体如下：

（1）、取待评价路径在笛卡尔空间下的末端位姿表示形式；

Path={[x_1,y_1,z_1,α_1,β_1,γ_1],…,[x_m,y_m,z_m,α_m,β_m,γ_m]}；

其中x,y,z代表机械手末端在笛卡尔坐标系的空间位置，α,β,γ代表机械手末端绕笛卡尔坐标系的三个坐标轴的旋转角度；m表示待评价路径Path的路径点的个数；

（2）、对待评价路径的开始点和结束点两端进行填充，两边各填充q个点；以从待评价路径的尾部开始填充的第一个路径点为例来说明填充过程：

参见图4，以尾部第一个路径点为对称中心，计算出倒数第二个路径点相对于对称中心的镜像点，以该镜像点为新的填充点，重复操作直到尾部相对原序列增加了q个路径点。

（3）、对填充的路径上原本就存在的路径点，获取其前后各q个路径点，总共2q+1个路径点拟合一条直线，方法可采用最小二乘法或者SVD分解法来进行直线拟合，得到该路径点处的直线方程。从直线方程的角度，求得对应的方向向量，其中，方向向量一般有两个，且方向相反，这里取与该路径点朝向下一个路径点方向一致的方向向量作为该路径点的方向向量，以此类推得到所有路径点的方向向量；

（4）、计算所有相邻的路径点的方向向量的夹角，已知两条拟合直线的方向向量为v1和v2，两者的夹角θ计算公式如下：

其中，

表示方向向量v1和v2的点积，

和

表示方向向量v1和v2的模；

（5）、统计夹角中小于阈值T2的路径点的比例，作为路径平滑度的表征量A3，阈值T2的取值区间为[0°，180°]，通常阈值T2可以设定为90°、60°等，根据实际情况，灵活选择阈值T2的值，其中，阈值T2越小代表对路径平滑度的要求越高；

式中，

为夹角中小于阈值T2的路径点的个数；m为待评价路径的路径点的个数；A3值越大代表路径更为平滑。

4、运动范围评价

在需要限制运动范围的场景中，越小的运动范围能够节省空间，同时也越能降低场地运营成本，例如多机械手运动需要互相不干扰的场景中；可量化指标为，从关节空间中统计机械手运动路径的所有路径点在每个关节上的运动幅度，运动幅度越小越好，运动范围的表征量A4计算如下：

；

n代表机械手的关节个数，k代表机械手中的第k个关节，

分别代表待评价路径中的所有路径点在第k个关节处能达到的最小关节角度位置和最大关节角度位置，两者的差值代表第k个关节在待评价路径中的运动幅度，

为第k个关节的额定最大运动幅度，由额定的运动范围确定,所述额定的运动范围可从机械手出厂说明书获取,其中，第k个关节的额定运动范围为[Ak_min,Ak_max]，其中，Ak_min为第k个关节的额定最小关节角度位置，Ak_max为第k个关节的额定最大关节角度位置,第k个关节额定最大运动幅度Range_k=Ak_max-Ak_min。

5、机械手运行稳定性最优

路径点靠近机械手运动边界范围的比例越低越好，根据机器手关节的运动范围按照阈值T3设定边界缓冲区，统计落在边界缓冲区的路径点的比例，比例越大，路径规划结果越差。T3为百分比，一般可取5%，对第k个关节按照阈值T3设定边界缓冲区的规则为：

根据机械手说明书，获取第k个关节的额定运动范围为[Ak_min,Ak_max]，得到第k个关节额定最大运动幅度Rangek，将运动范围前后各缩减T3，缩减的区域为边界缓冲区，表示为[Ak_min,Ak_min+Rangek×T3]和[Ak_max-Rangek×T3，Ak_max]；同理可以得到所有关节的边界缓冲区。统计满足不落在任一关节的边界缓冲区（即落在[Ak_min+Rangek×T3,Ak_max-Rangek×T3]）中的路径点的个数，稳定性表征量A5计算如下：

A5=未落在边界缓冲区的路径点的个数/总路径点的个数;

6、路径规划精细程度

路径规划精细程度可用路径点密度表示，当路径规划结果满足无碰撞要求后，一般来说，路径点密度越大越好，越大说明路规划结果越精细，路径点密度过小，路径点过于稀疏，会对于后续的轨迹规划带来巨大的压力，极端案例如只给起点和终点也可以认为是一种路径规划结果，但是这种路径规划结果等同于无路径规划，这会为机械手的运动带来了极大的不确定性。

路径点密度

计算如下：

其中，L为路径在笛卡尔空间的长度，由相邻路径点连线的长度累加得到；m为路径点的个数，同样，计算

的过程中可采用与计算能耗表征量A2一样的方法，即在确定好机械手和任务目标后，只考虑基础约束，所述基础约束指的是机械手能够从起点无碰撞地抵达终点，采用基于随机搜索的路径生成方法如快速扩展随机树算法等大量生成满足任务目标的随机路径，按照上述公式计算每条随机路径的

，得到所有随机路径的

值后，计算这些随机路径的

值的平均值

和方差

后，再对待评价路径的

进行归一化：

其中，A6取值大部分情况在-1到1之间，A6取值越大代表密度越大，路径规划结果越精细。

7、综合评价

根据以上的评判标准，最终生成对运动规划结果的评价

：

其中，N为约束条件的个数，即N=6；h代表第h个约束条件；通过比较多条待评价路径

的大小，当

值越大时，代表该路径规划结果越优。

具体案例

参见图1-图8，以下则以具体的案例来对本发明的评价方法进行简述：

首先，设定待评价路径为一个3轴机械手（即n=3）产生的结果，在关节空间中的路径可表示为：

Path={[0°,0°,30°],[0°,20°,30°],[0°,20°,50°]}；

在笛卡尔空间中的路径可表示为：

Path={[135,80,300],[140,90,310],[150,100,320]}；

两种表示方式可通过正逆运动学解算互相转换。

第一步：避障效果评价

检测机械手处于每一个路径点的状态的时候是否发生了自身与自身的碰撞、自身与环境的碰撞，采用的技术为本专业领域常用的建立包围盒的方法，通过判断机械手各个关节的包围盒（例如圆柱体包围盒）和环境障碍物包围盒（AABB包围盒、圆柱体包围盒、球形包围盒等）的关系判断碰撞。

如果发生碰撞，返回路径不合格的结果，评价结束，不执行后续判断；

若未发生碰撞，按照公式统计未落在障碍物包围盒缓冲区的路径点的个数，计算避障效果表征量A1;在本案例中，假设有三个路径点中机械手均未发生自身与自身，自身与环境障碍物之间的碰撞，但是有一个路径点使得当机械手处于该路径点的时候会有某一部分处于障碍物包围盒缓冲区内，那么避障效果表征量A1=2/3；

第二步：能耗评价

（1）、查询机械手说明书，得到三个关节电机的额定功率为50瓦特,30瓦特,20瓦特，那么机械手的三个关节的关节能耗权重分别为0.5,0.3,0.2；

（2）、计算加权平均累积运动角度，

Path_difference={[0°,20°,0°],[0°,0°,20°]}；

=0.5x(0+0)+0.3x(20+0)+0.2x(0+20)=1（单位：度）

（3）、归一化，基于同一机械手模型和起点、终点，只考虑基础约束，所述基础约束指的是机械手能够从起点无碰撞地抵达终点，采用基于随机搜索的路径生成方法如快速扩展随机树算法等大量生成满足任务目标的随机路径，用同样的方法和参数计算每条随机路径的

值，构建加权能耗参考集合（包含待评价路径的

值），统计得到加权能耗参考集合的平均值

=0.8度和方差

=0.4后，再对待评价路径的

进行归一化：

；

第三步：路径平滑度评价

（1）、取路径在笛卡尔空间的表示方式；

Path={[135,80,300],[140,90,310],[150,100,320]}；

（2）、对路径点进行首尾扩充，首尾各填充1个，预备用3个点为一对来计算斜率：

首部扩充点（即H点）为：2×[135,80,300]-[140,90,310]=[130,70,290];

尾部扩充点（即I点）为：2×[150,100,320]-[140,90,310]=[160,110,330];

扩充后的路径={[130,70,290],[135,80,300],[140,90,310],[150,100,320],[160,110,330]]}；

填充过程如图4所述，黑色圆点为原路径点，“x”表示的点为扩充的路径点。具体在本案例中扩充后的路径如图5所示，E点、F点和G点为路径中的原路径点，E为起点，G为终点；H点和I点为扩充后的路径点。

（3）、从起点开始按顺序每3个路径点为一组分别进行直线拟合，如图6-图8所示，得到3条直线的直线方程;

H、E、F三点拟合的直线方程为：

；

E、F、G三点拟合的直线方程为：

；

F、G、I三点拟合的直线方程为：

；

根据上述直线方程得到每个路径点沿着路径往后延伸的方向向量，左下角的H点为路径起点扩充点，右上角的I点为路径终点扩充点，均用“x”表示；可得三个路径点处拟合直线的方向向量为：

=(0.33,0.66,0.66)；

=(0.67,0.52,0.52)；

=(0.57,0.57,0.57)；

其中，

代表H、E、F三点拟合直线的方向向量，由H点指向F点，

代表E、F、G三点拟合直线的方向向量，由E点附近指向G点附近，

代表F、G、I三点拟合直线的方向向量，由F点指向I点。

（4）、计算所有相邻的点的方向向量的夹角θ：

;

;

（5）、将阈值T2设置为30°，则所有路径点均满足不超过阈值T2的条件；

故A3=1；

第四步：运动范围评价

查阅机械手出厂说明书可知，3轴的运动范围分别为45度-145度，根据路径可以计算得：

A4=1-(0+0.2+0.2)/3=0.87；

第五步：机械手运行稳定性评价

3轴机械手三个关节的运动范围均为45度-145度，额定最大运动幅度为100度，两端各缩减额定最大运动幅度的5%，得到每个关节的边界缓冲区范围为[45度,50度]和[140度,150度]，非边界缓冲区范围均为[50度,140度]，则Path={[0°,0°,30°],[0°,20°,30°],[0°,20°,50°]}，全部落在非边界缓冲区内；

故A5=1；

第六步：路径点密度评价

（1）、计算路径在笛卡尔空间的线段累积长度，L=33.3（cm）,则

=3/33.3(个/cm)；

（2）、归一化，因为

单独无法作为绝对的衡量标准，可采用在与计算能耗表征量A2一样的方法，在确定好机械手和任务目标后，只考虑基础约束，所述基础约束指的是机械手能够从起点无碰撞地抵达终点，采用基于随机搜索的路径生成方法如快速扩展随机树算法等大量生成满足任务目标的随机路径，按照上述公式计算每条随机路径的

，得到所有随机路径的

的平均值

=0.08(个/cm)和方差

=0.04，对待评价路径的

进行归一化；

第七步：综合评价

综合评价前可以自定义每个约束条件占据的权重或者采用平均分配的方法分配权重，本实施例中则平均分配权重，最终生成对运动规划结果的评价：

由于

的评价涵盖了很多与机械手强相关的变量例如运动范围、关节耗能排行等等，且与路径规划的起始点有关，因此

的绝对值不具备太多参考价值，

体现的一个相对的衡量标准，最好是用于同一机械手在同一个任务场景下的路径规划结果的评价，具体做法是：基于某些路径规划方法用于同一机械手在同一个任务场景下生成不同的路径，代入本发明的评价方法中，得到的

值越大，则代表路径结果越优。

在平均分配各方面权重的情况下，根据经验，

>0.35的时候路径规划结果较好，

<0.15的时候路径规划结果较差。

上述为本发明较佳的实施方式，但本发明的实施方式并不受上述内容的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、块合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。