差速转向式双推无人艇航向与航速联合控制研究

袁 健

（齐鲁工业大学（山东省科学院），海洋仪器仪表研究所，山东省海洋监测仪器装备技术重点实验室，国家海洋监测设备工程技术研究中心，山东 青岛 266100）

0 引言

无人水面艇（Unmanned Surface Vessel，USV）是一种智能化、自主化的海洋无人智能装备，在水文环境探测和监测、海洋测绘、海洋维权、敌情侦测等领域将发挥重要作用.其配备先进的供电系统、控制系统、传感器系统、通信系统，可进行大范围海洋环境信息采集和监测[1-2]，军事方面可执行多种战争和非战争军事任务，如侦察、反潜、反特种作战以及巡逻等作战任务，在无人作战系统占有重要位置，因此倍受世界各国的重视[3-5].面对复杂未知的海洋环境，如何设计一种稳定可靠的无人艇自主控制系统，如何实现无人艇的自主航行控制以及自主导航策略是无人水面艇自主控制研究的关键技术[6-8]，也是当下海洋无人系统智能控制的一个热点.无人艇航行时受到海浪、海风等外扰动的影响，是一个非线性、不确定性和动态不稳定性系统，因此，在这种情形下，基于无人艇动态模型建模的控制律设计方法其实际的控制效果不佳.PID控制算法具有结构简单，鲁棒性强的优点，但当无人艇动态特性或所处环境发生变化时，其控制参数需进行适应性修订，否则将导致控制效果变差，参数的修订受到人员经验、外界干扰等影响，不可控且低重复性，因此为提升传统PID控制算法性能，增强其适应性，需要对传统PID控制算法进行优化改进.文献[9]研究了具有不确定性扰动的水面无人艇的航迹跟踪控制，提出一种鲁棒的轨迹跟踪控制器设计方法用于处理外扰动不确定情形下的控制设计.文献[10]提出一种修正的模糊神经网络方法，用于实现无人艇的航向控制.文献[11]采用模糊控制方法设计了无人艇模糊控制规则和推理，实现了不依赖于动力学模型的模糊PID航向控制算法，并应用于无人艇航控系统试验.文献[12]针对无人艇航向控制中存在的模型参数和外界干扰不确定性问题，基于反步控制思想设计无人艇的航向控制律，提出一种由舵角控制输入控制航向的自适应航向控制算法，利用李雅普诺夫函数验证该控制算法能够使得闭环控制系统一致有界.文献[13]建立了无人艇操纵运动模型，设计了一种基于轨迹线性化的快慢回路时标分离的无人艇航向控制方法，并将非线性干扰观测器设计方法与轨迹线性化控制方法结合，提出基于非线性干扰观测器的轨迹线性化航向控制方法.文献[14]对无人艇的航向跟踪控制系统和外部环干扰进行了数学建模，并基于分数阶微积分方法设计了一种航向控制的分数阶控制器，进一步给出了一种径向基函数神经网络学习算法，实现了该控制器的参数整定.

本文针对一种双电推的无人艇航向和航速联合控制问题，研究对象为大容量锂电池为能源的，两个涵道式推进器推进的小型无人水面监测艇，该无人艇的推进装置由2个对称设置于无人艇底的，可以正反向转动的涵道式推进器组成.双推无人艇航向控制与舵控无人艇不同，其通过控制左右推进器的转速差进行航行控制，转速差大则无人艇转向幅度大，转速差小则无人艇转向幅度小，很难精确确定转速差与转动角度之间的关系式，另一方面，无人艇的精确动力学模型建模也非常困难，因此我们采用模糊PID控制方法，模拟人工操控无人艇的操作模式，对操控规则采用模式语言进行描述和模糊化处理，提出了航向和航速联合控制的模糊PID闭环控制方法，通过构建模糊控制规则表，并以无人艇航向的实际误差及误差变化率作为模糊控制的输入，PID的参数作为模糊输出，实现PID控制器控制参数自适应调整，增强无人艇控制的环境应变能力.进一步根据系统控制流程编写了闭环控制模块软件，并设计了下位机控制器和运动控制器的控制协议.最后通过岸边实验获取了该无人艇的速度控制因子，并通过仿真和岸边实验验证了所提控制方法的有效性.

