CN111487966A - 一种基于航路点的水面无人艇自适应路径跟踪控制方法 - Google Patents

Abstract

一种基于航路点的水面无人艇自适应路径跟踪控制方法，属于控制技术领域。主要是为了解决无人艇转弯角度大于90°时经典的LOS制导会产生较大的超调，导致的转弯跟踪精度低的问题。本发明基于提出的自适应LOS圆半径解算出基本视线角，根据路径偏差和航向偏差对基本视线角进行补偿得到最终期望视线角；接着设计了基于虚拟点的转向策略，采用三个小角度转向过渡大角度转向克服转弯角度较大时产生的严重超调问题。同时本发明还设计了航速解算器和智能自适应S面航向控制器及智能自适应积分S面航速控制器，可提高无人艇的跟踪效率和抗干扰能力，也能够很好的应对无人艇模型的复杂性性和不确定性。主要用于水面无人艇自适应路径跟踪控制。

Description

一种基于航路点的水面无人艇自适应路径跟踪控制方法

技术领域

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

背景技术

水面无人艇作为一种重要的海洋智能装备，在海底测绘、海洋平台和海上风力发电厂巡检、海洋环境监测和近海侦查防卫等方面发挥了重要作用，为了精确高效地完成任务，同时考虑到USV的欠驱动性和复杂多变的海洋环境对其操纵性的影响，在实际应用中，对无人艇沿预设期望路径跟踪的能力要求越来越高。

在无人艇的路径跟踪控制策略中，基于预设路径点的复合直线路径跟踪由于路径点可以提前设定，更符合无人艇的路径跟踪工程应用需求，现有的路径跟踪控制方法大多基于Line-of-Sight(LOS)进行制导律设计，但由于LOS圆半径为定值，当路径偏差较大时容易导致收敛时间过长，而且传统的LOS制导角不能根据路径偏差和艏向偏差进行在线调整补偿，导致无人艇不能快速地收敛到期望路径上；一般当无人艇转弯角度小于90°时，采用经典LOS制导基本上能满足跟踪要求，但当无人艇转弯角度大于90°时，采用经典LOS制导会产生较大的超调；同时，一般将无人艇的跟踪的航速设为定值，在转向时，速度相对过快，直线航行时，速度相对较低，降低了转弯的跟踪精度，同时也增大了完成跟踪所需的时间。

发明内容

本发明是主要是为了解决无人艇转弯角度大于90°时经典的LOS制导会产生较大的超调，导致的转弯跟踪精度低的问题。

一种基于航路点的水面无人艇自适应路径跟踪控制方法，其特征在于，所述方法包括以下步骤：

基于自适应LOS圆半径对应的LOS制导律进行视线角解算，然后根据路径偏差和航向偏差对视线角进行补偿得到最终期望航向角，使无人艇在最终期望航向角的引导下不断向期望路径运动；

基于虚拟点的转向策略，采用三个小角度转向过渡大角度转向的方式进行转向：

将行使路径上的相邻的三个航路点记为P_k-1、P_k和P_k+1，坐标分别为(x_k-1,y_k-1)、(x_k,y_k)和(x_k+1,y_k+1)，在P_k-1 P_k上设置虚拟转向点A，在P_k P_k+1上设置虚拟转向点C；A、B、C坐标分别为(x_A,y_A)、(x_B,y_B)和(x_C,y_C)，且线段AP_k的距离等于线段P_kC的距离；B点坐标采用△AP_kC的几何中心；

根据转向角度计算出A和C点坐标：

当x_k-x_k-1＝0时，x_A＝x_k-1＝x_k，y_A＝y_k-μk₈θ_tR_tmin；

当y_k-y_k-1＝0时，y_A＝y_k-1＝y_k，x_A＝x_k-Θk₈θ_tR_tmin；

当x_k-x_k-1≠0，y_k-y_k-1≠0时，

当x_k+1-x_k＝0时，x_C＝x_k＝x_k+1，y_C＝y_k+μk₈θ_tR_tmin；

当y_k+1-y_k＝0时，y_C＝y_k＝y_k+1，x_C＝x_k+Θk₈θ_tR_tmin；

