CN110308719B - 一种水面无人艇路径跟踪控制方法 - Google Patents

Abstract

本发明公开了一种水面无人艇路径跟踪控制方法，通过当前无人艇运动的状态信息、位置坐标信息，进行路径点的离散并根据点更新机制进行目标点的更新；通过状态信息进行航向规划，计算当前目标点的视线角

视线余角

横侧偏差S_E、视线补偿量并求得当前航向误差

根据航向规划信息，进行航向控制，计算控制器输出力矩N_p，根据规划航速和海况估算推进器推力X_p；对输出推力X_p、输出力矩N_p进行推力分配，求得各推进器的执行信号，并控制推进器执行指令动作。本发明使得无人艇可以跟踪曲线路径，保证了路径跟踪的快速性以及稳定性，在风浪存在的条件下大大避免了侧漂的发生，实现了对航速较为准确的控制，对于无人艇的路径跟踪控制有重要的作用。

Description

一种水面无人艇路径跟踪控制方法

技术领域

本发明属于水面无人艇领域，具体涉及一种水面无人艇路径跟踪控制方法。

背景技术

水面无人艇(USV)又称为水面机器人，是一种需要极少人为干预甚至无需人为干预即可按照指令执行任务的任务平台，可用于执行扫雷、侦查、作战、观测等许多水面任务。路径跟踪能力为水面无人艇实现自主功能应所具备的最基本的能力之一，为使无人艇自主航行的基本保证。

现阶段，无人艇的路径跟踪技术发展仍不够成熟，绝大多数的无人艇路径跟踪算法仍停留在理论研究以及仿真阶段，极少有应用于实际无人艇的路径跟踪方法发布。对于仿真环境中存在着的大量细节简化以及对环境干扰处理的理想化，实际的水面环境往往极为复杂，各种水动力往往也是非线性的，环境干扰力也有较大的随机性，以目前的仿真技术很难对实际环境作用进行准确的计算，因此也导致了大多数的在仿真环境下取的完美结果的路径跟踪方法在实艇应用中出现响应慢、鲁棒性差等一系列的问题。同时为实现无人艇航速的精确感知，所需要的传感器往往较为昂贵，因此，对于大多数无人艇而言，由于感知精确速度的传感器较为昂贵，其航速一般是由通过卫星定位所得到的近似的和速度而并非准确的分速度，因此也导致对航速的精确控制也较为困难，且若使用该速度作为无人艇航速控制的输入，在风浪存在的条件下往往会导致很大的侧漂，增加了路径跟踪误差，当风浪较大时，会使得无人艇无法对路径进行跟踪。

发明内容

本发明的目的在于提供了一种水面无人艇路径跟踪控制方法，使无人艇实现对路径的高精度跟踪以及在考虑到风浪以及自身速度感知存在较大误差的情况下实现对航速的控制，使得USV在对航速感知存在较大误差且有环境干扰的情况下对规划航速进行修正，使路径跟踪误差以及航速跟踪误差保持在可允许的范围内。

本发明的目的是这样实现的：

一种水面无人艇路径跟踪控制方法，具体的实现步骤为：

步骤1.通过仪器接收当前无人艇运动的状态信息以及位置坐标信息，并进行转化处理；

步骤2.进行路径点的离散并根据点更新机制进行目标点的更新；

步骤3.通过状态信息进行航向规划，在航向规划模块中，计算当前目标点的视线角

视线余角

横侧偏差S_E、视线补偿量并求得当前航向误差

步骤4.根据航向规划信息，进行航向控制，计算控制器输出力矩N_p，根据规划航速和海况估算推进器推力X_p；

步骤5.对输出推力X_p、输出力矩N_p进行推力分配，求得各推进器的执行信号，并控制推进器执行指令动作。

所述步骤1的具体内容为：下位机通过串口接收相关传感器传递的感知信息，对接收到的信息通过相应的通讯协议进行解码，得到无人艇当前的经纬度坐标、航速、艏向角、风速、流速，将无人艇的经纬度坐标(S_lat,S_lon)转化为NED坐标系下的坐标点(N_s,E_s)。

所述步骤2的具体步骤为：

步骤2.1.将当前路径离散为按顺序散布在路径上的点阵P＝{P₁,P₂,……,P_n}，其中P_i为点阵中第i个点的大地坐标，P_i＝[N_i,E_i]，1≤i≤n，点阵之间的距离D_i,i+1，满足D_i,i+1＞R_C，R_C为根据位置感知误差而设置的目标点范围半径；

