基于零空间避障的机械臂末端轨迹跟踪算法

摘要

本发明提供了一种基于零空间避障的机械臂末端轨迹跟踪算法。采用具有解析式形式的最小伪距离代替传统欧式距离作为避障的接近度指标，结合梯度投影法完成机械臂的零空间避障任务。设计一种自适应正定系数矩阵K和速度误差饱和函数将实时末端轨迹运行结果反馈给冗余机械臂运动学反解，根据反馈结果自适应调节关节角速度以减小末端轨迹跟踪误差。本发明不仅解决了冗余机械臂末端轨迹跟踪问题，同时完成了零空间避障任务。采用伪距离可以提前采取避障动作，保证机械臂关节空间运行平稳，末端轨迹跟踪算法可以显著提高轨迹跟踪精度，最大限度完成末端主任务。冗余机械臂的仿真实验结果验证了算法的有效性和优越性。

权利要求

1.一种基于零空间避障的机械臂末端轨迹跟踪算法，其特征是包括以下步骤：(1)建立冗余机械臂正运动学方程：T(q)＝T(q1)T(q2)T(q3)T(q4)T(q5)T(q6)T(q7)，其T 4×4

中，q＝[q1,q2,q3,q4,q5,q6,q7]为关节角度矢量，T(q)∈R 为机械臂末端工具坐标系相对

4×4

基坐标系的位姿变换矩阵；q1、q2、q3、q4、q5、q6、q7为关节角；R 是4×4的实数矩阵；

(2)给出避障模型及最小伪距离计算方法，将机械臂至障碍物的最小伪距离定义为优化目标指标函数，采用运动学反解得到的关节角速度矢量解决机械臂零空间避障问题；

(3)通过末端轨迹跟踪算法将实时轨迹运行结果反馈给冗余机械臂运动学反解，根据反馈结果自适应调节关节角速度大小以减小跟踪误差，使得机械臂末端具有高精度轨迹跟踪效果；

(4)通过避障模型及最小伪距离计算方法和末端轨迹跟踪算法，推导出基于零空间避障的机械臂末端轨迹跟踪算法公式，实现冗余机械臂零空间避障的同时保证末端轨迹高精度跟踪；

其中：所述避障模型及最小伪距离计算方法具体为：(2.1)采用统一的超二次曲面解析式 拟合非结构环境下的障碍物，其中，(x,y,z)为超二次曲面上点的坐标，(x0,y0,z0)为超二次曲面中心点在基坐标系{o0‑x0y0z0}下的坐标，h1>0,h2>0,h3>0和m≥1,n≥1,p≥1分别描述超二次曲面的体积参数和形状参数；

(2.2)采用规则球体拟合环境障碍物，建立机械臂杆件上点到球曲面伪距离表达式其中，(x,y,z)为机械臂杆件上的伪距离计算选取点，Rs∈R为给定障碍物安全球体半径，Rs＝robs+rl，robs为球形障碍物模型半径，rl为拟合圆柱体连杆最大半径；

(2.3)环境中存在N个障碍物时，机器人和各个障碍物之间的伪距离分别为计算最小伪距离的可行性公式即机械臂躲避所有障碍物的最小伪距离定义

其中：所述末端轨迹跟踪算法具体为：(3.1)机械臂末端位置误差为e＝xd‑x，其中，xd为末端期望位置，x为末端实际位置；

(3.2)设计一种自适应正定系数矩阵其中，k1，k2和k3分别为x，y和z方向的位置误差减小调节系数，ex，ey和ez分别为x，y和z方向的位置误差；||·||表示欧几里德范数；

(3.3)机械臂末端位置误差两边同时对时间t进行求一阶导数，则机械臂末端速度误差表达式为

(3.4)给出速度误差饱和函数：其中， 为饱和矢量；

(3.5)结合自适应正定系数矩阵K和速度误差饱和函数 得到机械臂末端轨迹跟+

踪算法公式 其中，J为雅可比矩阵J的Moore‑Penrose伪逆；

