摘 要：蛇形機(jī)器人模仿蛇的身體結(jié)構(gòu)與運(yùn)動(dòng)機(jī)理，具有體積小、自由度多等特點(diǎn)，在復(fù)雜環(huán)境中表現(xiàn)出優(yōu)秀的運(yùn)動(dòng)性能。設(shè)計(jì)一種適用于狹小管道環(huán)境的蛇形機(jī)器人，采用模塊化的構(gòu)型，機(jī)器人共擁有5個(gè)自由度。利用Denavit-Hartenberg方法對(duì)機(jī)器人進(jìn)行運(yùn)動(dòng)學(xué)分析，基于從機(jī)器人尾部到頭部的前向軌跡跟蹤控制算法，建立機(jī)器人的軌跡跟蹤運(yùn)動(dòng)控制方法；設(shè)計(jì)實(shí)驗(yàn)樣機(jī)并開展運(yùn)動(dòng)控制實(shí)驗(yàn)和實(shí)際管道轉(zhuǎn)彎實(shí)驗(yàn)，驗(yàn)證了機(jī)器人結(jié)構(gòu)設(shè)計(jì)的合理性、逆運(yùn)動(dòng)學(xué)的正確性和運(yùn)動(dòng)控制方法的有效性。

關(guān)鍵詞：蛇形機(jī)器人；軌跡跟蹤；運(yùn)動(dòng)學(xué)；管道環(huán)境應(yīng)用

中圖分類號(hào)：TP242 文獻(xiàn)標(biāo)志碼：B 文章編號(hào)：1671-5276（2024）04-0201-07

Design of Snake Robot in Pipeline and Implementation of Trajectory Tracking Algorithm

YU Yangguang， JI Aihong， QIU Jiahui， LIU Yahong， QIN Guodong

（College of Mechanical and Electrical Engineering， Nanjing University of Aeronautics and Astronautics， Nanjing 210016， China）

Abstract：Snake robot， imitating the body structure and motion mechanism of snake， small in size and multiple with degrees of freedom， displays excellent motion performance in complex environment. A snake robot suitable for narrow pipe environment was proposed. With modular configuration， the proposed robot had 5 degrees of freedom， and kinematics analysis was conducted by Denavit-Hartenberg method. Based on the forward trajectory tracking control algorithm from the tail to the head of the robot， the trajectory tracking motion control method of the robot was established. The experimental prototype was designed and the experiments on motion control and actual pipe turning were carried out， which verified the rationality of the robot structure design， the correctness of the inverse kinematics and the effectiveness of the motion control method.

Keywords：snake robot; trajectory tracking; kinematics; pipeline application

0 引言

自然界中，蛇具有優(yōu)異的運(yùn)動(dòng)能力，這取決于其獨(dú)特的身體結(jié)構(gòu)與運(yùn)動(dòng)方式。蛇有130～500個(gè)脊椎骨，每?jī)晒?jié)脊椎骨通過一個(gè)球窩結(jié)構(gòu)連接在一起，雖然兩個(gè)脊椎骨之間繞垂直軸的相對(duì)旋轉(zhuǎn)運(yùn)動(dòng)為10°～20°，繞水平軸的相對(duì)旋轉(zhuǎn)運(yùn)動(dòng)僅有幾度，但蛇的靈活性正是由這些脊椎骨的微小運(yùn)動(dòng)總和而產(chǎn)生^[1]。這種機(jī)械式的互聯(lián)為仿生蛇形機(jī)器人帶來(lái)了設(shè)計(jì)靈感。

蛇形機(jī)器人^[2]以蛇類脊柱為模仿對(duì)象，相鄰關(guān)節(jié)間可以彎曲一定角度，通常由若干相同的模塊通過多自由度的關(guān)節(jié)串聯(lián)而成^[3]。因其具有體積小、運(yùn)動(dòng)靈活、環(huán)境適應(yīng)性強(qiáng)等特點(diǎn)，被廣泛應(yīng)用于設(shè)備維護(hù)、災(zāi)害救援等領(lǐng)域。蛇形機(jī)器人在結(jié)構(gòu)上可分為三類：帶有從動(dòng)輪的蛇形機(jī)器人、不帶從動(dòng)輪的蛇形機(jī)器人和配備有主動(dòng)推進(jìn)裝置的蛇形機(jī)器人。前兩類無(wú)主動(dòng)推進(jìn)裝置的蛇形機(jī)器人，需要像真正的蛇一樣在橫向波動(dòng)中移動(dòng)，依靠自身扭動(dòng)產(chǎn)生的驅(qū)動(dòng)力進(jìn)行運(yùn)動(dòng)，限制多，速度慢。相比之下，具有主動(dòng)推進(jìn)裝置的蛇形機(jī)器人可以在每個(gè)節(jié)段中產(chǎn)生推進(jìn)力，即使在沒有橫向波動(dòng)的情況下也能移動(dòng)。