当x_k+1-x_k≠0，y_k+1-y_k≠0时，

其中，k₈是回转系数，θ_t是转向角度，R_tmin是最小回转半径,

且当x_k+1＞x_k或x_k＞x_k-1时，Θ＝1，否则Θ＝-1；当y_k+1＞y_k或y_k＞y_k-1时，μ＝1，否则μ＝-1。

当无人艇沿∠P_k-1P_kP_k+1航行时，如果转弯角度大于90°，转换成沿∠P_k-1ABCP_k+1航行。

进一步地，基于自适应LOS圆半径对应的LOS制导律进行视线角解算的具体过程包括以下步骤：

自适应LOS圆半径如下：

式中，R_min是LOS的最小圆半径，δ是过渡层的厚度，k₁是LOS圆半径可调系数，y_e是路径偏差；当|y_e|＞R_min+δ时，无人艇以当前最小前视距离收敛至期望路径，当路径偏差|y_e|≤R_min-δ时，无人艇以最小内切圆半径R_min趋向于期望路径，而当R_min-δ＜|y_e|≤R_min+δ时，LOS圆半径能够在[R_min,R_min+δ]之间光滑过渡。

一系列的航路点坐标记为P₁,P₂,…,P_k-1,P_k,P_k+1,…,P_n，P_k(x_k,y_k)为第k个坐标，P_LOS(x_LOS,y_LOS)为LOS的位置坐标，α_k-1是期望路径方位角，R_min是最小视线圆半径，R_δ是过渡层的视线圆半径，R_k是第k个切换下一个期望路径点的圆半径，Δ＞0是前视距离；

在连接相邻的两个路径点的直线单元上，通过下式解得视线法参考位置：

(x_LOS-x)²+(y_LOS-y)²＝R²

式中，R是以无人艇为中心所做的LOS圆半径，即自适应LOS圆半径；(x,y)为无人艇实时位置，(x_k-1,y_k-1)为预定的第k-1个航迹点坐标；

然后由投影算法计算出无人艇趋于期望路径的视线角ψ_LOS为：

无人艇在视线角的引导下不断向参考点运动，从而逐渐收敛到期望路径上。

进一步地，所述根据路径偏差和航向偏差对视线角进行补偿得到最终期望航向角的过程是基于航向偏差的LOS视线角补偿器和基于路径偏差的LOS视线角补偿器实现的；

基于航向偏差的LOS视线角补偿器如下：

式中，Δψ是航向角偏差，ψ_r是无人艇的实际航向，ψ_H是航向补偿角，δ_max是无人艇实际航行中的最大转向舵角，k₂是航向偏差补偿器的可调系数；

基于路径偏差的LOS视线角补偿器如下：

式中，ψ_P是路径偏差补偿角，k₃是路径偏差补偿器的可调系数；

无人艇的最终期望航向角为：

ψ_d＝ψ_LOS+ψ_H+ψ_P。

进一步地，所述的一种基于航路点的水面无人艇自适应路径跟踪控制方法，还包括利用航速解算器实时解算出无人艇的最佳期望速度的步骤，然后解算出最佳期望航速传给速度控制环节；

所述的航速解算器为航向偏差和路径点距离联合航速解算器，具体如下：

式中，Δψ_d是实际航向与实际期望航向的偏差，v_d是无人艇的期望航速，v_max是无人艇最大航速，Δl是距下一个路径点的距离，k₄、k₅是航速解算器可调参数。

进一步地，所述的一种基于航路点的水面无人艇自适应路径跟踪控制方法，还包括利用智能自适应S面航向控制器对航向进行控制的步骤；

所述智能自适应S面航向控制器如下：

其中，f^h表示航向控制输出，e^h和ec^h分别为航向偏差和航向偏差变化率，需要归一化处理，

和

表示航向控制器参数基准值，

和

表示航向控制器可调参数。

进一步地，所述的一种基于航路点的水面无人艇自适应路径跟踪控制方法，还包括利用智能自适应积分S面航速控制器对航速进行控制的步骤；

所述智能自适应积分S面航速控制器如下：

式中：f^v表示航速控制输出，k是积分步长，e^v和ec^v分别为航速偏差和航速偏差变化率，也需要归一化处理，

