CN102298326B - 欠驱动auv自适应轨迹跟踪控制装置及控制方法 - Google Patents

Abstract

本发明提供的是一种欠驱动AUV自适应轨迹跟踪控制装置及控制方法。测量元件(3)采集的AUV实际位置和航向角以及参考路径生成器(1)产生的参考位置和参考航向角信息，通过微分同胚变换器(6)得到新的状态变量，然后将新的状态变量及传感器(5)采集的速度和角速度信息，传递给参数估计器(11)和纵向推力及转艏力矩控制器(14)，通过解算得到控制指令去驱动执行机构，调整AUV的纵向推力和转艏力矩。本发明无需已知AUV的惯性质量参数和水动力阻尼参数，实现在指定时间按指定速度到达指定位置。由于本发明考虑的AUV是欠驱动的，可以减少系统的能源消耗和制造成本，减少系统的重量，提高推进效率。

Description

欠驱动AUV自适应轨迹跟踪控制装置及控制方法

技术领域

本发明是一种AUV轨迹跟踪控制装置，本发明也涉及一种AUV轨迹跟踪控制方法。具体地说是一种基于非线性控制理论的欠驱动AUV在模型参数未知情况下的轨迹跟踪控制装置及方法。

背景技术

导致水下机器人欠驱动的原因主要有以下几个方面：(1)减少成本和重量。对于水下机器人来说，推进器的成本很高而且消耗系统的能源较大，因而现在大多数水下机器人是欠驱动力的。(2)推进效率的影响。全驱动水下机器人当前进速度增加时，槽道推进器的推力减额将越来越严重，该自由度上的运动很难预测。(3)系统可靠性的需要。当全驱动系统的一个或多个推进装置驱动失败或失灵时，完全驱动变成欠驱动。近年来，水下机器人的轨迹跟踪(Trajectory tracking)控制得到许多学者的关注。轨迹跟踪(TT)控制问题的难点在于要求系统在指定时间到达指定位置，它的实现要比路径跟踪(Path following)困难得多，对于全驱动系统，TT问题已经能够较好解决，而欠驱动自治水下机器人(Autonomous underwatervehicle，AUV)由于系统的独立控制输入量少于其自由度，使系统具有不可积的加速度约束，并且AUV的运动学和动力学模型具有高度非线性和耦合性，使控制设计更加困难。欠驱动AUV的轨迹跟踪控制的典型方法是利用多变量模型的局部线性化和解耦使系统自由度与控制输入自由度相等，然后利用线性或非线性控制方法设计控制器。然而，这些方法只能保证在选定的运行点邻域内是稳定的。基于Lyapunov的非线性设计可以克服上述限制。Jiang，2002针对欠驱动水面船提出了基于Lyapunov直接方法的全局跟踪控制问题，但没有考虑模型参数不确定情况。

发明内容

本发明的目的在于提供能够在惯性质量参数和水动力参数未知情况下实现欠驱动AUV的轨迹跟踪控制，即在指定时间内按指定速度到达指定位置的欠驱动AUV自适应轨迹跟踪控制装置。本发明的目的还在于提供一种欠驱动AUV自适应轨迹跟踪控制方法。

本发明的目的是这样实现的：

本发明的欠驱动AUV自适应轨迹跟踪控制装置包括参考路径生成器1、微分同胚变换器6、纵向速度虚拟控制器9、艏摇角速度虚拟控制器10、参数估计器11、纵向推力及转艏力矩控制器14，测量元件3采集的AUV实际位置和航向角以及参考路径生成器1产生的参考位置和参考航向角信息，通过微分同胚变换器6得到新的状态变量，然后将新的状态变量及传感器5采集的速度和角速度信息，传递给参数估计器11和纵向推力及转艏力矩控制器14，通过解算得到控制指令去驱动执行机构，调整AUV的纵向推力和转艏力矩。

本发明的欠驱动AUV自适应轨迹跟踪控制装置还可以包括：

1、参考路径生成器1通过给定期望纵向速度和艏摇角速度，将产生的位置、航向角信息传递给微分同胚变换器6的同时将期望侧向速度信号反馈给参数估计器11和角速度虚拟控制器10。

2、测量元件3包括位置、角度传感器4和线速度、角速度传感器5，位置、角度传感器4采集的信息传递给微分同胚变换器6，线速度、角速度传感器5采集的信息同时传递给纵向推力及转艏力矩控制器14和参数估计器11。

