基于STM32的无人艇自主导航控制系统的设计

张玉婷，陈褒丹,任 佳,Vladimir Shikhin

(1. 海南大学 信息科学技术学院，海南 海口570228；2.莫斯科动力学院 自动化与计算机工程学院，莫斯科)



基于STM32的无人艇自主导航控制系统的设计

张玉婷1，陈褒丹1,任 佳1,Vladimir Shikhin2

(1. 海南大学 信息科学技术学院，海南 海口570228；2.莫斯科动力学院 自动化与计算机工程学院，莫斯科)

针对无人艇远程操控和自主导航问题，设计了基于STM32的无人艇自主导航控制系统.船载系统以STM32微控制器为核心控制单元，采用互补滤波算法完成了无人艇陀螺仪角速度数据、加速度数据和磁力计数据的解算，并通过串级PID算法对航向角进行闭环控制，最后通过LABVIEW软件平台设计开发出地面站控制系统，实现了对无人艇的远程操控和自主导航功能.

无人艇； 姿态解算； PID控制器设计； 地面站设计

在当今海洋战略格局中，水面无人艇扮演的角色日益重要，它集船舶设计、智能控制、远程通信、运动控制和探测侦查等专业技术为一体[1].目前，水面无人艇主要应用于军事领域，主要执行反潜、扫雷、反恐、缉私、精确打击、海上搜救、水文地理勘测和通信等任务[2-4].但无人艇在民用领域的发展尚处于起步阶段，其原因在于无人艇控制系统的智能化程度不高，应用拓展性不强.

文献[5]进行了模型船控制方面的研究，基于PC104/386工控机设计开发了一个船舶运动控制试验平台，并在海上环境通过了对该平台的测试.沈阳航天新光公司研发的“天象一号”是我国第一艘真正意义上的无人艇[6]，其具有自稳定功能，能够在复杂和恶劣的海洋环境中进行工作，并采用增强型动力系统的设计，将其续航时间提高到20 d左右.哈尔滨工程大学在水下机器人技术研究领域取得了大量成果，目前己经完成了无人艇概念设计、艇体设计和实艇建造等工作[7-9].目前我国对水面无人艇的研究仅仅处于起步阶段，很多关键领域的难题需要攻克，尤其是在复杂海况下自动导航和路径规划方面的研究[10].

笔者设计了基于STM32的无人艇自主导航控制系统，该系统使用三轴陀螺仪、三轴加速度计和三轴磁力计进行姿态测量，采用互补滤波算法将各姿态数据进行融合，使用GPS实现位置追踪，通过PID控制算法实现对无人艇航向姿态的控制.实验证明该无人艇稳定性好，可通过地面站系统进行远程操控，同时实现自主导航的功能，适用于海洋环境的监测以及海事搜救.

1 系统结构与硬件组成

无人艇自主导航控制系统以32位微控制器STM32F103VC为核心控制单元，采用集成三轴陀螺仪、三轴加速度计的姿态板MPU6050和HMC5883L三轴磁力计作为无人艇的姿态测量系统，UBLOX NEO-M8N GPS模块组成无人艇的位置测量系统.各模块通过I2C总线与主控制器通信，主控制器将姿态测量系统采集到的原始数据进行融合处理，通过数据解算得到当前无人艇的航向角、俯仰角和横滚角.姿态和位置数据通过PID控制器运算输出相应的PWM信号驱动舵机，从而实现无人艇位置与航向的控制.无人艇硬件电路框架图如图1所示.

2 自主导航与控制系统设计

系统开始运行后，首先进行各模块的初始化，采集并上传各传感器信号，系统时间等；再将各传感器数据融合解算出当前无人艇姿态，每3 ms进入一次中断，输出PWM信号实现对舵机的航向控制；无人艇当前姿态数据每50 ms上传一次，同时监听地面站是否有指令发出.程序流程图如图2所示.

2.1 姿态解算 三轴加速度计、三轴陀螺仪和三轴磁力计是惯性测量中最常用的传感器.陀螺仪采集无人艇在航向、俯仰、横滚3个方向的角速度，数据通过时域积分得到各个方向的角度.陀螺仪动态特性良好，但由于易受噪声、温度变化等因素的干扰，导致测量精度较低，存在累积漂移误差，通过多次积分误差增大，使得测量的角度与实际角度存在较大差异.加速计和磁力计静态特性良好，不存在累计误差问题，但容易受机体振动和外部磁场等因素的干扰，使其动态可信度降低.从频域的角度分析来看，陀螺仪适用于高频段的姿态测量，加速度计、磁力计适用于低频段的姿态测量.因此，单独使用陀螺仪或者加速度计和磁力计，都存在一定偏差，需要对姿态测量系统进行多传感器数据融合，从而求解出准确的姿态信息.卡尔曼滤波算法和互补滤波算法是多惯性器件姿态解算中的常用算法.卡尔曼滤波算法计算量大，对核心处理器的运算速度和精度有很高的要求；互补滤波算法对惯性器件的精度要求较低，而且简单可靠.根据各惯性器件在频域上的特点，采用互补滤波算法对姿态角进行解算，算法如图3所示.

