吳　青　王　樂　柳晨光　初秀民

(武漢理工大學(xué)物流工程學(xué)院1)　武漢　430063)　(武漢理工大學(xué)ITS智能交通系統(tǒng)研究中心2)　武漢　430063)

(武漢理工大學(xué)能源與動力工程學(xué)院3)　武漢　430063)　(國家水運安全工程技術(shù)研究中心4)　武漢　430063)

?

基于MPC的無人船運動控制及可視化仿真系統(tǒng)實現(xiàn)*

吳青1,2,4)王樂1,2,4)柳晨光2,3,4)初秀民2,4)

(武漢理工大學(xué)物流工程學(xué)院1)武漢430063)(武漢理工大學(xué)ITS智能交通系統(tǒng)研究中心2)武漢430063)

(武漢理工大學(xué)能源與動力工程學(xué)院3)武漢430063)(國家水運安全工程技術(shù)研究中心4)武漢430063)

摘要：運動控制可視化對研究在環(huán)境干擾和自動避碰需求下的無人船運動控制具有重要意義.建立了風(fēng)浪干擾下的船舶運動數(shù)學(xué)模型，基于模型設(shè)定一系列約束，運用模型預(yù)測控制(model predictive control, MPC)算法對船舶運動軌跡進行控制.以Visual C++為開發(fā)環(huán)境，搭建了無人船運動可視化仿真平臺，實現(xiàn)了基于Matlab與VC混合編程的多約束MPC控制器，在軟件界面上能實時顯示船舶運動軌跡和船舶運動相關(guān)參數(shù)的動態(tài)變化.實驗結(jié)果表明，系統(tǒng)能夠較好地模擬船舶真實航行軌跡，并能根據(jù)不同船舶特性更改船舶運動參數(shù)，為無人船運動控制研究提供測試仿真平臺.

關(guān)鍵詞：無人船；MPC；Matlab；C++；運動控制；仿真系統(tǒng)

0引言

無人船因具有高出航率、低成本、惡劣情況下能夠替代人進行各種活動等優(yōu)點，已成為船舶未來重要發(fā)展趨勢.無人船軌跡跟蹤控制是實現(xiàn)船舶自主航行的關(guān)鍵，近十幾年來船舶軌跡跟蹤有眾多研究成果，包括理論分析、數(shù)值計算和實驗驗證等，從反步法到自適應(yīng)反步法、全狀態(tài)反饋、輸出反饋、滑?？刂?、視距(line of sight, LOS)再到神經(jīng)網(wǎng)絡(luò)控制、魯棒控制、MPC、和智能控制等算法來實現(xiàn)[1-5]，船舶軌跡跟蹤的控制效果越來越好.針對船舶軌跡控制可視化研究，采用的算法及技術(shù)主要有：將固定坐標(biāo)系下的跟蹤誤差重定義成新的動坐標(biāo)系下的跟蹤誤差，基于LyaPunov直接法和反步法設(shè)計了全驅(qū)動水面艇的航跡跟蹤控制器，運用VegaPrime和VC++6.0軟件作為開發(fā)環(huán)境進行開發(fā)，具有航跡跟蹤誤差在有限時間內(nèi)漸近收斂為0的優(yōu)點[6]；采用四階Runge-Kutta數(shù)值積分法進行系統(tǒng)航跡數(shù)學(xué)模型的求解，基于PID控制算法設(shè)計航向控制器，利用Multigen Creator及Vega的三維建模與仿真技術(shù)、基于COM的Matlab與C/C++的混合編程技術(shù)、在線仿真技術(shù)等,實現(xiàn)了參數(shù)在線調(diào)整、控制算法在線運算[7-8]；針對制導(dǎo)算法和最優(yōu)航向控制算法設(shè)計出一種經(jīng)濟實用的航跡控制算法，基于Matlab和VC++的混合編程技術(shù)，仿照真實的船舶控制環(huán)境，設(shè)計出一種單機非實時的船舶運動控制仿真系統(tǒng)，具有較高的仿真精度[9].

