基于動力學(xué)降噪模型的蛇形臂機(jī)器人末端軌跡跟隨算法

摘要：針對超冗余蛇形機(jī)器臂在狹小受限空間的運(yùn)動規(guī)劃問題，提出了一種基于等效多連桿動力學(xué)降噪模型的末端軌跡跟隨算法。將蛇形臂視作關(guān)節(jié)點(diǎn)約束在末端無碰撞軌跡曲線上運(yùn)動的多連桿系統(tǒng)，基于虛功原理推導(dǎo)了該剛?cè)狁詈舷到y(tǒng)的動力學(xué)方程。臂節(jié)的長度條件由連桿的軸向剛度保證，并通過模型降噪方法從模型層面濾除高頻分量影響，使常規(guī)顯式算法可用于等效多連桿系統(tǒng)方程的高效求解。采用所提算法可以指定蛇形臂末端或基座的速度規(guī)律，進(jìn)而實(shí)現(xiàn)蛇形臂沿末端軌跡的運(yùn)動規(guī)劃。由于蛇形臂關(guān)節(jié)被嚴(yán)格限制在可行性軌跡曲線上運(yùn)動，因此可以實(shí)現(xiàn)在狹小深腔環(huán)境下的避障運(yùn)動規(guī)劃。通過求解平面和空間典型數(shù)值算例可知，采用所提算法進(jìn)行蛇形機(jī)械臂運(yùn)動規(guī)劃具有足夠的計(jì)算精度和效率。

關(guān)鍵詞：蛇形臂機(jī)器人；運(yùn)動規(guī)劃；末端軌跡跟隨；模型降噪；剛?cè)狁詈?/p>

中圖分類號：TP242

Tip-trajectory Following Algorithm for Snake-arm Robots Based on Dynamics Denoising Model

ZHANG Zhigang1 JIN Yongli1 WANG Dongyin1 FU Zhijun1 QIN Guodong^2*

1.Henan Key Laboratory of Intelligent Manufacturing of Mechanical Equipment，Zhengzhou University of Light Industry，Zhengzhou，450002

2.Institute of Plasma Physics，Chinese Academy of Sciences，Hefei，230026

Abstract： Based on dynamics model smoothing formulation of equivalent multi-link system， a tip-trajectory following algorithm was proposed for motion planning of hyper-redundant SARs in narrow and confined spaces. Regarding SAR as a multi-link system of joint points constrained to move on collision-free trajectory curve of tip， and the dynamics equation of rigid-flexible coupling system was derived using virtual work principle. The length conditions of robot arm segments were guaranteed by axial stiffness of connecting rods， and the influences of high-frequency components were filtered out from the model level by employing model denoising method， so that the conventional explicit algorithm might be used to calculate the solution of equation of equivalent multi-link system efficiently. The proposed algorithm may specify the velocity law of tip or base of snake-arms， and realize the motion planning of SAR along tip-trajectory. Due to the strictly limited joint points of SAR moving on feasible trajectory curve， obstacle avoidance motion planning may be achieved in narrow and deep cavity environments. After solving typical numerical examples in planar and spatial cases， the results show that the proposed algorithm has sufficient computational accuracy and efficiency for the motion planning of SARs.

Key words： snake-arm robot（SAR）; motion planning; tip-trajectory following; model denoising; rigid-flexible coupling

0 引言

基于“仿生肌腱”設(shè)計(jì)的蛇形臂機(jī)器人（snake-arm robot，SAR）結(jié)構(gòu)質(zhì)量小、運(yùn)動靈活性高，且臂長范圍無電子元件，特別適于高危、高輻射及狹小深腔作業(yè)環(huán)境需求，是近年來機(jī)器人前沿技術(shù)領(lǐng)域的研究熱點(diǎn)之一^［1］。

蛇形臂機(jī)器人的超長串聯(lián)結(jié)構(gòu)和超冗余自由度使其具備極好的環(huán)境適應(yīng)性，實(shí)現(xiàn)狹小受限空間內(nèi)的避障運(yùn)動是其運(yùn)動規(guī)劃的核心問題。廣泛應(yīng)用于冗余機(jī)械臂逆運(yùn)動學(xué)求解的雅可比矩陣偽逆法^［2］以關(guān)節(jié)整體運(yùn)動量最小為優(yōu)化目標(biāo)，以機(jī)械臂末端運(yùn)動為約束來確定關(guān)節(jié)角，實(shí)施過程沒有考慮避障要求。在此基礎(chǔ)上提出的梯度投影法^［3］，可添加避障作為附加優(yōu)化目標(biāo)，將梯度投影到雅可比矩陣的零空間中，利用零空間的性質(zhì)，在保證跟蹤機(jī)械臂末端位姿的前提下進(jìn)行避障運(yùn)動規(guī)劃。然而對于超冗余度機(jī)器人，這類方法往往耗時(shí)過長，并且會出現(xiàn)奇異值問題而造成關(guān)節(jié)速度陡增和運(yùn)動方向的不明確。