由于自由度較多，蛇形機(jī)器人的控制方法比較復(fù)雜。常用的運(yùn)動(dòng)控制方法主要有基于蛇形曲線的控制方法、基于數(shù)學(xué)模型的控制方法和基于CPG（central pattern generators）的控制方法等^[4-5]?；谏咝吻€的控制方法運(yùn)算簡(jiǎn)便，但不利于傳感信息的引入；基于數(shù)學(xué)模型的控制方法可以實(shí)現(xiàn)對(duì)蛇形機(jī)器人的精確控制，但建模過程比較復(fù)雜；基于CPG的控制方法，可以在缺乏高層調(diào)節(jié)和外部反饋的情況下自動(dòng)產(chǎn)生穩(wěn)定的振蕩行為，但僅適用于機(jī)器人的節(jié)律性運(yùn)動(dòng)控制。

針對(duì)蛇形機(jī)器人的運(yùn)動(dòng)控制，OWEN^[6]基于大量的生物蛇實(shí)驗(yàn)推導(dǎo)出了蜿蜒運(yùn)動(dòng)軌跡曲線（Serpenoid曲線）用于蛇形機(jī)器人的運(yùn)動(dòng)控制。ARIIZUMI等^[7]基于蛇形機(jī)器人運(yùn)動(dòng)有側(cè)滑約束條件建立了蛇形機(jī)器人的運(yùn)動(dòng)學(xué)模型，基于該運(yùn)動(dòng)學(xué)模型在機(jī)器人頭部額外添加一個(gè)剛性連桿，用于控制蛇形機(jī)器人的運(yùn)動(dòng)方向。LILJEB?CK等^[8]提出應(yīng)用虛設(shè)的機(jī)構(gòu)VSOP（virtual structure for orientation and position）將蛇形機(jī)器人等效為機(jī)械臂進(jìn)行建模研究。CHO等^[9]提出頂尖隨動(dòng)算法，直接控制機(jī)器人的頭部單元，后續(xù)的主體單元跟蹤頭部單元的運(yùn)動(dòng)。這種后級(jí)單元的運(yùn)動(dòng)跟隨前級(jí)變化的移位控制方法在其他機(jī)器人上也有應(yīng)用^[10]，但此方法根據(jù)機(jī)器人頭部的運(yùn)動(dòng)來(lái)確定后級(jí)各單元的運(yùn)動(dòng)，很難直接改變機(jī)器人頭部以外部分的運(yùn)動(dòng)。唐超權(quán)等^[11]利用Cohen神經(jīng)元模型，為蛇形機(jī)器人的運(yùn)動(dòng)控制設(shè)計(jì)了多模態(tài)中樞模式發(fā)生器模型，用來(lái)產(chǎn)生機(jī)器人運(yùn)動(dòng)的多種步態(tài)信號(hào)。

蛇形機(jī)器人優(yōu)秀的運(yùn)動(dòng)靈活性與環(huán)境適應(yīng)性，賦予了其應(yīng)用于狹小管道環(huán)境的可能。在電力、化工等行業(yè)存在大量管徑狹小、環(huán)境復(fù)雜的管道，此類管道在極端環(huán)境下易于損壞，需要機(jī)器人在管道內(nèi)部執(zhí)行檢測(cè)、維修等操作^[12]。為了滿足狹小管道環(huán)境對(duì)蛇形機(jī)器人尺寸和運(yùn)動(dòng)能力的實(shí)際需求，本文提出了一種適用于管道內(nèi)部的蛇形機(jī)器人，采用機(jī)器人關(guān)節(jié)與行進(jìn)機(jī)構(gòu)配合的構(gòu)型，保證了復(fù)雜環(huán)境下機(jī)器人的運(yùn)動(dòng)能力；采用從機(jī)器人尾部到頭部的前向軌跡跟蹤算法，實(shí)現(xiàn)了機(jī)器人的軌跡跟蹤運(yùn)動(dòng)控制，并搭建機(jī)器人樣機(jī)完成實(shí)驗(yàn)，驗(yàn)證了機(jī)器人結(jié)構(gòu)設(shè)計(jì)的合理性和運(yùn)動(dòng)控制方法的有效性。

