差速轉(zhuǎn)向式雙推無人艇航向與航速聯(lián)合控制研究

袁 健

（齊魯工業(yè)大學(xué)（山東省科學(xué)院），海洋儀器儀表研究所，山東省海洋監(jiān)測(cè)儀器裝備技術(shù)重點(diǎn)實(shí)驗(yàn)室，國家海洋監(jiān)測(cè)設(shè)備工程技術(shù)研究中心，山東 青島 266100）

0 引言

無人水面艇（Unmanned Surface Vessel，USV）是一種智能化、自主化的海洋無人智能裝備，在水文環(huán)境探測(cè)和監(jiān)測(cè)、海洋測(cè)繪、海洋維權(quán)、敵情偵測(cè)等領(lǐng)域?qū)l(fā)揮重要作用.其配備先進(jìn)的供電系統(tǒng)、控制系統(tǒng)、傳感器系統(tǒng)、通信系統(tǒng)，可進(jìn)行大范圍海洋環(huán)境信息采集和監(jiān)測(cè)[1-2]，軍事方面可執(zhí)行多種戰(zhàn)爭和非戰(zhàn)爭軍事任務(wù)，如偵察、反潛、反特種作戰(zhàn)以及巡邏等作戰(zhàn)任務(wù)，在無人作戰(zhàn)系統(tǒng)占有重要位置，因此倍受世界各國的重視[3-5].面對(duì)復(fù)雜未知的海洋環(huán)境，如何設(shè)計(jì)一種穩(wěn)定可靠的無人艇自主控制系統(tǒng)，如何實(shí)現(xiàn)無人艇的自主航行控制以及自主導(dǎo)航策略是無人水面艇自主控制研究的關(guān)鍵技術(shù)[6-8]，也是當(dāng)下海洋無人系統(tǒng)智能控制的一個(gè)熱點(diǎn).無人艇航行時(shí)受到海浪、海風(fēng)等外擾動(dòng)的影響，是一個(gè)非線性、不確定性和動(dòng)態(tài)不穩(wěn)定性系統(tǒng)，因此，在這種情形下，基于無人艇動(dòng)態(tài)模型建模的控制律設(shè)計(jì)方法其實(shí)際的控制效果不佳.PID控制算法具有結(jié)構(gòu)簡單，魯棒性強(qiáng)的優(yōu)點(diǎn)，但當(dāng)無人艇動(dòng)態(tài)特性或所處環(huán)境發(fā)生變化時(shí)，其控制參數(shù)需進(jìn)行適應(yīng)性修訂，否則將導(dǎo)致控制效果變差，參數(shù)的修訂受到人員經(jīng)驗(yàn)、外界干擾等影響，不可控且低重復(fù)性，因此為提升傳統(tǒng)PID控制算法性能，增強(qiáng)其適應(yīng)性，需要對(duì)傳統(tǒng)PID控制算法進(jìn)行優(yōu)化改進(jìn).文獻(xiàn)[9]研究了具有不確定性擾動(dòng)的水面無人艇的航跡跟蹤控制，提出一種魯棒的軌跡跟蹤控制器設(shè)計(jì)方法用于處理外擾動(dòng)不確定情形下的控制設(shè)計(jì).文獻(xiàn)[10]提出一種修正的模糊神經(jīng)網(wǎng)絡(luò)方法，用于實(shí)現(xiàn)無人艇的航向控制.文獻(xiàn)[11]采用模糊控制方法設(shè)計(jì)了無人艇模糊控制規(guī)則和推理，實(shí)現(xiàn)了不依賴于動(dòng)力學(xué)模型的模糊PID航向控制算法，并應(yīng)用于無人艇航控系統(tǒng)試驗(yàn).文獻(xiàn)[12]針對(duì)無人艇航向控制中存在的模型參數(shù)和外界干擾不確定性問題，基于反步控制思想設(shè)計(jì)無人艇的航向控制律，提出一種由舵角控制輸入控制航向的自適應(yīng)航向控制算法，利用李雅普諾夫函數(shù)驗(yàn)證該控制算法能夠使得閉環(huán)控制系統(tǒng)一致有界.文獻(xiàn)[13]建立了無人艇操縱運(yùn)動(dòng)模型，設(shè)計(jì)了一種基于軌跡線性化的快慢回路時(shí)標(biāo)分離的無人艇航向控制方法，并將非線性干擾觀測(cè)器設(shè)計(jì)方法與軌跡線性化控制方法結(jié)合，提出基于非線性干擾觀測(cè)器的軌跡線性化航向控制方法.文獻(xiàn)[14]對(duì)無人艇的航向跟蹤控制系統(tǒng)和外部環(huán)干擾進(jìn)行了數(shù)學(xué)建模，并基于分?jǐn)?shù)階微積分方法設(shè)計(jì)了一種航向控制的分?jǐn)?shù)階控制器，進(jìn)一步給出了一種徑向基函數(shù)神經(jīng)網(wǎng)絡(luò)學(xué)習(xí)算法，實(shí)現(xiàn)了該控制器的參數(shù)整定.

