水上搜救機(jī)器人航跡跟蹤控制器設(shè)計*

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

(武漢理工大學(xué)航運(yùn)學(xué)院1) 武漢 430063) (國家水運(yùn)安全工程技術(shù)研究中心2) 武漢 430063)(內(nèi)河航運(yùn)技術(shù)湖北省重點(diǎn)實驗室3) 武漢 430063) (武漢理工大學(xué)智能交通系統(tǒng)研究中心4) 武漢 430063)

0 引 言

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

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

本文以三推進(jìn)器水上機(jī)器人為研究對象，對其在航跡跟蹤控制問題進(jìn)行研究.首先，運(yùn)用二次規(guī)劃方法實現(xiàn)推力分配的優(yōu)化；其次，選擇S面控制方法進(jìn)行航向航速控制，在此基礎(chǔ)上加入基于反饋誤差的自適應(yīng)調(diào)整項，提高航向航速控制的準(zhǔn)確性，再結(jié)合改進(jìn)的LOS制導(dǎo)算法設(shè)計內(nèi)外環(huán)分離的航跡跟蹤控制器.最后，通過仿真和水池試驗驗證航跡跟蹤控制器的性能.

1 三推進(jìn)器水上搜救機(jī)器人介紹與數(shù)學(xué)建模

三推進(jìn)器水上機(jī)器人的外形結(jié)構(gòu)見圖1.機(jī)器人由三個圓柱浮體構(gòu)成，浮體下方安裝三個固定方向推進(jìn)器，機(jī)器人運(yùn)動形式類似于輪式機(jī)器人，上方圓臺是控制箱.通過三個推進(jìn)器推力的分配，可實現(xiàn)水上機(jī)器人自轉(zhuǎn)、回旋、跟蹤等多種姿態(tài)控制.

圖1 水上機(jī)器人結(jié)構(gòu)外形圖

三推進(jìn)器水上機(jī)器人的運(yùn)動控制系統(tǒng)框架見圖2，分為控制層、通信層、感知層,以及執(zhí)行層.

圖2 水上搜救機(jī)器人運(yùn)動控制系統(tǒng)框圖

圖3 三推進(jìn)器水上機(jī)器人平面運(yùn)動示意

水上搜救機(jī)器人的運(yùn)動學(xué)模型為

(1)

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

(2)

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

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

(3)

2 航跡跟蹤控制器設(shè)計

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

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

2.1 基于二次規(guī)劃的推力分配

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

(4)

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

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

s.t.τmin≤τ≤τmax

(5)

將上式展開

(6)

再令，

(7)

(8)

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

表1 推力分配參數(shù)表

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

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

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

2.2 航向航速控制

常規(guī)S面控制器的控制模型為

(9)

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

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

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

改進(jìn)后的S面表達(dá)式式為

(10)

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

步驟4對當(dāng)前調(diào)整項進(jìn)行削減.

步驟5如果計算出來的調(diào)整項數(shù)值大于當(dāng)前偏差，對調(diào)整項進(jìn)行低通濾波處理.

工程運(yùn)用中具體表示形式為

(11)

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

(12)

βt為削減因子.

(13)

分析改進(jìn)后的S面控制器，調(diào)整項的加入相當(dāng)于在整個控制模型中增加了積分環(huán)節(jié)，提高S面控制對局部小偏差的反應(yīng)能力，削減環(huán)節(jié)與低通濾波環(huán)節(jié)使得調(diào)整項的輸出更加的平滑.

2.3 航跡制導(dǎo)律

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

圖6 LOS制導(dǎo)原理圖

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

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

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

(14)

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

3 仿真與水池實驗

3.1 仿真分析

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

圖7 改進(jìn)LOS與傳統(tǒng)LOS比較

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

3.2 水池實驗

水上機(jī)器人的航跡跟蹤實船實驗在試驗水池進(jìn)行，水面有風(fēng)的影響.實物與實驗場地見圖8.

圖8 水上機(jī)器人實物圖

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

圖9 航向保持

圖10 線速度保持

然后，為驗證文中設(shè)計的航跡跟蹤控制器的實際控制效果，進(jìn)行航跡跟蹤實驗，分別直線跟蹤與曲線跟蹤，曲線設(shè)為半徑為15 m的圓，直線段設(shè)定關(guān)鍵點(diǎn)見表2.水上機(jī)器人航跡跟蹤運(yùn)動軌跡見圖11.

表2 路徑規(guī)劃信息表 m

圖11 航跡跟蹤運(yùn)動軌跡

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

4 結(jié) 論

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

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

3) 在下一步的研究中，將結(jié)合三推進(jìn)器水上機(jī)器人轉(zhuǎn)向靈活的特點(diǎn)，設(shè)計滿足回轉(zhuǎn)半徑約束的制導(dǎo)律來實現(xiàn)對復(fù)雜路徑的跟蹤.