1 蛇形機(jī)器人

1.1 結(jié)構(gòu)設(shè)計(jì)

狹小管道環(huán)境需要特種機(jī)器人具備小型化、輕量化、多自由度的特點(diǎn)以及靈活的運(yùn)動(dòng)能力。針對(duì)直徑在80mm～100mm的厘米級(jí)管道，本文提出了一種蛇形機(jī)器人構(gòu)型以適用于狹小管道環(huán)境下的運(yùn)動(dòng)和相關(guān)任務(wù)執(zhí)行。設(shè)計(jì)需求歸納如下：

1）具備主動(dòng)運(yùn)動(dòng)能力，運(yùn)動(dòng)靈活性高，便于控制；

2）結(jié)構(gòu)尺寸小，管徑適應(yīng)性強(qiáng)，對(duì)管徑尺寸要求??；

3）對(duì)彎管、T型管等復(fù)雜管道環(huán)境適應(yīng)性強(qiáng)；

4）可搭載圖像采集傳輸模塊等執(zhí)行器，滿足管道內(nèi)檢測(cè)需求。

機(jī)器人整體方案設(shè)計(jì)對(duì)機(jī)器人的運(yùn)動(dòng)性能起決定作用，主要包括運(yùn)動(dòng)方式、驅(qū)動(dòng)方式和關(guān)節(jié)連接方式等。綜合分析設(shè)計(jì)需求與不同運(yùn)動(dòng)方式的機(jī)器人，本文選取的機(jī)器人形式為配備有主動(dòng)驅(qū)動(dòng)裝置的蛇形機(jī)器人，機(jī)器人為多關(guān)節(jié)結(jié)構(gòu)，相鄰關(guān)節(jié)間可以彎曲一定角度，擁有更多的自由度，更有利于機(jī)器人在狹小管道內(nèi)的靈活運(yùn)動(dòng)。電機(jī)直接驅(qū)動(dòng)執(zhí)行機(jī)構(gòu)的方式占用空間小，有利于降低機(jī)器人整體尺寸，故機(jī)器人主動(dòng)推進(jìn)裝置選擇為直流電機(jī)直接驅(qū)動(dòng)的主動(dòng)輪結(jié)構(gòu)，輪式結(jié)構(gòu)運(yùn)動(dòng)速度快，結(jié)構(gòu)簡(jiǎn)單，同時(shí)保證了較大的驅(qū)動(dòng)力。機(jī)器人關(guān)節(jié)連接方式選擇為正交連接，該連接方式具有可靠性高、結(jié)構(gòu)工藝性好、控制簡(jiǎn)單等優(yōu)點(diǎn)，機(jī)器人各關(guān)節(jié)采用舵機(jī)直接驅(qū)動(dòng)的形式，實(shí)現(xiàn)俯仰或偏轉(zhuǎn)自由度。

根據(jù)上述整體設(shè)計(jì)方案，蛇形機(jī)器人采用三模塊的結(jié)構(gòu)（圖1（a）），單個(gè)模塊如圖1（b）所示，包含機(jī)器人關(guān)節(jié)和行進(jìn)機(jī)構(gòu)兩部分。機(jī)器人關(guān)節(jié)與行進(jìn)機(jī)構(gòu)配合的構(gòu)型可以使機(jī)器人兼具較多的自由度和較高的運(yùn)動(dòng)速度。機(jī)器人關(guān)節(jié)由兩個(gè)呈正交布置的伺服電機(jī)組成，類似于“十字結(jié)構(gòu)”，具備偏轉(zhuǎn)、俯仰兩個(gè)自由度，能在水平或垂直平面內(nèi)轉(zhuǎn)動(dòng)，轉(zhuǎn)動(dòng)角度范圍為（-90°，90°）。行進(jìn)機(jī)構(gòu)包括直流電機(jī)、支架與主動(dòng)輪。支架兩端伸出懸臂用于連接關(guān)節(jié)部分。

1.2 彎管通過性設(shè)計(jì)

機(jī)器人的尺寸是決定其在管道內(nèi)通過性的最關(guān)鍵因素，在設(shè)計(jì)中應(yīng)根據(jù)管道形狀及尺寸判斷機(jī)器人的尺寸設(shè)計(jì)區(qū)間以保證機(jī)器人的通過性。

直管對(duì)機(jī)器人的尺寸要求較低，機(jī)器人寬和高不超過管道內(nèi)徑即可。

