一种六自由度机械臂空间轨迹优化方法

摘要

本发明涉及一种六自由度机械臂空间轨迹优化方法，该方法主要分为两个阶段：路径简化和路径平滑；路径简化部分沿着待优化路径随机采样两个非连续段上的点，若直接连接这两个点的路径是有效的、无碰撞的，则用其替换连接两个点的原始路径段，以消除多余的状态点；路径平滑部分，利用分段贝塞尔曲线思想，但与之不同的是，本发明并没有用多项式拟合出曲线，而是通过多次迭代不断增加状态点、更新状态点位置来达到平滑的目的。该方法能够平滑原始路径，减少路径上的冗余状态点，提高机械臂在运动过程中关节轨迹的平滑性，对于减少机械臂关节驱动和传动器件的振动和冲击，提高机械臂轨迹精度和机械臂本体的使用寿命具有重要的意义。

权利要求

1.一种六自由度机械臂空间轨迹优化方法，其特征在于，其包括以下步骤：步骤S1、初始化所述机械臂空间轨迹，该空间轨迹为包含n个状态点的待优化的分段线性路径γ＝(state0，state1，...，statei，...，staten‑1)；

步骤S2、对该分段线性路径进行路径简化，所述路径简化包含：沿所述分段线性路径取两个随机采样点R0、R1，根据所述两个随机采样点R0、R1的位置构造出两个状态点P0、P1，调用碰撞检测算法，若状态点P0、P1的直接路径与障碍有碰撞，则迭代次数加1，否则判断状态点P0、P1是否在同一段线性路径上，若是，则迭代次数加1，否则，计算该两个状态点P0、P1的直接距离d1，以及这两个状态点沿所述分段线性路径γ的距离d2，若d1＜d2，则用线段P0‑P1替代原有路径，消除原有路径上的状态点，迭代次数加1；

步骤S3、判断迭代次数，若迭代次数未达到最大迭代次数，则重复执行步骤S2，否则该路径简化结束，获得简化路径γ’＝(state’0，state’1，...，state’i，...，state’n‑l)；

步骤S4、对获得的简化路径γ’进行路径平滑，所述路径平滑包含：在路径γ’上的每一分段路径上增加一个新的状态点，对下标索引i是除0外的偶数的状态点依次更新：在状态点state’i‑1与状态点state’i分段的中点处取一点记为temp1；在状态点state’i与状态点state’i+1分段的中点处取一点记为temp2；在状态点temp1与状态点temp2分段的中点处取一点记为temp3，分别连接状态点state’i‑1与状态点temp3、状态点temp3与状态点state’i+1构成两个分段路径，调用碰撞检测算法验证所述两个分段路径的有效性，若有一条无效则此次更新结束；若均有效，计算状态点state’i与状态点temp3的直接距离d，若d大于某一设定阈值minChange，用状态点temp3更新状态点state’i，否则结束此次更新；

步骤S5、当路径γ’上的所有状态点更新完毕，迭代次数加1，若迭代次数小于设定迭代次数，重复执行步骤S4，反之路径平滑步骤结束。

2.根据权利要求1的所述优化方法，其特征在于，所述步骤S1中statei表示为：路径γ上的状态点，表示六自由度机械臂在某一时刻在构型空间的信息，用一个6维向量表示statei＝(r1，r2，…，rj，…，r6)，rj代表机械臂各轴的角度，j＝1‑6。

3.根据权利要求1的所述优化方法，其特征在于，所述步骤S2中采样两个随机点R0、R1的方法为：

在范围(0，dists[n‑1])上采样一个随机值T0，以表示在路径γ上随机采样一个点R0，其沿着路径γ距离第一个状态点的长度为T0；同理，随机采样点R1其沿着路径γ距离第一个状态点的长度为T1；其中，dists表示数组，其大小为n，dists[i‑1]表示前i个状态点的路径累计长度，dists[0]＝0，dists[i]＝dists[i‑1]+D(statei‑1，statei)。

4.根据权利要求3的所述优化方法，其特征在于，所述步骤S2中状态点P0、P1的构造方法为：

S2.1用二分法在数组dists中查找第一个大于或等于T0的数，并返回这个数的索引next，路径γ上的第next个状态点statenext即为随机采样点R0最近的下一个状态点；用二分法在数组dists上查找最后一个小于或等于T0的数，并返回这个数的索引previous，路径γ上的第previous个状态点stateprevious即为随机采样点R0最近的上一个状态点；

