机器人操作臂是工业自动化中的重要设备，其末端点的轨迹跟踪控制在实际应用中具有重要意义，近年来操作臂轨迹跟踪控制一直是工程和学术界的研究热点问题^[1-3]。由于机器人操作臂动力学特性的高非线性和强耦合性，其名义模型的构建在控制应用中具有重要作用^[4-5]。从Lagrange方程推导得出的Udwadia-Kalaba方程可对具有相互约束的复杂机械系统进行动力学建模，相对简单地建立系统运动方程并进行求解^[6-12]，该方程可在不出现Lagrange乘子等额外参数的情况下得到系统显式的动力学方程，而且能够对相互作用力等给出合理的度量，由于表达简洁及适用约束的普遍性，其在各个领域逐渐受到越来越多的关注及应用^[13-14]。
Udwadia-Kalaba方程所构建的操作臂动力学名义模型，在不确定机器人操作臂的控制器中往往起到两方面作用：一方面提供名义控制力，另一方面提供系统所要求的约束流形^[15-17]。在操作臂的轨迹跟踪应用中，通过动力学名义模型积分求解获得的约束流形与期望约束应保持一致。同时，当名义模型的操作臂初始位置不满足期望约束方程时，也需要以尽快的速度将不相容的初始条件处理到满足期望约束流形的位置。但Udwadia-Kalaba方程应用时，要求系统的初始条件满足给定约束方程^[6-7]，这在有些情况下难以得到满足；另外Udwadia-Kalaba方程所建立的机器人动力学名义模型求解时产生的积分误差会随积分过程逐渐累积，最终造成约束漂移^{[14, 18]}。
为了解决上述问题，Udwadia等^[19-20]提出利用基于Baumgarte约束违约抑制的渐近稳定方法进行解决；张新荣和孟为来^[21]采用虚位移分解的方法对约束违约误差进行了控制；刘佳和刘荣^[18]在机器人操作臂的速度约束层面引入修正项进行修正，提高了仿真精度。上述方法虽然都可以解决机器人操作臂初始位置与给定约束方程不相容的问题，使操作臂末端点渐近收敛到约束流形，并可对积分累积误差进行控制，但对名义模型中约束轨迹的提供来说，渐近约束的方式并不适用，而且所得约束流形和期望约束流形间积分累积误差的具体控制范围难以确定。
本文针对基于Udwadia-Kalaba方程所构建的控制系统中动力学名义模型要求初始条件满足期望约束方程，以及数值积分求解约束轨迹产生误差累积的问题，提出了采用违约误差直接消除法对其进行处理。该方法可从数值处理的角度将不满足约束方程的初始状态快速约束到期望约束流形。采用迭代方式修正积分累积误差，可使名义模型输出约束以更小的误差与期望约束保持一致。对构建的空间三杆操作臂动力学名义模型及其末端点期望运动轨迹进行数值仿真，并与Baumgarte约束稳定法修正后的约束结果进行比较。
1 Udwadia-Kalaba方程 典型多关节操作臂的动力学方程一般形式可由二阶非线性微分方程描述为

(1)

式中：q=[q₁, q₂, …, q_n]^T为用于描述系统的n维广义位置；M(q)为正定且对称的n×n维质量矩阵；C(q, )为系统的离心力和哥氏力矩阵项；G(q)表示重力项；τ为控制力矩；为广义速度；为广义加速度。
根据系统的实际工作情况，在系统中施加m个相互独立的约束，若给定的约束轨迹足够平滑，相对于时间对其进行二次微分可得

(2)

式中：A(q, , t)为m×n维约束矩阵；b(q, , t)为m维列阵。
在系统中施加约束力矩Q_c(q, , t)，使系统根据约束的要求进行运动。在约束力矩的作用下，多关节操作臂的动力学方程可改写为

(3)

式中：Q=-C-G为系统无约束时的广义力矩项。
根据Udwadia-Kalaba方程，满足工作约束所施加的系统广义约束力矩为^[7]

(4)

式中：(AM^-1/2)⁺为AM^-1/2的Moore-Penrose广义逆。
根据式(3)和式(4)，可得操作臂在约束方程式(2)条件下的系统广义加速度方程：

(5)

2 违约误差直接消除 约束违约误差产生于式(5)数值积分求解的过程中。因此，可在求解的每个积分步，直接在操作臂末端点位置或速度层面消除约束违约误差。直接消除违约误差的位置和速度修正表达式为

(6)

(7)

式中：q^u为未修正时获得的积分位置；δq为消除违约的位置修正项；v为未修正时的积分速度；δv为用以保证满足速度约束方程的速度修正项。
受约束操作臂系统中轨迹约束方程为Φ，速度约束方程为(q, )，则有

(8)