和

表示航速控制器参数基准值，

和

表示航速控制器可调参数，t表示时间。

有益效果：

本发明首先充分考虑路径跟踪过程中路径偏差与制导律性能的关系，提出了自适应LOS圆半径，再根据路径偏差和航向偏差对视线角进行补偿，使无人艇能够更快地收敛到期望路径；同时本发明还提出了基于虚拟点的转向策略，采用三个小角度转向过渡大角度转向克服传统的LOS在转弯角度较大时产生严重的超调的问题，大幅度提高了无人艇的路径跟踪精度。

同时本发明还设计了航速解算器，可实时解算出无人艇的最佳期望速度，能够极大地提高跟踪精度和效率。本发明还设计了智能自适应S面航向控制器和智能自适应积分S面航速控制器，能够很好的应对无人艇模型的复杂性性和不确定性，提高了控制器的自适应能力，进一步提高了无人艇的路径跟踪精度和效率。

所以发明的整体技术方案不仅能够解决现有技术存在的转弯角度大于90°时经典的LOS制导会产生较大的超调导致的转弯跟踪精度低的问题。还能够解决无人艇在路径跟踪时对期望路径的收敛速度慢、跟踪精度差和跟踪效率低的问题。

由以上仿真可知，本发明设计的路径跟踪方法可有效的引导无人艇按照预定路径航行，同时具有较好的拐点性能，在切换路径后，能以更快的速度收敛到期望航线，路径偏差较小，跟踪精度较高。

附图说明

图1为参考坐标系示意图；

图2为大角度转向示意图；

图3为自适应LOS圆半径的制导策略示意图；

图4为路径跟踪仿真图；

图5为路径偏差仿真图；

图6为速度响应仿真图；

图7为艏向角响应仿真图。

具体实施方式

具体实施方式一：

本实施方式为一种基于航路点的水面无人艇自适应路径跟踪控制方法，在说明具体的控制方案之前，首先对参数和相关关键技术进行一下说明。

本发明涉及的参数定义如下：

η＝[x,y,ψ]^T为USV在大地坐标系{E}下的位置和艏向角；v＝[u,v,r]^T为USV在艇体坐标系{B}下的纵向速度、横向速度和艏向的角速度；J(ψ)∈R^3×3为无人艇从艇体坐标系{B}到大地坐标系{E}的旋转矩阵；M∈R^3×3为惯性矩阵；C(v)∈R^3×3为科氏力和向心力矩阵；D∈R^3×3为阻尼力矩阵；B∈R^3×2为执行器配置矩阵；f＝[f_u,f_r]^T为控制的输入量，其中f_u是螺旋桨推力，f_r是舵角产生的力矩；d＝[d_u,d_v,d_r]^T为由外界的海风、海浪和海流引起的干扰力/力矩；R_min为LOS的最小圆半径；δ为过渡层的厚度；Δψ为航向角偏差；ψ_r为无人艇的实际航向；ψ_LOS为视线角；ψ_H为航向补偿角；δ_max为无人艇实际航行中的最大转向舵角；θ_t为转向角度；R_tmin为最小回转半径。

相关关键技术：

本发明首先充分考虑路径跟踪过程中路径偏差与制导律性能的关系，通过定义一个与路径偏差相关的LOS圆半径，即自适应LOS圆半径；然后再根据路径偏差和航向偏差对视线角进行补偿，使无人艇能够更快地收敛到期望路径；最后，为了克服传统的LOS在转弯角度较大时产生严重的超调，提出了基于虚拟点的转向策略，采用三个小角度转向过渡大角度转向；为了提高跟踪效率，设计了航速解算器，可实时解算出无人艇的最佳期望速度；针对无人艇模型的复杂性性和不确定性，设计了智能自适应S面航向控制器和智能自适应积分S面航速控制器。

自适应LOS圆半径：考虑到路径跟踪过程中路径偏差与制导律性能的关系，定义一个与路径偏差相关的自适应LOS半径，可实时以最佳距离收敛到期望路径。

LOS视线角补偿器：在跟踪过程中，根据无人艇的路径偏差和航向偏差，采用sigmod函数对LOS视线角进行实时补偿调整，使无人艇能够更快地收敛到期望航向角。

