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

Abstract

本发明公开了一种水面无人艇路径跟踪控制方法，该方法包括以下步骤：1)设定期望跟踪路径，将期望跟踪路径离散化形成期望路径点阵；2)判断无人艇距离期望路径点阵的起始点的距离；3)根据设计参数和无人艇位置确定参考点的位置，根据路径曲率信息求得参考点处的曲率，然后根据制导算法公式求得加速度：4)计算由加速度引起的在Δt时间内航向角改变量；5)根据航向角改变量和当前无人艇航向角计算得到期望航向角；6)由期望航向和无人艇实际航向得到的航向偏差信息，计算得到当前时刻无人艇的舵角输出值，将其发送给底层执行机构，实现路径跟踪。本发明方法不依赖于无人艇精确数学模型，可跟踪任意类型路径。

Description

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

技术领域

本发明涉及无人艇路径跟踪控制技术，尤其涉及一种水面无人艇路径跟踪控制方法。

背景技术

水面无人艇作为一种自动化、智能化作战任务平台，可执行战略打击、扫雷侦查、海洋数据采集等多种任务。随着任务的大型化、复杂化程度加深，对无人艇的自主控制能力要求也越来越高，而路径跟踪能力作为衡量水面无人艇自主控制能力的关键指标之一，对其精度要求也越来越高。

目前国内外研究人员提出了不少水面无人艇路径跟踪控制方法，在外环制导方面，由于水面无人艇的欠驱动特性，现有方法大多采用基于LOS的制导算法，尽管有学者提出了ILOS、可变视距LOS等改进的LOS算法来提升跟踪精度，但由于LOS算法是一种直线制导算法，对曲线路径的跟踪精度较低，同时在折线路径转折处，若角度变化剧烈，无人艇由于具有大惯性、大时滞的特性，会造成较大精度损失，且有可能引起内环艏向控制器的超调。在内环控制方面，分为基于模型的控制与无模型控制两大类，基于模型的控制尽管理论上控制精度高，但需要知道精确的水动力参数，而该参数需要进行大量实验才能得到，尤其是目前我国运用于实际工程的无人艇均为中小型艇，在实际任务场景下更容易由于遭受风、浪、流的干扰造成无人艇的模型失配。无模型控制方法以PID控制为典型代表，从目前实际工程运用情况来看，基于PID的艏向控制是有效的，但PID本身并不适合控制大惯性、大时滞的对象，因此控制精度难以进一步提高。

发明内容

本发明要解决的技术问题在于针对现有技术中的缺陷，提供一种水面无人艇路径跟踪控制方法。

本发明解决其技术问题所采用的技术方案是：一种水面无人艇路径跟踪控制方法，包括以下步骤：

1)设定期望跟踪路径，将期望跟踪路径离散化形成期望路径点阵；

2)判断无人艇距离期望路径点阵的起始点的距离，若距离大于L，则无人艇朝向起始点行驶，直至距离小于L；其中，L为无人艇与参考点的设定距离；所述参考点为期望路径点阵中的点；

3)根据设计参数L和无人艇位置确定参考点的位置，根据路径曲率信息求得参考点处的曲率F，然后根据制导算法公式求得加速度Acc：

其中，V为无人艇的航向与航速的矢量，D为无人艇距离期望路径的径向距离，α为无人艇航向与无人艇和参考点连线之间的夹角，F表示参考点处的曲率值，n、m为设计常数，根据仿真或实测数据进行参数优化后确定，从而尽可能地提升控制精度；

4)计算由加速度引起的在Δt时间内航向角改变量Δψ为:

5)根据Δψ和当前无人艇航向角计算得到期望航向角，将其作为无人艇航向控制器的输入；

6)由期望航向和无人艇实际航向得到的航向偏差信息，计算得到当前时刻无人艇的舵角输出值，将其发送给底层执行机构，实现路径跟踪；

7)进入下一个控制周期，转入步骤3)开始不断重复上述流程，直至无人艇距离期望路径终点小于L，最终完成对期望路径的跟踪。

按上述方案，所述步骤6)中当前时刻无人艇的舵角输出值获取方式如下：

6.1)建立考虑环境干扰的CARIMA无人艇航向运动数学模型：

A(z^-1)ψ(t)＝z^-dB(z^-1)Δδ(t)+C(z^-1)ξ(t)

其中ψ表征航向角，δ表征舵角，C(z^-1)ξ(t)为表征环境扰动和测量误差的有色噪声，d表征系统纯时延，Δ＝1-z^-1为差分算子；

考虑船舶运动特点，将CARIMA航向运动模型输入阶数设定为2阶，输出阶数设定为2阶或3阶，干扰项阶数设定为1阶或2阶。

6.2)根据CARIMA无人艇航向运动模型递推得到未来N时刻的预测航向输出：

Y＝Y_m+GΔU；

