CN112068440A - 基于模型预测控制的auv回收对接动力定位控制方法 - Google Patents

Abstract

本发明公开了一种基于模型预测控制的AUV回收对接动力定位控制方法，即针对AUV回收对接过程中的动力定位控制，通过自适应无迹卡尔曼滤波算法完成状态估计，并将状态估计的结果与期望状态对比完成反馈，将AUV复杂的非线性系统中线性部分进行线性处理，同时利用RBF神经网络对非线性函数的逼近能力，根据其非线性部分通过带自调整功能的RBF神经网络补偿器产生补偿量，应用模型预测控制器实现动力定位系统的闭环控制。本发明通过神经网络与模型预测控制结合解决了因AUV动力定位过程的模型不准确造成的影响，提高其动力定位控制精度及稳定性。

Description

基于模型预测控制的AUV回收对接动力定位控制方法

技术领域

本发明涉及一种回收对接过程的动力定位控制方法，尤其涉及一种基于RBF(径向基)神经网络补偿器的模型预测动力定位控制方法。属于机器人技术领域。

背景技术

无人水下机器人按作业方式不同可以分为有缆遥控水下机器人(RemoteOperated Vehicle，ROV)和自主水下机器人(Autonomous Underwater Vehicle，AUV)两类。ROV依靠脐带缆进行供能，由于脐带缆限制必须在母船附近运动。AUV与母船无连接，使用自身携带的电池组提供能源，具有下潜深度深、活动范围广、隐蔽性高的优点，但是由于自身能源有限，在水下工作一段时间后AUV必须考虑如何回收和布放。目前，AUV在海上主要通过船用吊臂进行水面回收和布放，该种方式步骤繁琐，如果AUV无法一次性完成任务就需要频繁的起吊，这无疑降低了AUV的使用效率。AUV水下回收要求AUV在水下完成与回收装置的自主对接，摆脱了人为干预的繁琐过程，提高了作业效率和隐蔽性，是AUV相关技术的研究热点。

AUV水下回收会面临诸多干扰，复杂的外部环境，不确定海流与波浪扰动，还包括自身定位系统的测量噪声。由于水下主要的定位措施为航位推算，其误差会随时间累计，为了获取精确的位置信息在回收时会使用水下声光传感器对定位信息进行校准，但是，声学信号存在时滞性，光学信号传播距离短且角度受限，常常会丢失定位信标导致AUV水下定位存在很大的困难。

为了提高水下回收的适应性，不仅需要AUV拥有更高的机动性以应对各种突发状况，而且也要求AUV具备抵抗环境扰动的动力定位能力。AUV动力定位是一种微定位方法，通过滤除AUV运动中的高频成份，使用辅助推进器对低频干扰进行补偿，使AUV能够更好的通过控制系统保持目标位置和轨迹，具有稳定性高、机动性强的特点。AUV水下回收受海流影响很大，尤其当存在侧向海流时，AUV水下回收的成功率会大大降低。进一步提高AUV抵抗环境干扰的动力定位能力，对使AUV更加灵活、安全、可靠具有重要意义。

动力定位常用于AUV或者船舶系统上，而AUV和船舶系统都是复杂的非线性系统，若以线性模型进行动力定位控制，则难以保证在实际应用中动力定位的控制精度。申请号为“201610457249.4”的专利文献公开了“一种基于运动线性模型和区域性能指标的预测控制方法”，其所采用的线性化假定作为预测控制算法的条件，难以在AUV和船舶系统等复杂非线性系统中进行实际的应用，可能因模型不准确导致动力定位精度不高及稳定性不足的问题，影响动力定位效果。

发明内容

本发明的目的在于提供一种基于模型预测控制的AUV回收对接动力定位控制方法，针对因模型不准确对动力定位控制的影响，采用自适应无迹卡尔曼滤波算法完成状态估计，并将状态估计的结果与期望状态对比完成反馈，将AUV复杂的非线性系统中线性部分进行线性处理，同时利用RBF神经网络对非线性函数的逼近能力，根据其非线性部分通过带自调整功能的RBF神经网络补偿器产生补偿量，应用模型预测控制器实现动力定位系统的闭环控制，减小模型不准确对预测控制效果的影响，提高AUV回收对接过程中动力定位控制的精度与稳定性。

本发明的目的通过以下技术方案予以实现：

