基于伪逆的导轨机械臂关节速度纠偏运动规划方案

李克讷，张 增，王温鑫

（广西科技大学电气与信息工程学院，广西柳州545006）

（∗通信作者电子邮箱likene@163.com）

0 引言

近年来，机器人技术在现代工业生产中广泛应用，并带来了实际生产效益，使其受到了越来越多的关注和重视［1-3］。其中，机械臂运动规划是机器人技术中的一个重要组成部分［4-7］。在实际工业生产中，基座固定的工业机械臂由于臂长与自由度的原因，工作空间是有限的。然而，工作场所内常常摆放着一些在机械臂工作空间之外的装配部件和需搬运的物品，因此可能会出现机械臂因末端不能到达物体所在位置而无法完成给定的抓取任务。导轨机械臂［2，8］将机械臂基座安装在导轨上并实现一定距离的移动，扩大了机械臂的作业空间，使其更为有效地完成给定任务。因此，导轨机械臂逐渐被应用于工业生产之中［2，8-9］。

通常，机械臂实际应用场所的环境比较复杂，充满了各种不确定的因素。特别是在环境的温湿度变化较大、电磁辐射干扰、传感器误差、机械臂关节电机本身的反应能力差等各种因素的影响下，机械臂的关节速度值可能会偏离期望值［6，10-13］。在实际工程应用中，为了顺利完成任务，导轨机械臂的基座和末端执行器以及各个关节之间的运动需要相互协调。对于导轨机械臂而言，其基座在运动过程中也可能会对关节速度产生影响并使之偏离期望值。一旦关节速度偏离期望值，就会造成关节位移的偏差。该偏差会通过导轨机械臂的连杆结构逐级累加至末端执行器，造成末端执行器的运动轨迹偏离期望轨迹，从而无法将所需零部件运送到指定地点，进而导致任务的失败。而且，若末端执行器的运动轨迹与期望轨迹相差过大还会使导轨机械臂与环境障碍物发生碰撞，致使机械臂损坏，甚至危及操作人员及周围物品的安全。由此可见，在任务执行过程中，关节速度偏离期望值问题的解决是导轨机械臂安全和顺利工作的重要保障，具有重要的实际意义。但目前有关该方面的研究较少。文献［10］提出了一种基于退化雅可比矩阵的速度突变抑制方案，该方案有效抑制了机械臂因关节故障而引起的末端速度突变，使末端速度能收敛于任务执行的期望解。文献［14］利用三次多项式样条插值的方法生成机械臂的期望轨迹曲线，并且将曲线进行离散化处理，保证了机械臂在运行过程中实际关节速度与期望速度的良好跟随性。还有学者将冗余度机械臂的协作问题转化成一个可完全分解的受约束二次规划求解问题，并采用对偶递归神经网络求解该运动控制模型，以此对关节速度进行协调控制［15］。但是，这些方法都是在速度层上针对基座固定的机械臂进行运动规划以保证关节或末端速度能跟随规划的期望曲线，无法直接应用于解决导轨机械臂关节速度偏离期望值的问题。

为此，本文设计了一种导轨机械臂关节速度纠偏运动规划方案，旨在纠正导轨机械臂在任务执行过程中的关节速度偏离现象；同时，还对末端执行器随基座一起运动所造成的实时轨迹和期望轨迹之间的位置误差进行误差修正。本文主要工作如下：1）根据正向运动学方程采用伪逆算法对导轨机械臂在速度层上进行求解；2）对设计的关节速度纠偏方法以及末端执行器位置误差修正方法给出详细的说明及推导；3）采用Matlab软件进行仿真以验证所提方案的可行性和有效性。

1 运动规划方案描述

1.1 冗余度解析

定义导轨机械臂基座固定时的末端执行器笛卡儿位置向量为x(t) ∈Rm（R 为实数空间，m为工作空间维数），根据正向运动学方程［16］可得：

式中：θ(t) ∈Rn为关节角向量（n为关节空间维数）；h(⋅) ∈Rm为由机械臂结构参数推导所得的非线性可微函数。

将式（1）对时间t求导可得：

式中：(t) ∈Rm为基座固定时的末端执行器速度向量；(t) ∈Rn为关节速度向量；J为m×n维的雅可比矩阵。