上式中，Y为预测航向输出，Y_m为历史航向数据，ΔU为未来舵角输入，N为预测时刻，G为控制矩阵，k为当前时刻；

6.3)计算舵角最优输出值；

设置如下性能指标函数进行优化求解：

J＝E{(Y-Y_r)^T(Y-Y_r)+ΔU^TΓΔU}

Y_r为期望输出序列值，使得闭环响应沿着一条指定的、平滑的曲线到达设定值。通过极小化目标函数，计算得到本时刻无人艇的舵角最优输出值：

δ(k)＝δ(k-1)+Δδ(k)

＝δ(k-1)+H(G^TG+Γ)^-1G^T(Y_r-Y_m)

其中：

H＝[1,0,…,0]

6.4)执行机构接收舵角输出信号，控制舵机实现无人艇的转向。

按上述方案，所述步骤6)中还包括利用无人艇的历史舵角、航向信息，采用带遗忘因子的递推增广最小二乘法对CARIMA无人艇航向运动模型进行在线辨识，反馈校正的步骤，计算公式为：

式中：

其中，K(t)为权因子，P(t)为正定协方差，λ为遗忘因子，遗忘因子λ为0.95≤λ＜1。

本发明产生的有益效果是：

1、本发明不依赖于无人艇精确数学模型，可跟踪任意类型路径，由于考虑参考点曲率，因此能更好的跟踪曲线路径。同时由于考虑了本艇与期望路径的径向距离，当本艇距离期望路径较远时能更快的逼近期望路径，进一步提升跟踪精度。

2、本发明设计的基于广义预测控制的航向控制器具有较强的鲁棒性，通过系统在线辨识克服系统由于外部干扰等因素引起的模型失配以及控制系统的不稳定性，采用滚动式的有限时域优化来代替一成不变的全局优化，具有较好的航向控制精度。

附图说明

下面将结合附图及实施例对本发明作进一步说明，附图中：

图1是本发明实施例的方法原理示意图；

图2是本发明实施例的基于GPC的航向保持控制原理图；

图3是本发明实施例的制导算法原理示意图；

图4是本发明实施例的人艇路径跟踪仿真结果图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。

如图1所示，一种水面无人艇路径跟踪控制方法，包括以下步骤：

1)设定期望跟踪路径，将期望跟踪路径离散化形成期望路径点阵。然后设定制导律中的各项参数L、m、n。

2)判断无人艇距离期望路径起始点的距离，若距离大于L，则无人艇朝向起始点行驶，直至距离小于L。

3)根据设计参数L和无人艇位置可以确定参考点的位置，从而根据路径信息求得参考点处的曲率F，根据制导算法公式求得加速度Acc；本实施例采用的制导算法原理如图3所示；

计算可得由加速度引起的在Δt时间内航向角改变量Δψ为:

由Δψ和当前无人艇航向角可计算得到期望航向角，将其作为无人艇航向控制器的输入。

4)建立考虑环境干扰的CARIMA无人艇航向运动数学模型：

A(z^-1)ψ(t)＝z^-dB(z^-1)Δδ(t)+C(z^-1)ξ(t) (8.a)

其中ψ表征航向角，δ表征舵角，C(z^-1)ξ(t)为表征环境扰动和测量误差的有色噪声，d表征系统纯时延，Δ＝1-z^-1为差分算子。

考虑船舶运动特点，将CARIMA航向运动模型输入阶数设定为2阶，输出阶数设定为2阶或3阶，干扰项阶数设定为1阶或2阶。

传统的GPC算法中，通过引入Diophantine方程来求取未来j步的最优预测，这样则需要进行大量的矩阵计算并且在计算中并不能保证矩阵可逆，本方案直接根据CARIMA航向运动模型递推求解预测输出，避免求解Diophantine方程，从预测复杂度上降低了一个阶次，大大减少了计算量，同时不受C(z^-1)稳定的限制。本发明设计的航向控制器原理如图2所示：

根据CARIMA无人艇航向运动模型递推得到未来N步的预测输出：

Y＝Y_m+GΔU

上式中，Y为预测航向输出，Y_m为历史航向数据，ΔU为未来舵角输入，N为预测长度，G为控制矩阵。

设置如下性能指标函数进行优化求解：

J＝E{(Y-Y_r)^T(Y-Y_r)+ΔU^TΓΔU}

Y_r为期望输出序列值，使得闭环响应沿着一条指定的、平滑的曲线到达设定值。通过极小化目标函数，计算得到本时刻无人艇的舵角最优输出值：

δ(k)＝δ(k-1)+Δδ(k)

＝δ(k-1)+H(G^TG+Γ)^-1G^T(Y_r-Y_m)

其中：

H＝[1,0,…,0]

执行机构接收舵角输出信号，控制舵机实现无人艇的转向。

