无人船航向控制自动纠偏方法及系统与流程

本发明涉及无人船技术领域，特别涉及一种无人船航向控制自动纠偏方法及系统。

背景技术：

无人船的航向控制是指通过遥控器操作无人船方向、油门控制信号输出，达到无人船按照控制路线行驶目的。

现有无人船的人工航向控制常用的处理系统和方法，包括无人船控制系统接收到人工控制的遥控发射机的方向和油门控制信号后，直接将方向和油门输入信号转换为无人船动力控制单元部分的方向和油门电机输出。但是，这种控制没有考虑无人船的水域情况及无人船电机硬件在水下工作的场景特性，导致在实际的航行过程中，现有无人船动力控制部分往往会因为水流、天气等因素无法精确执行航线要求，特别是在直线航线控制航向时容易出现偏差，同时，由于无人船用于动力输出的电机工作场景在水下，电机受水流的影响在遥控控制方式下输入和输出性能特性也无法达到线性比例从而导致航线过程出现偏差，尽管通过改造增加电机硬件输出反馈装置来校验电机的硬件特性来适应水下场景，但实现技术难度复杂并且会增加无人船的整体成本。

技术实现要素：

为了克服上述现有技术的不足，本发明提供一种无人船航向控制自动纠偏方法及系统，实现了人工模式控制无人船精确直线航行，本发明通过自动实时修正航向动力，不增加硬件成本且简单易用。

本发明无人船航向控制自动纠偏方法，通过航向偏差纠正值补偿航向动力输出，所述航向偏差纠正值采用以下方式计算：

航向偏差乘以比例负常数按比例消除偏差，得到第一航向偏差修正数据；

对航向偏差进行微分计算后乘以微分控制比例负常数，得到第二航向偏差修正数据；

对航向偏差进行积分计算后乘以积分控制比例负常数，得到第三航向偏差修正数据；

将第一航向偏差修正数据、第二航向偏差修正数据、第三航向偏差修正数据相加，得到航向偏差纠正值。

本发明无人船航向控制自动纠偏系统，包括动力输出控制单元，通过航向偏差纠正值补偿航向动力输出，所述动力输出控制单元包括：

航向偏差保存单元，用于保存航向偏差

比例消除偏差单元，用于对航向偏差乘以比例负常数按比例消除偏差，得到第一航向偏差修正数据；

微分消除偏差单元，用于对航向偏差进行微分计算后乘以微分控制比例负常数，得到第二航向偏差修正数据；

积分消除偏差单元，用于对航向偏差进行积分计算后乘以积分控制比例负常数，得到第三航向偏差修正数据；

累计单元，用于将第一航向偏差修正数据、第二航向偏差修正数据、第三航向偏差修正数据相加，得到并输出航向偏差纠正值。

与现有技术相比，本发明的有益效果在于：将航向偏差分别经过比例、微分、积分消除航向偏差并反馈作用系统，达到实际输出偏航角无限趋近于预置目标偏航角的目标，提升无人船人工控制模式航行控制精度，对直线航线作业任务特别有效。在满足无人船人工控制需求应用中，将自动航向纠正控制与无人船人工控制模式融合，创造一种基于人工控制的自动纠偏模式，并且在不增加无人船硬件成本的情况下达到更易于消除复杂水域航行带来的航行偏差，使得本发明更实用、易用。

附图说明

图1为本发明无人船航向控制自动纠偏方法优选实施例流程示意图；

图2为本发明无人船航向控制自动纠偏系统优选实施例结构示意图。

具体实施方式

为使本发明的目的、技术方案和优点表达得更加清楚明白，下面结合附图及具体实施案例对本发明做进一步详细说明。

本发明无人船航向控制自动纠偏方法，通过航向偏差纠正值补偿航向动力输出，如图1所示，所述航向偏差纠正值采用以下方式计算：

S101、航向偏差乘以比例负常数按比例消除偏差，得到第一航向偏差修正数据；优选地，包括：

S101-1、输入航向偏差数据error_course。

S101-2、设置一个比例负常数Kp，其是按比例消除偏航角偏差数据的负常数，其值优选为[-0.6,-0.4]。

S101-3、将偏航角偏差数据error_course乘以比例负常数Kp，得到第一航向偏差修正数据output1＝error_course+error_course×Kp。

S102、对航向偏差进行微分计算后乘以微分控制比例负常数，得到第二航向偏差修正数据；

S102-1、确定一个微分控制比例负常数Kd，其值优选为[-0.6,-0.4]。

S102-2、将偏航角偏差数据error_course进过微分一阶求导后，通过时间预估误差变化趋势derivative：

derivative＝(error_couser(k)-error_course(k-1))/T

本发明T表示周期计算航向偏差的采样间隔，error_couser(k)表示第kT时刻得到的航向偏差。