步骤2.2.按照目标点更新规则，向航向规划发送当前目标点，航向规划中的路径点存储空间记为{P_i-1,P_i}，其中P_i-1表示P₁或上一个已到达的目标点，P_i表示当前准备到达的目标点，在每一个控制节拍中计算无人艇当前位置坐标[N_s,E_s]与当前目标点P_i:[N_i,E_i]的距离Distance，

当Distance＜R_C时，对点进行更新，R_C为所设定的目标点范围半径，更新机制为

所述步骤3的具体步骤为：

步骤3.1.结合无人艇当前的位置坐标S:(N_s，E_s)和首向角

求取当前的视线夹角

横侧偏差S_E；

步骤3.2.依据横侧偏差值S_E，将横侧偏差无因次化后，利用参数可调的Sigmoid函数，计算目标视线角的补偿值，其中Sigmoid补偿函数为

式中rec_S_E为上一控制循环时所计算所得S_E值，

为视线余角，D_SE为横侧偏差的变化率，Δt表示控制节拍，S_M为Sigmoid函数输出，k_SE、k_DSE为可调参数，将影响量均除以船宽B进行无因次化；

步骤3.3.将视线补偿值

与目标视线角

进行叠加，得到航向误差角

所述步骤4的具体步骤为：

步骤4.1.通过Sigmoid艏向控制器计算控制力矩N_p，计算公式为

其中，a_E为无人艇的艏摇角速度，I_E为角速度误差积分，其中可调整参数为比例参数k₁、速度参数k₂、积分参数k₃、幅值参数

步骤4.2.根据规划航速和海况估算推进器推力X_p；

步骤4.3.对于欠驱动无人艇，控制其沿船首方向的前进速度，根据流速u_w对规划航速u_p进行补偿修正得到补偿后的规划航速u_I，

式中

为流向，

为无人艇当前艏向；

步骤4.4.根据补偿后的规划航速u_I，通过查表运算，得到控制器的输出推力X_p，所查表格为通过仿真或者实验测得的推力-航速对应表格。

所述步骤5的具体步骤为：

步骤5.1.求取左推进器的推力X_PL和右推进器的执行推力X_PR，计算公式为

步骤5.2.根据推力和推进器控制信号对应关系，求取推进器额定控制信号大小，并通过串口发送到推进器驱动器，控制推进器的运行，即完成了本循环的控制，之后经过一个控制节拍后，进入下一控制循环。

本发明的有益效果在于：本发明考虑到水面无人艇的实际航行过程中的路径跟踪问题以及速度感知不精确的问题，通过对路径进行点迹理解以及非线性视线补偿使得无人艇可以跟踪曲线路径。同时非线性视线补偿控制器中的可调参数保证了路径跟踪的快速性以及稳定性，在风浪存在的条件下大大避免了侧漂的发生。也考虑了实际情况下无人艇普遍存在的航速感知误差问题，提出了推力查表估算方法，实现了对航速较为准确的控制，对于无人艇的路径跟踪控制有重要的作用。

附图说明

图1为本发明的总执行框图。

图2为本发明的的规划路径离散图。

图3为本发明的视线角以及非线性视线补偿角所涉及的相关计算量的示意图。

图4为本发明的的角度取值定义图。

图5为本发明的的推力查表流程图。

具体实施方式

下面结合附图对本发明做进一步的描述：

实施例1

一种水面无人艇路径跟踪控制方法，具体的实现步骤为：

(1)下位机通过串口接收相关传感器传递的感知信息，具体包括GPS、电子罗盘等。对接收到的信息通过相应的通讯协议进行解码，得到无人艇当前的经纬度坐标、航速、艏向角、风速、流速等。而后，将无人艇的经纬度坐标(S_lat,S_lon)转化为NED坐标系下的坐标点(N_s,E_s)。

(2)从规划处接收规划的路径，进行路径的点迹理解，根据路径的复杂程度以及跟踪精度要求，根据一定的距离将路径离散为按顺序散布在路径上的点阵P＝{P₁,P₂,……,P_n}，记航向规划模块中，存储的当前目标点为P_i,定义该模块中的点存储空间为{P_i-1,P_i},其中2≤i≤n，并在每一个控制循环中根据无人艇与目标点距离判断是否应对点进行更新。

(3)根据当前无人艇的位置以及存储空间中的坐标点，进行航向规划，计算当前目标点的视线角