一种基于模型预测控制的AUV回收对接动力定位控制方法,其特征在于，该方法包含下列步骤：

1)初始化：建立大地坐标系与运动坐标系，构建AUV六自由度空间运动方程，AUV根据当前回收对接任务指令设定深度、姿态角、轨迹及速度期望值信息，进而计算与实际深度、姿态角、轨迹及速度之间误差；

2)状态估计：利用AUV的传感器所提供的深度、姿态、轨迹及速度信息并根据状态估计算法对AUV的实际状态进行估计；

3)线性处理：根据实际状态估计值与设定状态期望值的误差，将其线性部分输出至模型预测控制器；

4)设计RBF神经网络补偿器：计算实际状态估计值与设定状态期望值误差中的非线性部分并利用带自调整功能的RBF神经网络补偿器产生补偿量；

5)设计模型预测控制器：以线性处理和RBF神经网络补偿器的补偿量作为输入量利用模型预测控制器产生控制量，依据模型预测控制控制量输出进行推力分配完成AUV回收对接动力定位控制。

进一步地，步骤1)所述初始化为：建立大地坐标系(E-ξηζ)和运动坐标系(O-xyz)，大地坐标系以水平面一点E为原点，ξ轴指向地理北向、η轴指向地理东向，ζ轴指向地心；运动坐标系的原点O在AUV上，x、y、z三轴分别指向AUV的艏向、右舷和底部。构建AUV六自由度空间运动方程，AUV根据当前回收对接任务指令设定深度、姿态角、轨迹及速度期望值信息，通过与实际状态估计反馈信息进行比较，进而计算与实际深度、姿态角、轨迹及速度之间误差，AUV六自由度空间运动方程如下式所示；

式中：X、Y、Z分别为AUV在运动坐标系x、y、z轴所受到的力；K、M、N分别为AUV在运动坐标系x、y、z轴所受到的转矩；m为物体质量；I_x,I_y,I_z是转动惯量；u,v,w分别为AUV在运动坐标系下x、y、z轴的速度；p,q,r分别为AUV在运动坐标系下x、y、z轴的角速度；x_g、y_g、z_g分别为x、y、z轴重心坐标。

进一步地，步骤2)具体步骤包括：

步骤(2.1)：状态估计初始化：给定系统状态X₀和方差的初始值P₀，并将AUV六自由度运动和传感器测量方程简化：

式中：f(·)是AUV六自由度方程；h(·)是传感器测量方程；x是AUV当前的状态；

是x的一阶导数；ang＝[φ,θ,ψ]^T是AUV姿态角；φ、θ、ψ分别为横滚角、纵倾角、航向角；

为估计的推进器系统的推力；y为传感器测量值；

将上式离散化为：

式中：F(·)＝1+Δt·f(·)为离散后的AUV六自由度近似方程；Δt为采样时间；下标k、k-1分别表示为k、k-1时刻的状态；

步骤(2.2)：利用UKF(无迹卡尔曼滤波)算法估计当前系统状态，其中采样点及其对应权值的计算为：

其中，λ为尺度参数，χ_0,k-1表示初始采样点，χ_j,k-1表示第j个采样点；

和

分别为初始采样点均值和方差的权值，

和

分别对应第j个采样点均值和方差的权值，α表示采样点围绕均值扩散程度，为非负的权系数；β为运动变量；

表示k-1时刻估计误差协方差；

步骤(2.3):引入自适应缩放因子Δ_k，针对在AUV面对不同环境自适应调整其观测权值，减少环境对状态估计造成的影响,UKF先验均方差公式为：

其中

为不包含过程噪声的先验均方差；Q_k为过程噪声的方差；P_k'为引入自适应因子的先验均方差；通过引入Δ_k对于不同程度的噪声取得不同的权值，提高自适应性。

步骤(2.4):时间更新:将时间t更新为t＝t+1。

步骤(2.5):根据状态是否满足动力定位要求判断任务是否结束，若没结束则根据当前状态，以模型预测控制器控制量输出完成推力分配，并根据步骤(2.2)继续执行状态估计。

进一步地，步骤3)具体步骤包括：

针对AUV系统为复杂的非线性系统，将其非线性系统分解为非线性部分与线性部分叠加，其中线性部分作为单独部分输出至控制器。

