水上無(wú)人艇目標(biāo)跟蹤控制方法研究

秦世洋，張 騰，武紹寬，李孟委,3

(1.中北大學(xué) 儀器與電子學(xué)院，山西 太原 030051；2.中北大學(xué) 儀器科學(xué)與動(dòng)態(tài)測(cè)試重點(diǎn)實(shí)驗(yàn)室，山西 太原 030051；3.中北大學(xué) 南通智能光機(jī)電研究院，江蘇 南通 226000)

0 引 言

眾所周知，地球大約三分之二的面積被海洋覆蓋[1]，但很多區(qū)域未被徹底探索過(guò)，由于氣候變化、環(huán)境異常和國(guó)家安全問(wèn)題研究的需要，人們從商業(yè)、科學(xué)和軍事領(lǐng)域?qū)Ｑ筮M(jìn)行了開(kāi)發(fā)，如水深測(cè)量[2]，環(huán)境監(jiān)測(cè)，采樣評(píng)估[3-5]，石油、天然氣和礦山勘探[6]，以及海上平臺(tái)或管道的建設(shè)與維護(hù)[7-8]，還有交通運(yùn)輸，移動(dòng)通信中繼[9-10]等，水上無(wú)人艇(USV)被廣泛用于這些開(kāi)發(fā)的應(yīng)用中，而且應(yīng)用要求越來(lái)越高.然而，盡管USV具有巨大的應(yīng)用潛力和操作自主性，但從技術(shù)演示到實(shí)際應(yīng)用的轉(zhuǎn)變卻是一個(gè)緩慢的過(guò)程.這一重大差距原因在于USV控制應(yīng)用不可避免地受到風(fēng)、浪和海流等環(huán)境的影響，而小型USV由于慣性小、體積小、對(duì)環(huán)境的干擾尤其敏感.

為了解決這些問(wèn)題，越來(lái)越多的控制技術(shù)被設(shè)計(jì)和改進(jìn)以提高USV在海洋研究領(lǐng)域應(yīng)用的性能[11].從本質(zhì)上講，控制策略的發(fā)展是以不同的控制目標(biāo)為導(dǎo)向的，根據(jù)調(diào)研[12-14]，這些目標(biāo)大致可分為三類(lèi)：

1)設(shè)定點(diǎn)跟蹤[15-17]：這是最基本的控制目標(biāo)，它使USV的位置和方向在不受時(shí)間限制的情況下達(dá)到預(yù)期要求；

2)軌跡跟蹤[18-20]：USV被驅(qū)動(dòng)來(lái)跟蹤隨時(shí)間變化的參考軌跡，同時(shí)遵守預(yù)先定義的時(shí)間約束和空間約束；

3)路徑跟蹤[21-23]：USV需要通過(guò)驅(qū)動(dòng)來(lái)跟蹤預(yù)期的不隨時(shí)間變化的路徑，與軌跡跟蹤相比，由于不遵循時(shí)間約束，實(shí)現(xiàn)了更平滑的路徑.

本文設(shè)計(jì)的控制目標(biāo)為跟蹤設(shè)定點(diǎn)，針對(duì)這一問(wèn)題，在過(guò)去的幾十年中，已經(jīng)有幾種控制策略被提出來(lái).Dai等人[24]提出一種基于模型的控制技術(shù)，通過(guò)自適應(yīng)控制律來(lái)減弱干擾，以達(dá)到控制目的.Annamalai等人[25]提出了一種基于模型的現(xiàn)代LQR控制技術(shù).Naeem等人[18]提出的是一種PID方法，更多的控制方式見(jiàn)文獻(xiàn)[26].

隨著計(jì)算機(jī)計(jì)算能力的增強(qiáng)，使得先進(jìn)的控制器設(shè)計(jì)成為可能.這種情況下出現(xiàn)的一個(gè)突出的控制策略是模型預(yù)測(cè)控制(MPC).回溯到20世紀(jì)70年代末，模型預(yù)測(cè)控制最初的構(gòu)想是用來(lái)控制化學(xué)工業(yè)過(guò)程[27]，它通常具有緩慢的動(dòng)力學(xué)和以秒或分鐘計(jì)算的采樣時(shí)間.由于高性能處理器的使用，MPC已經(jīng)被應(yīng)用于快速系統(tǒng).