本文針對(duì)一種雙電推的無人艇航向和航速聯(lián)合控制問題，研究對(duì)象為大容量鋰電池為能源的，兩個(gè)涵道式推進(jìn)器推進(jìn)的小型無人水面監(jiān)測(cè)艇，該無人艇的推進(jìn)裝置由2個(gè)對(duì)稱設(shè)置于無人艇底的，可以正反向轉(zhuǎn)動(dòng)的涵道式推進(jìn)器組成.雙推無人艇航向控制與舵控?zé)o人艇不同，其通過控制左右推進(jìn)器的轉(zhuǎn)速差進(jìn)行航行控制，轉(zhuǎn)速差大則無人艇轉(zhuǎn)向幅度大，轉(zhuǎn)速差小則無人艇轉(zhuǎn)向幅度小，很難精確確定轉(zhuǎn)速差與轉(zhuǎn)動(dòng)角度之間的關(guān)系式，另一方面，無人艇的精確動(dòng)力學(xué)模型建模也非常困難，因此我們采用模糊PID控制方法，模擬人工操控?zé)o人艇的操作模式，對(duì)操控規(guī)則采用模式語言進(jìn)行描述和模糊化處理，提出了航向和航速聯(lián)合控制的模糊PID閉環(huán)控制方法，通過構(gòu)建模糊控制規(guī)則表，并以無人艇航向的實(shí)際誤差及誤差變化率作為模糊控制的輸入，PID的參數(shù)作為模糊輸出，實(shí)現(xiàn)PID控制器控制參數(shù)自適應(yīng)調(diào)整，增強(qiáng)無人艇控制的環(huán)境應(yīng)變能力.進(jìn)一步根據(jù)系統(tǒng)控制流程編寫了閉環(huán)控制模塊軟件，并設(shè)計(jì)了下位機(jī)控制器和運(yùn)動(dòng)控制器的控制協(xié)議.最后通過岸邊實(shí)驗(yàn)獲取了該無人艇的速度控制因子，并通過仿真和岸邊實(shí)驗(yàn)驗(yàn)證了所提控制方法的有效性.

1 控制系統(tǒng)設(shè)計(jì)

該雙推無人艇的控制系統(tǒng)軟件包括上位機(jī)控制軟件、下位機(jī)主控制軟件、下位機(jī)運(yùn)動(dòng)控制軟件，控制結(jié)構(gòu)如圖1所示.上位機(jī)和下位機(jī)控制軟件分別安裝在兩塊PCM9310主板上，操作系統(tǒng)為ubuntu16.2，控制軟件采用MOOS-Ivp框架進(jìn)行開發(fā)，該軟件框架為MIT和Oxford機(jī)器人實(shí)驗(yàn)室共同開發(fā)和維護(hù)的基于C++的分布式機(jī)器人開源控制軟件系統(tǒng)框架[15].下位機(jī)運(yùn)動(dòng)控制器采用STM32F103ZC控制板.上位機(jī)與下位機(jī)主控制器之間通過無線網(wǎng)橋進(jìn)行通信連接，下位機(jī)主控制器與運(yùn)動(dòng)控制器之間通過RS323串口進(jìn)行數(shù)據(jù)通信.本文主要研究雙推無人艇航向與航速控制方法，主要涉及下位機(jī)控制軟件和運(yùn)動(dòng)控制器的算法和軟件設(shè)計(jì)，上位機(jī)的主要功能是實(shí)現(xiàn)用戶控制命令的下發(fā)和下位機(jī)采集數(shù)據(jù)的呈現(xiàn)，不在本文討論范圍之內(nèi).由于采用電子調(diào)速器控制推進(jìn)電機(jī)與采用伺服電機(jī)控制推進(jìn)電機(jī)的控制方式不同，電子調(diào)速器可以接受1 000-2 000 us的脈寬調(diào)制（PWM）信號(hào)，通過控制單片機(jī)的PWM引腳產(chǎn)生1 000-2 000 us的PWM信號(hào)輸出給電子調(diào)速器，電子調(diào)速器才能響應(yīng).其中1 000-1 500 us為電調(diào)控制直流無刷電機(jī)反轉(zhuǎn)信號(hào)，1 500 us到2 000 us為電調(diào)控制電機(jī)正轉(zhuǎn)信號(hào)，中值1 500 us為電機(jī)停止信號(hào)，并且PWM值與1 500 us的差值越大，則轉(zhuǎn)速越高，所以雙推無人艇航向控制與舵控?zé)o人艇不同，其通過控制左右推進(jìn)器的轉(zhuǎn)速差進(jìn)行航行控制，轉(zhuǎn)速差大則無人艇轉(zhuǎn)向幅度大，轉(zhuǎn)速差小則無人艇轉(zhuǎn)向幅度小，其關(guān)系式很難精確描述，為此我們針對(duì)雙推無人艇航速和航向聯(lián)合控制的需求，研究了MOOS-Ivp架構(gòu)下相應(yīng)軟件模塊的功能和系統(tǒng)組成，并配置了保證控制系統(tǒng)正常運(yùn)行所需的軟件模塊，提出了航向和航速聯(lián)合控制的模糊PID閉環(huán)控制方法.其采用星型組織架構(gòu)，以MoosDB為軟件體系的消息發(fā)布中心，其他模塊化相對(duì)獨(dú)立，并通過消息的訂閱和發(fā)布機(jī)制進(jìn)行組織的軟件架構(gòu)模式.該軟件架構(gòu)可以實(shí)現(xiàn)各個(gè)軟件模塊的靈活配置，自由增減，通過配置文件即可實(shí)現(xiàn)對(duì)軟件模塊的配置，非常適合于復(fù)雜環(huán)境下無人艇分布式控制軟件的研發(fā).

