基于Ardupilot的微型無人船航向控制系統(tǒng)設(shè)計

方洵　黃輝

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

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

中圖分類號：TP273 文獻標志碼：A 文章編號：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 課題研究背景、目的及意義

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

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

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

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

2 無人船控制系統(tǒng)平臺設(shè)計

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

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

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

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

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

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

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

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

3.1 Fast-RTPS

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

3.2 MavLink協(xié)議解析

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

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

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

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

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

4.1 Mahony算法

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

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

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

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

5 無人船模糊PD航向控制

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

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

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

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

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

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

6 結(jié)束語

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

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

參考文獻：

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

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

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