(9)

方程中δΦ和δ可用约束方程变量表示为

(10)

(11)

将式(10)、式(11)分别代入式(8)、式(9)，整理后可得

(12)

(13)

根据Moore-Penrose广义逆阵的概念

(14)

代入式(12)、式(13)可得

(15)

(16)

各积分步后修正的广义坐标即为

(17)

(18)

所得式(17)、式(18)可以在动力学方程的数值积分求解过程，在各个积分步直接完成位置和速度的约束违约抑制，从而方便地应用于修正Udwadia-Kalaba方程构建名义模型产生的数值积分解误差。为了提高轨迹约束流形上的违约消除精度，往往对式(17)进行迭代应用。
3 动力学建模 3.1 无约束操作臂的动力学建模 为说明直接消除违约误差的方法在Udwadia-Kalaba方程所构建机器人操作臂动力学名义模型中的应用，选用图 1所示的三杆空间操作臂作为典型空间操作臂结构进行动力学建模。图中：l_i-1为操作臂中第i根杆的长度；r_i-1为第i根杆重心到关节末端的距离；q_i为第i个关节的广义位置。根据Udwadia-Kalaba方法完成无约束空间操作臂动力学方程的构建。

图 1 三杆空间操作臂 Fig. 1 Three-link spatial manipulator

图选项

由式(3)可知，动力学方程在无外加约束力时可写为

(19)

则操作臂无约束状态下的动力学方程可写为

(20)

由Lagrangian动力学方法可得式(20)中的各项参数。左侧的质量矩阵项中各参数分别为

方程右侧的哥氏力和离心力矩阵中各项分别为

方程右侧的操作臂重力矩阵中各项分别为

式中：m_i为第i个杆的质量；I_xi、I_yi和I_zi分别为第i个杆关于Cartesian坐标系中x、y和z轴的转动惯量; g为重力加速度项。
3.2 受约束操作臂动力学名义模型 在机器人操作臂无约束模型的基础上，给定约束要求。若为执行特定任务，操作臂末端运动轨迹为一空间椭圆，其工作空间中参数方程可表示为

(21)

(22)

(23)

根据图 1及操作臂前向运动学，可得操作臂基座处Cartesian坐标与广义坐标中位置的关系。

(24)

(25)

(26)

整理式(21)~式(26)并求二阶导数后，可得式(2)形式的方程：

(27)

式中：

根据所得式(20)、式(27)中的各项参数，即可完成操作臂的动力学方程的构建。
4 数值仿真 4.1 无违约纠正的仿真 给定操作臂名义模型中各结构参数：m₁=m₂=m₃=1 kg，l₀=l₁=l₂=1 m，r₀=r₁=r₂=0.5 m, I_x1= kg·m²，I_y1= kg·m²，I_z1=0，I_x2=1 kg·m²，I_y2= kg·m²，I_z2= kg·m²，I_x3=1 kg·m²，I_y3= kg·m²，I_z3= kg·m²。在MATLAB2015a中采用定步长四阶龙格库塔积分法仿真，取积分时间步长为0.01 s，仿真时间为20 s。Udwadia-Kalaba所构建动力学模型要求初始条件满足约束方程。故在t=0的初始条件，求得符合约束条件的名义操作臂各关节初始位置为：q₁(0)=-13.5°, q₂(0)=50°, q₃(0)=-100°。
对轨迹约束方程进行一阶求导后，在初始条件，得符合约束条件的各关节初始角速度为：₁(0)=0.75 rad/s, ₂(0)=-0.24 rad/s, ₃(0)=-0.3 rad/s。
将所得式(5)形式的动力学参数方程积分2次后求数值解，即可得在给定约束情况下，三杆操作臂各关节角度随时间变化的曲线，如图 2中实线部分所示。其中实线、虚线和点划线分别表示杆1、杆2和杆3随时间变化的轨迹。可以看出，随着时间的推移，较粗线条表示的各杆件积分值与较细线条表示的期望理论值间距离越来越大，轨迹约束违约误差逐渐增大。图 3为操作臂各关节角速度随时间变化的曲线。相较于位置曲线所得数值积分值的较粗线条与期望理论角速度的较细线条间相差较少，随时间增加速度约束违约误差也逐渐增大。

图 2 操作臂各关节角度随时间变化曲线 Fig. 2 Curves of each manipulator joint's angle changing with time

图选项

图 3 操作臂各关节角速度随时间变化曲线 Fig. 3 Curves of each manipulator joint's angular velocity changing with time

图选项

在空间椭圆轨迹的约束下，操作臂末端在Cartesian空间中运动20 s的轨迹如图 4所示。图 4中虚线表示根据给定期望轨迹约束定义的理论解曲线，实线表示积分求解加速度动力学方程后所得的数值积分解轨迹。可以看出，仿真时间内数值解轨迹严重偏离期望轨迹。因此，需要采取合适的方式进行约束，控制违约误差。

