CN110618686B - 一种基于显式模型预测控制的无人船航迹控制方法 - Google Patents

Abstract

本发明公开了一种基于显式模型预测控制的无人船航迹控制方法，包括步骤：首先建立无人船的操纵模型，获取无人船的位置姿态及参考航迹点信息；其次根据路径规划算法实时计算期望的航速和艏向，将船舶航迹控制简化为艏向控制；最后通过显式模型预测控制算法得到推进器转速和舵角，并作用于无人船以实现船舶航迹控制。本发明将显式模型预测控制算法应用于无人船航迹控制领域，通过引入多参数二次规划理论，将复杂的在线优化过程转换为简单的查表过程，降低了在线计算的复杂度，提升了航迹控制系统的实时性。

Description

一种基于显式模型预测控制的无人船航迹控制方法

技术领域

本发明涉及无人船航迹控制技术领域，尤其涉及一种基于显式模型预测控制的无人船航迹控制方法。

背景技术

无人水面船(USV，Unmanned Surface Vessel)作为一种无人化、智能化运载平台，广泛应用于科研、商业和探险等方面。精确的航迹控制是保证船舶安全航行、省时节能的基础，因此成为船舶运动控制领域的研究热点。

目前，国内外学者对无人船航迹控制问题进行了研究，提出了模糊控制，反步控制，神经网络等方法。但上述方法并未考虑船舶的操纵特性约束，在实际应用中存在一定的局限性。模型预测控制(Model predictive control，MPC)是一种先进的智能反馈控制方法,能够显式处理系统输入-输出和状态约束，并通过建立指标函数优化系统性能，对解决无人船航迹控制问题具有独特的优势，因此，越来越多的学者使用MPC算法设计无人船航迹控制器。Annamalai^[1]等针对无人船航行时可能发生的动力学性能突变，设计了一种自适应模型预测控制器。Liu^[2]等设计了一种基于干扰观测器的模型预测控制器，以抑制环境干扰，但实验结果存在一定的误差，需提高观测器精度。李国胜^[3]等考虑航向与舵减横摇之间存在强耦合的特性，采用4自由度船舶模型，设计了基于预测控制的航迹控制器，实现了航向与舵减横摇的协调控制，但未考虑舵速的限制。张子昌^[4]等通过路径规划算法将航迹控制转化为航向控制，并设计了基于模型预测控制的航向控制器。然而上述方法均需要反复的在线求解最优问题，计算量较大，难以应用于实时性要求较高的系统中。

发明内容

发明目的：针对以上问题，本发明提出一种基于显式模型预测控制的无人船航迹控制方法，有效缩短了在线计算时间，满足无人船航迹控制的实时性要求。

技术方案：为实现本发明的目的，本发明所采用的技术方案是：基于显式模型预测控制的无人船航迹控制方法，包括步骤：

(1)建立水面无人船三自由度操纵模型，构建状态空间表达式作为预测模型，并获取预设路径的航迹点坐标；

(2)获取无人船的位置、艏向角和速度信息及预设路径的航迹点坐标；

(3)根据当前无人船与航迹点的位置，通过路径规划算法求取导引点坐标和无人船的期望艏向角，将船舶航迹控制转化为艏向控制；

(4)根据显式模型预测控制算法计算控制舵角并作用于无人船，以实现船的航迹控制；

(5)判断无人船是否到达当前航迹点，若未到达，则继续以该航迹点作为跟踪目标；反之，判断航迹点是否为终点，若是，则结束航行，若不是，则将下一航迹点设为当前航迹点，进入步骤2继续跟踪预设路径。

进一步地，所述步骤1中，设定无人船以固定的纵荡速度航行，因此可设状态空间表达式的状态量x＝[ψ v r]^T，输入量u＝δ_r，输出量y＝ψ，其中，ψ为艏向角，v为横荡速度，r为转向率，δ_r为控制舵角。

进一步地，所述步骤1中，状态空间模型采用控制量增量的形式表示，并在状态空间中加入参考状态，改写的状态空间模型为：

其中，

