方洵　黃輝

摘 要：APM系統(tǒng)平臺(tái)上通用配置方式與Mavlink通訊協(xié)議參數(shù)理解。在RTPS協(xié)議下進(jìn)行控制系統(tǒng)設(shè)計(jì)，對(duì)無(wú)人船航向控制過程中的風(fēng)浪混合干擾，對(duì)PX4平臺(tái)原生mahony算法與導(dǎo)航控制算法修繕，結(jié)合模糊PID控制技術(shù)，提出有效的PD控制手段，實(shí)現(xiàn)控制參數(shù)自適應(yīng)調(diào)整與優(yōu)化。采用Ardupilot平臺(tái)反饋消息，并在Matlab上對(duì)控制算法優(yōu)化分析，得到姿態(tài)控制優(yōu)化曲線，航向控制的壓線、點(diǎn)情況較好。

關(guān)鍵詞：Ardupilot；pixhawk；mahony算法；Fast RTPS；模糊PID

中圖分類號(hào)：TP273 文獻(xiàn)標(biāo)志碼：A 文章編號(hào)：2095-2945（2018）30-0027-05

Abstract： The general configuration mode on APM system platform and the parameter understanding of Mavlink communication protocol are introduced. In this paper， the control system is designed under the RTPS protocol， and the original mahony algorithm and navigation control algorithm of PX4 platform are repaired under the mixed wind and wave interference in the course control of micro unmanned surface vessel （USV）. Based on Fuzzy PID control technology， a simple and effective PD control method is proposed to realize the adaptive adjustment and optimization of control parameters. The Ardupilot platform was used to provide feedback information， and the optimization analysis of control algorithm was carried out in Matlab. The optimized curve of attitude control was obtained.

Keywords： Ardupilot； pixhawk； mahony algorithm； Fast RTPS； Fuzzy PID

1 概述

1.1 課題研究背景、目的及意義

在無(wú)人技術(shù)領(lǐng)域，相對(duì)無(wú)人機(jī)，無(wú)人車的迅猛發(fā)展，無(wú)人船的研究相對(duì)滯后。近年來(lái)，船舶智能化程度不斷提升，融入大數(shù)據(jù)、云計(jì)算、物聯(lián)網(wǎng)等眾多新技術(shù)[1]。無(wú)人船的發(fā)展目標(biāo)為全自主型，具備自主航行，環(huán)境感知與自適應(yīng)控制等功能。無(wú)人船研究多集中為船體設(shè)計(jì)、路徑規(guī)劃導(dǎo)航、水面物標(biāo)探測(cè)與自主識(shí)別、規(guī)避障礙、運(yùn)動(dòng)控制等多個(gè)方面[1]。無(wú)人船研究屬于多學(xué)科交叉，多領(lǐng)域融合的綜合項(xiàng)目，對(duì)促進(jìn)學(xué)科發(fā)展與水產(chǎn)養(yǎng)殖業(yè)發(fā)展具有重要意義。

1.2 無(wú)人船發(fā)展現(xiàn)狀與背景分析

早在二戰(zhàn)時(shí)期，美軍就在無(wú)人水面艇（USV）上架設(shè)了槍炮和導(dǎo)彈，通過遠(yuǎn)程操控來(lái)對(duì)敵人進(jìn)行攻擊。最早研究多集中在軍用領(lǐng)域，如今，國(guó)內(nèi)外有很多研究機(jī)構(gòu)和公司進(jìn)行了船舶無(wú)人化研究，USV是研究中的熱點(diǎn)，在軍事和民用都有廣闊的應(yīng)用前景[2]。其中已軍用化的有，美國(guó)