1 控制系统设计

该双推无人艇的控制系统软件包括上位机控制软件、下位机主控制软件、下位机运动控制软件，控制结构如图1所示.上位机和下位机控制软件分别安装在两块PCM9310主板上，操作系统为ubuntu16.2，控制软件采用MOOS-Ivp框架进行开发，该软件框架为MIT和Oxford机器人实验室共同开发和维护的基于C++的分布式机器人开源控制软件系统框架[15].下位机运动控制器采用STM32F103ZC控制板.上位机与下位机主控制器之间通过无线网桥进行通信连接，下位机主控制器与运动控制器之间通过RS323串口进行数据通信.本文主要研究双推无人艇航向与航速控制方法，主要涉及下位机控制软件和运动控制器的算法和软件设计，上位机的主要功能是实现用户控制命令的下发和下位机采集数据的呈现，不在本文讨论范围之内.由于采用电子调速器控制推进电机与采用伺服电机控制推进电机的控制方式不同，电子调速器可以接受1 000-2 000 us的脉宽调制（PWM）信号，通过控制单片机的PWM引脚产生1 000-2 000 us的PWM信号输出给电子调速器，电子调速器才能响应.其中1 000-1 500 us为电调控制直流无刷电机反转信号，1 500 us到2 000 us为电调控制电机正转信号，中值1 500 us为电机停止信号，并且PWM值与1 500 us的差值越大，则转速越高，所以双推无人艇航向控制与舵控无人艇不同，其通过控制左右推进器的转速差进行航行控制，转速差大则无人艇转向幅度大，转速差小则无人艇转向幅度小，其关系式很难精确描述，为此我们针对双推无人艇航速和航向联合控制的需求，研究了MOOS-Ivp架构下相应软件模块的功能和系统组成，并配置了保证控制系统正常运行所需的软件模块，提出了航向和航速联合控制的模糊PID闭环控制方法.其采用星型组织架构，以MoosDB为软件体系的消息发布中心，其他模块化相对独立，并通过消息的订阅和发布机制进行组织的软件架构模式.该软件架构可以实现各个软件模块的灵活配置，自由增减，通过配置文件即可实现对软件模块的配置，非常适合于复杂环境下无人艇分布式控制软件的研发.

图1 无人艇控制结构

1.1 MOOS的核心模块

MOOS-Ivp软件模块安装在下位机主控制器PCM9310板上.该软件运行环境为ubuntu16.2，通过*.moos文件进行系统的初始化参数配置，可以选择程序需要启动的软件模块，以及实现对应模块的参数初始化配置，通过团体名实现对模块组的区分.本项目针对无人艇控制的具体需求，采用的MOOS软件系统可以实现分布式控制，在上位机运行一个MOOSDB模块、一个可视化用户界面（pMarineViewer）模块；其中MOOSDB模块需要给定其团体名和IP地址，下位机需要启动的MOOS软件模块为MOOSDB，pHelmIvp，pLogger，pMarinePID，pNodeReporter，pShare，uMS，uSimMarine，pCompass、pAct等模块.其中，MOOSDB为MOOS软件架构体系的核心，是其他模块订阅和分发消息的中心.pHelmIvp为系统的决策输出模块，通过采集uSimMarine模块的输出和上位机用户输入的设定速度和航向信息，并对输入信息进行决策规划，输出期望速度desired_speed和期望航向desired_heading.pLogger模块以文件的形式实现对系统运行过程中消息的全纪录.pMarinePID为航速和航向PID控制模块.pNodeReporter模块设计无人艇节点相关信息的记录.pShare通过UDP的方式实现两个团体之间的数据通信，需要设置其端口号，如input为9 200，output为9 201.uMS模块可以对各个模块发布的信息进行统一的界面化现实.uSimMarine为系统仿真模块，通过系统仿真模块模拟真实无人艇的导航速度、航向的输出.pCompass主要实现组合导航模块的航向和航速信息获取，模块通过USB转232串口模块采集电子罗盘的航向角和GPS输出的航速值，将航向角标准化到0-360°范围，航速单位转换到m/s.并将采集的这些信息通过Notify（参数1，参数2）函数发布到MOOSDB模块.pAct模块一方面实现对运动控制器上传的电压、电流数据帧进行分析处理和发布，一方面将pMarinePID模块的输出的期望PWM值（Desired_thrust）和期望转动角度（Desired_rudder）进行打包装帧处理，并通过串口将成帧数据发送到运动控制器.