圖1 無人艇控制結(jié)構(gòu)

1.1 MOOS的核心模塊

MOOS-Ivp軟件模塊安裝在下位機(jī)主控制器PCM9310板上.該軟件運(yùn)行環(huán)境為ubuntu16.2，通過*.moos文件進(jìn)行系統(tǒng)的初始化參數(shù)配置，可以選擇程序需要啟動(dòng)的軟件模塊，以及實(shí)現(xiàn)對(duì)應(yīng)模塊的參數(shù)初始化配置，通過團(tuán)體名實(shí)現(xiàn)對(duì)模塊組的區(qū)分.本項(xiàng)目針對(duì)無人艇控制的具體需求，采用的MOOS軟件系統(tǒng)可以實(shí)現(xiàn)分布式控制，在上位機(jī)運(yùn)行一個(gè)MOOSDB模塊、一個(gè)可視化用戶界面（pMarineViewer）模塊；其中MOOSDB模塊需要給定其團(tuán)體名和IP地址，下位機(jī)需要啟動(dòng)的MOOS軟件模塊為MOOSDB，pHelmIvp，pLogger，pMarinePID，pNodeReporter，pShare，uMS，uSimMarine，pCompass、pAct等模塊.其中，MOOSDB為MOOS軟件架構(gòu)體系的核心，是其他模塊訂閱和分發(fā)消息的中心.pHelmIvp為系統(tǒng)的決策輸出模塊，通過采集uSimMarine模塊的輸出和上位機(jī)用戶輸入的設(shè)定速度和航向信息，并對(duì)輸入信息進(jìn)行決策規(guī)劃，輸出期望速度desired_speed和期望航向desired_heading.pLogger模塊以文件的形式實(shí)現(xiàn)對(duì)系統(tǒng)運(yùn)行過程中消息的全紀(jì)錄.pMarinePID為航速和航向PID控制模塊.pNodeReporter模塊設(shè)計(jì)無人艇節(jié)點(diǎn)相關(guān)信息的記錄.pShare通過UDP的方式實(shí)現(xiàn)兩個(gè)團(tuán)體之間的數(shù)據(jù)通信，需要設(shè)置其端口號(hào)，如input為9 200，output為9 201.uMS模塊可以對(duì)各個(gè)模塊發(fā)布的信息進(jìn)行統(tǒng)一的界面化現(xiàn)實(shí).uSimMarine為系統(tǒng)仿真模塊，通過系統(tǒng)仿真模塊模擬真實(shí)無人艇的導(dǎo)航速度、航向的輸出.pCompass主要實(shí)現(xiàn)組合導(dǎo)航模塊的航向和航速信息獲取，模塊通過USB轉(zhuǎn)232串口模塊采集電子羅盤的航向角和GPS輸出的航速值，將航向角標(biāo)準(zhǔn)化到0-360°范圍，航速單位轉(zhuǎn)換到m/s.并將采集的這些信息通過Notify（參數(shù)1，參數(shù)2）函數(shù)發(fā)布到MOOSDB模塊.pAct模塊一方面實(shí)現(xiàn)對(duì)運(yùn)動(dòng)控制器上傳的電壓、電流數(shù)據(jù)幀進(jìn)行分析處理和發(fā)布，一方面將pMarinePID模塊的輸出的期望PWM值（Desired_thrust）和期望轉(zhuǎn)動(dòng)角度（Desired_rudder）進(jìn)行打包裝幀處理，并通過串口將成幀數(shù)據(jù)發(fā)送到運(yùn)動(dòng)控制器.