S2.2若随机采样点R0与状态点statenext的距离dists[next]‑T0＜threshold，则将statenext作为简化路径一端的状态点P0；若随机采样点R0与状态点stateprevious的距离T0‑dists[previous]＜threshold，则将stateprevious作为简化路径一端的状态点P0；若前两种情况均不满足，则在随机采样点R0处新增一个状态点作为简化路径一端的状态点P0；同理可根据随机采样点R1得到另一状态点P1。

5.根据权利要求1的所述优化方法，其特征在于，所述步骤S2中状态点P0、P1的直接距离d1表示为：d1＝D(P0，P1)。

6.根据权利要求1的所述优化方法，其特征在于，所述步骤S2中状态点P0、P1的沿所述分段线性路径γ的距离d2表示为：假设沿着路径γ，P0与P1之间存在k个状态点，则d2＝D(P0，state1)+D(state1，state2)+…+D(statek，P1)。

7.根据权利要求1的所述优化方法，其特征在于，所述步骤S4中在两个状态点分段的中点处取一点temp的计算方法为：

假设两个状态点分别为statefrom、stateto：则

temp from to from

其中rj ＝rj +(rj ‑rj )*0.5，j＝1‑6。

8.根据权利要求1的所述优化方法，其特征在于，所述步骤S4中阈值minChange＝length*0.01，其中length为路径γ’的长度。

9.根据权利要求4的所述优化方法，其特征在于，所述步骤S2.2中threshold表示为：随机采样点近似为真实状态点的判断阈值，threshold＝n*0.005。

10.根据权利要求3、5或6的所述优化方法，其特征在于，函数D表示为两个状态点的直接距离：

其中，i为状态点state的下标索引。

说明书

一种六自由度机械臂空间轨迹优化方法

技术领域

[0001] 本发明涉及机器人运动规划领域，具体涉及一种六自由度机械臂空间轨迹优化方法。

背景技术

[0002] 工业生产生活中，机械臂主要用于码垛、焊接、抛光等作业，在整个机械臂的运动过程中需要更高的运动精度。这种运动往往是点到点的运动，需要沿着指定的路径进行连
续运动，不仅需要确定机械臂关节的初始点位型和终点位型，还要对整个目标路径的若干
路径点进行轨迹规划。由于工业上使用的机械臂多是高纬度的，在高维空间内规划轨迹路
径往往采用的是基于概率的路径规划算法。基于概率采样的路径规划算法，比如RRT、PRM及
其改进算法具有在高维空间能够快速解决运动规划的问题，但是用于构造求解路径的是一
些空间离散状态集，并且由于该类路径算法的概率性、随机性，综合导致了规划出来的路径
存在冗余状态节点过多、路径转折点多、路径不平滑等问题。

[0003] 若运动规划路径存在上述几个问题，机械臂在工作过程中容易出现位移、速度、加速度的突变，将影响工作过程中的稳定性。因此可见，恰当的轨迹规划及优化对机械臂高
效、精确、可靠运行至关重要。

[0004] 轨迹路径优化算法的选择具体取决于机械臂所使用的成本度量。为了适应工业要求，轨迹优化必须非常快速。通常的路径优化方法是利用启发式算法，从初始路径开始，在
其附近搜索大量的空间状态点集，找到经过这些空间状态点的最佳路径，然后迭代搜索局
部最小值。这种方法很通用，但是很耗费资源，速度可能很慢。

发明内容

[0005] 针对现有技术中存在的技术问题，本发明的首要目的是提供一种六自由度机械臂空间轨迹优化方法，该方法主要分为两个阶段：路径简化和路径平滑。路径简化部分沿着待
优化路径随机采样两个非连续段上的点，若直接连接这两个点的路径是有效的、无碰撞的，
则用其替换连接两个点的原始路径段，以消除多余的状态点；路径平滑部分，利用分段贝塞
尔曲线思想，但与之不同的是，本发明并没有用多项式拟合出曲线，而是通过多次迭代不断
增加状态点、更新状态点位置来达到平滑的目的。该方法能够平滑原始路径，减少路径上的
冗余状态点，提高机械臂在运动过程中关节轨迹的平滑性，对于减少机械臂关节驱动和传
动器件的振动和冲击，提高机械臂轨迹精度和机械臂本体的使用寿命具有重要的意义。基
于上述目的，本发明至少提供如下技术方案：