“Spartan Scount”無(wú)人艇、以色列“Protector”無(wú)人艇。2017年，Google與勞斯萊斯合作研發(fā)分析輪船周圍情況的AI系統(tǒng)。2016年，國(guó)際海洋高新科技博覽會(huì)，云州智能M80實(shí)現(xiàn)智能吊放過程，未來(lái)有望成為軍民兩用產(chǎn)品[2]。武漢勞雷綠灣船舶科技研發(fā)鋁合金無(wú)人艇，包含SM、M、I、H系列船體以及復(fù)合動(dòng)力船艇等。2014年，武漢楚航測(cè)控科技發(fā)布最新無(wú)人船水域測(cè)量機(jī)器人系統(tǒng)，包含遙控測(cè)量與岸基控制系統(tǒng)。由上海大學(xué)研制的“精海7號(hào)”無(wú)人艇采用高集成模塊設(shè)計(jì)，智能避障導(dǎo)航系統(tǒng)能夠在海面航行中自主避開前方障礙完成預(yù)設(shè)目標(biāo)。2017年，中科華澄研制生產(chǎn)智能安防水面機(jī)器人，用于港口、碼頭、河道等安防巡邏。中海達(dá)信息技術(shù)自產(chǎn)研發(fā)無(wú)人船周邊傳感器，包含單波探測(cè)、側(cè)掃聲吶、短基站定位、流速剖儀等，同時(shí)研制出iBoat系列智能無(wú)人測(cè)量船。隨無(wú)人船基礎(chǔ)研究深入，國(guó)內(nèi)數(shù)字漁業(yè)也隨之興盛起來(lái)。農(nóng)業(yè)部發(fā)布《2018年漁業(yè)漁政工作要點(diǎn)》，推進(jìn)水產(chǎn)養(yǎng)殖行業(yè)。2017年挪威DNV GL團(tuán)隊(duì)開發(fā)seatrue無(wú)人船采用先進(jìn)的冷藏技術(shù)以保持海鮮的新鮮度。近年來(lái)，“養(yǎng)蝦熱”席卷全球，與水產(chǎn)養(yǎng)殖業(yè)息息相關(guān)的各項(xiàng)產(chǎn)業(yè)鏈日漸完善，多項(xiàng)專利，養(yǎng)殖產(chǎn)品，補(bǔ)貼政策不斷出現(xiàn)，養(yǎng)殖類無(wú)人船研究具有巨大的商業(yè)價(jià)值。

2 無(wú)人船控制系統(tǒng)平臺(tái)設(shè)計(jì)

本文采用微型無(wú)人船，船體采用注塑工藝生產(chǎn)加工后完成，整體機(jī)身為620*263*180mm，凈重為2.7kg[3]。配置有水循環(huán)冷卻系統(tǒng)，GPS/RTK導(dǎo)航系統(tǒng)，完成位置估計(jì)，工業(yè)機(jī)IMU慣導(dǎo)姿態(tài)估計(jì)系統(tǒng)，數(shù)據(jù)傳輸模塊，圖像接受傳輸模塊。船載控制系統(tǒng)以Pixhawk2.4.8飛控板為核心，通過串口、I2C、SPI與IMU、GPS、無(wú)線通訊模塊通訊，采用3000mah鋰電池作為供電電源；基于RTPS實(shí)現(xiàn)發(fā)布接受指令，獲取無(wú)人船當(dāng)前姿態(tài)位置消息，控制左右電機(jī)轉(zhuǎn)速差異，實(shí)現(xiàn)無(wú)人船自主航向控制。采用Ardupilot平臺(tái)岸基監(jiān)控系統(tǒng)，實(shí)現(xiàn)無(wú)人船位置、航向、視角、速度等消息的實(shí)時(shí)監(jiān)控與調(diào)節(jié)。無(wú)人船模型如圖1所示。

2.1 硬件控制系統(tǒng)設(shè)計(jì)

選用Pixhawk2.4.8作為核心處理模塊，包含32位ARM CortexM4高性能處理器，可運(yùn)行NuttX RTOS實(shí)時(shí)操作系統(tǒng)，主頻為168MHZ，256K RAM，2M Flash。板載L3GD20 3軸數(shù)字16位陀螺儀，LSM303D 3軸14位加速度/磁強(qiáng)計(jì)、MPU6000 3軸陀螺/加速度計(jì)、MS5611高精度氣壓計(jì)等，同時(shí)外設(shè)豐富（UART，I2C，SPI，CAN）；采用樂迪 M8N GPS定位模塊，工作在雙模模式下定位精度達(dá)1米，單模精度2.5米，測(cè)速精度 0.1米/秒；無(wú)線通訊部分采用3DR Radio數(shù)傳模塊，空中的數(shù)據(jù)傳輸速率高達(dá) 250kbps，發(fā)送MavLink協(xié)議幀和狀態(tài)報(bào)告，實(shí)現(xiàn)岸基信息交互；控制系統(tǒng)設(shè)計(jì)采用水下推進(jìn)器差速控制方式；電源部分設(shè)計(jì)采用雙電源，控制用電、動(dòng)力用電分開。硬件控制系統(tǒng)設(shè)計(jì)如圖2所示。

2.2 軟件控制系統(tǒng)設(shè)計(jì)

