基于轨迹优化的渐近稳定控制方法，用于具有第一关节执行器的垂直欠驱动操纵器 陈宇飞（Yufei Chen） 王乐军（Lejun Wang） 何斌（Bin He） 秦雷（Lei Qin） 高原宇（Yu Gao）

《Actuators》：Asymptotic Stabilization Control Based on Trajectory Optimization for Vertical Underactuated Manipulators with the First Joint Actuator Yufei Chen, Lejun Wang, Bin He, Lei Qin and Yu Gao

　　摘要 不充分驱动系统控制是非线性系统控制中的一个核心课题。对于仅有第一个关节被驱动的三连杆垂直不充分驱动操纵器（APP操纵器），实现摆动上升和平衡的控制目标具有挑战性。本文的优势如下：（i）提出的方法避免了传统分割控制中平衡区域划分的问题，防止了由于划分不当而无法在目标位置稳定。（ii）通过参数调整优化的基于时间的切换条件比基于状态的切换条件更容易满足。（iii）所提出的控制器有效抑制了由切换引起的状态波动，使得过渡更加平滑。（iv）该控制器避免了奇异性问题。主要步骤如下：首先建立APP操纵器的动态模型；然后设计一条轨迹，引导主动链节从初始位置移动到目标位置附近；在此基础上，根据链节之间的耦合关系优化轨迹参数，以确保所有链节能够同时到达目标位置附近；接着开发基于NFTSM的跟踪控制器，沿优化轨迹引导链节运动；之后，采用基于LQR的稳定控制器将系统锁定在目标位置；最后通过仿真验证了所提方法的有效性。

1. 引言 不充分驱动系统是一类输入数量小于自由度数量的非线性系统[1]。许多现有系统属于此类别，例如船舶起重机[2,3]、水面船舶[4,5]以及在近年来广泛应用于各个领域的机器人[6,7]。由于其在控制灵活性和能源效率方面的优势，不充分驱动系统在非线性控制领域具有重要的理论和工程价值。机器人操纵器是一种典型的不充分驱动系统。由于缺乏相应的执行器，操纵器的被动链节无法直接控制。此外，被动链节的动态方程是非可积的，这使得系统受到二阶非完整约束[8,9]的影响。此外，链节之间的强耦合关系在运动过程中容易产生奇异性[10]，这使得控制不充分驱动的操纵器变得尤为困难。本文关注的是仅有第一个关节被驱动的三连杆垂直操纵器（APP操纵器）。与Pendubot[11,12]和Acrobot[13]等系统相比，垂直APP操纵器表现出更复杂的非线性和耦合关系，在摆动上升过程中更容易出现奇异性，使得其控制更加困难。对于垂直操纵器，控制目标是从垂直向下的初始位置（DIP）将其摆动上升，最终稳定在垂直向上的目标位置（UTP）[14,15]。目前，人们广泛采用分割控制方法来实现这一控制目标[16]。该方法的核心是将运动空间划分为摆动上升区域和平衡区域，并分别设计控制器来完成摆动上升和稳定控制。设计摆动上升区域控制器的方法包括基于能量的方法[17]、基于模糊控制的方法[18]等。对于平衡区域，通常使用线性二次调节器（LQR）[19]或极点配置方法[20]来设计稳定控制器。然后通过在摆动上升区域和平衡区域之间切换控制器来达到控制目标。尽管上述分割控制方法在许多情况下是有效的，但目前仍没有统一的平衡区域划分标准，这使得基于状态的切换条件难以满足。如果该区域设置得过大，容易导致扭矩突变甚至稳定失败；如果设置得过小，则切换条件难以满足，且系统无法可靠地进入平衡区域，可能导致控制时间过长。此外，系统的强耦合关系和固有的非线性特性在摆动上升阶段容易引入奇异行为。轨迹规划也是切换控制中的常见方法[21,22]。与传统的基于能量的控制方法相比，这种方法避免了复杂的运动解耦，简化了控制器设计[23]。轨迹规划主要包括两个关键阶段：轨迹优化和跟踪。（1）对于轨迹优化，采用智能优化算法来寻找满足控制约束的轨迹参数是有效的策略。典型的算法包括遗传算法[24]和粒子群优化算法[25]。这些智能优化方法为本论文提出的设计提供了宝贵的启示。（2）对于轨迹跟踪，传统的非线性滑模（SM）控制被广泛采用[26]，但它无法实现有限时间内收敛，且收敛速度较慢。为了解决这一限制，研究人员相继提出了终端滑模（TSM）控制[27,28]、非奇异终端滑模（NTSM）控制[29,30]和非奇异快速终端滑模（NFTSM）控制[31,32]。其中，NFTSM不仅可以有效消除奇异性，还能加速跟踪误差的收敛。通过在NFTSM控制律中引入连续边界层，可以显著抑制抖动现象，从而有效消除控制器切换时的波动。对于垂直APP操纵器，本文提出了一种基于分割控制和轨迹优化的渐近稳定控制策略。控制结构如图1所示。首先为active链节设计一条直接从DIP摆动到UTP附近的单一轨迹；然后结合active链节和passive链节之间的耦合关系，采用改进的差分进化（IDE）算法进行离线优化轨迹参数，以确保所有链节同时到达UTP附近；接着应用基于NFTSM的跟踪控制器沿优化轨迹引导链节运动；当系统接近UTP时，切换到基于LQR的稳定控制器，以补偿重力干扰并保持稳态平衡。