彎管對(duì)機(jī)器人的尺寸要求較高，機(jī)器人能否順利通過彎管主要由管道曲率及機(jī)器人尺寸決定。彎管主要參數(shù)如下：曲率半徑R、管道內(nèi)徑D、直角彎管彎曲角度γ=90°。機(jī)器人不具備彎管通過性一般是由于機(jī)器人過于細(xì)長(zhǎng)或過于短粗兩種情況造成的，極端位置處于彎管中機(jī)器人傾斜45°處。

將彎管建模為如圖2所示的圓柱形管段，機(jī)器人單個(gè)模塊簡(jiǎn)化為圓柱形結(jié)構(gòu)，長(zhǎng)度為L(zhǎng)，直徑為d。根據(jù)圖2可以求出管道內(nèi)徑、曲率半徑和機(jī)器人尺寸之間的關(guān)系。

當(dāng)機(jī)器人過于細(xì)長(zhǎng)，即機(jī)器人簡(jiǎn)化模型的長(zhǎng)徑比L/d較大時(shí)，機(jī)器人前后端面位于管道的直邊部分。機(jī)器人與彎管的尺寸滿足0lt;d≤（R+D/2）sin45°－（R－D/2），此時(shí)通過幾何關(guān)系計(jì)算機(jī)器人長(zhǎng)度和直徑的關(guān)系如下：

當(dāng)機(jī)器人過于短粗，即機(jī)器人簡(jiǎn)化模型的長(zhǎng)徑比L/d較小時(shí)，機(jī)器人前后端面位于管道彎曲部分。機(jī)器人與彎管的尺寸滿足（R+D/2）sin45°－（R－D/2）lt;d≤D，此時(shí)機(jī)器人長(zhǎng)度和直徑的關(guān)系如下：

本文所設(shè)計(jì)機(jī)器人針對(duì)的管道尺寸參數(shù)為：R=65mm，D=100mm。在此彎管環(huán)境下，機(jī)器人為保證通過性應(yīng)滿足以下條件。

當(dāng)機(jī)器人簡(jiǎn)化模型的直徑滿足0lt;d≤66.3mm時(shí)，長(zhǎng)度計(jì)算公式如下：

當(dāng)滿足66.3mmlt;d≤100mm時(shí)，長(zhǎng)度計(jì)算公式如下：

前文將機(jī)器人模型簡(jiǎn)化為圓柱形結(jié)構(gòu)，實(shí)際中機(jī)器人結(jié)構(gòu)多樣，不局限于圓柱形結(jié)構(gòu)。如果機(jī)器人為矩形體結(jié)構(gòu)，那么機(jī)器人中心所處平面不位于管道軸心平面，即機(jī)器人未必與管道同軸。另外機(jī)器人運(yùn)動(dòng)卡死時(shí)還可能發(fā)生跳動(dòng)，造成機(jī)器人所處平面的改變以擴(kuò)大運(yùn)動(dòng)空間通過管道。本文在機(jī)器人設(shè)計(jì)中對(duì)其尺寸進(jìn)行了更強(qiáng)的約束，若機(jī)器人為矩形體結(jié)構(gòu)，其寬高構(gòu)成的斜邊尺寸應(yīng)小于圓柱形結(jié)構(gòu)的直徑d，如圖3所示。

本文設(shè)計(jì)機(jī)器人單個(gè)模塊的長(zhǎng)寬高分別為L(zhǎng)=115mm，a=50mm，h=50mm，代入公式計(jì)算如下：

機(jī)器人在寬高已定的情況下，模塊的長(zhǎng)度可取范圍為（0，152.7）。設(shè)計(jì)中L=115mm，故機(jī)器人的尺寸設(shè)計(jì)在彎管通過性方面是合理的。

2 運(yùn)動(dòng)控制方法

2.1 軌跡跟蹤算法