此外，人工勢場法^［4］、模糊邏輯算法^［5］、遺傳算法^［6］及神經(jīng)網(wǎng)絡(luò)算法^［7］等也被用于冗余機(jī)器人避障規(guī)劃，但上述研究大多基于開放空間和單一或少數(shù)規(guī)則障礙物場景，對于狹小受限空間、多連續(xù)障礙，如復(fù)雜管道、深腔環(huán)境等避障路徑連續(xù)性工況，普遍存在計(jì)算量大、實(shí)時(shí)性弱、穩(wěn)定性及靈活性差等不足。

針對超冗余蛇形臂機(jī)器人在復(fù)雜受限環(huán)境下的運(yùn)動規(guī)劃問題，WILLIAMS等^［8］提出了一種末端軌跡跟隨方法（follow-the-leader approach）。該方法的核心思想是在受限空間規(guī)劃出一條無碰撞路徑軌跡的基礎(chǔ)上，控制蛇形臂末端沿該軌跡運(yùn)動，而其余機(jī)械臂關(guān)節(jié)則跟隨末端的運(yùn)動軌跡，使運(yùn)動中的蛇形臂盡可能逼近路徑曲線，從而實(shí)現(xiàn)避障目標(biāo)。

末端軌跡跟隨方法在實(shí)施上有兩個(gè)關(guān)鍵：①多障礙受限環(huán)境下的無碰撞軌跡規(guī)劃；②蛇形臂關(guān)節(jié)點(diǎn)在可行性軌跡上位置的快速確定。無碰撞軌跡可采用A*算法^［9］、蟻群算法^［10］等自動路徑規(guī)劃算法在線或離線生成，而蛇形臂沿軌跡曲線運(yùn)動時(shí)關(guān)節(jié)位置的更新是該方法的核心。為此，PALMER等^［11］以蛇形臂末端位置誤差和路徑偏移量為目標(biāo)函數(shù)，采用優(yōu)化方法對運(yùn)動關(guān)節(jié)位置參數(shù)進(jìn)行解算，但存在計(jì)算量大、末端位置跟蹤精度不高等問題。為了提高求解效率，王俊剛等^［12］提出基于迭代步長順序查找來實(shí)現(xiàn)機(jī)械臂關(guān)鍵點(diǎn)與離散路徑曲線快速匹配，并通過線性插值優(yōu)化方法減小因路徑離散處理引起的誤差。但這類方法仍僅從位置信息出發(fā)，本質(zhì)上仍是反復(fù)迭代求解關(guān)于各個(gè)關(guān)節(jié)位置的非線性方程組。

為了提高蛇形機(jī)械臂在受限空間上避障運(yùn)動規(guī)劃問題的計(jì)算效率，本文提出了一種基于動力學(xué)模型的末端軌跡跟隨運(yùn)動規(guī)劃方法，并對兩個(gè)典型算例進(jìn)行了仿真求解。

1 超冗余蛇形機(jī)械臂的運(yùn)動學(xué)描述

本文研究的蛇形機(jī)械臂由驅(qū)動基座、直線導(dǎo)軌、標(biāo)準(zhǔn)臂節(jié)、鋼絲繩纜和萬向節(jié)組成。基座可沿直導(dǎo)軌運(yùn)動，臂節(jié)間通過十字軸萬向節(jié)串聯(lián)，并通過鋼絲進(jìn)行驅(qū)動，其整體結(jié)構(gòu)如圖1所示。

為了對蛇形臂運(yùn)動學(xué)進(jìn)行描述，在各臂節(jié)質(zhì)心r^（i）c處建立連體基坐標(biāo)系［e^（i）1 e^（i）2 e^（i）3］，其中e^（i）1指向臂節(jié)長度方向，e^（i）2和e^（i）3分別沿臂節(jié)兩端萬向節(jié)轉(zhuǎn)軸方向。相鄰i-1與i號臂節(jié)間的運(yùn)動學(xué)關(guān)系如圖2所示。

臂節(jié)i姿態(tài)矩陣Ai=［e^（i）1 e^（i）2 e^（i）3］可由臂節(jié)i-1姿態(tài)矩陣Ai－1=［e^（i－1）1 e^（i－1）2