3、纵向速度虚拟控制器9根据新的状态变量信息及参考线速度、参考艏摇角速度信息得到线速度虚拟控制量，然后将该量传递给参数估计器11。

4、角速度虚拟控制器10根据参考线速度、角速度、实际线速度、角速度、中间状态变量、新的状态变量信息，产生角速度虚拟控制量，然后将该量传递给参数估计器11。

5、参数估计器11根据参考速度、实际速度及新的状态变量信息，通过李亚普诺夫直接法产生参数的估计值，并将这些信息传递给纵向推力及转艏力矩控制器14。

6、纵向推力及转艏力矩控制器14分为纵向推力控制器15和转艏力矩控制器16，针对跟踪误差动态方程根据参考纵向速度、参考艏摇角速度、新的状态变量、参数估计值、实际的线速度和角速度信息，通过李亚普诺夫法和反步法解算出纵向推力和转艏力矩信息传递给AUV的执行机构，调整AUV的位置、航向和速度。

本发明的欠驱动AUV自适应轨迹跟踪控制方法包括：

由参考路径生成器1、微分同胚变换器6、纵向速度虚拟控制器9、艏摇角速度虚拟控制器10、参数估计器11、纵向推力及转艏力矩控制器14构成控制装置；

(1)、参考路径生成器通过参考输入产生期望轨迹，得到期望的位置、航向角和速度信息；

(2)、位置传感器采集AUV的位置、航向角信息，将期望的位置、航向角和当前的状态信息一起传递给微分同胚变换器；

(3)、微分同胚变换器包括大地坐标到船体坐标变换器和欠驱动项补偿器，大地坐标到船体坐标变换器得到船体坐标系下的状态变量，并将该中间变量信息传递给欠驱动项补偿器，然后由欠驱动项补偿器得到新的状态变量；

(4)、虚拟控制器包括纵向线速度虚拟控制和艏摇角速度虚拟控制，线速度控制器根据新的状态变量信息、参考纵向速度、参考艏摇角速度信息产生线速度虚拟控制量，并同时传递给参数估计器和纵向推力控制器；艏摇角速度虚拟控制器根据中间状态变量、参考线速度、角速度、新的状态变量信息产生角速度虚拟控制量，传递给参数估计器；

(5)、参数估计器利用李亚普诺夫直接法将产生的参数估计值传递给纵向推力控制器和转艏力矩控制器；

(6)、控制器通过李亚普诺夫法和反步法解算出纵向推力和转艏力矩量传递给AUV的执行机构，调整AUV的位置、航向、速度。

此发明的优点在于无需已知AUV的惯性质量参数和水动力阻尼参数，实现在指定时间按指定速度到达指定位置。由于本发明考虑的AUV是欠驱动的，可以减少系统的能源消耗和制造成本，减少系统的重量，提高推进效率。并且当全驱动系统的一个或多个推进装置驱动失败或失灵时，可以保证AUV的可靠运行。本发明通过仿真试验验证了方案的可行性和有效性。

附图说明

图1为欠驱动AUV自适应轨迹跟踪控制系统总体结构图；

图2为位置和艏向跟踪误差曲线；

图3为线速度和角速度跟踪误差曲线；

图4为纵向推力和艏摇力矩曲线；

图5为AUV模型参数估计值。

具体实施方式

下面对本发明进行详细描述：

图1给出了本发明的欠驱动AUV自适应轨迹跟踪控制装置的组成。

在图1中各数字代表的装置如下：1-参考路径生成器；2-水下机器人AUV；3-测量元件；4-位置、角度传感器；5-线速度、角速度传感器；6-微分同胚变换器；7-大地坐标到船体坐标变换器；8-欠驱动项补偿器；9-线速度虚拟控制器；10-角速度虚拟控制器；11-参数估计器；12-参数θ₁→θ₄估计值；13-参数θ₅→θ₁₀估计值；14-控制器；15-纵向推力控制器；16-转艏力矩控制器。

