CN112731915A - 一种基于卷积神经网络优化nmpc算法的直接航迹控制方法 - Google Patents

Abstract

本发明涉及一种基于卷积神经网络优化NMPC算法的直接航迹控制方法，属于控制技术领域。水面USV的航迹控制采用直接性控制算法具有较高的控制精度，但是算法复杂，容易出现延时。本发明提出了基于卷积神经网络优化NMPC算法的非线性模型预测控制算法设计直接型USV航迹控制器，考虑到USV航迹控制中容易出现的非线性、时滞性问题，采用卷积神经网络算法对预测控制算法进行最优控制序列求解。仿真实验表明，该算法有效地提高了航迹控制的准确性和实时性。

Description

一种基于卷积神经网络优化NMPC算法的直接航迹控制方法

技术领域

本发明涉及一种基于卷积神经网络优化NMPC算法的直接航迹控制方法，属于 控制技术领域。

背景技术

USV(Unmanned Surface Vessel，简称USV)，是一种无人操作的水面船。它 具有机动性强、速度快、自主化、无人化等特点，其主要功能是代替人执行一些 特殊的、对人有危险的任务。USV在实际航行环境中会受到外部干扰的影响，精 准的航迹控制是USV执行各项任务的基础保障，也是USV运动控制领域的研究热 点。

USV航迹控制算法主要分为两类，一类是间接控制算法，其设计思路是将航 迹控制问题转化为航向控制问题，主要实现方式是将其分解为双闭环控制系统， 内环由传统的航向控制算法构成，外环则使用航向解算算法，优点是可直接使用 已有的自动舵技术，但控制精度相对较低。一类是直接控制算法，控制精度高， 控制算法直接作用于航迹，但由于过程非线性，计算复杂，容易出现时滞性问题。

发明内容

本发明所要解决的技术问题是提供一种基于卷积神经网络优化NMPC算法的直 接航迹控制方法。

本发明的技术方案如下：

采用基于非线性模型预测控制(NMPC)算法的航迹控制器，根据预设航迹信 息以及USV的初始状态信息，直接控制USV的航迹；

航迹控制器的输出量为USV的舵角控制量以及航速控制量，航迹控制器收到 USV反馈的状态量，包括USV的位置坐标信息和以及航向角信息，对输出的控制 量进行自我校正；所述航迹控制器原理模型主要包括USV预测模型、目标函数以 及约束信息三个方面，所解决的USV非线性航迹控制问题用以下有限时域内的最 优化问题来表示：

s.t.χ_k+1,t＝f(χ_k,t,u_k.t),k＝t,…H_p-1

u_min≤u(k)≤u_max

Δu_min≤Δu(k)≤Δu_max

其中，状态量的矩阵形式为χ＝[x y ψ]^T、控制量的矩阵形式为u＝[u δ]^T，

x,y为USV重心在惯性坐标系的空间坐标；ψ为USV在惯性坐标系的方位角， u为USV速度的纵向分量，v为USV速度的横向分量，δ为舵角；

χ(t+1)＝f(χ(t),u(t))的差分方程为：

目标函数

其中，Hp是MPC航迹控制器的预测时域，H_c是MPC航迹控制器的控制时 域，ρ为权重系数，ε为松弛因子。

根据USV本身的控制特点，在输出控制量的过程中要考虑舵角约束以及航速 约束。本发明中使用的USV舵角以及舵角控制量增量约束为：

本发明中使用的USV航速以及航速增量约束为：

进一步地，所述航迹控制器对预测输出值进行自我校正时，采用卷积神经网络 算法进行局部寻优，获得下一时刻的USV舵角控制量以及航速控制量信息。

具体求解步骤如下：

(1)初始化参数：初始温度T0，降温速率q，结束温度Tend，对应于每个温度 T的迭代次数L，以当前时刻的控制量矩阵作为初始可行解矩阵u₀；

(2)对当前温度T，求解局部最优解，即重复步骤(3)至(6)；

(3)对当前可行解矩阵u₀在控制量约束范围内随机产生一个新解u₁；

(4)对应于公式(11)所示的目标函数J，求解其对应于可行解矩阵u₀和u₁的增 量dJ＝dJ(u₁)-dJ(u₀)；

(5)根据判断条件确定当前局部最优解，即若dJ＜0，则认为新解u₁为最优 解，用u₁替代u₀；否则，计算对于新解的接受概率exp(-df/T)，当满足接受概 率exp(-df/T)＞rand(rand为(0,1)区间上的随机数)时，也认为新解u₁为最优 解，用u₁替代u₀；否则，仍认为u₀为最优解。