1.2 下位機(jī)運(yùn)動(dòng)控制器

下位機(jī)運(yùn)動(dòng)控制器通過485接口實(shí)現(xiàn)對(duì)溫濕度、電壓電流模塊信息的采集，同時(shí)解析下位機(jī)主控器PCM9310下發(fā)的控制幀，解析出對(duì)應(yīng)字段信息繼而驅(qū)動(dòng)對(duì)應(yīng)的IO口和PWM信號(hào)輸出，將左右PWM信號(hào)輸出給對(duì)應(yīng)推進(jìn)器的電調(diào)進(jìn)行轉(zhuǎn)速控制.下位機(jī)運(yùn)動(dòng)控制器采用STM32F103ZC微處理器模塊，晶振：72 MHz，具有16路PWM輸出功能.本項(xiàng)目采用TIM3的2路PWM輸出端口對(duì)應(yīng)左右推進(jìn)器轉(zhuǎn)速的脈寬調(diào)制.由于所采用電調(diào)的工作頻率為50 Hz，所以在初始化程序中需要設(shè)置PWM通道的輸出頻率：PWM頻率為50 Hz，初始化PWM頻率的函數(shù)參數(shù)為（72 000 000，71，19 999），計(jì)算過程為：72 000 000/20 000/72＝50 Hz.

1.3 下位機(jī)主控制器與運(yùn)動(dòng)控制器的通信協(xié)議

該通信協(xié)議為下位機(jī)主控制器發(fā)送到運(yùn)動(dòng)控制器的協(xié)議，主要功能為實(shí)現(xiàn)對(duì)下位機(jī)主控制器pAct軟件模塊所封裝的控制信息下發(fā)給下位機(jī)運(yùn)動(dòng)控制器PCM9310板，如圖2所示.ASCII編碼方式，采用逗號(hào)分隔，其協(xié)議格式為：

圖2 下位主控制器與推進(jìn)器控制器之間的協(xié)議格式

其中，$USV—表示數(shù)據(jù)幀開始；

手動(dòng)自動(dòng)—取值為A和M，A表示自動(dòng)控制信號(hào)，M表示遙控器控制；

運(yùn)行指令—取值為0，1，2，3，4.0表示前進(jìn)，1表示停止，2表示后退，3表示左轉(zhuǎn)，4表示右轉(zhuǎn)；

左調(diào)速PWM—取值在1 000 us和2 000 us之間，其中1 000-1 500 us表示推進(jìn)器反轉(zhuǎn)，1 500-2 000 us表示推進(jìn)器正轉(zhuǎn)；

右調(diào)速PWM—取值在1 000 us和2 000 us之間，其中1 000-1 500 us表示推進(jìn)器反轉(zhuǎn)，1 500-2 000 us表示推進(jìn)器正轉(zhuǎn)；

CRC—CRC-16，UTF-8格式編碼；

這里給出一個(gè)示例，如：$USV，A，3，1 600，1 800，XX表示下位機(jī)主控制器發(fā)送的指令為的自動(dòng)控制指令，左轉(zhuǎn)，左推進(jìn)器PWM值為1 600 us，左推進(jìn)器PWM值為1 800 us，XX表示兩個(gè)字節(jié)的CRC.

2 控制原理

2.1 雙推無人艇航向模糊控制原理

雙推無人艇航向控制與舵控?zé)o人艇不同，其通過控制左右推進(jìn)器的轉(zhuǎn)速差進(jìn)行航行控制，轉(zhuǎn)速差大則無人艇轉(zhuǎn)向幅度大，轉(zhuǎn)速差小則無人艇轉(zhuǎn)向幅度小.對(duì)于有舵的無人艇而言，當(dāng)舵角為負(fù)值時(shí)，表示舵順指針轉(zhuǎn)動(dòng)，此時(shí)無人艇逆時(shí)針轉(zhuǎn)角.同理，當(dāng)舵角為正值時(shí)，表示舵逆指針轉(zhuǎn)動(dòng)，此時(shí)無人艇順時(shí)針轉(zhuǎn)角.Moos-ivp自帶MarinePID模塊，可以實(shí)現(xiàn)基本的PID控制，其根據(jù)模塊訂閱的航向角（NAV-heading）、航速（NAV-speed）、期望速度（Desired_speed）、期望航向（Desired_heading）等信息，產(chǎn)生期望PWM值（Desired_thrust）、期望轉(zhuǎn)動(dòng)角度（Desired_rudder）.傳統(tǒng)PID航向控制算法其控制原理為