基于虚拟点的转向策略：为了克服传统的LOS在转弯角度较大时产生严重的超调，提出了基于虚拟点的转向策略，采用三个小角度转向过渡大角度转向，减小了无人艇大角度转向时的超调问题。

联合航速解算器：根据无人艇实际航向角与所跟踪的路径方位角的偏差，同时考虑距前后两路径点的距离，采用高斯函数解算出最佳期望航速传给速度控制环节，大幅度提高了无人艇的跟踪效率。

智能自适应S面航向控制方法：仅利用误差和误差变化率实时调整S面控制器的参数，不过度依赖操作人员的经验，结构简单，在实际工程应用中具有较好的控制效果。

本发明可以针对三自由度运动学方程和动力学非完全对称模型对应的无人艇进行控制，也可以针对其他形式的运动学方程和动力学非完全对称模型对应的无人艇进行控制。本实施方式以针对三自由度运动学方程和动力学非完全对称模型对应的无人艇为例进行说明，水面无人艇运动数学模型如下：

参考坐标系示意图如图1所示；对无人艇的水平面运动进行分析，在大地坐标系{E}下的位置和艏向角可表示为η＝[x,y,ψ]^T，在艇体坐标系{B}下的纵向速度、横向速度和艏向的角速度可表示为v＝[u,v,r]^T，则无人艇水平面的三自由度运动学方程和动力学非完全对称模型可表示如下：

式中：J(ψ)∈R^3×3代表无人艇从艇体坐标系{B}到大地坐标系{E}的旋转矩阵、M∈R^3×3代表惯性矩阵、C(v)∈R^3×3代表科氏力和向心力矩阵、D∈R^3×3代表阻尼力矩阵、B∈R^3×2代表执行器配置矩阵，它们分别定义为：

正常量m₁₁、m₂₂、m₂₃、m₃₂、m₃₃、d₁₁、d₂₂、d₂₃、d₃₂是无人艇的水动力系数，f＝[f_u,f_r]^T是控制的输入量，其中f_u是螺旋桨推力，f_r是舵角产生的力矩；d＝[d_u,d_v,d_r]^T是由外界的海风、海浪和海流引起的干扰力/力矩。

本实施方式中的无人艇只配备了一个方向舵和一个螺旋桨，没有侧向的直接控制输入，故为欠驱动***，对于左右舷相互对称的无人艇，其艇体的坐标系中心可以转换，以使M^-1Bf＝[τ_u,0,τ_r]^T，其中τ_u、τ_r分别为控制力和控制力矩，同理也可定义转换后的外界环境干扰项为M^-1d＝[d_u,d_v,d_r]^T，则该欠驱动无人艇模型的分量形式可表示为：

式中：

N_u(v,r)、X(u)、Y(u)、N_r(u,v,r)是连续的光滑函数。

本发明的具体控制过程包括：

S1、基于自适应LOS圆半径，设计自适应半径的LOS制导律：

无人艇的制导***可以实时地为航向控制***提供一个参考艏向角，用于引导USV趋向于期望路径。本发明从实用性出发，针对航路点跟踪提出一种基于过渡层的自适应LOS圆半径的制导策略，自适应LOS圆半径如下：

式中，R_min是LOS的最小圆半径，δ是过渡层的厚度，k₁是LOS圆半径可调系数，y_e是路径偏差；当|y_e|＞R_min+δ时，无人艇以当前最小前视距离收敛至期望路径，当路径偏差|y_e|≤R_min-δ时，无人艇以最小内切圆半径R_min趋向于期望路径，而当R_min-δ＜|y_e|≤R_min+δ时，LOS圆半径能够在[R_min,R_min+δ]之间光滑过渡。

自适应LOS圆半径的制导策略如图3所示，定义一系列的航路点坐标：P₁,P₂,…,P_k-1,P_k,P_k+1,…,P_n，P_k(x_k,y_k)为第k个坐标，P_LOS(x_LOS,y_LOS)为LOS的位置坐标，α_k-1是期望路径方位角，R_min是最小视线圆半径，R_δ是过渡层的视线圆半径，R_k是第k个切换下一个期望路径点的圆半径，Δ＞0是前视距离。