其中：所述基于零空间避障的机械臂末端轨迹跟踪算法具体为：(4.1)机械臂零空间避障算法公式为 其中，k为关节速度矢量放+

大系数， 为优化目标指标函数H(q)的梯度投影矩阵；(I‑JJ)为雅可比零空间N(J)的映射矩阵；

(4.2)械臂末端轨迹跟踪算法公式为 采用自适应正定系数矩阵K和速度误差饱和函数 共同约束冗余机械臂运动学反解，轨迹规划中动态调整机械臂末端轨迹误差；

(4.3)结合(4.1)和(4.2)，得到基于零空间避障的机械臂末端轨迹跟踪算法公式实现障碍物避障的同时并完成末端轨迹高精度跟踪运动。

说明书

基于零空间避障的机械臂末端轨迹跟踪算法

技术领域

[0001] 本发明属于机械臂运动规划算法领域，具体涉及一种基于零空间避障的机械臂末端轨迹 跟踪算法。

背景技术

[0002] 冗余机械臂因存在冗余度而具有较好的灵活性和操作性，同时在运动避障、避奇异、关节 力矩优化等方面具有较大的优势，目前已经成为机器人前沿技术领域的研究热点
之一。然而， 机器人在运动过程中与障碍物发生碰撞将导致机械臂不能完成给定的任务，
甚至对自身机械 臂造成不可逆转的伤害，无碰撞地运动规划成为其在工业领域应用的重
要前提。机械臂避障可 以分为机械臂零空间避障和障碍物出现在末端期望轨迹避障(末端
避障)。基于传统欧式距离 的梯度投影算法可以较好地完成机械臂零空间避障，但是末端
轨迹追踪效果较差，无法实现高 精度的末端轨迹跟踪运动。近年来国内外学者采用最小伪
距离代替欧氏距离作为碰撞危险性 衡量指标，以线段‑拟合几何体模式建立碰撞模型取得
了较大的进展。关于机械臂末端轨迹跟 踪方法，法传统轨迹跟踪算法采用常值正定矩阵控
制机械臂避障过程中产生的轨迹误差，但此 方法在机械臂运动规划中不具有自适应反馈
特性，不能实现实时动态误差调整，末端轨迹误差 减小效果一般，无法实现高精度的轨迹
跟踪。针对冗余自由度机械臂需要设计一种更加有效地 轨迹跟踪算法，根据误差大小实时
反馈给机械臂关节角速度矢量，以提高末端轨迹追踪精度。 末端轨迹高精度跟踪是机械臂
运动规划的重要内容，这对完成附加在机械臂上主任务具有重 要意义。

发明内容

[0003] 本发明的技术解决问题是：克服现有技术的不足，提供一种基于零空间避障的机械臂末端 轨迹跟踪算法，解决了末端轨迹跟踪误差较大的问题，同时完成了冗余机械臂零
空间避障任 务，并且各关节角运行平稳无突变现象。仿真结果验证了所提算法的有效性和
优越性。

[0004] 为了实现上述目的，本发明采用了如下技术方案：

[0005] 本发明提供了一种基于零空间避障的机械臂末端轨迹跟踪算法，其包括以下步骤：

[0006] (1)建立冗余机械臂正运动学方程：T(q)＝T(q1)T(q2)T(q3)T(q4)T(q5)T(q6)TT 4×4
(q7)，其中， q＝[q1,q2,q3,q4,q5,q6,q7] 为关节角度矢量，T(q)∈R 为机械臂末端工具坐
标系相对基坐标 系的位姿变换矩阵。

[0007] (2)给出避障模型及最小伪距离计算方法，将机械臂至障碍物的最小伪距离定义为优化 目标指标函数，采用运动学反解得到的关节角速度矢量解决机械臂零空间避障问
题。

[0008] (3)通过末端轨迹跟踪算法将实时轨迹运行结果反馈给冗余机械臂运动学反解，根据反 馈结果自适应调节关节角速度大小以减小跟踪误差，使得机械臂末端具有高精度
轨迹跟踪效 果。