其通過調(diào)節(jié)KP、KI、KD實(shí)現(xiàn)航向自動(dòng)控制，其中KP影響系統(tǒng)響應(yīng)速度和控制精度，過大易引起震蕩，過小則降低系統(tǒng)響應(yīng)速度；KI起到消除系統(tǒng)穩(wěn)態(tài)誤差的作用，過大會(huì)引起超調(diào)，過小則降低控制精度；KD用于預(yù)測(cè)偏差變化，適當(dāng)?shù)闹悼捎行嵘到y(tǒng)動(dòng)態(tài)特性.但當(dāng)無人艇動(dòng)態(tài)特性或所處環(huán)境發(fā)生變化時(shí)，其控制參數(shù)需進(jìn)行適應(yīng)性修訂，否則將導(dǎo)致航向控制效果變差，然而參數(shù)的整定受到人員經(jīng)驗(yàn)、外界干擾等影響，不可控且低重復(fù)性，因此為提升傳統(tǒng)PID航向控制算法性能，增強(qiáng)其適應(yīng)性，需要對(duì)傳統(tǒng)PID航向控制算法進(jìn)行優(yōu)化改進(jìn).所以由于海流、海浪和風(fēng)的影響導(dǎo)致系統(tǒng)的PID控制參數(shù)需要進(jìn)行適應(yīng)性調(diào)節(jié)，利用模糊控制方法根據(jù)航向角度誤差和角速度誤差實(shí)時(shí)調(diào)整PID三個(gè)參數(shù).基本的PID航向控制其適用于舵控的無人航行器，對(duì)于雙推差分調(diào)速的無人艇，由于沒有舵角控制信息，所以需要將產(chǎn)生的舵角控制信號(hào)區(qū)間映射到左右推進(jìn)器所需的PWM控制信號(hào)的區(qū)間上.但這一映射關(guān)系由于轉(zhuǎn)向角度和推進(jìn)器推力差之間以及航速之間的關(guān)系并不明確.因此需要實(shí)驗(yàn)研究這一映射關(guān)系.在同一個(gè)轉(zhuǎn)速推力差的前提下，設(shè)置不同的航速，進(jìn)行岸邊實(shí)驗(yàn)，得到不同航速下兩個(gè)推進(jìn)器轉(zhuǎn)速差值與轉(zhuǎn)向角度的關(guān)系曲線.為了使無人艇控制程序能適應(yīng)不同的海流、風(fēng)等擾動(dòng)的影響，適應(yīng)性調(diào)節(jié)PID算法的控制參數(shù)，采用模糊控制方法，以航行偏差和偏差率作為模糊控制器的輸入，PID的比例、積分和微分系數(shù)作為輸出，根據(jù)不同的航向偏差和偏差率適應(yīng)性地輸出解模糊后的控制參數(shù)，模糊PID控制器結(jié)構(gòu)如圖3所示.

圖3 模糊PID控制器結(jié)構(gòu)

式中，c決定了隸屬度函數(shù)的中心點(diǎn)，σ決定了函數(shù)曲線的寬度.e（t）、（t）的隸屬度函數(shù)如圖4所示.

圖4 誤差和誤差導(dǎo)數(shù)的隸屬度函數(shù)

無人艇航向航速聯(lián)合控制方法控制無人艇轉(zhuǎn)向時(shí)，由于初始航向偏差較大，先給一個(gè)較大的差速轉(zhuǎn)角，讓轉(zhuǎn)向運(yùn)動(dòng)快速啟動(dòng)、加速，待回轉(zhuǎn)角速度達(dá)到穩(wěn)態(tài)后，產(chǎn)生一個(gè)適當(dāng)?shù)霓D(zhuǎn)角維持等角速度回轉(zhuǎn)；當(dāng)實(shí)際航向接近期望航向時(shí)，產(chǎn)生一個(gè)較小的差速轉(zhuǎn)角，直至達(dá)到期望航向和航速.接著進(jìn)行模糊推理，并根據(jù)模糊規(guī)則，將其規(guī)則轉(zhuǎn)換為可以程序?qū)崿F(xiàn)的模糊語句：If（e（t）isX）and（（t）is Y）then（KPis Z），設(shè)置模糊推理器，對(duì)KP、KI、KD分別進(jìn)行解模糊處理，其中的解模糊后輸出結(jié)果如圖5所示.