本发明采用非线性模型预测控制(NMPC)算法对水面USV进行直接航迹控 制，采用卷积神经网络算法对预测控制算法进行最优控制序列求解，进一步提高 航迹控制的准确性和实时性。模型预测控制采用反馈校正，可应对多种不确定性 问题(包括系统干扰、模型失配等)，并通过滚动优化策略在线求解局部最优化问 题，极大地减少了在线优化计算量，提高算法的响应速度。

附图说明

图1，USV坐标系。

图2，航迹跟踪效果(无干扰)。

图3，状态量误差变化曲线(无干扰)。

图4，控制量变化曲线(无干扰)。

图5，曲线航迹跟踪效果(有干扰)。

图6，状态量误差变化曲线(有干扰)。

图7，控制量变化曲线(有干扰)。

图8，扰动信号曲线图。

具体实施方式

本发明提供一种基于卷积神经网络优化NMPC算法的直接航迹控制方法。

本发明方法中，基于NMPC的航迹控制器根据预设航迹信息以及USV的初始 状态信息，直接控制USV的航迹，航迹控制器的输出量为USV的舵角控制量以及 航速控制量。

具体地，航迹控制器首先对预设航迹信息进行离散化处理，获得离散化后的第 一个目标航迹点信息，根据此目标航迹点信息，计算USV舵角控制量以及所需航 速控制量，进行航迹跟踪，获取离散化后的航迹信息上的下一个航迹点，重复以 上动作，直到离散化后的航迹信息上的最后一个点。

航迹跟踪过程中，航迹控制器收到USV的状态量反馈信号，包括USV的位置 坐标信息和以及航向角信息，对预测输出值进行自我校正。所述航迹控制器原理模 型包括USV预测模型、目标函数以及约束信息三个方面。

USV预测模型

一般来说，USV的运动情况需要使用六个自由度来描述，包括横摇、纵摇、 艏摇、横荡、纵荡、垂荡。为描述USV在六自由度中的运动情况，需建立如图1 所示的USV坐标系，包括固定坐标系o-x₁y₁z₁(简称“定系”)和运动坐标系 G-xyz(简称“动系”)。“定系”是指固定于地球上的惯性坐标系。“动系”是指 跟随船体运动的附体坐标系，原点设在USV的船体重心G处，Gx为USV水平面 方向，Gy为USV横剖面方向，Gz为USV的纵中剖面方向。正方向按右手系的规 定。

USV在附体坐标系G-xyz和惯性坐标系o-x₁y₁z₁中涉及到的运动参数如表 1所示。其中X，Y，Z为USV所受外力在附体坐标系G-xyz上的投影；K，M， N为USV力矩在附体坐标系G-xyz上的投影；u，v，w为USV的速度在附体 坐标系G-xyz上的投影；p，q，r为USV的角速度在附体坐标系G-xyz上的 投影；(x,y,z)为USV重心在惯性坐标系o-x₁y₁z₁的空间坐标；φ，θ，ψ为USV 在惯性坐标系o-x₁y₁z₁中的三个欧拉角，φ称为横倾角、θ称为纵倾角、ψ称为方 位角。

表1 USV运动参数

自由度	力和力矩	速度和	位置和
				纵荡	X	u	x
横荡	Y	v	y
				垂荡	Z	w	z
横摇	K	p	φ
				纵摇	M	q	θ
艏摇	N	r	ψ

船舶在海洋上的航行情况既可以采用“动系”中的速度向量u,v,w和角速度向 量p,q,r来表示，也可以使用定系中位置向量的导数

及欧拉角姿态向量的导 数

来表示。研究者可根据自身课题研究需要确定表示方式。另外，船舶中 的“动系”和“定系”之间还存在如下变换关系：

a)位置向量的导数

与速度向量u,v,w之间的坐标变换公式：

其中，变换矩阵T_V为：

本发明的USV的直接航迹控制器重点关注USV在水平面的运动，暂且忽略橫倾、 纵倾和升沉对USV运动的影响。因此将USV的六自由度模型的(1)和公式(2)进行 化简，得到USV的三自由度模型为:

其中，

为USV位置信息的横坐标的导数，

为USV位置信息的纵坐标的导 数，

为USV艏向角的导数，ψ为USV的艏向角，u为USV速度的纵向分量，v 为USV速度的横向分量，r为USV的角速度。

水面USV航行过程中，当满足u＞＞0且v≈0时，可忽略横漂角对USV运动 的影响，此时方程(3)可进一步简化为：