由上述研究現(xiàn)狀可見，MPC算法在處理非線性系統(tǒng)和輸入約束條件問題上具有一定的優(yōu)勢，其中最明顯的優(yōu)點是能在控制過程中增加多種約束，適于船舶運動控制過程多約束的特點[10]，因此本文選擇MPC作為實現(xiàn)無人船船舶控制的方法.本文基于MPC控制方法設(shè)計了一種考慮各種約束和環(huán)境干擾的船舶軌跡跟蹤控制器，并利用Matlab與VC++混合編程方式實現(xiàn)了對控制器跟蹤性能的仿真測試，并實時顯示船舶運動軌跡和運動過程中相關(guān)參數(shù)的實時變化情況，同時繪制軌跡誤差分析圖.

1風(fēng)浪干擾下的無人船模型建立

無人船運動情況復(fù)雜，分為6個自由度，本研究忽略垂蕩運動、橫搖運動,以及縱搖運動3個運動方向的自由度，僅討論前進運動、橫移運動和艏搖運動3自由度的平面運動問題.船舶運動受力情況見圖1.

圖1　船舶運動受力情況

風(fēng)浪干擾下的無人船模型：

(1)

2風(fēng)浪干擾模型

2.1規(guī)則波浪模型

求波浪的干擾力和力矩，需要詳細的船舶結(jié)構(gòu)數(shù)據(jù)資料，特別是型線圖，然后進行數(shù)值積分，計算較為復(fù)雜.在確保一定的精度的前提下，給出簡單的計算方法是十分必要的.根據(jù)建立規(guī)則波浪干擾力和力矩模型.

式中：a=ρg(1-e-kd)/k2；b=kL/2·cosχ；

其中：L為船長；B為船寬；d為船舶吃水；k為波數(shù)；h為波高；ωe為遭遇頻率；ω為波浪頻率；ρ為水密度；g為重力加速度；χ為遭遇角.

則風(fēng)浪干擾力與力矩τd=[Xwave;Ywave;Nwave].

2.2不規(guī)則波浪模型

不規(guī)則風(fēng)浪干擾力與力矩選取.

τd=Coe×rand(3,1)-0.5×Coe

(3)

式中：Coe為干擾系數(shù)；rand(·)為幅度為1的零均值隨機噪聲.

3基于MPC的軌跡跟蹤控制器設(shè)計

模型預(yù)測控制采用多步測試、在線滾動優(yōu)化性能指標(biāo)和反饋校正等控制策略，來克服受控對象建模誤差和結(jié)構(gòu)、參數(shù)與環(huán)境等不確定因素的影響，顯示出較高的魯棒性能和良好的應(yīng)用效果，控制原理見圖2.本文基于上述全驅(qū)動無人船狀態(tài)空間模型，進行離散化.

圖2　MPC控制原理圖

3.1狀態(tài)空間模型離散化

無人船全驅(qū)動運動模型可表示為狀態(tài)空間方程的形式，具體如下.

(4)

系數(shù)矩陣p1，p2為

輸出部分系數(shù)矩陣

(5)

由無人船運動狀態(tài)空間方程可知，無人船運動控制為非線性控制，此類問題通常有2種處理方式：(1)轉(zhuǎn)換成線性系統(tǒng)狀態(tài)方程后再離散化，應(yīng)用線性運動控制理論和方法進行運動控制；(2)直接離散化，應(yīng)用非線性運動控制理論和方法進行運動控制.

雖然第1種處理方式理論上控制精度不如第2種，但由于其具有計算速度快、控制方法成熟等優(yōu)點而被廣泛應(yīng)用于非線性系統(tǒng)問題處理中，因此本文選擇第1種方式進行控制器設(shè)計.

1) 非線性狀態(tài)方程線性化非線性狀態(tài)方程線性化方法采用泰勒展開法，忽略高階項.

非線性狀態(tài)方程

(6)

線性化狀態(tài)方程

(7)

2) 離散化式(5)狀態(tài)方程是連續(xù)的，不能直接用于模型預(yù)測控制器的設(shè)計，故要對其離散化處理.

離散化狀態(tài)方程：

(8)

3.2軌跡跟蹤控制器實現(xiàn)