1.2 下位机运动控制器

下位机运动控制器通过485接口实现对温湿度、电压电流模块信息的采集，同时解析下位机主控器PCM9310下发的控制帧，解析出对应字段信息继而驱动对应的IO口和PWM信号输出，将左右PWM信号输出给对应推进器的电调进行转速控制.下位机运动控制器采用STM32F103ZC微处理器模块，晶振：72 MHz，具有16路PWM输出功能.本项目采用TIM3的2路PWM输出端口对应左右推进器转速的脉宽调制.由于所采用电调的工作频率为50 Hz，所以在初始化程序中需要设置PWM通道的输出频率：PWM频率为50 Hz，初始化PWM频率的函数参数为（72 000 000，71，19 999），计算过程为：72 000 000/20 000/72＝50 Hz.

1.3 下位机主控制器与运动控制器的通信协议

该通信协议为下位机主控制器发送到运动控制器的协议，主要功能为实现对下位机主控制器pAct软件模块所封装的控制信息下发给下位机运动控制器PCM9310板，如图2所示.ASCII编码方式，采用逗号分隔，其协议格式为：

图2 下位主控制器与推进器控制器之间的协议格式

其中，$USV—表示数据帧开始；

手动自动—取值为A和M，A表示自动控制信号，M表示遥控器控制；

运行指令—取值为0，1，2，3，4.0表示前进，1表示停止，2表示后退，3表示左转，4表示右转；

左调速PWM—取值在1 000 us和2 000 us之间，其中1 000-1 500 us表示推进器反转，1 500-2 000 us表示推进器正转；

右调速PWM—取值在1 000 us和2 000 us之间，其中1 000-1 500 us表示推进器反转，1 500-2 000 us表示推进器正转；

CRC—CRC-16，UTF-8格式编码；

这里给出一个示例，如：$USV，A，3，1 600，1 800，XX表示下位机主控制器发送的指令为的自动控制指令，左转，左推进器PWM值为1 600 us，左推进器PWM值为1 800 us，XX表示两个字节的CRC.

2 控制原理

2.1 双推无人艇航向模糊控制原理

双推无人艇航向控制与舵控无人艇不同，其通过控制左右推进器的转速差进行航行控制，转速差大则无人艇转向幅度大，转速差小则无人艇转向幅度小.对于有舵的无人艇而言，当舵角为负值时，表示舵顺指针转动，此时无人艇逆时针转角.同理，当舵角为正值时，表示舵逆指针转动，此时无人艇顺时针转角.Moos-ivp自带MarinePID模块，可以实现基本的PID控制，其根据模块订阅的航向角（NAV-heading）、航速（NAV-speed）、期望速度（Desired_speed）、期望航向（Desired_heading）等信息，产生期望PWM值（Desired_thrust）、期望转动角度（Desired_rudder）.传统PID航向控制算法其控制原理为

