鐘雨軒，陳曉虎，馬向峰

(1.上海大學 機電工程與自動化學院，上海 200444；2.中國船舶工業(yè)系統(tǒng)工程研究院，北京100094)

0 引 言

目前，無人艇（Unmanned Surface Vessel，USV）的應用還集中在水文測量和環(huán)保領域[1]，軍事方面[2]的應用還較少。執(zhí)行水文測量和環(huán)保領域任務時，USV一般以較低速度（＜10 kn）航行。而軍事方面很多時候需要USV 滿足高速[3-4]（＞20 kn）緊急避障性能要求。莊肖波等[5]提出基于蟻群算法（ACA）[6]實現(xiàn)高速無人艇對運動目標避碰規(guī)劃的方法。但蟻群算法計算量大，求解時間長，在實際應用中，由于循環(huán)次數(shù)有限，很難得到全局最優(yōu)解。吉大海等[7]提出一種基于行為的動態(tài)障礙物危險規(guī)避算法。然而該避障算法只從運動學層面上分析了動態(tài)障礙物約束，而未考慮船體動力學對避障性能的影響；該算法中避障被視為獨立于控制的問題，容易出現(xiàn)控制無法達到路徑規(guī)劃期望值的問題。Bj?rn-Olav Holtung Eriksen 等[8]設計并實施了基于非線性規(guī)劃（Nonlinear Programming，NLP）的中層COLAV 算法，可避開靜態(tài)和動態(tài)障礙物。但該算法屬于策略式規(guī)劃控制，沒有應用到反應式避障控制。

高速場景對避障的實時性有更高要求。在傳統(tǒng)算法架構中，避障被視為獨立于控制的問題，易出現(xiàn)控制無法達到路徑規(guī)劃期望值的問題。無人艇控制系統(tǒng)是多輸入多輸出系統(tǒng)，無人艇避障問題屬于多變量約束優(yōu)化問題，如采用PID 控制，需要設計多個控制器，且無法兼顧多個輸入輸出量間的耦合關系，控制效果較差。而模型預測控制[9]是處理多變量約束優(yōu)化問題最有效的方法之一。魯棒控制、最優(yōu)控制、自適應控制等也是處理多變量約束優(yōu)化問題有效的方法，但這些方法對船體模型精度要求很高。而模型預測控制采用了近似測量和計算，能在每個步長內(nèi)不斷滾動優(yōu)化，能夠補償由于模型失配、時變、干擾等引起的不確定性，及時補償，始終將新的優(yōu)化建立在實際的基礎上，使控制保持實際的最優(yōu)。為此，本文設計了基于模型預測控制（Model Predictive Control，MPC）的USV 速度障礙物（Velocity Obstacle，VO）避障算法（以下簡稱MPC_VO 算法）。通過仿真實驗，驗證了該算法適用于高速緊急避障場景，并分析了模型不確定對該算法避障效果的影響。

1 MPC_VO 算法設計

1.1 MPC 控制算法

MPC 控制[10]原理如圖1 所示。k 軸為當前狀態(tài)，左側為過去狀態(tài)，右側為將來狀態(tài)。

MPC 控制算法有多種不同的表達式，但都有著相同的基本結構，如圖2 所示。其基本特點[11]都是基于模型的預測、滾動優(yōu)化和前饋-反饋控制結構。

其中： ysp為 系統(tǒng)設定輸出； yr為參考軌跡；u 為控制量輸入； y為實際輸出值； ym為模型輸出值； yc為預測輸出。

圖 1 MPC 原理圖Fig.1 The principle of model predictive control

圖 2 MPC 原理框圖Fig.2 The control structure of model predictive control

本文主要用狀態(tài)空間模型描述系統(tǒng)。狀態(tài)空間的非線性離散時間模型的表達式如下：

狀態(tài)空間的線性連續(xù)時間模型的表達式如下：

其中： X(t)為 n 維的狀態(tài)向量； y(t) 為 p維的輸出向量，u(t) 為 m 維的控制向量，是輸入向量； e(t)為誤差量。A,B,C,D,K為系統(tǒng)矩陣。

對式（3）和式（4）進行一階歐拉離散化，得到狀態(tài)空間的線性離散時間模型，其表達式如下：

其中：X（k）為離散的n 維的狀態(tài)向量；y（k+1）為離散的p 維的輸出向量；u（k）為離散的m 維控制向量，是輸入向量； e(k)為誤差量，A，B，C，D，K 為系統(tǒng)矩陣。

把離散模型作為預測模型，迭代得到如下預測狀態(tài)：

其中： Np為 預測時域， Nc為控制時域，一般 Nc＜Np。

1.2 基于VO 分析的代價函數(shù)構造