[0009] (4)通过避障模型及最小伪距离计算方法和末端轨迹跟踪算法，推导出基于零空间避障 的机械臂末端轨迹跟踪算法公式，实现冗余机械臂零空间避障的同时保证末端轨
迹高精度跟 踪。

[0010] 所述避障模型及最小伪距离计算方法具体为：

[0011] (2.1)采用统一的超二次曲面解析式 拟合非结构环 境下的障碍物，其中，(x,y,z)为超二次曲面上点的坐标，(x0,y0,z0)为超二次曲面
中心点在基 坐标系{o0‑x0y0z0}下的坐标，h1＞0,h2＞0,h3＞0和m≥1,n≥1,p≥1分别描述
超二次曲面的体积 参数和形状参数。

[0012] (2.2)采用规则球体拟合环境障碍物，建立机械臂杆件上点到球曲面伪距离表达式 其中，(x,y,z)为机械臂杆件上的伪距离
计算 选取点，Rs∈R为给定障碍物安全球体半径，Rs＝robs+rl，robs为球形障碍物模型半径，
rl为拟 合圆柱体连杆最大半径。

[0013] (2.3)环境中存在N个障碍物时，机器人和各个障碍物之间的伪距离分别为 这样我们就推导出了计算最小伪距离的可行性公式，即机械
臂躲 避所有障碍物的最小伪距离定义

[0014] 所述末端轨迹跟踪算法具体为：

[0015] (3.1)机械臂末端位置误差为e＝xd‑x，其中，xd为末端期望位置，x为末端实际位置。

[0016] (3.2)设计一种自适应正定系数矩阵 其中，k1，k2和k3分别为x，y和z方向的位置误差减小调节系数，ex，ey和ez分别为x，y和z方向的位置误 
差。

[0017] (3.3)机械臂末端位置误差两边同时对时间t进行求一阶导数，则机械臂末端速度误差 表达式为

[0018] (3.4)给出速度误差饱和函数 其中， 为饱和矢量。

[0019] (3.5)结合自适应正定系数矩阵K和速度误差饱和函数 得到机械臂末端轨+
迹跟 踪算法公式 其中，J为雅可比矩阵J的Moore‑Penrose伪逆。

[0020] 所述基于零空间避障的机械臂末端轨迹跟踪算法具体为：

[0021] (4.1)机械臂零空间避障算法公式为 其中，k为关节速度矢量放 大系数， 为优化目标指标函数H(q)的梯度投影矩阵。

[0022] (4.2)械臂末端轨迹跟踪算法公式为 采用自适应正定系数矩阵 K和速度误差饱和函数 共同约束冗余机械臂运动学反解，轨迹规划中动态调
整机械臂末 端轨迹误差。

[0023] (4.3)结合(4.1)和(4.2)，得到基于零空间避障的机械臂末端轨迹跟踪算法公式 实现障碍物避障的同时并完成末端轨迹高精度
跟 踪运动。

[0024] 本发明提出的基于零空间避障的机械臂末端轨迹跟踪算法与以往研究相比，具有以下特 点：

[0025] (1)本发明提出的采用最小伪距离作为优化目标指标函数，通过选取合适的避障逃逸速 度，能够解决冗余机械臂的零空间实时在线避障问题，且机械臂各关节角运行平稳
无突变现 象。

[0026] (2)设计一种自适应正定系数矩阵K和速度误差饱和函数 通过对末端位置误差 和速度误差实时反馈，所提出的算法可以显著提高轨迹跟踪精度，最大限度完成末端
主任务。

[0027] (3)仿真结果验证了基于零空间避障的机械臂末端轨迹跟踪算法的有效性和优越性，在 完成多障碍物避障的同时实现末端轨迹高精度跟踪运动，末端轨迹跟踪效果较好，
具有数值计 算简单，适用范围广，方法新颖的优点。

附图说明

[0028] 图1为本发明方法流程图。

[0029] 图2为7‑DOF机械臂运动学模型图。

[0030] 图3为空间点到球曲面伪距离三种关系示意图。