y(k)的期望值r(k)＝[ψ_los 0]，控制增量Δu(k)＝u(k)-u(k-1)。

进一步地，所述步骤4中，将控制增量作为状态量构建目标函数，具体定义为：

其中H_p和H_c分别是预测时域和控制时域，且H_c≤H_p；y_k+i|k，r_k+i|k和Δu_k+i|k分别表示在k时刻系统实际输出量，参考输出量和控制增量在k+i时刻的预测值；

是由最优控制增量构成的控制增量序列；Q,R和P分别是状态，输入和终端权重矩阵，且满足Q＝Q^T≥0,R＝R^T≥0,P≥0。

进一步地，所述步骤5中，判断无人船是否到达当前航迹点的方法为：计算无人船到当前航迹点的距离，若距离小于1.2m，则表明无人船到达当前航迹点，并将下一航迹点设为当前航迹点。

有益效果：与现有技术相比，本发明的突出优点是：

(1)用传统的状态空间模型作为预测模型时，其反馈控制率u(k)＝-Kx(k)中不存在积分环节，当受到环境干扰时，状态量会存在静差，为消除稳态静差，本发明采用输入量增量形式的状态空间模型，以实现无静差控制；

(2)采用显式模型预测控制算法，将传统模型预测控制的反复在线优化过程，转化为离线计算和在线查表两部分，大大减少了在线计算时间，节省了硬件资源，使算法能满足无人船航迹控制实时性的要求。

附图说明

图1是基于显式模型预测控制的无人船航迹控制方法流程图；

图2是航迹控制原理图；

图3是路径规划算法示意图；

图4是无人船航迹跟踪效果图；

图5是航迹跟踪误差图；

图6是艏向角曲线图；

图7是控制器输出量曲线图；

图8是基于EMPC和MPC的控制器CPU用时对比图

具体实施方式

下面结合附图和实施例对本发明的技术方案作进一步的说明。

如图2控制方法流程图和图3控制系统结构框图所示，本发明所述的基于显式模型预测控制的无人船航迹控制方法，包括步骤：

步骤一：建立水面无人船三自由度操纵模型，构建状态空间表达式作为预测模型，并获取预设路径的航迹点坐标；

为了方便描述无人船的航迹控制问题，考虑无人船的纵荡速度为常值，无人船的三自由度操纵模型为：

式中，η＝[x y ψ]^T为无人船位置向量，其中(x,y)为船体在北东坐标系下的位置，ψ为艏向角；ν＝[u_c v r]^T为无人船速度向量，其中，u_c为纵荡速度，v为横荡速度，r为转向率，δ_r为控制舵角；参数

d₂₂＝-Y_v，d₃₃＝-N_r，其中m为无人船的质量，I_z为艏摇方向的转动惯量，

为附加质量，X_u、Y_v、N_r为阻尼系数；N_δ为舵系数。

将船舶动力学模型重写为状态空间模型：

其中：

x＝[ψ v r]^T,u＝δ_r,y＝ψ

采用欧拉法对方程离散化处理，可得到离散时间的状态空间模型：

式中：A_d,B_d,C_d为离散化后的状态空间矩阵。本发明采用增量形式的状态空间模型，以实现无静差控制。另外，对于跟踪问题，本发明在状态空间中加入参考状态，改写的状态空间模型如下所示：

其中，

y(k)的期望值r(k)＝[ψ_los 0]，控制量Δu(k)＝u(k)-u(k-1)。

定义基于Δu的性能指标函数为：

式中，H_p和H_c分别是预测时域和控制时域，且H_c≤H_p；y_k+i|k，r_k+i|k和Δu_k+i|k分别表示在k时刻系统实际输出量，参考输出量和控制增量在k+i时刻的预测值；

是由最优控制增量构成的控制增量序列；Q,R和P分别是状态，输入和终端权重矩阵，且满足Q＝Q^T≥0,R＝R^T≥0,P≥0。

步骤二：获取无人船的位置、艏向角和速度信息及预设路径的航迹点坐标；

步骤三：根据当前无人船与航迹点的位置，通过路径规划算法求取导引点坐标和无人船的期望艏向角，将船舶航迹控制转化为艏向控制；