2. 系统动态模型 APP操纵器的结构如图2所示。设第i个（?? =1,2,3）链节的质量、长度和关于质心的惯性分别为????、????和????。此外，??????表示第i个关节到该链节质心的距离，g为重力加速度。该系统的动态模型可以通过拉格朗日方程建立。选择广义坐标?? =[??1,??2,??3]??，系统的动态方程可以表示为： ???(??)?¨??+???(??,˙??)?˙??+???(??)=?? (1) 其中???(??) ∈??3×3是一个正定对称惯性矩阵，???(??,˙??)?˙?? ∈??3×1代表科里奥利力和离心力向量，???(??) ∈??3×1代表重力向量。它们的结构如下： ???(??)=?? ????11??12??13??21??22??23??31??32??33?? ??,???(??,˙??)?˙??+???(??)=?? ????1??2??3?? ?? (2) 这些表达式可以在[33]中找到。关节扭矩由?? =[??1,0,0]??给出，其中只有第一个关节的扭矩不为零，反映了APP操纵器的不充分驱动特性。将方程（2）代入方程（1）得到： ?{ {?{ {???11?¨??1+??12?¨??2+??13?¨??3+??1=??1??21?¨??1+??22?¨??2+??23?¨??3+??2=0??31?¨??1+??32?¨??2+??33?¨??3+??3=0(3a)(3b)(3c) 方程（3b）和（3c）揭示了active链节和passive链节之间的耦合关系，这对控制器设计和分析至关重要。为了便于后续的控制分析和仿真实现，定义状态向量?? =[??1,??2,??3,??4,??5,??6]??，系统可以表示为标准状态空间形式： ?{ {?{ {??˙??1=??4,˙??2=??5,˙??3=??6, (4a) ˙??4:6=???(??)+???(??)???1. (4b) 其中非线性函数???(??)和输入矩阵???(??)满足： {???(??)=????1?(??)????(??,˙??)???(????)=???1?(??)???1,??1=[100]?? (5)

3. 轨迹优化 本节首先为active链节设计一条轨迹，以确保它从初始状态过渡到目标状态。根据active链节和passive链节之间的动态耦合关系，可以间接从active链节的轨迹推导出passive链节的状态。随后，开发一个轨迹评估函数，综合考虑active链节和passive链节的状态。最后，引入IDE算法优化轨迹参数，确保所有链节同时收敛到UTP附近。

3.1. 轨迹设计 定义[ˉ??1,˙ˉ??1]为active链节的状态。根据系统控制目标，active链节的初始状态和目标状态分别为[??1???,0]和[??1???,0]。基于此，为active链节设计一条连续轨迹： ˉ??1=??1???+??1???2????(?????1)22???21+??2???2????(?????2)22???22+[?????????(?????+1)?1]?(??1??????1???)????????+1 (6) 其中??1,??2,??1,??2,??1,??2,??是未确定的轨迹参数。对ˉ??1?(??)求导，得到active链节的速度轨迹˙ˉ??1?(??)： ˙ˉ??1?(??)=???2????(??10???1???)?????????·?????3???+??3???(?????3???+1)2+2?????? ? ? ????1????(?????1)22???21+??2????(?????2)22???22?? ? ? ? ?????2??? ? ? ????1?(?????1)??21????(?????1)22???21+??2?(?????2)??22????(?????2)22???22?? ? ? ?? (7) 对˙ˉ??1?(??)进一步求导，得到active链节的加速度轨迹?ˉ??1?(??)： ¨ˉ??1?(??)=??2?(??10???1???)??????3???3?[(1???3???)?????3???+1???3???·2?????3???+1??????3????(1???3???)?2????2???3????(1???3???)(?????3???+1)3]+2??? ? ? ????1????(?????1)22???21+??2????(?????2)22???22?? ? ? ???2?????? ? ? ????1?(?????1)??21????(?????1)22???21+??2?(?????2)??22????(?????2)22???22?? ? ? ?????2?[??1??21???1?(?????1)2??41]????(?????1)22???21???2?[??2??22???2?(?????2)2??42]????(?????2)22???22 (8) 可以验证： {ˉ??1?(0)=??1???,˙ˉ??1?(0)=0（初始位置），ˉ??1?(∞)=??1???,˙ˉ??1?(∞)=0（目标位置）。 (9) 从上述轨迹可以看出，˙ˉ??1和?ˉ??1是均匀连续的。这条轨迹允许active链节从初始状态[??1???,0]开始移动，并最终收敛到目标状态[??1???,0]。由于passive链节不能直接施加扭矩，因此直接进行轨迹规划是不可行的。然而，通过利用它们与active链节的动态耦合关系，可以从方程（3b）推导出passive链节的状态： ?{ { {?{ { {?¨ˉ??2=??3???23???2???33+(??23???31???21???33)?¨ˉ??1??22???33???23???32¨ˉ??3=???3???22???2???32+(??22???31???21???32)?¨ˉ??1??22???33???23???32 (10) 可以通过数值方法求解相应的微分方程，得到passive链节的状态变量ˉ??2, ˉ??3和˙ˉ??2, ˙ˉ??3。为了确保active链节和passive链节在统一规定的时间范围内渐近收敛，制定了包括两个链节状态约束的完整目标函数： ??=|ˉ??2?2???2???|+∣˙ˉ??2∣+|ˉ??3?2???3???|+∣˙ˉ??3∣?________?________?PassiveLinkConstraint+|ˉ??1?2???1???|+∣˙ˉ??1∣?___?___?ActiveLinkConstraint(??1,??2,??3∈?+) (11) ?{ {?{ {?ˉ??1=ˉ??这个控制器的核心在于设计了一种具有快速收敛特性的非奇异滑模面，并结合了连续边界层方法，有效抑制了控制器切换引起的状态波动。主动连杆的位置跟踪误差方程（15a）和速度跟踪误差方程（15b）定义如下：{??=??1?ˉ??1,˙??=˙??1?˙ˉ??1}（15a）（15b）。通过对方程（15b）中的表达式求导，然后代入方程（4b），我们得到：¨??=¨??1?¨ˉ??1=??+??????¨ˉ??1（16）。NFTSM表面定义如下：??=??+???|??|??1·sign?(??)+???|˙??|??2·sign?(˙??）（17），其中?? >0, ?? >0, 1 ??2。求导后的结果为：˙??=˙??+?????1?|??|??1?1?˙??·sign?(??)+?????2?|˙??|??2?1·sign?(˙??)?(??+??????¨ˉ??1）（18）。定义到达律为：˙??=?????????·sign?(??）（19）。整合方程（18）和（19），得到控制器如下：??=?1???(???¨ˉ??1+?????+??·sign?(??)·1+?????1?|??|??1?1·sign?(??)?????2?|˙??|??2?2·sign?(˙??）（20）。各参数的描述见表1。本文设计的滑模具有以下两个特点：表1. NFTSMC的参数。 （1）非奇异性：条件1 ??; 1 if |??|≤??; -1 if ??
5. 稳定控制器设计
当?? >????时，APP操纵器已经到达UTP附近，因此需要立即在UTP处进行稳定。由于重力的持续作用，单独的NFTSMC无法在UTP处稳定操纵器。然而，目标位置附近的系统动态可以用线性模型近似表示。因此，我们采用LQR方法设计一个稳定控制器，实现在目标位置的稳定调节。稳定控制器的设计基于UTP处的线性化系统模型：˙??=?????+?????（26），其中A是系统矩阵，B是对应于主动连杆控制输入的输入矩阵。定义LQR性能指标，以平衡状态偏差和控制力：Ψ=∫∞0[??????????+?????2]????（27），其中正定矩阵R限制状态偏离目标位置，正半定矩阵Q惩罚控制输入??的幅度。根据最优控制理论，最小化Ψ的稳定控制器采用状态反馈形式：??=??????（28），其中增益向量K是从代数Riccati方程的对称正定解W中得到的：???????+???????????????1????????+??=0（29），具体来说，?? =???1????????。给定Q和R后，可以通过求解Riccati方程得到K。将方程（28）代入线性系统，得到稳定的闭环动态，从而在目标位置保持操纵器的稳定性。

6. 仿真结果
在本节中，通过在三个区间[2.0,2.5],[2.5,3.0], 和 [3.0,3.5]内设置????来进行仿真实验。实验旨在验证所提出控制策略的有效性。此外，在控制器切换期间，还比较了NFTSMC和SMC的波动抑制性能，以进一步验证所提控制器的有效性。
6.1. 情况1
APP操纵器的结构参数取自[33]，见表2。初始状态设置为[???,0,0,0,0,0]，目标状态设置为[0,?2???,2???,0,0,0]。本文中使用的IDE算法的参数设置如下：{????=50,??max=500,??0=0.5,?????0=0.75}（30）。将???? ∈[2.0,2.5]，结合表2，使用IDE算法优化后得到的轨迹参数如下：{????=2.2465,??=4.9401,??1=?2.4844,??2=1.7705,??1=0.3206,??2=0.9612,??1=0.0673,??2=0.1735}（31）。图4中的仿真结果表明，当?? <2.2465 s时，连杆在跟踪控制器方程（22）的作用下逐渐从DIP摆动到UTP附近。在此过程中，最大扭矩约为186 N·m，处于合理范围内。当?? =2.2465 s时，控制器从跟踪控制器方程（22）平滑切换到稳定控制器方程（28）。这种切换不会导致状态突变，也不会破坏系统的收敛趋势。进入?? >2.246 s的阶段后，所有连杆的关节角度稳定在[0,?2???,2???]，角速度和扭矩稳定在[0,0,0]，末端执行器通过稳定控制器方程（28）在UTP处稳定。图4. ???? ∈[2.0,2.5] s的仿真结果。
6.2. 情况2
为了进一步验证所提出方法的有效性，初始状态设置为[???,0,0,0,0,0]，目标状态设置为[0,0,0,0,0,0]。IDE算法的参数保持与方程（30）一致。将???? ∈[2.5,3.0]作为约束，结合表2，使用IDE算法优化后得到的轨迹参数如下：{????=2.8600,??=4.5037,??1=?6.8000,??2=7.5151,??1=0.0703,??2=0.6026,??1=0.2320,??2=0.4164}（32）。如图5所示，使用跟踪控制器方程（22）时，当?? <2.86 s时，所有连杆从DIP摆动到UTP附近。然而，在?? <0.95 s期间，施加在关节上的控制扭矩相对较大，如图5c所示。突然的角速度变化导致较大的角加速度，因此需要更大的扭矩。当达到切换时间?? =2.86 s时，控制器从跟踪控制器方程（22）切换到稳定控制器方程（28）。由于此时?? <0.001，所有连杆的状态已经接近最终稳态。因此，这种切换不会破坏关节角度、角速度或扭矩的收敛行为。当?? >2.86 s时，系统达到稳态，所有关节角度保持在[0,0,0]，施加在第一个关节上的扭矩和所有关节的角速度趋近于零。图5. ???? ∈[2.5,3.0] s的仿真结果。
6.3. 情况3
在这种情况下，系统的目标状态设置为[0,?10???,6???]，而IDE算法的初始状态和参数保持不变。对于???? ∈[3.0,3.5]，优化后得到的轨迹参数如下：{????=3.1428,??=4.2905,??1=?5.6340,??2=7.5830,??1=0.0052,??2=0.4340,??1=0.3161,??2=0.4480}（33）。如图6所示，当???? <3.1428 s时，连杆在跟踪控制器方程（22）的作用下摆动到UTP附近。当????? =3.1428 s时，控制器切换完成。从图6c可以看出，在控制器切换瞬间没有突然的扭矩变化，因此实现了更平滑的过渡。当????? >3.1428 s后，系统达到目标状态，其中关节角度的稳态值为[0,?10???,6???] rad，角速度和扭矩的稳态值接近零。所提出的方法有效避免了平衡区域划分的难度以及奇异性问题；系统可以在规定时间内直接摆动到UTP附近。图6. ???? ∈[3.0,3.5] s的仿真结果。
6.4. 控制器性能验证
6.4.1. 波动抑制性能
为了清楚地展示NFTSMC在控制器切换时刻的状态波动抑制性能，我们在???? ∈[2.5,3.0]的范围内获得了一组满足?? <0.06的轨迹参数。优化的轨迹参数如下：{????=2.8728,??=4.4983,??1=?6.7954,??2=7.5191,??1=0.0705,??2=0.6025,??1=0.2321,??2=0.4162}（34）。在这种情况下，由于f相对较大，当连杆在????时移动到UTP附近时存在偏差，导致关节角度波动。对于方程（22）中的NFTSMC，参数如下：{??=18,??=10,??=12,??=8,??1=1.6,??2=1.1,??=0.01}（35）。对于方程（23）中的SMC，使用[23]中的参数如下：??1=1.2,??2=1.0,??3=0.1}（36）。如图7所示，在控制器切换瞬间，SMC中的符号函数导致明显的角度波动。相比之下，NFTSMC在相同的结构参数和仿真条件下产生较小的角度波动。图7. ????时SMC和NFTSMC的角度波动。图7中的角度波动数据汇总在表3中。根据表3中的统计结果，可以计算SMC的绝对角度波动幅度。具体计算公式如下：??=|??SMC???NFTSMC|/??SMC×100%=56.8%（37）。根据绝对角度波动幅度，NFTSMC可以在控制器切换时刻抑制波动。具体来说，NFTSMC的角度波动抑制效果比SMC提高了50%以上。
6.4.2. 鲁棒性能
为了验证NFTSMC的鲁棒性，在?? ∈[1.0, 2.0] s期间施加了振幅为0.5的阶跃干扰???(??)。结果如图8所示。图8. 干扰下的NFTSM和SMC的跟踪误差和滑模面变化。(a) 跟踪误差比较；(b) 滑模面变化。图8a显示NFTSMC实现了较低的峰值误差和较短的恢复时间。图8b进一步表明，在干扰下，SMC的滑模面表现出较大的偏差，而NFTSMC的滑模面保持较小的偏差。这些结果验证了NFTSMC相比SMC具有更高的跟踪精度和鲁棒性。
7. 结论
本文提出了一种基于轨迹优化的APP操纵器渐近稳定控制方法，实现了系统的摆动和稳定控制。首先建立了APP操纵器的动态模型，并为主动连杆规划了从DIP到UTP的轨迹。然后，基于轨迹和连杆之间的耦合关系，使用IDE算法在规定的时间范围内优化轨迹参数，使所有连杆能够同时到达UTP附近。在此基础上，引入了控制器切换方案以实现控制目标。在摆动阶段，设计了基于NFTSM的跟踪控制器，使主动连杆沿着优化轨迹摆动。在系统接近UTP附近后，进一步应用基于LQR的稳定控制器在UTP处稳定系统。最后，在仿真中将控制器切换时间设置为[2.0,3.5] s，验证了所提方法的有效性。结果表明，与基于能量的控制相比，所提方法实现了更快的摆动和稳定，避免了区域划分并简化了切换条件。NFTSMC和SMC的比较仿真显示，控制器在切换期间有效抑制了状态波动，并提供了更平滑的稳定效果。实际上，本文提出的轨迹设计不依赖于系统的结构参数，仅依赖于连杆的初始和目标状态。因此，系统结构参数的变化不会影响轨迹设计，证明了所提方法的普适性。此外，所提方法避免了平衡区域划分的难度，设计的时间切换条件比状态切换条件更容易满足。设计的控制器有效抑制了状态波动并避免了奇异性。在实际情况中，系统可能受到许多因素的影响，如混沌、建模不确定性和外部干扰[34]。因此，仍需进一步提高方法的整体鲁棒性。在未来的工作中，我们将建立实验平台来进一步验证所提方法的有效性并改进控制策略。