CN105867417B - 一种dvl测速失效时的uuv反步滑模动力定位控制方法 - Google Patents

Abstract

一种DVL测速失效时的UUV反步滑模动力定位控制方法，涉及一种UUV动力定位控制方法。为了解决在UUV反步滑模动力定位控制中测速传感器DVL测量失效的问题。包括：在DVL测速失效时，测量UUV北向、东向位置和艏向角，速度估计器在线估计出北向、东向速度和艏向角。根据北向、东向、艏向角测量值与期望值的误差和估计的北向、东向速度、艏向角，构造使UUV渐进稳定的反步滑模控制律，解算出UUV在纵荡、横荡和艏摇三个自由度上的控制向量。控制向量经过推力分配，得到UUV主推进器、水平辅助推进器的推力，从而使UUV达到期望的北向、东向位置和艏向角，实现在DVL测速失效时UUV在水平面内的反步滑模动力定位控制。

Description

一种DVL测速失效时的UUV反步滑模动力定位控制方法

技术领域

本发明涉及一种UUV动力定位控制方法，特别涉及一种DVL测速失效时的UUV反步滑模动力定位控制方法。

背景技术

UUV在执行水面定点拍照、水面区域值守、水面布放回收时的位置保持等任务时，需要通过安装的推进器完成位置和姿态的控制，实现动力定位。而UUV在水面的动力定位是具有一定非线性的特点，而且在水面也会受到风、浪、流干扰的影响，因此设计一个具有一定非线性、鲁棒性的控制器，对UUV水面动力定位的任务而言是十分必要的。

基于反步法设计的滑模动力定位控制方法，一方面可以保证系统的渐进稳定性，另一方面可以在UUV动力学模型非线性、不确定性的情况保证一定的控制精度。在CNKI中检索发现，西北工业大学高剑等在《机械科学与技术》(2006年第6期)发表的文章《基于自适应反演滑模控制的AUV水平面动力定位方法》针对AUV水平面动力定位问题，提出了自适应反演控制方法，提高了在海流扰动下控制系统的鲁棒性。但是，在UUV反步滑模动力定位控制方法中，通常需要UUV的速度信息。但是，工程应用速度传感器DVL时有失效现象，例如DVL设备软硬件故障、软质海底、海底深度超过速度传感器DVL的量程等，这将导致依赖于速度传感器DVL的反步滑模动力定位控制方法失效。

发明内容

本发明的目的是为了解决在UUV反步滑模动力定位控制中，测速传感器DVL测量失效的问题，本发明提供一种DVL测速失效时的UUV反步滑模动力定位控制方法。

本发明的一种DVL测速失效时的UUV反步滑模动力定位控制方法，所述方法包括如下步骤：

步骤1：UUV在水面进行纵荡、横荡和艏摇的三自由度反步滑模动力定位控制，获得期望向量η_d＝[x_d y_d ψ_d]^T，x_d、y_d和ψ_d分别表示UUV在大地坐标下的北向位置、东向位置和艏向角的期望值；

步骤2：对UUV的经纬度和艏向角进行采集，同时采用速度传感器DVL对UUV进行速度采集，确定向量η＝[x y ψ]^T，x、y和ψ分别表示采集的UUV在大地坐标下的北向位置、东向位置和艏向角的值；

步骤3：故障检测模块实时检测，当检测到测速无效时，产生触发信息启动速度估计器对UUV的速度信息进行估计，速度估计器是以UUV的水平面三自由度动力学模型为基础，以采集的北向位置x、东向位置y和艏向角ψ作为速度估计器的输入，并且引入了北向位置、东向位置和艏向角估计误差的滑模面函数以消除速度估计器模型的不确定性：

其中，为UUV在大地坐标系下的北向位置、东向位置和艏向角的估计向量，J∈R^3×3是大地坐标系与UUV坐标系之间的变换矩阵，为UUV坐标下的北向速度、东向速度和艏向角速度估计向量，sat(*)为饱和函数，为速度估计器的滑模面函数，φ₁为边界层厚度；M∈R^3×3为UUV的惯性质量矩阵；C∈R^3×3为UUV离心力和哥式力矩阵；D∈R^3×3为粘性矩阵，为外界环境的扰动估计向量，τ∈R^3×1为控制力向量；L₁、L₂、L₃∈R^3×3为速度估计器增益对角矩阵；