根据任务执行要求，可获得执行任务所需的期望位置向量rd(t) ∈Rm和期望速度向量(t) ∈Rm。当基座以某一规划速度v(t) ∈Rm沿着指定轨迹s(t) ∈Rm移动时，定义基座移动时的末端执行器笛卡儿位置向量为r(t) ∈Rm。考虑到基座运动对机械臂末端的影响，为了顺利执行任务，需要满足关系r(t)=rd(t)=x(t) +s(t)和(t)=(t)=(t) +v(t)。因此，对于导轨机械臂，可根据期望位置向量和基座运动状态在执行任务前将(t)的期望值(t)设定为：

结合式（2）和式（3）可得：

式中：(t) ∈Rn为期望关节速度向量。

当导轨机械臂的关节空间维数大于任务空间的维数时（n>m），式（4）可能存在多个或无穷个相应的期望关节速度解。可采用伪逆算法得到期望关节速度的解析解［17］，即：

式中：JΔ为雅可比矩阵J的广义逆矩阵，JΔ=JT(JJT)-1∈Rn×m；I∈Rn×n为单位矩阵；z∈Rn为一个可以任意设置的向量，通常用来满足第二任务的实现，如避障、关节奇异位形躲避等［18］。

值得指出的是，本文的目的在于纠正任务执行过程中偏离期望值的关节速度，为了方便探讨和研究，可将向量z设置为零向量。因此，式（5）可简化为：

1.2 关节速度纠偏

为了纠正导轨机械臂在任务执行过程中发生偏离的关节速度，本文对机械臂关节速度进行约束控制，使发生偏离的关节速度能收敛至期望值。

设计如下时变函数j(t)：

式中：μ（μ> 0）为控制电机变速过程快慢的标量系数，可根据电机性能和实际需要进行设定；tp为关节速度发生偏离的时刻。

显然，当t≥tp时，时变函数值会由1 逐渐减少，最后衰减于0。本文仿真部分取μ=3.4。

设偏离期望值后的关节速度为(t)，定义其与期望关节速度之间的差异量为：

当关节速度偏离期望值时，结合式（7）和式（8）可设计关节速度(t)为：

显然，当t≥tp时(t)可随时间逐渐收敛于(t)。因此，可用时变函数j(t)控制导轨机械臂使偏离的关节速度回到期望的关节速度上。

综上所述，结合式（6）和式（9）可将导轨机械臂的关节速度(t)规划为：

式中：T为任务执行的时间周期。

1.3 误差修正

需要注意的是，根据正向运动学方程，x(t)和(t)都与相应的关节速度(t)有着密切的关系。如果关节速度偏离冗余度解析求解得到的期望关节速度，即使采用纠偏方法使其回到期望值，仍然会导致x(t)和(t)出现误差。对于导轨机械臂，基座运动时的末端执行器位置向量r(t)、速度向量(t)分别满足关系：r(t)=x(t) +s(t)，(t)=(t) +v(t)。因此，如果没能及时对该误差进行消除或减少，它将导致r(t)、(t)分别偏离任务期望位置向量rd(t)、任务期望速度向量(t)，从而影响任务执行的精度，甚至造成任务失败。为了解决这一问题，可采用以下设计的位置误差修正方法。

定义x(t)与期望值xd(t)之间的实时误差σ(t)为：

为了减少该误差，设计σ(t)对时间的导数(t)如下：

式中κ为可任意设置的非负标量系数。

进一步地，可将κ设计为时变系数：

式中κ0为可任意设置的正标量系数。本文仿真取κ0=1。

将式（13）代入式（12），可得：

以下证明若在t=t1时产生了误差σ(t1)=x(t1)-xd(t1)，x(t)与(t)在式（14）的条件下，会随时间按指数规律分别收敛到xd(t)、d(t)。

进而可得：

式中C为待定系数。

将式（18）代入式（17）可得：

将式（20）对时间t求导可得：

由 式（20） 和式（21） 可知当t→+∞ 时，|x(t)-xd(t)|→0，|(t)-(t)|→0。可见x(t)、(t)能随时间按指数规律分别收敛到xd(t)、(t)，从而保证了当基座移动时，若关节速度发生了偏离，末端执行器仍能顺利完成给定任务。证毕。

综上所述，导轨机械臂的关节速度纠偏运动规划方案控制框图如图1所示。

