一種內(nèi)窺式搜索機(jī)器人的設(shè)計與運(yùn)動學(xué)分析

李鳳剛，趙磊，何廣平，狄杰建

（北方工業(yè)大學(xué)機(jī)械與材料工程學(xué)院，北京 100144）

1 引言

地震、采礦事故等災(zāi)害的發(fā)生導(dǎo)致了大量的人員傷亡，據(jù)統(tǒng)計，災(zāi)害后的72h是搜尋和救助幸存者的關(guān)鍵時間[1]，然而，災(zāi)后通道受阻會導(dǎo)致大多數(shù)救援設(shè)備難以進(jìn)入廢墟，這對救援工作的開展提出了嚴(yán)峻挑戰(zhàn)。

研究人員正嘗試開發(fā)搜索設(shè)備進(jìn)入災(zāi)難現(xiàn)場執(zhí)行救援任務(wù)。輪式搜索機(jī)器人[2]結(jié)構(gòu)簡單，成本低，平面移動速度快；但在面對復(fù)雜的廢墟環(huán)境是穿越能力略顯不足。履帶式搜索機(jī)器人[3]具有較強(qiáng)的通過能力，不僅適用于平坦的地面的運(yùn)動，而且適用于非結(jié)構(gòu)性環(huán)境；但它們的體積相對較大，不能在災(zāi)后環(huán)境中進(jìn)入狹窄的空間[4]。仿生搜索機(jī)器人起步晚，但發(fā)展迅速；文獻(xiàn)[5]是典型的超冗余度串聯(lián)仿生機(jī)器人[6]，模塊化的設(shè)計使其在沙漠、廢墟等環(huán)境下可快速前行，然而該類機(jī)器人在轉(zhuǎn)角過大處容易卡在廢墟中并且難以擺脫困境。連續(xù)體[7]與軟體[8]機(jī)器人由于固有的柔順性和連續(xù)變形的能力，使其在復(fù)雜的災(zāi)后現(xiàn)場的中有著良好的應(yīng)用前景。通過推進(jìn)裝置工業(yè)內(nèi)窺鏡探入廢墟縫內(nèi)部以獲取狹小內(nèi)部的實時狀態(tài)是消防人員搜尋幸存者有效途徑之一，但在復(fù)雜多變的廢墟間隙中，工業(yè)內(nèi)窺鏡存在嚴(yán)重的轉(zhuǎn)向問題，很難將其頭部準(zhǔn)確的推入預(yù)期的間隙或通道[9]。

以增加內(nèi)窺式搜索機(jī)器人的導(dǎo)向性與靈活性為目的，設(shè)計了一款由并聯(lián)拉線驅(qū)動的內(nèi)窺式搜索機(jī)器人，其具備連續(xù)體機(jī)器人的靈活變形能力和工業(yè)內(nèi)窺鏡成本低廉與方便攜帶的特點，可協(xié)助消防員完成災(zāi)難現(xiàn)場復(fù)雜狹小空間的搜索工作。

接著，研究機(jī)器人導(dǎo)向末端的運(yùn)動學(xué)特點，給出了導(dǎo)向末端完整的運(yùn)動學(xué)建模過程。然后，基于獲得的運(yùn)動學(xué)模型給出了導(dǎo)向末端工作空間的分析結(jié)果和正、逆運(yùn)動學(xué)仿真算例。最后，通過實驗驗證了內(nèi)窺式搜索機(jī)器人末端的彎轉(zhuǎn)導(dǎo)向能力與狹小通道的穿越能力。

2 結(jié)構(gòu)設(shè)計與分析