结合图1，本发明的欠驱动AUV自适应轨迹跟踪控制装置包括参考路径生成器1，微分同胚变换器6，纵向速度虚拟控制器9，艏摇角速度虚拟控制器10，参数估计器11，控制器14。传感器4采集AUV的实际位置和航向角以及参考路径生成器1产生的参考位置和参考航向角信息，通过微分同胚变换器6得到新的状态变量，然后将新的状态变量及传感器5采集的速度和角速度信息，传递给参数估计器11和纵向推力及转艏力矩控制器14，通过解算得到控制指令去驱动执行机构，调整AUV的纵向推力和转艏力矩，实现欠驱动AUV轨迹跟踪。

参考路径生成器1通过虚拟AUV模型方程产生参考位置信号x_d、y_d和参考航向角ψ_d，传感器采集的AUV的实际位置x，y、航向角ψ信息同时传递给大地坐标到船体坐标变换器7，该系统通过(z₁，z₂，z₃)＝(xcos(ψ)+ysin(ψ)，-xsin(ψ)+ycos(ψ)，ψ)得到中间状态变量z₁，z₂，z₃，及中间变量的误差量z_1e，z_2e，z_3e，并发送给欠驱动项补偿器8。

欠驱动项补偿器8根据中间状态变量z₁，z₂，z₃和误差量z_1e，z_2e，z_3e通过关系式(ξ₁，ξ₂，ξ₃)＝(z_1e-λ₁r_dz_2e，z_2e，z_3e)，λ₁＞0得到新的状态变量ξ₁，ξ₂，ξ₃，及跟踪误差动态方程：

${\stackrel{\·}{\mathrm{\ξ}}}_1={\mathrm{\λ}}_1{r}_d^2{\mathrm{\ξ}}_1+(r_d-{\mathrm{\λ}}_1{\stackrel{\·}{r}}_d+{\mathrm{\λ}}_1^2{r}_d^3){\mathrm{\ξ}}_2-{\mathrm{\λ}}_1{r}_d{v}_e+u_e+(z_2+{\mathrm{\λ}}_1{r}_d{z}_1)r_e$

${\stackrel{\·}{\mathrm{\ξ}}}_2=-r_d{\mathrm{\ξ}}_1-{\mathrm{\λ}}_1{r}_d^2{\mathrm{\ξ}}_2+v_e-z_1{r}_e$

${\stackrel{\·}{\mathrm{\ξ}}}_3=r_e$

${\stackrel{\·}{u}}_e=\frac{m_{22}}{m_{11}}\mathrm{vr}-\frac{d_{11}}{m_{11}}u+\frac{1}{m_{11}}{\mathrm{\τ}}_u-{\stackrel{\·}{u}}_d$

${\stackrel{\·}{v}}_e=-\frac{m_{11}}{m_{22}}{u}_e{r}_d-\frac{d_{22}}{m_{22}}{v}_e-\frac{m_{11}}{m_{22}}{\mathrm{ur}}_e$

${\stackrel{\·}{r}}_e=\frac{m_{11}-m_{22}}{m_{33}}\mathrm{uv}-\frac{d_{33}}{m_{33}}r+\frac{1}{m_{33}}{\mathrm{\τ}}_r-{\stackrel{\·}{r}}_d$

其中：τ_u，τ_r为纵向推力和转艏力矩；u_d，r_d为给定参考值；v_d为期望侧向速度；u，v，r为AUV实际速度；u_e，v_e，r_e＝(u-u_d，v-v_d，r-r_d)。

欠驱动项补偿器8将得到的状态变量ξ₁，ξ₂，ξ₃传递给虚拟控制器9和10。

线速度虚拟控制器9根据非线性方法得到关系式λ₃＞0，将该控制量发送给参数估计器11和控制器14。

参数θ₁→θ₄估计值12根据线速度虚拟控制量、参考路径生成器产生的期望侧向速度、传感器采集的AUV实际线速度、角速度信号、状态变量ξ₁，ξ₂，ξ₃，基于李亚普诺夫直接法得到参数的估计值，将该值传递给纵向推力控制器15。

