一种基于改进EMPC的高速无人艇路径跟踪控制方法

技术领域

本发明涉及高速无人艇控制技术领域，具体涉及一种基于改进EMPC的高速无人艇路径跟踪控制方法。

背景技术

路径跟踪技术作为无人艇安全、自主、精准和快速完成各项任务的重要保障，其控制目标是设计控制器使得无人艇精准的跟踪并保持在跟踪空间中一条独立于时间的期望路径上。欠驱动无人艇高速航行过程中存在非线性、模型不确定性、外界干扰时变性以及在实际工程应用中参数自适应性、控制器鲁棒性、跟踪效果稳定性和航行安全性等问题，因此设计自适应能力强、鲁棒性高、抗干扰能力强、实时性高的路径跟踪控制器，对高速无人艇任务适应能力、调试安全性将发挥重要作用。

目前主要的控制方法有反步法、滑模控制、动态面控制、干扰观测器(DisturbanceObservers,DO)、神经网络、自适应控制和模型预测控制等。但这些方法在实际应用中各有缺点，针对这些缺点，国内外研究人员提出了各种改进方案。如Sun Z等人针对积分滑模在船舶跟踪控制时会出现不可消除项ψ

现有最接近本文方法为陈天元硕士在基于显示模型预测控制的无人船航迹控制方法研究中提出的利用显示模型预测控制“离线计算，在线综合”的特点来解决无人船实际航行中的实时性要求的问题。该方法首先利用视线制导法(LOS)将无人船航迹控制简化为艏向控制，再将显示模型预测控制算法应用于无人船艏向控制问题中，在保证控制精度的同时，提高了计算速度，在一定程度上解决了高速航行时的实时性问题。

上述最接近本文的技术方案，虽然在一定程度上减少了计算时间，但在实际控制时损失了一定的控制精度，因此需要在离线计算时，加入合适的算法对各个状态分区上的目标函数进行最优化求解，得到相应的最优控制律，以此来补偿损失的控制精度。同时传统的LOS制导法的参数自适应能力较弱，且易受环境干扰产生漂角，因此需要分别对直线和曲线制导进行改进，减少横侧偏差，提升路径切换时的自适应能力，使得USV能够更快的到达期望路径。

发明内容

本发明提供了一种基于改进EMPC的高速无人艇路径跟踪控制方法，以解决现有技术中会损失控制精度，制导法的参数自适应能力较弱，且易受环境干扰产生漂角的问题。

本发明提供了一种基于改进EMPC的高速无人艇路径跟踪控制方法，包括如下步骤：

步骤1：构建欠驱动水面高速无人艇的动力学模型、运动学模型、固定坐标系、无人艇载体坐标系；

步骤2：在制导方法中，结合当前船速通过动态视线法对LOS前视园半径进行优化，在优化后的LOS前视园半径基础上结合横侧偏差率获取期望航向角，并根据实际航向角获得偏向角；

步骤3：在控制方法的EMPC控制器离线状态中，通过白鹭群优化算法，对离线状态下的各个状态分区进行最优控制律求解，得到各个状态分区及对应分区上的线性控制律；

步骤4：在控制方法的EMPC控制器在线状态中，通过可达分区查找法查找步骤3中获得的对应分区上的线性控制律，用查询到的线性控制率控制无人艇运行，直至无人艇跟踪上最后一个期望路径点，完成跟踪控制过程。

进一步地，所述步骤1中，所述欠驱动水面高速无人艇的动力学模型中的干扰模型包括：风干扰力模型、浪干扰力模型、流干扰力模型，具体公式为：

式中，[u，v，r]

进一步地，所述风干扰力模型为：

式中，

进一步地，所述浪干扰力模型为：

式中，ρ为海水密度，χ为遭遇角，m为海浪谱频率的分割数量，Xb，Yb，Nb为试验系数，

进一步地，所述流干扰力模型为：

式中，F

进一步地，所述步骤2中，通过当前船速对LOS前视园半径进行优化的公式如下：

R

式中，R

在优化后的LOS前视园半径基础上结合横侧偏差率获取期望航向角的具体公式如下：

式中，

进一步地，所述白鹭群优化算法中的判别条件为：

本发明的有益效果：

