水上搜救機器人航跡跟蹤控制器設計*

肖長詩 高欣國 文元橋 周 陽 隋忠義 向皓聞

(武漢理工大學航運學院1) 武漢 430063) (國家水運安全工程技術研究中心2) 武漢 430063)(內河航運技術湖北省重點實驗室3) 武漢 430063) (武漢理工大學智能交通系統研究中心4) 武漢 430063)

0 引 言

針對某些水域來往船只稀少、氣象和水文環境復雜，實施快速人工搜救困難的問題，設計了一種能水上自主搜救的機器人模型，機身采用呈正三角分布的三個浮筒形式，既保障搜救機器人自身穩定性，又可避免搜救過程對落水人員的傷害.該機器人運動靈活，可原地轉向；配備三個推進器，速度較快；結構簡單，易于回收和維護.

實現機器人自主搜救首先需要解決航跡跟蹤控制.航跡跟蹤是對規劃好的路徑進行跟蹤，有間接航跡跟蹤和直接航跡跟蹤兩種方式[1].間接航跡跟蹤分為外環制導環和內環控制環，直接航跡跟蹤則是將兩個環節集成在一起，這種航跡跟蹤通?；谏窠浘W絡等方法實現.由于間接航跡跟蹤結構簡單，較容易實現，在工程應用上較廣泛.而間接航跡跟蹤內環控制方法主要有反步法、內?？刂?、模糊控制和神經網絡等等.反步法和內?？刂颇苓_到較好的跟蹤效果，但需要精確的被控對象數學模型[2-3].模糊控制和神經網絡不需要精確的被控對象模型，但要求設計人員具有豐富的知識和實際經驗，且航跡跟蹤誤差波動較大[4-5].S面控制方法是模糊思想與PID控制結構的結合，它具有不依賴精確數學模型、適用于非線性控制和便于調節參數等優點[6].文獻[7-9]將S面控制方法運用于水下機器人的運動控制中，并進行了參數自調整的優化，通過實驗驗證了S面控制器具有既快又穩的控制效果.在外環制導研究中，LOS(line-of-sight)循跡已經在輪式機器人、飛行器和水面船舶上得到了廣泛的應用.Fossen等[10]在傳統的LOS制導算法加入積分環節，消除緩變環境載荷對船舶位置所產生的橫向偏離.與傳統船舶的路徑跟蹤相比，三推進器水上機器人還需要解決推進器布置冗余的分配問題，常規的偽逆法分配由于沒有考慮推力極限的約束，會造成推力分配過飽和的情況.

本文以三推進器水上機器人為研究對象，對其在航跡跟蹤控制問題進行研究.首先，運用二次規劃方法實現推力分配的優化；其次，選擇S面控制方法進行航向航速控制，在此基礎上加入基于反饋誤差的自適應調整項，提高航向航速控制的準確性，再結合改進的LOS制導算法設計內外環分離的航跡跟蹤控制器.最后，通過仿真和水池試驗驗證航跡跟蹤控制器的性能.

1 三推進器水上搜救機器人介紹與數學建模

三推進器水上機器人的外形結構見圖1.機器人由三個圓柱浮體構成，浮體下方安裝三個固定方向推進器，機器人運動形式類似于輪式機器人，上方圓臺是控制箱.通過三個推進器推力的分配，可實現水上機器人自轉、回旋、跟蹤等多種姿態控制.

圖1 水上機器人結構外形圖

三推進器水上機器人的運動控制系統框架見圖2，分為控制層、通信層、感知層,以及執行層.

圖2 水上搜救機器人運動控制系統框圖

圖3 三推進器水上機器人平面運動示意

水上搜救機器人的運動學模型為

(1)

水上搜救機器人動力建模的水動力阻力項采用線性形式，即所受阻力與速度呈線性關系，則機器人在水平面的水動力方程為

(2)

式中：M為機器人的慣性質量矩陣；C為向心力和科氏力系數矩陣；D為阻力系數矩陣；τ為水上機器人的推力及力矩矩陣.

由于推進器的安裝方向為縱向，所以橫向上沒有推力輸出，而式中的系數則通過實船實驗數據辨識得到.因此，水上機器人的動力學模型為

(3)

2 航跡跟蹤控制器設計