[0031] 图4为简化计算点的伪距离示意图。

[0032] 图5为机械臂最近点与障碍物球面示意图。

[0033] 图6为本发明算法的机械臂连杆运动轨迹示意图。

[0034] 图7为运动过程中最小伪距离变化示意图。

[0035] 图8为机械臂末端轨迹误差示意图。

[0036] 图9为机械臂关节角度变化示意图。

具体实施方式

[0037] 整个发明的实施流程见图1，具体实施方式详述如下：

[0038] (1)建立冗余机械臂正运动学方程

[0039] 各个连杆齐次变换矩阵

[0040]

[0041] 其中，qi为关节角，ai‑1为连杆长度，αi‑1为扭角，di为连杆偏置。图2为一个7‑DOF 机械臂运动学模型图。

[0042] 机械臂末端工具坐标系相对基坐标系的正运动学方程描述为

[0043]

[0044] 其中，q＝[q1,q2,q3,q4,q5,q6,q7]T为关节角度矢量，RTool为末端姿态矩阵，pTool为末端位 置矢量。可由式(1)获取各个连杆齐次变换矩阵T(q1)T(q2),…,T(q7)，这样可以得
到冗余机 械臂正运动学方程T(q)，为后续的仿真实验做准备。

[0045] (2)避障模型建立及最小伪距离计算方法

[0046] 现有避障距离接近度指标是机器人与障碍物最近点之间的最小欧式距离，对于形状较为 复杂的障碍物，用精确的解析式来描述它们的实际形状是非常困难的。避免机械臂
与障碍物发 生碰撞仅需要得到两者之间距离接近程度即可，根据所计算的结果做出避障
动作，无需得到实 际的欧式距离大小。本发明中采用一种新颖的伪距离代替传统欧式距离
用于评价机械臂与障 碍物的接近程度。采用统一的超二次曲面解析式描述障碍物

[0047]

[0048] 其中，(x,y,z)为超二次曲面上点的坐标，(x0,y0,z0)为超二次曲面中心点在基坐标系 {ob‑xbybzb}下的坐标，h1＞0,h2＞0,h3＞0和m≥1,n≥1,p≥1分别描述超二次曲面的
体积参数和 形状参数。

[0049] 对于环境中不规则的障碍物采用规则几何体拟合其外形，简化障碍物形状。由于球体表面 能够包络各种形状的障碍物，是一种较为理想的规则几何体。以三维空间中的球
体为例，建立 点到球曲面伪距离表达式

[0050]

[0051] 其中，Rs∈R为给定障碍物安全球体半径，Rs＝robs+rl，robs为球形障碍物模型半径，rl为 拟合圆柱体连杆最大半径，改变以往将机械臂杆件视为无厚度的直线情况，有助于运
动过程中 碰撞距离的检测，增加避障结果的准确性与可靠性。

[0052] 图3展示了机械臂上任意点与拟合障碍物球曲面的三种位置关系。若杆件上一点(x,y,z) 在球曲面内，则伪距离值dp(x,y,z)＜0，即机械臂杆件已经和障碍物发生碰撞。若
杆件上一点 (x,y,z)在球曲面上，则伪距离值dp(x,y,z)＝0，即机械臂杆件刚好与障碍物
发生接触。若杆件 上一点(x,y,z)在球曲面外，则伪距离值dp(x,y,z)＞0，即机械臂杆件远
离障碍物，此时机械臂 无碰撞危险。

[0053] 如图4所示，我们简化伪距离计算模型，以提高计算效率。选取机械臂杆件端点Ai,Bi和 杆件中间点Mi作为伪距离计算的选取点，最小伪距离计算公式为

[0054]

[0055] 其中，i表示机械臂第i个杆件， 分别表示点Ai,Bi和Mi至障碍 物的伪距离值，取其最小值作为该杆件至障碍物的最小伪距离。

[0056] 对于存在n个连杆的机械臂，可以将整个机械臂到障碍物的最小伪距离定义为

[0057]