在连接相邻的两个路径点的直线单元上，通过下式解得视线法参考位置：

(x_LOS-x)²+(y_LOS-y)²＝R²

式中，R是以无人艇为中心所做的LOS圆半径，即自适应LOS圆半径；(x,y)为无人艇实时位置，(x_k-1,y_k-1)为预定的第k-1个航迹点坐标；

然后由投影算法计算出无人艇趋于期望的视线角ψ_LOS为：

无人艇在视线角的引导下不断向参考点运动，从而逐渐收敛到期望路径上，在快到达下一个路径点P_k时，通过与设定的转换半径进行比较，判断USV是否进入下一个直线路径单元的跟踪。

S2、根据路径偏差和航向偏差对期望视线角进行补偿，使无人艇能够更快地收敛到期望路径：

本发明中的路径跟踪是通过航向控制的方式间接实现的，虽然这种方式非常有效，但是不能快速地收敛到期望路径，为了实现更快地到达期望路径的目的，本发明分别设计了基于航向偏差和路径偏差的高斯函数航向角补偿器。

基于航向偏差的补偿器设计：

式中，Δψ是航向角偏差，ψ_r是无人艇的实际航向，ψ_LOS是期望视线角，ψ_H是航向补偿角，k_p是无人艇实际航行中的最大转向舵角，k₂是航向偏差补偿器的可调系数。

基于路径偏差的LOS视线角补偿器：

式中，ψ_P是路径偏差补偿角，k₃是路径偏差补偿器的可调系数，y_e是路径偏差。

无人艇的最终期望航向角为：

ψ_d＝ψ_LOS+ψ_H+ψ_P。

S3、为了克服传统的LOS在转弯角度较大时产生严重的超调，本发明还进一步提出了基于虚拟点的转向策略，采用三个小角度转向过渡大角度转向的方式进行转向：

当无人艇大角度转向时，本发明采用三个小转角过渡大转角的转向策略，如图2所示所示；P_k-1、P_k和P_k+1是相邻的三个航路点，坐标分别为(x_k-1,y_k-1)、(x_k,y_k)和(x_k+1,y_k+1)，A、B和C是三个虚拟转向点，坐标分别为(x_A,y_A)、(x_B,y_B)和(x_C,y_C)，且线段AP_k的距离等于线段P_kC的距离，首先根据转向角度计算出A和C点坐标，B点坐标采用△AP_kC的几何中心。当无人艇沿∠P_k-1P_kP_k+1航行时，由于转弯角度较大(转弯角度大于90°)，可转换成沿∠P_k-1ABCP_k+1航行，大大减小了转弯过程中的超调。

当x_k-x_k-1＝0时，x_A＝x_k-1＝x_k，y_A＝y_k-μk₈θ_tR_tmin；

当y_k-y_k-1＝0时，y_A＝y_k-1＝y_k，x_A＝x_k-Θk₈θ_tR_tmin；

当x_k-x_k-1≠0，y_k-y_k-1≠0时，

当x_k+1-x_k＝0时，x_C＝x_k＝x_k+1，y_C＝y_k+μk₈θ_tR_tmin；

当y_k+1-y_k＝0时，y_C＝y_k＝y_k+1，x_C＝x_k+Θk₈θ_tR_tmin；

当x_k+1-x_k≠0，y_k+1-y_k≠0时，

其中，k₈是回转系数，θ_t是转向角度，R_tmin是最小回转半径,

且当x_k+1＞x_k或x_k＞x_k-1时，Θ＝1，否则Θ＝-1；当y_k+1＞y_k或y_k＞y_k-1时，μ＝1，否则μ＝-1。

S4、为了提高跟踪效率，本发明还设计了航速解算器，可实时解算出无人艇的最佳期望速度；

设计航向偏差和路径点距离联合航速解算器，联合航速解算器可以根据无人艇实际航向角与所跟踪的路径方位角的偏差，同时考虑距前后两路径点的距离，解算出最佳期望航速传给速度控制环节；设计如下：