該內(nèi)窺式搜索機(jī)器人主要由導(dǎo)向末端、柔性臂、攝像頭模組以及驅(qū)動裝置構(gòu)成，如圖1所示。導(dǎo)向末端由柔性骨架、導(dǎo)線盤和驅(qū)動線三部分組成。其柔性骨架保證運(yùn)動過程中導(dǎo)向末端軸向不可拉伸或壓縮；周向均布三個穿線孔的導(dǎo)線盤可限定導(dǎo)驅(qū)動線的位置與形狀；三根驅(qū)動線與端部導(dǎo)線盤剛性連接。柔性臂是基部導(dǎo)線盤以下的部分，包含多媒體數(shù)據(jù)線、驅(qū)動線、解耦套管以及外部套管；解耦套管的解耦原理類似與自行車的剎車系統(tǒng)，柔性臂運(yùn)動過程任意改變套管區(qū)的形狀，并不會引起無套管區(qū)拉線長度的變化。攝像頭模組集成了視頻、音頻系統(tǒng)和照明系統(tǒng)。

圖1 內(nèi)窺式搜索機(jī)器人機(jī)械結(jié)構(gòu)Fig.1 Mechanical Structure of Endoscopic Search Robot

該機(jī)器人導(dǎo)向末端采用冗余的驅(qū)動方式，通過協(xié)調(diào)改變3根驅(qū)動線的長度使機(jī)器人完成2自由度(DOF)的彎轉(zhuǎn)運(yùn)動，以滿足搜索現(xiàn)場探尋非結(jié)構(gòu)性環(huán)境中狹小減小或通道的需求。此外，機(jī)器人采用線驅(qū)動的方式有效的減輕了自重，縮小了體積；柔性骨架固有的連續(xù)性與柔順性能夠降低人機(jī)接觸過程的沖擊力，提高搜索過程的安全性。

3 導(dǎo)向末端的運(yùn)動學(xué)建模方法

由于導(dǎo)向末端采用電機(jī)后置的拉線驅(qū)動方式，使得其關(guān)節(jié)空間和驅(qū)動空間不相同。以獲取拉線長度變化量與機(jī)器人末端位姿變化的對應(yīng)關(guān)系為目的，設(shè)定三個空間，如圖2所示。

圖2 運(yùn)動學(xué)分析流程Fig.2 Kinematics Analysis Process

進(jìn)而分析獲得機(jī)器人導(dǎo)向末端的全局正向運(yùn)動學(xué)與逆運(yùn)動學(xué)。圖2中S1S2S3表示三根驅(qū)動線的長度，?∈[0，2π]為導(dǎo)向末端彎曲后所在平面與x軸正方向的夾角，θ∈[0，π]表征導(dǎo)向末端彎曲后的曲率角，(x y z)表示導(dǎo)向末端頂部在工作空間中的坐標(biāo)值；基于導(dǎo)向末端軸向不可伸縮與延伸的特點，設(shè)中心柔性骨架長度為定值S。

3.1 關(guān)節(jié)空間到工作空間的映射

對機(jī)器人導(dǎo)向末端建模如下：基坐標(biāo)系(o0?x0y0z0)固定在基部導(dǎo)線盤的幾何中心O0，末端標(biāo)系(o1?x1y1z1)固定在頂端導(dǎo)線盤的幾何中心O1；其x指向基礎(chǔ)導(dǎo)線盤的第一個孔眼z軸豎直朝向上，y軸方向可由右手定則確定。基于恒定曲率假設(shè)建立導(dǎo)向末端的運(yùn)動學(xué)模型，如圖3所示。

圖3 工作空間位姿模型Fig.3 Workspace Pose Model

即假設(shè)導(dǎo)向末端運(yùn)動過程中，柔性骨架彎曲形狀近似為圓弧且曲率處處相等［10］。

由圖3 機(jī)器人工作時(θ≠0)的位姿可得，基坐標(biāo)到末端坐標(biāo)的變換可以通過四個基本變換實現(xiàn)：首先將基準(zhǔn)坐標(biāo)原點O0平移至末端坐標(biāo)系原點O1；然后將平移得到的坐標(biāo)繞其z軸旋轉(zhuǎn)角度?；接著將旋轉(zhuǎn)得到的坐標(biāo)系繞其y軸旋轉(zhuǎn)角度θ；最后坐標(biāo)系繞新z軸旋轉(zhuǎn)??。按上述過程我們將相應(yīng)的旋量坐標(biāo)ξ∈?6寫成：