饱和函数sat(*)具体形式如下：

其中，φ₁为正常数，表示边界层的厚度，i＝1,2,3；

步骤4：根据期望向量η_d与向量获得偏差向量反步滑模动力定位控制器再根据向量和偏差向量η_e，获得控制力向量τ；

步骤5：根据获得的控制力向量τ进行推力分配，使UUV的主推进器和水平辅助推进器在分配得到的推力下工作，从而使UUV北向位置、东向位置和艏向角达到期望值。

所述步骤4中，控制力向量τ：

误差向量中间虚拟镇定向量α₁＝c₁z₁，c₁为α₁的增益矩阵，表示第一个误差向量的比例系数，误差向量控制器的滑模面函数s₂＝Kz₁+z₂，K为滑模面函数s₂中误差向量z1与误差向量z2的比例系数；h为控制器滑模面函数s₂的增益矩阵，κ为滑模面饱和切换函数增益矩阵，φ₁、φ₂为边界层厚度。

所述步骤5中，根据获得的控制力向量τ进行推力分配：

F_ml＝0.5(τ_x+βτ_N/L_m),-γ₁F_1max≤F_ml≤γ₁F_1max (8)

F_mr＝0.5(τ_x-βτ_N/L_m),-γ₁F_1max≤F_mr≤γ₁F_1max (9)

F_bh＝0.5(τ_y-(1-β)τ_N/L_h),-γ₂F_2max≤F_bh≤γ₂F_2max (10)

F_sh＝0.5(τ_y+(1-β)τ_N/L_h),-γ₂F_2max≤F_sh≤γ₂F_2max (11)

其中，F_ml、F_mr、F_bh、F_sh分别表示左主推进器、右主推进器、艏水平辅助推进器、艉水平辅助推进器分配的推力；Lm、Lh分别表示左主推进器和右主推进器间距、艏水平辅助推进器和艉水平辅助推进器间距；γ₁、γ₂分别为主推进器、水平辅助推进器限额比，F_1max、F_2max分别为主推进器、水平辅助推进器能够提供的最大推力，β表示推力分配系数；β＝0时完全由水平辅助推进器来提供转艏力矩，β＝1时完全由主推进器来提供转艏力矩。

所述步骤2中，采用位置测量传感器GPS和姿态传感器罗经对UUV的经纬度和艏向角进行采集。

本发明的有益效果在于，可以在速度测量传感器DVL测量失效时，利用速度估计器估计出UUV较精确的速度信息。反步滑模控制器可以提高UUV动力定位的控制精度，并且在一定的外界环境的干扰下具有一定的鲁棒性，满足作业任务动力定位控制的需求。实用性强，在工程实现中简单易用，可以根据UUV推进器的健康状态调整推力分配系数，控制主推占转艏力矩的比例；采用基于速度估计器的反步滑模动力定位控制方法解决了DVL测速失效时反步滑模控制律中速度信息缺失的问题。

可以在速度测量传感器DVL测量失效时，利用速度估计器估计出UUV较精确的速度信息，为UUV反步滑模控制器提供速度输入，使UUV能在一定的外界环境干扰下实现动力定位功能，且具有一定的鲁棒性。另外，本发明可以根据UUV推进器的工作于健康状态调整推力分配系数，调整主推进器占转艏力矩的比例，实用性强，在工程实现中简单易用。

附图说明

图1为具体实施方式中的一种DVL测速失效时的UUV反步滑模动力定位控制方法的原理示意图；

图2为UUV推进器配置示意图；

图3为北向位置的测量值及北向位置、速度估计值的对比图；

图4为东向位置的测量值及东向位置、速度估计值的对比图；

图5为艏向角的测量值及艏向角、角速度的估计值的对比图；

图6为推力分配完成后左、右主推进器的推力变化图；

图7为推力分配完成后艏、艉水平辅助推进器的推力变化图。

具体实施方式

结合图1至图7说明本实施方式，本实施方式所述的一种DVL测速失效时的UUV反步滑模动力定位控制方法，所述方法包括如下步骤：

