嚴(yán)衛(wèi)生，王衛(wèi)國，高 劍，李勇強

（西北工業(yè)大學(xué) 航海學(xué)院，陜西 西安 710072）

多無人船編隊控制半實物仿真系統(tǒng)開發(fā)

嚴(yán)衛(wèi)生，王衛(wèi)國，高 劍，李勇強

（西北工業(yè)大學(xué) 航海學(xué)院，陜西 西安 710072）

設(shè)計并實現(xiàn)了用于模擬多無人船編隊的實時半實物仿真系統(tǒng)。該系統(tǒng)通過對電壓信號的數(shù)模模數(shù)轉(zhuǎn)換模擬電機控制信號的輸出和采集，通過串行通訊端口模擬測量設(shè)備的數(shù)據(jù)輸出和數(shù)據(jù)采集。利用PC104平臺搭建無人船控制器，使用基于VxWorks的實時控制軟件；利用工控機和VC++6.0環(huán)境搭建無人船仿真中心，實現(xiàn)無人船仿真過程數(shù)據(jù)的圖形化顯示。最后仿真實驗表明該系統(tǒng)可以良好的模擬多無人船的編隊行為，在驗證編隊算法和測試無人船嵌入式控制軟件方面有很強的實用性。

無人船；實時操作系統(tǒng)；編隊控制；半實物仿真

無人船（Unmanned Surface Vehicle，USV）作為開發(fā)和利用海洋的重要工具之一，憑借其模塊化、無人化、小型化、智能化等優(yōu)點，正受到越來越多國家的重視并投入研究。對于單個無人船難以完成復(fù)雜的任務(wù)，無人船的協(xié)同編隊顯得尤為重要。在一些具體任務(wù)中，維持適當(dāng)?shù)年犘文軌蛱岣吖ぷ餍?，加快對目?biāo)任務(wù)的完成，降低了系統(tǒng)消耗，同時又增強了無人船隊系統(tǒng)的魯棒性。目前，無人船編隊系統(tǒng)開發(fā)主要階段有：系統(tǒng)設(shè)計，控制器設(shè)計，控制算法實現(xiàn)及實驗驗證；當(dāng)實驗遇到問題時需要重新回到控制器設(shè)計環(huán)節(jié)重新設(shè)計。然而無人船編隊實驗代價大，過程受多方面因素影響，在開發(fā)初期，不宜通過實際編隊實驗去分析檢驗編隊系統(tǒng)設(shè)計。因此為了在開發(fā)初期更好的對系統(tǒng)進(jìn)行驗證和校準(zhǔn)，需要設(shè)計在實驗室可以更容易實現(xiàn)的半實物仿真系統(tǒng)，以半實物仿真代替實物實驗。同時，在Matlab仿真編隊算法時忽略了模塊間數(shù)據(jù)通信的延遲及精度丟失，這一問題可能導(dǎo)致在實際系統(tǒng)中編隊算法的發(fā)散，而半實物仿真可以彌補這一問題更有效的完成算法的驗證。因此，無人船編隊半實物仿真對系統(tǒng)設(shè)計的驗證和編隊算法的驗證具有重要意義[1-3]。

1 系統(tǒng)工作原理

雙推進(jìn)器無人船系統(tǒng)主要由后面的兩個推進(jìn)器控制船的運動，左右推進(jìn)器合力推動船前進(jìn)，而兩個推進(jìn)器推力不同時，控制船的轉(zhuǎn)向。無人船上控制器負(fù)責(zé)采集無人船運動狀態(tài)參數(shù)，根據(jù)控制目標(biāo)解算出無人船運動所需的實時推力和轉(zhuǎn)矩，轉(zhuǎn)化為相對應(yīng)的左右推進(jìn)器控制電壓并輸出出去驅(qū)動電機轉(zhuǎn)動從而控制無人船運動[4]。無人船運動狀態(tài)可由一組狀態(tài)參數(shù)表示，具體為：前向速度u，側(cè)向速度v，航向角角速度w，北向位置坐標(biāo)x，東向位置坐標(biāo)y，航向角Ψ。

編隊仿真系統(tǒng)中有一個運動仿真中心，一個遙控中心和多個無人船控制節(jié)點。其中，仿真中心完成多條無人船航行過程的動力學(xué)仿真，根據(jù)無人船運動的差分模型，以控制節(jié)點的推進(jìn)器控制電壓為輸入，解算無人船下一時刻狀態(tài)為輸出。在這個過程中，仿真中心接收各控制節(jié)點輸出的推進(jìn)器控制電壓，然后，將新的船體運動狀態(tài)參數(shù)以標(biāo)準(zhǔn)傳感器格式通過串口發(fā)送給各船的控制節(jié)點?？刂乒?jié)點完成對船上控制器的仿真，即包括接收傳感器數(shù)據(jù)，航行控制解算和控制輸出。多個控制節(jié)點通過串口以標(biāo)準(zhǔn)傳感器格式分別接收對應(yīng)船的狀態(tài)參數(shù)，參與到航行解算中去，得到各自船的推進(jìn)器控制電壓，并以模擬電壓形式輸出到仿真中心。這樣，仿真中心與多個控制節(jié)點構(gòu)成了一個完整的控制回路，在仿真中心將可以看到多個船只按照設(shè)定的運動軌跡航行，這樣的工作模式也稱為自主運動模式，既僅需要控制人員輸入控制目標(biāo)即可。