其中，R=S/θ。通過定義算子“∧”將矢量從?6映射到SE(3)的李代數(shù)se(3)，即將六維向量ξ映射為4 × 4 階矩陣。由此旋量坐標(biāo)（1）對應(yīng)的李代數(shù)可以寫成：

根據(jù)乘積指數(shù)公式：

獲得該機(jī)器人從關(guān)節(jié)空間到工作空間段的齊次變換矩陣如下：

3.2 關(guān)節(jié)空間到驅(qū)動空間的映射

為推導(dǎo)機(jī)器人關(guān)節(jié)空間Θ=[? θ]T到驅(qū)動空間q=[S1S2S3]T的映射關(guān)系，在機(jī)器人工作過程中我們可以建立幾何模型，如圖4所示。并做出機(jī)器人在x1o1y1面橫切圖，如圖5所示。當(dāng)導(dǎo)向末端彎曲方向與x0軸夾角為?，彎曲角度為θ時，可得：

圖4 驅(qū)動空間與關(guān)節(jié)空間之間的關(guān)系Fig.4 Relationship Between Driving Space and Joint Space

圖5 x1o1y1面橫切圖Fig.5 x1o1y1 Sectional View

驅(qū)動線對應(yīng)的曲率半徑Ri可被記為：

式中：S—弧oo1的長度；

r—穿線孔到導(dǎo)線盤中心的距離。

驅(qū)動線長度Si可表示為：

綜合式（6）～式（8）可得關(guān)節(jié)空間到驅(qū)動空間的映射為：

3.3 驅(qū)動空間到關(guān)節(jié)空間的映射

本小節(jié)中，將分析機(jī)器人的驅(qū)動空間q=[S1S2S3]T到位姿空間Θ=[? θ]T的映射。

由式（9）可得：

進(jìn)而確定驅(qū)動空間到關(guān)節(jié)空間的映射：

3.4 工作空間到關(guān)節(jié)空間的映射

連續(xù)體搜索機(jī)器人在運(yùn)動過程中，其中基部導(dǎo)線盤原點O0固定在右旋歐幾里德空間的原點，其末端在基坐標(biāo)下的位置可被參數(shù)化為Ω=[x y z]T。

本節(jié)在工作空間x、y、z已知的條件下確定關(guān)節(jié)空間參數(shù)Θ=[? θ]T。

機(jī)器人彎轉(zhuǎn)運(yùn)動過程，如圖6所示。?可表示為：

在圖6中，A為O’1在x0o0y0面上的投影，B為弧OO’1的圓心，進(jìn)而可得出：

圖6 工作空間與關(guān)節(jié)空間之間的關(guān)系Fig.6 Relationship Between Workspace and Joint Space

在Rt△ABO'1中，根據(jù)Pythagoras定理可得：

進(jìn)而得到：

式（12）與式（18）為工作空間到關(guān)節(jié)空間的映射。

4 運(yùn)動學(xué)仿真研究

為了驗證所得運(yùn)動學(xué)模型的有效性，基于MATLAB 分析了導(dǎo)向末端的工作范圍，并給出了搜索機(jī)器人末端的運(yùn)動學(xué)的仿真算例，算例中用到的基本參數(shù)如下：柔性骨架長度為150mm，驅(qū)動線軸心所在的分度圓半徑為10.5mm；運(yùn)動過程中的彎轉(zhuǎn)參數(shù)范圍為θ∈[?π，π]、?∈[0，2π]。

4.1 工作空間分析

為了標(biāo)定該機(jī)器人導(dǎo)向末端的工作空間，以基盤中心為坐標(biāo)原點，繪制機(jī)器人末端點空間取值范圍，如圖7所示。