[0006] 一种六自由度机械臂空间轨迹优化方法，其包括以下步骤：

[0007] 步骤S1、初始化所述机械臂空间轨迹，该空间轨迹为包含n个状态点的待优化的分段线性路径γ＝(state0,state1,…,statei,…,staten‑1)；

[0008] 步骤S2、对该分段线性路径进行路径简化，所述路径简化包含：沿所述分段线性路径取两个随机采样点R0、R1，根据所述两个随机采样点R0、R1的位置构造出两个状态点P0、P1，
调用碰撞检测算法，若状态点P0、P1的直接路径与障碍有碰撞，则迭代次数加1，否则判断状
态点P0、P1是否在同一段线性路径上，若是，则迭代次数加1，否则，计算该两个状态点P0、P1
的直接距离d1，以及这两个状态点沿所述分段线性路径γ的距离d2，若d1替代原有路径，消除原有路径上的状态点，迭代次数加1；

[0009] 步骤S3、判断迭代次数，若迭代次数未达到最大迭代次数，则重复执行步骤S2，否则该路径简化结束，获得简化路径γ’＝(state’0,state’1,…,state’i,…,state’n‑1)；

[0010] 步骤S4、对获得的简化路径γ’进行路径平滑，所述路径平滑包含：在路径γ’上的每一分段路径上增加一个新的状态点，对下标索引i是除0外的偶数的状态点依次更新：在
状态点state’i‑1与状态点state’i分段的中点处取一点记为temp1；在状态点state’i与状态
点state’i+1分段的中点处取一点记为temp2；在状态点temp1与状态点temp2分段的中点处取
一点记为temp3，分别连接状态点state’i‑1与状态点temp3、状态点temp3与状态点state’i+1
构成两个分段路径，调用碰撞检测算法验证所述两个分段路径的有效性，若有一条无效则
此次更新结束；若均有效，计算状态点state’i与状态点temp3的直接距离d，若d大于某一设
定阈值minChange，用状态点temp3更新状态点state’i，否则结束此次更新；

[0011] 步骤S5、当路径γ’上的所有状态点更新完毕，迭代次数加1，若迭代次数小于设定迭代次数，重复执行步骤S4，反之路径平滑步骤结束。

[0012] 进一步的，所述步骤S1中statei表示为：路径γ上的状态点，表示六自由度机械臂在某一时刻在构型空间的信息，用一个6维向量表示statei＝(r1,r2,…,rj,…,r6)，rj代表
机械臂各轴的角度，j＝1‑6。

[0013] 进一步的，所述步骤S2中采样两个随机点R0、R1的方法为：

[0014] 在范围(0,dists[n‑1])上采样一个随机值T0，以表示在路径γ上随机采样一个点R0，其沿着路径γ距离第一个状态点的长度为T0；同理，随机采样点R1其沿着路径γ距离第
一个状态点的长度为T1；其中，dists表示数组，其大小为n，dists[i‑1]表示前i个状态点的
路径累计长度，dists[0]＝0，dists[i]＝dists[i‑1]+D(statei‑1,statei)。

[0015] 进一步的，所述步骤S2中状态点P0、P1的构造方法为：

[0016] S2.1用二分法在数组dists中查找第一个大于或等于T0的数，并返回这个数的索引next，路径γ上的第next个状态点statenext即为随机采样点R0最近的下一个状态点；用二
分法在数组dists上查找最后一个小于或等于T0的数，并返回这个数的索引previous，路径
γ上的第previous个状态点stateprevious即为随机采样点R0最近的上一个状态点；

[0017] S2.2若随机采样点R0与状态点statenext的距离dists[next]‑T0dists[previous]况均不满足，则在随机采样点R0处新增一个状态点作为简化路径一端的状态点P0；同理可根
据随机采样点R1得到另一状态点P1。

[0018] 进一步的，所述步骤S2中状态点P0、P1的直接距离d1表示为：d1＝D(P0,P1)。

[0019] 进一步的，所述步骤S2中状态点P0、P1的沿所述分段线性路径γ的距离d2表示为：假设沿着路径γ，P0与P1之间存在k个状态点，则d2＝D(P0,state1)+D(state1,state2)+...+
D(statek,P1)。

[0020] 进一步的，所述步骤S4中在两个状态点分段的中点处取一点temp的计算方法为：