將航跡跟蹤控制器設計分為三部分：航向航速控制器、制導律和推力分配三部分，對于冗余的推力分配問題，運用二次規劃進行優化；航向航速控制采用改進S面控制方法；在航跡跟蹤制導環節，在LOS制導算法加入時變的前世距離，使得機器人實際運動更加靈活，最后通過仿真和水池實驗驗證控制器的效果.航跡跟蹤控制器的基本框架見圖4.

圖4 航跡跟蹤控制器基本框架

2.1 基于二次規劃的推力分配

三推進器水上機器人的三個方向的合力(矩)向量為

(4)

由于在橫向推力上有三個推進器提供動力，屬于冗余推力分配問題，而推進器的布置矩陣是一個非滿秩矩陣，無法通過矩陣的逆來進行計算.傳統的方法是采用B的偽逆矩陣進行推力的分配，而該方法沒有考慮推力幅值的約束因素，容易造成推力分配過飽和現象.因此,本節利用二次規劃數學優化的方法，引入性能指標來建立三個推進器的推力分配函數：三個推進器輸出的合力(矩)與期望的合力(矩)誤差最小化，為

minJ=(Bτ-Td)T(Bτ-Td)

s.t.τmin≤τ≤τmax

(5)

將上式展開

(6)

再令，

(7)

(8)

為驗證基于結合二次規劃推力分配的有效性，設計了同時用二次規劃方法和偽逆法的對比仿真實驗.仿真參數見表1.

表1 推力分配參數表

三推進器實際航行中，航向變化較頻繁，而航速改變情況較少，設定沿縱向方向的推力不變，繞Z軸的合力矩為正弦信號輸入，即Td=[40,0,10+17sin0.25t]，具體的仿真圖見圖5.

圖5 推力分配仿真對比效果圖

由推進器2的對比效果圖可知，偽逆法求出來的推力超過了推進器最大的幅值，基于二次規劃方法的推力分配解決了偽逆法存在的不足，且三個推進器的推力變化較為平滑，符合實際工程運用中的要求.

2.2 航向航速控制

常規S面控制器的控制模型為

(9)

文獻[6]將風浪流等影響因素，考慮成在某一段時間內的固定干擾，通過調整S面的偏移來達到消除固定干擾的作用，這樣在整體的運動時間內，實現S面自調整的目的.該種策略相當于在控制模型中加入了有條件的積分環節，適合用于平穩的運動控制中，當外界環境或期望值變化較明顯時，該種策略的改進效果不明顯.

本節針對實際應用中三推進器水上機器人航向航速變化頻繁的特點，為解決由于三個推進器安裝方向誤差、外界干擾對航向航速控制的影響.對S面中的調整項策略做改進，提出一種基于反饋誤差的自適應調整策略.航向和航速分別用改進后的S面進行控制.

在傳統教學過程中，都是由教師講授知識和技能，學生一味地跟著學習，簡單的采用期末考試來衡量學生成績的好壞，這些方式都無法激發學生的創新意識。計算機專業是實踐性很強的專業，教師通過講解和演示，學生最多學會了一些技術方法和簡單的應用，根本沒有創新的意識。學生通過參加大賽，可以激發他們學習的興趣和潛能，對知識面進行更加深入的學習，從而引導和激發學生的創新意識，逐漸可以提出自己解決問題的新思路和新方法。

改進后的S面表達式式為

(10)

步驟1記錄最近N條航向或航速偏差值.

步驟4對當前調整項進行削減.

步驟5如果計算出來的調整項數值大于當前偏差，對調整項進行低通濾波處理.

工程運用中具體表示形式為

(11)

式中：et為t時刻的航向或航速偏差；λ為低通濾波參數；η為調整因子.

(12)

βt為削減因子.

(13)

分析改進后的S面控制器，調整項的加入相當于在整個控制模型中增加了積分環節，提高S面控制對局部小偏差的反應能力，削減環節與低通濾波環節使得調整項的輸出更加的平滑.

2.3 航跡制導律

航跡跟蹤中最常用的航跡制導律是LOS導航算法，通過將當前位置與期望航跡的距離誤差帶入制導律中，求得期望航向，水上機器人只需跟蹤指令航向即可有收斂的保持航跡.LOS制導策略可分為基于包圍圈和前視距離兩種，本文采用基于前視距離的LOS制導律來進行優化.基于前視距離的LOS制導原理見圖6.

