蜿蜒步態下的蛇形機器人路徑跟蹤控制研究

喬海曄，吳清輝，張震

（1.佛山職業技術學院，佛山廣東，528137；2.廣州市威控機器人有限公司，廣州廣東，510500）

1 項目綜述

2012 年，美國卡內基梅隆大學開發的Uncle Sam 蛇形機器人。該機器人引入了柔順控制，使得其運動特性更加貼近生物蛇，且其關節采用正交連接，每個關節都裝有一個電機，使得其能做出更為復雜的運動，并很好的實現了攀爬及翻越能力[1]。目前，將多足機器人與蛇形機器人投入實際救援行動的應用不多，大部分還處在實驗室環境下的研究階段[2]。蛇形機器人特殊的肢體結構和運動原理使其機體的運動控制與路徑跟蹤存在著極大的挑戰，是蛇形機器人領域的研究熱點之一。

蛇形機器人在蜿蜒步態下的路徑跟蹤控制問題，首要任務是控制蛇形機器人的運動方向。Hirose 首先為蛇機器人引入了一種旋轉行為，通過在曲率上增加一個恒定的偏移量來增加體波[3]。通過調節這個恒定的偏移量，機器人就能夠進行轉向，這種方法稱為“偏移轉向”。后續的工作證明了偏移轉彎在二維蛇機器人運動問題中的有效性。葉長龍通過幅值調整、相位調整等方法改變蛇形機器人蜿蜒運動的運動方向，并在幅值調整法中進行了初步的定量轉彎半徑和轉彎角度的研究[4]。Dai 等人使用幾何力學的技術，將微分幾何應用于剛體運動，來設計一種使原地轉彎成為可能的步態，這種方法稱為“幾何轉彎”[5]。這兩種平面內的轉動行為都是通過沿機器人體傳播一個行波并調制行波的參數來實現的。Wang 等人受微小線蟲線蟲行為的啟發，提出了一種新穎的無肢機器人原地轉彎步態[6]。為了簡化機器人內部自由度的控制，引入了一個受生物啟發的模板，其中兩個共面行波疊加產生一個平面內旋轉運動，即歐米伽旋轉，使得機器人能夠在高度受限的空間中轉彎。實現蛇形機器人跟隨期望路徑的次要任務是使蛇形機器人根據期望路徑調節運動方向。

在進行目標識別或追蹤任務時，常常會因頭部關節擺動幅度過大而導致前方目標丟失。因此如何保證蛇形機器人在蜿蜒運動下的頭部穩定成為實現定向運動的首要目標。2016 年，來自新西蘭的研究者 Pinwei Jin 和 ChiKit Au 提出了一種頭部固定的方法[7]，重新設計機器人頭部關節的控制函數，通過添加補償函數的方式使機器人頭部在運動過程中始終朝向一個方向，即分段控制。而該方法的缺陷在于補償函數的參數確定方法較為復雜，且蛇頭與整體之間并不連貫，需要對頭部舵機進行單獨的函數控制。而在機器人蜿蜒步態控制器設計領域同樣存在諸多成果。2017 年，來自日本的學者Ryo Ariizumi 針對無側滑約束的蛇形機器人設計了頭部位置控制器[8]，應用于頭部軌跡導航控制。文章分析了各向同性摩擦力時機器人的運動情況，并認為奇異狀態下的機器人具備可操作性，是進行控制的關鍵。但是對于摩擦是各向異性時的情況仍缺乏討論[9]。

2 蛇形機器人技術特點

面對災后復雜狹窄空間，蛇形機器人因其狹長形態和靈活的運動能力，能夠實現對管道等狹窄空間的搜索，彌補了足式機器人無法探測到狹窄空間的缺點。不同于輪式或者足式機器人具有獨立地推進機構，蛇形機器人由于其關節和連桿耦合度高，驅動機構需同步整個機體的運動，因此蛇形機器人的控制設計是極具挑戰性的[10]。在路徑跟蹤控制中，蛇形機器人的質心位置在前向蜿蜒運動中并不是直線運動，而是周期性地在前向直線附近擺動，除此，方向角也在直線附近周期性擺動，僅靠傳統路徑跟蹤控制方法將其狀態控制到理想直線路徑下是無意義的，因此蛇形機器人軌跡跟蹤控制難度較大[11]。

直線路徑跟蹤控制是實現機器人沿指定路徑移動并進行三維重建的前提。通過基于滑模控制理論的直線路徑跟蹤控制技術，減小了蛇形機器人在穩態運動時的誤差。頭部控制是保證傳感器及相機數據穩定的基礎。基于補償函數的頭部控制方法，提高了圖像及傳感器信息的穩定性和準確性。此基礎上研究轉彎方法，可以提高蛇形機器人避障效率。

3 蛇形機器人自適應運動控制技術

■3.1 基于滑模控制理論的直線路徑跟蹤控制技術

蛇形機器人基于滑模理論的軌跡跟蹤控制技術路線圖總結如圖1 所示。

圖1 基于滑模理論的軌跡跟蹤控制技術路線圖