e^（i－1）3］表示為

Ai=Ai－1Ri（1）

其中轉(zhuǎn)動矩陣Ri為

Ri=cos αicos βi－sin αicos asin βicos βisin αicos αisin asin βi－sin βi0cos βi（2）

式中：αi、βi分別為萬向節(jié)依次繞轉(zhuǎn)軸e^（i）1和e^（i）2的轉(zhuǎn)角。

相鄰臂節(jié)間質(zhì)心矢徑r^（i－1）c與r^（i）c的傳遞關(guān)系可以表示為

r^（i）c=r^（i－1）c+Ai－1r-^（i－1）Q－Air-^（i）P（3）

式中：r-^（i－1）Q和r-^（i）P分別為萬向節(jié)鉸點(diǎn)在i-1和i號臂節(jié)中的局部坐標(biāo)。

2 末端無碰撞軌跡的三次樣條插值

采用末端軌跡跟隨方法進(jìn)行超冗余蛇形機(jī)械臂的運(yùn)動規(guī)劃，可先借助自動路徑規(guī)劃算法通過在線或離線方式生成無碰撞軌跡離散點(diǎn)，然后插值形成足夠光滑的末端軌跡曲線。鑒于三次樣條具有二階導(dǎo)數(shù)連續(xù)、能夠保證運(yùn)動平順性等優(yōu)勢，本文采用三次樣條插值構(gòu)造末端軌跡曲線。

將自動路徑規(guī)劃得到的m+1個(gè)離散點(diǎn)標(biāo)記為r0，r1，r2，…，rm，選用近似弧長參數(shù)s作為三次樣條插值的一組分割參數(shù)，則位于節(jié)點(diǎn)ri－1和ri之間曲線可以由Hermite插值表示為

r（s）=N^（0）1ri－1+N^（0）2ri+Li（N^（1）1ri－1，s+N^（1）2ri，s）（4）

Li=si－si－1

其中，Li為曲線段弧長；si－1和si分別為兩端節(jié)點(diǎn)處對應(yīng)弧坐標(biāo)；Hermite插值基函數(shù)為

N^（0）1=1－3ξ2+2ξ3" N^（0）2=3ξ2－2ξ3

N^（1）1=ξ－2ξ2+ξ3" N^（1）2=－ξ2+ξ3（5）

ξ=（s－si－1）/Li

式中：ξ為歸一化參數(shù)。

軌跡曲線插值式（4）中出現(xiàn)的離散點(diǎn)導(dǎo)矢ri－1，s和ri，s可由邊界條件和三次樣條在節(jié)點(diǎn)處二階導(dǎo)數(shù)連續(xù)性條件提供。如對于第一類邊界條件，曲線端部切向?qū)竢0，s和rm，s為已知，中間節(jié)點(diǎn)處滿足的二階導(dǎo)數(shù)連續(xù)性條件可以表示為

［ri－1，s ri，s ri+1，s］Li2Li（1+Ki）LiKi=

［ri－1 ri ri+1］－33（1－K2i）3（6）

Ki=Li/Li+1

式中：Ki為相鄰梁段的弧長比。

將連續(xù)性條件（式（6））與端部已知條件r0，s和rm，s統(tǒng)一記作ri，sA=ΦmB，其中ri，s為插值節(jié)點(diǎn)導(dǎo)矢矩陣，ri，s=［r0，s r1，s … rm，s］，Φm為節(jié)點(diǎn)參數(shù)矩陣，Φm=［r0 r1 …rm r0，s rm，s］，系數(shù)矩陣A和B分別為

A=L1102L1（1+K1）00L1K1Li2Li（1+Ki）LiKiLm－12Lm－1（1+Km－1）00Lm－1Km－101（7）

B=－3003（1－K21）3K21－33（1－K2i）3K2i－3003（1－K2m－1）103K2m－101（8）

由此可以解得ri，s=Φm（BA^－1）=ΦmHg，因此節(jié)點(diǎn)i的導(dǎo)矢為ri，s=ΦmHgi，其中列陣Hgi=Hg（∶，i）。根據(jù)Φm的排布，節(jié)點(diǎn)i的矢徑可以表示為ri=ΦmHri，其中Hri為定位布爾矩陣。將其代入式（4），可得軌跡曲線上弧長s處的位矢為

r（s）=ΦmN^（i）m" s∈［si－1，si］（9）

其中，插值函數(shù)矩陣N^（i）m為

