机械臂是高端制造、医疗康复与国防安全等国家关键领域的重要装备[1]，其轨迹跟踪的精度直接决定作用效能。随着应用场景从结构化环境向非结构化环境拓展，机械臂系统面临的非光滑和非线性动力学[2]问题(如摩擦诱导的驱动迟滞以及铰内间隙引起的摩擦和碰撞等)极易引发系统的振荡和抖动[3]，破坏机械臂的操作稳定性。传统的控制方法在处理此类强非线性问题[4]时多依赖运动学补偿算法，但在处理非光滑因素(如库仑摩擦)时，控制性能往往无法得到明显的提升。
Udwadia-Kalaba (U-K)方法为受约束多体系统的动力学建模提供了一种新颖的解析框架[5]。与传统的动力学建模方法不同，U-K方法并没有直接采用 Lagr-ange乘子或其他辅助变量对原动力学方程进行等效，而是采用矩阵伪逆的代数计算技巧，直接显式地得到系统的广义约束力[6-11]。尽管这一方法在各种复杂系统的动力学建模过程中得到广泛的应用[12-14]，但在处理含库仑摩擦等非光滑因素的多体系统建模问题时，需要做特殊的处理，避免理想约束力和非理想约束力之间相互独立的假设与客观物理实际不符。姚文莉等对 U-K方法进行拓展[15]，并验证了拓展后的 U-K方法在处理含摩擦系统时的准确性[16]。
在控制领域应用方面，Udwadia[17]首次将控制目标视为受控对象的伺服约束条件，通过主动施加伺服约束力，使受控对象实现控制需求。该方法不依赖系统的线性假设，对不确定性和非线性具有较强的鲁棒性[18-19]。刘才山[20]认为，U-K方程对受约束运动提供一个全新的解释，即该方程类似一个自然选择的反馈控制器的作用，完整和非完整约束起到限制约束运动加速度的作用。陈力[21]利用 U-K方法实现机械臂末端轨迹的高精度控制。Huang等[22]等基于 U-K方法开发自适应鲁棒控制方法，并推广至欠驱动系统。Wei等[23]等结合 Lyapunov稳定性理论，证明基于 U-K的控制方法在收敛速度与精度方面存在优势。但是，上述研究尚未考虑非光滑因素的影响，在实际应用时仍然面临摩擦和间隙碰撞等引发的控制性能退化问题。
为进一步探索 U-K方法在含摩擦机械臂系统轨迹跟踪控制中的应用潜力，本文基于对 U-K方法的拓展，并结合 Baumgart违约稳定化方法[24] (BSM方法)修正因外部扰动带来的约束违约问题，提出一种基于改进 U-K方法的伺服控制策略。首先，针对含库仑摩擦的系统，对拓展后的 U-K方法进行拆分，建立主动约束力和被动约束力的显式表达式，并据此构建伺服控制方法。然后，以受双边约束的平面二连杆机械臂系统为研究对象，引入光滑牛顿算法[25]来处理动力学方程不连续的问题，实现数值计算的稳定和高效，并结合 BSM对约束条件进行修正，保证控制的稳定性。最后，基于MATLAB平台，设计多类复杂轨迹并进行仿真分析，验证伺服控制方法在高精度、渐近稳定以及摩擦抑制方面的有效性和鲁棒性。
1 U-K 动力学建模方法及其拓展
对于一个受约束的机械系统，其动力学方程可表示为
其 中，q= [q1 ，q2 ，…，qn]T为系统的 n个 广义坐 标，
表示系统的广义速度，
表示系统的广义加速度。M(q，t)为质量矩阵，
为广义外力向量，
和
分别为系统的广义理想约束力和广义非理想约束力项。上角标“+”表示矩阵的Moore-Penrose广义逆，A是加速度约束方程的雅可比矩阵，b是加速度约束方程的余项。C是引进的已知 n维向量，表示非理想约束力在广义坐标上的投影。
从式(1)可知，在经典 U-K方法中，
和
并无直接耦合关系。但是，当系统存在非光滑因素(如库仑摩擦)时，理想约束力与非理想约束力的耦合效应使得经典 U-K方法得到的结果与传统动力学方法不相符。为了解决非光滑因素带来的耦合效应问题，需要对经典 U-K方法进行拓展。
基于上述考量，姚文莉等[15]提出一种拓展的UK方法，其理想约束力和非理想约束力的显式表达式如下:
其中，
可表示为
的明确函数，代表非理想约束力与理想约束力之间的耦合关系。在理想约束力向量
的表达式中，新增项
的虚功为零，在保持理想约束力性质的同时，揭示了理想约束力与非理想约束力间的耦合关系，并由郭明明[16]验证了拓展的 U-K方法在处理含摩擦系统动力学问题的精准性和正确性。
2 基于改进 U-K 方法的伺服控制策略
针对存在初始扰动的轨迹跟踪控制问题，本文引入 BSM方法[24]和伺服约束[26]，并基于改进的 UK方法，提出一种伺服控制策略。将目标点的轨迹转化为系统的约束条件，通过 BSM方法对约束条件的不相容性进行修正，利用改进的 U-K方法，显式地得到系统的控制力，最后通过控制力实现对目标轨迹的精准跟踪和控制。
考虑一个多刚体系统，假设目标点的期望轨迹为 ℓ( t) ，系统的伺服约束和被动约束可以表示为
其中，Φ 为被动约束，Ψ 为伺服约束，h = h( q，t)是目标点的广义坐标形式。整理式(3)，可得
求解式(4)，可得受约束下系统的加速度为
其中，
。
利用改进 U-K方法，受约束下系统的动力学方程可表示为
其中，
，λ为系统的被动约束力，τ 为主动的伺服控制力。基于矩阵空间的互补性，求解式(6)，可得约束力的显式表达式:
其中，
为伺服约束在其零空间上的投影矩阵。
根据上述推导，受约束系统的动力学方程可改写为
其中，f ( λ) 表示与被动约束力有函数关系的其他约束力(比如摩擦力)。求解式(8)，则加速度可表示为
通过对加速度积分并代回系统的初始状态，便可实现对系统的伺服控制。
可以发现，基于改进 U-K方法建立的伺服约束流程不需要求解加速度。在给定系统运动轨迹或运动状态的前提下，根据当前时刻的位移和速度，便可直接得到显式的控制力和力矩，从而实现对目标点的精准控制。
基于改进 U-K方法的系统伺服控制流程如图1所示。
图1 基于改进 U-K方法的伺服控制流程
Fig.1 Servo control flow chart based on modefied U-K method
3 数值算例
3.1 平面二连杆机械臂模型
本文建立受双边约束的平面二连杆机械臂系统的动力学模型，如图2所示。该模型由一个滑块与两个刚性摆杆组成，通过滑块的平移和摆杆的旋转，使得末端受控点 P在平面内实现轨迹跟踪效果。系统的广义坐标为
，我们只考虑滑移铰内的库仑摩擦，旋转铰内的摩擦和碰撞忽略不计。
图2 平面二连杆机械臂力学模型
Fig.2 Mechanical model of the orbital robotic arm
将滑块与轨道之间简化为点接触，采用库仑干摩擦模型[27]，表达式为
其中，0μ 和μ 分别为系统的静摩擦系数和动摩擦系数，
是物体间的相对运动速度，
为物体间的相对运动加速度。
基于改进 U-K方法的伺服控制策略，可得到被动约束力 λ 和控制力矩τ 的显式表达式:
由于接触面上存在库仑干摩擦，所以式(12)中 λ 的显式表达式会出现以下两种情况。
1) 第一种情况: 黏滞阶段
。当系统处于黏滞阶段时，摩擦可视为接触处加速度等于零的理想约束条件，即
=0。此时，系统的非理想约束力
=0，λ =[Fτ Fn]T 。因此，主动约束力τ和被动约束力 λ 可直接由式(12)求解得到。
2) 第二种情况: 黏滑切换(
)或滑动阶段
。当系统处于黏滑切换或滑动阶段时，切向摩擦力 Fτ 是法向支持力 Fn的函数(如式(10)所示)。此时，λ= Fn，系统的非理想约束力
=
。因此，可推导出法向支持力 Fn的显式表达式为
其中，Wτ 为非理想约束力的雅可比矩阵。
当系统处于第二种情况时，由于式(11)中法向支持力的方向需依赖 sgn (Fn)进行判断，由此产生的方向不确定性给系统动力学方程的求解带来一定的困难。在求解过程中，实时判断法向接触力的方向，流程复杂且数值计算稳定性较差。本文将法向接触力方向判断问题转换为一组绝对值方程，并采用光滑牛顿算法进行数值迭代，使得改进 U-K伺服控制策略能够应用在此类非光滑双边约束系统的轨迹跟踪任务中。
基于上述分析，法向支持力 Fn的解析式可转化为如下绝对值方程:
为求解绝对值方程(式(14))，引入光滑近似函数
(δ 为光滑参数)来替代原绝对值函数，将非光滑问题转化为光滑问题求解。光滑牛顿算法的伪代码如算法 1所示。
算法 1光滑牛顿算法伪代码
输入: 质量矩阵 M，广义外力 Q，约束雅可比矩阵 Wτ ，约束矩阵 A，约束向量 b，主动约束矩阵ψq ，被动约束矩阵 qΦ ，摩擦系数项 d
输出: 绝对值方程的解 x
1: 给出光滑牛顿算法中参数的值ε，δmin ，n和σ
2: 令 x = x0; δ =δ0
// 定义初始迭代参数的数值
3: for i=1:n
4: φ(i) = f ( x( i - 1)，δ ( i -1))
// 构造参与迭代的平滑函数
5: φ′( i) =dφ (i) /dt
6: F =p ( A， x， b，h ( d ，ψ q ，Φ q，A， Wτ ，M))
// 绝对值方程的平滑化处理
7: J (i) = A + h *diag(φ ′(i))
// 计算平滑化后绝对值方程的雅可比矩阵
8: if N ( F ) ＜ ε ‖δ(i - 1)＜δmin
9: 输出前一时刻下的值 x(i-1)
10: else
11: Δ x =J ( i) \( -F)
12: x( i ) = x( i - 1)+Δx
13: δ( i) = σ ·δ( i-1)
// 如果未满足条件，则计算下一次迭代的初始值
14: end if
// 结束 if命令
15: end for
// 结束 for命令
16: 输出迭代解 x
该流程将方向判断的逻辑嵌入连续可微的函数中，通过牛顿法的二次收敛性来快速逼近解，从而避免传统方法条件判断带来的问题，可以保证数值计算的稳定性与效率。
3.2 轨迹控制仿真
结合基于改进 U-K方法的伺服控制策略与光滑牛顿算法，双边约束下平面二连杆机械臂系统的轨迹控制方法，探究不同轨迹条件下非光滑因素(库仑摩擦)对系统控制精度的影响。通过建立系统几何位置与广义坐标之间的关系，推导出非理想约束力的雅可比矩阵为 Wτ = [-1 0 0 0]。本文设计了 3组轨迹: 螺旋线轨迹、“∞”轨迹和卷积线轨迹，利用 MATLAB对机械臂系统进行轨迹控制仿真，以便获取轨迹控制数据和广义约束力的变化趋势。
3.2.1 螺旋线轨迹跟踪
在工作空间内，给定二连杆机械臂末端点轨迹约束条件:
其中，a0 = 1，b0 = 10，c0 = 0为螺旋线的参数。
任选式(15)中的一点作为起点，可通过解析给出广义坐标和广义速度的初值。本文将系统初值设在精确值附近，即设定初值条件与约束条件不相容的情况作为初始扰动，进一步采用 BSM方法[24]对伺服约束进行违约修正。
采用 MATLAB ode45积分器求解，绝对误差和相对误差设为 1×10-6，轨迹跟踪情况以及误差分析结果如图3所示。
图3 螺旋线轨迹仿真
Fig.3 Helix trajectory simulation
从宏观动力学角度分析，基于改进 U-K方法的伺服控制策略对螺旋线性轨迹具有较高的控制精度和稳定性。引入 BSM方法可以精准地修正系统初始扰动带来的控制偏差，修正后，系统在 1.9 s左右的控制误差接近 0; 末端的误差相轨迹连续而光滑地收敛于原点，验证了改进 U-K伺服控制方法的正确性及其实现渐近稳定的能力，系统动力学在宏观上服从指定的约束。
观察摩擦力和关节力矩图(图3)，分析其微观力学机理。由于双边约束的特性，法向支持力在系统惯性作用下高频震荡，支持力的连续切换使得切向摩擦力围绕一个相对稳定的数值波动。同时，改进 U-K伺服控制方法敏锐地捕捉到系统的持续震荡，因此输出相同规律的控制力矩，对摩擦力进行补偿，说明该方法能主动将轨迹跟踪问题转化为力调节问题，并通过补偿非线性摩擦来实现全局渐近的控制稳定性。
3.2.2 “∞”轨迹跟踪
给定机械臂系统末端点轨迹的约束条件:
其中，Ax = 1.5为轨迹 x方向上的幅值，Ay = 1为轨迹 y方向上的幅值，ω = π/2为周期。
与螺旋线轨迹的处理方法相同，“∞”轨迹跟踪的MATLAB仿真结果如图4所示。
图4 “∞”轨迹仿真
Fig.4 “∞”trajectory simulation
同样，对“∞”轨迹的控制系统进行 BSM修正。轨迹跟踪误差(图4(b))显示，在 1.4 s左右，轨迹控制误差趋于 0，同时 X和 Y方向的误差相轨迹图均连续且快速地收敛于原点，表明控制律成功地将期望轨迹作为二次约束施加于系统，并产生理论上所需的广义控制力，驱使系统状态严格而稳定地收敛至期望的约束流形。
从力矩图(图4(e))可知，初始时力矩较大，表明系统在启动时需要较大的力矩来克服惯性和摩擦力，才能准确地达到期望轨迹; 随后力矩下降，并在一定范围内波动，表明系统已成功地实现跟踪效果，进入稳定状态。控制力矩的变化反映了改进 UK伺服控制策略的有效性。同时，观察摩擦力和支持力的变化(图4(f))可知，在控制过程中出现 stickslip现象，而控制误差趋于 0，说明系统在发生动态变化(即静摩擦与动摩擦的切换)时依然保持较高的控制精度，法向支持力在一定范围内小幅度地变化，符合改进 U-K伺服控制方法对非线性力的补偿规律。力和力矩的变化从侧面说明，改进 U-K伺服控制策略设计良好，在跟踪非光滑系统时，可以确保控制效果的渐近稳定性。
3.2.3 卷积线轨迹跟踪
在工作空间内，给定机械臂系统末端点轨迹的约束条件如下:
卷积线轨迹的 MATLAB仿真结果如图5所示。可以看出，由于卷积线的增长特性，系统的跟踪效果相较于其他轨迹出现不同的现象。从轨迹误差和相轨迹图可以观察到，由于系统无初始扰动，末端的轨迹跟踪误差在 0附近保持 10-8量级的震荡，与误差相轨迹相呼应; X和 Y方向的误差相轨迹均先偏离目标点，再绕回到目标点附近小幅度地震荡(量级始终保持在 10-4)，说明控制律的精度非常高，但高精度的控制易受非线性因素(如摩擦)影响，在小幅度内持续震荡，最终系统趋于渐近稳定状态。
图5 卷积线轨迹仿真
Fig.5 Convolution line trajectory simulation
从力和力矩图可知，静止开始运动的机械臂跟踪大范围曲线轨迹时，随着摆动幅度增大，控制力矩和法向支持力相应地增大，切向摩擦力也随之增大。力矩的剧烈波动说明在控制过程中，改进 U-K伺服控制方法为实现对轨迹的精准控制，通过输出波动的力矩动态调整对非线性力进行补偿。摩擦力也呈现相应的变化规律，本质是控制力矩在环境约束下的表现形式。关节力矩的反作用力通过机构传递到基座，通过与外界交互的摩擦力来实现平衡。由此也验证了动力学模型及控制策略的正确性。
4 结论
本文将 Udwadia-Kalaba方法系统地推广至含非光滑因素机械臂系统的轨迹跟踪控制中，并建立一套适用于含摩擦系统的伺服控制理论框架。通过构建考虑摩擦的双边约束动力学模型，结合伺服控制方法，实现广义约束力的显式表达，并引入 Baumgart违约稳定化方法，有效地处理约束违约问题，显著地提高系统在控制过程中的稳定性。针对动力学方程不连续的情况，本文提出基于光滑牛顿算法的绝对值方程数值解法，可以有效地避免传统迭代中因条件判断引起的数值不稳定，能够保证求解的高效性与可靠性。
仿真结果表明，本文提出的伺服控制策略在多种复杂轨迹下均能实现高精度、渐近稳定的跟踪与控制，末端轨迹误差可以快速收敛，并保持在较小范围内。同时，本文方法能够有效地抑制因摩擦引起的系统抖振和黏滑现象，表现出较强的鲁棒性和适应性。研究结果不仅验证了基于改进 U-K方法的伺服控制策略在处理含摩擦控制问题时的有效性和鲁棒性，也为非光滑复杂多体系统的控制问题提供了新的解决思路。
[1]计时鸣，黄希欢.工业机器人技术的发展与应用综述.机电工程，2015，32(1): 1-13
[2]Xu M，Hu W，Han Z，et al.Symmetry-breaking dynamics of a flexible hub-beam system rotating around an eccentric axis.Mechanical Systems and Signal Processing，2025，222: 111757
[3]Hu W，Xi X，Song Z，et al.Coupling dynamic behaviors of axially moving cracked cantilevered beam subjected to transverse harmonic load.Mechanical Systems and Signal Processing，2023，204: 110757
[4]朱仲文，蒋智涛，王维志，等.Multi-Shooting 非线性 MPC 无人驾驶汽车轨迹跟踪控制.控制理论与应用，2025，42(8): 1477-1485
[5]Udwadia F E，Kalaba R E.A new perspective on constrained motion.Proceedings of the Royal Society of London.Series A: Mathematical and Physical Sciences，1992，439: 407-410
[6]Udwadia F E，Kalaba R E.Nonideal constraints and lagrangian dynamics.Journal of Aerospace Engineering，2000，13(1): 17-22
[7]Udwadia F E，Kalaba R E.Explicit equations of motion for mechanical systems with nonideal constraints.Journal of Applied Mechanics，2001，68(3): 462-467
[8]Udwadia F E，Kalaba R E.What is the general form of the explicit equations of motion for constrained mechanical systems?.Journal of Applied Mechanics，2002，69(3): 335-339
[9]Udwadia F E，Kalaba R E，Phohomsiri P.Mechanical systems with nonideal constraints: explicit equations without the use of generalized inverses.Journal of Applied Mechanics，2004，71(5): 615-621
[10]Udwadia F E.New general principle of mechanics and its application to general nonideal nonholonomic systems.Journal of Engineering Mechanics，2005，131(4): 444-450
[11]Udwadia F E，Phohomsiri P.Explicit equations of motion for constrained mechanical systems with singular mass matrices and applications to multi-body dynamics.Proceedings of the Royal Society A: Mathematical，Physical and Engineering Sciences，2006，462:2097-2117
[12]赵睿英，焦生杰，王欣.多手指机器人动力学建模方法.长安大学学报(自然科学版)，2015，35(3): 151-158
[13]罗丽.基于U-K 方法的二自由度冗余驱动并联机器人动力学建模[D].西安: 长安大学，2017
[14]梁斌，袁亮，冉腾，等.基于 Udwadia-Kalaba 理论的四足机器人单腿动力学分析.机械传动，2022，46(12): 1-6
[15]姚文莉，杨流松.关于非理想系统的伍德瓦迪亚-卡拉巴动力学方程的讨论.力学与实践，2015，37(1):104-108
[16]郭明明.非光滑刚体系统动力学的 Udwadia-Kalaba方法研究[D].青岛: 青岛理工大学，2021
[17]Udwadia F E.Optimal tracking control of nonlinear dynamical systems.Proceedings of the Royal Society A: Mathematical，Physical and Engineering Sciences，2008，464: 2341-2363
[18]Udwadia F E，Mylapilli H.Constrained motion of mechanical systems and tracking control of nonlinear systems // Awrejcewicz J.Applied Non-linear Dynamical Systems: Vol.93.Cham: Springer International Publishing，2014: 213-244
[19]张佳乐，赵睿英，冯艳丽，等.基于Udwadia-Kalaba理论的自行车机器人平衡控制方法.工程科学学报，2023，45(2): 318-325
[20]刘才山.分析动力学中的基本方程与非完整约束.北京大学学报(自然科学版)，2016，52(4): 756-766
[21]陈力.基于Udwadia-Kalaba 方程的机械臂末端轨迹跟踪控制研究[D].西安: 长安大学，2018
[22]Huang K，Wang M，Zhen S.Udwadia-Kalaba equation based adaptive robust control for two-wheeled inverted pendulum system: underactuation and uncertainty.International Journal of Control，Automation and Systems，2021，19(7): 2420-2430
[23]Wei J，Tao G，Zhang J，et al.Foot trajectory following control of hexapod robot based on Udwadia-Kalaba theory.Nonlinear Dynamics，2023，111(15): 14055-14075
[24]Baumgarte J.Stabilization of constraints and integrals of motion in dynamical systems.Computer Methods in Applied Mechanics and Engineering，1972，1(1): 1-16
[25]马小燕.绝对值方程的算法研究[D].呼和浩特: 内蒙古大学，2018
[26]Chen Y H.Mechanical systems under servo constraints: the Lagrange’s approach.Mechatronics，2005，15(3): 317-337
[27]刘丽兰，刘宏昭，吴子英，等.机械系统中摩擦模型的研究进展.力学进展，2008，38(2): 201-213