其通过调节KP、KI、KD实现航向自动控制，其中KP影响系统响应速度和控制精度，过大易引起震荡，过小则降低系统响应速度；KI起到消除系统稳态误差的作用，过大会引起超调，过小则降低控制精度；KD用于预测偏差变化，适当的值可有效提升系统动态特性.但当无人艇动态特性或所处环境发生变化时，其控制参数需进行适应性修订，否则将导致航向控制效果变差，然而参数的整定受到人员经验、外界干扰等影响，不可控且低重复性，因此为提升传统PID航向控制算法性能，增强其适应性，需要对传统PID航向控制算法进行优化改进.所以由于海流、海浪和风的影响导致系统的PID控制参数需要进行适应性调节，利用模糊控制方法根据航向角度误差和角速度误差实时调整PID三个参数.基本的PID航向控制其适用于舵控的无人航行器，对于双推差分调速的无人艇，由于没有舵角控制信息，所以需要将产生的舵角控制信号区间映射到左右推进器所需的PWM控制信号的区间上.但这一映射关系由于转向角度和推进器推力差之间以及航速之间的关系并不明确.因此需要实验研究这一映射关系.在同一个转速推力差的前提下，设置不同的航速，进行岸边实验，得到不同航速下两个推进器转速差值与转向角度的关系曲线.为了使无人艇控制程序能适应不同的海流、风等扰动的影响，适应性调节PID算法的控制参数，采用模糊控制方法，以航行偏差和偏差率作为模糊控制器的输入，PID的比例、积分和微分系数作为输出，根据不同的航向偏差和偏差率适应性地输出解模糊后的控制参数，模糊PID控制器结构如图3所示.

图3 模糊PID控制器结构

式中，c决定了隶属度函数的中心点，σ决定了函数曲线的宽度.e（t）、（t）的隶属度函数如图4所示.

图4 误差和误差导数的隶属度函数

无人艇航向航速联合控制方法控制无人艇转向时，由于初始航向偏差较大，先给一个较大的差速转角，让转向运动快速启动、加速，待回转角速度达到稳态后，产生一个适当的转角维持等角速度回转；当实际航向接近期望航向时，产生一个较小的差速转角，直至达到期望航向和航速.接着进行模糊推理，并根据模糊规则，将其规则转换为可以程序实现的模糊语句：If（e（t）isX）and（（t）is Y）then（KPis Z），设置模糊推理器，对KP、KI、KD分别进行解模糊处理，其中的解模糊后输出结果如图5所示.

2.2 控制方法

在所涉及的MOOS下位机软件模块中，我们设计了三个模块参与了无人艇航向和航速控制，分别是：pHelmIvp、pAct、PIDmarine模块.其中pHelmIvp模块根据订阅的无人艇的即时航向NAV-heading、航速NAV-speed以及设定的航速Desired_speed-debug和设定航向Desired_heading-debug信息，产生Desired_speed、Desired_heading；PIDmarine模块订阅NAV-heading、NAV-speed、Desired_speed、Desired_heading，经过PID运算产生Desired_thrust、Desired_rudder，进一步将Desired_rudder转化为无人艇左右推进器的PWM值：thrustleft、thrustright，通过Notify（）函数进行发布.pAct模块订阅PIDmarine模块发布的thrustleft、thrustright信息，并根据下位主控制与运动控制器之间的通信协议生成控制帧，装帧完成后通过串口发给下位机运动控制器PCM9310板.pAct模块还需完成航向信息（NAV-heading）、航速信息（NAV-speed）的数据采集，NAV-heading信息是通过串口采集的北微电子罗盘输出的航向角信息，NAV-speed信息是通过串口采集的GPS模块的航速经过转换而实现的，即时二者信息进行发布，满足PID控制模块闭环控制的需求.并且pAct模块通过串口采集运动控制器上传的电压、电流数据帧，并按照设计的协议进行数据字段的解析，解析出来的左右推进器的电压、电流、主控制器的电压和电流，以及控制舱的温湿度信息进行消息的发布，其他软件模块均可对该信息进行订阅和显示.