5)利用无人艇的历史舵角、航向信息，采用带遗忘因子的递推增广最小二乘法对CARIMA无人艇航向运动模型进行在线辨识，从而达到反馈校正的作用，减少干扰量给系统带来的不确定性影响。计算公式为：

式中：

其中，K(t)为权因子，P(t)为正定协方差，λ为遗忘因子，遗忘因子λ通常应选0.95≤λ＜1。

6)进入下一个控制周期，从步骤四开始不断重复上述流程，直至无人艇距离期望路径终点小于L，最终完成对期望路径的跟踪。

对上述的方案进行MATLAB仿真，验证本发明提出一种考虑海上环境干扰的水面无人艇路径跟踪方法的控制效果。验证时，CARIMA无人艇航向运动数学模型为：

ψ(t)-1.689ψ(t-1)+0.6879ψ(t-2)＝0.1688δ(t-2)+0.1464δ(t-3)+ξ(t)/Δ (15)

初始航速为10m/s，期望路径为正弦路径。控制参数L＝25，m＝1，n＝1，改进广义预测控制参数N＝15,Γ＝0.8,α＝0.75；带有遗忘因子的最小二乘辨识参数λ＝0.98,p(0)＝10⁶,控制步长取1s。路径跟踪结果如图4所示，红线表示本艇位置，蓝线表示期望路径，由图4可以看出，本发明设计的路径跟踪方法可以让无人艇在存在环境干扰的情况下，精确平滑且快速地跟踪期望路径，具有较好的跟踪性能。

应当理解的是，对本领域普通技术人员来说，可以根据上述说明加以改进或变换，而所有这些改进和变换都应属于本发明所附权利要求的保护范围。

Claims (2)

1.一种水面无人艇路径跟踪控制方法，其特征在于，包括以下步骤：

1)设定期望跟踪路径，将期望跟踪路径离散化形成期望路径点阵；

2)判断无人艇距离期望路径点阵的起始点的距离，若距离大于L，则无人艇朝向起始点行驶，直至距离小于L；其中，L为无人艇与参考点的设定距离；所述参考点为期望路径点阵中的点；

3)根据设计参数L和无人艇位置确定参考点的位置，根据路径曲率信息求得参考点处的曲率F，然后根据制导算法公式求得加速度Acc：

其中，V为无人艇的航向与航速的矢量，D为无人艇距离期望路径的径向距离，α为无人艇航向与无人艇和参考点连线之间的夹角，F表示参考点处的曲率值，n、m为设计常数，根据仿真或实测数据进行参数优化后确定；

4)计算由加速度引起的在Δt时间内航向角改变量Δψ为:

5)根据航向角改变量Δψ和当前无人艇航向角计算得到期望航向角，将其作为无人艇航向控制器的输入；

6)由期望航向和无人艇实际航向得到的航向偏差信息，计算得到当前时刻无人艇的舵角输出值，将其发送给底层执行机构，实现路径跟踪；所述步骤6)中当前时刻无人艇的舵角输出值获取方式如下：

6.1)建立考虑环境干扰的CARIMA无人艇航向运动数学模型：

A(z^-1)ψ(t)＝z^-dB(z^-1)Δδ(t)+C(z^-1)ξ(t)

其中，ψ为航向角，δ为舵角，C(z^-1)ξ(t)为环境扰动和测量误差的有色噪声，d为系统时延，Δ＝1-z^-1为差分算子；

6.2)根据CARIMA无人艇航向运动模型递推得到未来N时刻的预测航向输出：

Y＝Y_m+GΔU；

上式中，Y为预测航向输出，Y_m为历史航向数据，ΔU为未来舵角输入，N为预测时刻，G为控制矩阵，k为当前时刻；

6.3)计算舵角最优输出值；

设置如下性能指标函数进行优化求解：

J＝E{(Y-Y_r)^T(Y-Y_r)+ΔU^TΓΔU}

Y_r为期望输出序列值，使得闭环响应沿着一条指定的、平滑的曲线到达设定值，通过极小化目标函数，计算得到本时刻无人艇的舵角最优输出值：

δ(k)＝δ(k-1)+Δδ(k)

＝δ(k-1)+H(G^TG+Γ)^-1G^T(Y_r-Y_m)

其中：

H＝[1,0,…,0]

6.4)执行机构接收舵角输出信号，控制舵机实现无人艇的转向；

7)进入下一个控制周期，转入步骤3)开始不断重复上述流程，直至无人艇距离期望路径终点小于L，最终完成对期望路径的跟踪。

2.根据权利要求1所述的水面无人艇路径跟踪控制方法，其特征在于，所述步骤6)中还包括利用无人艇的历史舵角、航向信息，采用带遗忘因子的递推增广最小二乘法对CARIMA无人艇航向运动模型进行在线辨识，反馈校正的步骤，计算公式为：

式中：

其中，K(t)为权因子，P(t)为正定协方差，λ为遗忘因子。