圖1 系統(tǒng)工作原理結(jié)構(gòu)圖Fig.1 Structure diagram of simulation system

遙控中心完成對各船運動的實時監(jiān)控和緊急控制的功能，在岸上可由控制人員直接參與控制船只的運動，這種情況下船工作在遙控運動模式。在這種模式中，遙控中心接收各控制節(jié)點發(fā)送過來的船運動狀態(tài)參數(shù)，然后將人工控制指令，轉(zhuǎn)化為對推進(jìn)器電壓的直接控制，發(fā)送到各控制節(jié)點。遙控中心與控制節(jié)點的通信需要構(gòu)建無線通信網(wǎng)絡(luò)來實現(xiàn)。系統(tǒng)工作原理結(jié)構(gòu)圖，如圖 1所示。

2 硬件設(shè)計

2.1 仿真中心

仿真中心需要完成顯示船體狀態(tài)，串口信息輸出和讀取模擬電壓值這3個功能。因此設(shè)計仿真中心由一臺工控機實現(xiàn)，為這臺工控機配置3個標(biāo)準(zhǔn)RS-232串口和一個PCI擴展的A/D，D/A擴展板，這里選用9112型擴展板，包含16路A/D通道，可以滿足需求[1]。

2.2 控制節(jié)點

控制節(jié)點需要完成傳感器數(shù)據(jù)接收，無線通信數(shù)據(jù)接收發(fā)送和電機電壓輸出等功能，其中，數(shù)據(jù)接收發(fā)送由串口實現(xiàn)，電壓以模擬電壓信號形式輸出。因此，設(shè)計控制節(jié)點由嵌入式PC104控制平臺實現(xiàn)，配置兩個RS-232串口，及兩路D/A輸出通道?？刂乒?jié)點結(jié)構(gòu)圖，如圖 2所示。

船上控制器PC104工控機掛載的設(shè)備有一個MTI-G傳感器，一個無線模塊和兩個電機調(diào)速模塊。MTI-G傳感器周期性的提供姿態(tài)，3個角速度信息和GPS位置信息，與控制器的接口為RS232；無線模塊周期性的將無人船的航行信息發(fā)送給遙控中心，并且實時接收遙控中心的控制指令，與控制器的接口為RS232；電機調(diào)速模塊根據(jù)控制器發(fā)出的模擬信號，控制電機轉(zhuǎn)動，與控制器的接口為D/A通道。

圖2 控制節(jié)點結(jié)構(gòu)圖Fig.2 Structure diagram of node system

2.3 遙控中心

遙控中心需要完成無線通信數(shù)據(jù)接收發(fā)送的功能，其中，數(shù)據(jù)接收發(fā)送由串口實現(xiàn)。因此，設(shè)計遙控中心由普通筆記本電腦實現(xiàn)，配置一個USB轉(zhuǎn)RS-232串口即可。

2.4 接口描述

仿真中心與控制節(jié)點有兩個接口，分別是標(biāo)準(zhǔn)串口和兩路D/A，A/D通道。其中，串口實現(xiàn)傳感器數(shù)據(jù)的模擬，兩路D/A，A/D通道實現(xiàn)兩路電機控制電壓傳輸?shù)哪M?？刂乒?jié)點與遙控中心有一個由無線模塊構(gòu)建的無線通信網(wǎng)絡(luò)實現(xiàn)數(shù)據(jù)交流，而這個通信接口在邏輯上也可簡化理解為串口通信。

3 軟件設(shè)計

3.1 仿真中心軟件

仿真中心軟件為VC++6.0下編寫的MFC程序，包含串口驅(qū)動，PCI9112擴展卡驅(qū)動和曲線繪圖組件等。半實物仿真系統(tǒng)要求仿真系統(tǒng)實時接收控制節(jié)點的輸入，并產(chǎn)生實時動態(tài)輸出，較高的實時性才可以保證半實物仿真系統(tǒng)的穩(wěn)定性和精度。本軟件采用了多媒體定時器（multimedia timer），保證模型迭代的實時性，定時精度為1 ms，無人船模型的仿真步長根據(jù)經(jīng)驗選擇為10 ms。對于傳感器的定時輸出，采用Visual C++的WM_TIMER消息映射實現(xiàn)。