图1 导轨机械臂关节速度纠偏运动规划方案控制框图Fig.1 Control block diagram of motion planning scheme for deviation correction of rail manipulator joint velocity

2 实验仿真与结果分析

由于本文的目的仅在于纠正导轨机械臂偏离期望值的关节速度以及由此引起的末端执行器偏离期望轨迹的现象，且不包括其他优化指标。因此，在本章中，以基座直线移动和圆形移动的四连杆冗余度机械臂为例，对导轨机械臂关节速度纠偏运动规划方案进行了仿真实验，以讨论和分析该方案的有效性。机械臂相关参数如下：关节空间维数n=4，工作空间维数m=2，四个连杆长度分别为[1，0.8，0.7，0.5]T（单位为m）。

2.1 弧形轨迹跟踪

设基座直线移动机械臂的末端执行器从指定起始点去跟踪一段弧形轨迹。机械臂的实际关节角向量qa和期望关节角向量qd的初始值（单位为rad）。弧形轨迹的参数方程为：

图2 基于伪逆算法的弧形轨迹跟踪仿真Fig.2 Simulation of circular trajectory tracking based on pseudoinverse algorithm

图2（a）给出的是机械臂的运动过程。图2（b）表示当基座运动时末端执行器跟踪给定弧形轨迹的执行状况。从图2（b）可见，机械臂的实际运动轨迹与期望的轨迹基本重合。图2（c）给出的是末端执行器分别在X轴和Y轴方向随时间变化的位置误差曲线。由误差曲线可知，X轴方向和Y轴方向误差值均小于1×10-5m，可见伪逆算法能保证机械臂有较好的轨迹跟踪精度；而且，根据图2（d）给出的关节速度的变化曲线，可知关节速度在机械臂运行过程中保持连续光滑的状态，有利于关节电机的运行及任务的执行。

为了模拟关节速度在执行任务过程中偏离期望值的情形，本节仿真在t=2.524 1 s 处给关节速度加扰动使其偏离期望值，具体仿真结果如图3所示。

图3 关节速度受扰的弧形轨迹跟踪仿真Fig.3 Simulation of circular trajectory tracking suffered from joint velocity disturbance

图3（a）给出了机械臂的运动过程。图3（b）给出了基座运动时末端执行器跟踪给定弧形轨迹的执行状况。从图3（b）可见，末端执行器实际运动轨迹明显偏离了期望轨迹。而且，从图3（c）的误差曲线可以看到，在时段t=2.524 1～4.658 5 s 内，X轴方向的误差值由增大到，Y轴方向的误差值由增大到。可见，在速度发生偏离后的很短时间内机械臂末端执行器产生了较大的位置误差，且误差持续存在于整个任务执行过程中。该误差的产生主要是因为在图3（d）中的关节速度在t=2.524 1 s 时刻偏离了图2（d）所示的期望关节速度曲线，这不利于机械臂的安全运行及任务的执行。

为了纠正任务执行过程中偏离的关节速度，可采用时变函数j(t)对关节速度进行约束调整。而且，为了及时消除末端执行器在任务执行过程中产生的位置误差，可考虑将关节速度纠偏方法（式（10））与误差修正方法（式（14））相结合得到优化方案（式（22）），仿真结果如图4所示。

图4（a）显示了机械臂的运动过程。在图4（b）中，末端实际运动轨迹与期望轨迹基本重合，表明优化后的机械臂能顺利跟踪期望轨迹。从图4（c）可以看到，X轴方向的位置最大误差值出现在t=3.125 1 s 时刻,；Y轴方向的位置最大误差值出现在t=3.080 6 s 时刻，。两方向的位置误差到最大值后都迅速减少，在t=5.090 4 s 时刻误差分别减小到|ex|=5.140 6×10-5m，|ey|=1.452 4×10-5m。从图4（d）可见，当关节速度在t=2.524 1 s 时刻发生偏离后能自动收敛于图2（d）中所示的期望关节速度曲线，且关节速度在任务执行过程中随时间连续且平滑地变化。因此，仿真结果验证了本文所提方案的有效性。

图4 采用优化方案的弧形轨迹跟踪仿真Fig.4 Simulation of circular trajectory tracking by using optimized scheme

2.2 直线轨迹跟踪