軌跡跟蹤是蛇形機(jī)器人進(jìn)入狹窄工作空間的常用方法之一，其目的是控制機(jī)器人各個(gè)關(guān)節(jié)的轉(zhuǎn)動(dòng)角度、角速度等變量使機(jī)器人能對(duì)一條給定的軌跡進(jìn)行跟蹤。機(jī)器人軌跡跟蹤算法的關(guān)鍵是以一定的方式不斷更新多關(guān)節(jié)機(jī)器人的空間位置，不能將移動(dòng)機(jī)器人整體視作一個(gè)點(diǎn)，也不能只關(guān)注機(jī)器人頭部位置的擬合，而是要對(duì)機(jī)器人整體進(jìn)行每個(gè)關(guān)節(jié)的軌跡跟蹤。具有主動(dòng)推進(jìn)裝置的蛇形機(jī)器人通常采用移位控制，根據(jù)機(jī)器人頭部的移動(dòng)或運(yùn)動(dòng)學(xué)來(lái)確定后級(jí)各單元的運(yùn)動(dòng)，很難直接改變機(jī)器人頭部以外部分的運(yùn)動(dòng)。而傳統(tǒng)的蛇形臂等超冗余度機(jī)器人采用的軌跡跟蹤過程主要有兩種：一種為已知機(jī)器人末端關(guān)節(jié)的位置或姿態(tài)，然后根據(jù)給定軌跡反向求解其余關(guān)節(jié)的關(guān)節(jié)變量，完成逆運(yùn)動(dòng)學(xué)求解；另一種為驅(qū)動(dòng)箱進(jìn)給運(yùn)動(dòng)，正向依次擬合各個(gè)關(guān)節(jié)到給定軌跡上求解關(guān)節(jié)變量，完成逆運(yùn)動(dòng)學(xué)求解。受此啟發(fā)，本文針對(duì)所設(shè)計(jì)的蛇形機(jī)器人，將機(jī)器人各關(guān)節(jié)以一定速度沿連桿切向方向進(jìn)給，從機(jī)器人尾部開始，依次將每個(gè)關(guān)節(jié)擬合到跟蹤曲線，使機(jī)器人的軌跡跟蹤簡(jiǎn)化為各機(jī)器人連桿末端對(duì)任務(wù)曲線離散點(diǎn)的連續(xù)跟蹤擬合的過程，最終求解關(guān)節(jié)變量以完成逆運(yùn)動(dòng)學(xué)求解。

本文采用跟蹤算法的原理如圖4所示。圖中粗線部分表示機(jī)器人的一個(gè)簡(jiǎn)化連桿，初始時(shí)刻連桿首尾兩端位置為W^j－1₀，W^j₀，機(jī)器人以一定速度運(yùn)動(dòng)。假設(shè)連桿的首尾兩端在t時(shí)刻移動(dòng)到W^j－1_i－1，W^j_i－1處，在下一時(shí)刻t+Δt時(shí)，連桿尾端更新至W^j－1_i位置。通過桿長(zhǎng)l可以確定連桿首端W^j_i的位置區(qū)間，連桿的位置更新為W^j－1_i，W^j_i。

如圖4中的局部放大圖所示，連桿首端W ^j_i移動(dòng)至離散曲線上P_i和P_i+1的工作區(qū)域中，在極限小距離內(nèi)曲線可以近似由直線代替，通過兩點(diǎn)確定一條直線，可以獲得在P_i（x_i，y_i，z_i）和P_i+1（x_i+1，y_i+1，z_i+1）處的線性方程：

W^j_i在基坐標(biāo)系中的位置坐標(biāo)可以通過機(jī)器人的正運(yùn)動(dòng)學(xué)方程獲得。本文設(shè)計(jì)的蛇形機(jī)器人每個(gè)相鄰關(guān)節(jié)的運(yùn)動(dòng)方向均保持正交，且機(jī)器人為多模塊結(jié)構(gòu)，相鄰模塊間的坐標(biāo)變換規(guī)律相同。本文利用D-H分析法（denavit-hartenberg method）^[13]，完成機(jī)器人的運(yùn)動(dòng)學(xué)建模，建立如圖5所示的坐標(biāo)系，D-H參數(shù)如表1所示。

蛇形機(jī)器人相鄰模塊之間的齊次變換矩陣相同，以其中一個(gè)模塊為例，從坐標(biāo)系{4}到坐標(biāo)系{1}的齊次變換矩陣為：

式中：l_i（i=1，2，3）是連桿長(zhǎng)度；θ₂、θ₃是關(guān)節(jié)轉(zhuǎn)動(dòng)角度。

可以得到相鄰模塊之間的齊次變換矩陣，基于此機(jī)器人各模塊的位姿通過逐次變換也可以準(zhǔn)確地描述出來(lái)。

通過線性方程和機(jī)器人的運(yùn)動(dòng)學(xué)方程，聯(lián)立式（7）、式（12）可以得到：

該方程包含3個(gè)未知量α_i，β_i，k，存在唯一的解，即蛇形機(jī)器人的逆運(yùn)動(dòng)學(xué)解。蛇形機(jī)器人的軌跡跟蹤就是通過聯(lián)立過P_i，P_i+1的直線方程和機(jī)器人的正運(yùn)動(dòng)學(xué)方程，得到逆運(yùn)動(dòng)學(xué)解α_i，β_i和相應(yīng)的坐標(biāo)位置x_i，y_i，z_i來(lái)完成。軌跡跟蹤算法求解流程如圖6所示。