N^（i）m=N^（0）1Hri－1+N^（0）2Hri+Li（N^（1）1Hgi－1+N^（1）2Hgi）（10）

3 多連桿動力學(xué)降噪模型及末端跟隨運(yùn)動規(guī)劃

3.1 等效多連桿系統(tǒng)的動力學(xué)降噪模型

蛇形臂沿末端軌跡運(yùn)動時(shí)，各關(guān)節(jié)鉸點(diǎn)被嚴(yán)格限制在可行性曲線上，其運(yùn)動學(xué)模型可等效為受軌跡曲線約束的鉸接多連桿系統(tǒng)，如圖3所示。

對于等效連桿Bi，其兩端關(guān)節(jié)鉸點(diǎn)在軌跡曲線上的弧坐標(biāo)分別為s^（i）J和s^（i+1）J。根據(jù)式（9），關(guān)節(jié)鉸點(diǎn)的矢徑分別為r^（i）J=r（s^（i）J）和r^（i+1）J=r（s^（i+1）J）。因此，連桿Bi上任一點(diǎn)矢徑可由線性插值表示為

r^（i）B（s-）=（1－ζ）r^（i）J+ζr^（i+1）J（11）

ζ=s-/LB

式中：s-為沿連桿軸向定義的物質(zhì)坐標(biāo)；ζ為歸一化坐標(biāo)；LB為等效連桿的長度，即臂節(jié)兩端萬向節(jié)鉸點(diǎn)間距離。

對式（11）兩端求導(dǎo)可得連桿上任一點(diǎn)速度

r·^（i）B（s-）=（1－ζ）r^（i）J，ss·^（i）J+ζr^（i+1）J，ss·^（i+1）J=Ν^（i）Bq·^（i）s（12）

q·^（i）s=［s·^（i）J s·^（i+1）J］T" Ν^（i）B=［（1－ζ）r^（i）J，s ζr^（i+1）J，s］

r^（i）J，s=r（s^（i）J）/s" r^（i+1）J，s=r（s^（i+1）J）/s

對式（12）再求一次導(dǎo)數(shù)可得加速度

r¨^（i）B（s-）=Ν^（i）Bq¨^（i）s+Ν·^（i）Bq·^（i）s（13）

等效連桿Bi的慣性力虛功率為

δp^（i）ine=mB∫10（δr·^（i）B）Tr¨^（i）Bdζ=

（δq·^（i）s）T（M^（i）rq¨^（i）s+F^（i）r）（14）

式中：mB為連桿質(zhì)量，質(zhì)量陣M^（i）r=mB∫10（Ν^（i）B）TΝ^（i）Bdζ，慣性力列陣F^（i）r=mB［∫10（Ν^（i）B）TΝ·^（i）Bdζ］q·^（i）s。

由柔性多連桿組成的剛?cè)狁詈舷到y(tǒng)的動力學(xué)方程表現(xiàn)明顯為剛性特征，就算采用剛性微分方程數(shù)值解法的計(jì)算效率也不高，因此，有效解決這一問題也是本文方法成功實(shí)施的關(guān)鍵。

近年來提出模型降噪方法^［13］通過將彈性內(nèi)力在一小段時(shí)間域h內(nèi)平均，進(jìn)而從模型層面濾除高頻分量影響，使顯式微分方程數(shù)值解法也能高效應(yīng)用于傳統(tǒng)剛性問題的求解。根據(jù)模型降噪方法，可將等效連桿的內(nèi)力虛功率表示為

δp^（i）str=LB∫10δε·^（i）BEA（ahε¨^（i）B+bhε·^（i）B+ε^（i）B）dζ（15）

ah=h2/6" bh=h/2

式中：E為等效連桿的彈性模量；A為等效連桿的截面積；ε^（i）B為軸向應(yīng)變；ah、bh為與降噪因子h相關(guān)的系數(shù)。

連桿Bi的軸向應(yīng)變可以表示為

ε^（i）B（s-）=ΔLB/LB=（e^（i）1）T（r^（i+1）J－r^（i）J）/LB－1（16）

其中，ΔLB為等效連桿Bi的長度增量；e^（i）1為桿長方向單位基矢量：

e^（i）1=（r^（i+1）J－r^（i）J）/‖r^（i+1）J－r^（i）J‖

對式（16）求導(dǎo)可得軸向應(yīng)變速率為

ε·^（i）B（s-）=（e^（i）1）T（r·^（i+1）J－r·^（i）J）/LB=N^（i）εq·^（i）s（17）

N^（i）ε=（1/LB）［－（e^（i）1）Tr^（i）J，s （ei1）Tr^（i+1）J，s］