将加速度数据进行低通滤波，过滤掉短周期内加速度数据的波动，减少了船体晃动对姿态解算的影响.陀螺仪在低频段时，具有较大的测量误差，所以需要对陀螺仪数据进行高通滤波，再将处理后的数据进行融合计算.

低通滤波器在频域上的传递函数为

(1)

高通滤波器在频域上的传递函数为

(2)

在地理坐标系中，加速度计和磁力计分别测得当前地理坐标系下的重力矢量和正北方向的磁矢量，陀螺仪在船体坐标系下测得当前无人艇的姿态，对于同一个向量，在不同坐标系下表示时，其大小和方向一定相同.加速度计和磁力计对陀螺仪数据的修正实质上是对地理坐标系与船体坐标系相互转换所生成的旋转矩阵的修正.

将旋转矩阵矢量v与加速度计测得的重力矢量a作叉积得到俯仰和横滚角误差ea，

(3)

无人艇的航向定义在地理坐标系的X-O-Y平面，将加速度计修正后的旋转矩阵矢量m与磁力计测得正北方向的磁矢量w作叉积得到航向角误差eh，

(4)

叉积的模为角度误差的正弦，在角度很小的情况下可近似于角度，且与陀螺仪积分误差成正比，加权后与陀螺仪角增量叠加修正误差.g为陀螺仪实测出的角速度，Δθ为修正后的陀螺仪角速度，γ和λ分别为加速度计和磁力计纠正系数，

Δθ=g·Δt+γ·ea+λ·eh，

(5)

再将修正后的陀螺仪角速度采用一阶龙格库塔法进行四元数的更新，从而更新无人艇的姿态.qi(n)为当前四元数分量，qi(n+1)为更新后的四元数分量，

(6)

2.2 PID控制器设计 位置控制是在水平面内对无人艇规划航迹偏差的控制.通过GPS全球定位系统获得当前无人艇的经度、纬度和时间等信息，算出当前位置与目标航点的距离与夹角，并结合无人艇自身解算出的姿态信息作为航迹偏差的反馈，从而控制无人艇的舵机改变航向，使得无人艇按规划航线自主航行.

为了提高航向控制的稳定性，采用串级PID控制算法，将航向角角度与角速度分别进行PID控制.外环角度控制器的输出作为内环角速度控制器的输入，不仅可以准确控制姿态角同时也能增加角度控制的阻尼系数，让角度变化缓慢进行，实现角度的平稳转换.根据PID控制器的原理，设kp，ki，kd分别为比例、积分和微分系数，调整比例系数可以提高系统的响应速度，减小稳态误差；调整积分系数可以减小数据上下震荡的误差；调整微分系数可以预先处理和修正信号，加快控制系统的速度，减少调整时间.

Δφ=kp(φ-φd)+ki∫(φ-φd)dt ，

(7)

(8)

2.3 地面站软件设计 地面站是无人艇自主导航控制系统的重要组成部分，能够实现对无人艇航行状态的自主控制和实时监测功能，基于LABVIEW的地面站软件控制系统可以满足无人艇组装测试阶段的实验要求和航行阶段的操纵控制与自主导航的要求.地面站主要功能有：

1) 实时监测 实时监测无人艇工作时的系统状态，包括工作模式、电池电压和舵机电压等，显示当前无人艇的GPS位置信息，监控航向PID参数；

2) 智能分析 通过采集航向PID的目标值，当前值，控制量参数,分析其变化趋势,并结合其他传感器信息统一分析,从而做出控制决策，保证无人艇的航行安全，如油门控制、档位控制和航向控制等；

3) 电子地图导航 电子地图导航功能通过MapInfo公司推出的基于Active技术的可编程控件Mapx来实现.将MapInfo格式的地图文件导入LABVIEW软件平台，实现对地图图层的加载、居中、平移和放缩等操作，并通过电子地图写入目标点坐标，实现GPS导航功能；

图6 地面站主界面

4) 故障诊断 对无人艇上搭载的传感器、电路等其他设备进行状态监控，对故障及时警报并采取处理措施；

5) 数据处理、保存与显示 直观显示各传感器采集到的数据,并实现实时数据存储.

地面站功能结构如图5所示.

2.3.1 地面站主界面设计 为了满足无人艇组装测试阶段的实验要求和航行阶段的操纵控制与自主导航的要求，地面站主界面包括6个模块，分别为串口设置模块、系统状态模块、航向PID监控模块、控制区域模块、GPS位置信息模块和PID参数设置模块，地面站主界面如图6所示.

2.3.2 通信协议 无人艇要实现与地面站控制系统有效、可靠、无差错地通信，离不开通信协议的支持，数据帧格式和字节数都是在通信协议中定义.地面站控制系统根据约定的通信协议将无人艇发送的数据进行数据解析，并把解析后数据更新到波形和数值显示.同时地面站控制系统也将按照约定的通信协议将操作命令数据封装成数据帧发送给无人艇，无人艇再按照通信协议解析，并执行相应操作,此过程提高了数据传输的可靠性、安全性以及传输的容错能力.本文所采用帧格式如表1所示.