S102-3、将误差变化趋势乘以微分控制比例负常数Kd，，得到第二航向偏差修正数据output2＝derivative×Kp。

S103、对航向偏差进行积分计算后乘以积分控制比例负常数，得到第三航向偏差修正数据；

S103-1、确定一个积分控制比例负常数Ki，其值优选为[-0.06,-0.04]。

S103-2、计算一定时间周期T内偏航角的累计偏差output3

output3＝(error_course(k-1)+error_course(k))×Ki×T；

S104、将第一航向偏差修正数据、第二航向偏差修正数据、第三航向偏差修正数据相加，得到航向偏差纠正值output

output＝output1+output2+output3

通过周期获取实际偏航角输出反馈，输入新的航向偏差error_course，经过比例、微分、积分消除航向偏差并反馈作用系统，达到实际输出偏航角无限趋近于预置目标偏航角的目标。

实际上，对于消除error_course，其离散状态的计算公式可以归纳为：

u(k)＝Kp×error_course(k)+Ki∑error_course(k)

+Kd×(error_couser(k)-error_course(k-1))/T

优选地，所述航向偏差error_course采用以下方式获取：

采用预置目标航向值target_course与实时获取的偏航角arhs.yaw的差值为航向偏差，即error_course＝target_course-arhs.yaw。

所述预置目标航向值设置方法包括如下：

当进入某模式，将第一次从姿态解算单元ARHS获取的偏航角(yaw)设置为预置目标航向值，其是控制无人船直线航行参照的关键指标。后面每个采样周期获取的偏航角为实时获取的偏航角arhs.yaw。

通过无人船控制系统姿态解算单元ARHS可以解算出无人船的船体坐标系与地面坐标系，这两个坐标系之间的关系是三个欧拉(Euler)角，分别是俯仰角(pitch)、偏航角(yaw)、滚转角(roll)，这三个角可以反应船体相对地面的姿态，其中偏航角表示船体坐标系xb轴在水平面上投影与地面坐标系xg轴在水平面之间的夹角，由地面坐标系xg轴逆时针转至船体坐标系xb轴的投影线时，偏航角为正，及船头右偏航为正，反之为负。

特别说明的是，ARHS是一种主要采用惯性传感器以数据融和算法精确测量空间姿态的算法，其为现有技术，本专利并不是研究ARHS本身，只是用到这个算法输出的航姿结果。

ARHS解算的yaw偏航角即用来表明当前航向偏转角度，而pitch、roll常用于空中姿态位置标识，无人船基于水平面，所以pitch、roll对本发明实际参考意义不大。

优选地，所述航向偏差error_course可以进一步优化，对航向偏差进行过滤，即考虑无人船水域作业场景，启动自动纠偏模式后，偏航角数据的过滤策略为基于原始参考目标原点正负偏差45°范围为有效数据，即基于参考原点左偏或右偏45°的航向偏差判定有效航向偏差，大于这个范围时为无效航向偏差，忽略该值。

优选地，所述航向偏差的计算在自动纠偏模式下进行。

优选地，所述自动纠偏模式采用以下方式进行判断，即判定一定周期内(优选10ms)无人船控制系统遥控方向信道输入值steering的变化范围，当steering的变化范围介于输入范围中间值正负1％区间，说明处于直线航线操作，判断为自动纠偏模式；如果steering输入值超出输入范围中间值正负1％区间范围浮动，说明处于非直线航线操作，则不是自动纠偏模式。

所述捕获无人船控制系统遥控方向信道输入值steering包括控制系统读取遥控方向控制信道的脉冲宽度调制PWM，PWM表示方波高电平时间跟周期的比例，即占空比，例如1秒高电平1秒低电平的PWM波占空比是50％。

优选地，所述自动纠偏模式运行在人工遥控控制模式下。

优选地，所述人工遥控控制模式采用以下方式进行判断：捕获和解析无人船控制系统控制模式信道输入值，当输入值control_mode为手动方式manual，即为人工遥控控制模式。

本发明无人船航向控制自动纠偏系统，包括动力输出控制单元，通过航向偏差纠正值补偿航向动力输出，如图2所示，所述动力输出控制单元包括：

航向偏差保存单元，用于保存航向偏差。

比例消除偏差单元，用于对航向偏差乘以比例负常数按比例消除偏差，得到第一航向偏差修正数据；优选地，包括：

S101-1、输入航向偏差数据error_course。

S101-2、设置一个比例负常数Kp，其是按比例消除偏航角偏差数据的负常数，其值优选为[-0.6,-0.4]。

S101-3、将偏航角偏差数据error_course乘以比例负常数Kp，得到第一航向偏差修正数据output1＝error_course+error_course×Kp。