如图3所示，预设路径由航迹点构成，控制无人船艏向角与导引艏向角ψ_LOS相同时，无人船便能跟踪预设路径。路径规划(line-of-sight,LOS)角可由LOS位置和LOS向量表示，LOS位置是相邻航迹点之间航迹线上的一点，无人船根据期望艏向角能够到达的位置。LOS向量是从无人船当前位置指向LOS位置的向量。

无人船当前位置用P(x,y)表示，LOS位置用P_los(x_los,y_los)表示，当前和下一航迹点分别用P_k(x_k,y_k)，P_k+1(x_k+1,y_k+1)表示。当不考虑环境力影响时，期望的艏向角为：

ψ_los(e)＝χ_p+χ_r(e)(6)

式中χ_p，χ_r(e)分别为预设路径的切向角和航速与路径的夹角：

χ_p＝α_k＝atan2(y_k+1-y_k,x_k+1-x_k)(7)

Δ为预设的前视距离。e为无人船与(P_k，P_k+1)间航迹段的偏差值：

e＝-[x-x_k]sin(α_k)+[y-y_k]cos(α_k)(9)

利用路径规划算法，求取导引艏向角ψ_LOS，并控制无人船的艏向角收敛于ψ_LOS，可确保无人船位置收敛于预设航迹。因此可将

问题转换为

问题。

步骤四：根据显式模型预测控制算法计算期望的艏向角并作用于无人船，以实现船的航迹控制；

对于带约束有限时间最优控制问题(5)，由(4)可得：

式中i＝1,2,...,H_p。

式(10)表明任意时刻系统的状态

均可由初始状态

和输入向量Δu线性表示。根据式(10)，预测时域内，系统的状态向量表示为：

式中：

将式(11)带入式(5)中，并忽略常数项，可得：

式中，

矩阵G，W和E可由式(11)计算得到。

接着继续定义

式(12)可以简化为：

其中，

式(13)中，z是ξ_k的一个仿射函数，可利用一阶Karush-Kuhn-Tucker(KKT)条件求解^[9]，其显式表达式为：

同时根据KKT条件，我们可以得到对应于初始状态ξ_k的一个临界区域CR₀。将

带入式(14)中，得到CR₀内控制序列ΔU关于ξ_k的显式表达式，根据滚动优化原理，将控制序列ΔU的第一项作用于被控对象中，则

同理，利用上述方法，可得到其余区域的状态分区及对应的控制率，即

式中，i∈{1,...,n}，n为状态分区个数。

因此最终得到的显式PWM控制律如下所示：

式中，H_i，K_i是仅与序列号为i的状态分区相关的定长矩阵。

将上述离线计算过程的有效数据H_i、K_i、F_i、G_i预先存储在系统中，则在线计算过程可转换为查表过程：检测无人船系统在某一时刻的状态，判定该状态所在的状态分区，得到相应的控制率Δu，并作用于无人船上，然后更新无人船的状态量；下一时刻，再次检测无人船的状态量作为初始状态，重复上述查表过程，以实现无人船航迹的显式模型预测控制。

步骤五：判断无人船是否到达当前航迹点，若未到达，则继续以该航迹点作为跟踪目标；反之，判断航迹点是否为终点，若是，则结束航行，若不是，则将下一航迹点设为当前航迹点，进入步骤2继续跟踪预设路径。

判断规则如下：计算无人船到当前航迹点的距离，若距离小于1.2m，则表明无人船到达当前航迹点，并将下一航迹点设为当前航迹点。

实施例：为了验证本发明方法的可行性和有效性，以一艘长为1.6m的模型船为被控对象进行仿真试验，采用的船舶操纵模型如公式(1)所示，模型中的具体参数为：u_c＝0.4m/s：I_z＝2.7kgm²，N_δ＝-0.5Y_v＝-20.5，N_r＝-1.2522，