2.2 仿真實(shí)驗(yàn)

本文首先通過仿真驗(yàn)證了軌跡跟蹤算法的有效性，針對(duì)機(jī)器人實(shí)際應(yīng)用的管道環(huán)境存在彎管，在x-y平面設(shè)計(jì)了轉(zhuǎn)彎曲線作為目標(biāo)軌跡，曲線參數(shù)的設(shè)定取決于實(shí)際彎管的尺寸參數(shù)。

仿真中的蛇形機(jī)器人結(jié)構(gòu)如圖1所示，擁有3個(gè)模塊，5個(gè)自由度。機(jī)器人設(shè)置為以恒定速度50mm/s移動(dòng)。采用軌跡跟蹤算法對(duì)機(jī)器人的每個(gè)關(guān)節(jié)求解運(yùn)動(dòng)學(xué)逆解，得到仿真結(jié)果。機(jī)器人轉(zhuǎn)彎軌跡跟蹤仿真的序列圖如圖7所示。從圖中可以看出機(jī)器人各關(guān)節(jié)在跟蹤目標(biāo)曲線時(shí)具有很好的擬合度，逆運(yùn)動(dòng)學(xué)求解正確，在整個(gè)跟蹤過程中，機(jī)器人運(yùn)動(dòng)平穩(wěn)、位置偏移較小。

轉(zhuǎn)彎曲線僅需機(jī)器人在二維平面進(jìn)行軌跡跟蹤，不涉及三維空間運(yùn)動(dòng)，所以機(jī)器人俯仰關(guān)節(jié)的旋轉(zhuǎn)角度α_i總是0，只有偏轉(zhuǎn)關(guān)節(jié)的旋轉(zhuǎn)角度β_i需要進(jìn)行計(jì)算，結(jié)果如圖8所示。從圖中可以看出，關(guān)節(jié)轉(zhuǎn)角在整個(gè)跟蹤過程中，由初始時(shí)刻的0 rad轉(zhuǎn)變?yōu)?5π/18 rad，再以相反的趨勢(shì)轉(zhuǎn)變?yōu)? rad。整個(gè)過程中轉(zhuǎn)角變化十分平穩(wěn)，關(guān)節(jié)逆運(yùn)動(dòng)學(xué)解無(wú)奇異，并且每個(gè)時(shí)刻的關(guān)節(jié)旋轉(zhuǎn)角度均處于關(guān)節(jié)轉(zhuǎn)角的變化范圍（-π/2，π/2）之內(nèi)，跟蹤任務(wù)可以在設(shè)計(jì)的極限旋轉(zhuǎn)角度內(nèi)完成。

3 樣機(jī)實(shí)驗(yàn)

3.1 實(shí)驗(yàn)設(shè)計(jì)

蛇形機(jī)器人樣機(jī)的長(zhǎng)寬高尺寸為310mm×50mm×50mm，機(jī)器人連接件、支架和主動(dòng)輪由高性能樹脂材料通過3D打印技術(shù)制作而成。該機(jī)器人配備4個(gè)伺服電機(jī)（KST DS215MG），3個(gè)直流電機(jī)（CHW-GW12T-N20VA），主動(dòng)輪外緣覆蓋有橡膠圈用于增加摩擦力。實(shí)際測(cè)得機(jī)器人的直線運(yùn)動(dòng)速度為50mm/s，機(jī)器人總質(zhì)量為300g，各零部件之間通過螺栓螺母連接。機(jī)器人上安裝有5個(gè)熒光標(biāo)記點(diǎn)，以便于后期機(jī)器人關(guān)鍵點(diǎn)的捕捉與運(yùn)動(dòng)分析，標(biāo)記點(diǎn)分別位于機(jī)器人每一模塊的關(guān)節(jié)部分和行進(jìn)機(jī)構(gòu)部分，具體位置如圖9中放大圖所示（本刊黑白印刷，相關(guān)疑問咨詢作者）。蛇形機(jī)器人置于如圖9所示長(zhǎng)1 800mm，寬860mm的水平實(shí)驗(yàn)平臺(tái)上。實(shí)驗(yàn)平臺(tái)周圍安裝有4臺(tái)高速攝像機(jī)，從4個(gè)不同的視角拍攝機(jī)器人的運(yùn)動(dòng)實(shí)驗(yàn)，用來(lái)確定標(biāo)記點(diǎn)在整個(gè)空間區(qū)域的三維坐標(biāo)信息。實(shí)驗(yàn)中選擇與軌跡跟蹤仿真中相同的目標(biāo)曲線，利用算法求解出的關(guān)節(jié)轉(zhuǎn)角對(duì)機(jī)器人進(jìn)行控制。針對(duì)目標(biāo)曲線的軌跡跟蹤運(yùn)動(dòng)，共進(jìn)行了10次實(shí)驗(yàn)。在高速攝像機(jī)獲取運(yùn)動(dòng)序列后，運(yùn)行程序跟蹤所設(shè)置的5個(gè)熒光標(biāo)記點(diǎn)，然后利用直接線性變換DLT方法（direct linear transformation）獲得任意時(shí)刻下這些標(biāo)記點(diǎn)的坐標(biāo)數(shù)據(jù)^[14]。