图 4 操作臂末端在Cartesian空间运动轨迹 Fig. 4 Motion trajectories of end point of manipulator in Cartesian space

图选项

4.2 修正违约误差的仿真结果与讨论 利用本文所提违约误差直接消除法和Udwadia-Kalaba方程中常用的Baumgarte约束稳定法^{[15, 20-21]}对所出现违约误差修正后进行比较。任意给定机器人操作臂各杆的初始位置为：q₁(0)=45°，q₂(0)=45°，q₃(0)=-90°；初始速度为：₁(0)=0，₂(0)=0，₃(0)=0。
利用式(17)对4.1节所得位置迭代修正2次，再利用式(18)修正1次速度违约误差，将结果与稳定参数α=50, β=400的Baumgarte约束稳定法的修正结果进行比较。
图 5为仿真时间内采用2种方法修正数值解后，操作臂末端在工作空间的运动轨迹。图中虚线表示给定期望约束的理论轨迹曲线。图 5(a)中实线表示Baumgarte约束稳定法修正后操作臂末端在工作空间的运动轨迹，图 5(b)中实线表示误差直接消除法修正后操作臂末端在工作空间的运动轨迹。与图 4相比较，2种方法修正违约误差后都可将操作臂末端点约束与期望轨迹一致。但直观来看，误差直接消除法比Baumgarte约束稳定法在初始状态约束到期望轨迹的距离更短，积分累积误差的修正效果也更好。

图 5 修正后工作空间操作臂末端轨迹 Fig. 5 Corrected trajectories of end point of spatial manipulator

图选项

图 6显示了操作臂末端数值积分值与理论值之间的误差。图 6中点划线表示各时刻操作臂末端修正前的数值积分值与期望约束理论值之间的距离。由于要求初始条件满足约束方程，因此其误差值从零开始增加，且因误差累积操作臂末端逐渐远离所给定期望轨迹。图 6中虚线显示了Baumgarte约束稳定法修正后，可将操作臂末端与期望约束轨迹间的距离控制在一定范围内，但该范围受到稳定参数选择等因素的制约，难以有效给定具体值。在该修正方法情况下，机械臂末端在开始阶段从初始值渐近稳定到期望轨迹附近。图 6中实线显示了误差直接消除法修正后操作臂末端与所给期望轨迹间误差情况。在该方法的约束下，通过迭代修正次数的增加可将所得约束轨迹与期望约束流形间的误差控制的足够小。由图 6可知，在目前修正情况下，所得误差比Baumgarte约束稳定法修正后所得误差要小，且从初始状态修正到期望约束的过程也更加直接，因此可更加满足控制器中动力学名义模型的需要。

图 6 给定值和求解值之间的空间距离 Fig. 6 Spatial distance of position given values and solution values

图选项

5 结论 本文采用Udwadia-Kalaba方程的建模思想，对三杆操作臂进行了建模仿真，并采用Baumgarte约束稳定法和误差直接消除法对操作臂末端进行了违约修正。研究过程中揭示的主要结论有：
1) 相对于Baumgarte约束稳定法对操作臂末端点初始位置不满足约束方程时的渐近抑制修正，误差直接消除法可更为直接地将不满足约束方程的初始值修正为约束流形上的点，从而更便于机器人控制器中动力学名义模型的应用。
2) 采用误差直接消除法可直接修正动力学方程数值求解过程中随积分产生的误差。修正精度可随修正迭代次数的增加而升高，从而将所得操作臂末端轨迹与期望理论轨迹间的误差控制的足够小。这将更有利于Udwadia-Kalaba方程在控制器设计中的应用。
本文中针对操作臂名义模型进行了违约消除研究。对具有参数及环境不确定性的真实机器人进行控制，设计实验对理论结果验证的研究，将是下一步主要工作方向。

参考文献