[0058] 环境中可能存在多个障碍物，机器人与第j个障碍物的伪距离定义为 存在N个障 碍物时，机器人和各个障碍物之间的伪距离分别为 这
样我们就推 导出了最小伪距离计算的可行性公式，即

[0059]

[0060] 当 机械臂与所有障碍物都没有发生碰撞处于安全状态，即是机械 臂运动过程中无碰撞地前提。

[0061] (3)末端轨迹跟踪算法

[0062] 三次多项式插值函数表达式为

[0063] x(t)＝a0+a1t+a2t2+a3t3   (8)

[0064] 其中，a0,a1,a2和a3为运动规划中的轨迹参数，式(8)两边同时对时间t进行求一阶导数， 可得到轨迹速度函数表达式

[0065]

[0066] 为保证机械臂起始运行平稳，x(t)和 至少需要满足以下四个约束条件

[0067]

[0068] 其中，t0＝0为运动起始时间，tf为运动结束时间，x0，x1分别为运动起始和结束位置。前 两个等式是约束起始点和结束点位置，后两个等式是约束起始点和结束点速度。

[0069] 由式(8)、(9)和(10)可得参数a0,a1,a2和a3表达式

[0070]

[0071] 雅可比矩阵J∈Rm×n(m，n分别为机械臂操作空间自由度和关节角数目)将机械臂关节 角速度矢量 映射为末端速度矢量 即

[0072]

[0073] 由式(12)可得冗余机械臂的运动学逆解公式

[0074]

[0075] 其中，J+为雅可比矩阵J的Moore‑Penrose伪逆，当满足rank(J)＝m时，则J+＝JTT ‑1
(JJ) 。

[0076] 采用雅可比伪逆矩阵求解冗余机械臂运动学逆解带有累计误差，特别当采样时间步长较 大时，机械臂末端轨迹误差较为明显，完成给定末端任务效果较差，故需要对产生
的轨迹误差 进行缩小。机械臂末端位置误差为

[0077] e＝xd‑x   (14)

[0078] 传统末端轨迹减少方法采用正定矩阵K作为调节项实现，通过选取合适的正定矩阵来提 高末端轨迹跟踪精度，减少位置误差。然而该方法实际位置跟踪精度效果并不理
想，且无法根 据不同运动时刻误差大小调节K主对角线元素大小，K一旦确定整个机械臂运
动过程始终保 持不变，无法实现动态调整轨迹误差大小。

[0079] 本发明中设计一种自适应正定系数矩阵K

[0080]

[0081] 其中，k1，k2和k3分别为x，y和z方向的位置误差减小调节系数，ex，ey和ez分别为x， T
y和z方向的位置误差，即e＝[ex,ey,ez] ，||·||表示欧几里德范数。这样正定矩阵K将根据
末 端轨迹误差大小自适应调节比例系数，整个运动过程处于动态变化之中，达到自适应减
少误差 的目的。

[0082] 式(14)两边同时对时间t进行求一阶导数，则机械臂末端速度误差表达式为

[0083]

[0084] 提出一种用于减少末端轨迹误差的速度误差饱和函数

[0085]

[0086] 其中， 为饱和矢量，为提前设置的常值矢量。

[0087] 根据本发明设计的自适应正定系数矩阵K和速度误差饱和函数 可得到机械臂末 端轨迹跟踪算法公式

[0088]

[0089] 式(18)用于机械臂末端轨迹跟踪，能够根据轨迹误差大小实时自适应调节正定矩阵主对 角线元素大小及速度误差饱和函数。正定矩阵K和速度误差饱和函数 共同约
束冗余机 械臂运动学反解，使得机械臂末端具有高精度轨迹跟踪效果。

[0090] (4)基于零空间避障的机械臂末端轨迹跟踪算法

[0091] 机械臂运动规划中生成预定末端轨迹的同时最大化目标函数 确保其总是大于0 以实现零空间避障任务。将最小伪距离定义为优化目标指标函数

[0092]

[0093] 得到优化目标指标函数后，结合式(18)可以得到基于零空间避障的机械臂末端轨迹跟踪算 法公式