圖7 搜索機(jī)器人的工作空間Fig.7 Work Space of Massage Robot

仿真結(jié)果表明，該機(jī)器人導(dǎo)向末端工作空間形狀近似一個半球面，其工作空間坐標(biāo)最大值為：Xmax=Ymax=108.7mm，Zmax=150mm。

4.2 運(yùn)動學(xué)仿真算例

在導(dǎo)向末端頂點可達(dá)的工作空間范圍內(nèi)規(guī)劃一條如式（19）所示的空間螺旋軌跡，基于前文獲得的逆運(yùn)動學(xué)算法對導(dǎo)向末端進(jìn)行運(yùn)動控制。

所給算例中，起始狀態(tài)機(jī)器人末端坐標(biāo)為( 0，0，150)，導(dǎo)向末端驅(qū)動線的長度為150mm，彎曲角度?與旋轉(zhuǎn)角度θ均為0°；當(dāng)采樣間隔為0.12s時，機(jī)器人導(dǎo)向末端在（0～6）內(nèi)的空間位姿，如圖8中的彩色弧線所示。連接末端點的洋紅色曲線表示機(jī)器人末端的空間軌跡。

圖8 運(yùn)動軌跡與空間位姿Fig.8 Motion Trajectory and Spatial Pose

由圖9中關(guān)節(jié)空間參數(shù)隨時間的變化可知，當(dāng)運(yùn)動停止時，彎、轉(zhuǎn)角度值θ=3.142、φ=6.283；且該過程中關(guān)節(jié)空間參數(shù)曲線斜率均隨時間的延長而增大，表征著運(yùn)動過程中彎轉(zhuǎn)速度都在增長，與圖8中空間姿態(tài)間隙逐漸增大相吻合。

圖9 關(guān)節(jié)空間參數(shù)隨時間變化曲線Fig.9 Curves of Time Parameters of Virtual Joint Space

此外，算例中三根驅(qū)動線長度隨時間的變化曲線，如圖10所示。結(jié)果顯示，該過程中拉線長度變化量最大的為L1，其最大拉動長度為32.99mm；在運(yùn)動停止時L2、L3長度變化相同，均為16.49mm。所得仿真數(shù)據(jù)對樣機(jī)的加工制作具有指導(dǎo)意義。

圖10 驅(qū)動空間參數(shù)隨時間變化曲線Fig.10 Curve of Driving Space Parameters Over Time

5 原理樣機(jī)實驗

5.1 彎轉(zhuǎn)控制實驗

為進(jìn)一步驗證導(dǎo)向末端的空間彎轉(zhuǎn)能力，在無約束環(huán)境下對導(dǎo)向末端運(yùn)動性能進(jìn)行彎轉(zhuǎn)驗證實驗。令導(dǎo)向末端首先在10s內(nèi)沿φ=0°平面內(nèi)完成60°彎曲；然后，在接下來的30s內(nèi)完成順時針（俯視）周向轉(zhuǎn)動。為保證運(yùn)動的平穩(wěn)性，彎轉(zhuǎn)動作的運(yùn)動規(guī)劃通過關(guān)節(jié)空間中的五次多項式插值完成，兩個動作的起止時刻導(dǎo)向末端頂點的速度、加速度均設(shè)置為零，得到的關(guān)節(jié)空間運(yùn)動參數(shù)，如圖11所示。

圖11 關(guān)節(jié)空間運(yùn)動規(guī)劃參數(shù)Fig.11 Joint Space Motion Planning Parameters

圖像為平滑的曲線，可有效避免驅(qū)動沖擊，對電機(jī)具有保護(hù)作用。彎、轉(zhuǎn)過程的空間位姿仿真結(jié)果與實驗結(jié)果對比，如圖12、圖13所示。

圖12 彎曲運(yùn)動空間位姿對比Fig.12 Comparison of Postures in Bending Motion Space

