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

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

1 引言

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

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

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

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

2 結構設計與分析

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

圖1 內窺式搜索機器人機械結構Fig.1 Mechanical Structure of Endoscopic Search Robot

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

3 導向末端的運動學建模方法

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

圖2 運動學分析流程Fig.2 Kinematics Analysis Process

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

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

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

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

即假設導向末端運動過程中，柔性骨架彎曲形狀近似為圓弧且曲率處處相等［10］。

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

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

根據乘積指數公式：

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

3.2 關節(jié)空間到驅動空間的映射

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

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

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

驅動線對應的曲率半徑Ri可被記為：

式中：S—弧oo1的長度；

r—穿線孔到導線盤中心的距離。

驅動線長度Si可表示為：

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

3.3 驅動空間到關節(jié)空間的映射

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

由式（9）可得：

進而確定驅動空間到關節(jié)空間的映射：

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

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

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

機器人彎轉運動過程，如圖6所示。?可表示為：

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

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

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

進而得到：

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

4 運動學仿真研究

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

4.1 工作空間分析

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

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

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

4.2 運動學仿真算例

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

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

圖8 運動軌跡與空間位姿Fig.8 Motion Trajectory and Spatial Pose

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

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

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

圖10 驅動空間參數隨時間變化曲線Fig.10 Curve of Driving Space Parameters Over Time

5 原理樣機實驗

5.1 彎轉控制實驗

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

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

圖像為平滑的曲線，可有效避免驅動沖擊，對電機具有保護作用。彎、轉過程的空間位姿仿真結果與實驗結果對比，如圖12、圖13所示。

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

圖13 周轉運動空間位姿對比Fig.13 Spatial Posture Comparison of Turnover Movement

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

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

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

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

5.2 搜索性能實驗

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

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

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

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

線驅動的方式滿足搜索機器人進一步小型化制作的要求，減小的導線盤尺寸與增加柔性臂表面光滑度，將能夠進一步提升搜索機器人對小尺寸通道與多轉向通道的適應能力。

6 結論

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

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

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

性能實驗表明，所設計的搜索機器人對空間復雜的狹小通道有良好的適應能力強，能夠為搜索救援任務提供有效幫助。