基于MPC的無(wú)人船運(yùn)動(dòng)控制及可視化仿真系統(tǒng)實(shí)現(xiàn)*

吳　青　王　樂　柳晨光　初秀民

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

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

?

基于MPC的無(wú)人船運(yùn)動(dòng)控制及可視化仿真系統(tǒng)實(shí)現(xiàn)*

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

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

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

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

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

0引言

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

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

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

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

圖1　船舶運(yùn)動(dòng)受力情況

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

(1)

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

2.1規(guī)則波浪模型

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

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

其中：L為船長(zhǎng)；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的零均值隨機(jī)噪聲.

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

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

圖2　MPC控制原理圖

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

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

(4)

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

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

(5)

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

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

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

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

(6)

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

(7)

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

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

(8)

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

1) 預(yù)測(cè)方程

(9)

則每步預(yù)測(cè)：

寫成矩陣的形式：

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

(10)

式中：

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

(11)

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

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

(12)

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

約束條件：

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

4.1模塊設(shè)計(jì)

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

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

4.2程序流程

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

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

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

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

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

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

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

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

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

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

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

由圖6b)可知，波高h(yuǎn)為0.06 m，全驅(qū)動(dòng)的跟蹤誤差均方差約為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及以上時(shí)，實(shí)際軌跡跟蹤不上理想軌跡，見圖7.

圖7　波高為0.3 m時(shí)的跟蹤效果

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

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

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

6結(jié) 束 語(yǔ)

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

參 考 文 獻(xiàn)

[1]邱荷珍,王磊,王洪超.船舶軌跡跟蹤研究綜述[J].實(shí)驗(yàn)室研究與探索,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ù)測(cè)控制的全驅(qū)動(dòng)船舶路徑跟蹤控制研究[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]王國(guó)英.全驅(qū)動(dòng)水面艇運(yùn)動(dòng)控制建模及三維仿真實(shí)現(xiàn)研究[D].哈爾濱:哈爾濱工程大學(xué),2011.

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

[8]陸灝銘,陳瑋.船舶運(yùn)動(dòng)可視化仿真平臺(tái)的設(shè)計(jì)與實(shí)現(xiàn)[J].計(jì)算機(jī)仿真,2012,29(8):277-281.

[9]楊樹仁.船舶航跡控制算法的研究與仿真[D].長(zhǎng)春:吉林大學(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

中圖法分類號(hào)：U674.942

收稿日期：2016-02-10

吳青(1962- )：女，碩士，教授、博士生導(dǎo)師，主要研究領(lǐng)域?yàn)榻煌ò踩刂婆c裝備

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