1) 預(yù)測方程

(9)

則每步預(yù)測：

寫成矩陣的形式：

最終狀態(tài)預(yù)測矩陣：

(10)

式中：

參考軌跡序列設(shè)定以橢圓為例：

(11)

2) 目標(biāo)函數(shù)

目標(biāo)函數(shù)若要保證全驅(qū)動水面船舶能快速且平穩(wěn)地追蹤期望軌跡，需加入系統(tǒng)狀態(tài)量的偏差、控制量.目標(biāo)函數(shù)為

(12)

轉(zhuǎn)換成二次規(guī)劃的標(biāo)準(zhǔn)形式為

約束條件：

4可視化仿真系統(tǒng)設(shè)計

4.1模塊設(shè)計

仿真系統(tǒng)按實現(xiàn)功能劃分為多模塊，主要包括：系統(tǒng)仿真模塊、參數(shù)設(shè)置模塊、控制模塊、數(shù)據(jù)保存模塊、顯示模塊、軌跡誤差分析模塊.其中仿真模塊是核心，其余模塊與仿真模塊直接關(guān)聯(lián)，方便實現(xiàn)整個仿真過程.

圖3　仿真系統(tǒng)模塊設(shè)計

4.2程序流程

基于MPC算法的Matlab程序?qū)崿F(xiàn)流程圖見圖4.

圖4　MPC算法Matlab程序?qū)崿F(xiàn)流程圖

圖4表明，船舶運動控制系統(tǒng)下一采樣時刻的狀態(tài)輸出值，是取當(dāng)前采樣時刻值輸入到數(shù)學(xué)模型中進行運算得到的結(jié)果.

系統(tǒng)仿真的核心運算采用式(13)及約束條件編寫Matlab/m文件程序?qū)崿F(xiàn)，Matlab程序所承擔(dān)的運算任務(wù)僅是運算單個時刻的值，以此來保證仿真的實時性.

仿真系統(tǒng)的程序流程圖見5.

圖5　仿真系統(tǒng)程序流程圖

4.3仿真系統(tǒng)實現(xiàn)件

仿真軟件功能包含設(shè)置、開始、暫停、繼續(xù)、保存、清空、數(shù)據(jù)顯示及干擾、誤差曲線分析等.

為了驗證仿真系統(tǒng)的合理性和準(zhǔn)確性，采用已知實船進行仿真實驗.無人船參數(shù)參照挪威科技大學(xué)的無人船II號，船長L=1.255 m，質(zhì)量m=23.8 kg.每次預(yù)測步數(shù)Np為10，控制步數(shù)Nc為8，系統(tǒng)采樣周期Tsample為2 s.

運用VC++與Matlab混合編程進行仿真可得到無人船全驅(qū)動模型軌跡跟蹤仿真結(jié)果及誤差圖見圖6.

圖6　無人船軌跡跟蹤結(jié)果及誤差(規(guī)則波)

由圖6b)可知，波高h為0.06 m，全驅(qū)動的跟蹤誤差均方差約為0.44，表明具有較好的跟蹤效果.

若風(fēng)浪干擾程度不同，波浪周期T=28 s(風(fēng)引起的波浪周期一般為1～30 s),遭遇角χ=45°，跟蹤誤差結(jié)果見表1.

表1　不同環(huán)境干擾下軌跡跟蹤誤差結(jié)果分析

由于船模較小，因此抗風(fēng)浪性能有限，當(dāng)風(fēng)浪高為0.3 m及以上時，實際軌跡跟蹤不上理想軌跡，見圖7.

圖7　波高為0.3 m時的跟蹤效果

將能跟蹤上理想軌跡的誤差均方差繪成折線圖，見圖8.

圖8　不同環(huán)境干擾下軌跡跟蹤誤差結(jié)果分析

由仿真效果及誤差分析圖可知，基于MPC算法的軌跡跟蹤控制器跟蹤情況良好，無人船運動可視化仿真平臺能夠較好地實現(xiàn)無人船軌跡跟蹤仿真.

6結(jié) 束 語