进一步地，步骤4)具体步骤包括：根据步骤3)将误差部分的线性部分处理之后，利用RBF神经网络对非线性函数的逼近能力对误差的非线性部分产生补偿量，其中RBF神经网络选取Gaussian函数作为径向基函数：

其中x₀为基函数中心，x为输入向量，||x-x₀||为x与x₀之间的径向距离；σ为基函数的宽度，且σ＞0，σ越小代表函数的选择性越强。Gaussian函数具有易于表达、径向对称、便于解析的优点。

隐含层输出向量Φ定义为：

Φ＝[φ₁,…,φ_i,…φ_N]^T (7)

其中：

上式中，φ_i(·)为第i个隐含节点的输出，N为隐含节点个数，c_i为第i个隐含节点的中心，σ_i为第i个隐含节点的基函数宽度，且σ_i＞0；

RBF神经网络输出层包括若干个线性单元，设从第i个隐含节点到第j个输出节点权值为w_ij，则第j个输出节点的输出为：

通过RBF神经网络对非线性部分进行补偿，并输出线性控制量至控制器。

进一步地，步骤5)具体步骤包括：以步骤3)、步骤4)结果作为模型预测控制器控制量输入，以模型预测控制器控制量输出完成推力分配。

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

1.本发明采用的RBF神经网络补偿器能够对复杂的非线性函数有更好的逼近能力，从而减少模型不准确带来的影响。

2.本发明使用的自适应无迹卡尔曼滤波能够对因AUV面对的干扰对状态估计的不准确性进行改善，使得状态估计的结果更为准确，提高状态估计精度。

3.本发明将线性部分与非线性部分单独处理，能够提高估计状态与实际状态的准确性，从而提升动力定位控制精度。

4.将模型预测控制与RBF神经网络补偿器结合，能以系统和直观的方式处理多变量约束系统在线优化控制，具有良好的控制性能和较强的鲁棒性。

5.以模型预测控制算法作为控制器能够提高算法执行效率，更易于工程应用。

附图说明

图1是本发明动力定位控制结构框图；

图2是本发明状态估计算法流程图；

图3是本发明RBF神经网络结构图。

具体实施方式

下面结合附图和具体实施例对本发明作进一步说明。

如图1所示：本发明基于模型预测控制的AUV回收对接动力定位控制方法共有设定状态期望值、状态估计、线性处理、RBF神经网络补偿器、模型预测控制器设计、推力分配几个过程。其中，以状态期望值输入作为整个动力定位控制系统的输入量，以AUV系统状态估计作为反馈，将状态估计结果与期望值对比完成误差计算，并将其结果进行线性处理以及进行RBF补偿器设计后输入至模型预测控制器，最终计算出控制量并完成推力分配。

RBF神经网络补偿器模型预测动力定位控制系统包括以下几个步骤：

步骤1：所述初始化为：建立大地坐标系(E-ξηζ)和运动坐标系(O-xyz)，大地坐标系以水平面一点E为原点，ξ轴指向地理北向、η轴指向地理东向，ζ轴指向地心；运动坐标系的原点O在AUV上，x、y、z三轴分别指向AUV的艏向、右舷和底部。构建AUV六自由度空间运动方程，AUV根据当前回收对接任务指令设定深度、姿态角、轨迹及速度期望值信息，通过与实际状态估计反馈信息进行比较，进而计算与实际深度、姿态角、轨迹及速度之间误差，AUV六自由度空间运动方程如下式所示；

式中：X、Y、Z分别为AUV在运动坐标系x、y、z轴所受到的力；K、M、N分别为AUV在运动坐标系x、y、z轴所受到的转矩；m为物体质量；I_x,I_y,I_z是转动惯量；u,v,w分别为AUV在运动坐标系下x、y、z轴的速度；p,q,r分别为AUV在运动坐标系下x、y、z轴的角速度；x_g、y_g、z_g分别为x、y、z轴重心坐标。

进一步地，步骤2)如图2状态估计算法流程图所示，具体步骤包括：

步骤(2.1)：状态估计初始化：给定系统状态X₀和方差的初始值P₀，并将AUV六自由度运动和传感器测量方程简化：

式中：f(·)是AUV六自由度方程；h(·)是传感器测量方程；x是AUV当前的状态；

是x的一阶导数；ang＝[φ,θ,ψ]^T是AUV姿态角；φ、θ、ψ分别为横滚角、纵倾角、航向角；