视线余角

横侧偏差S_E、使用S面函数计算视线补偿量并叠加视线角

最终求得当前航向误差

输入航向控制器中。

(4)根据当前的流速信息对规划航速进行修正得到修正后的规划航速u_I，后根据所控制无人艇的航速—推力性能对应表格，进行查表运算，求得控制器的输出推力X_p，在航向控制器中，对

进行数值积分得到误差角积分I_E，结合角速度a_E计算控制器输出力矩N_p。

(6)进行航行指令的理解，对X_p、N_p进行推力分配，得到各桨的推力值以及推力信号，并通过串口将信号传递至推进器驱动器，控制推进器，完成当前控制循环，经过一个控制节拍后，进入下一相同控制循环。

本发明提出的一种基于非线性补偿视线法的水面无人艇路径跟踪控制方法，如图1所示，具体有以下几个步骤：

步骤1：通过串口数据接收当前无人艇运动的状态信息以及位置坐标信息，并进行转化处理。

(1.1)下位机通过串口接收相关传感器传递的感知信息，具体包括GPS、电子罗盘等。

(1.2)对接收到的信息通过相应的通讯协议进行解码，得到无人艇当前的经纬度坐标、航速、艏向角、风速、流速等。

(1.3)在当前无人艇运行的区域内，任选一点作为NED坐标系的原点，设该点经纬度为(O_lat,O_lon)，式中，下标lat表示纬度，lon表示经度。并将无人艇的经纬度坐标(S_lat,S_lon)转化为NED坐标系下的坐标点(N_s,E_s)，转化公式如下：

式中，经纬度坐标代入单位为度，输出坐标单位为米。

步骤2：进行路径点的离散并根据点更新机制进行目标点的更新

(2.1)从规划处接收规划的路径，根据路径的复杂程度以及跟踪精度要求，根据一定的距离将路径离散为按顺序散布在路径上的点阵P＝{P₁,P₂,……,P_n}，如图2所示。

(2.2)记航向规划模块中，存储的当前目标点为P_i,定义该模块中的点存储空间为{P_i-1,P_i},其中2≤i≤n，在第一个控制循环开始时，将P2作为当前目标点传递至存储空间。

(2.3)在每一个控制循环中根据当前无人艇与当前目标点距离判断是否需要对目标点进行更新。当Distance＜R_c时，对当前目标点按如下法则进行更新，无人艇当前位置与目标点距离及更新机制按如下公式计算：

步骤3：在航向规划模块中，计算当前目标点的视线角

视线余角

横侧偏差S_E、视线补偿量并最终求得当前航向误差θ_E，该步骤中所涉及的各角度示意如图3所示。

(3.1)根据当前的首向角

无人艇位置的NED坐标，将当前目标点坐标转化为无人艇随艇坐标系xoy下的坐标P_i:(x_i,y_i)，该坐标系与水平面平行，其中o点为坐标系原点，位于无人艇质心，x轴垂直指向船首，y轴垂直指向右舷，转化公式如下：

(3.2)根据当前目标点在xoy坐标系下的位置，计算该点连线与x轴正方向的夹角即

为使后续定义及计算方便统一，对其取值范围及符号进行规定，连接点o与P_i，若y_i＞0,则将ox轴顺时针旋转，其与oP_i重合的角度为

取值从0至π递加；反之若y_i＜0,则将ox轴逆时针旋转，其与oP_i重合的角度为

取值从0至-π递减，如图4所示：

可采用以下公式进行计算：

(3.3)根据存储空间中的目标点无人艇当前的位置和首向角进行处理，计算视线余角

该角的取值范围为

当无人艇位置位于目标点连线右侧时，定义

为负值，反之为正值，因此为判断无人艇位于的方位，可使用如下方法：

首先，构件向量V_p:(N_p,E_p)，为由上一个目标点P_i-1指向当前目标点P_i的向量，计算方法如下:

构件向量

为由上一个目标点P_i-1指向当前船舶位置S:(N_s，E_s)的向量，计算方法如下：

以正北轴为基准，顺时针旋转为正向，分别求取向量V_p、V_s在NED坐标系中的方位角

取方位角的取值范围为[0,2π]，对任意向量V:(N,E)，其方位角

可按如下公式计算：

求得向量V_p、V_s得而方位角后，对方位角求差值：

根据该差值，通过如下判据判定无人艇所在的规划线方位：