微分消除偏差单元，用于对航向偏差进行微分计算后乘以微分控制比例负常数，得到第二航向偏差修正数据；

S102-1、确定一个微分控制比例负常数Kd，其值优选为[-0.6,-0.4]。

S102-2、将偏航角偏差数据error_course进过微分一阶求导后，通过时间预估误差变化趋势derivative

derivative＝(error_couser(k)-error_course(k-1))/T

本发明T表示周期计算航向偏差的采样间隔error_couser(k)表示第kT时刻得到的航向偏差。

S102-3、将误差变化趋势乘以微分控制比例负常数Kd，，得到第二航向偏差修正数据output2＝derivative×Kp。

积分消除偏差单元，用于对航向偏差进行积分计算后乘以积分控制比例负常数，得到第三航向偏差修正数据；

S103-1、确定一个积分控制比例负常数Ki，其值优选为[-0.06,-0.04]。

S103-2、计算一定时间周期T内偏航角的累计偏差output3

output3＝(error_course(k-1)+error_course(k))×Ki×T；

累计单元，用于将第一航向偏差修正数据、第二航向偏差修正数据、第三航向偏差修正数据相加，得到并输出航向偏差纠正值output

output＝output1+output2+output3

优选地，所述航向偏差保存单元的航向偏差采用以下方式获取：

采用预置目标航向值target_course与实时获取的偏航角arhs.yaw的差值为航向偏差，即error_course＝target_course-arhs.yaw。

所述预置目标航向值设置方法包括如下：

当进入某模式，将第一次从姿态解算单元ARHS获取的偏航角(yaw)设置为预置目标航向值，其是控制无人船直线航行参照的关键指标。后面每个采样周期获取的偏航角为实时获取的偏航角arhs.yaw。

通过无人船控制系统姿态解算单元ARHS可以解算出无人船的船体坐标系与地面坐标系，这两个坐标系之间的关系是三个欧拉(Euler)角，分别是俯仰角(pitch)、偏航角(yaw)、滚转角(roll)，这三个角可以反应船体相对地面的姿态，其中偏航角表示船体坐标系xb轴在水平面上投影与地面坐标系xg轴在水平面之间的夹角，由地面坐标系xg轴逆时针转至船体坐标系xb轴的投影线时，偏航角为正，及船头右偏航为正，反之为负。

特别说明的是，ARHS是一种主要采用惯性传感器以数据融和算法精确测量空间姿态的算法，其为现有技术，本专利并不是研究ARHS本身，只是用到这个算法输出的航姿结果。

ARHS解算的yaw偏航角即用来表明当前航向偏转角度，而pitch、roll常用于空中姿态位置标识，无人船基于水平面，所以pitch、roll对本发明实际参考意义不大。

优选地，所述航向偏差error_course可以进一步优化，对航向偏差进行过滤，即考虑无人船水域作业场景，启动自动纠偏模式后，偏航角数据的过滤策略为基于原始参考目标原点正负偏差45°范围为有效数据，即基于参考原点左偏或右偏45°的航向偏差判定有效航向偏差，大于这个范围时为无效航向偏差，忽略该值。

优选地，所述无人船航向控制自动纠偏系统包括模式控制单元，与所述动力输出控制单元相连接；

作为一种可实现方式，所述模式控制单元用于自动纠偏模式判断，采用以下方式进行判断，即判定一定周期内(优选10ms)无人船控制系统遥控方向信道输入值steering的变化范围，当steering的变化范围介于输入范围中间值正负1％区间，说明处于直线航线操作，判断为自动纠偏模式；如果steering输入值超出输入范围中间值正负1％区间范围浮动，说明处于非直线航线操作，则不是自动纠偏模式。

所述捕获无人船控制系统遥控方向信道输入值steering包括控制系统读取遥控方向控制信道的脉冲宽度调制PWM，PWM表示方波高电平时间跟周期的比例，即占空比，例如1秒高电平1秒低电平的PWM波占空比是50％。

作为另一种可实现方式，所述模式控制单元用于人工遥控控制模式判断，采用以下方式进行判断：捕获和解析无人船控制系统控制模式信道输入值，当输入值control_mode为手动方式manual，即为人工遥控控制模式。

作为另一种可实现方式，所述模式控制单元用于自动纠偏模式判断和人工遥控控制模式判断，具体判断方式如文上述，不再赘述。

以上实施例在满足无人船人工控制需求应用中，将自动航向纠正控制与无人船人工控制模式融合，创造一种基于人工控制的自动纠偏模式。

以上所举实施例，对本发明的目的、技术方案和优点进行了进一步的详细说明，所应理解的是，以上所举实施例仅为本发明的优选实施方式而已，并不用以限制本发明，凡在本发明的精神和原则之内对本发明所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。