在高速緊急避障場景，有效避開障礙物和減小能耗是很必要的。因此，可以構造避障和減小能耗的代價函數(shù)，把高速緊急避障問題轉換為滿足一定約束條件的數(shù)值優(yōu)化問題，可以VO 法基本原理建立VO 避障評價體系。VO 算法是反應式避障算法，要求計算要求低，應對環(huán)境的變化快，所以沒有參考軌跡，控制向量也沒有參考值。另外，考慮能量效率，傾向于采用小的加速度。因此，基于避障和能耗效率構造代價函數(shù)：

其中： Jvo為避障代價函數(shù)； Je為 能耗代價函數(shù)； λ1和λ2為權重系數(shù)。

為此，可以構造狀態(tài)量的代價函數(shù)，VO 法原理如圖3 所示。

圖 3 VO 法構造代價函數(shù)原理圖Fig.3 The principle of velocity obstacle

在相對空間中，以正北方向為基準，USV（A）與障礙物（B）位置的連線和正北方向的夾角為，相對速度和正北方向的夾角為。構造避障代價函數(shù) Jvo為：

能耗代價函數(shù) Je為 ：

uk為當前時刻的船體控制量。

權重系數(shù)是調(diào)節(jié)多項代價函數(shù)之間的權重的。由于在高速緊急避障場景中，有效避開障礙物是最重要的，能耗效率其次；又通過計算發(fā)現(xiàn) Je的 量級是 Jvo的106倍，為突出 Jvo的影響，選擇 λ1=108,λ2=1。

nstep 為預測步長。MPC 控制是以nstep 為單位滾動優(yōu)化，nstep 越長，求得的解越接近于全局解，但模型失配、時變、干擾等引起的不確定性會更加明顯。反之，nstep 越短，求得的解將為次優(yōu)解，但模型失配、時變、干擾等引起的不確定性會不明顯。仿真實驗取nstep=20。MPC 控制采用近似測量與計算，不是實現(xiàn)整個控制輸入序列，因為實際產(chǎn)生的USV 狀態(tài)與模型有很大差異，所以是在每個時間步不斷地重新評估控制輸入的最優(yōu)序列。

障礙物信息可以通過USV 搭載的傳感器探測到，

其中：

因此，可以把帶有約束條件的模型預測控制問題轉化為目標函數(shù)式和約束條件式的二次規(guī)劃問題。

2 MPC_VO 算法避障效果

在當障礙物（簡稱OBS.）在USV 的正上方且兩者對遇行駛場景（見圖4）中，USV 的起始速度為16 kn，OBS.的速度為20 kn。

圖 4 對遇場景Fig.4 Avoidance process of head on scene

從避障效果圖4 和圖5 中USV 航跡角的變化圖與USV 與OBS.距離的變化圖可以看出，USV 向左側轉向避開OBS.，距離保持在安全閾值之上，滿足安全避障要求。

圖 5 對遇場景航跡角和距離的變化Fig.5 Variation trend of angle and distance in head on scene

在當USV 與OBS.交叉相遇的行駛場景（見圖6）中，USV 的起始速度為16 kn，OBS.的速度為18 kn。

從圖6 和圖7 中USV 航跡角的變化圖與USV 與OBS.距離的變化圖可以看出，USV 與OBS.距離是安全距離閾值的3 倍左右，滿足安全避障要求，且安全系數(shù)高。航跡角變化率很大，滿足迅速緊急避障要求。

多運動障礙物避障仿真如圖8 所示，USV 起始速度為16 kn，OBS.1 速度為20 kn，OBS.2 速度為25 kn，OBS.3 速度為16 kn。

從避障效果圖8 和圖9 中USV 航跡角的變化圖與USV 與OBS.距離的變化圖可以看出，USV 與3 個OBS.距離在安全距離閾值之上，滿足安全避障要求。每次遇到障礙物時航跡角變化率都很大，滿足迅速緊急避障要求。

3 MPC_VO 算法與VO 算法避障性能比較分析

分析MPC_VO 算法與傳統(tǒng)VO 算法的避障效果，在對遇場景中，設計了與圖4 相同的場景和初始條件。傳統(tǒng)的VO 算法避障效果如圖10 所示。

從避障效果圖10（b）和圖10（c）與圖11（b）距離圖可以看出，USV 與OBS.發(fā)生碰撞。

在交叉場景中，設計了與圖6 相同的場景與初始條件。傳統(tǒng)的VO 算法的避障效果如圖12 所示?？梢钥闯?，USV 成功避開了OBS.。但從圖13 可以發(fā)現(xiàn)，USV 在避障時航跡角左右擺動，避障效果較差，且U S V 到O B S.的距離較小，比同等條件下的MPC_VO 算法危險。

圖 6 左交叉場景Fig.6 Avoidance process of cross from left scene

圖 7 左交叉場景Fig.7 Variation trend of angle and distance in cross from left scene

圖 8 多障礙物場景Fig.8 Avoidance process of multi-obstacle scene

