雙冗余機器人協(xié)調(diào)運動中關節(jié)點位追蹤的控制策略研究

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

0 引言

隨著機器人技術的不斷發(fā)展與成熟，雙冗余機器人可在制造業(yè)、醫(yī)療服務和農(nóng)業(yè)果蔬采摘領域中進行效、便捷的作業(yè)操作[1-5]。雙機器人系統(tǒng)由兩套獨立控制器分別控制的兩臺機械臂組成，由于雙機器人系統(tǒng)的兩套控制器難以實現(xiàn)信息交互，離線編程的從臂末端無法直接讀取主臂末端可達的期望位姿。因此，對雙冗余機器人控制策略的研究十分重要。

雙機器人協(xié)調(diào)運動主要分為兩類：一類是各機械臂末端相對位姿關系保持不變的耦合運動；另一類是從臂末端相對主臂末端產(chǎn)生相對運動軌跡的疊加運動。王天瑞等針對軸孔裝配任務需求分析了雙機器人疊加運動，基于粒子群多目標優(yōu)化求解了雙機器人協(xié)作裝配位置。蘇赫朋等采用主從控制對雙機器人緊約束和松約束的協(xié)調(diào)運動進行分析，通過仿真驗證了雙機器人耦合關系約束的可行性。張振宇等基于主從控制分析了雙工業(yè)機器人耦合運動和疊加運動，仿真驗證了協(xié)同搬運物體時緊約束的有效性，但未考慮實際設備運行中通信響應時間的影響或操作空間可達的準確性。在雙機器人系統(tǒng)疊加運動中，精確協(xié)調(diào)和控制是確保運動無干擾或碰撞的關鍵。在此基礎上加入碰撞檢測算法，可大幅提高雙機器人疊加運動的安全性。當前主要有沿坐標軸對齊的矩形盒（Axis-AlignedBoundingBox，AABB）、任意方向的矩形盒（Oriented Bounding Box，OBB）、球體（Sphere）包圍盒等混合算法]。從飛等[2]針對雙機器人協(xié)調(diào)運作時可能發(fā)生的碰撞問題，根據(jù)機器人幾何特征建立了包絡簡化模型，并通過仿真驗證了算法的可行性，但未對離線編程機器人實際運行情況進行監(jiān)測。在球體和膠囊體包圍盒的基礎上，劉建春等[13采用二次投影法，根據(jù)包圍盒投影面的相交情況判斷雙機械臂是否存在碰撞問題，但未具體說明該算法與實際雙機器人系統(tǒng)的結(jié)合方式。

以上研究中，雙機器人系統(tǒng)協(xié)調(diào)運動多以仿真的形式實現(xiàn)，忽略了實際影響因素；此外，在面對疊加運動可能出現(xiàn)的碰撞問題時，如何在實際雙機器人系統(tǒng)中準確建立虛擬包絡體是關鍵。因此，在雙機器人系統(tǒng)中準確追蹤關節(jié)位置是實現(xiàn)包絡體與雙機器人系統(tǒng)虛實融合的必要條件。本文以雙冗余機器人系統(tǒng)為研究對象，將雙冗余機器人協(xié)作空間中各關節(jié)點位的精確追蹤問題轉(zhuǎn)化為機器人工作空間求解問題，提出了基于主從交替的各關節(jié)點位追蹤控制策略，試驗驗證得知，各關節(jié)計算位置與實際位置距離的最大誤差為 0.465mm ；最終通過三角形搬運軌跡的耦合運動，證明了雙冗余機器人協(xié)調(diào)運動約束的有效性，為離線編程中包絡體的建立提供條件。

1雙冗余機器人運動模型建立

本文以由兩個7自由度的XMateER3Pro協(xié)作機器人構(gòu)成的雙機器人裝配系統(tǒng)為研究對象。整個系統(tǒng)中，令右臂為“Leader”，左臂為“Follower”；定義世界坐標系{W}與Leader的基坐標系重合；左、右臂分別與各自控制器連接，上位機通過交換機在PyCharm軟件環(huán)境中同時控制左、右臂。雙冗余機器人裝配系統(tǒng)如圖1所示。

7自由度冗余機械臂在仿生學角度上與人體上肢更為相似，比較符合實際應用。在完成操作任務時，可利用冗余機械臂多余的自由度來完成本體避障、避奇異、避關節(jié)限位、關節(jié)力矩優(yōu)化、增加操作度等附加任務[14]，其主要構(gòu)型分為兩兩垂直型和連續(xù)三軸平行型，其結(jié)構(gòu)上的差異如圖2所示。實際雙冗余機器人系統(tǒng)的位置關系以及Matlab軟件中機器人的模型如圖3所示。

雙冗余機器人的控制方式主要有主從控制、位置/力混合控制、阻抗控制和自適應控制等[15]。要實現(xiàn)雙冗余機器人系統(tǒng)協(xié)調(diào)運動，建立雙機器人基于位置的約束關系是關鍵。對單機械臂運動學進行分析，采用標準D-H法對機械臂建模，得到雙冗余機器人系統(tǒng)坐標系，如圖4所示。

圖4中， d₁=341.5mm ， d₃=394.5mm ， d_s= 366.0mm ， d₇=250.3mm ， a_i-1=0 （ i=1 ，2，…，7）;? ”表示方向矢量垂直所在平面向外；‘ ’表示方向矢量垂直所在平面向內(nèi)。與ABB YuMi 機器人的運動學方程相比，本文方法是將世界坐標系{W}定義在Leader基坐標系，簡化了計算量，減少了誤差積累。7自由度冗余機械臂D-H參數(shù)如表1所示。

1.2 主臂運動學分析

機械臂運動學分析是研究其運動特性的關鍵。表1給出了描述機器人相鄰連桿的各個參數(shù)。表1中，連桿長度 a_i-1 為關節(jié)軸 i-1 與軸i之間的公垂線長度；連桿扭轉(zhuǎn)角 α_i-1 表示關節(jié)軸 i 平移 a_i-1 距離后與軸i-1相交；連桿偏距 d_i 為相鄰兩個連桿共軸線方向上的距離；關節(jié)角 θ_i 為相鄰兩個連桿以公軸線為旋轉(zhuǎn)軸的夾角。相鄰連桿坐標系位姿相對關系為

式中，s θ_i 表示 sinθ_i ；c θ_i 表示 cosθ_i ； cα_i-1 表示（204號 cosα_i-1 ；s ?α_i-1 表示 sinα_i-1 。

由式（1）可得末端位姿，即

其中， ₇⁰T 各元素值分別為

式中， s₁ 表示 sinθ₁ ： c₂ 表示 cosθ₂ ；其他類同。

1.3主臂末端空間位置誤差分析

在主機械臂工作空間內(nèi)隨機采樣20組末端位置數(shù)據(jù)，與對應的計算結(jié)果進行誤差分析。利用兩點距離公式[式（4）]求得計算位置和實際位置的差值。計算結(jié)果和實際結(jié)果如表2所示。

由表2可得， Δx 平均值為 0.320mm ，△y平均值為 -0.045mm ， Δz 平均值為 0.334mm ；由式（4）得，最大距離誤差為 0.465mm ，小于設備實際充許的最大標定誤差 2.02mm 。由此保證了機器人在運行中關節(jié)點位追蹤的準確性。

在安全性方面，為包圍盒碰撞檢測算法建立虛擬墻功能。試驗結(jié)果如圖5所示。其中，圖5（a）、圖5（b）和圖5（c）所示分別為 x 軸、 y 軸和 z 軸方向上計算值與實際值的對比，每一組中的計算值與實際值幾乎相同；圖5（d）為所取樣本及與其對應的計算結(jié)果誤差分布圖。其中，空間距離為每組采樣點位到{W}距離與計算點位到{W}距離的差值， y 軸方向計算結(jié)果范圍較大， x 和 z 軸方向較小；求得的各組空間距離誤差穩(wěn)定在 0.3～0.35mm ，驗證了計算結(jié)果的準確性。

2雙冗余機器人系統(tǒng)關節(jié)追蹤

2.1 從臂關節(jié)空間分析

由于兩套控制器各自獨立控制，無法實現(xiàn)Fol-lower相對于Leader實際位置的關節(jié)點位追蹤，設備接口功能僅能獲取機械臂末端相對于自身基坐標系的笛卡兒空間參數(shù) [x ， y ， z ， ，b， 或關節(jié)空間參數(shù) [θ₁ ， θ₂ ，…， 。因此，采用以Leader為主臂、Follower為從臂的控制策略，建立以Leader基坐標系為{W}的位置約束，以實現(xiàn)雙冗余機器人系統(tǒng)對關節(jié)點位的準確追蹤。

將Follower每個關節(jié)坐標系的齊次變換矩陣左乘 ，可得{W}下的表達式。其中， ₀^wT 為Follower基坐標系基于Leader坐標系的齊次變換矩陣。各關節(jié)空間轉(zhuǎn)化為笛卡兒空間參數(shù)的齊次變換矩陣如表3所示。

2.2雙冗余機器人關節(jié)空間參數(shù)轉(zhuǎn)化

由表1可知，軸2和軸3坐標系在空間中的位置相同；而對實際設備進行幾何分析易得，軸3所在位置處于軸2和軸4連線的中點處；同理可得軸5所在位置，如圖6所示。

Follower和Leader各關節(jié)坐標系原點分別為

式中， d_x 為圖4中Follower和Leader基坐標系的距離；x_2F 為Follower軸2在 中 x 方向上的值； y_2F 為Fol-lower軸2在 中 y 方向上的值； x_fr 為Follower末端在 中 x 方向上的值； y_fF 為Follower末端在 中 y 方向上的值； z_4L 和 z_6L 分別為Leader軸4和軸6在 中 z 方向上的值； z_fL 為Leader末端在 中 z 方向上的值；其他同理。

2.3靜態(tài)關節(jié)追蹤的實現(xiàn)

為驗證式（5）的準確性，在實際操作環(huán)境中，通過人工示教對雙冗余機器人系統(tǒng)隨機采樣12組數(shù)據(jù)P_j=[F_i ， L_i]（j=1 ，2，…，12； i=1 ，2，…，7），其中， P_j 為雙冗余機器人系統(tǒng)中所有關節(jié)的位置參數(shù)；F_i ， L_i 分別為Follower和Leader的基坐標和各關節(jié)坐標的位置 的集合。通過PyCharm環(huán)境將實際系統(tǒng)的位置虛實交互，結(jié)果如圖7所示。其中，圖7（a）與圖7（b）中的每一組結(jié)果編號一一對應。

2.4雙余機器人系統(tǒng)安全性分析

在第1.3節(jié)中，通過試驗驗證了關節(jié)位置追蹤的準確性，在此基礎上引入包圍盒算法，進一步說明關節(jié)位置追蹤對雙機器人協(xié)調(diào)運動安全性的影響。若計算平均距離誤差較大，則雙機器人系統(tǒng)中各關節(jié)位置追蹤不準確，將影響所建立包圍盒的位置精度，如圖8所示。

其中， d_x 為包圍盒球心或底面圓心在 x 方向上的期望值與實際值的差； d_y 為 y 方向上的期望值與實際值的差。

在離線編程任務中，精確的關節(jié)位置追蹤可實現(xiàn)任意機械臂在任意姿態(tài)下對關節(jié)的球包圍盒建立和對連桿的圓柱包圍盒建立。由于連桿兩端為機械臂關節(jié)，基于連桿建立的包圍盒問題可轉(zhuǎn)化為空間中求解有限線段的圓柱體函數(shù)問題。定義空間中存在 和 ，待測距離點為 ，可得空間線段距離表達式為

式中， |τ| 為空間中兩點距離； d_p1 為空間中點到線段的距離。

最終的包圍盒與雙機器人系統(tǒng)虛實結(jié)合結(jié)果如圖9所示。其中，最左側(cè)圖表示實際雙冗余機器人系統(tǒng)隨機取樣位姿；中間圖表示雙冗余機器人系統(tǒng)關節(jié)追蹤結(jié)果；最右側(cè)圖表示根據(jù)關節(jié)追蹤結(jié)果進行的包圍盒建立。最終實現(xiàn)雙機器人系統(tǒng)在任意姿態(tài)下的包圍盒建立，并有效實現(xiàn)了離線編程中的碰撞檢測。誤差范圍內(nèi)可實現(xiàn)碰撞檢測算法中包圍盒的準確建立，在雙機器人離線編程的協(xié)調(diào)運動中能夠有效避免碰撞發(fā)生，提高了雙冗余機器人系統(tǒng)的安全性。

3雙冗余機器人協(xié)調(diào)運動的關節(jié)追蹤實現(xiàn)

3.1主從交替控制策略

雙冗余機器人系統(tǒng)關節(jié)位置靜態(tài)追蹤的實現(xiàn)中采用以Leader為主臂、Follower為從臂的控制方法。對于雙冗余機器人協(xié)調(diào)運動中的關節(jié)位置追蹤，需采用主、從交替控制策略，面對協(xié)調(diào)運動任務，以Follower為主臂、Leader為從臂，定義面向協(xié)調(diào)任務的世界坐標系 位于Follower基坐標系上，實現(xiàn)Follower基于Leader末端位姿參數(shù)約束關系的驅(qū)動。

通過接口命令獲取Leader末端位姿參數(shù)（ χ_xL ， y_L ，z_L ， a_L ， ， ∣c_L? ，該參數(shù)可以求得 T_L1 旋轉(zhuǎn)算子部分；進一步可得到Leader末端可達點位在Follower坐標系中的笛卡兒空間參數(shù)，即

式中， z₀ 表示Leader末端基于坐標系 的平移算子參數(shù)。

田式（/特cauCi不田夢雙仕 一的表達式，即

式中， c_aL 表示 cos（a_L） ； c_cL 表示 cos（c_L） ； s_aL 表示sin（a_L） ； s_bL 表示 sin（b_L） ； c_bF 表示 cos（b_F） ；其他同理； [x_F ， y_F ， z_F. 表示Leader末端可達點基于 的位置參數(shù)； [a_F ， b_F ， ∣c_F∣ 表示Leader末端可達點基于

的姿態(tài)參數(shù)。

雙冗余機器人系統(tǒng)協(xié)調(diào)運動任務中實現(xiàn)關節(jié)點位追蹤的策略如圖10所示。

3.2協(xié)調(diào)運動中關節(jié)追蹤的實現(xiàn)

實際操作環(huán)境中，通過人工示教在操作空間中取3個目標點位 ， y_l ， z_l ， a_ι ， b_ι ， c_l]（i=1 ，2，3）并作為期望位姿，驅(qū)動Leader并設置時間周期為0.05s ，作采樣插補點位，通過正運動學分析獲取末端瞬時位姿的矩陣表達式合集 。同時，根據(jù)建立的以Leader為從、Follower為主的約束關系和歐拉角表示法，可獲得 各個元素值在Follower中的表達式合集 。由此得出，主、從關系的交替未影響實際設備的驅(qū)動功能，無振動出現(xiàn)。左、右臂協(xié)調(diào)運動中實現(xiàn)關節(jié)點位準確追蹤的反饋機制如圖11所示。

統(tǒng)計實際設備的測試結(jié)果，PyCharm環(huán)境下利用雙線程功能同時對雙機器人進行驅(qū)動，響應時間分別為0.2427s和0.2433s。并將所有末端點位、關節(jié)點位可視化，位置追蹤結(jié)果如圖12所示。

4結(jié)論

1）PyCharm環(huán)境中，利用機器人底層接口 1kHz 實時控制頻率，采用雙線程通信，保證了雙冗余機器人系統(tǒng)耦合運動的時效性和穩(wěn)定性，實現(xiàn)了雙冗余機器人系統(tǒng)的協(xié)同工作，提高了整體運動的協(xié)調(diào)性和效率。

2）基于Leader為主、Follower為從實現(xiàn)關節(jié)追蹤功能的反饋機制，實現(xiàn)了雙冗余機器人系統(tǒng)離線編程運動中對各關節(jié)位置及末端執(zhí)行器位姿的準確追蹤，關節(jié)位置最大誤差為 0.465mm 。

3）根據(jù)關節(jié)追蹤功能，在離線編程任務中精確建立包圍盒，實現(xiàn)了全自動追蹤，可預防碰撞事件發(fā)生，保障系統(tǒng)的安全性和穩(wěn)定性。

參考文獻

[1] SHENGJC，HEDX，GUQ，etal.Dual-arm robot fast pick-and-place ona moving conveyor[J].Journal of Physics：ConferenceSeries，2022，2216（1）：012025.

[2] ZHAOD，SUNFC，LILX.The system designand robotic manip-ulation skillsofdualarm autonomousmobilerobot forbombre-moving[J].JournalofPhysics：Conference Series，2022，2188（1）：012006.

[3] SEPULVEDAD，F(xiàn)ERNANDEZR，NAVASE，etal.Robotic au-bergine harvestingusing dual-arm manipulation[J].IEEEAccess，2020，8：121889-121904.

[4] GARABINIM，CAPORALED，TINCANIV，etal.WRAPP-up：adual-armrobot forintralogistics[J].IEEERoboticsamp;AutomationMagazine，2021，28（3）：50-66.

[5] WUQH，LIM，QIX Z，et al.Coordinated control of a dual-armrobot for surgical instrument sortingtasks[J].RoboticsandAuton-omousSystems，2019，112：1-12.

[6] 陳陽，鄭甲紅，王婧.雙機器人協(xié)同控制研究綜述[J].計算機系統(tǒng)應用，2022，31（2）：13-21.CHENYang，ZHENGJiahong，WANGJing.Reviewon dual-robotcooperative control[J]. Computer Systems and Applications，2022，31（2）：13-21.

[7] 孟石，戴先中，甘亞輝.多機器人協(xié)作系統(tǒng)軌跡約束關系分析及示教方法[J].機器人，2012，34（5）：546-552.MENG Shi，DAI Xianzhong，GAN Yahui.Path constraint relationandtrajectoryteachingmethod formulti-robotcooperationsystem[J].Robot，2012，34（5）：546-552.

[8] 王天瑞，陶平.雙機械臂協(xié)作最優(yōu)裝配位置及多目標軌跡優(yōu)化[J].機械傳動，2024，48（1）：39-45.WANG Tianrui，TAO Ping.Optimal assembly positionand multi-objective trajectoryoptimization for dual roboticarmscollabora-tion[J].JournalofMechanical Transmission，2024，48（1）：39-45.

[9] 蘇赫朋，苗鴻賓，李夢虔，等.雙機器人不同約束下的協(xié)調(diào)運動軌跡規(guī)劃方法的研究[J].制造技術與機床，2024（1）：85-92.SUHepeng，MIAO Hongbin，LI Mengqian，et al.Research onco-ordinated motion trajectory planningmethod of dual robotswithdifferent constraints[J].Manufacturing Technology amp; MachineTool，2024（1）：85-92.

[10］張振宇，鐘佩思，王祥文，等.雙工業(yè)機器人不同約束下協(xié)同運動軌跡規(guī)劃[J].制造技術與機床，2023（6）：26-32.ZHANGZhenyu，ZHONGPeisi，WANGXiangwen，etal.Collab-orativemotiontrajectoryplanningfor dual industrial robotswithdifferent constraints[J].Manufacturing Technology amp; MachineTool，2023（6）：26-32.

[11]陳友東，晏亮，谷平平.雙機器人系統(tǒng)的碰撞檢測算法[J].北京航空航天大學學報，2013，39（12）：1644-1648.CHENYoudong，YANLiang，GUPingping.Detectioncollisional-gorithm for two-manipulator system[J]. Journal ofBeijingUniver-sityofAeronauticsand Astronautics，2013，39（12）：1644-1648.

[12]從飛，張秋菊.雙機器人協(xié)調(diào)系統(tǒng)中的碰撞問題研究[J].機械傳動，2017，41（11）：13-16.CONG Fei，ZHANGQiuju.Research of the collision problem fordual robot coordinationsystem[J].Journal ofMechanical Trans-mission，2017，41（11）：13-16.

[13]劉建春，秦昆，林彥鋒，等.雙機械臂碰撞檢測算法研究[J].機械傳動，2021，45（1）：40-44.LIUJianchun，QINKun，LIN Yanfeng，etal.Research oncollisiondetectionalgorithmof dual manipulator[J].Journal ofMechanicalTransmission，2021，45（1）：40-44.

[14]SHIMIZUM，KAKUYAH，YOONWK，etal.Analytical inversekinematic computation for 7-DOF redundant manipulatorswithjoint limitsand itsapplication to redundancy resolution[J].IEEETransactions onRobotics，2008，24（5）：1131-1142.

[15]王琪，閔華松.雙臂機器人的協(xié)調(diào)控制算法綜述[J].計算機工程與應用，2021，57（1）：1-16.WANG Qi，MIN Huasong.Review of coordinated control algo-rithms for dual-arm robots[J].Computer Engineering and Applica-tions，2021，57（1）：1-16.

Research on control strategies for joint point tracking in coordinated motion of dual-redundant robots

GAO Lin ZHANG Zhiyuan WANG ShaofengSHI Yanqing YU PengweiLIANG Chaohui （SchoolofMechanicalEngineering，InnerMongolia UniversityofScienceamp;Technology，Baotou O140lo，China）

Abstract：[Objective]Withthecomplexityofoperationaltasks，thedual-redundantrobots havemoreoperationalflexibility andadaptabilitythanthesix-DOFrobotarms.Aimingatthesecurityproblems intheinformationinteractionandcoordinated motion betweenthe control systemsof dual-redundant robots，aposition-based alternate master-slavecontrol strategy was proposed fora dual-redundantrobot， with the masterarm as“Leader”andthe slave arm as“Folower\".[Methods]A positional constraintrelationwasestablishedforthemaster-slavecontrolofadual-redundantroboticsystem.Combinedwiththe joint pointtracking，real-timepositionalparameters during motion werecaptured.The mappingofeachnodepositionontheclosed chainmotionpathofthedual-redundantrobotsystemwasrealized basedonthetime interpolation.[Results]Thetestresults showthatthemaximumeror between thereal-timetracking point parametersofeachjointandtheactualnodeposition parameters of the dual-redundant robot system under arbitrary atttude is 0.465mm ，which realizes thevirtual and real interactive trajectorymapping，andprovidesacuratepositiondata fortheapplicationofcolision detectionalgorithminthe actual dual-redundant robot system.

KeyWords：Dual-redundant robot;Position tracking; Cooperative control; Master-slave control