2.2 控制方法

在所涉及的MOOS下位機(jī)軟件模塊中，我們?cè)O(shè)計(jì)了三個(gè)模塊參與了無人艇航向和航速控制，分別是：pHelmIvp、pAct、PIDmarine模塊.其中pHelmIvp模塊根據(jù)訂閱的無人艇的即時(shí)航向NAV-heading、航速NAV-speed以及設(shè)定的航速Desired_speed-debug和設(shè)定航向Desired_heading-debug信息，產(chǎn)生Desired_speed、Desired_heading；PIDmarine模塊訂閱NAV-heading、NAV-speed、Desired_speed、Desired_heading，經(jīng)過PID運(yùn)算產(chǎn)生Desired_thrust、Desired_rudder，進(jìn)一步將Desired_rudder轉(zhuǎn)化為無人艇左右推進(jìn)器的PWM值：thrustleft、thrustright，通過Notify（）函數(shù)進(jìn)行發(fā)布.pAct模塊訂閱PIDmarine模塊發(fā)布的thrustleft、thrustright信息，并根據(jù)下位主控制與運(yùn)動(dòng)控制器之間的通信協(xié)議生成控制幀，裝幀完成后通過串口發(fā)給下位機(jī)運(yùn)動(dòng)控制器PCM9310板.pAct模塊還需完成航向信息（NAV-heading）、航速信息（NAV-speed）的數(shù)據(jù)采集，NAV-heading信息是通過串口采集的北微電子羅盤輸出的航向角信息，NAV-speed信息是通過串口采集的GPS模塊的航速經(jīng)過轉(zhuǎn)換而實(shí)現(xiàn)的，即時(shí)二者信息進(jìn)行發(fā)布，滿足PID控制模塊閉環(huán)控制的需求.并且pAct模塊通過串口采集運(yùn)動(dòng)控制器上傳的電壓、電流數(shù)據(jù)幀，并按照設(shè)計(jì)的協(xié)議進(jìn)行數(shù)據(jù)字段的解析，解析出來的左右推進(jìn)器的電壓、電流、主控制器的電壓和電流，以及控制艙的溫濕度信息進(jìn)行消息的發(fā)布，其他軟件模塊均可對(duì)該信息進(jìn)行訂閱和顯示.

2.2.1 航向控制

航向控制需要實(shí)現(xiàn)的功能是通過上位機(jī)給定一個(gè)航向角Desired_heading并發(fā)布，下位機(jī)PID模塊訂閱該期望航向角信息，以及pAct發(fā)布的NAV-heading信息，通過PID控制算法實(shí)現(xiàn)航向的保持，具體控制流程如圖5所示.我們采用電子羅盤反饋航向角信息（NAV-heading）.當(dāng)PID模塊產(chǎn)生的無人艇的Desired_rudder為負(fù)值，其代表無人艇需要控制順時(shí)針轉(zhuǎn)動(dòng)，此時(shí)要求無人艇右邊推進(jìn)器的推力大于左邊推進(jìn)器的推力，即右邊的PWM值大于左邊的PWM值.類推，當(dāng)PID模塊產(chǎn)生的無人艇的Desired_rudder為正值，代表無人艇需要控制其逆時(shí)針轉(zhuǎn)動(dòng)，此時(shí)要求左邊推進(jìn)器的推力大于右邊推進(jìn)器的推力，即左邊的PWM值大于右邊的PWM值.具體的左右推進(jìn)器的PWM值產(chǎn)生方法為：在當(dāng)前推進(jìn)器PWM值Desired_thrust的基礎(chǔ)上，左邊推進(jìn)器的PWM值=左邊推進(jìn)器的當(dāng)前PWM值＋rudder*500/30；右邊推進(jìn)器的PWM值=右邊推進(jìn)器的當(dāng)前PWM值-rudder*500/30，其中rudder為經(jīng)過PID產(chǎn)生的轉(zhuǎn)向角值（Desired_rudder）；500對(duì)應(yīng)著推進(jìn)器PWM差值為500 us且作用時(shí)間為1 s，此時(shí)無人艇最大轉(zhuǎn)向角度約為30°，左右推進(jìn)器的PWM值的差值為500 us，并且經(jīng)過實(shí)驗(yàn)得知rudder角與PWM差值之間存在線性關(guān)系.由于PWM存在飽和的情況，我們?cè)O(shè)定經(jīng)過計(jì)算后的左右推進(jìn)器的PWM值（thrustleft、thrustright），若其值大于2 000 us，則其值取為2 000 us，若其值小于1 000 us，則其值取為1 000 us.

圖5 航向反饋的閉環(huán)航向控制方法