以沿著全局x軸正方向運動為例，直線路徑跟蹤控制器的具體目標是機器人初始時刻處于任何姿態下，經過有限的時間后均能沿著全局x軸正方向運動。設定蛇形機器人的質心全局位置坐標為(p x,py)，前進的方向角為θ。所以控制目標即為py和，使得它們最終收斂于py=0，=0的極限環。

根據轉彎角和控制參數ΔA之間存在定量關系，可以通過轉彎實現機器人的定向運動。

學生通過觀看Lagrange定理微視頻自學，教師要及時追蹤學生在線自測的反饋信息，并收集學生的疑惑、不理解的公式及證明的推導等。這節課的疑惑是Lagrange定理的證明中輔助函數的構造。在課堂上重點講解難點疑惑，對于這節課，重點和難點是構造輔助函數對Lagrange中值定理的證明，可以多花點時間。

下面進行滑模控制器的設計。

選取蛇形機器人控制系統的切換面如下：

由于蛇形機器人系統可能存在遠離滑動模塊的情況，所以選用冪次趨近律公式作為本系統的趨近律。

由上式可得：

所以蛇形機器人的控制率公式如下：

滑模控制器，即方向控制器具體控制框圖如圖2 所示。

圖2 基于滑模控制理論的方向控制器

為了驗證上述控制器的穩定性。取李雅普諾夫函數V=s2/2，取V≥ 0。可得V˙=ss˙。結合冪次趨近律公式得：

由于冪次趨近律公式中設定了k>0.1，0

■3.2 基于補償函數的頭部控制技術

引入補償函數：

使得：

■3.3 基于頭部穩定的轉彎技術

在保證了蛇頭穩定的基礎上，蛇形機器人通過安裝在頭部的紅外傳感器來檢測其前方是否有障礙物，當在前進方向中遇到障礙物時，蛇形機器人基于幅值調整法來改變其運動的幅度，從而達到控制機器人蜿蜒運動中避開障礙物的目的。

基于幅值調整法的蛇形曲線如圖3 所示，其基本思想是通過調整幅值ΔA，來改變機器人的運動方向，從而實現轉彎。其中幅值角和轉彎角存在定量關系，φ=a2-a1=2aΔA,a1≈a(1-ΔA),a2≈a(1+ΔA)。

為保證蛇頭固定，基于補償函數的思想，蛇頭部分的控制函數為：

所以基于頭部控制的轉彎控制函數如下式，假設奇數關節為俯仰關節，偶數關節為偏航關節。

4 仿真平臺運動控制實驗

蛇形機器人基于VREP 仿真平臺的運動實驗結果如下：

■4.1 基于補償函數的頭部控制實驗

對蛇形機器人蜿蜒運動進行頭部控制仿真，現期望蛇頭運動方向與x 軸平行，其仿真實驗圖如圖4 所示。

可以發現，上述實驗蛇形機器人頭部攝像機讀取的信息非常穩定，由此可見蛇頭部分得以很好的控制。對比沒有頭部控制下的蛇頭運動方向角，如圖5所示，可以發現，基于補償函數的控制方法使得頭部運動的穩定性大幅度提高。

圖5 有無頭部控制下的頭部運動方向角對比圖

■4.2 基于頭部控制下的轉彎實驗

基于上述頭部控制方法，本項目利用Vrep 進行蜿蜒運動時轉彎的仿真實驗，其轉彎過程仿真圖如圖6 所示。

圖6 基于幅值調整法的轉彎仿真實驗圖

令蛇形機器人從全局坐標系原點出發，沿x軸正方向運動，并設定控制參數A=40°,K=60°，W=50°/s。在t∈[20,40]時，ΔA=10°，而在t∈[80,120]時，ΔA=-2 0°，其余時刻ΔA均為0。得到的仿真結果如圖7 所示。

圖7 基于幅值調整法的轉彎仿真結果圖

由上述結果可以發現，蛇形機器人轉彎角度比較連續，且更好地維持了蜿蜒時的波形曲線。

■4.3 基于滑模控制的直線跟蹤控制實驗

利用Vrep 進行仿真實驗，令蛇形機器人初始時刻質心坐標為（0，-5m），方向角為θ=45°，最終得到的運動仿真結果如圖8 所示。

圖8 基于滑模控制的機器人運動曲線

由上述仿真結果得，機器人在80s 左右達到穩態，且方向角在零附近波動，則本項目設計的控制器合理。并將本項目設計的滑模控制器與傳統PI 控制進行對比，在機器人穩定達到預期直線后，機器人質心Y 坐標與期望值之間的誤差結果見表1。可發現，與PI 控制相比，滑模控制的精確度更高。

表1 滑模控制與PI控制誤差對比

5 結果驗證

蛇形機器人自適應運動控制已在VREP仿真和實體上得到實現，并將其應用于蛇形機器人管道探測中，仿真和實體運動場景如圖9 所示。

圖9 蛇形機器人基于蜿蜒運動的管道探索仿真和實物運行圖