该实施例中设置离散航迹点为((0,0)，(40,40)，(0,80),(80,120)，(200,60)，(240,60)，(160，-40)，(80,0)，(40，-40)，(0,0))，设定方向舵的舵角和舵速的约束分别为-30°<δ<30°，-20°<Δδ<20°，预测时域N_y＝10，控制时域N_c＝4，权重系数矩阵Q＝diag[1000100]，R＝0.1，P可由Riccati方程求解。

图4为无人船航迹跟踪效果图，通过仿真结果可以看出，采用基于LOS和EMPC相结合的航迹控制器能够使船舶快速跟踪上期望的轨迹，有效实现航迹控制。

图5-图7将显式模型预测控制方法和线性二次调节器(linear quadraticregulator,LQR)方法的控制效果进行对比。图5为两种方法下无人船航迹偏差曲线，图6和图7为无人船通过第一个航迹点时艏向角和舵角的变化曲线。由图5可知，除了在航迹点切换阶段误差较大外，两种方法的航迹偏差均控制在较小的范围内，满足航迹控制要求。但从图6和图7可以看出，由于EMPC能够实时处理舵角幅值和变化率约束限制，使得EMPC作用下艏向角的超调量远小于LQR，因此EMPC控制率更符合实际工程要求。

图8为基于EMPC和MPC的控制器CPU用时对比图，可以看出EMPC控制器占用CPU的时间远小于MPC；另外，随着无人船通过航迹点的个数增加，两种方法所消耗的时间也不断增加，但EMPC算法的增速明显低于MPC算法。

文献1：Annamalai A S K,Sutton R,Yang C,et al.Robust Adaptive Controlof an Uninhabited Surface Vehicle[J].Journal of Intelligent&Robotic Systems,2015,78(2):319-338.

文献2：Liu Z,Geng C,Zhang J.Model predictive controller design withdisturbance observer for path following of unmanned surface vessel[C]//2017IEEE International Conference on Mechatronics and Automation(ICMA).IEEE,2017.

文献3：李国胜,张军,刘志林,孙太任.有横摇角约束的欠驱动船舶航迹跟踪协调控制[J].武汉理工大学学报(交通科学与工程版),2017,41(02):283-288.

文献4：张子昌,林莉,徐雪峰,赵光.基于模型预测控制的潜艇水平面航迹控制算法[J].中国舰船研究,2019,14(03):172-178.

Claims (1)

1.一种基于显式模型预测控制的无人船航迹控制方法，其特征在于，包括步骤：

(1)建立无人船三自由度操纵模型，并构建状态空间表达式作为预测模型；

无人船的三自由度操纵模型为：

式中，η＝[x y ψ]^T为无人船位置向量，其中(x,y)为船体在北东坐标系下的位置，ψ为艏向角；ν＝[u_c v r]^T为无人船速度向量，其中，u_c为纵荡速度，v为横荡速度，r为转向率，δ_r为控制舵角；参数

d₂₂＝-Y_v，d₃₃＝-N_r，其中m为无人船的质量，I_z为艏摇方向的转动惯量，

为附加质量，X_u、Y_v、N_r为阻尼系数；N_δ为舵系数；

将船舶动力学模型重写为状态空间模型：

其中：

x＝[ψ v r]^T,u＝δ_r,y＝ψ

采用欧拉法对方程离散化处理，得到离散时间的状态空间模型：

式中：A_d,B_d,C_d为离散化后的状态空间矩阵，在状态空间中加入参考状态，改写的状态空间模型如下所示：

其中，

y(k)的期望值r(k)＝[ψ_los 0]，控制量△u(k)＝u(k)-u(k-1)；

定义基于△u的性能指标函数为：

式中，H_p和H_c分别是预测时域和控制时域，且H_c≤H_p；y_k+i|k，r_k+i|k和△u_k+i|k分别表示在k时刻系统实际输出量，参考输出量和控制增量在k+i时刻的预测值；

是由最优控制增量构成的控制增量序列；Q，R和P分别是状态，输入和终端权重矩阵，且满足Q＝Q^T≥0,R＝R^T≥0,P≥0；

(2)获取无人船的位置、艏向角和速度信息及预设路径的航迹点坐标；