[0094]

[0095] 式(20)右端第一项为冗余机械臂轨迹跟踪角速度项，自适应正定系数矩阵K和速度误差 饱和函数 用于减少机械臂末端位置误差，保证高精度的轨迹跟踪。第二项为
+
避障任务角 速度项，其中，(I‑JJ)为雅可比零空间N(J)的映射矩阵。

[0096]

[0097] 为优化目标指标函数H(q)的梯度投影矩阵，用来优化最靠近障碍物连杆的避障逃 逸速度。k为关节速度矢量放大系数

[0098]

[0099] 其中，kmax为k的最大值，dpm为伪距离警戒阈值。kmax的选取较为重要，过大将会对关 节轨迹平稳性造成影响，过小可能无法完成机械臂零空间避障运动，造成避障结果失败。dpm
可以根据障碍物球体半径大小及机械臂实际运行速度极限选取。

[0100] 图5展示了机械臂最近点与障碍物球面示意图。 时，机械臂最近点在警戒边 界以外，k＝0，此时零空间不产生避障运动。 时，机械臂最近点在警戒
区域内， 计算放大系数k值的大小，机械臂本体产生避障动作，在完成末端轨迹跟踪主任务
的同时也 完成机械臂本体避障从属任务。 减小过程中，由于放大平滑变化，k值呈现
类余弦曲线 形状，在避障过程中机械臂各关节运动平稳，避免了抖动振荡现象。

[0101] (5)仿真实验和分析

[0102] 为了验证所提出的基于零空间避障的机械臂末端轨迹跟踪算法的可行性及有效性，采用 Matlab建立7‑DOF冗余机械臂的运动学模型，并进行仿真实验。仿真实验条件设置
如下，机 械臂起始点关节角度(采用弧度制)

[0103] q0＝[‑0.164,0.726,1.265,2.012,‑0.820,‑2.094,‑1.499]T

[0104] 起始点x0和结束点x1位置分别为

[0105] x0＝[0.3m,0.3m,0.6m]T,x1＝[‑0.3m,0.3m,0.6m]T

[0106] robs＝0.03m,rl＝0.03m

[0107] 运动总时长T＝3.0s，时间步长dt＝0.1s。采用多障碍物条件下进行避障，三个半径大小相 同的球形障碍物中心空间位置分别为

[0108] oobs1＝[0.15m,0.15m,0.6m]T,oobs2＝[0.03m,0.15m,0.7m]T,oobs3＝[‑0.09m,T
0.15m,0.7m]

[0109] 伪距离警戒阈值dpm＝3.69，对应欧式距离为0.07m。

[0110] 自适应正定系数矩阵K主对角线元素中系数k1＝700，本文中仅对末端x轴运动方向进行 研究，所以k2||ey||＝k3||ez||＝0.5为常值，即K＝diag[700||ex||,0.5,0.5]。饱和
矢量 

[0111] 本发明提出的基于零空间避障的机械臂末端轨迹跟踪算法仿真结果如图6‑9所示。图6展 示了机械臂运动过程中各杆件的空间构型变化，末端轨迹按预定直线行走，避开
了所有的障碍 物。由图7可知全程最小伪距离大于零，这表明机械臂全程未与障碍物发生
碰撞，一直处于安 全状态。

[0112] 如图8所示，本文提出的末端轨迹跟踪算法产生的最大末端误差为0.01m，在轨迹跟踪 过程中自适应正定系数矩阵K和速度误差饱和函数s a t(e&)根据末端实时跟 踪情
况，不断进行自我调整，将调整结果实时在线反馈给机械臂关节角速度，不断进行关节 角
速度的自我更新以产生较小的位置误差，保证高精度的末端轨迹跟踪。

[0113] 同时从图9机械臂关节角度矢量变化情况可知，由于采用伪距离可以提前采取避障动作， 机械臂关节空间运动运行平稳，光滑无抖振现象，这对关节角度连续平稳变化起
到重要作用， 能够满足一般冗余度机器人系统末端轨迹高精度跟踪的要求。