又已知USV角速度r和舵角δ之间的关系可用方程式(5)表示。方程式(5)中， K表示USV的旋回性指数，T表示USV的追随性指数。K、T的值可由USV的 Z形操纵性试验获得。

将公式(5)带入公式(4)中，并以采样时间T_s进行离散化处理，可得差分方程为：

在水面USV的航迹控制中，若令状态量的矩阵形式为χ＝[x y ψ]^T、控制量的矩 阵形式为u＝[u δ]^T，根据公式(6)表示的USV输入状态量与输出控制量之间的关系， 可以得出USV的非线性离散模型的一般表示为：

χ(t+1)＝f(χ(t),u(t)) (7)

目标函数

基于非线性模型预测控制的航迹控制器中，目标函数的选取要能够充分反映 USV航迹跟踪参考航迹的程度。本发明结合USV航迹控制的预测模型，设计目标 函数如下：

其中，Hp是MPC航迹控制器的预测时域，H_c是MPC航迹控制器的控制时 域，ρ为权重系数，ε为松弛因子。

该目标函数既考虑了USV对预设航迹的跟踪能力，也考虑了控制量增量的约 束，有效的避免了控制量的突变。另外，在目标函数中加入了松弛因子，可以防止 出现没有可行解的情况。

约束条件

根据USV本身的控制特点，在输出控制量的过程中要考虑舵角约束以及航速 约束。本发明中使用的USV模型舵角以及舵角控制量增量约束为：

本发明中使用的USV航速以及航速增量约束为：

综上所述，USV非线性航迹控制问题可以用以下有限时域内的最优化问题来 表示：

算法流程与步骤

优化算法的实时性以及准确性直接影响所设计的航迹控制器的控制效果，由于非线性目标函数较难求得解析解，此处针对公式(11)所示的局部最优化问题，采用 卷积神经网络算法进行最优控制序列求解，具体求解步骤如下：

(1)初始化参数：初始温度T0，降温速率q，结束温度Tend，对应于每个温度 T的迭代次数L，以当前时刻的控制量矩阵作为初始可行解矩阵u₀；

(2)对当前温度T，求解局部最优解，即重复步骤(3)至(6)；

(3)对当前可行解矩阵u₀在控制量约束范围内随机产生一个新解u₁；

(4)对应于公式(11)所示的目标函数J，求解其对应于可行解矩阵u₀和u₁的增 量dJ＝dJ(u₁)-dJ(u₀)；

(5)根据判断条件确定当前局部最优解，即若dJ＜0，则认为新解u₁为最优 解，用u₁替代u₀；否则，计算对于新解的接受概率exp(-df/T)，当满足接受概 率exp(-df/T)＞rand(rand为(0,1)区间上的随机数)时，也认为新解u₁为最优 解，用u₁替代u₀；否则，仍认为u₀为最优解。

设置终止条件，使得温度T衰减至结束温度Tend时，输出当前的最优解u₀， 不满足终止条件时，对当前温度T按降温速率进行衰减运算后，重复执行步骤2。

仿真试验

本发明中的实验船的基本参数如表2所示。

表2 USV主要参数

在公式(6)中USV的旋回性指数K和USV的追随性指数T可以通过对实验船进 行10°/10°的Z形操纵性试验来获得。在平静的湖面进行了实验船的Z形(10°/10°) 试验，获得的实验船艏向角和舵角关系变化曲线，对USV试验数据进行拟合，求 得实验船的K、T参数的值为：K＝0.49，T＝1.94。

针对表2提出的试验船参数，对设计的基于卷积神经网络算法改进的NMPC 的航迹控制器进行航迹控制效果仿真。各项参数设置为：

(1)USV初始状态：位置坐标(0,-22)，航向角ψ＝0°，航速v＝0m/s。

(2)预设曲线航迹信息：原点为(0,0)、半径为20米的圆弧轨迹。期望跟踪速 度v_d＝3m/s。

(3)设置采样时间为50ms。

(4)控制器参数设置：预测时域H_p＝10，控制时域H_c＝2，权重矩阵

卷积神经网络优化控制器参数：初始温度T₀＝200，终止温度T_end＝e^-3，温度下降速率q＝0.9，对应于每个温度处的迭代次数 L＝200。

仿真试验分别在无干扰和有干扰情况下进行仿真验证。

无干扰条件下，对于预设圆弧形航迹，航迹跟踪仿真实验获得的结果如图2 至4所示。