3.2 實(shí)驗(yàn)結(jié)果

完成運(yùn)動(dòng)控制實(shí)驗(yàn)后，將10次實(shí)驗(yàn)中各標(biāo)記點(diǎn)的坐標(biāo)數(shù)據(jù)求取平均值并進(jìn)行線下擬合，得到機(jī)器人各標(biāo)記點(diǎn)的運(yùn)動(dòng)軌跡。圖10顯示了蛇形機(jī)器人進(jìn)行轉(zhuǎn)彎運(yùn)動(dòng)的實(shí)驗(yàn)結(jié)果。

在整個(gè)運(yùn)動(dòng)過程中，蛇形機(jī)器人按照目標(biāo)軌跡進(jìn)行軌跡跟蹤，與目標(biāo)軌跡保持了較好的擬合度，這與理論模型非常吻合。在運(yùn)動(dòng)過程的某些階段，機(jī)器人標(biāo)記點(diǎn)軌跡與目標(biāo)曲線存在一定誤差。5個(gè)標(biāo)記點(diǎn)最大距離誤差的平均值為12mm，誤差百分比為2.8%。這些實(shí)驗(yàn)結(jié)果較好地驗(yàn)證了機(jī)器人運(yùn)動(dòng)學(xué)分析與軌跡跟蹤算法的正確性。

為了進(jìn)一步驗(yàn)證機(jī)器人在管道環(huán)境下的運(yùn)動(dòng)性能，本文開展了實(shí)際管道內(nèi)的轉(zhuǎn)彎實(shí)驗(yàn)。管道參數(shù)如下：入口直徑80mm，90°直角轉(zhuǎn)彎至直徑100mm的管道。實(shí)驗(yàn)結(jié)果如圖11所示，可以觀察出機(jī)器人能適應(yīng)此類狹小管道環(huán)境，完成管道內(nèi)的轉(zhuǎn)彎運(yùn)動(dòng)。

通過對(duì)機(jī)器人的運(yùn)動(dòng)控制實(shí)驗(yàn)和實(shí)際管道內(nèi)的轉(zhuǎn)彎實(shí)驗(yàn)，得出結(jié)論：蛇形機(jī)器人的結(jié)構(gòu)設(shè)計(jì)具備合理性，針對(duì)機(jī)器人設(shè)計(jì)的軌跡跟蹤算法具備有效性，并且機(jī)器人在管道環(huán)境下進(jìn)行了實(shí)際測(cè)試，運(yùn)動(dòng)性能滿足使用要求。

4 結(jié)語(yǔ)

1）基于蛇的身體結(jié)構(gòu)所帶來(lái)的仿生設(shè)計(jì)靈感，結(jié)合狹小管道應(yīng)用場(chǎng)景，設(shè)計(jì)了三模塊五自由度蛇形機(jī)器人。機(jī)器人多關(guān)節(jié)與行進(jìn)機(jī)構(gòu)配合的構(gòu)型，保證了蛇形機(jī)器人的多自由度，提高了機(jī)器人對(duì)管道環(huán)境的適應(yīng)性。

2）對(duì)蛇形機(jī)器人進(jìn)行了運(yùn)動(dòng)學(xué)分析，基于從機(jī)器人尾部到頭部的前向軌跡跟蹤控制算法，實(shí)現(xiàn)了蛇形機(jī)器人的軌跡跟蹤運(yùn)動(dòng)控制。仿真結(jié)果表明：機(jī)器人軌跡跟蹤偏移量較小，逆運(yùn)動(dòng)學(xué)求解的關(guān)節(jié)角度變化滿足連續(xù)性與平穩(wěn)性，可以實(shí)現(xiàn)機(jī)器人各關(guān)節(jié)精確跟蹤目標(biāo)軌跡的控制需求。

