[发明专利]一种水面无人艇路径跟踪控制方法

权利要求书

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进行推力分配，求得各推进器的执行信号，并控制推进器执行指令动作。