方位	取值范围
		左侧	(0,π)∪[-2π,-π)
右侧	(-π，0)∪(π,2π]
		线上	其他

(3.4)在求得无人艇相对于规划线方位后，求取无人艇首向与V_p的夹角

为后续计算方便，定义夹角范围为[-π,π],其中首向角位于V_p顺时针方向时为正，逆时针方向时为负。最后，通过以下公式便可求得

(3.5)求取横侧偏差S_E

首先，当规划线不与E轴平行时，计算规划线P_i-1P_i的斜率k_p，而后根据点到直线的距离公式即可求得横侧偏差S_E：

当P_i-1P_i//E时：

(3.6)使用Sigmoid函数对目标视线角进行补偿修正，最后得到航向误差角

S面函数的形式为：

式中，F表示决定控制输出相关的影响量，j表示影响量的数目，k_i为可调参数。对于本发明而言，控制输出量为视线补偿角

相关的影响量为横侧偏差S_E和横侧偏差的变化率D_SE，因此，视线补偿角按下式计算:

式中rec_S_E为上一控制循环时所计算得而S_E值，Δt表示控制节拍，为算法移植考虑，将影响量均除以船宽B进行无因次化。

(3.7)最后，将视线角与视线补偿量进行叠加即可得到航向误差角：

步骤4：计算控制器输出力矩N_p,

对于输出力矩而言，Sigmoid函数的影响量为误差角

以及角速度a_E，同时考虑到修正稳态误差，加入误差角积分I_E，故计算力矩的S面函数如下：

式中，

为比例系数，表示不考虑积分作用时给推进器分配的最大执行力矩。

步骤5：根据规划航速和海况估算推进器推力

(5.1)对于多数无人艇而言，其航速通常是通过卫星定位进行感知的，如GPS、北斗导航等，但通过该方法感知得到的航速为运动的合速度，且在航速较低时，其航向角误差极大，无法通过航向角而求得无人艇各个自由度运动的分速度。因此，需要先建立所控制的无人艇直航状态下的推力-航速的对应表格：

前进时：

倒车时：

该表格可以通过流体力学软件进行计算，也可以在无人艇第一次试验时通过试验进行测试。

(5.2)根据当前的流速信息对规划航速进行修正，因无人艇受风面积较小，风的作用对其航速影响较小，故航速修正考虑流的作用即可，记规划航速为u_p，流速u_w，流向

考虑到无人艇为欠驱动，故只对其纵向速度进行补偿：

式中，u_I为进行补偿后的规划航速。

(5.3)最后，根据补偿后的规划航速，通过查表运算，得到控制器的输出推力X_p，当u_I≥0时，查表流程如图5所示：

图5中插值运算的插值公式为：

步骤6：对X_p、N_p进行推力分配，求得各推进器的执行信号。

(6.1)首先，应求取左推进器的推力X_pL和右推进器的执行推力X_pR，通过如下公式计算：

(6.2)根据推力和推进器控制信号对应关系，求取推进器额定控制信号大小，并通过串口发送到推进器驱动器，控制推进器的运行，即完成了本循环的控制，之后经过一个控制节拍后，进入下一控制循环。

实施例2

本发明公开了一种基于非线性补偿视线法的水面无人艇路径跟踪控制方法。针对路径上的路径点视线和水面无人艇与期望路径的偏差，结合Sigmoid函数以及推力估计提出了以非线性视线法为基础的无人艇路径跟踪控制方法。本方法分为点迹理解、航向规划、航向控制和指令理解四大部分。其中点迹理解为将拟跟踪路径进行离散化处理，得到系列路径点作为航向规划时的输入。航向规划为根据从路径跟踪模块所接收的路径点，并结合无人艇当前的位置、航向、系统状态等信息规划出期望航向作为航向控制器输入，同时向路径点返回到点信息，航向控制为根据航向规划输出的航向误差，计算应控制的转矩大小，并在无人艇自身速度感知不准确的情况下，在考虑环境干扰作用下根据期望航速对实际推力进行预估，控制航速。最后指令理解器通过对航向控制指令进行理解并计算左右推进器驱动信号，将信号传递至推进器驱动器控制推进器的功率实现水面无人艇的路径跟踪。

一种基于非线性补偿视线法的水面无人艇路径跟踪控制方法，包括以下步骤：

(1)根据规划路径信息，进行点迹理解：