3）搭建了蛇形機(jī)器人樣機(jī)，設(shè)計(jì)并開展了運(yùn)動(dòng)控制實(shí)驗(yàn)和實(shí)際管道環(huán)境下的轉(zhuǎn)彎實(shí)驗(yàn)。實(shí)驗(yàn)結(jié)果表明：機(jī)器人軌跡跟蹤精度較高，運(yùn)動(dòng)平穩(wěn)，在實(shí)際管道環(huán)境下可以實(shí)現(xiàn)直角轉(zhuǎn)彎，進(jìn)一步驗(yàn)證了機(jī)器人設(shè)計(jì)的合理性、逆運(yùn)動(dòng)學(xué)的正確性和運(yùn)動(dòng)控制方法的有效性。未來(lái)將著重于解決機(jī)器人運(yùn)動(dòng)中存在的滑動(dòng)問題，通過結(jié)構(gòu)優(yōu)化和誤差補(bǔ)償?shù)确绞綇浹a(bǔ)由滑動(dòng)帶來(lái)的誤差。今后，準(zhǔn)備探索將該機(jī)器人應(yīng)用于豎直管道的可能性。

參考文獻(xiàn)：

[1] （挪）帕爾·李列巴克（Pal Liljeback），等. 蛇形機(jī)器人：建模、機(jī)電設(shè)計(jì)及控制[M]. 譚天樂，譯. 北京：國(guó)防工業(yè)出版社，2015：5-14.

[2] MANZOOR S， CHOI Y. A unified neural oscillator model for various rhythmic locomotions of snake-like robot[J]. Neurocomputing，2016，173：1112-1123.

[3] 張?jiān)拢?董雷， 宦榮華，等. 風(fēng)電葉片管道內(nèi)窺履帶機(jī)器人的設(shè)計(jì)與運(yùn)動(dòng)分析[J]. 中國(guó)機(jī)械工程， 2021， 32（15）：1884-1889.

[4] 龍艷琴. 變剛度二維蛇形機(jī)器人機(jī)構(gòu)設(shè)計(jì)與運(yùn)動(dòng)控制研究[D]. 南京：東南大學(xué)，2021.

[5] 唐超權(quán)， 馬書根， 李斌，等. 具有環(huán)境適應(yīng)能力的蛇形機(jī)器人仿生控制方法[J]. 中國(guó)科學(xué)（信息科學(xué)）， 2014， 44（5）：647-663.

[6] OWEN T. Biologically inspired robots：snake-like locomotors and manipulators[J]. Robotica，1994，12（3）：282.

[7] ARIIZUMI R，TANAKA M，MATSUNO F. Analysis and heading control of continuum planar snake robot based on kinematics and a general solution thereof[J]. Advanced Robotics，2016，30（5）：301-314.

[8] LILJEB?CK P，STAVDAHL ?，PETTERSEN K Y. Modular pneumatic snake robot 3d modelling，implementation and control[J]. IFAC Proceedings Volumes，2005，38（1）：19-24.

[9] CHO C N，JUNG H，SON J，et al. An intuitive control algorithm for a snake-like natural orifice transluminal endoscopic surgery platform：a preliminary simulation study[J]. Biomedical Engineering Letters，2016，6（1）：39-46.

[10] TANAKA M，NAKAJIMA M，TANAKA K. Smooth control of an articulated mobile robot with switching constraints[J]. Advanced Robotics，2016，30（1）：29-40.

[11] 唐超權(quán)， 馬書根， 李斌，等. 基于神經(jīng)步進(jìn)激勵(lì)機(jī)制的蛇形機(jī)器人環(huán)境自適應(yīng)仿生控制策略[J]. 機(jī)械工程學(xué)報(bào)， 2013，49（1）：53-62.

[12] 邢利輝， 王亞慧， 鄧蕊. 管道探測(cè)蛇形機(jī)器人控制系統(tǒng)設(shè)計(jì)及仿真[J]. 小型微型計(jì)算機(jī)系統(tǒng)， 2020， 41（6）：1340-1344.

[13] DENAVIT J，HARTENBERG R S. A kinematic notation for lower-pair mechanisms based on matrices[J]. Journal of Applied Mechanics，1955，22（2）：215-221.

[14] DIANE H， THERIAULT， NATHAN W， et al. A protocol and calibration method for accurate multi-camera field videography[J]. Journal of Experimental Biology， 2014， 217：1843-1848.

收稿日期：2023-01-06