表1 帧格式

3 系统调试与实现

无人艇在海南省三亚市三亚湾进行了海上测试，测试内容包括无人艇的运动姿态的稳定性，航向PID控制的精准性，无线数据传输的稳定性和地面站与无人艇命令交互的及时性.测试无人艇采用长为3.8 m，重量120 kg的冲锋艇，测试条件为东北风5～6级，浪高1 m，涌高1.5 m，海温21.2～26.9 ℃.

经过多次重复的实验发现，在船舶众多电磁环境较复杂的三亚湾港口，无人艇姿态测量数据基本未受到影响，无人艇能快速准确地执行来自地面站的命令，当风和海浪将无人艇吹离规划目标点时，无人艇能快速地自主修正航向.

图7 测试路径

测试结果表明，无人艇可以对地面站控制系统的指令做出快速响应，自主调整航向到达目标点；航行速度可在0～2 m·s-1范围内灵活变化；无人艇航行姿态比较稳定，在有风浪的海面具备良好的适航性.测试路径如图7所示.

在无人艇姿态测试中，绿色线代表航向角，红色线代表俯仰角，蓝色线代表横滚角.由图8可知在测试时间内，地面站能精确的跟踪无人艇姿态的变化，各个方向上的姿态角数据平滑变化，无突跳情况出现.

在无人艇航向PID控制测试中，绿色线代表地面站输出的目标角度，红色线代表无人艇的当前实际角度.由图9可知在测试时间内，航向角在达到稳态后，绿色线和红色线基本重合，无人艇的实际航向角度基本等于地面站输出的目标航向角度，测试结果的误差在允许范围内，进而证明了无人艇自主导航控制系统的可行性.

4 小 结

设计了基于STM32的无人艇自主导航控制系统，对无人艇相关硬件及整体参数的构成进行了说明，对控制模块所涉及算法进行了描述，并基于地面站对各模块进行调试.实验证明该无人艇稳定性好，可通过地面站系统进行远程操控，同时实现自主导航的功能，适用于海洋环境的监测以及海事搜救.

[1] Yan R, Pang S, Sun H, et al. Development and missions of unmanned surface vehicle[J]. Journal of Marine Science and Application, 2010, 9(4): 451-457.

[2] Ellenrieder K D V. Development of a USV-based Bridge Inspection System[C]. Washington: IEEE Press, 2015.

[3] Liu Y, Noguchi N. Development of an unmanned surface vehicle for autonomous navigation in a paddy field[J]. Engineering in Agriculture Environment & Food, 2015, 9(1):21-26.

[4] Scott G P, Henshaw C G, Walker I D, et al. Autonomous Robotic Refueling of an Unmanned Surface Vehicle in Varying Sea States[C]. Hamburg: IEEE Press, 2015.

[5] 任光，牛井超，颜德文，等. 新型船舶运动控制平台及海上试验[J]. 中国造船，2002,43(1):64-69.

[6] 刘铁军，刘文远. 航天新光天人船 奉献科技奥运[J]. 航天工业管理，2008(8):46-46.

[7] 吴恭兴. 无人艇操纵性与智能控制技术研究[D]. 哈尔滨：哈尔滨工程大学，2011.

[8] 廖煜雷. 无人艇的非线性运动控制方法研究 [D]. 哈尔滨：哈尔滨工程大学，2012.

[9] 曾薄文. 喷水推进水面无人艇的非线性控制方法研究[D]. 哈尔滨：哈尔滨工程大学，2012.

[10] 冯浩明. 水面无人船航行控制器开发研究[D]. 哈尔滨：哈尔滨工程大学，2013.

USV’s Autonomous Control System Design Based on STM32

Zhang Yuting, Chen Baodan, Ren Jia, Vladimir Shikhin

(1. College of Information Sciences and Technology, Hainan University, Haikou 570228, China;2. Institute of Automatics and Computer Engineering, Moscow Power Engineering Institute, Russia)

Aiming at the problem of the Unmanned Surface Vessel’s (USV) autonomous navigation, a autonomous navigation control system for USV based on STM32 was designed. The STM32 microprocessor was used as the core control unit of shipboard system. The complementary filtering algorithm was used to calculate gyro angular velocity data, accelerometer angel data and magnetometer heading data, cascade PID controller was used for the closed loop control; LABVIEW software was used to design and develop a ground control station, which achieve the function of the USV’s remote control and autonomous navigation.

USV; position calculation; PID controller design; ground control station design

2016-05-12

国家国际科技合作专项(2015DFR10510)；国家自然科学基金(61562018)；海南省高等学校科学研究项目(HNKY2014-04)；海南省重点实验室和工程技术研究中心建设专项项目(gczx2014004)

张玉婷(1992-)，女，重庆荣昌人，海南大学2014级硕士研究生，研究方向：海洋通信，E-mail:17982894@qq.com

陈褒丹(1962-)，男，海南海口人，高级工程师，研究方向：海洋通信，E-mail:1417252623@qq.com

1004-1729(2016)03-0221-07

TP 181

A DOl：10.15886/j.cnki.hdxbzkb.2016.0034