为估计的推进器系统的推力；y为传感器测量值；

将上式离散化为：

式中：F(·)＝1+Δt·f(·)为离散后的AUV六自由度近似方程；Δt为采样时间；下标k、k-1分别表示为k、k-1时刻的状态；

步骤(2.2)：利用UKF(无迹卡尔曼滤波)算法估计当前系统状态，其中采样点及其对应权值的计算为：

其中，λ为尺度参数，χ_0,k-1表示初始采样点，χ_j,k-1表示第j个采样点；

和

分别为初始采样点均值和方差的权值，

和

分别对应第j个采样点均值和方差的权值，α表示采样点围绕均值扩散程度，为非负的权系数；β为运动变量；

表示k-1时刻估计误差协方差；

步骤(2.3):引入自适应缩放因子Δ_k，针对在AUV面对不同环境自适应调整其观测权值，减少环境对状态估计造成的影响,UKF先验均方差公式为：

其中

为不包含过程噪声的先验均方差；Q_k为过程噪声的方差；P_k'为引入自适应因子的先验均方差；通过引入Δ_k对于不同程度的噪声取得不同的权值，提高自适应性。

步骤(2.4):时间更新:将时间t更新为t＝t+1。

步骤(2.5):根据状态是否满足动力定位要求判断任务是否结束，若没结束则根据当前状态，以模型预测控制器控制量输出完成推力分配，并根据步骤(2.2)继续执行状态估计。

进一步地，步骤3)具体步骤包括：

针对AUV系统为复杂的非线性系统，将其非线性系统分解为非线性部分与线性部分叠加，其中线性部分作为单独部分输出至控制器。

进一步地，步骤4)具体步骤包括：根据步骤3)将误差部分的线性部分处理之后，利用RBF神经网络对非线性函数的逼近能力对误差的非线性部分产生补偿量，RBF神经网络通常由三层组成，分别是输入层、隐含层和输出层，如图3所示为RBF神经网络结构图，其中RBF神经网络选取Gaussian函数作为径向基函数：

其中x₀为基函数中心，x为输入向量，||x-x₀||为x与x₀之间的径向距离；σ为基函数的宽度，且σ＞0，σ越小代表函数的选择性越强。Gaussian函数具有易于表达、径向对称、便于解析的优点。

隐含层输出向量Φ定义为：

Φ＝[φ₁,…,φ_i,…φ_N]^T (7)

其中：

上式中，φ_i(·)为第i个隐含节点的输出，N为隐含节点个数，c_i为第i个隐含节点的中心，σ_i为第i个隐含节点的基函数宽度，且σ_i＞0；

RBF神经网络输出层包括若干个线性单元，设从第i个隐含节点到第j个输出节点权值为w_ij，则第j个输出节点的输出为：

通过RBF神经网络对非线性部分进行补偿，并输出线性控制量至控制器。

步骤5)具体步骤包括：以步骤3)、步骤4)结果作为模型预测控制器控制量输入，以模型预测控制器控制量输出完成推力分配。

除上述实施例外，本发明还可以有其他实施方式，凡采用等同替换或等效变换形成的技术方案，均落在本发明要求的保护范围内。

Claims (6)

1.一种基于模型预测控制的AUV回收对接动力定位控制方法,其特征在于，该方法包含下列步骤：

1)初始化：建立大地坐标系与运动坐标系，构建AUV六自由度空间运动方程，AUV根据当前回收对接任务指令设定深度、姿态角、轨迹及速度期望值信息，进而计算与实际深度、姿态角、轨迹及速度之间误差；

2)状态估计：利用AUV的传感器所提供的深度、姿态、轨迹及速度信息并根据状态估计算法对AUV的实际状态进行估计；

3)线性处理：根据实际状态估计值与设定状态期望值的误差，将其线性部分输出至模型预测控制器；

4)设计RBF神经网络补偿器：计算实际状态估计值与设定状态期望值误差中的非线性部分并利用带自调整功能的RBF神经网络补偿器产生补偿量；

5)设计模型预测控制器：以线性处理和RBF神经网络补偿器的补偿量作为输入量利用模型预测控制器产生控制量，依据模型预测控制控制量输出进行推力分配完成AUV回收对接动力定位控制。