本研究首先建立了無人船運動模型，并基于MPC算法設(shè)計了軌跡跟蹤控制器，運用VC++與Matlab混合編程搭建了無人船運動控制可視化仿真系統(tǒng)，并對仿真系統(tǒng)進行驗證，實現(xiàn)了數(shù)字仿真與可視化仿真的統(tǒng)一.

參 考 文 獻

[1]邱荷珍,王磊,王洪超.船舶軌跡跟蹤研究綜述[J].實驗室研究與探索,2014,33(4):4-8.

[2]ASHRAFIUON H, MUSKE K R, McNINCH L C. Review of nonlinear tracking and setpoint control approaches for autonomous under-actuated marine vehicles[C]. American Control Conference (ACC),American IEEE, 2010：5203-5211.

[3]王曉飛.基于解析模型預(yù)測控制的全驅(qū)動船舶路徑跟蹤控制研究[D].上海:上海交通大學(xué),2009.

[4]MOREIRA L, SOARES C G. Autonomous ship model to perform manoeuvring tests[J]. Journal of Maritime Research: JMR, 2011,8(2):29-46.

[5]DING F, WANG Y, WANG Y. Trajectory-tracking controller design of under-actuated surface vessels[C].OCEANS, IEEE, 2011:1-5.

[6]王國英.全驅(qū)動水面艇運動控制建模及三維仿真實現(xiàn)研究[D].哈爾濱:哈爾濱工程大學(xué),2011.

[7]趙桂梅.船舶航跡/航向控制系統(tǒng)可視化仿真[D].哈爾濱:哈爾濱工程大學(xué), 2010.

[8]陸灝銘,陳瑋.船舶運動可視化仿真平臺的設(shè)計與實現(xiàn)[J].計算機仿真,2012,29(8):277-281.

[9]楊樹仁.船舶航跡控制算法的研究與仿真[D].長春:吉林大學(xué),2014.

[10]ZHENG H, NEGENBORN R R, LODEWIJKS G. Trajectory tracking of autonomous vessels using model predictive control[C]. 19th IFAC World Congress (IFAC WC’14)， IFAC, 2014:8812-8818.

Motion Control and Visual Simulation System of Unmanned Surface Vessel Based on the Model Predictive Control

WU Qin1,2,4)WANG Le1,2,4)LIU Chenguang2,3,4)CHU Xiuming2,4)

(SchoolofLogisticEngineering,WuhanUniversityofTechnology,Wuhan430063,China)1)(IntelligentTransportSystemResearchCenter,WuhanUniversityofTechnology,Wuhan430063,China)2)(SchoolofEnergyandPowerEngineering,WuhanUniversityofTechnology,Wuhan430063,China)3)

(NationalEngineeringResearchCenterforWaterTransportSafety,Wuhan430063,China)4)

Abstract：Motion control visualization is of great significance to the research on motion control of unmanned surface vessel (USV) under the environmental disturbance and demand of the automatic collision avoidance. First, motion mathematical model of USV under the wind and wave disturbance is established, a series of constraints are set based on the model, and Model Predictive Control (MPC) algorithm is used to control the motion trajectory of USV. Using Visual C++ (VC) as the development platform, the visual simulation platform of USV motion is established. In addition, multi-constraint MPC controller based on the hybrid programming of Matlab and VC is developed, which can realize real-time display of the vessel motion trajectories and dynamic changes in parameters related to vessel motion on the software interface. The experimental results show that the system can relatively well simulate the true navigation track of full-actuated and under-actuated vessels, change the motion parameters of vessel according to different vessel features, and provide test simulation platform for the research on motion control of USV.

Key words：unmanned surface vessel (USV); model predictive control (MPC); Matlab; C++; motion control; simulation system

doi:10.3963/j.issn.2095-3844.2016.02.009

中圖法分類號：U674.942

收稿日期：2016-02-10

吳青(1962- )：女，碩士，教授、博士生導(dǎo)師，主要研究領(lǐng)域為交通安全控制與裝備

*交通運輸部科技成果推廣項目(2015326548030)、湖北省自然科學(xué)基金項目(2015CFA111)資助