本方法针对传统模型预测控制在高速无人艇路径跟踪过程中参数计算量大，求解效率慢的问题，采用了显示模型预测控制的方法，同时在离线计算部分引入了白鹭群优化算法对各状态分区最优控制律的求解进行改进。结合ESOA算法和EMPC控制器的优点，利用白鹭群优化算法对参数、初值和目标函数等要求低，且存在分布寻优策略、全局收敛性好、收敛迅速等优点，对目标函数进行最优求解，在整体上提升了高速无人艇路径跟踪的控制精度。同时，改进的LOS制导法包含了对干扰产生漂角的补偿，在一定程度上减小了横侧偏差，同时提高了制导参数的自适应能力。

附图说明

通过参考附图会更加清楚的理解本发明的特征和优点，附图是示意性的而不应理解为对本发明进行任何限制，在附图中：

图1为本发明具体实施例中视线法制导示意图；

图2为本发明具体实施例中白鹭群优化算法框图；

图3为本发明具体实施例中优化算法收敛曲线对比图。

图4为本发明具体实施例中可达分区算法流程图；

图5为本发明具体实施例中控制系统框图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明实施例提供一种基于改进EMPC的高速无人艇路径跟踪控制方法，包括如下步骤：

步骤1：构建欠驱动水面高速无人艇的运动学、动力学数学模型。采用Fossen船舶动力学数学模型，将船舶六自由度模型简化为无人艇纵荡、横荡和艏摇三自由度的无人艇运动模型。

1)水面高速无人艇运动学模型：

2)水面高速无人艇动力学模型：

式中，[x，y，ψ]

本具体实施例中的无人艇仅可产生纵向推力τ

式中，

风速一般包含一个缓慢变化的分量(平均风速)和一个高频分量(阵风)，作业在水面无人艇上的合力和力矩通常根据相对风速V

(5)

γ

V

(6)

u

v

式中，ψ

式中，C

对于海上的波浪我们一般用线性叠加来描述，为了预报水面无人艇在不规则海浪中所受的干扰力，在水面无人艇耐波性理论中通常采用根据大量的海浪观测数据所统计的海浪谱对其进行描述，常用的海浪谱有PM谱、单参数谱以及双参数谱，本文采用国际船模水池会议所推荐的ITTC双参数海浪谱对不规则海浪进行预报，公式如下：

式中，ζ

根据海浪谱，可以得到海浪的波面方程：

波长为：

最后，根据波浪对船体作用力公式，可得到海浪对水面无人艇的作用力计算公式如下：

式中，ρ为海水密度，χ为遭遇角，m为海浪谱频率的分割数量，Xb，Yb，Nb为试验系数，可按J.W English等人试验所得的回归公式进行估计：

记海流流速为V

根据坐标转换公式可得：

将式3-22代入上式得：

/>

设水面无人艇运动对地速度在船体坐标系上的分量为u、v，水面无人艇运动对水流的相对速度的投影为：

(16)

v

对于水面无人艇动力学模型，其流体相对运动而产生的作用力为：

(17)

F

使用相对速度对实际速度替换，可得海流干扰后的流体作用力：

(18)

F

差分后，可得到海流作用力：

步骤2：在制导方法中，结合当前船速通过动态视线法对LOS前视园半径进行优化，在优化后的L0S前视园半径基础上结合横侧偏差率获取期望航向角，并根据实际航向角获得偏向角。假设当前时刻无人艇所在位置为点p

根据无人艇当前位置和期望圆心位置求得目标角度：

根据圆跟踪方向求得期望路径点为：

则可得期望航向角：

在复杂海况环境下，无人艇进行圆路径跟踪时，会产生一个对航行方向造成干扰的漂角，为消除漂角影响，以及根据横侧偏差、横侧偏差变化律，设计一个S面控制得弧度纠偏制导律，来引导无人艇进行圆跟踪。

无人艇距离期望路径横侧偏差为：

并且，横侧偏差变化率可得：

无人艇期望航向夹角为：

根据上式可得无人艇纠偏后的期望航向为：

式中，

因此，再根据实际航向角

路径段的更新。在选择路径P