步骤1：UUV6在水面进行纵荡、横荡和艏摇的三自由度反步滑模动力定位控制，首先初始化北东坐标系的原点，并且开始启动位置测量传感器GPS12、姿态传感器罗经11和速度传感器DVL9，位置测量传感器GPS12获得UUV的经纬度；姿态传感器罗经11获得UUV6的艏向角ψ；速度传感器DVL9获得UUV的速度；

步骤2：位置测量传感器GPS12采集到的UUV6的经纬度信息lon和lat，经过坐标变换10，获得UUV6在大地坐标下的北向位置x和东向位置y；姿态传感器罗经11获得UUV6的艏向角ψ；向量η＝[x y ψ]^T，x、y和ψ分别表示采集的UUV6在大地坐标下的北向位置、东向位置和艏向角的值；速度传感器DVL9获得UUV6的速度；

步骤3：DVL故障检测模块8实时检测，当检测到速度传感器DVL9测速失效的状态时，立刻发送启动速度估计器7的触发信息，速度估计器7开始启动；

步骤4：速度估计器7是以UUV6的水平面三自由度动力学模型为基础，以北向位置x、东向位置y和艏向角ψ作为速度估计器7的输入，并且引入了北向位置、东向位置和艏向角估计误差的滑模面函数以消除速度估计器模型的不确定性，确定速度估计器模型：

其中，为UUV6在大地坐标系下的北向位置、东向位置和艏向角的估计向量，J∈R^3×3是大地坐标系与UUV6坐标系之间的变换矩阵，为UUV6坐标下的北向速度、东向速度和艏向角速度估计向量，sat(*)为饱和函数，为速度估计器7的滑模面函数，φ₁为边界层厚度；M∈R^3×3为UUV6的惯性质量矩阵；C∈R^3×3为UUV6离心力和哥式力矩阵；D∈R^3×3为粘性矩阵，为外界环境的扰动估计向量，τ∈R^3×1为控制力向量；L₁、L₂、L₃∈R^3×3为速度估计器7增益对角矩阵；

饱和函数sat(*)具体形式如下：

其中，φ₁为正常数，表示边界层的厚度，i＝1,2,3；

根据UUV6的水平面3自由度动力学模型、GPS测量、转换后的北东坐标以及罗经测量到的艏向角，设计速度估计器模型。为抵抗速度估计器模型的不确定性，选取北向、东向位置和艏向角的估计误差作为滑模面，使得估计值按照滑模面上轨迹逐渐逼近位姿的真实值，这样也会使得速度的估计逐渐逼近于速度的真实值，为控制器提供精确的状态信息，为高精度的控制性能提供可能。为防止抖振，采用了饱和函数sat(.)。

步骤5：根据采集的北向位置x、东向位置y和艏向角ψ，解算速度估计器模型，估计出UUV6的速度信息并且得到向量

步骤6：通过比较器1，计算期望向量η_d与向量之间的偏差向量偏差向量η_e和向量同时输入至反步滑模动力定位控制器2中；期望向量η_d＝[x_d y_d ψ_d]^T，x_d、y_d和ψ_d分别表示UUV6在大地坐标下的北向位置、东向位置和艏向角的期望值；

步骤7：北向、东向和艏向角估计值与期望向量的偏差为误差向量中间虚拟镇定向量α₁＝c₁z₁，c₁为α₁的增益矩阵，表示第一个误差向量的比例系数，K为滑模面函数s₂中误差向量z1与误差向量z2的比例系数；得到

步骤8：设定误差向量

步骤9：定义第一个Lyapunov函数V₁，并对V₁求导进行误差向量z₁的稳定性分析：

然后，对误差向量z₂求导：

设计第二个Lyapunov函数:

s₂＝Kz₁+z₂ (16)

其中，s₂为反步滑模控制器的滑模面函数，K为控制器的滑模面增益矩阵，K∈R^3×3为3阶正定对角阵；

对V₂进行求导：

根据Lyapunov稳定性理论，为使系统渐进稳定，设计如下滑模控制律：

其中，h为控制器滑模面函数增益矩阵，κ为滑模面饱和切换函数增益矩阵，φ₁、φ₂为边界层厚度；

在DVL测速失效情况下，根据位置测量传感器GPS12采集得到的UUV经纬度信息，姿态传感器罗经采集的艏向角信息，速度估计器7估计的UUV6速度信息推算得到具有鲁棒性的反步滑模控制律。反步滑模控制律是基于反步法设计的，并且在控制律中添加误差向量z₁和z₂的组合滑模面函数来抵消模型的不确定性以及外界扰动。