對于無人船的仿真采用面向?qū)ο蟮能浖O(shè)計方法[5]，設(shè)計類CAutoUSV完成對無人船的運動學(xué)仿真，將無人船的所有運動狀態(tài)參數(shù)抽象出來封裝進(jìn)這個類中作為私有產(chǎn)量。這樣關(guān)于無人船的運動將完全由該類的內(nèi)部函數(shù)運算實現(xiàn)，分析無人船的特性可知，需要一個函數(shù)實現(xiàn)無人船在時間上的狀態(tài)遞推，解算下一時刻船的狀態(tài)，設(shè)計函數(shù)CAutoUSV：：onestep（）實現(xiàn)船的狀態(tài)遞推，既模擬船在時間上的運動。還需要設(shè)計函數(shù)CAutoUSV：：getStates（）實現(xiàn)無人船狀態(tài)的輸出，相當(dāng)于使用傳感器測量船的狀態(tài)。還需要設(shè)計函數(shù)CAutoUSV：：control（）函數(shù)實現(xiàn)外部控制的輸入，相當(dāng)于推進(jìn)器作用于船的運動。由于模型解算對于時間的要求很高，所以函數(shù)control（）和onestep（）需要在多媒體時鐘的響應(yīng)函數(shù)中執(zhí)行，而getStates（）需要在時鐘消息的響應(yīng)函數(shù)中執(zhí)行，獲得船的實時狀態(tài)，并以傳感器格式發(fā)送出去。

仿真中心軟件提供了良好的圖形界面，實時顯示各個無人船的位置曲線，可以直觀的看到仿真的效果。仿真軟件模擬的傳感器輸出格式如表 1所示。

3.2 控制節(jié)點軟件

控制節(jié)點軟件運行于船上控制器，其實時性要求很高，因此控制中心軟件采用VxWorks嵌入式實時多任務(wù)操作系統(tǒng)，進(jìn)行任務(wù)調(diào)度，以保證系統(tǒng)的實時性。船上軟件中的任務(wù)主要有主任務(wù)，控制指令接收任務(wù)，MTI-G數(shù)據(jù)采集任務(wù)，無線通信驅(qū)動模塊，電機控制任務(wù)。針對大量串口數(shù)據(jù)的收發(fā)，控制節(jié)點軟件設(shè)計了高速有效串口數(shù)據(jù)讀寫緩沖區(qū)，保證了串口數(shù)據(jù)的實時更新。同時把串口的I/O操作放在任務(wù)中去，與主控制任務(wù)分開，保證了軟件的穩(wěn)定性和實時性。

表1 MTI-G傳感器數(shù)據(jù)格式Tab.1 MTI-G sensor data format

對于電機控制，不能有劇烈的提速。因此，控制機設(shè)計了提速保障機制，即在單位時間內(nèi)設(shè)置提速上限，將過大的提速，逐漸提高上去，確保電機穩(wěn)定工作。

圖3 控制節(jié)點軟件流程圖Fig.3 Flow chart the software of node system

3.3 遙控中心軟件

遙控中心軟件工作在岸上的遙控計算機上，配備1個標(biāo)準(zhǔn)串口接無線模塊，與各船上的無線模塊構(gòu)建無線通信網(wǎng)絡(luò)，實時監(jiān)控兩條船的航行情況。并且在必要情況下通過無線模塊發(fā)出控制信息，直接控制船的航行。

遙控軟件接收控制節(jié)點發(fā)送過來的無人船航行狀態(tài)數(shù)據(jù)，其具體格式如表 1所示。同時遙控軟件發(fā)送到控制節(jié)點的數(shù)據(jù)格式如表 2所示。

表2 遙控軟件發(fā)送至控制節(jié)點的數(shù)據(jù)格Tab.2 Communication data format

4 實驗驗證

為了驗證本系統(tǒng)的實用性，進(jìn)行3條無人船的編隊運動控制實驗。本次實驗采用主從跟隨的編隊算法，設(shè)定一條船為主船，引領(lǐng)船隊的航行，跟蹤一條既定軌跡[6]，其余兩條為跟隨船，以三角性姿態(tài)跟隨主船運動。主船跟蹤的軌跡曲線為：y=5sin（0.02πx），三條船的初始坐標(biāo)為：主船（0，0），從船分別為（-50，-50），（0，50）。跟隨船以三角性編隊跟隨主船[7-8]，隊形保持為邊長為10的等邊三角形，以主船為頂點的中垂線時刻指向船的航向。實驗結(jié)果如圖 4所示。

圖4 三船編隊實驗結(jié)果圖Fig.4 Experimental Results Figure

實驗結(jié)果表明，本系統(tǒng)有足夠的穩(wěn)定性和實時性，可以滿足多無人船編隊仿真的要求。