MPC的策略是在預(yù)測(cè)范圍內(nèi)計(jì)算一個(gè)最優(yōu)控制序列，通過(guò)最小化某些給定的成本函數(shù)來(lái)表示控制目標(biāo).它依賴(lài)于系統(tǒng)的動(dòng)態(tài)模型來(lái)預(yù)測(cè)它對(duì)控制輸入的響應(yīng).MPC的一個(gè)重要特性是它的約束處理能力，它使系統(tǒng)狀態(tài)和控制輸入保持在一個(gè)理想的操作區(qū)域內(nèi)，所涉及的優(yōu)化問(wèn)題很大程度上取決于成本函數(shù)和約束的公式.如果成本函數(shù)是二次方程，約束條件是一個(gè)凸集，那么得到的優(yōu)化將是一個(gè)二次規(guī)劃問(wèn)題(QP)，這是MPC公式中的一種常見(jiàn)的方法[19].

本文通過(guò)對(duì)USV建模，提出了估計(jì)USV在全局坐標(biāo)系下的位置和計(jì)算航向角參考值的算法，并將其應(yīng)用到模型預(yù)測(cè)控制方案中，滿(mǎn)足海風(fēng)擾動(dòng)的約束，該模型預(yù)測(cè)控制方法被用于解決USV跟蹤目標(biāo)點(diǎn)問(wèn)題.

1 水上無(wú)人艇模型

1.1 模型構(gòu)建

本文研究的水上無(wú)人艇(USV)是一個(gè)雙體船的結(jié)構(gòu)[28]，在每個(gè)船體的末端有兩個(gè)螺旋槳.該模型的行進(jìn)和航向轉(zhuǎn)變是由電機(jī)轉(zhuǎn)速產(chǎn)生的推進(jìn)力和扭矩來(lái)實(shí)現(xiàn)的.系統(tǒng)中唯一的執(zhí)行器是兩個(gè)馬達(dá)，當(dāng)兩個(gè)馬達(dá)以相同的速度旋轉(zhuǎn)時(shí)，水上無(wú)人艇直行，速度不同時(shí)，則會(huì)產(chǎn)生扭矩，使水上無(wú)人艇轉(zhuǎn)變航向.圖1 所示為無(wú)人艇的模型視圖，F(xiàn)1、F2為馬達(dá)產(chǎn)生的推力，F(xiàn)v為行駛的阻力，θ為航向角，w和h分別為艇的寬度與長(zhǎng)度.

圖1 水上無(wú)人艇的結(jié)構(gòu)模型

假設(shè)水上無(wú)人艇在它的局部坐標(biāo)系XOY中是一個(gè)具有2個(gè)自由度的剛體，利用牛頓第二定律, 可以推導(dǎo)出它的直行方程和角加速度方程為

(1)

(2)

式中：m表示水上無(wú)人艇的質(zhì)量；k表示阻力系數(shù)；I表示慣性矩；F1、F2表示螺旋槳產(chǎn)生的推力.水上無(wú)人艇的參數(shù)見(jiàn)表1.

(3)

表1 水上無(wú)人艇的參數(shù)

1.2 航跡推算

上節(jié)討論了無(wú)人艇在局部坐標(biāo)中描述的變量.但是，在無(wú)人艇跟蹤目標(biāo)時(shí)，需要估計(jì)或測(cè)量無(wú)人艇在全局坐標(biāo)系的位置，以便將其推向目標(biāo)點(diǎn).為了實(shí)現(xiàn)這一點(diǎn)，本文采用了航跡推算的技術(shù)[29].

(4)

將這組非線(xiàn)性狀態(tài)方程沿著無(wú)人艇的當(dāng)前航向角線(xiàn)性化，得到

(5)