纵向推力控制器15根据李亚普诺夫直接法和反步法得到关系式： ${\mathrm{\τ}}_u=-c_1{\stackrel{\‾}{u}}_e+{\widehat{\mathrm{\θ}}}_1{\mathrm{\λ}}_2{r}_d{v}_e-{\widehat{\mathrm{\θ}}}_2\mathrm{vr}+{\widehat{\mathrm{\θ}}}_{3}u+{\widehat{\mathrm{\θ}}}_4(-{\mathrm{\ξ}}_1+{\stackrel{\·}{u}}_d+{\stackrel{\·}{\mathrm{\α}}}_{\mathrm{ue}}),$ 其中：c₁＞0，λ₂＞0，为参数θ₁→θ₄的估计值，α_ue为纵向推力虚拟控制量。由此得到纵向推力τ_u，并作用于AUV来调整AUV的位置和纵向速度。

角速度虚拟控制器10通过期望速度、角速度、传感器采集的实际线速度和角速度信息、中间变量z₁，z₂，z₃、状态变量ξ₁，ξ₂，ξ₃，根据关系式 ${\mathrm{\β}}_{\mathrm{re}}=-c_2[{\mathrm{\ξ}}_1(z_2+{\mathrm{\λ}}_1{r}_d{z}_1)-{\mathrm{\ξ}}_2{z}_1-\frac{{\mathrm{\λ}}_2{m}_{11}}{m_{22}}{v}_{e}u+{\mathrm{\ξ}}_3],$ 其中：c₂，λ₁＞0得到控制量α_re发送给参数估计器(11)和控制器(14)。

参数θ₅→θ₁₀估计值13根据AUV实际速度、期望速度、中间量z₁，z₂，z₃，状态变量ξ₁，ξ₂，ξ₃，以及角速度虚拟控制量得到参数估计值，并发送给转艏力矩控制器16。

转艏力矩控制器16根据关系式

${\mathrm{\τ}}_r=-c_3{\stackrel{\‾}{r}}_e+{\widehat{\mathrm{\θ}}}_5[{-\mathrm{\ξ}}_1(z_2+{\mathrm{\λ}}_1{r}_d{z}_1)+{\mathrm{\ξ}}_2{z}_1-{\mathrm{\ξ}}_3+{\stackrel{\·}{r}}_d+{\stackrel{\·}{\mathrm{\α}}}_{\mathrm{rea}}]+{\widehat{\mathrm{\θ}}}_6(-c_2{\mathrm{\λ}}_{2}u{u}_e{r}_d-c_2{\mathrm{\λ}}_2{u}^2{r}_e)-$

${\widehat{\mathrm{\θ}}}_7{c}_2{\mathrm{\λ}}_2{\mathrm{uv}}_e+{\widehat{\mathrm{\θ}}}_8({\mathrm{\λ}}_2{v}_{e}u+c_2{\mathrm{\λ}}_2{v}_{e}u)-{\widehat{\mathrm{\θ}}}_9\mathrm{uv}+{\widehat{\mathrm{\θ}}}_{10}r$

其中：c₃＞0，

为参数θ₅→θ₁₀的估计值，得到转艏力矩τ_r，并作用于AUV来调整AUV的位置、航向及速度。

参数估计器11根据关系式其中：

Γ＝diag(γ₁，γ₂，·γ₁₀)，γ_i＞0，i＝1，2，·10为自适应控制参数，得到参数估计值

本发明采用某自治水下机器人的动态数学模型进行仿真验证。质量m＝185kg；其它参数：

m₁₁＝215，m₂₂＝265，m₃₃＝215，d₁₁＝70，d₂₂＝100，d₃₃＝50

设定初始条件为：(x(0)，y(0)，ψ(0)，u(0)，v(0)，r(0))＝(1，-1，1，0，0.1，0)

参考路径初始条件：(x_d(0)，y_d(0)，ψ_d(0)，v_d(0))＝(0，0，0，0)

参考速度：u_d(t)＝1m/s，r_d(t)＝0.2rad/s

主要控制参数选择：λ₁＝0.08，λ₂＝5，λ₃＝0.5Γ＝diag(0.1，190，1000，20，5.5，10000，1，1，2000，0.2)

仿真结果见附图2-图5。

通过对仿真曲线和数据分析后可以看出，在本发明提出的自适应控制律作用下，AUV能够克服模型参数未知的干扰作用，快速跟踪上参考路径，保持给定姿态，前向速度及艏摇角速度，使跟踪误差x(t)-x_d(t)，y(t)-y_d(t)，ψ(t)-ψ_d(t)，u(t)-u_d(t)，v(t)-v_d(t)，r(t)-r_d(t)，在60s左右收敛到零，参数估计误差有界且收敛到有限值，仿真结果表明，本发明控制律具有全局渐近跟踪特性，对于模型参数未知具有较好的自适应性。