[0021] 假设两个状态点分别为statefrom、stateto：

[0022]

[0023]

[0024] 则

[0025] 其中

[0026] 进一步的，所述步骤S4中阈值minChange＝length*0.01，其中length为路径γ’的长度。

[0027] 进一步的，所述步骤S2.2中threshold表示为：随机采样点近似为真实状态点的判断阈值，threshold＝n*0.005。

[0028] 进一步的，函数D表示为两个状态点的直接距离：

[0029]

[0030] 其中，i为状态点state的下标索引。

附图说明

[0031] 图1是本发明优化方法中分段线性路径示意图。

[0032] 图2是本发明优化方法中采样示意图。

[0033] 图3是本发明优化方法中路径简化示意图。

[0034] 图4是本发明优化方法中单次路径平滑示意图。

[0035] 图5是本发明优化方法中平滑前后对比示意图。

具体实施方式

[0036] 接下来将结合本发明的附图对本发明实施例中的技术方案进行清楚、完整地描述，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实
施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的其它实施例，均属于
本发明保护的范围。下面来对本发明做进一步详细的说明。

[0037] 本发明提供一种六自由度机械臂空间轨迹优化方法，该方法包括以下几个步骤：

[0038] 步骤S1、初始化所述机械臂空间轨迹，该空间轨迹为包含n个状态点的待优化的分段线性路径γ＝(state0,state1,…,statei,…,staten‑1)。

[0039] 首先进行参数设定及辅助数据结构初始化：

[0040] γ：表示待优化的分段线性路径，是n个状态点state的有序集合，表示为：γ＝(state0,state1,…statei…,staten‑1)，i为状态点state的下标索引，即i取0至n‑1,如图1
所示；

[0041] statei：路径γ上的状态点，表示六自由度机械臂在某一时刻在构型空间的信息，用一个6维向量表示statei＝(r1,r2,…,rj,…,r6)，rj代表机械臂各轴的角度，j＝1‑6；

[0042] maxSteps：简化路径过程的最大迭代次数，初始化为路径γ所包含的状态点总数n；

[0043] threshold：随机点近似为真实状态点的判断阈值，threshold＝n*0.005；

[0044] range：路径简化过程中单段线性路径的长度范围，range＝n*0.33；

[0045] 数组dists：大小为n，dists[i‑1]表示前i个状态点的路径累计长度，dists[0]＝0，dists[i]＝dists[i‑1]+D(statei‑1,statei)，其中函数D(statei‑1,statei)表示状态点
statei‑1到状态点statei的长度，即直接距离，表示为：

[0046]

[0047] 其中，i为状态点state的下标索引。

[0048] P0，P1：初始化为空，表示简化路径两端的状态。

[0049] 步骤S2、对该分段线性路径进行路径简化，所述路径简化包含：沿所述分段线性路径取两个随机采样点R0、R1，根据所述两个随机采样点R0、R1的位置构造出两个状态点P0、P1，
调用碰撞检测算法，若状态点P0、P1的直接路径与障碍有碰撞，则迭代次数加1，否则判断状
态点P0、P1是否在同一段线性路径上，若是，则迭代次数加1，否则，计算该两个状态点P0、P1
的直接距离d1，以及这两个状态点沿所述分段线性路径γ的距离d2，若d1替代原有路径，消除原有路径上的状态点，迭代次数加1。

[0050] 在范围(0,dists[n‑1])上采样一个随机值T0，以表示在路径γ上随机采样一个点R0，其沿着路径γ距离第一个状态点的长度为T0。

[0051] 状态点P0、P1的构造方法为：利用二分法在数组dists上查找第一个大于或等于T0的数，并返回这个数的索引next，路径γ上的第next个状态点statenext即为随机采样点最
近的下一个状态点；利用二分法在数组dists上查找最后一个小于或等于T0的数，并返回这
个数的索引previous，路径γ上的第previous个状态点stateprevious即为随机采样点最近的
上一个状态点；

[0052] 若随机采样点R0与状态点statenext的距离dists[next]‑T0dists[previous]情况均不满足，则在随机采样点R0处新增一个状态点作为简化路径的一端状态点P0。

[0053] 同样的，在端点P0的range范围内再次采样一个随机值T1，以表示在路径γ上随机采样一个点R1，其沿着路径γ距离第一个状态点的长度为T1，端点的状态点P0的range范围
表示为：(max(0,T0‑range),min(T0+range,dists[n])))。然后按照上述获得状态点P0的相
同方法的过程获得简化路径的另一端点的状态点P1。见图2所示。