(3)根据当前无人船与航迹点的位置，通过路径规划算法求取无人船的期望航速和艏向，将船舶航迹控制简化为艏向控制；

预设路径由航迹点构成，控制无人船艏向角与导引艏向角ψ_LOS相同时，无人船便能跟踪预设路径，路径规划角由LOS位置和LOS向量表示，LOS位置是相邻航迹点之间航迹线上的一点，无人船根据期望艏向角能够到达的位置，LOS向量是从无人船当前位置指向LOS位置的向量；

无人船当前位置用P(x,y)表示，LOS位置用P_los(x_los,y_los)表示，当前和下一航迹点分别用P_k(x_k,y_k)，P_k+1(x_k+1,y_k+1)表示；当不考虑环境力影响时，期望的艏向角为：

ψ_los(e)＝χ_p+χ_r(e)   (6)

式中，χ_p，χ_r(e)分别为预设路径的切向角和航速与路径的夹角：

χ_p＝α_k＝atan2(y_k+1-y_k,x_k+1-x_k)   (7)

△为预设的前视距离，e为无人船与(P_k，P_k+1)间航迹段的偏差值：

e＝-[x-x_k]sin(α_k)+[y-y_k]cos(α_k)   (9)

利用路径规划算法，求取导引艏向角ψ_LOS，并控制无人船的艏向角收敛于ψ_LOS，可确保无人船位置收敛于预设航迹，因此将

问题转换为

问题；

(4)根据显式模型预测控制算法计算控制舵角并作用于无人船，以实现船的航迹控制；

对于带约束有限时间最优控制问题式(5)，由式(4)得到：

式中，i＝1,2,...,H_p，式(10)表明任意时刻系统的状态

均可由初始状态

和输入向量△u线性表示；

根据式(10)，预测时域内，系统的状态向量表示为：

式中：

将式(11)带入式(5)中，并忽略常数项，得到：

式中，

矩阵G，W和E可由式(11)计算得到；

接着继续定义

式(12)简化为：

其中，

式(13)中，z是ξ_k的一个仿射函数，可利用一阶Karush-Kuhn-Tucker(KKT)条件求解，其显式表达式为：

同时根据KKT条件，得到对应于初始状态ξ_k的一个临界区域CR₀；

将

带入式(14)中，得到CR₀内控制序列△U关于ξ_k的显式表达式，根据滚动优化原理，将控制序列△U的第一项作用于被控对象中，则

同理，利用上述方法，得到其余区域的状态分区及对应的控制率，即

式中，i∈{1,...,n}，n为状态分区个数；

因此，最终得到的显式PWM控制率如下所示：

式中，H_i，K_i是仅与序列号为i的状态分区相关的定长矩阵；

将上述离线计算过程的有效数据H_i、K_i、F_i、G_i预先存储在系统中，则在线计算过程可转换为查表过程：检测无人船系统在某一时刻的状态，判定该状态所在的状态分区，得到相应的控制率△u，并作用于无人船上，然后更新无人船的状态量；下一时刻，再次检测无人船的状态量作为初始状态，重复上述查表过程，以实现无人船航迹的显式模型预测控制；

最后，将控制增量作为状态量构建目标函数，具体定义为：

其中，H_p和H_c分别是预测时域和控制时域，且H_c≤H_p；y_k+i|k，r_k+i|k和△u_k+i|k分别表示在k时刻系统实际输出量，参考输出量和控制增量在k+i时刻的预测值；

是由最优控制增量构成的控制增量序列；Q,R和P分别是状态，输入和终端权重矩阵，且满足Q＝Q^T≥0,R＝R^T≥0,P≥0；

(5)判断无人船是否到达当前航迹点，若未到达，则继续以该航迹点作为跟踪目标；反之，判断航迹点是否为终点，若是，则结束航行，若不是，则将下一航迹点设为当前航迹点，进入步骤(2)继续跟踪预设路径；

判断规则如下：计算无人船到当前航迹点的距离，若距离小于1.2m，则表明无人船到达当前航迹点，并将下一航迹点设为当前航迹点。

Images