由航迹跟踪效果图和状态量误差变化曲线可以看出，对于设定的圆形航迹， 基于NMPC的直接航迹控制器可以根据当前时刻USV的状态信息，调整USV的 舵角控制量和航速控制量，快速地跟踪到设定航迹，且最终的跟踪误差为零。由 控制量变化曲线可以看出，在USV由接收到设定圆形航迹信息时刻的位置进行航 迹跟踪时，舵角控制量以及航速控制量会立刻做出响应，直到USV跟踪到预设航 迹上，航速稳定在期望航速上，舵角控制量以及航速控制量保持不变，实现预设 圆形航迹的精确跟踪控制。

对所设计的航迹控制器添加对应于USV实时航速的的干扰信号，对于有干扰 情况，外部干扰信号设置为USV航速的25％，干扰信号对USV的航迹控制系统产 生的影响可用公式(12)表示：

其中，T_s为采样时间。此处取T_s＝1。

获得的航迹控制曲线以及过程中的干扰信号曲线如5到8所示。

在有干扰的情况下，由航迹跟踪效果图可以看出，在USV接收到预设航迹信 息初期，航迹跟踪效果较差，但是基于NMPC先预测后控制的思想，一段时间后 基于NMPC-SA的航迹控制器对干扰信号进行了预测补偿，控制效果慢慢变好，到 后期基本上能通过舵角补偿量实现对预设圆形航迹的精确跟踪控制。

针对传统的粒子群算法容易陷入局部最优解的问题，本发明利用卷积神经网络算法的突跳特性提出了一种改进算法，通过加入多目标优化，提升了算法的实用性。 仿真结果显示，改进算法可以优化USV自主航行的路径平滑度，提高路径的安全 性，满足USV全局路径规划的要求。针对USV航迹控制中容易出现的非线性、时 滞性问题，提出了基于非线性模型预测控制(NMPC)算法对水面USV进行直接 航迹控制，并采用卷积神经网络算法对预测控制算法进行最优控制序列求解，提升 了算法的实用性。仿真结果显示，改进算法有效地提高了航迹控制的准确性和实时 性。

Claims (4)

1.一种基于卷积神经网络优化NMPC算法的直接航迹控制方法，其特征在于：

采用基于非线性模型预测控制算法的航迹控制器，根据预设航迹信息以及USV的初始状态信息，直接控制USV的航迹；

航迹控制器的输出量为USV的舵角控制量以及航速控制量，航迹控制器收到USV反馈的状态量，包括USV的位置坐标信息和以及航向角信息，对输出的控制量进行自我校正；所述航迹控制器原理模型包括USV预测模型、目标函数以及约束信息三个方面，所解决的USV非线性航迹控制问题用以下有限时域内的最优化问题来表示：

s.t.χ_k+1,t＝f(χ_k,t,u_k.t),k＝t,…H_p-1

u_min≤u(k)≤u_max

Δu_min≤Δu(k)≤Δu_max

其中，状态量的矩阵形式为χ＝[x y ψ]^T、控制量的矩阵形式为u＝[u δ]^T，

x,y为USV重心在惯性坐标系的空间坐标；ψ为USV在惯性坐标系的方位角，

u为USV速度的纵向分量，v为USV速度的横向分量，δ为舵角；

χ(t+1)＝f(χ(t),u(t))的差分方程为：

目标函数

其中，Hp是MPC航迹控制器的预测时域，H_c是MPC航迹控制器的控制时域，ρ为权重系数，ε为松弛因子。

2.根据权利要求1所述的控制方法，其特征在于，USV舵角以及舵角控制量增量约束为：

3.根据权利要求1所述的控制方法，其特征在于，USV航速以及航速增量约束为：

4.根据权利要求1所述的控制方法，其特征在于，采用卷积神经网络算法进行局部寻优，获得下一时刻的USV舵角控制量以及航速控制量信息，具体求解步骤如下：

(1)初始化参数：初始温度T0，降温速率q，结束温度Tend，对应于每个温度T的迭代次数L，以当前时刻的控制量矩阵作为初始可行解矩阵u₀；

(2)对当前温度T，求解局部最优解，即重复步骤(3)至(6)；

(3)对当前可行解矩阵u₀在控制量约束范围内随机产生一个新解u₁；

(4)对应于公式(11)所示的目标函数J，求解其对应于可行解矩阵u₀和u₁的增量dJ＝dJ(u₁)-dJ(u₀)；

(5)根据判断条件确定当前局部最优解，即若dJ＜0，则认为新解u₁为最优解，用u₁替代u₀；否则，计算对于新解的接受概率exp(-df/T)，当满足接受概率exp(-df/T)＞rand(rand为(0,1)区间上的随机数)时，也认为新解u₁为最优解，用u₁替代u₀；否则，仍认为u₀为最优解。

Images