[0054] 调用碰撞检测算法，若端点P0、P1的直接路径与障碍有碰撞，则为无效，此次迭代结束，迭代次数加1，反之执行下述步骤。

[0055] 若端点P0、P1在同一段线性路径上，则此次迭代结束，迭代次数加1，因为这种情况没有简化的余地了。否则，计算P0到P1的直接距离d1＝D(P0,P1)，以及沿着路径γ，P0到P1的
距离d2，假设沿着路径γ，P0与P1之间存在k个状态点，则d2＝D(P0,state1)+D(state1,
state2)+...+D(statek,P1)。若d1>＝d2则本次迭代结束，反之用新路径P0‑P1替代原来在此
段内的路径，祛除P0与P1之间的k个状态点，见图3所示。更新状态点数n，更新数组dists、
threshold以及range。迭代次数加1。

[0056] 步骤S3、判断迭代次数，若迭代次数未达到最大迭代次数maxSteps，则重复执行步骤S2，否则，该路径简化结束，获得简化路径γ’＝(state’0,state’1,…,state’i,…,
state’n‑1)。

[0057] 步骤S4、对获得的简化路径γ’进行路径平滑，该路径平滑包含：在路径γ’上的每一分段路径上增加一个新的状态点，对下标索引i是除0外的偶数的状态点依次更新：在状
态点state’i‑1与状态点state’i分段的中点处取一点记为temp1；在状态点state’i与状态点
state’i+1分段的中点处取一点记为temp2；在状态点temp1与状态点temp2分段的中点处取一
点记为temp3，分别连接状态点state’i‑1与状态点temp3、状态点temp3与状态点state’i+1构
成两个分段路径，调用碰撞检测算法验证所述两个分段路径的有效性，若有一条无效则此
次更新结束；若均有效，计算状态点state’i与状态点temp3的直接距离d，若d大于某一设定
阈值minChange，用状态点temp3更新状态点state’i，否则结束此次更新。

[0058] 该过程利用分段贝塞尔曲线的思想来平滑路径，正常情况下，分段贝塞尔曲线是利用若干个状态点，通过多项式公式为其一次性拟合出一条曲线。本专利与其不同的是，在
每一次迭代中，在路径上增加状态点，将路径细分，并更新状态点的状态，以提高平滑度。

[0059] 首先，参数设定：

[0060] γ’：步骤一执行完成后得到的简化路径，也是n个状态点state的有序集合，表示为：γ’＝(state’0,state’1,…,state’i,…,state’n‑1)；

[0061] maxSteps：平滑路径过程的最大迭代次数，初始化为3；

[0062] minChange：新状态点与旧状态点的距离改变量若大于minChange，则予以更新，反之不更新。minChange＝length*0.01，其中length为路径γ’的长度。

[0063] 在路径γ’的每一个分段中间增加一个状态点，这样具有n个状态点的原路径γ’的状态点数增加至2n‑1个。

[0064] 对下标索引i是偶数(除0外)的状态点依次更新，执行以下操作：

[0065] 在状态点state’i‑1与状态点state’i分段的中点处取一点记为temp1；

[0066] 在状态点state’i与状态点state’i+1分段的中点处取一点记为temp2；

[0067] 在状态点temp1与状态点temp2分段的中点处取一点并用其赋予temp3；

[0068] 分别连接状态点state’i‑1与状态点temp3、状态点temp3与状态点state’i+1，构成两分段路径，并调用碰撞检测算法验证两分段路径的有效性，若有一条无效则此次更新结束；
若均有效，计算状态点state’i与状态点temp3的直接距离D(state’i,temp3)，若D(state’i,
temp3)>minChange，用temp3点更新状态state’i，否则结束此次更新。见图4。

[0069] 对于六自由度机械臂而言，在两个状态点statefrom、stateto分段的中点处取一点temp的计算方法为：

[0070] 假设：

[0071]

[0072]

[0073] 则：

[0074]

[0075] 其中

[0076] 步骤S5、当路径γ’上的所有状态点更新完毕，迭代次数加1，若迭代次数小于设定迭代次数maxSteps，重复执行步骤S4，反之路径平滑步骤结束。最终平滑后的路径见图5。

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