圖 9 右交叉場景Fig.9 Variation trend of angle and distance in multi-obstacle scene

綜上所述，在高速場景中，傳統(tǒng)的VO 算法存在避不開障礙物或避障時首向搖擺不定避障效果較差的情況，而MPC_VO 算法在相同場景中表現(xiàn)較好。

4 模型不確定性對算法的性能影響分析

為了分析模型不確定性對MPC_VO 避障算法的影響，在圖8 多障礙物場景中分別驗證船體的質(zhì)量m，船體的轉動慣量Iz，船體的長度L，線性阻尼項和非線性阻尼項對避障性能的影響。

圖 10 對遇場景VO 避障效果圖Fig.10 Avoidance process of head on scene using VO method

圖 11 對遇場景Fig.11 Variation trend of angle and distance in head on scene using VO method

圖 12 左交叉場景VO 避障效果圖Fig.12 Avoidance process of cross from left scene using VO method

圖 13 左交叉場景Fig.13 Variation trend of angle and distance in cross from left scene using VO method

圖14 和圖15 分別是改變質(zhì)量m 后的模型和原始模型的避障航跡角和X Y 坐標。可以看出，質(zhì)量m±20%的模型避障輸出和原始模型避障輸出位置點和航跡角都非常接近，且從圖15（a）可以看出質(zhì)量m 增大20%X 坐標和原始模型的X 坐標幾乎重合。因此，船體質(zhì)量m 的不確定性對MPC_VO 避障算法的避障效果影響不顯著。

圖 14 改變質(zhì)量m 的模型與原始模型避障航跡角Fig.14 Avoidance angle of changed weight model and initial model

圖16 和圖17 分別是改變轉動慣量Iz后的模型和原始模型的避障航跡角及XY 坐標。從圖17（a）可以看出轉動慣量Iz減小20%的X 坐標和原始模型的X 坐標幾乎重合。在圖16 中，雖然轉動慣量Iz增大20%的模型在避開第2 個障礙物時的航跡角和原始模型避障航跡角偏差較大，但不超過5°。因此，船體的轉動慣量Iz的不確定性對MPC_VO 避障算法的避障效果影響不顯著。

圖 15 改變質(zhì)量m 的模型與原始模型避障XY 坐標Fig.15 X and Y coordinate of changed weight model and initial model

圖 16 改變轉動慣量Iz 的模型與原始模型避障航跡角Fig.16 Avoidance angle of changed rotational inertia model and initial model

圖18 和圖19 分別是改變船長L 后的模型和原始模型的避障航跡角和XY 坐標。從圖18 改變船長L 后的避障航跡圖可以看出，船長L 減小20%的模型在避開第2 個障礙物時的航跡角和原始模型避障航跡角偏差較大，最大角度偏差為10°左右。從圖19 圖（b）可以看出船長L 減小20%的模型與原始模型的輸出位置Y 坐標最大偏差為4 m 左右。因此，船長L 的不確定性對MPC_VO 避障算法的避障效果影響顯著些。

圖20 和21 分別是改變線性阻尼項和非線性阻尼項的模型和原始模型避障航跡角及XY 坐標。從圖20可以看出，改變線性阻尼項的模型與原始模型避障航跡角最大偏差有25°左右，Y 坐標位置偏差有10 m 左右，改變非線性阻尼項后的模型與原始模型的避障航跡角最大偏差為5°左右。因此，船體的改變線性阻尼項和非線性阻尼項的不確定性對MPC_VO 避障算法的避障效果影響是比較顯著。

圖 17 改變轉動慣量Iz 的模型與原始模型避障XY 坐標Fig.17 X and Y coordinate of changed rotational inertia model and initial model

圖 18 改變船長L 的模型與原始模型避障航跡角Fig.18 Avoidance angle of changed boat length model and initial model

圖 19 改變船長L 的模型與原始模型避障XY 坐標Fig.19 X and Y coordinate of changed boat length model and initial model

圖 20 改變線性阻尼項和非線性阻尼項的模型與原始模型避障航跡角Fig.20 Avoidance angle of changed linear damping model and initial model

圖 21 改變線性阻尼項和非線性阻尼項的模型與原始模型避障XY 坐標Fig.21 X and Y coordinate of changed linear damping model and initial model

綜上所述，MPC_VO 算法不斷滾動優(yōu)化，能夠補償由于模型失配、時變、干擾等引起的不確定性，及時補償。

5 結 語

本文針對路徑規(guī)劃和控制分開設計的策略容易出現(xiàn)控制無法達到路徑規(guī)劃期望值的問題，融合局部路徑規(guī)劃和船體控制，設計了MPC_VO 算法。通過仿真實驗實現(xiàn)了該算法的避障功能，并將該算法與傳統(tǒng)VO 算法避障效果作對比，驗證了該算法的有效性。同時，通過分析模型不確定性對該避障算法性能的影響，證明了該算法的魯棒性。