2.2.1 航向控制

航向控制需要实现的功能是通过上位机给定一个航向角Desired_heading并发布，下位机PID模块订阅该期望航向角信息，以及pAct发布的NAV-heading信息，通过PID控制算法实现航向的保持，具体控制流程如图5所示.我们采用电子罗盘反馈航向角信息（NAV-heading）.当PID模块产生的无人艇的Desired_rudder为负值，其代表无人艇需要控制顺时针转动，此时要求无人艇右边推进器的推力大于左边推进器的推力，即右边的PWM值大于左边的PWM值.类推，当PID模块产生的无人艇的Desired_rudder为正值，代表无人艇需要控制其逆时针转动，此时要求左边推进器的推力大于右边推进器的推力，即左边的PWM值大于右边的PWM值.具体的左右推进器的PWM值产生方法为：在当前推进器PWM值Desired_thrust的基础上，左边推进器的PWM值=左边推进器的当前PWM值＋rudder*500/30；右边推进器的PWM值=右边推进器的当前PWM值-rudder*500/30，其中rudder为经过PID产生的转向角值（Desired_rudder）；500对应着推进器PWM差值为500 us且作用时间为1 s，此时无人艇最大转向角度约为30°，左右推进器的PWM值的差值为500 us，并且经过实验得知rudder角与PWM差值之间存在线性关系.由于PWM存在饱和的情况，我们设定经过计算后的左右推进器的PWM值（thrustleft、thrustright），若其值大于2 000 us，则其值取为2 000 us，若其值小于1 000 us，则其值取为1 000 us.

图5 航向反馈的闭环航向控制方法

2.2.2 航速控制

航速控制需要实现的功能是通过上位机给定一个航速Desired_speed并发布的情形下，下位机PID模块订阅该期望航速信息，还需订阅pAct发布的NAV-heading信息，通过PID控制算法实现无人艇航速的保持，具体控制流程如图6所示.我们采用GPS反馈无人艇航速信息.通过设置一个速度因子变量Speed_factor，参数配置其是否为零决定了程序是采用常规控制还是模糊PID控制，当Speed_factor直接给定非零时，通过计算以下公式计算推进器的PWM值：推进器的PWM值＝Speed_factor*Desired_speed＋1 500，其中Desired_speed为通过上位机给定的期望速度，通过该公式可以计算给定的推进器的PWM值，计算完成后将Desired_thrust进行发布，从而实现直接驱动推进器产生给定的转速，控制流程如图7所示.当Speed_factor参数配置为直接给定零时，程序采用PID航速控制，通过pAct模块采集GPS的速度反馈值，与上位机给定的速度值相减，通过PID计算delta_thrust，通过 Desired_thrust＝Desired_thrust＋delta_thrust计算 Desired_thrust.其中Speed_factor变量的给定，是通过下位机MarinePID模块初始化参数里的Speed_factor参数进行初始化的，MarinePID模块里的函数vclip（dval，0，Speed_factor）取出该值并赋值给m_Speed_factor，然后通过setspeedfactor（）方法实现对MarinePID模块中该Speed_factor参数的设置.

图6 Speed_factor=0时速度反馈的航速模糊PID控制方法

图7 Speed_factor!=0时带速度因子的常规PID航速控制方法

3 实验与分析

岸边实验2：在给定的无人艇航速下和左右推进器PWM差值下，无人艇会做一个圆周运动，但是由于短时间内不容易测量转向角，我们通过观测无人艇圆周运动单位时间内的转角变化，也即角速度是否恒定来决定是否启用无人艇角速度测量.无人艇进行圆周运动时，初始运动状态不稳定，待无人艇运动稳定后，程序测量两次相同航向角时，无人艇所转动的角度和所用的时间t，根据公式：角速度ω＝弧度变化量/所用时间，计算角速度ω＝2/一圈的时间，得到角速度ω.我们记录了PWM中值固定情形下，不同转速差值下的角速度值，如表2所示.通过实验发现，当左推进器PWM＝1 350 us，右推进器PWM＝1 850 us时，无人艇有最大角速度为0.52 rad/s.