船載控制系統(tǒng)Pixy基于RTOS嵌入式系統(tǒng)實(shí)現(xiàn)多任務(wù)并行處理，包含航向控制、軌跡跟蹤、GPS與IMU數(shù)據(jù)輸入、無(wú)線指令傳輸與處理等任務(wù)。本文在Pixhawk控制器上通過MavLink協(xié)議實(shí)現(xiàn)實(shí)時(shí)多任務(wù)處理。軟件設(shè)計(jì)具體包括：100ms獲取當(dāng)前航向與航點(diǎn)位置，并與無(wú)線數(shù)傳模塊通訊，消息轉(zhuǎn)發(fā)到岸基控制器上；每50ms更新一次IMU與GPS數(shù)據(jù)消息，同時(shí)控制PWM口發(fā)出相應(yīng)的脈沖調(diào)制波形，調(diào)節(jié)電機(jī)轉(zhuǎn)速；算法通過模糊控制器以航向偏差信號(hào)和變化率為輸入值，PD控制后為輸出值，經(jīng)自適應(yīng)PD控制器產(chǎn)生控制信號(hào)，實(shí)現(xiàn)無(wú)人船航線跟蹤功能。

2.3 岸基監(jiān)控系統(tǒng)設(shè)計(jì)

岸基控制系統(tǒng)包括無(wú)線數(shù)傳模塊與計(jì)算器，采用Ardupilot監(jiān)控界面，具有固件更新，指令收發(fā)、消息查閱、航線顯示等功能，能夠遠(yuǎn)程控制無(wú)人船，采集修改控制參數(shù)。

3 無(wú)人船F(xiàn)ast-RTPS通訊架構(gòu)

3.1 Fast-RTPS

Eprosima Fast RTPS是通過c++實(shí)現(xiàn)的實(shí)時(shí)發(fā)布訂閱協(xié)議，該協(xié)議通過不可靠的傳輸（如UDP）提供發(fā)布者-訂閱者通信，由對(duì)象管理組（OMG）定義和維護(hù)。RTPS也是由OMG為數(shù)據(jù)分發(fā)服務(wù)（DDS）標(biāo)準(zhǔn)定義的操作性協(xié)議。

3.2 MavLink協(xié)議解析

MavLink協(xié)議最早由蘇黎世聯(lián)邦理工學(xué)院計(jì)算機(jī)視覺與幾何實(shí)驗(yàn)組的 Lorenz Meier于2009年發(fā)布，并遵循LGPL開源協(xié)議。MavLink協(xié)議是在串口通訊基礎(chǔ)上的一種更高層的開源通訊協(xié)議。

如圖3，消息幀格式由起始標(biāo)志位STX、字節(jié)長(zhǎng)度LEN、消息幀序號(hào)SEQ、系統(tǒng)編號(hào)SYS、單元編號(hào)COMP、消息包編號(hào)MSG、有效載荷PAYLOAD、高位校驗(yàn)CKA、低位校驗(yàn)CKB組成。本文通過自定義MavLink消息，實(shí)現(xiàn)創(chuàng)建，發(fā)送，接受三個(gè)過程，最后設(shè)置速率。

如圖4，首先msg/ca_trajectory.msg自定義uORB ca_tr

ajectory消息，并且在 mavlink_msg_ca_trajectory.h有了一個(gè)自定義mavlink_ca_trajectory 消息。添加mavlink的頭文件和uorb消息到mavlink_messages.cpp，最后附加流類streams_lis到mavlink_messages.cpp底部。確保啟用流，并在mavlink_receiver.h中增加一個(gè)用來(lái)處理接收信息得函數(shù)，增加一個(gè)處理類MavlinkReceiver 中的輸入MavLink消息的函數(shù)與uORB消息發(fā)布者，確定函數(shù)在MavlinkReceiver：：handle_message（）中被調(diào)用。通過MavLinkstatus找到端口號(hào)，相應(yīng)地將輸出transport protocol： UDP （），以次來(lái)設(shè)置流速率大小。

4 無(wú)人船姿態(tài)控制系統(tǒng)

4.1 Mahony算法

互補(bǔ)濾波要求兩個(gè)信號(hào)的干擾噪聲處在不同的頻率，以確保融合后的信號(hào)能夠覆蓋需求頻率。兩個(gè)濾波器的截止頻率一致，需要根據(jù)有用頻率的值對(duì)其進(jìn)行取舍。在飛行控制方面，Mahony濾波相比對(duì)于卡爾曼濾波具有時(shí)間短，速度快，效果好等優(yōu)勢(shì)。Mahony濾波器主要過程分為預(yù)測(cè)、加速度校正、測(cè)力計(jì)校正、GPS校正、四元數(shù)更新。具體濾波操作如圖5所示。

4.2 姿態(tài)模式控制

根據(jù)Ardupilot平臺(tái)給出的Comter Attitude Control文檔手冊(cè)，采用經(jīng)典PID控制手段。AC庫(kù)中提供多種可行的模式控制時(shí)手段對(duì)數(shù)據(jù)頻率、控制手段、參數(shù)變量、位置矢量進(jìn)行快速變換，以達(dá)到快速響應(yīng)當(dāng)前需要的目的（如圖6）。