利用上述狀態(tài)空間模型就能估計(jì)無(wú)人艇在全局坐標(biāo)系下的地理位置.在實(shí)際應(yīng)用中，可以將GPS、指南針獲取的無(wú)人艇的地理位置數(shù)據(jù)和該模型獲得的數(shù)據(jù)進(jìn)行耦合，可以開(kāi)發(fā)卡爾曼濾波器以便更好地估計(jì)位置.

1.3 航向角參考值計(jì)算

在使用MPC算法時(shí)，需要預(yù)測(cè)Np步的航向角θ參考值，便于非線(xiàn)性系統(tǒng)線(xiàn)性化.因?yàn)樵撍惴ǖ哪繕?biāo)是提供一個(gè)角度參考來(lái)了解無(wú)人艇的位置，所以，需要一個(gè)角度參考，而速度參考只是一個(gè)常數(shù).

該算法工作方式相對(duì)簡(jiǎn)單，算法的流程如下：

如果i

步驟 1：計(jì)算無(wú)人艇的當(dāng)前位置與下個(gè)目標(biāo)點(diǎn)的角度；

步驟 2：在步驟1計(jì)算的角度基礎(chǔ)上創(chuàng)建一個(gè)虛擬線(xiàn)性化的狀態(tài)空間模型；

步驟 3：在這個(gè)虛擬的狀態(tài)空間模型中提供兩個(gè)相等的推力，以獲得所需的前進(jìn)速度；

步驟 4：將計(jì)算的角度保存到模型預(yù)測(cè)控制的角度參考向量中；

步驟 5：如果無(wú)人艇的位置與設(shè)定的目標(biāo)點(diǎn)的距離小于0.1 m，表示已到達(dá)目標(biāo)點(diǎn)，則考慮下一個(gè)目標(biāo)點(diǎn)，否則i加1，重復(fù)步驟1；

結(jié)束.

2 模型預(yù)測(cè)控制算法

2.1 狀態(tài)預(yù)測(cè)

假設(shè)無(wú)人艇在k時(shí)刻的狀態(tài)已知，由式(5)重復(fù)迭代可以預(yù)測(cè)出時(shí)刻k+j的狀態(tài)為

(6)

現(xiàn)定義

可得

Xs(k)=Txk+HUs(k),

(7)

其中

2.2 代價(jià)函數(shù)

代價(jià)函數(shù)的控制目標(biāo)是通過(guò)調(diào)節(jié)控制輸入使系統(tǒng)的輸出驅(qū)動(dòng)到預(yù)定的軌跡，這些被調(diào)節(jié)的輸出可以是狀態(tài)變量或它的線(xiàn)性組合，因此定義

Yk=Crxk,

其中，Y∈Rny表示由矩陣定義的受控輸出向量.此外，期望的軌跡表示Yref∈Rny.

代價(jià)函數(shù)根據(jù)未來(lái)的輸入序列Us(k)來(lái)表達(dá)輸出跟蹤期望目標(biāo)點(diǎn)，其定義[27]為

(8)

式中：第一項(xiàng)表示未來(lái)Np步跟蹤誤差的總和；第二項(xiàng)表示所需要的控制輸入總和；權(quán)重矩陣Qy>0,Ru>0根據(jù)調(diào)節(jié)的目標(biāo)性能來(lái)選擇.式(8)可寫(xiě)為

(9)

其中

S=dig[Cr,…,Cr],

Q=dig[Qy,…,Qy],

R=dig[Ru,…,Ru],

F=H(SQS+R)H,

2.3 約 束

MPC的主要優(yōu)點(diǎn)是可以處理約束，這樣可以應(yīng)對(duì)系統(tǒng)固有的限制，如執(zhí)行器飽和、操作限制或安全問(wèn)題.這里，約束的輸出變量為

對(duì)于未來(lái)的預(yù)測(cè)時(shí)域i∈[1,…,Np]

這些約束能被寫(xiě)成向量形式

預(yù)測(cè)Np步后的輸出約束方程可以重寫(xiě)為

(10)

其中

S=dig[Cc,…,Cc],

Ac=dig[Acone,…,Acone],