式中，Δψ_d是实际航向与实际期望航向的偏差，v_d是无人艇的期望航速，v_max是无人艇最大航速，Δl是距下一个路径点的距离，k₄、k₅是航速解算器可调参数。

S5、针对无人艇模型的复杂性性和不确定性，本发明还设计了智能自适应S面航向控制器和智能自适应积分S面航速控制器。

设计智能自适应S面航向控制器：

虽然现有的神经网络控制、模糊控制和进化算法都可以对经典S面控制器的参数进行整定，但这些方法在实际应用时，调节都比较繁琐，同时要求操作人员具有非常丰富的现场经验，在实际应用中有较大的局限性，因此本发明设计了一种智能自适应S面航向控制器，仅利用误差和误差变化率实时调整S面控制器的参数，结构简单，且具有较好的控制效果。智能自适应S面航向控制器如下：

其中，f^h表示航向控制输出，e^h和ec^h分别为航向偏差和航向偏差变化率，需要归一化处理，

和

表示航向控制器参数基准值，

和

表示航向控制器可调参数。

设计智能自适应积分S面航速控制器如下：

式中：f^v表示航速控制输出，k是积分步长，e^v和ec^v分别为航速偏差和航速偏差变化率，也需要归一化处理，

和

表示航速控制器参数基准值，

和

表示航速控制器可调参数。智能自适应积分S面航速控制器中的f_α(·,·)、f_β(·,·)与智能自适应S面航向控制器中的f_α(·,·)、f_β(·,·)函数形式相同。

实施例

为验证本发明提出的路径跟踪控制算法的有效性和高效性，现将其应用到一种无人艇模型中进行验证。在直角坐标系下，定义X轴为正东方向，Y轴为正北方向，且沿Y轴正半轴转动。

无人艇的初始状态设置为[x(0),y(0),ψ(0)]^T＝[31m,25m,-40°]^T、[u(0),v(0),r(0)]^T＝[0m/s,0m/s,0rad/s]^T，最大速度为v_max＝2.5m/s，参数R_min＝3m，R_k＝2m，δ＝2m，δ_max＝30°，k₁＝3，k₂＝k₃＝k₄＝k₅＝2，

k＝0.01，设计的路径点信息如下：

表1路径点信息

本发明的仿真效果如图4至图7所示，其中图4为路径跟踪仿真图，图5为路径偏差仿真图，图6为速度响应仿真图，图7为艏向角响应仿真图。

由以上仿真可知，本发明设计的路径跟踪方法可有效的引导无人艇按照预定路径航行，同时具有较好的拐点性能，在切换路径后，能以更快的速度收敛到期望航线，路径偏差较小，跟踪精度较高。

本发明与现有技术方案的比较：

无人艇的间接路径跟踪方案主要有两部分组成，一部分是制导律的计算，另一部分是航向和航速控制器的设计。要完成无人艇间接路径跟踪这一类任务的要求，除本发明外，还有其他经典LOS制导律计算和航向和航速控制器的设计，以下简单介绍这两部分，并将它们与本发明方案进行对比。

(1)基于经典LOS的制导律计算：

基于经典LOS的视线圆半径是固定值，这样形式虽然简单，但收敛的速度较慢，跟踪精度不高，后来，Liu T等人使用了一个可变的LOS圆半径，使LOS圆和路径之间总是有一个交集，但可变半径的调整范围较小，对小折角的路径自适应性较低。Fossen等提出了比例型LOS，Khaled等提出了指数型LOS，其中，虽然指数型LOS方法有效改善了LOS角的收敛性，但由于引入了指数函数和朗伯W函数，降低了LOS角的求解速度。

本发明在此基础上进行了创新和改进，不仅设计了基于过渡层的自适应LOS圆半径，还基于路径偏差和航向误差对LOS角进行了实时补偿调整，同时为了提高跟踪精度，提出了利用三个小转弯过渡大转弯的转向策略。

(2)航向和航速控制器设计

航向和航速控制器设计主要有基于误差和基于模型两大类，其中基于模型的控制算法，如反步法、鲁棒控制等，虽然可以达到精确的控制效果，但是由于实际应用时，不同海况下的无人艇精确的运动模型很难得到，致使该算法的控制精度受到很大影响。基于误差的控制方法，如PID、S面等，虽然不依赖精确的数学模型，但其控制参数不能自适应调整或调整比较繁琐。