通過使用P控制器將角度誤差（目標(biāo)角度與實(shí)際角度的差值）轉(zhuǎn)換為所需的旋轉(zhuǎn)速率，然后使用PID控制器將旋轉(zhuǎn)速率誤差轉(zhuǎn)換為電機(jī)命令。Ardupilot原生經(jīng)典PID控制器流程如圖7所示。本文通過修改原生PID飛行姿態(tài)控制器，使之滿足無(wú)人船航向控制要求。

5 無(wú)人船模糊PD航向控制

模糊控制系統(tǒng)由模糊數(shù)據(jù)和規(guī)則庫(kù)、模糊器、模糊推理機(jī)和解模糊器組成。模糊控制系統(tǒng)用作控制器時(shí)稱為模糊控制器。利用模糊控制器對(duì)參數(shù)的優(yōu)化，得到了航向控制的自適應(yīng)模糊PD控制器，系統(tǒng)結(jié)構(gòu)如圖8所示。

實(shí)驗(yàn)結(jié)果測(cè)試表面，I對(duì)實(shí)驗(yàn)測(cè)試影響不大，故可直接采用機(jī)模糊PD控制器。圖8以無(wú)人船航向偏差與航點(diǎn)偏差及其變化率作為輸入量；模糊隸屬度函數(shù)選擇為三角形分布；解模糊化方法采用面積中心平均法。模糊控制規(guī)律如表1、表2所示。

如圖9，圖10，本文通過數(shù)傳模塊回傳數(shù)據(jù)，在Ardupilot平臺(tái)上記錄數(shù)據(jù)，并生成曲線。優(yōu)化參數(shù)后，取最優(yōu)曲線數(shù)據(jù)，導(dǎo)入Matlab中進(jìn)行分析比較。兩種控制方式試驗(yàn)結(jié)果如圖11、圖12所示。

從試驗(yàn)結(jié)果比較可知，兩種控制方法均能實(shí)現(xiàn)無(wú)人船航向控制。從控制效果看，經(jīng)典PID控制速度快，但超調(diào)量大，航向控制準(zhǔn)確度不高。模糊控制相應(yīng)速度快，超調(diào)小，準(zhǔn)確度與精密度較好。航線偏移量少于0.4m， 航線與航向重合時(shí)，航向偏差量為3°，且受環(huán)境干擾少，在航線改變情況小，能迅速調(diào)整參數(shù)，加速航線貼合速度。

通過Ardupilot數(shù)據(jù)日志記錄，選取一直線航向?yàn)橹毕蚝拥?，少風(fēng)少浪環(huán)境進(jìn)行測(cè)試，設(shè)定左右航點(diǎn)，觀察航點(diǎn)壓線情況，同時(shí)在Matlab得到其航線包絡(luò)線如圖13所示。航點(diǎn)離線偏差較小，效果較好。

無(wú)人船軌跡巡航工作流程如圖14所示。通過系統(tǒng)模塊，控制器與遠(yuǎn)程工作站共同協(xié)作完成任務(wù)。本文根據(jù)實(shí)驗(yàn)數(shù)據(jù)與分析結(jié)果，重新設(shè)計(jì)船體模型（如圖15），使船體在滿足航行控制要求的前提下，逐步增加避障與水產(chǎn)養(yǎng)殖功能。

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

本文基于Ardupilot平臺(tái)進(jìn)行微型無(wú)人船體軟硬件設(shè)計(jì)，借助RTPS與MAVLink協(xié)議改善原生航向控制算法，引入模糊控制手段，取得較好的航線貼合效果，同時(shí)滿足傳感器之間實(shí)時(shí)通訊傳輸任務(wù)，完成微型無(wú)人船航線跟蹤功能。

在已有的船體基礎(chǔ)上設(shè)計(jì)投料機(jī)構(gòu)，最終應(yīng)用于農(nóng)業(yè)與水產(chǎn)養(yǎng)殖。

參考文獻(xiàn)：

[1]張樹凱.基于數(shù)據(jù)驅(qū)動(dòng)的無(wú)人船航線自動(dòng)生成[D].大連海事大學(xué)，2016.

[2]AMOV LAB， Unmanned ship development platform[EB/OL].成都鉑貝科技，http：//www.amovauto.com/portal.php，2018.

[3]沈宏濤，婁奇鶴，王 ，等.基于RTPS的變電站自動(dòng)化網(wǎng)絡(luò)通信系統(tǒng)研究[J].電力自動(dòng)化設(shè)備，2005（02）：25-29.