为了验证提出航速和航向控制方法的有效性，一方面通过MOOS系统的模拟功能实现对控制方法有效性的验证，在模拟实验验证通过的基础上，进行了岸边控制功能的实验验证，并通过岸边实验获取了无人艇的转角速度信息，进一步优化了无人艇的控制算法相关参数.

岸边实验1：该实验是为了实现航速与PWM值关系曲线和Speed_factor参数的获取，如图8所示.在上位机设置PWM值为1 550 us到2 000 us的如表1所示的数值，pAct将自动获取Desired_thrust并将两个推进器的PWM设置为表1中的PWM值，通过GPS模块测得无人艇航速如表1所示.根据速度与PWM值增量的线性关系，可以获取无人艇系统的速度因子Speed_factor为25.Desired_thrust与推进器PWM值之间的转换关系为：推进器PWM值＝k*Desired_thrust＋1 500，这里k＝5为转化系数.将Speed_factor值写入到下位机MOOS系统的MarinePID模块的初始化参数中，或者将MOOS初始化文件的Speed_factor修改为25，为下一步模拟仿真做好准备.

表1 测得的PWM值与推进器转速、航速的关系（1节海流速度）

表2 Delta_PWM＝100，150，200，250，300，400，500us时的角速度值

图8 无人艇岸边试验

模拟仿真1：采用常规PID的航速控制和航向的闭环控制方式.在MOOS初始化配置中，将Speed_factor参数设置为125，通过计算Desired_thrust＝speed_factor*Desired_speed，计算期望的推进器PWM值，并写入到数据帧的对应字段中.进一步运行系统相应模块和MarineViewer界面，进行八边形路径点跟踪，仿真结果如图9所示.可以看到无人艇系统可以实现对八边形航路点的跟踪，图9中最下面为无人艇的期望航向角以及实际的航向角的实时曲线.

图9 无人艇的航路点跟踪控制结果、期望航向角以及实际的航向角

模拟仿真2：采用模糊PID的航速控制和航向的闭环控制方式.在MOOS初始化配置中，将Speed_factor参数设置为0，通过计算Desired_thrust＝Desired_thrust＋delta_thrust，计算期望的推进器PWM值，并写入到数据帧的对应字段中.进一步运行系统相应模块和MarineViewer界面，进行八边形路径点的跟踪，仿真结果如图10所示.可以看到采用模糊PID的航速控制和航向的闭环控制模式，无人艇系统可以实现对八边形航路点的跟踪.当期望速度为3.5 m/s时，对比常规PID速度控制与模糊PID速度控制的控制精度，常规速度控制方法的速度误差为1 m/s以内.当PID算法的比例系数KP＝0.5，KD＝2.0，KI＝0时，与常规PID速度控制方法相比，模糊PID速度跟踪控制方法对期望速度的跟踪精度要高一个数量级，速度误差控制在0.1 m/s以内.

图10 无人艇的航路点跟踪控制结果、期望航向角以及实际的航向角

4 结论

从无人水面监测艇自主控制的工程设计需求出发，针对无人监测艇的自主控制系统的结构组成、各功能模块的技术要求、下位机主控制模块和运动控制模块的设计及其通信控制协议进行了设计，基于MOOS-Ivp软件平台编写了航向和速度联合控制的分布式控制软件模块，对比了采用常规PID控制和模糊控制方法的无人艇航向航速控制效果，并在岸边试验站对该试验艇的运动控制系统进行了实验验证，证明了基于模糊控制方法的无人艇自主控制方法的有效性.