为了讨论当机械臂的基座运动轨迹以及期望跟踪轨迹发生改变时本文所提的纠偏方案仍然可行，本节讨论末端执行器跟踪直线轨迹的仿真情况。机械臂的实际关节角向量qa和期望关节角向量qd的初始值。该直线轨迹的起点为(1.742，2.225)，终点为(-0.804 2，2.544)。执行器的起始点坐标和直线起点坐标一致，任务执行周期T=10 s。基座运动的圆形轨迹参数方程为：

式中：参数λ=0.3，角频率。为方便进行比较，首先在正常情况（关节速度无偏离）下用伪逆算法对机械臂跟踪直线轨迹进行求解仿真，仿真结果如图5所示。

图5（a）给出了机械臂的运动过程。从图5（b）可见，机械臂末端的实际运动轨迹与期望的轨迹基本重合。由图5（c）可知，末端执行器在X轴方向和Y轴方向的误差值均保持在1×10-5m 之内。可见，伪逆算法能保证机械臂有较好的轨迹跟踪精度。根据图5（d）显示的关节速度变化曲线，可知关节速度在任务执行过程中保持连续光滑的状态，有利于关节电机的运行。

为了模拟关节速度在执行任务过程中偏离期望值的情形，在t=4.783 6 s 处给关节速度加扰动使其偏离期望值，具体仿真结果如图6所示。

图6（a）显示了机械臂的运动过程。从图6（b）可见，末端执行器实际运动轨迹明显偏离了期望轨迹。从图6（c）可以看到，在t=4.783 6～6.639 8 s 时段内，X轴方向的误差值由|ex|=2.274 3×10-6m增大到|ex|=0.457 0 m；Y轴方向的误差值由|ey|=1.201 5×10-6m 增大到|ey|=0.285 1 m。可见，在速度偏离后的很短时间内机械臂末端执行器产生了较大的位置误差，且误差持续存在于整个任务的执行过程中。该误差的产生主要是因为图6（d）中的关节速度曲线在t=4.783 6 s 时刻偏离了图5（d）所示的期望关节速度曲线，这不利于机械臂的安全运行及任务的执行。

图5 基于伪逆算法的直线轨迹跟踪仿真Fig.5 Simulation of linear path trajectory tracking based on pseudoinverse algorithm

图6 关节速度受扰的直线轨迹跟踪仿真Fig.6 Simulation of linear path trajectory tracking suffered from joint velocity disturbance

为了纠正机械臂在任务执行过程中产生偏离的关节速度，可采用时变函数j(t)对关节速度进行约束控制；并且，为了及时消除末端执行器出现的位置误差，将关节速度纠偏方法与误差修正方法相结合得到优化方案，仿真结果如图7所示。

图7（a）显示了机械臂的运动过程。在图7（b）中，末端执行器的实际运动轨迹与期望轨迹基本重合，表明优化后的机械臂能顺利跟踪期望轨迹。从图7（c）可以看到，X轴方向的位置最大误差值出现在t=5.260 4 s 时刻，|ex|max=0.005 7 m；Y轴方向的位置最大误差值出现在t=5.343 9 s 时刻，|ey|max=0.001 6 m。两方向的位置误差达到最大值后均迅速降低，在t=7.486 6 s 时刻分别下降到|ex|=1.662 7×10-5m，|ey|=1.035 6×10-5m。从图7（d）可以看到，当关节速度在t=4.783 6 s 时刻发生偏离后能自动收敛于图5（d）中显示的期望关节速度曲线，且在任务执行过程中随时间连续且平滑地变化。由仿真结果可知，当机械臂的基座运动轨迹以及期望跟踪轨迹发生改变时，本文所提的纠偏方案仍然可行。

图7 采用优化方案的直线轨迹跟踪仿真Fig.7 Simulation of linear path trajectory tracking by using optimized scheme

3 结语

本文提出了基于伪逆的导轨机械臂关节速度纠偏运动规划方案，从对关节速度约束控制的角度入手，解决了导轨机械臂执行轨迹跟踪任务时关节速度偏离期望值的问题。同时，考虑了末端执行器在任务执行过程中产生位置误差的不利影响，通过误差修正方法，保证机械臂顺利完成轨迹跟踪任务，大大提升了方案的可行性。仿真结果表明了所提纠偏方案是可行且有效的。