步骤10：反步滑模动力定位控制器2的参数初始化完成后，解算得到了一个时间拍内的控制力向量τ，经过推力分配模型3，把纵荡、横荡、艏摇三个自由度上的控制合力分配到UUV6装配的主推进器4和水平辅助推进器5上，如图2所示，UUV6的推进器包括主推进器4、水平辅助推进器5和垂直辅助推进器，主推进器4包括左主推进器13和右主推进器14，水平辅助推进器5包括艏水平辅助推进器18和艉水平辅助推进器17；垂直辅助推进器包括UUV艉部垂直辅助推进器15和艏部垂直辅助推进器16；推力分配模型3如下：

F_ml＝0.5(τ_x+βτ_N/L_m),-γ₁F_1max≤F_ml≤γ₁F_1max

F_mr＝0.5(τ_x-βτ_N/L_m),-γ₁F_1max≤F_mr≤γ₁F_1max

F_bh＝0.5(τ_y-(1-β)τ_N/L_h),-γ₂F_2max≤F_bh≤γ₂F_2max

F_sh＝0.5(τ_y+(1-β)τ_N/L_h),-γ₂F_2max≤F_sh≤γ₂F_2max

其中，F_ml、F_mr、F_bh、F_sh分别表示左主推进器13、右主推进器14、艏水平辅助推进器18、艉水平辅助推进器17分配的推力；Lm、Lh分别表示左主推进器13和右主推进器14间距、艏水平辅助推进器18和艉水平辅助推进器17间距；γ₁、γ₂分别为主推进器4、水平辅助推进器5限额比，F_1max、F_2max分别为主推进器4、水平辅助推进器5能够提供的最大推力，β表示推力分配模型3的系数；β＝0时完全由水平辅助推进器5来提供转艏力矩，β＝1时完全由主推进器4来提供转艏力矩；

步骤11：完成一个时间拍的控制作用后，从步骤5至步骤9开始下一个时间拍对UUV6进行反步滑模动力定位控制；

在仿真平台下，使UUV6从北东坐标下的初始位置(0m，1m，10°)、初始速度(0m/s，0/s，0rad/s)动力定位至目标位置(5m，5m，30°)。速度估计器7、反步滑模控制器的各个参数，在反复调试下观察估计效果及控制效果后，选择最佳的参数如下：

L₁＝diag(10,10,10)、L₂＝diag(10,10,1000)、L₃＝diag(10,10,10)；

K＝diag(0.5,0.5,0.5)、c₁＝diag(0.41,0.35,0.41)、φ₁＝1、φ₂＝1；

h＝diag(1,1,1)、κ＝diag(0.3,0.3,0.3)、β＝0、γ₁＝0.8、γ₂＝0.8；

仿真结果如图3至图7，图3至图5显示了采用速度估计器7的北向、东向位置、艏向角的估计值和测量值的对比，可以看到位置估计值与测量值误差很小，从而保证了速度估计值趋于实际的速度。图6和图7显示了主推和水平辅推的推力实际变化曲线，可以看到推力变化平滑，有利于推进器健康长久的工作。仿真结果表明，速度估计器7的估计误差很小，可以在DVL失效时在线估计出UUV6的速度，推力分配模型3方法能够有效的把广义控制向量分配到各个推进器上，实现了UUV6的反步滑模动力定位控制。

Claims (4)

1.一种DVL测速失效时的UUV反步滑模动力定位控制方法，其特征在于，所述方法包括如下步骤：

步骤1：UUV在水面进行纵荡、横荡和艏摇的三自由度反步滑模动力定位控制，获得期望向量η_d＝[x_d y_d ψ_d]^T，x_d、y_d和ψ_d分别表示UUV在大地坐标下的北向位置、东向位置和艏向角的期望值；

步骤2：对UUV的经纬度和艏向角进行采集，同时采用速度传感器DVL对UUV进行速度采集，确定向量η＝[x y ψ]^T，x、y和ψ分别表示采集的UUV在大地坐标下的北向位置、东向位置和艏向角的值；