5 結(jié) 論

針對無人船編隊系統(tǒng)開發(fā)的需要，本文設(shè)計并實現(xiàn)了一種用于模擬多無人船編隊的實時半實物仿真系統(tǒng)，該系統(tǒng)通過對電壓信號的數(shù)模模數(shù)轉(zhuǎn)換模擬電機控制信號的輸出和采集，通過串行通訊端口模擬測量設(shè)備的數(shù)據(jù)輸出和數(shù)據(jù)采集。利用PC104平臺搭建無人船控制器，使用基于VxWorks的實時控制軟件；利用工控機和VC++6.0環(huán)境搭建無人船仿真中心，實現(xiàn)無人船仿真過程數(shù)據(jù)的圖形化顯示。分析仿真實驗結(jié)果，可以良好的模擬多無人船的編隊行為。在驗證編隊算法和測試無人船嵌入式控制軟件方面有很強的使用價值。

[1] 高劍，嚴(yán)衛(wèi)生.一種用于AUV導(dǎo)航控制軟件開發(fā)與系統(tǒng)測試的半實物仿真系統(tǒng)[J].西北工業(yè)大學(xué)學(xué)報，2007，25（1）：87-91.

GAO Jian，YAN Wei-sheng.Improving hardware-in-the-Loop simulation system for AUV navigation and control software development and testing[J].Journal of Northwestern Polytechnical University，2007，25（1）：87-91.

[2] 段翀，謝壽生.某型渦扇發(fā)動機半實物仿真控制系統(tǒng)[J].推進(jìn)技術(shù)，2005，26（5）：434-438。

DUAN Chong，XIE Shou-sheng.Hardware-in-the-loop simulation of a turbofan aeroengine control system[J].Journal of Propulsion Technology，2005，26（5）：434：438

[3] 嚴(yán)衛(wèi)生，高劍.水下航行器導(dǎo)航與控制實時仿真系統(tǒng)[J].西北工業(yè)大學(xué)學(xué)報，2003，21（4）：423-437

YAN Wei-sheng，GAO Jian.Real-time navigation and control simulation system for a long-distance autonomous underwater vehicle[J].Journal of Northwestern Polytechnical University，2003，21（4）：423-437.

[4] Fossen Thor I.Marine control systems：Guidance，nacigation and control of ships，rigs and underwater vehicles[M].Trondheim，Norway：Marine Cybernetics，2002.

[5] Stephen Prata.C++ Primer Plus中文版[M].5版.北京：人民郵電出版社，2005.

[6] 高劍，劉富檣.欠驅(qū)動自主水面船的非線性路徑跟蹤控制[J].機器人，2012.34（3）：329-336.

GAO Jian，LIU Fu-qiang.Non-linear path following control of underactuated autonomous surface vehicles[J].Robot，2012，34（3）：329-336.

[7] 趙寧寧，徐德民.多自主水下航行器編隊控制系統(tǒng)設(shè)計[J].火力與指揮控制，2011，36（1）：154-159.

ZHAO Ning-ning，XU De-min.Coordinated control system design for multiple autonomous underwater vehicle[J].Fire Control and Command Control，2011，36（1）：154-159.

[8] 嚴(yán)衛(wèi)生，徐德民.自主水下航行器導(dǎo)航技術(shù)[J].火力與指揮控制，2004，29（6）：11-19.

YAN Wei-sheng，XU De-min.Surver of navigation technology for autonmous underwater vehicle[J].Fire Control ＆ Command Control，2004，29（6）：11-19.

Design and development of a simulation system for multi-USV formation control

YAN Wei-sheng，WANG Wei-guo，GAO Jian，LI Yong-qiang
（College of Marine Engineering，Northwestern Polytechnical University，Xi'an 710072，China）

A simulation system for multi-USV formation control is designed and accomplished in this paper.The system simulates the input and output of voltage signal which is used to control the motor by way of A/D and D/A conversion，and simulate the communication of sensor data via RS-232 port.PC104 platform and VxWorks real-time control software is used to build the USV's controller.The IPC and VC++6.0 software environment is utilized to build multi-USV simulation center，which will achieve the graphical display of USVs' states parameter.Finally Simulation results show that the system can properly and effectively simulate multi-USV formation.The system has a strong practicality in aspect of the validating of multi-USV formation algorithms and the testing of embedded control software.

USV；PC104；VxWorks；Formation control

TP302

A

1674-6236（2014）11-0043-04

2013-09-21 稿件編號：201309151

嚴(yán)衛(wèi)生（1968—），男，江蘇南通人，博士，教授。研究方向：水下航行器的制導(dǎo)、控制與仿真、水下目標(biāo)跟蹤、變結(jié)構(gòu)控制與智能控制。