将选择(x

(23)

R

其中，e

对预测控制模型进行线性化处理，通过给定的期望状态、LOS算法处理后的期望航向角

在任意参考点(xR，uR)处对函数进行一阶泰勒展开，得到式(33)

由式(33)减去(32)，得到

/>

其中，

对新的预测模型也就是式(34)进行离散化处理。离散化处理的方法有很多，比如龙格-库塔法等，这里采用前向欧拉的方法进行处理，得到式(35)：

其中，T为采样时间，I为单位矩阵，结合式子可得：

其中

将上述式子进行变换，化简为控制增量形式的状态空间模型：

其中

步骤3：在控制方法的EMPC控制器离线状态中，在约束条件的设置下，通过白鹭群优化算法，对离线状态下的各个状态分区进行最优控制律求解，得到各个状态分区及对应分区上的线性控制律。

约束条件设置：由于无人船的螺旋桨和舵机受机械性能的影响，其运动性能和速率是有限的，高速航行时易出现饱和现象。所以，设置在k时刻及预测时域内，控制量极限、控制增量、输出量约束。

目标函数的最优求解：考虑到目标的速率和系统输入的控制量能量的损耗。以系统的状态量偏差、控制量以及控制增量构建目标函数，目标函数如下：

(32)

J＝(Y-Yref)

其中，Yref为期望值，ΔU为控制增量，Q和R为权重矩阵(根据控制的需要不断调整)。

白鹭群优化算法由三个主要部分组成：坐等策略、激进策略和判别条件。算法框图如图2所示。

每个白鹭群可以由n支白鹭小队组成，每只白鹭小队又包含三只白鹭，其中白鹭A实行坐等策略，白鹭B和白鹭C分别采用激进策略中的随机游走和包围机制。

1)坐等策略(白鹭A)。第i个白鹭A的观测方程可以描述为

X

其中t为当前迭代次数，t

2)激进策略(白鹭B、白鹭C)。白鹭B为随机游走，更新位置的方式如下：

X

r

白鹭C为包围策略，更新位置的方式如下：

D

D

X

其中X

3)判别条件。白鹭小队的每只白鹭计算出更新的位置后，会共同决定白鹭小队的更新位置，形式如下：

X

y

c

白鹭小队将三只白鹭更新的位置及适应度与上一次迭代的适应度比较，如果有一只白鹭的更新位置要优于上一次迭代的位置，则采纳更新；若每一只白鹭的更新位置都要比上一回差，则有33％的概率采纳更新位置最优的方案。

图3为白鹭群优化算法与粒子群算法的收敛曲线对比图，左半部图像为目标函数搜索空间，右半部为两种优化算法的收敛曲线，通过曲线对比，可明显看出白鹭群优化算法相较于粒子群算法收敛速度更快，求解精度更高。

利用白鹭群优化算法求解最优控制增量序列，得到式(50)：

ΔU

步骤4：在控制方法的EMPC控制器在线状态中，根据高速无人艇传感器输出的实际状态量，采用可达分区查找法进行在线查找，判断状态分区，查找对应的最优控制律，将控制分量作用到无人艇。

可达分区查找法算法流程图如图4所示，具体步骤如下：

1)对于初始状态x

2)根据子区域n得到对应的控制量u；

3)将该控制量作用于系统，在下一时刻检测系统状态量；

4)根据系统的状态量和可达分区搜索状态量所在状态分区。

5)重复(2)(3)(4)，直至查表结束。

将查表所得的最优控制分量式作用到高速无人艇上。

u(t)

整个控制系统框图如图5所示，本文改进了预测控制方法，采用LOS-ESOA-EMPC控制方法。通过建立欠驱动无人船的动力学模型作为预测控制的状态空间模型，将状态空间模型进行线性化、离散化等处理化为误差形式的控制增量方程作为系统模型，设定一条期望路径曲线，将期望路径均匀划分为多个期望点，获得期望点位置信息并根据当前无人艇状态采用改进的LOS制导算法计算期望航向角。期望航向角与实际航向角之间的误差作为EMPC控制器的输入，离线计算时，在初始状态和约束条件的设置下，采用白鹭群优化算法(ESOA)进行目标函数最优化求解，得到各个状态分区域及对应分区上的线性控制律，在线计算时，根据高速无人艇传感器输出的实际状态量，判断状态分区，查找对应的控制律，将控制分量作用到无人艇。根据规划好的期望路径，重复步骤1～3，待下一时刻，继续将高速无人艇测量传感器的实际值反馈给EMPC控制器，不断进行在线查找，直至高速无人艇跟踪至最后一个期望点。

虽然结合附图描述了本发明的实施例，但是本领域技术人员可以在不脱离本发明的精神和范围的情况下作出各种修改和变型，这样的修改和变型均落入由所附权利要求所限定的范围之内。