步骤3：故障检测模块实时检测，当检测到速度传感器DVL测速无效时，产生触发信息启动速度估计器对UUV的速度信息进行估计，速度估计器是以UUV的水平面三自由度动力学模型为基础，以采集的北向位置x、东向位置y和艏向角ψ作为速度估计器的输入，并且引入了北向位置、东向位置和艏向角估计误差的滑模面函数以消除速度估计器模型的不确定性：

其中，为UUV在大地坐标系下的北向位置、东向位置和艏向角的估计向量，J∈R^3×3是大地坐标系与UUV坐标系之间的变换矩阵，为UUV坐标下的北向速度、东向速度和艏向角速度估计向量，sat(*)为饱和函数，为速度估计器的滑模面函数，φ₁为边界层厚度；M∈R^3×3为UUV的惯性质量矩阵；C∈R^3×3为UUV离心力和哥式力矩阵；D∈R^3×3为粘性矩阵，为外界环境的扰动估计向量，τ∈R^3×1为控制力向量；L₁、L₂、L₃∈R^3×3为速度估计器增益对角矩阵；

饱和函数sat(*)具体形式如下：

其中，φ₁为正常数，表示边界层的厚度，i＝1,2,3；

步骤4：根据期望向量η_d与向量获得偏差向量将向量和偏差向量η_e输入至反步滑模动力定位控制器中，

步骤5：设定误差向量根据Lyapunov稳定性理论，获得稳定的控制力向量τ；

误差向量中间虚拟镇定向量α₁＝c₁z₁，c₁为α₁的增益矩阵，表示第一个误差向量的比例系数，K为反步滑模控制器的滑模面函数s₂中误差向量z₁与误差向量z₂的比例系数；

步骤6：反步滑模动力定位控制器的参数初始化完成后，解算得到了一个时间拍内的控制力向量τ，经过推力分配模型，把纵荡、横荡、艏摇三个自由度上的控制合力分配到UUV装配的主推进器和水平辅助推进器上；

步骤7：完成一个时间拍的控制作用后，从步骤3至步骤5开始下一个时间拍对UUV进行反步滑模动力定位控制。

2.根据权利要求1所述的一种DVL测速失效时的UUV反步滑模动力定位控制方法，其特征在于，所述步骤4中，控制力向量τ：

误差向量中间虚拟镇定向量α₁＝c₁z₁，c₁为α₁的增益矩阵，表示第一个误差向量的比例系数，误差向量控制器的滑模面函数s₂＝Kz₁+z₂，K为滑模面函数s₂中误差向量z1与误差向量z2的比例系数；h为控制器滑模面函数s₂的增益矩阵，κ为滑模面饱和切换函数增益矩阵，φ₁、φ₂为边界层厚度。

3.根据权利要求1或2所述的一种DVL测速失效时的UUV反步滑模动力定位控制方法，其特征在于，所述步骤5中，根据获得的控制力向量τ进行推力分配：

F_ml＝0.5(τ_x+βτ_N/L_m),-γ₁F_1max≤F_ml≤γ₁F_1max (8)

F_mr＝0.5(τ_x-βτ_N/L_m),-γ₁F_1max≤F_mr≤γ₁F_1max (9)

F_bh＝0.5(τ_y-(1-β)τ_N/L_h),-γ₂F_2max≤F_bh≤γ₂F_2max (10)

F_sh＝0.5(τ_y+(1-β)τ_N/L_h),-γ₂F_2max≤F_sh≤γ₂F_2max (11)

其中，F_ml、F_mr、F_bh、F_sh分别表示左主推进器、右主推进器、艏水平辅助推进器、艉水平辅助推进器分配的推力；Lm、Lh分别表示左主推进器和右主推进器间距、艏水平辅助推进器和艉水平辅助推进器间距；γ₁、γ₂分别为主推进器、水平辅助推进器限额比，F_1max、F_2max分别为主推进器、水平辅助推进器能够提供的最大推力，β表示推力分配系数；β＝0时完全由水平辅助推进器来提供转艏力矩，β＝1时完全由主推进器来提供转艏力矩。

4.根据权利要求3所述的一种DVL测速失效时的UUV反步滑模动力定位控制方法，其特征在于，所述步骤2中，采用位置测量传感器GPS和姿态传感器罗经对UUV的经纬度和艏向角进行采集。