基于Ardupilot的微型无人船航向控制系统设计

方洵　黄辉

摘 要：APM系统平台上通用配置方式与Mavlink通讯协议参数理解。在RTPS协议下进行控制系统设计，对无人船航向控制过程中的风浪混合干扰，对PX4平台原生mahony算法与导航控制算法修缮，结合模糊PID控制技术，提出有效的PD控制手段，实现控制参数自适应调整与优化。采用Ardupilot平台反馈消息，并在Matlab上对控制算法优化分析，得到姿态控制优化曲线，航向控制的压线、点情况较好。

关键词：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 課题研究背景、目的及意义

在无人技术领域，相对无人机，无人车的迅猛发展，无人船的研究相对滞后。近年来，船舶智能化程度不断提升，融入大数据、云计算、物联网等众多新技术[1]。无人船的发展目标为全自主型，具备自主航行，环境感知与自适应控制等功能。无人船研究多集中为船体设计、路径规划导航、水面物标探测与自主识别、规避障碍、运动控制等多个方面[1]。无人船研究属于多学科交叉，多领域融合的综合项目，对促进学科发展与水产养殖业发展具有重要意义。

1.2 无人船发展现状与背景分析

早在二战时期，美军就在无人水面艇（USV）上架设了枪炮和导弹，通过远程操控来对敌人进行攻击。最早研究多集中在军用领域，如今，国内外有很多研究机构和公司进行了船舶无人化研究，USV是研究中的热点，在军事和民用都有广阔的应用前景[2]。其中已军用化的有，美国

“Spartan Scount”无人艇、以色列“Protector”无人艇。2017年，Google与劳斯莱斯合作研发分析轮船周围情况的AI系统。2016年，国际海洋高新科技博览会，云州智能M80实现智能吊放过程，未来有望成为军民两用产品[2]。武汉劳雷绿湾船舶科技研发铝合金无人艇，包含SM、M、I、H系列船体以及复合动力船艇等。2014年，武汉楚航测控科技发布最新无人船水域测量机器人系统，包含遥控测量与岸基控制系统。由上海大学研制的“精海7号”无人艇采用高集成模块设计，智能避障导航系统能够在海面航行中自主避开前方障碍完成预设目标。2017年，中科华澄研制生产智能安防水面机器人，用于港口、码头、河道等安防巡逻。中海达信息技术自产研发无人船周边传感器，包含单波探测、侧扫声呐、短基站定位、流速剖仪等，同时研制出iBoat系列智能无人测量船。随无人船基础研究深入，国内数字渔业也随之兴盛起来。农业部发布《2018年渔业渔政工作要点》，推进水产养殖行业。2017年挪威DNV GL团队开发seatrue无人船采用先进的冷藏技术以保持海鲜的新鲜度。近年来，“养虾热”席卷全球，与水产养殖业息息相关的各项产业链日渐完善，多项专利，养殖产品，补贴政策不断出现，养殖类无人船研究具有巨大的商业价值。

2 无人船控制系统平台设计

本文采用微型无人船，船体采用注塑工艺生产加工后完成，整体机身为620*263*180mm，净重为2.7kg[3]。配置有水循环冷却系统，GPS/RTK导航系统，完成位置估计，工业机IMU惯导姿态估计系统，数据传输模块，图像接受传输模块。船载控制系统以Pixhawk2.4.8飞控板为核心，通过串口、I2C、SPI与IMU、GPS、无线通讯模块通讯，采用3000mah锂电池作为供电电源；基于RTPS实现发布接受指令，获取无人船当前姿态位置消息，控制左右电机转速差异，实现无人船自主航向控制。采用Ardupilot平台岸基监控系统，实现无人船位置、航向、视角、速度等消息的实时监控与调节。无人船模型如图1所示。

2.1 硬件控制系统设计

选用Pixhawk2.4.8作为核心处理模块，包含32位ARM CortexM4高性能处理器，可运行NuttX RTOS实时操作系统，主频为168MHZ，256K RAM，2M Flash。板载L3GD20 3轴数字16位陀螺仪，LSM303D 3轴14位加速度/磁强计、MPU6000 3轴陀螺/加速度计、MS5611高精度气压计等，同时外设丰富（UART，I2C，SPI，CAN）；采用乐迪 M8N GPS定位模块，工作在双模模式下定位精度达1米，单模精度2.5米，测速精度 0.1米/秒；无线通讯部分采用3DR Radio数传模块，空中的数据传输速率高达 250kbps，发送MavLink协议帧和状态报告，实现岸基信息交互；控制系统设计采用水下推进器差速控制方式；电源部分设计采用双电源，控制用电、动力用电分开。硬件控制系统设计如图2所示。

2.2 软件控制系统设计

船载控制系统Pixy基于RTOS嵌入式系统实现多任务并行处理，包含航向控制、轨迹跟踪、GPS与IMU数据输入、无线指令传输与处理等任务。本文在Pixhawk控制器上通过MavLink协议实现实时多任务处理。软件设计具体包括：100ms获取当前航向与航点位置，并与无线数传模块通讯，消息转发到岸基控制器上；每50ms更新一次IMU与GPS数据消息，同时控制PWM口发出相应的脉冲调制波形，调节电机转速；算法通过模糊控制器以航向偏差信号和变化率为输入值，PD控制后为输出值，经自适应PD控制器产生控制信號，实现无人船航线跟踪功能。