2.如权利要求1所述的基于模型预测控制的AUV回收对接动力定位控制方法,其特征在于，步骤1)所述初始化为:建立大地坐标系E-ξηζ和运动坐标系O-xyz，大地坐标系以水平面一点E为原点，ξ轴指向地理北向、η轴指向地理东向，ζ轴指向地心；运动坐标系的原点O在AUV上，x、y、z三轴分别指向AUV的艏向、右舷和底部；构建AUV六自由度空间运动方程，AUV根据当前回收对接任务指令设定深度、姿态角、轨迹及速度期望值信息，通过与实际状态估计反馈信息进行比较，进而计算与实际深度、姿态角、轨迹及速度之间误差。

3.如权利要求1所述的基于模型预测控制的AUV回收对接动力定位控制方法,其特征在于，步骤2)状态估计包括以下步骤：

步骤(2.1)：状态估计初始化：给定系统状态X₀和方差的初始值P₀，并将AUV六自由度运动和传感器测量方程简化：

式中：f(·)是AUV六自由度方程；h(·)是传感器测量方程；x是AUV当前的状态；

是x的一阶导数；ang＝[φ,θ,ψ]^T是AUV姿态角；φ、θ、ψ分别为横滚角、纵倾角、航向角；

为估计的推进器系统的推力；y为传感器测量值；

将上式离散化为：

式中：F(·)＝1+Δt·f(·)为离散后的AUV六自由度近似方程；Δt为采样时间；下标k、k-1分别表示为k、k-1时刻的状态；

步骤(2.2)：利用UKF无迹卡尔曼滤波算法估计当前系统状态，其中采样点及其对应权值的计算为：

其中，λ为尺度参数，χ_0,k-1表示初始采样点，χ_j,k-1表示第j个采样点；

和

分别为初始采样点均值和方差的权值，

和

分别对应第j个采样点均值和方差的权值，α表示采样点围绕均值扩散程度，为非负的权系数；β为运动变量；

表示k-1时刻估计误差协方差；

步骤(2.3):引入自适应缩放因子Δ_k，针对在AUV面对不同环境自适应调整其观测权值，减少环境对状态估计造成的影响,UKF先验均方差公式为：

其中

为不包含过程噪声的先验均方差；Q_k为过程噪声的方差；P′_k为引入自适应因子的先验均方差；通过引入Δ_k对于不同程度的噪声取得不同的权值，提高自适应性；

步骤(2.4):时间更新:将时间t更新为t＝t+1；

步骤(2.5):根据状态是否满足动力定位要求判断任务是否结束，若没结束则根据当前状态，以模型预测控制器控制量输出完成推力分配，并根据步骤(2.2)继续执行状态估计。

4.如权利要求1所述的基于模型预测控制的AUV回收对接动力定位控制方法,其特征在于，步骤3)针对AUV系统为复杂的非线性系统，将其非线性系统分解为非线性部分与线性部分叠加，其中线性部分作为单独部分输出至控制器。

5.如权利要求1所述的基于模型预测控制的AUV回收对接动力定位控制方法,其特征在于，步骤4)根据步骤3)将误差部分的线性部分处理之后，利用RBF神经网络对非线性函数的逼近能力对误差的非线性部分产生补偿量，其中RBF神经网络选取Gaussian函数作为径向基函数：

其中x₀为基函数中心，x为输入向量，||x-x₀||为x与x₀之间的径向距离；σ为基函数的宽度，且σ＞0，σ越小代表函数的选择性越强。Gaussian函数具有易于表达、径向对称、便于解析的优点；

隐含层输出向量Φ定义为：

Φ＝[φ₁,…,φ_i,…φ_N]^T (6)

其中：

上式中，φ_i(·)为第i个隐含节点的输出，N为隐含节点个数，c_i为第i个隐含节点的中心，σ_i为第i个隐含节点的基函数宽度，且σ_i＞0；

RBF神经网络输出层包括若干个线性单元，设从第i个隐含节点到第j个输出节点权值为w_ij，则第j个输出节点的输出为：

通过RBF神经网络对非线性部分进行补偿，并输出线性控制量至控制器。

6.如权利要求1所述的基于模型预测控制的AUV回收对接动力定位控制方法,其特征在于，步骤5)以步骤3)、步骤4)结果作为模型预测控制器控制量输入，以模型预测控制器控制量输出完成推力分配。

Images