Claims (2)

1.一种欠驱动AUV自适应轨迹跟踪控制装置，包括参考路径生成器(1)、微分同胚变换器(6)、纵向速度虚拟控制器(9)、艏摇角速度虚拟控制器(10)、参数估计器(11)、纵向推力及转艏力矩控制器(14)，其特征是：测量元件(3)采集的AUV实际位置和航向角以及参考路径生成器(1)产生的参考位置和参考航向角信息，微分同胚变换器包括大地坐标到船体坐标变换器和欠驱动项补偿器，大地坐标到船体坐标变换器得到船体坐标系下的状态变量，并将该中间变量信息传递给欠驱动项补偿器，然后由欠驱动项补偿器得到新的状态变量，然后将新的状态变量及传感器(5)采集的速度和角速度信息，传递给参数估计器(11)和纵向推力及转艏力矩控制器(14)，通过解算得到控制指令去驱动执行机构，调整AUV的纵向推力和转艏力矩；

参考路径生成器(1)通过给定期望纵向速度和艏摇角速度，将产生的位置、航向角信息传递给微分同胚变换器(6)的同时将期望侧向速度信号反馈给参数估计器(11)和角速度虚拟控制器(10)；

测量元件(3)包括位置、角度传感器(4)和线速度、角速度传感器(5)，位置、角度传感器(4)采集的信息传递给微分同胚变换器(6)，线速度、角速度传感器(5)采集的信息同时传递给纵向推力及转艏力矩控制器(14)和参数估计器(11)；

纵向速度虚拟控制器(9)根据新的状态变量信息及参考线速度、参考艏摇角速度信息得到线速度虚拟控制量，然后将该量传递给参数估计器(11)；

角速度虚拟控制器(10)根据参考线速度、角速度、实际线速度、角速度、中间状态变量、新的状态变量信息，产生角速度虚拟控制量，然后将该量传递给参数估计器(11)；

参数估计器(11)根据参考速度、实际速度及新的状态变量信息，通过李亚普诺夫直接法产生参数的估计值，并将这些信息传递给纵向推力及转艏力矩控制器(14)；

纵向推力及转艏力矩控制器(14)分为纵向推力控制器(15)和转艏力矩控制器(16)，针对跟踪误差动态方程根据参考纵向速度、参考艏摇角速度、新的状态变量、参数估计值、实际的线速度和角速度信息，通过李亚普诺夫法和反步法解算出纵向推力和转艏力矩信息传递给AUV的执行机构，调整AUV的位置、航向和速度。

2.一种欠驱动AUV自适应轨迹跟踪控制方法，其特征是：

由参考路径生成器(1)、微分同胚变换器(6)、纵向速度虚拟控制器(9)、艏摇角速度虚拟控制器(10)、参数估计器(11)、纵向推力及转艏力矩控制器(14)构成控制装置；

(1)参考路径生成器通过参考输入产生期望轨迹，得到期望的位置、航向角和速度信息；

(2)位置传感器采集AUV的位置、航向角信息，将期望的位置、航向角和当前的状态信息一起传递给微分同胚变换器；

(3)微分同胚变换器包括大地坐标到船体坐标变换器和欠驱动项补偿器，大地坐标到船体坐标变换器得到船体坐标系下的状态变量，并将该中间变量信息传递给欠驱动项补偿器，然后由欠驱动项补偿器得到新的状态变量；

(4)虚拟控制器包括纵向线速度虚拟控制器和艏摇角速度虚拟控制器，纵向线速度虚拟控制器根据新的状态变量信息、参考纵向速度、参考艏摇角速度信息产生线速度虚拟控制量，并同时传递给参数估计器和纵向推力控制器；艏摇角速度虚拟控制器根据中间状态变量、参考线速度、角速度、新的状态变量信息产生角速度虚拟控制量，传递给参数估计器；

(5)参数估计器利用李亚普诺夫直接法将产生的参数估计值传递给纵向推力控制器和转艏力矩控制器；

(6)纵向推力及转艏力矩控制器通过李亚普诺夫法和反步法解算出纵向推力和转艏力矩量传递给AUV的执行机构，调整AUV的位置、航向、速度。

Images