2.3 岸基监控系统设计

岸基控制系统包括无线数传模块与计算器，采用Ardupilot监控界面，具有固件更新，指令收发、消息查阅、航线显示等功能，能够远程控制无人船，采集修改控制参数。

3 无人船Fast-RTPS通讯架构

3.1 Fast-RTPS

Eprosima Fast RTPS是通过c++实现的实时发布订阅协议，该协议通过不可靠的传输（如UDP）提供发布者-订阅者通信，由对象管理组（OMG）定义和维护。RTPS也是由OMG为数据分发服务（DDS）标准定义的操作性协议。

3.2 MavLink协议解析

MavLink协议最早由苏黎世联邦理工学院计算机视觉与几何实验组的 Lorenz Meier于2009年发布，并遵循LGPL开源协议。MavLink协议是在串口通讯基础上的一种更高层的开源通讯协议。

如图3，消息帧格式由起始标志位STX、字节长度LEN、消息帧序号SEQ、系统编号SYS、单元编号COMP、消息包编号MSG、有效载荷PAYLOAD、高位校验CKA、低位校验CKB组成。本文通过自定义MavLink消息，实现创建，发送，接受三个过程，最后设置速率。

如图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中增加一个用来处理接收信息得函数，增加一个处理类MavlinkReceiver 中的输入MavLink消息的函数与uORB消息发布者，确定函数在MavlinkReceiver：：handle_message（）中被调用。通过MavLinkstatus找到端口号，相应地将输出transport protocol： UDP （），以次来设置流速率大小。

4 无人船姿态控制系统

4.1 Mahony算法

互补滤波要求两个信号的干扰噪声处在不同的频率，以确保融合后的信号能够覆盖需求频率。两个滤波器的截止频率一致，需要根据有用频率的值对其进行取舍。在飞行控制方面，Mahony滤波相比对于卡尔曼滤波具有时间短，速度快，效果好等优势。Mahony滤波器主要过程分为预测、加速度校正、测力计校正、GPS校正、四元数更新。具体滤波操作如图5所示。

4.2 姿态模式控制

根据Ardupilot平台给出的Comter Attitude Control文档手册，采用经典PID控制手段。AC库中提供多种可行的模式控制时手段对数据频率、控制手段、参数变量、位置矢量进行快速变换，以达到快速响应当前需要的目的（如图6）。

通过使用P控制器将角度误差（目标角度与实际角度的差值）转换为所需的旋转速率，然后使用PID控制器将旋转速率误差转换为电机命令。Ardupilot原生经典PID控制器流程如图7所示。本文通过修改原生PID飞行姿态控制器，使之满足无人船航向控制要求。

5 无人船模糊PD航向控制

模糊控制系统由模糊数据和规则库、模糊器、模糊推理机和解模糊器组成。模糊控制系统用作控制器时称为模糊控制器。利用模糊控制器对参数的优化，得到了航向控制的自适应模糊PD控制器，系统结构如图8所示。

实验结果测试表面，I对实验测试影响不大，故可直接采用机模糊PD控制器。图8以无人船航向偏差与航点偏差及其变化率作为输入量；模糊隶属度函数选择为三角形分布；解模糊化方法采用面积中心平均法。模糊控制规律如表1、表2所示。

如图9，图10，本文通过数传模块回传数据，在Ardupilot平台上记录数据，并生成曲线。优化参数后，取最优曲线数据，导入Matlab中进行分析比较。两种控制方式试验结果如图11、图12所示。

从试验结果比较可知，两种控制方法均能实现无人船航向控制。从控制效果看，经典PID控制速度快，但超调量大，航向控制准确度不高。模糊控制相应速度快，超调小，准确度与精密度较好。航线偏移量少于0.4m， 航线与航向重合时，航向偏差量为3°，且受环境干扰少，在航线改变情况小，能迅速调整参数，加速航线贴合速度。

通过Ardupilot数据日志记录，选取一直线航向为直向河道，少风少浪环境进行测试，设定左右航点，观察航点压线情况，同时在Matlab得到其航线包络线如图13所示。航点离线偏差较小，效果较好。

无人船轨迹巡航工作流程如图14所示。通过系统模块，控制器与远程工作站共同协作完成任务。本文根据实验数据与分析结果，重新设计船体模型（如图15），使船体在满足航行控制要求的前提下，逐步增加避障与水产养殖功能。

6 结束语

本文基于Ardupilot平台进行微型无人船体软硬件设计，借助RTPS与MAVLink协议改善原生航向控制算法，引入模糊控制手段，取得较好的航线贴合效果，同时满足传感器之间实时通讯传输任务，完成微型无人船航线跟踪功能。

在已有的船体基础上设计投料机构，最终应用于农业与水产养殖。

参考文献：

[1]张树凯.基于数据驱动的无人船航线自动生成[D].大连海事大学，2016.

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

[3]沈宏涛，娄奇鹤，王 ，等.基于RTPS的变电站自动化网络通信系统研究[J].电力自动化设备，2005（02）：25-29.