(1.1)将当前路径离散为按顺序散布在路径上的点阵P＝{P₁,P₂,……,P_n}，其中P_i＝[N_i,E_i]，1≤i≤n，为点阵中第i个点的大地坐标，并且点阵之间的距离

满足D_i,i+1＞R_C，R_C为考虑到位置感知误差而设置的到点判断距离。式中，[N_i,E_i]表示大地坐标系NOE中的坐标，其中O为大地坐标系原点，N指向正北，E指向正东。

(1.2)按照目标点更新规则，向航向规划规划中发送当前目标点，航向规划中的路径点存储空间记为{P_i-1,P_i}，其中P_i-1表示P₁或上一个已到达的目标点，P_i表示当前准备到达的目标点。在每一个控制节拍中计算无人艇当前位置坐标[N_s,E_s]与当前目标点P_i:[N_i,E_i]的距离Distance，计算公式为：

当Distance＜R_C时，对点进行更新，R_C为所设定的目标点范围半径，更新机制为：

(2)通过状态信息进行航向规划：

(2.1)利用当前存储空间中的两点，结合无人艇当前的位置坐标S:(N_s，E_s)和首向角

求取当前的视线夹角

横侧偏差S_E。

(2.2)依据横侧偏差值，将横侧偏差无因次化后，利用参数可调的Sigmoid函数，计算目标视线角的补偿值，其中Sigmoid补偿函数有如下形式：

式中rec_S_E为上一控制循环时所计算所得S_E值，Δt表示控制节拍，S_M为Sigmoid函数输出，为算法移植考虑，将影响量均除以船宽B进行无因次化。

(2.3)将视线补偿值

与目标视线角

进行叠加，得到航向误差角

(3)根据航向规划信息，进行航向控制

(3.1)通过Sigmoid艏向控制器计算控制力矩N_p，控制器有如下形式：

其中，a_E为无人艇的艏摇角速度，I_E为角速度误差积分，可调整参数如下：

(3.2)根据规划航速和海况估算推进器推力

(3.3)对于欠驱动无人艇，控制其沿船首方向的前进速度，首先，根据流速u_w对规划航速u_p进行补偿修正得到补偿后的规划航速u_I，方法如下：

式中

为流向，

为无人艇当前艏向

(3.4)根据补偿后的规划航速u_I，通过查表运算，得到控制器的输出推力X_p，所查表格为通过仿真或者实验测得的推力-航速对应表格。

(4)对控制指令进行理解，并控制推进器执行指令动作

(4.1)首先，应求取左推进器的推力

和右推进器的执行推力X_pR，通过如下公式计算：

(4.2)根据推力和推进器控制信号对应关系，求取推进器额定控制信号大小，并通过串口发送到推进器驱动器，控制推进器的运行，即完成了本循环的控制，之后经过一个控制节拍后，进入下一控制循环。

Claims (4)

1.一种水面无人艇路径跟踪控制方法，其特征在于，具体的实现步骤为：

步骤1：通过仪器接收当前无人艇运动的状态信息以及位置坐标信息，并进行转化处理；

步骤2：进行路径点的离散并根据点更新机制进行目标点的更新；

步骤2.1：将当前路径离散为按顺序散布在路径上的点阵P＝{P₁，P₂，……，P_n}，其中P_i为点阵中第i个点的大地坐标，P_i＝[N_i，E_i]，1≤i≤n，点阵之间的距离D_i，i+1，满足D_i，i+1＞R_C，R_C为根据位置感知误差而设置的目标点范围半径；

步骤2.2：按照目标点更新规则，向航向规划发送当前目标点，航向规划中的路径点存储空间记为{P_i-1，P_i}，其中P_i-1表示P₁或上一个已到达的目标点，P_i表示当前准备到达的目标点，在每一个控制节拍中计算无人艇当前位置坐标[N_s，E_s]与当前目标点P_i：[N_i，E_i]的距离Distance，

当Dis tan ce＜R_C时，对点进行更新，R_C为所设定的目标点范围半径，更新机制为

步骤3：通过状态信息进行航向规划，在航向规划模块中，计算当前目标点的视线角

视线余角

横侧偏差S_E、视线补偿量并求得当前航向误差

步骤3.1：根据当前的首向角

无人艇位置的NED坐标S：(N_s，E_s)，将当前目标点坐标转化为无人艇随艇坐标系xoy下的坐标P_i：(x_i，y_i)，该坐标系与水平面平行，其中o点为坐标系原点，位于无人艇质心，x轴垂直指向船首，y轴垂直指向右舷，转化公式如下：