bc=[bcone,…,bcone]T.

結(jié)合式(7)，該約束可以重寫(xiě)為

由于馬達(dá)的轉(zhuǎn)速被限制，輸入約束為

Umin≤Us≤Umax.

2.4 最優(yōu)化問(wèn)題

根據(jù)2.1～2.3中的結(jié)果，本文MPC算法所需要的最優(yōu)化問(wèn)題如下

(11)

(12)

Umin≤Us≤Umax.

(13)

由上可知，目標(biāo)函數(shù)(11)為二次型，約束(12)、(13)為線(xiàn)性約束，因此，式(11)～(13)是一個(gè)QP問(wèn)題，可以用現(xiàn)有的工具包來(lái)求解，例如qpOASES包.

3 仿真實(shí)驗(yàn)結(jié)果

通過(guò)仿真來(lái)檢驗(yàn)水上無(wú)人艇USV模型對(duì)目標(biāo)點(diǎn)跟蹤的性能，仿真使用的模型參數(shù)值如表1 所示.目標(biāo)點(diǎn)設(shè)計(jì)為一個(gè)五角星模型，其在XOY坐標(biāo)系下的坐標(biāo)為：

(0，0)→(10，0)→(15.9，18.1)→(0.5，6.9)→(19.5，6.9)→(4.1，18.1)→(10，0).

圖2 USV跟蹤目標(biāo)點(diǎn)的軌跡

設(shè)水上無(wú)人艇USV的初始狀態(tài)為Xk=[001.418 100 00]，采樣周期τ=0.1 s，預(yù)測(cè)時(shí)域Np=35，為了評(píng)估這些輸出和控制輸入的偏差，規(guī)定了以下加權(quán)矩陣

Qy=dig[2,1],Ru=I2.

為了加強(qiáng)USV運(yùn)行的穩(wěn)定性，對(duì)其航向角、直行速度和控制輸入進(jìn)行了約束

為了解決式(11)～(13)涉及的QP問(wèn)題，本文使用開(kāi)源軟件包qpOASES，如文獻(xiàn)[30]所述，該求解器有一種特別適用于MPC應(yīng)用的數(shù)據(jù)包.

圖2 為USV跟蹤目標(biāo)點(diǎn)軌跡的仿真結(jié)果.從圖2(a)可知，某些情況下，LQR算法生成的軌跡沒(méi)有達(dá)到設(shè)定點(diǎn)，從圖2(b)可知，MPC算法生成的軌跡基本達(dá)到設(shè)定點(diǎn)，根據(jù)仿真結(jié)果還可知，LQR算法軌跡偏差遠(yuǎn)大于MPC算法生成的軌跡的偏差.在達(dá)到了跟蹤目標(biāo)點(diǎn)的效果，MPC算法以一種相當(dāng)優(yōu)化的方式驅(qū)動(dòng)USV路徑所有的目標(biāo)點(diǎn).圖3 為USV前進(jìn)速度的仿真，可以看出模型的前進(jìn)速度比較平穩(wěn).圖4 為USV航向角的仿真，可以看出實(shí)際的航向角基本跟蹤了虛擬計(jì)算的參考值.

圖3 USV的前進(jìn)速度

圖4 USV的航向角

4 結(jié) 論

針對(duì)水上無(wú)人艇(USV)的目標(biāo)點(diǎn)跟蹤問(wèn)題，在估算出航向角的基礎(chǔ)上提出了一種模型預(yù)測(cè)控制器.對(duì)于采用模型預(yù)測(cè)技術(shù)進(jìn)行USV控制的相關(guān)工作，該方案的主要貢獻(xiàn)在于將非線(xiàn)性的模型，利用航向角的參考轉(zhuǎn)化為線(xiàn)性的模型預(yù)測(cè)控制模型，使用的方法是先估算出USV的位置和航向角參考，再利用模型預(yù)測(cè)控制處理約束的能力，以確保系統(tǒng)的穩(wěn)定性.研究結(jié)果表明，本文提出的方法可以完成USV跟蹤目標(biāo)點(diǎn)任務(wù).