2.2.2 航速控制

航速控制需要實(shí)現(xiàn)的功能是通過上位機(jī)給定一個(gè)航速Desired_speed并發(fā)布的情形下，下位機(jī)PID模塊訂閱該期望航速信息，還需訂閱pAct發(fā)布的NAV-heading信息，通過PID控制算法實(shí)現(xiàn)無人艇航速的保持，具體控制流程如圖6所示.我們采用GPS反饋無人艇航速信息.通過設(shè)置一個(gè)速度因子變量Speed_factor，參數(shù)配置其是否為零決定了程序是采用常規(guī)控制還是模糊PID控制，當(dāng)Speed_factor直接給定非零時(shí)，通過計(jì)算以下公式計(jì)算推進(jìn)器的PWM值：推進(jìn)器的PWM值＝Speed_factor*Desired_speed＋1 500，其中Desired_speed為通過上位機(jī)給定的期望速度，通過該公式可以計(jì)算給定的推進(jìn)器的PWM值，計(jì)算完成后將Desired_thrust進(jìn)行發(fā)布，從而實(shí)現(xiàn)直接驅(qū)動(dòng)推進(jìn)器產(chǎn)生給定的轉(zhuǎn)速，控制流程如圖7所示.當(dāng)Speed_factor參數(shù)配置為直接給定零時(shí)，程序采用PID航速控制，通過pAct模塊采集GPS的速度反饋值，與上位機(jī)給定的速度值相減，通過PID計(jì)算delta_thrust，通過 Desired_thrust＝Desired_thrust＋delta_thrust計(jì)算 Desired_thrust.其中Speed_factor變量的給定，是通過下位機(jī)MarinePID模塊初始化參數(shù)里的Speed_factor參數(shù)進(jìn)行初始化的，MarinePID模塊里的函數(shù)vclip（dval，0，Speed_factor）取出該值并賦值給m_Speed_factor，然后通過setspeedfactor（）方法實(shí)現(xiàn)對(duì)MarinePID模塊中該Speed_factor參數(shù)的設(shè)置.

圖6 Speed_factor=0時(shí)速度反饋的航速模糊PID控制方法

圖7 Speed_factor!=0時(shí)帶速度因子的常規(guī)PID航速控制方法

3 實(shí)驗(yàn)與分析

岸邊實(shí)驗(yàn)2：在給定的無人艇航速下和左右推進(jìn)器PWM差值下，無人艇會(huì)做一個(gè)圓周運(yùn)動(dòng)，但是由于短時(shí)間內(nèi)不容易測(cè)量轉(zhuǎn)向角，我們通過觀測(cè)無人艇圓周運(yùn)動(dòng)單位時(shí)間內(nèi)的轉(zhuǎn)角變化，也即角速度是否恒定來決定是否啟用無人艇角速度測(cè)量.無人艇進(jìn)行圓周運(yùn)動(dòng)時(shí)，初始運(yùn)動(dòng)狀態(tài)不穩(wěn)定，待無人艇運(yùn)動(dòng)穩(wěn)定后，程序測(cè)量兩次相同航向角時(shí)，無人艇所轉(zhuǎn)動(dòng)的角度和所用的時(shí)間t，根據(jù)公式：角速度ω＝弧度變化量/所用時(shí)間，計(jì)算角速度ω＝2/一圈的時(shí)間，得到角速度ω.我們記錄了PWM中值固定情形下，不同轉(zhuǎn)速差值下的角速度值，如表2所示.通過實(shí)驗(yàn)發(fā)現(xiàn)，當(dāng)左推進(jìn)器PWM＝1 350 us，右推進(jìn)器PWM＝1 850 us時(shí)，無人艇有最大角速度為0.52 rad/s.

為了驗(yàn)證提出航速和航向控制方法的有效性，一方面通過MOOS系統(tǒng)的模擬功能實(shí)現(xiàn)對(duì)控制方法有效性的驗(yàn)證，在模擬實(shí)驗(yàn)驗(yàn)證通過的基礎(chǔ)上，進(jìn)行了岸邊控制功能的實(shí)驗(yàn)驗(yàn)證，并通過岸邊實(shí)驗(yàn)獲取了無人艇的轉(zhuǎn)角速度信息，進(jìn)一步優(yōu)化了無人艇的控制算法相關(guān)參數(shù).