對式（17）再求導(dǎo)可得軸向應(yīng)變的二階時(shí)間導(dǎo)數(shù)為

ε¨^（i）B（s-）=N^（i）εq¨^（i）s+N·^（i）εq·^（i）s（18）

將式（16）～式（18）代入式（15），可得連桿Bi的內(nèi)力虛功率

δp^（i）str=（δq·^（i）s）T［ahK^（i）q¨^（i）s+（ahK·^（i）c+

bhK^（i））q·^（i）s+F^（i）s］（19）

其中，廣義節(jié)點(diǎn)力F^（i）s=EALB∫10（N^（i）ε）Tε^（i）Bdζ，矩陣K^（i）和K·^（i）c分別為

K^（i）=EALB∫10（N^（i）ε）TN^（i）εdζ

K·^（i）c=EALB∫10（N^（i）ε）TN·^（i）εdζ（20）

等效連桿Bi的外力虛功率可以表達(dá)為

δp^（i）exp=（δq·^（i）s）TF^（i）e（21）

其中，F(xiàn)^（i）e為等效連桿Bi所受到的廣義外力列陣。

對于由n個(gè)臂節(jié)組成的蛇形機(jī)械臂，由關(guān)節(jié)鉸點(diǎn)在軌跡曲線上的弦長坐標(biāo)組成的系統(tǒng)參數(shù)列陣為qs=［s^（1）J s^（2）J … s^（n+1）J］T，則等效連桿Bi的弦長參數(shù)可以表示為q^（i）s=Tiqs，其中Ti為定位布爾矩陣。

蛇形臂等效連桿系統(tǒng)的虛功率方程可表示為

∑ni=1（δp^（i）ine+δp^（i）str－δp^（i）exp）=0

將式（14）、式（19）和式（21）代入，并由虛變分δq·s的獨(dú)立性可得

Mq¨s+Cq·s=F（22）

式中：系統(tǒng)質(zhì)量陣M=∑ni=1TTi（M^（i）r+ahK^（i））Ti；系統(tǒng)阻尼矩陣C=∑ni=1TTi（ahK·^（i）c+bhK^（i））Ti；系統(tǒng)廣義外力列陣F=∑ni=1TTi（F^（i）e－F^（i）r－F^（i）s）。

3.2 末端跟隨運(yùn)動的速度控制

蛇形臂沿末端軌跡運(yùn)動時(shí)，通常有兩種速度控制方式：①指定驅(qū)動基座沿直線導(dǎo)軌的速度；②指定蛇形臂末端沿規(guī)劃軌跡曲線的速度。

被控關(guān)節(jié)鉸點(diǎn)的弦長坐標(biāo)可以表達(dá)為時(shí)間t的已知函數(shù)，將其記作s^ctlJ（t），除此之外其余鉸點(diǎn)的弧長坐標(biāo)組集為列陣sa=qs－s^ctlJ，相應(yīng)地將式（22）進(jìn)行重新分塊表示為

MccMcaMacMaas¨^ctlJs¨a+CccCcaCacCaas·^ctlJs·a=FcFa（23）

據(jù)此可以求得

s¨a=MTaa（Fa－Macs¨^ctlJ－Cacs·^ctlJ－Caas·a）（24）

對式（24）積分便可以得到各關(guān)節(jié)沿軌跡曲線的運(yùn)動規(guī)律，得到各時(shí)刻關(guān)節(jié)鉸點(diǎn)曲線參數(shù)s^（i）J，進(jìn)而由式（9）得到各關(guān)節(jié)鉸點(diǎn)的位置矢量r^（i）J。

3.3 機(jī)械臂關(guān)節(jié)轉(zhuǎn)角的反解

在得到各時(shí)刻關(guān)節(jié)鉸點(diǎn)位置r^（i）J的基礎(chǔ)上，可以通過反解各萬向節(jié)對應(yīng)鉸坐標(biāo)αi、βi，進(jìn)而確定蛇形臂姿態(tài)。由于驅(qū)動基座只沿直線導(dǎo)軌運(yùn)動，其姿態(tài)不變，故萬向節(jié)關(guān)節(jié)角的求解可以從與驅(qū)動基座連接的1號臂節(jié)開始依次進(jìn)行。

在臂節(jié)i-1姿態(tài)（e^（i－1）1，e^（i－1）2，e^（i－1）3）已遞推求得的情況下，由圖4可知臂節(jié)i相對于臂節(jié)i-1的轉(zhuǎn)角βi滿足關(guān)系cos（π/2+βi）=e^（i）1·e^（i－1）3，因此轉(zhuǎn)角βi為

