面向人機共享控制的機械臂動態連續仿真研究

中圖分類號：TP242.6 DOI： 10.16578/j. issn.1004.2539.2025.05.007

0 引言

目前，共享控制作為一種主流的機器人控制方式，在面對復雜任務和作業環境時，能夠充分發揮人和機器的各自優勢[，被廣泛應用在醫療康復訓練[2-3]導航避障4-5以及遠程操作6-等方面。當機械臂處于共享控制狀態時，控制方式的改變會導致切換時刻控制器輸入信號產生突變，造成機械臂振動、關節磨損等問題，進而影響控制系統的安全與穩定。

針對這類情況，于振中[0將波預測方法用于變時延遙操作共享控制系統中，并提出一種從端校正控制方法，能夠消除遙操作過程中由時延、數據丟包以及預測模型不準確引起的穩態位置誤差。周大鵬在人機交互中采用了一種與主從臂位置誤差相關的力反饋方式，從而使操作者了解從臂對主臂的跟隨情況，提高了系統運行安全性。此類方法雖能消除遙操作過程中機械臂的位置誤差，但沒有考慮共享控制方式下由于控制方式轉變產生的角度突變問題。GHALAMZAN等為解決主從抓取與操縱中共享控制的問題，引入一種面向任務的速度可操縱性成本函數，用于識別抓取后運動期間機械手的最大運動能力，減少了控制工作量。OZDAMAR等[3提出一種遠程操縱框架，可單獨組合控制任意數量機械臂，通過模仿操作者手臂阻抗和位置，參考解決物理交互的復雜任務。此類方法注重對于共享控制策略與框架的研究，實現對應交互任務，但忽略了控制過程中誤差信號對系統動態連續性的影響。

綜上所述，共享控制的動態連續性研究主要有如下問題：一是只關注遙操作控制中時延、控制算法受限等因素導致的角度突變[14-17]，采用主從端校準的方式或關節角度補償等方法解決誤差信號，但對于共享控制中控制權重切換導致的角度誤差缺少對應研究；二是針對人機共享控制效果的研究主要集中在共享控制框架及控制策略等方面，對控制過程中系統連續性的研究不足，對運行角速度與角加速度連續性的分析較少。

針對上述問題，本文給出可行的共享控制策略，嘗試將控制權重切換時刻下機械臂自主與操作者遙操作輸入信號誤差導致的角度突變，看作一個局部軌跡規劃問題，采用Bezier曲線局部擬合的方法實現局部軌跡優化，使機械臂運行軌跡在控制權重由機器向人轉變過程中輸入角度信號連續、速度可導，抑制加速度振蕩，最終克服角度突變對控制系統的影響。

1人機共享控制策略

針對機械臂末端到達目標物位置的任務，根據給定的起始點與終點位置進行軌跡規劃。在該過程中，位于遠端操作端的操作者根據交互界面顯示的關節角度，判斷是否需要進行人為干預。若需人為干預，則依靠共享策略調整控制權重 η ，決定機械臂的控制方式，最后將共享控制指令傳輸給機械臂。

本文通過關節角閾值 、時間閾值 進行判斷，建立人機共享控制模型，將整個共享控制定義為3個不同階段的工作狀態，具體如下：

式中， X ， θ 1 T 分別為操作者指令信號、實際與預期角度誤差、干預持續時間； 、 、 分別為操作者指令信號、實際與預期角度誤差、干預持續時間的閥值；式（1）由上至下分別為主從控制、自主控制和共享控制狀態。

2局部軌跡優化

考慮到本文的研究場景涉及交互方式構造軌跡，故將機械臂在控制權重切換時刻的角度突變問題轉變為局部軌跡規劃問題，即定義一段時間作為軌跡規劃時間，將兩種控制方式下輸入位置信號視為軌跡的兩個端點，通過該段時間進行局部軌跡優化，使輸入信號能夠平穩地從自主控制下的輸入信號過渡至操作者的遙操作控制信號，使整體輸入信號平穩連續。

針對軌跡規劃的曲線選擇，考慮到本文中間段的軌跡優化只涉及路徑點的局部數據，并不涉及兩邊相鄰段，選擇局部軌跡規劃的方法更為有效；同時，滿足速度可導、加速度連續及對最大速度與加速度的約束效果。

3次Bezier曲線插值方法作為一種局部擬合算法，計算量相對較小；單個控制點的改變對整體曲線的影響不大[18]。其曲線方程為

式中， 為運動時間； 表示多項式對于連續函數的逼近形式，即伯恩斯坦（Bernstein）基函數； 為第 j 段Bezier曲線形狀的控制點； 為三次伯恩斯坦多項式的組合數系數，用于控制基函數 的權重比例； 為用于加權不同控制點 對曲線的影響。

由于曲線需要經過首末2個點，則直接給出 和 ，剩余控制點可由 和 上所預設的切線方向確定，使Bezier曲線的中間點與各端點的1階導數幅值相等，即

式中， α 為Bezier曲線中對應點的1階導數幅值，即速度大小。

以此保證中間控制點處的速度相等。同時，針對中間控制點，選擇為

式中， 分別為曲線起始、結束的時間參數。

根據Bezier曲線段的插值點 計算軌跡的步驟如下：

1）給定一系列需插值的點 及對應時刻的 定義切向量和曲率向量分別為

并通過估計每一處的導數求其切向量，即

2）在每一對 中，Bezier曲線 [0，1]通過以下條件獲得

利用此過程，得到一條由 n 段Bezier曲線組成的軌跡，每段軌跡均歸一化在區間[0，1]上。同時，為將整條幾何路徑表示為同一變量，將每段軌跡單位化處理，進而將每段曲線表示為參數 u 的函數，即

由于相鄰Bezier曲線的切向量方向相同但幅值不同，利用此方式構造的曲線 p （ u ） 是速度曲線 連續的。為了獲得 連續軌跡，通過縮放各段曲線的方式參數化各段曲線，假設

式中， 為第 k 段Bezier曲線的歸一化局部參數，用于將全局參數轉化為局部參數的中間變量； 為基

于控制點間距的縮放因子，用于參數歸一化。各段的初始時刻為

基于上述方法處理后，各段端點處的切向量均為單位長度，從而使軌跡速度曲線保持連續。

3 仿真與分析

3.1仿真設計與模型建立

為驗證基于3次Bezier曲線插值的機械臂軌跡優化方法的整體有效性，進行仿真分析。本文給出機械臂的相關參數（表1），以此保證仿真結果的可信度。

在SolidWorks軟件中根據D-H改進參數建立AUBOi5機械臂模型并生成其.URDF文件，得到上述公式中對應變量。將文件導人Simulink中，建立機械臂的Simscape模型。機械臂仿真動力學模型如圖1所示。

在動力學方程中，關節角度、角速度和角加速度三者的數學關系，是機械臂關節驅動對執行器位置與速度作用的體現。故基于同一機械臂動力學模型進行仿真，以角度、角速度和角加速度作為指標，利用3次Bezier曲線、樣條曲線和5次多項式曲線分別進行曲線局部軌跡優化，對比分析本文所用算法對于控制系統穩定連續的有效性。

樣條曲線通過一系列控制點生成一條光滑且連續的軌跡，該方法能夠將機械臂的動作軌跡轉化為光滑軌跡，以此提高整體運動質量和效率。優化軌跡的連續性和平滑性，可減少運動過程中的振動和機械臂磨損。樣條曲線的數學模型為

當 時，其對應系數為

5次多項式為局部軌跡規劃中常用的算法之一，被廣泛應用于最優軌跡規劃控制中。通過對初始點與終點添加動力學邊界約束條件，實現局部的軌跡規劃。相比于3次多項式曲線，5次多項式曲線添加了對于加速度的限制，從角度位置、速度以及加速度6個約束條件對初始點和終點進行約束，以此獲得連續的加速度軌跡曲線，減少對于運動鏈和慣性負載敏感的應用中期望值受影響的情況。其數學模型為

對應多項式系數為

式中， h 為始末時刻下的總位移。

基于上述數學模型，在Simulink下搭建對應軌跡生成模塊；同時，根據本次仿真的目的，給出基于局部軌跡優化的機械臂控制框圖，如圖2所示。

針對仿真采用的運行軌跡給出定義：0＼～10s為整體共享控制時間段，0＼～4s為機械臂自主控制時間段，6＼～10s為操作者的遙操作控制時間段，4＼～6s為局部軌跡優化時間段。針對自主控制和遙操作階段，分別給定端點時刻處的位置、速度及加速度，采用點對點的5次多項式軌跡規劃，所得軌跡分別作為自主控制與遙操作控制的輸入信號。

3.2 仿真結果及分析

3.2.1關節位置角度曲線

針對機械臂的運動軌跡控制，關節轉角直接影響軌跡優化的結果。作為描述關節位置的參數，關節角的變化導致力矩的產生，關節角度的動態連續利于機械臂平穩運行。將前文建立的機械臂動力學模型與軌跡規劃方法相結合，利用Matlab/Simulink仿真軟件生成對應關節角度曲線，如圖3所示。

通過圖3可以看出，在4＼～6s的軌跡規劃時間段內，采用3次Bezier曲線、樣條曲線和5次多項式曲線3種規劃方式的關節位置角度曲線均保持平滑，且在 t = 4 s和 t = 6 s時間點與相鄰兩端軌跡連續，無明顯角度突變現象，即在控制權重發生變化時，關節角度整體位置曲線是連續的。

3.2.2關節角速度曲線

角速度作為描述角度變化的參數，通過角速度連續變化可以更改控制機械臂位置，同時在動力學模型中直接影響關節的慣性與慣性力。基于歐拉動力學方程，力矩的實時調節能力使得當角速度連續變化（角加速度有界）時，控制系統可通過前饋補償和反饋調節來抵消慣性耦合效應。這種主動力矩控制能有效平滑關節驅動力矩的瞬態波動，從而降低運動沖擊對機械結構的動態載荷，延長傳動部件的使用壽命。關節角速度曲線變化如圖4所示。

通過圖4可知，3次Bezier曲線、樣條曲線、5次多項式曲線振幅依次為： 由此說明，采用3次Bezier曲線插值軌跡優化，機械臂關節角速度曲線在給定的軌跡規劃時間內變化平穩，變化幅度相較其他兩種軌跡規劃方式更小，利于機械臂運動控制；同時，在 t = 4 s 和 t = 6 銜接處無明顯突變，能夠有效避免加速度突變導致的力矩躍變和關節振動，減少磨損，保持系統的穩定性。

3.2.3 關節角加速度曲線

角加速度作為描述角度變化率的參數。控制機械臂的連續角加速度，能夠更好地控制機械臂的速度和位置變化；同時，在動力學模型中其與關節受到的合力與合力矩密切相關，并影響關節的慣性力和加速度。動態連續的角加速度有利于調整機械臂的動力學響應，減小由于角加速度躍變帶來的沖擊力和額外應力，進而實現對機械臂的精確控制。軌跡優化后的角加速度曲線如圖5所示。

圖5中，在整體運行軌跡曲線中，兩端各自代表機器和操作者的運行軌跡。由于機械臂關節耦合、關節控制方式等的作用，加速度曲線出現振蕩現象。但在軌跡優化時間段內，本文采用的3次Bezier曲線插值的方法加速度無明顯突變現象，且在5＼～5.5s時間段內產生的振蕩幅度比其他兩種方式更小，魯棒性能更優，可有效減小由加速度突變帶來的沖擊力，利于實現對機械臂精確、平滑的運動控制。

4結論

針對機械臂在共享控制過程中，控制權重改變導致的輸入角度突變的問題，采用3次Bezier曲線插值的方式，對兩種控制方式在不同時刻下信號值進行局部擬合，通過Matlab/Simulink平臺搭建機械臂的動力學模型并作為控制對象，對軌跡規劃后整體曲線的角度位置、角速度和角加速度進行了分析。仿真結果表明，本文采用算法能夠實現在給定軌跡規劃時間段內角度位置曲線平滑、角速度曲線連續，并抑制角加速度曲線振蕩，具有良好的魯棒性，驗證了所采用算法的有效性。

本文采用的創新方法是以控制權重改變造成的角度突變對系統連續性的影響為研究目標，針對性地提出一種抗角度突變的局部軌跡規劃優化方法。將機械臂自主控制與操作者遙操作下的輸入信號誤差看作整體軌跡曲線中的兩個端點，通過3次Bezier曲線插值的方法實現局部軌跡優化，保證整體輸入信號連續，從而使控制系統動態連續；在簡化算法的同時還減少了對設備硬件的依賴。后續可在本文采用的算法基礎上進一步優化，主要從兩個方面進行考慮： ① 提高Bezier曲線的階數，構建 或更高階連續軌跡曲線，通過對更多參數進行限制，實現機械臂關節加速度或加加速度等曲線連續平滑； ② 將神經網絡或深度學習相關算法與人機共享控制相結合，探索有效擬合軌跡曲線的方法，最終實現機械臂的動態連續，但目前該方法在人機共享領域的應用還有待研究。

參考文獻

[1]ZHUYC，YANGCJ，WEIQX，etal.Human-robot shared control for humanoid manipulator trajectory planning［J].Industrial Robot，2020，47（3）：395-407.

[2] RAVIOLAA，COCCIAA，DEMARTINA，etal.Developmentof ahuman-robot interfacefora safeand intuitive telecontrol ofcollaborative robots in industrial applications[C]//Advances in Service and Industrial Robotics.Cham：Springer，2022：556-563.

[3]XUY，DINGC，SHUXK，et al. Shared control of a robotic arm usingnon-invasive brain-computer interface and computer vision guidance[J].Robotics and Autonomous Systems，2019，115： 121-129.

[4]LIYN，TEEKP，YANR，etal.Reinforcement learning for human-robot shared control[J].Assembly Automation， 2 0 2 0 ， 4 0 （ 1 ） ： 105-117.

[5] LIC，ZHENG ZX，YUANJP.Trajectory tracking forrepeatedimpact-based detumbling using a multi-arm space robot[J].Aerospace Science and Technology，2023，133：108144.

[6] GIRBES-JUANV，SCHETTINOV，GRACIAL，etal.Combining haptics and inertial motion capture to enhance remote control of a dual-arm robot[J].Journal on Multimodal User Interfaces，2022， 16（2）：219-238.

[7]UBEDA A，IANEZ E，AZORiN JM. Shared control architecture based on RFID to control a robot arm using a spontaneous brainmachine interface[J].Robotics and Autonomous Systems，2013， 61（8）：768-774.

[8]ZHAO J，YAO XB，ZHANG L. The optimization of initial posture with avoidance of the sudden changein joint velocity for fault tolerant operations of two coordinating redundant manipulators[J]. MechanismandMachine Theory，2005，40（6）：659-668.

[9]羅莎祁.非結構環境下基于示教學習的多自由度機械臂共享控

制方法研究[D].重慶：重慶大學，2022：21-22. LUO Shaqi. Research on shared control method of multi-degree-offreedom manipulator based on learning from demonstration in unstructured environments[D].Chongqing：Chongqing University， 2022：21-22.

[10]于振中.移動操作機器人及其共享控制的力反饋遙操作研究 [D].哈爾濱：哈爾濱工業大學，2010：47-116. YU Zhenzhong.Research on mobile manipulator and its force feedback teleoperation based on shared control mode[D].Harbin： HarbinInstituteofTechnology，2010：47-116.

[11]周大鵬.一種液壓機械臂的主從交互控制系統研究[D].濟南：山 東建筑大學，2021：10-59. ZHOU Dapeng.Research on a master-slave interaction control system of a hydraulic robotic arm[D]. Jinan：Shandong Jianzhu University，2021：10-59.

[12]GHALAMZANAM，ABI-FARRAJF，GIORDANOPR，etal. Human-in-the-loop optimisation：mixed initiative grasping for optimally facilitating post-grasp manipulative actions[C]/2017 IEEE/RSJ International Conference on Intelligent Robots and Systems（IROS）.NewYork：IEEE，2017：3386-3393.

[13]OZDAMARI，LAGHI M，GRIOLIG，et al.A shared autonomy reconfigurablecontrol frameworkfortelemanipulationofmulti-arm systems[J].IEEE RoboticsandAutomationLetters，2022，7（4）： y95/-9944.

[14]MAD，SUNQ，LIX.Synchronization of master-slave chaotic systemwithcouplingtime-varyingdelaybasedonsampled-datacontrol[C]//The 27th Chinese Control and Decision Conference （2015 CCDC）.2015：6545-6550.

[15]MANSORNAN，JAMALUDDINMH，SHUKORAZ，etal.A studyof accuracyandtimedelayforbilateralmaster-slave industrial robotic arm manipulator system[J].MATECWeb ofConferences，2018，150：01015.

[16]KANAISHI K，ISHIBASHI Y，HUANG P，et al. Effects of adaptive Δ -causality control for cooperation between remote robot systemswith force feedback byusingmaster-slave relation[C]//Proceedingsofthe 3rd International Conference on Telecommunicationsand Communication Engineering.NewYork：Association for ComputingMachinery，2019：34-37.

[17]WANG ZS，LIYB，SUNP，etal.A multi-objective approach for thetrajectoryplanningof a7-DOF serial-parallel hybrid humanoid arm[J].MechanismandMachineTheory，2021，165：104423.

[18]CHENT，CAIYF，CHENL，et al. Trajectory and velocity planning method of emergency rescue vehicle based on segmented three-dimensional quartic Bezier curve[J].IEEE Transactions on Intelligent Transportation Systems，2023，24（3）：3461-3475.

Dynamic continuous simulation study of robotic arms for human-machine shared control

YAO Zhenqiu12 ZHOU Xingqi12 GUO Hao2FENG Chuanpu1，2 （1.SchoolofNavalArchitecture amp;OceanEnginering，Jiangsu UniversityofScienceandTechnology，Zhenjiang 2110o，China） （2.MarineEquipmentandTechnologyInstitute，JiangsuUnversityofScienceandTechnology，Zhenjiang22o，China）

Abstract：[ObjectivelThe human-machineshared control iswidelyused intheremotecontrolofrobotsasacontrol methodthatcombines theoperator’sjudgmentaldecisions withthefinecontrolof theroboticarm.Whenthecontrolweightof theroboticarmisswitched，thejointanglechangesabruptlyduetothecompositionoftheroboticarm，signaldelayandother reasons，whichaffectsthedynamiccontinuityofthecontrolsystem.[Methods]Tosolvethisproblem，afeasiblesharedcontrol strategywasproposed todeterminethecontrol weight，alocalfitingalgorithmbasedontheinterpolationof3times Bezier curves Wasusedtorealizetheoptimizationofthetrajectoryoftheroboticarm，andthecontinuityof thecontrolsystemof the roboticarm wassimulatedandverified withrespectotheoptiizedtrajectoryofthisalgorithm.[Results]Thesimulationresults showthatbycomparingdifferentrajectoryplanningcurves，theadoptedmethodcaneectivelyinhibitthesuddenchangeofthe anglecausedbytheswitchingofcontrolweights，realizethecontinuoussmoothingofthejointangleandangularvelocitycurves atthe switching moment，andeffectively inhibit the acceleration oscillations，which has good robustness.

Keywords：Shared control; Angular discontinuity; Trajectory optimization; Dynamic continuity