本发明基于误差控制方法提出了一种智能自适应S面航向控制器和智能自适应积分S面航速控制器，不仅可以在线自适应调整控制器的参数，而且调节时不需要过度依赖操作经验，操作比较简便。

同时，其他方案都没有考虑到跟踪过程中的速度解算问题，致使跟踪的效率不高，本发明设计了基于航向偏差和路径偏差的联合速度解算器，大幅度提高了无人艇的路径跟踪精度和效率。

需要注意的是，具体实施方式仅仅是对本发明技术方案的解释和说明，不能以此限定权利保护范围。凡根据本发明权利要求书和说明书所做的仅仅是局部改变的，仍应落入本发明的保护范围内。

Claims (8)

1.一种基于航路点的水面无人艇自适应路径跟踪控制方法，其特征在于，所述方法包括以下步骤：

基于自适应LOS圆半径对应的LOS制导律进行视线角解算，然后根据路径偏差和航向偏差对视线角进行补偿得到最终期望航向角，使无人艇在最终期望航向角的引导下不断向期望路径运动；

基于虚拟点的转向策略，采用三个小角度转向过渡大角度转向的方式进行转向：

将行使路径上的相邻的三个航路点记为P_k-1、P_k和P_k+1，坐标分别为(x_k-1,y_k-1)、(x_k,y_k)和(x_k+1,y_k+1)，在P_k-1 P_k上设置虚拟转向点A，在P_k P_k+1上设置虚拟转向点C；A、B、C坐标分别为(x_A,y_A)、(x_B,y_B)和(x_C,y_C)，且线段AP_k的距离等于线段P_kC的距离；B点坐标采用△AP_kC的几何中心；

根据转向角度计算出A和C点坐标：

当x_k-x_k-1＝0时，x_A＝x_k-1＝x_k，y_A＝y_k-μk₈θ_tR_tmin；

当y_k-y_k-1＝0时，y_A＝y_k-1＝y_k，x_A＝x_k-Θk₈θ_tR_tmin；

当x_k-x_k-1≠0，y_k-y_k-1≠0时，

当x_k+1-x_k＝0时，x_C＝x_k＝x_k+1，y_C＝y_k+μk₈θ_tR_tmin；

当y_k+1-y_k＝0时，y_C＝y_k＝y_k+1，x_C＝x_k+Θk₈θ_tR_tmin；

当x_k+1-x_k≠0，y_k+1-y_k≠0时，

其中，k₈是回转系数，θ_t是转向角度，R_tmin是最小回转半径,l_APk＝k₈θ_tR_tmin，l_CPk＝k₈θ_tR_tmin，且当x_k+1＞x_k或x_k＞x_k-1时，Θ＝1，否则Θ＝-1；当y_k+1＞y_k或y_k＞y_k-1时，μ＝1，否则μ＝-1；

当无人艇沿∠P_k-1P_kP_k+1航行时，如果转弯角度大于90°，转换成沿∠P_k-1ABCP_k+1航行。

2.根据权利要求1所述的一种基于航路点的水面无人艇自适应路径跟踪控制方法，其特征在于，所述基于自适应LOS圆半径对应的LOS制导律进行视线角解算的具体过程包括以下步骤：

自适应LOS圆半径如下：

R_min-δ＜|y_e|≤R_min+δ

式中，R_min是LOS的最小圆半径，δ是过渡层的厚度，k₁是LOS圆半径可调系数，y_e是路径偏差；当|y_e|＞R_min+δ时，无人艇以当前最小前视距离收敛至期望路径，当路径偏差|y_e|≤R_min-δ时，无人艇以最小内切圆半径R_min趋向于期望路径，而当R_min-δ＜|y_e|≤R_min+δ时，LOS圆半径能够在[R_min,R_min+δ]之间光滑过渡；

一系列的航路点坐标记为P₁,P₂,…,P_k-1,P_k,P_k+1,…,P_n，P_k(x_k,y_k)为第k个坐标，P_LOS(x_LOS,y_LOS)为LOS的位置坐标，α_k-1是期望路径方位角，R_min是最小视线圆半径，R_δ是过渡层的视线圆半径，R_k是第k个切换下一个期望路径点的圆半径，Δ＞0是前视距离；