βi=arccos（e^（i）1·e^（i－1）3）－π/2（25）

記基矢量e^（i）1與e^（i－1）1的夾角為γi，根據(jù)三余弦定理有cos γi=cos αicos βi，其中cos γi=e^（i－1）1·e^（i）1。因此繞e^（i－1）3軸的轉(zhuǎn)角αi為

αi=arccos（cos γi/cos βi）（26）

按照這個(gè)流程依次遞推求解便可得到全部十字軸萬向節(jié)的轉(zhuǎn)角αi、βi，進(jìn)而確定蛇形臂姿態(tài)。

4 仿真算例

為了驗(yàn)證本文提出算法的有效性，在MATLAB中編寫了相應(yīng)程序，并采用其顯式常微分方程求解器ode45仿真求解。本節(jié)算例中蛇形臂共有12個(gè)標(biāo)準(zhǔn)臂節(jié)，單臂節(jié)長104 mm，直徑40 mm，驅(qū)動基座最大進(jìn)給距離1248 mm。為了保證臂節(jié)長度條件，設(shè)等效連桿系統(tǒng)的連桿抗拉剛度EA=1×109 N。本節(jié)算例的仿真計(jì)算均在CPU為Intel Core i7-12650H、內(nèi)存為16GB的筆記本電腦上完成。

4.1 平面軌跡跟隨

在平面問題算例中，蛇形臂機(jī)器人需要穿過特定間隙并繞過直徑為160 mm的圓形障礙。據(jù)此任務(wù)，規(guī)劃出了直徑為200 mm的圓弧形可行性路徑曲線，如圖5中黑色曲線所示。

不失一般性，這里設(shè)置驅(qū)動基座沿直線導(dǎo)軌的運(yùn)動規(guī)律為

s·^ctlJ（t）=0.5v［1+sin（（t+3/2））π］" 0≤t≤1

v1lt;t≤9

0.5v［1+sin（（t－17/2））π］9lt;t≤10（27）

其中，勻速運(yùn)動階段v=0.12 m/s。采用本文算法仿真得到的蛇形臂進(jìn)行平面軌跡跟隨運(yùn)動的結(jié)果如圖5所示。

在用ode45進(jìn)行計(jì)算求解時(shí)，設(shè)定仿真時(shí)間為10 s，絕對誤差限為εa=1.0×10^-6，相對誤差限為εr=1.0×10^-3。根據(jù)文獻(xiàn)［14］推薦，對于蛇形臂這類剛度極大問題，取模型降噪因子h=0.1，采用MATLAB非剛性求解器ode45僅需3.19 s就可以完成仿真計(jì)算。相比之下，當(dāng)取h=0（即不進(jìn)行模型降噪），由于該系統(tǒng)方程剛性問題突出，導(dǎo)致非剛性常微分方程求解器ode45無法完成仿真計(jì)算。即便采用基于隱式算法的MATLAB剛性方程求解器ode15s，完成10 s仿真所耗費(fèi)機(jī)時(shí)也達(dá)到了823.88 s。這也進(jìn)一步證明了本文提出算法具有較高的計(jì)算效率。圖6給出了蛇形臂第3、6、9及12號萬向節(jié)關(guān)節(jié)轉(zhuǎn)角隨時(shí)間的變化曲線。

將求得的轉(zhuǎn)角代入正向運(yùn)動學(xué)模型可解得蛇形臂末端位置，圖7給出了末端位置與期望路徑曲線的偏差距離。分析可知，蛇形臂末端的最大偏差小于0.005 mm，具有非常高的跟蹤精度。

圖8對比了驅(qū)動基座與蛇形臂末端速度，從中可以看出，兩者存在明顯差異。這是由于當(dāng)蛇形臂進(jìn)入曲線段運(yùn)動時(shí)，臂節(jié)會產(chǎn)生角速度，臂節(jié)上兩端鉸點(diǎn)必然產(chǎn)生由角速度不為零帶來的速度差。因此，要實(shí)現(xiàn)對蛇形臂末端運(yùn)動速度的進(jìn)一步控制，就需要對所采用方法有更嚴(yán)苛要求。本文所提方法可以輕易實(shí)現(xiàn)對基座或末端的速度控制，這一點(diǎn)將在接下來算例中進(jìn)一步說明。

4.2 空間軌跡跟隨

在空間問題算例中，模擬蛇形臂穿過飛機(jī)機(jī)翼油箱內(nèi)部狹小空腔進(jìn)行檢修任務(wù)。這里油箱的整體尺寸為1200 mm×800 mm×200 mm，并由隔板將其分隔為四個(gè)儲油倉，如圖9所示。