[1]	LIANG X, WAN Y, ZHANG C. Task space trajectory tracking control of robot manipulators with uncertain kinematics and dynamics[J]. Mathematical Problems in Engineering, 2017(2017): 4275201.
[2]	SAEED K, MEHDI F M. Uncertainty estimation in robust tracking control of robot manipulators using the Fourier series expansion[J]. Robotica, 2015, 35(2): 310-336.
[3]	XIAO B, YIN S, KAYNAK O. Tracking control of robotic manipulators with uncertain kinematics and dynamics[J]. IEEE Transactions on Industrial Electronics, 2016, 63(10): 6439-6449. DOI:10.1109/TIE.2016.2569068
[4]	YAO J, DENG W. Active disturbance rejection adaptive control of uncertain nonlinear systems:Theory and application[J]. Nonlinear Dynamics, 2017, 89(3): 1611-1624. DOI:10.1007/s11071-017-3538-6
[5]	GALICKI M. Robust task space finite-time chattering-free control of robotic manipulators[J]. Journal of Intelligent and Robotic Systems Theory and Applications, 2017, 85(3-4): 471-489.
[6]	UDWADIA F E, KALABA R E. A new perspective on constrained motion[J]. Proceedings Mathematical and Physical Sciences, 1992, 439(1906): 407-410. DOI:10.1098/rspa.1992.0158
[7]	UDWADIA F E, KALABA R E. Equations of motion for mechanical systems:A unified approach[J]. Journal of Aerospace Engineering, 1996, 9(3): 64-69. DOI:10.1061/(ASCE)0893-1321(1996)9:3(64)
[8]	UDWADIA F E, PHOHOMSIRI P. Explicit equations of motion for constrained mechanical systems with singular mass matrices and applications to multi-body dynamics[J]. Proceedings:Mathematical, Physical and Engineering Sciences, 2006, 462(2071): 2097-2117. DOI:10.1098/rspa.2006.1662
[9]	SCHUTTE A, UDWADIA F. New approach to the modeling of complex multibody dynamical systems[J]. Journal of Applied Mechanics, 2011, 78(2): 856-875.
[10]	PETERS J, MISTRY M, UDWADIA F, et al. A unifying framework for robot control with redundant DOFS[J]. Autonomous Robots, 2008, 24(1): 1-12. DOI:10.1007/s10514-007-9051-x
[11]	UDWADIA F E, MYLAPILLI H. Constrained motion of mecha-nical systems and tracking control of nonlinear systems:Connections and closed-form results[J]. Nonlinear Dynamics and Systems Theory, 2014, 15(1): 73-89.
[12]	UDWADIA F E, KOGANTI P B. Optimal stable control for nonlinear dynamical systems:An analytical dynamics based app-roach[J]. Nonlinear Dynamics, 2015, 82(1-2): 547-562. DOI:10.1007/s11071-015-2175-1
[13]	刘佳, 刘荣. 双臂协调机械手动力学建模的新方法[J]. 北京航空航天大学学报, 2016, 42(9): 1903-1910. LIU J, LIU R. New approach for dynamics modeling of dual-arm cooperating manipulators[J]. Journal of Beijing University of Aeronautics and Astronautics, 2016, 42(9): 1903-1910. (in Chinese)
[14]	徐亚茹, 刘荣. 一种爬壁机器人动力学建模方法[J]. 北京航空航天大学学报, 2018, 44(2): 280-285. XU Y R, LIU R. An approach for dynamics modeling of climbing robot[J]. Journal of Beijing University of Aeronautics and Astronautics, 2018, 44(2): 280-285. (in Chinese)
[15]	UDWADIA F E, WANICHANON T. Control of uncertain nonlinear multibody mechanical systems[J]. Journal of Applied Mechanics, 2014, 81(4): 041020.
[16]	WANICHANON T, CHO H, UDWADIA F E.An approach to the dynamics and control of uncertain multi-body systems[C]//IUTAM Symposium on Dynamical Analysis of Multibody Systems with Design Uncertainties.Amsterdam: Elsevier, 2015: 43-52. https://www.sciencedirect.com/science/article/pii/S2210983815000140
[17]	KOGANTI P B, UDWADIA F E. Dynamics and precision control of uncertain tumbling multibody systems[J]. Journal of Guidance, Control, and Dynamics, 2017, 40(5): 1176-1190. DOI:10.2514/1.G002212
[18]	LIU J, LIU R. Simple method to the dynamic modeling of industrial robot subject to constraint[J]. Advances in Mechanical Engineering, 2016, 8(4): 1687814016646511.
[19]	UDWADIA F E. A new perspective on the tracking control of nonlinear structural and mechanical systems[J]. Proceedings Mathematical Physical and Engineering Sciences, 2003, 459(2035): 1783-1800. DOI:10.1098/rspa.2002.1062
[20]	CHO H, UDWADIA F E. Explicit control force and torque determination for satellite formation-keeping with attitude requirements[J]. Journal of Guidance, Control, and Dynamics, 2013, 36(2): 589-605. DOI:10.2514/1.55873
[21]	张新荣, 孟为来. 基于虚位移分解与伺服轨迹约束的机械系统跟踪控制[J]. 机械工程学报, 2015, 51(3): 45-50. ZHANG X R, MENG W L. Trajectory tracking control of mechanical systems based on virtual displacement decomposition and servo constraint following[J]. Journal of Mechanical Engineering, 2015, 51(3): 45-50. (in Chinese)