岸邊實(shí)驗(yàn)1：該實(shí)驗(yàn)是為了實(shí)現(xiàn)航速與PWM值關(guān)系曲線和Speed_factor參數(shù)的獲取，如圖8所示.在上位機(jī)設(shè)置PWM值為1 550 us到2 000 us的如表1所示的數(shù)值，pAct將自動(dòng)獲取Desired_thrust并將兩個(gè)推進(jìn)器的PWM設(shè)置為表1中的PWM值，通過GPS模塊測(cè)得無人艇航速如表1所示.根據(jù)速度與PWM值增量的線性關(guān)系，可以獲取無人艇系統(tǒng)的速度因子Speed_factor為25.Desired_thrust與推進(jìn)器PWM值之間的轉(zhuǎn)換關(guān)系為：推進(jìn)器PWM值＝k*Desired_thrust＋1 500，這里k＝5為轉(zhuǎn)化系數(shù).將Speed_factor值寫入到下位機(jī)MOOS系統(tǒng)的MarinePID模塊的初始化參數(shù)中，或者將MOOS初始化文件的Speed_factor修改為25，為下一步模擬仿真做好準(zhǔn)備.

表1 測(cè)得的PWM值與推進(jìn)器轉(zhuǎn)速、航速的關(guān)系（1節(jié)海流速度）

表2 Delta_PWM＝100，150，200，250，300，400，500us時(shí)的角速度值

圖8 無人艇岸邊試驗(yàn)

模擬仿真1：采用常規(guī)PID的航速控制和航向的閉環(huán)控制方式.在MOOS初始化配置中，將Speed_factor參數(shù)設(shè)置為125，通過計(jì)算Desired_thrust＝speed_factor*Desired_speed，計(jì)算期望的推進(jìn)器PWM值，并寫入到數(shù)據(jù)幀的對(duì)應(yīng)字段中.進(jìn)一步運(yùn)行系統(tǒng)相應(yīng)模塊和MarineViewer界面，進(jìn)行八邊形路徑點(diǎn)跟蹤，仿真結(jié)果如圖9所示.可以看到無人艇系統(tǒng)可以實(shí)現(xiàn)對(duì)八邊形航路點(diǎn)的跟蹤，圖9中最下面為無人艇的期望航向角以及實(shí)際的航向角的實(shí)時(shí)曲線.

圖9 無人艇的航路點(diǎn)跟蹤控制結(jié)果、期望航向角以及實(shí)際的航向角

模擬仿真2：采用模糊PID的航速控制和航向的閉環(huán)控制方式.在MOOS初始化配置中，將Speed_factor參數(shù)設(shè)置為0，通過計(jì)算Desired_thrust＝Desired_thrust＋delta_thrust，計(jì)算期望的推進(jìn)器PWM值，并寫入到數(shù)據(jù)幀的對(duì)應(yīng)字段中.進(jìn)一步運(yùn)行系統(tǒng)相應(yīng)模塊和MarineViewer界面，進(jìn)行八邊形路徑點(diǎn)的跟蹤，仿真結(jié)果如圖10所示.可以看到采用模糊PID的航速控制和航向的閉環(huán)控制模式，無人艇系統(tǒng)可以實(shí)現(xiàn)對(duì)八邊形航路點(diǎn)的跟蹤.當(dāng)期望速度為3.5 m/s時(shí)，對(duì)比常規(guī)PID速度控制與模糊PID速度控制的控制精度，常規(guī)速度控制方法的速度誤差為1 m/s以內(nèi).當(dāng)PID算法的比例系數(shù)KP＝0.5，KD＝2.0，KI＝0時(shí)，與常規(guī)PID速度控制方法相比，模糊PID速度跟蹤控制方法對(duì)期望速度的跟蹤精度要高一個(gè)數(shù)量級(jí)，速度誤差控制在0.1 m/s以內(nèi).

圖10 無人艇的航路點(diǎn)跟蹤控制結(jié)果、期望航向角以及實(shí)際的航向角

4 結(jié)論

從無人水面監(jiān)測(cè)艇自主控制的工程設(shè)計(jì)需求出發(fā)，針對(duì)無人監(jiān)測(cè)艇的自主控制系統(tǒng)的結(jié)構(gòu)組成、各功能模塊的技術(shù)要求、下位機(jī)主控制模塊和運(yùn)動(dòng)控制模塊的設(shè)計(jì)及其通信控制協(xié)議進(jìn)行了設(shè)計(jì)，基于MOOS-Ivp軟件平臺(tái)編寫了航向和速度聯(lián)合控制的分布式控制軟件模塊，對(duì)比了采用常規(guī)PID控制和模糊控制方法的無人艇航向航速控制效果，并在岸邊試驗(yàn)站對(duì)該試驗(yàn)艇的運(yùn)動(dòng)控制系統(tǒng)進(jìn)行了實(shí)驗(yàn)驗(yàn)證，證明了基于模糊控制方法的無人艇自主控制方法的有效性.