在這個(gè)算例中，指定蛇形臂末端速度滿足式（27）的運(yùn)動規(guī)律。仿真時(shí)間仍為10 s，同樣采用非剛性求解器ode45，誤差限設(shè)置與上一個(gè)算例相同。同樣設(shè)置磨光因子h=0.1，在相同配置臺式機(jī)上完成計(jì)算僅需耗時(shí)2.85 s，表明本文方法具有極高的計(jì)算效率。圖9給出了采用本文算法仿真得到的蛇形臂進(jìn)入機(jī)翼油箱進(jìn)行三維空間軌跡跟隨運(yùn)動的情形。

圖10給出了蛇形臂驅(qū)動基座與末端的速度對比。利用基座驅(qū)動規(guī)律便可以實(shí)現(xiàn)蛇形臂末端速度大小的精準(zhǔn)控制。這也是本文方法相較于現(xiàn)有末端軌跡跟隨算法所具有的獨(dú)特優(yōu)勢。

圖11給出了蛇形臂沿規(guī)劃路徑進(jìn)入機(jī)翼油箱狹小空間過程中，第3、6、9和12號萬向節(jié)關(guān)節(jié)轉(zhuǎn)角隨時(shí)間的變化曲線。

圖12給出了蛇形臂末端位置與期望路徑曲線的偏差距離，從中可知在進(jìn)行空間軌跡跟蹤過程中最大偏差小于0.018 mm，這再次證明了本文方法具有很高的跟蹤精度。

5 結(jié)論

基于等效多連桿動力學(xué)降噪模型提出了一種適用于超冗余蛇形臂機(jī)器人狹小受限環(huán)境運(yùn)動規(guī)劃的末端跟隨運(yùn)動規(guī)劃算法。

首先，利用路徑規(guī)劃算法得到無碰撞軌跡離散點(diǎn)，并采用三次樣條插值得到滿足連續(xù)性和光滑性要求的蛇形臂末端軌跡曲線。其次，將蛇形臂機(jī)器人視作關(guān)節(jié)鉸點(diǎn)沿末端軌跡曲線運(yùn)動的等效多連桿，利用虛功原理推導(dǎo)了該剛?cè)狁詈舷到y(tǒng)的動力學(xué)方程。為了降低剛?cè)狁詈舷到y(tǒng)方程求解的難度，采用模型降噪方法從模型層面濾除了高頻分量，使得常規(guī)顯式算法可以高效求解系統(tǒng)動力學(xué)方程。最后，求解了蛇形臂機(jī)器人在平面和空間狹小空間上的運(yùn)動規(guī)劃問題典型算例，驗(yàn)證了所提方法的精度和效率。

參考文獻(xiàn)：

［1］ HUANG Long， LIU Bei， ZHANG Leiyu， et al. Equilibrium Conformation of a Novel Cable-driven Snake-arm Robot under External Loads［J］. Micromachines， 2022， 13（7）：1149.

［2］ JAIN T， JAIN J K， ROY D. Joint Space Redundancy Resolution of Serial Link Manipulator：an Inverse Kinematics and Continuum Structure Numerical Approach［J］. Materials Today：Proceedings， 2021， 38：423-431.

［3］ 李亞昕， 王國磊， 張劍輝， 等. 基于碰撞反饋的冗余機(jī)器人避障規(guī)劃算法［J］. 清華大學(xué)學(xué)報(bào)（自然科學(xué)版）， 2022， 62（3）：408-415.

LI Yaxin， WANG Guolei， ZHANG Jianhui， et al. Obstacle Avoidance Algorithm for Redundant Robots Based on Collision Feedback［J］. Journal of Tsinghua University（Science and Technology）， 2022， 62（3）：408-415.

［4］ 萬俊， 孫薇， 葛敏， 等. 基于含避障角人工勢場法的機(jī)器人路徑規(guī)劃［J］. 農(nóng)業(yè)機(jī)械學(xué)報(bào)， 2024， 55（1）：409-418.

WAN Jun， SUN Wei， GE Min， et al. Robot Path Planning Based on Artificial Potential Field Method with Obstacle Avoidance Angles［J］. Transactions of the Chinese Society for Agricultural Machinery， 2024， 55（1）：409-418.

［5］ YAN Fei， WANG Yaoyao， JU Feng， et al. Adaptive Time Delay Control for Cable-driven Manipulators Using Fuzzy Logic Algorithm［J］. Proceedings of the Institution of Mechanical Engineers， Part I：Journal of Systems and Control Engineering， 2021， 235（6）：898-913.