步骤3.2：根据当前目标点在xoy坐标系下的位置，计算该点连线与x轴正方向的夹角即

对

取值范围及符号进行规定，连接点o与P_i，若y_i＞0，则将ox轴顺时针旋转，其与oP_i重合的角度为

取值从0至π递加；反之若y_i＜0，则将ox轴逆时针旋转，其与oP_i重合的角度为

取值从0至-π递减；

采用以下公式进行计算：

步骤3.3：根据存储空间中的目标点无人艇当前的位置和首向角进行处理，计算视线余角

该角的取值范围为

当无人艇位置位于目标点连线右侧时，定义

为负值，反之为正值；

为了判断无人艇位于的方位，使用如下方法：

构建向量V_p：(N_p，E_p)，为由上一个目标点P_i-1指向当前目标点P_i的向量；

构建向量

为由上一个目标点P_i-1指向当前船舶位置S：(N_s，E_s)的向量；

以正北轴为基准，顺时针旋转为正向，分别求取向量V_p、V_s在NED坐标系中的方位角

取方位角的取值范围为[0，2π]，对任意向量V：(N，E)，其方位角

可按如下公式计算：

求得向量V_p、V_s得而方位角后，对方位角求差值：

根据

的取值范围，通过如下判据判定无人艇所在的规划线方位：

方位 取值范围 左侧 (0，π)∪[-2π，-π) 右侧 (-π，0)∪(π，2π] 线上 其他

步骤3.4：在求得无人艇相对于规划线方位后，求取无人艇首向与V_p的夹角

定义夹角范围为[-π，π]，其中首向角位于V_p顺时针方向时为正，逆时针方向时为负；最后，通过以下公式便可求得

步骤3.5：求取横侧偏差S_E；

首先，当规划线不与E轴平行时，计算规划线P_i-1P_i的斜率k_p，而后根据点到直线的距离公式即可求得横侧偏差S_E：

当P_i-1P_i//E时：

步骤3.6：使用Sigmoid函数对目标视线角进行补偿修正，最后得到航向误差角

式中rec_S_E为上一控制循环时所计算得而S_E值；Δt表示控制节拍；为算法移植考虑，将影响量均除以船宽B进行无因次化；将视线角与视线补偿量进行叠加即可得到航向误差角：

步骤4：根据航向规划信息，进行航向控制，计算控制器输出力矩N_p，根据规划航速和海况估算推进器推力X_p；

步骤5：对输出推力X_p、输出力矩N_p进行推力分配，求得各推进器的执行信号，并控制推进器执行指令动作。

2.根据权利要求1所述的一种水面无人艇路径跟踪控制方法，其特征在于，所述步骤1的具体内容为：下位机通过串口接收相关传感器传递的感知信息，对接收到的信息通过相应的通讯协议进行解码，得到无人艇当前的经纬度坐标、航速、艏向角、风速、流速，将无人艇的经纬度坐标(S_lat，S_lon)转化为NED坐标系下的坐标点(N_s，E_s)。

3.根据权利要求1所述的一种水面无人艇路径跟踪控制方法，其特征在于，所述步骤4的具体步骤为：

步骤4.1：通过Sigmoid艏向控制器计算控制力矩N_p，计算公式为

其中，a_E为无人艇的艏摇角速度，I_E为角速度误差积分，其中可调整参数为比例参数k₁、速度参数k₂、积分参数k₃、幅值参数

步骤4.2：根据规划航速和海况估算推进器推力X_p；

步骤4.3：对于欠驱动无人艇，控制其沿船首方向的前进速度，根据流速u_w对规划航速u_p进行补偿修正得到补偿后的规划航速u_I，

式中

为流向，

为无人艇当前艏向；

步骤4.4：根据补偿后的规划航速u_I，通过查表运算，得到控制器的输出推力X_p，所查表格为通过仿真或者实验测得的推力-航速对应表格。

4.根据权利要求1所述的一种水面无人艇路径跟踪控制方法，其特征在于，所述步骤5的具体步骤为：

步骤5.1：求取左推进器的推力X_PL和右推进器的执行推力X_PR，计算公式为

其中，B为船宽；

步骤5.2：根据推力和推进器控制信号对应关系，求取推进器额定控制信号大小，并通过串口发送到推进器驱动器，控制推进器的运行，即完成了本循环的控制，之后经过一个控制节拍后，进入下一控制循环。

Images