在连接相邻的两个路径点的直线单元上，通过下式解得视线法参考位置：

(x_LOS-x)²+(y_LOS-y)²＝R²

式中，R是以无人艇为中心所做的LOS圆半径，即自适应LOS圆半径；(x,y)为无人艇实时位置，(x_k-1,y_k-1)为预定的第k-1个航迹点坐标；

然后由投影算法计算出无人艇趋于期望路径的视线角ψ_LOS为：

无人艇在视线角的引导下不断向参考点运动，从而逐渐收敛到期望路径上。

3.根据权利要求2所述的一种基于航路点的水面无人艇自适应路径跟踪控制方法，其特征在于，所述根据路径偏差和航向偏差对视线角进行补偿得到最终期望航向角过程是基于航向偏差的LOS视线角补偿器和基于路径偏差的LOS视线角补偿器实现的；

基于航向偏差的LOS视线角补偿器如下：

式中，Δψ是航向角偏差，ψ_r是无人艇的实际航向，ψ_H是航向补偿角，δ_max是无人艇实际航行中的最大转向舵角，k₂是航向偏差补偿器的可调系数；

基于路径偏差的LOS视线角补偿器如下：

式中，ψ_P是路径偏差补偿角，k₃是路径偏差补偿器的可调系数；

无人艇的最终期望航向角为：

ψ_d＝ψ_LOS+ψ_H+ψ_P。

4.根据权利要求1、2或3所述的一种基于航路点的水面无人艇自适应路径跟踪控制方法，其特征在于，所述方法还包括利用航速解算器实时解算出无人艇的最佳期望速度的步骤，然后解算出最佳期望航速传给速度控制环节；

所述的航速解算器为航向偏差和路径点距离联合航速解算器，具体如下：

式中，Δψ_d是实际航向与实际期望航向的偏差，v_d是无人艇的期望航速，v_max是无人艇最大航速，Δl是距下一个路径点的距离，k₄、k₅是航速解算器可调参数。

5.根据权利要求4所述的一种基于航路点的水面无人艇自适应路径跟踪控制方法，其特征在于，所述方法还包括利用智能自适应S面航向控制器对航向进行控制的步骤；

所述智能自适应S面航向控制器如下：

其中，f^h表示航向控制输出，e^h和ec^h分别为航向偏差和航向偏差变化率，需要归一化处理，

和

表示航向控制器参数基准值，

和

表示航向控制器可调参数。

6.根据权利要求5所述的一种基于航路点的水面无人艇自适应路径跟踪控制方法，其特征在于，所述方法还包括利用智能自适应积分S面航速控制器对航速进行控制的步骤；

所述智能自适应积分S面航速控制器如下：

式中：f^v表示航速控制输出，k是积分步长，e^v和ec^v分别为航速偏差和航速偏差变化率，也需要归一化处理，

和

表示航速控制器参数基准值，

和

表示航速控制器可调参数，t表示时间。

7.根据权利要求6所述的一种基于航路点的水面无人艇自适应路径跟踪控制方法其特征在于，所述水面无人艇采用三自由度运动学方程和动力学非完全对称模型。

8.根据权利要求7所述的一种基于航路点的水面无人艇自适应路径跟踪控制方法其特征在于，所述的三自由度运动学方程和动力学非完全对称模型可表示如下：

式中：J(ψ)∈R^3×3代表无人艇从艇体坐标系{B}到大地坐标系{E}的旋转矩阵、M∈R^3×3代表惯性矩阵、C(v)∈R^3×3代表科氏力和向心力矩阵、D∈R^3×3代表阻尼力矩阵、B∈R^3×2代表执行器配置矩阵；η＝[x,y,ψ]^T为大地坐标系{E}下的位置和艏向角；v＝[u,v,r]^T为艇体坐标系{B}下的纵向速度、横向速度和艏向的角速度；d＝[d_u,d_v,d_r]^T为由外界的海风、海浪和海流引起的干扰力/力矩；f为控制的输入量。

Images