［6］ 馬宇豪， 梁雁冰. 一種基于六次多項(xiàng)式軌跡規(guī)劃的機(jī)械臂避障算法［J］. 西北工業(yè)大學(xué)學(xué)報(bào)， 2020， 38（2）：392-400.

MA Yuhao， LIANG Yanbing. An Obstacle Avoidance Algorithm for Manipulators Based on Six-order Polynomial Trajectory Planning［J］. Journal of Northwestern Polytechnical University， 2020， 38（2）：392-400.

［7］ ZHANG Zhijun， ZHENG Lunan， CHEN Zhuo-ming， et al. Mutual-collision-avoidance Scheme Synthesized by Neural Networks for Dual Redundant Robot Manipulators Executing Cooperative Tasks［J］. IEEE Transactions on Neural Networks and Learning Systems， 2021， 32（3）：1052-1066.

［8］ WILLIAMS R L， TAMASI G. Follow-the-leader Control for the Payload Inspection and Processing System［C］∥Proceedings of ASME 1996 Design Engineering Technical Conferences and Computers in Engineering Conference. Irvine， 1996：V02BT02A057.

［9］ DAI Xiaolin， LONG Shuai， ZHANG Zhiwen， et al. Mobile Robot Path Planning Based on Ant Colony Algorithm with A* Heuristic Method［J］. Frontiers in Neurorobotics， 2019， 13：15.

［10］ LIU Yiyang， HOU Zheng， TAN Yuanyuan， et al. Research on Multi-AGVs Path Planning and Coordination Mechanism［J］. IEEE Access， 2020， 8：213345-213356.

［11］ PALMER D， COBOS-GUZMAN S， AXINTE D. Real-time Method for Tip Following Navigation of Continuum Snake Arm Robots［J］. Robotics and Autonomous Systems， 2014， 62（10）：1478-1485.

［12］ 王俊剛， 湯磊， 谷國迎， 等. 超冗余度機(jī)械臂跟隨末端軌跡運(yùn)動算法及其性能分析［J］. 機(jī)械工程學(xué)報(bào)， 2018， 54（3）：18-25.

WANG Jungang， TANG Lei， GU Guoying， et al. Tip-following Path Planning and Its Performance Analysis for Hyper-redundant Manipulators［J］. Journal of Mechanical Engineering， 2018， 54（3）：18-25.

［13］ 張志剛， 周翔， 毛紅生， 等. 絕對節(jié)點(diǎn)坐標(biāo)方法的模型降噪研究［J］. 振動與沖擊， 2021， 40（11）：139-146.

ZHANG Zhigang， ZHOU Xiang， MAO Hongsheng， et al. Model Denoising Based on Absolute Node Coordinate Method［J］. Journal of Vibration and Shock， 2021， 40（11）：139-146.

［14］ 齊朝暉， 曹艷， 王剛. 多柔體系統(tǒng)數(shù)值分析的模型降噪方法［J］. 力學(xué)學(xué)報(bào)， 2018， 50（4）：863-870.

QI Zhaohui， CAO Yan， WANG Gang. Model Smoothing Methods in Numerical Analysis of Flexible Multibody Systems［J］. Chinese Journal of Theoretical and Applied Mechanics， 2018， 50（4）：863-870.

（編輯 王旻玥）

基金項(xiàng)目：國家自然科學(xué)基金（12305251）；河南省科技攻關(guān)項(xiàng)目（232102221036，242102241049）；河南省外籍科學(xué)家工作室項(xiàng)目（GZS2023011）

作者簡介：

張志剛，男，1984年生，博士、副教授。研究方向?yàn)槎囿w系統(tǒng)動力學(xué)與控制。E-mail：zhigangzhang@foxmail.com。

秦國棟*（通信作者），男，1991年生，博士、助理研究員。研究方向?yàn)榫圩兌堰b操作維護(hù)機(jī)器人系統(tǒng)設(shè)計(jì)。E-mail：gdqin@ipp.ac.cn。

本文引用格式：

張志剛，靳勇利，王棟銀，等.基于動力學(xué)降噪模型的蛇形臂機(jī)器人末端軌跡跟隨算法［J］. 中國機(jī)械工程，2025，36（3）：407-413.

ZHANG Zhigang， JIN Yongli， WANG Dongyin， et al. Tip-trajectory Following Algorithm for Snake-arm Robots Based on Dynamics Denoising Model［J］. China Mechanical Engineering， 2025， 36（3）：407-413.