圖13 周轉(zhuǎn)運(yùn)動空間位姿對比Fig.13 Spatial Posture Comparison of Turnover Movement

實驗中，導(dǎo)向末端彎轉(zhuǎn)過程柔順、靈活，能夠在以基部導(dǎo)向盤為中心的任意平面內(nèi)完成彎曲運(yùn)動，與仿真預(yù)期的運(yùn)動過程具有一致性，證明了所提出運(yùn)動學(xué)模型的有效性。

實驗過程中，通過位移傳感器對驅(qū)動線的拉動長度進(jìn)行采集，采樣間隔為1s，將得到的結(jié)果與驅(qū)動空間參數(shù)仿真結(jié)果對比，如圖14所示。

圖14 驅(qū)動空間仿真與實驗參數(shù)對比Fig.14 Comparison of Driving Space Simulation and Experimental Earameters

實驗結(jié)果表明，最大驅(qū)動誤差不超2%，造成誤差的主要原因是導(dǎo)向末端柔性骨架支撐剛度不夠，導(dǎo)致其彎轉(zhuǎn)過程回拉彈力不足造成的。

5.2 搜索性能實驗

為驗證搜索機(jī)器人在復(fù)雜狹小空間內(nèi)的搜索能力，搭建廢墟三維模擬通道，如圖15所示。

圖15 搜索性能實驗Fig.15 Search and Rescue Performance Experiment

所給示例中，機(jī)器人在穿越模擬的三維廢墟狹小通道時，需依次完成擇向、轉(zhuǎn)向和內(nèi)窺等動作，并通過調(diào)整導(dǎo)向末端位姿將目標(biāo)點的實時信息傳給使用者。

實驗結(jié)果表明原理樣機(jī)能夠在轉(zhuǎn)角小于90°、直徑大于50mm的空間不規(guī)則通道內(nèi)順利完成擇向、轉(zhuǎn)向和內(nèi)窺操作；在沒有輔助推送設(shè)備的前提下，原理樣機(jī)的柔性臂能夠順利通過三次彎折為90°的空間轉(zhuǎn)向通道。

線驅(qū)動的方式滿足搜索機(jī)器人進(jìn)一步小型化制作的要求，減小的導(dǎo)線盤尺寸與增加柔性臂表面光滑度，將能夠進(jìn)一步提升搜索機(jī)器人對小尺寸通道與多轉(zhuǎn)向通道的適應(yīng)能力。

6 結(jié)論

（1）設(shè)計了一款具有靈活導(dǎo)向末端的內(nèi)窺式柔性搜索機(jī)器人。該機(jī)器人纖細(xì)、靈活的柔性臂對災(zāi)難現(xiàn)場的狹小通道有良好的適應(yīng)能力；面對多變的前行通道，導(dǎo)向末端能有效的擇向能力，并將前端空間狀態(tài)實時傳給操作者。

（2）提出了一種用于靈活導(dǎo)向末端的完整運(yùn)動學(xué)分析方法。正向運(yùn)動學(xué)中關(guān)節(jié)空間到工作空間的映射基于旋量理論獲得，驅(qū)動空間到關(guān)節(jié)空間的映射以及全局的逆運(yùn)動學(xué)映射通過幾何分析的方法得到。給出的運(yùn)動學(xué)解析映射簡單、直觀，可用于機(jī)器人導(dǎo)向末端的實時控制。

（3）通過對搜索機(jī)器人導(dǎo)向末端進(jìn)行仿真分析和原理樣機(jī)實驗，驗證了運(yùn)動學(xué)算法的正確性和機(jī)器人末端軌跡規(guī)劃與控制的可行性。

性能實驗表明，所設(shè)計的搜索機(jī)器人對空間復(fù)雜的狹小通道有良好的適應(yīng)能力強(qiáng)，能夠為搜索救援任務(wù)提供有效幫助。