圖6 LOS制導原理圖

圖6中ψr為制導航向，RP距離為LOS制導律的前視距離，一般取值為1.5～2.5倍船長[11].

當機器人距離期望航跡較遠時，需要較大的制導角度(即較小的前視距離RP)來較快的接近航跡，而當機器人距離期望航跡較近時，又需要制導航向較小來保證航跡跟蹤的平穩.而固定的前視距離很解決上述問題，因此提出了時變前視距離的改進，增加機器人航跡跟蹤的靈活性.

RP=(RPmax-RPmin)e-λ1d2+RPmin

(14)

式中：d為當前位置到期望路徑的距離;λ1為大于零的常值參數;RPmax為最大前視距離，這里為2.5倍的水上機器人長度;RPmin為最小前世距離，這里為1.5倍水上機器人長度.

3 仿真與水池實驗

3.1 仿真分析

通過上一節的數學模型，運用MATLAB軟件進行航跡跟蹤的數值仿真.在直角坐標系下，定義x軸為正東方向，y軸為正北方向，航向角為沿y軸正半軸順時針轉動的角度，角度范圍為[0°,360°).設計航線(0,20)到(30，60)， S面控制器的參數k1=3.5,k2=0.03，定速1 m/s，初始條件：初始位置(0,0)，初始航向角0°，無干擾條件下，見圖7.

圖7 改進LOS與傳統LOS比較

由圖7可知，改進的LOS制導算法能更快的跟蹤航跡，且時變的前視距離不會引起跟蹤過程的振蕩.航跡控制器整體具有良好的穩定性、快速性，為實船實驗提供了理論支持.

3.2 水池實驗

水上機器人的航跡跟蹤實船實驗在試驗水池進行，水面有風的影響.實物與實驗場地見圖8.

圖8 水上機器人實物圖

為驗證基于改進S面控制的航向航速控制器的實際控制效果，進行常規S面和改進之后的S面控制器的對比試驗，設定期望航向300°.期望速度1.0 m/s，實驗效果見圖9.實驗結果表明，常規S面航向控制最終穩定后會與期望值存在一個穩態誤差，這個穩態誤差的造成是由于推進器安裝偏移造成的.而加入自適應調整后的航向保持可以減少穩定誤差，使航向航速保持更為準確.最終航向保持誤差在1°以內，改進的航向控制器效果較好.圖10為航速保持實驗圖，綜合圖9～10可知，航向航速保持的響應時間為15 s左右，驗證了基于S面控制方法的航向航速控制器在響應時間上有較好的效果.

圖9 航向保持

圖10 線速度保持

然后，為驗證文中設計的航跡跟蹤控制器的實際控制效果，進行航跡跟蹤實驗，分別直線跟蹤與曲線跟蹤，曲線設為半徑為15 m的圓，直線段設定關鍵點見表2.水上機器人航跡跟蹤運動軌跡見圖11.

表2 路徑規劃信息表 m

圖11 航跡跟蹤運動軌跡

圖11b)每段直線跟蹤有兩端波峰，其中較陡的波峰是由于在轉向點處，期望航跡改變造成的，分析航跡誤差可忽略.由圖11可知，所設計的航跡控制器在實驗中可以完成航跡跟蹤的目標，說明基于間接航跡跟蹤的兩點改進是可行的.航跡跟蹤在穩定后誤差在1 m以內，滿足水上搜救機器人執行任務的要求，但轉彎處的誤差較大，達到3 m，這一點在后續的研究工作中還需要進一步的改進.

4 結 論

1) 實驗的效果證明，基于二次規劃的推力分配解決了偽逆法輸出過飽和的問題，使得控制量的輸出不會因為限幅而達不到控制算法的控制效果；針對S面航向航速控制方法的改進，能有效減少固定干擾帶來的誤差；在LOS制導加入時變的前視距離，能加快水上機器人跟蹤航跡的速度，增加航跡跟蹤的靈活性.

2) 以三推進器水上搜救機器人為研究對象，將設計的航跡跟蹤控制器應用于實驗中，為解決水上搜救機器人快速、精確的自主搜救奠定了理論和實驗基礎.

3) 在下一步的研究中，將結合三推進器水上機器人轉向靈活的特點，設計滿足回轉半徑約束的制導律來實現對復雜路徑的跟蹤.