CN103616821A - 一种六自由度航行器的鲁棒控制器的设计方法 - Google Patents

Abstract

本发明设计了一种用于六自由度欠驱动水下航行器的鲁棒跟踪控制器。考虑到欠驱动无法实现渐近稳定的特性，采取了令误差趋于极小值的方法，解决了Lyapunov函数的选择。采用backstepping方法以及函数映射技术设计了的鲁棒控制器，该控制器在航行器存在环境扰动的情况下，可以任意初始状态出发(包括转艏的初始速度为零)，自动地跟踪任意光滑的轨迹，并实现了较好的自适应跟踪效果。

Description

一种六自由度航行器的鲁棒控制器的设计方法

技术领域

本发明属于机器人控制控制技术领域，涉及一种六自由度欠驱动水下航行器的鲁棒轨迹跟踪控制器的设计方法。

背景技术

欠驱动***指独立变量个数少于***自由度个数的非线性***，它需要通过较少的驱动器完成复杂的运动控制任务；多数情况下欠驱动机械***为非完整约束***，同时又表现出一般光滑状态反馈控制无法镇定的性质。因此，欠驱动***的控制与跟踪的理论引起了越来越多的非线性研究者的兴趣。

在实际应用方面，欠驱动***由于驱动器的减少，具有重量轻、成本低、能耗少、维修方便等众多优点；另一方面，在控制器故障等场合，全驱动***会变为欠驱动***，欠驱动***容错能力也引起了众多关注。近年来，欠驱动***成为很多研究领域(如航空航天、船舶、车辆、机器人等)的新热点。

然而，在实际应用中，对于机器人***来说，由于其工作环境***，在实际控制中将面临更多的不确定性因素，因而其运动控制要求有较强的扰动抑制能力；具有高度鲁棒性的控制方法是解决该类问题的有效途径之一。作为空间的航行器，其自由度将由平面的三个扩展为六个，***更加复杂。

因此，设计一种合理的用于欠驱动***的鲁棒控制器，特别是用于空间的六自由度的控制器具有重要的应用价值。

发明内容

本发明所要解决的技术问题是设计一种六自由度的用于欠驱动水下航行器轨迹跟踪的鲁棒控制器，可使航行器在任意初始状态，存在环境扰动的情况下，实现良好的跟踪效果。

本发明主要包括以下内容：

(1)建立六自由度欠驱动水下航行器的动态模型；

(2)Lyapunov的选择方法；

(3)鲁棒控制器的设计；

(4)控制器参数的选择。

具体实施方式

本发明以六自由度欠驱动水下航行器为研究对象。

1.***模型的建立

六自由度欠驱动水下航行器的数学模型为：

$\begin{array}{c}{\stackrel{\·}{\mathrm{\η}}}_1=J_1\left({\mathrm{\η}}_2\right)V_1\\ {\stackrel{\·}{\mathrm{\η}}}_2=J_2\left({\mathrm{\η}}_2\right)V_2\\ M_1{\stackrel{\·}{V}}_1=-M_{1}C\left(V_2\right)V_1-\stackrel{\‾}{V_1}{D}_1\left(V_1\right)+{\mathrm{\τ}}_1+{\mathrm{\τ}}_{\mathrm{\ω}1}\left(t\right)\\ M_2{\stackrel{\·}{V}}_2=-M_{2}C\left(V_1\right)V_1-C\left(V_2\right)V_2-D_2\left(V_2\right)V_2+{\mathrm{\τ}}_2+{\mathrm{\τ}}_{\mathrm{\ω}2}\left(t\right)\end{array}.---\left(1\right)$

其中η₁＝(x y z)^T，η₂=(φ θ ψ)^T，代表***在直角坐标系中的位姿；V₁=(u v w)^T，V₂=(p q r)^T，代表各位姿的速度， ${\stackrel{\‾}{V}}_1=\mathrm{diag}\left(\begin{array}{ccc}u& v& w\end{array}\right);$ J₁(η₂)，J₂(η₂)，M₁，M₂，C(V₁)，C(V₂)，D₁(V₁)，D₂(V₂)分别代表了雅可比变换矩阵、惯性矩阵、科氏矩阵以及阻尼矩阵，分别表示为：

$J_1\left({\mathrm{\η}}_2\right)=\left(\begin{array}{ccc}\mathrm{c\ψc\θ}& -\mathrm{s\ψc\φ}+\mathrm{s\φs\θc\ψ}& \mathrm{s\ψs\φ}+\mathrm{s\θc\ψc\φ}\\ \mathrm{s\ψc\θ}& \mathrm{c\ψc\φ}+\mathrm{s\φs\θs\ψ}& -\mathrm{c\ψs\φ}+\mathrm{s\θs\ψc\φ}\\ -\mathrm{s\θ}& \mathrm{s\φc\θ}& \mathrm{c\φc\θ}\end{array}\right),J_2\left({\mathrm{\η}}_2\right)=\left(\begin{array}{ccc}1& \mathrm{s\φt\θ}& \mathrm{c\φt\θ}\\ 0& \mathrm{c\φ}& -\mathrm{s\φ}\\ 0& \mathrm{s\φ}/\mathrm{c\θ}& \mathrm{c\φ}/\mathrm{c\θ}\end{array}\right),C\left(V_1\right)=\left(\begin{array}{ccc}0& w& -v\\ -w& 0& u\\ v& -u& 0\end{array}\right),$

$C\left(V_2\right)=\left(\begin{array}{ccc}0& r& -q\\ -r& 0& p\\ q& -p& 0\end{array}\right),M_1=\mathrm{diag}\left(\begin{array}{ccc}{m}_{11}& m_{22}& m_{33}\end{array}\right),M_2=\mathrm{diag}\left(\begin{array}{ccc}{m}_{44}& m_{55}& m_{66}\end{array}\right),$

$D_1\left(v_1\right)=\mathrm{diag}(d_{11}+\underset{i=2}{\overset{3}{\mathrm{\Σ}}}{d}_{u1}{\left|u\right|}^{i-1},d_{22}+\underset{i=2}{\overset{3}{\mathrm{\Σ}}}{d}_{v1}{\left|v\right|}^{i-1},d_{33}+\underset{i=2}{\overset{3}{\mathrm{\Σ}}}{d}_{w1}{\left|w\right|}^{i-1}),$

$D\left(v_2\right)=\mathrm{diag}(d_{44}+\underset{i=2}{\overset{3}{\mathrm{\Σ}}}{d}_{p1}{\left|p\right|}^{i-1},d_{55}+\underset{i=2}{\overset{3}{\mathrm{\Σ}}}{d}_{q1}{\left|q\right|}^{i-1},d_{66}+\underset{i=2}{\overset{3}{\mathrm{\Σ}}}{d}_{r1}{\left|r\right|}^{i-1}),$

控制输入为

τ₁=(τ_u，0，0)^T，τ₂=(0，τ_q，τ_r)^T，

其中τ_u，τ_q，τ_r分别为推进力矩、仰俯力矩、转艏力矩。最后，环境的扰动量为如下向量

τ_w1(t)=(τ_wu(t)，τ_wv(t)，τ_ww(t))^T，τ_w2=(τ_wp(t)，τ_wq(t)，τ_wr(t))^T，

这些扰动量是未知的，仅知其上界如下：

$\left|{\mathrm{\&tau;}}_{\mathrm{wu}}\left(t\right)\right|\&le;{\mathrm{\&tau;}}_{\mathrm{wu}}^{\max }<\&infin;,\left|{\mathrm{\&tau;}}_{\mathrm{wv}}\left(t\right)\right|\&le;{\mathrm{\&tau;}}_{\mathrm{wv}}^{\max }<\&infin;,\left|{\mathrm{\&tau;}}_{\mathrm{ww}}\left(t\right)\right|\&le;{\mathrm{\&tau;}}_{\mathrm{ww}}^{\max }<\&infin;,\left|{\mathrm{\&tau;}}_{\mathrm{wp}}\left(t\right)\right|\&le;{\mathrm{\&tau;}}_{\mathrm{wp}}^{\max }<\&infin;,\left|{\mathrm{\&tau;}}_{\mathrm{wq}}\left(t\right)\right|\&le;{\mathrm{\&tau;}}_{\mathrm{wq}}^{\max }<\&infin;,\left|{\mathrm{\&tau;}}_{\mathrm{wr}}\left(t\right)\right|\&le;{\mathrm{\&tau;}}_{\mathrm{wr}}^{\max }<\&infin;$

|R_v(t)|≤R_vmax<∞，|R_r(t)|≤R_rmax<∞。

控制器设计目标是设计τ_u，τ_q，τ_r，使得***可以自动跟踪目标轨迹η_d=(x_d，y_d，z_d)^T，即使跟踪误差(x-x_d y-y_d z-z_d)^T全局稳定。记e=(e₁，e₂，e₃)^T=η₁-η_d为跟踪误差。

2.***Lyapunov函数的选择

首先做如下定义：

其中，β₁>0，z₁=(z₁₁，z₁₂，z₁₃)^T。 $z_2=\left(\begin{array}{c}q\\ r\end{array}\right)-\left(\begin{array}{ccc}0& 1& 0\\ 0& 0& 1\end{array}\right)\mathrm{\&alpha;},$ 其中α=-B^-1(A+β₂(z₁-δ))， $A=-M^{-1}{\stackrel{\&OverBar;}{V}}_{1}D\left(V_1\right)+M^{-1}{\widehat{\mathrm{\&tau;}}}_{w1}-J_1{\left({\mathrm{\&eta;}}_2\right)}^T{\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&eta;}}}_d-{\left(\begin{array}{ccc}0& {\mathrm{\&delta;}}_3& {\mathrm{\&delta;}}_2\end{array}\right)}^{T}p+{\mathrm{\&beta;}}_1{z}_1,$ $B=\left(\begin{array}{ccc}1/m_{11}& {\mathrm{\&delta;}}_3& -{\mathrm{\&delta;}}_2\\ 0& 0& {\mathrm{\&delta;}}_1\\ 0& -{\mathrm{\&delta;}}_1& 0\end{array}\right),$ δ=(δ₁，δ₂，δ₃)^T，δ₁≠0为任意小常数， ${\widetilde{\mathrm{\&tau;}}}_{w1}={\widehat{\mathrm{\&tau;}}}_{w1}-{\mathrm{\&tau;}}_{w1},{\widetilde{\mathrm{\&tau;}}}_{w2}={\widehat{\mathrm{\&tau;}}}_{w2}-{\mathrm{\&tau;}}_{w2}.$

选择如下Lyapunov函数：

$W=\frac{1}{2}{e}^{T}e+\frac{1}{2{\mathrm{\&beta;}}_1^2}{(z_1-\mathrm{\&delta;})}^T(z_1-\mathrm{\&delta;})+\frac{{\mathrm{\&Gamma;}}_1^{-1}}{2}{\widetilde{\mathrm{\&tau;}}}_{w1}^T{\widetilde{\mathrm{\&tau;}}}_{w1}+\frac{1}{2}{z}_2^T{z}_2+\frac{{\mathrm{\&Gamma;}}_2^{-1}}{2}{\widetilde{\mathrm{\&tau;}}}_{w2}^T{\widetilde{\mathrm{\&tau;}}}_{w2},---\left(3\right)$

则其沿着***的导数为：

$\begin{array}{c}\stackrel{\&CenterDot;}{W}=-{\mathrm{\&beta;}}_1{e}^{T}e+{\mathrm{\&delta;}}^T{J}_1{\left({\mathrm{\&eta;}}_2\right)}^{T}e+\frac{1}{{\mathrm{\&beta;}}_1^2}{(z_1-\mathrm{\&delta;})}^T(A+\mathrm{Bw})+{\widetilde{\mathrm{\&tau;}}}_{w1}^T{\mathrm{\&Gamma;}}_1^{-1}({\stackrel{\stackrel{\&CenterDot;}{^}}{\mathrm{\&tau;}}}_{w1}-\frac{1}{{\mathrm{\&beta;}}_1^2}{\mathrm{\&Gamma;}}_1{M}^{-1}(z_1-\mathrm{\&delta;}))\\ +z_2^T\left({\left(M_2^{\&prime;}\right)}^{-1}\mathrm{\&Lambda;}(-M_{2}C\left(V_1\right)V_1-C\left(V_2\right)V_2-D_2\left(V_2\right)V_2+{\mathrm{\&tau;}}_2+{\widehat{\mathrm{\&tau;}}}_{\mathrm{\&omega;}2}\left(t\right)-M_2\stackrel{\&CenterDot;}{\mathrm{\&alpha;}})\right)+{\mathrm{\&Gamma;}}_2^{-1}{\widetilde{\mathrm{\&tau;}}}_{w2}^T({\stackrel{\stackrel{\&CenterDot;}{^}}{\mathrm{\&tau;}}}_{w2}-{\mathrm{\&Lambda;}}^T{\mathrm{\&Gamma;}}_2{z}_2)\end{array},---\left(4\right)$

其中，w=(τ_u q r)^T。

3.鲁棒控制器的设计

设计如下鲁棒控制律：

${\stackrel{\stackrel{\&CenterDot;}{^}}{\mathrm{\&tau;}}}_{\mathrm{wu}}=\frac{{\mathrm{\&Gamma;}}_1}{m_{11}}\frac{1}{{\mathrm{\&beta;}}_1^2}\mathrm{proj}({\widehat{\mathrm{\&tau;}}}_{\mathrm{wu}},-(z_{11}-{\mathrm{\&delta;}}_1)),{\stackrel{\stackrel{\&CenterDot;}{^}}{\mathrm{\&tau;}}}_{\mathrm{wv}}=\frac{{\mathrm{\&Gamma;}}_1}{m_{22}}\frac{1}{{\mathrm{\&beta;}}_1^2}\mathrm{proj}({\widehat{\mathrm{\&tau;}}}_{\mathrm{wv}},-(z_{22}-{\mathrm{\&delta;}}_2)),{\stackrel{\stackrel{\&CenterDot;}{^}}{\mathrm{\&tau;}}}_{\mathrm{ww}}=\frac{{\mathrm{\&Gamma;}}_1}{m_{33}}\frac{1}{{\mathrm{\&beta;}}_1^2}\mathrm{proj}({\widehat{\mathrm{\&tau;}}}_{\mathrm{ww}},-(z_{33}-{\mathrm{\&delta;}}_3)),---\left(5\right)$

${\stackrel{\stackrel{\&CenterDot;}{^}}{\mathrm{\&tau;}}}_{\mathrm{\&omega;}2}=\mathrm{proj}({\widehat{\mathrm{\&tau;}}}_{\mathrm{\&omega;}2},{\mathrm{\&Lambda;}}^T{\mathrm{\&Gamma;}}_2{z}_2),---\left(6\right)$

将上述值代入(4)中，得到：

$\stackrel{\&CenterDot;}{W}\&le;-{\mathrm{\&beta;}}_1{e}^{T}e+{\mathrm{\&delta;}}^T{J}_1{\left({\mathrm{\&eta;}}_2\right)}^{T}e-(1/{\mathrm{\&beta;}}_1^2){(z_1-\mathrm{\&delta;})}^T{\mathrm{\&beta;}}_2(z_1-\mathrm{\&delta;})-{\mathrm{\&beta;}}_3{z}_2^2.---\left(8\right)$

4.参数的选择

对任给γ>0，都有：

${\mathrm{\&delta;}}^T{J}^T\left({\mathrm{\&eta;}}_2\right)e\&le;\frac{1}{2\mathrm{\&gamma;}}{\mathrm{\&delta;}}^T\mathrm{\&delta;}+\frac{\mathrm{\&gamma;}}{2}{e}^{T}J\left({\mathrm{\&eta;}}_2\right)J^T\left({\mathrm{\&eta;}}_2\right)e=\frac{\mathrm{\&gamma;}}{2}{e}^{T}e+\frac{1}{2\mathrm{\&gamma;}}{\mathrm{\&delta;}}^T\mathrm{\&delta;},---\left(9\right)$

将(8)变化为：

$\stackrel{\&CenterDot;}{W}\&le;-e^T({\mathrm{\&beta;}}_1-\frac{\mathrm{\&gamma;}}{2})e-{(z_1-\mathrm{\&delta;})}^T\frac{{\mathrm{\&beta;}}_2}{{\mathrm{\&beta;}}_1^2}(z_1-\mathrm{\&delta;})-{\mathrm{\&beta;}}_3{z}_2^2+\frac{1}{2\mathrm{\&gamma;}}{\left|\right|\mathrm{\&delta;}\left|\right|}^2,---\left(10\right)$

任给λ>0，γ>0，令 ${\mathrm{\&beta;}}_1-\frac{\mathrm{\&gamma;}}{2}\&GreaterEqual;\frac{\mathrm{\&lambda;}}{2},{\mathrm{\&beta;}}_2\&GreaterEqual;\frac{\mathrm{\&lambda;}}{2},{\mathrm{\&beta;}}_3\&GreaterEqual;\frac{\mathrm{\&lambda;}}{2}{m}_{\mathrm{\&psi;}},$ 则

$\stackrel{\&CenterDot;}{W}\&le;-\mathrm{\&lambda;W}+\frac{{\mathrm{\&Gamma;}}_1^{-1}}{2}{\widetilde{\mathrm{\&tau;}}}_{w1}^T{\widetilde{\mathrm{\&tau;}}}_{w1}+\frac{{\mathrm{\&Gamma;}}_2^{-1}}{2}{\widetilde{\mathrm{\&tau;}}}_{w2}^T{\widetilde{\mathrm{\&tau;}}}_{w2}+\frac{1}{2\mathrm{\&gamma;}}{\left|\right|\mathrm{\&delta;}\left|\right|}^2.---\left(11\right)$

根据proj的性质，可得

为有界的，因此，

$\stackrel{\&CenterDot;}{W}\&le;-\mathrm{\&lambda;W}+l.---\left(12\right)$

其中， $l=\frac{{\mathrm{\&Gamma;}}_1^{-1}}{2}{\widetilde{\mathrm{\&tau;}}}_{w1}^T{\widetilde{\mathrm{\&tau;}}}_{w1}+\frac{{\mathrm{\&Gamma;}}_2^{-1}}{2}{\widetilde{\mathrm{\&tau;}}}_{w2}^T{\widetilde{\mathrm{\&tau;}}}_{w2}+\frac{1}{2\mathrm{\&gamma;}}{\left|\right|\mathrm{\&delta;}\left|\right|}^2,$ 由比较引理得

$W\left(t\right)\&le;e^{-\mathrm{\&lambda;t}}W\left(0\right)+\frac{1}{\mathrm{\&lambda;}}.---\left(13\right)$

I即误差||e||收敛于半径为

的球域，***稳定。

本为六自由度欠驱动水下航行器设计了一种鲁棒跟踪控制器。考虑到欠驱动无法实现渐近稳定的特性，采取了令误差趋于极小值的方法，解决了Lyapunov函数的选择。对于有界的外部扰动，采用函数映射技术设计了控制器，实现了较好的跟踪效果及鲁棒性。

Claims (6)

1.一种用于六自由度欠驱动水下航行器的鲁棒跟踪控制器，其特征在于在控制器参数不确定的情况下，可以任意初始状态出发，自动地跟踪任意光滑的轨迹，并取得了良好的跟踪效果。

2.根据权利1所述的六自由度欠驱动水下航行器的鲁棒跟踪控制器，其特征在于航行器有如下的动态模型：

$\begin{array}{c}{\stackrel{\&CenterDot;}{\mathrm{\&eta;}}}_1=J_1\left({\mathrm{\&eta;}}_2\right)V_1\\ {\stackrel{\&CenterDot;}{\mathrm{\&eta;}}}_2=J_2\left({\mathrm{\&eta;}}_2\right)V_2\\ M_1{\stackrel{\&CenterDot;}{V}}_1=-M_{1}C\left(V_2\right)V_1-\stackrel{\&OverBar;}{V_1}{D}_1\left(V_1\right)+{\mathrm{\&tau;}}_1+{\mathrm{\&tau;}}_{\mathrm{\&omega;}1}\left(t\right)\\ M_2{\stackrel{\&CenterDot;}{V}}_2=-M_{2}C\left(V_1\right)V_1-C\left(V_2\right)V_2-D_2\left(V_2\right)V_2+{\mathrm{\&tau;}}_2+{\mathrm{\&tau;}}_{\mathrm{\&omega;}2}\left(t\right)\end{array}.$

3.根据权利1所述的六自由度欠驱动水下航行器的鲁棒跟踪控制器，其特征在于Lyapunov函数的选择为如下形式：

$W=\frac{1}{2}{e}^{T}e+\frac{1}{2{\mathrm{\&beta;}}_1^2}{(z_1-\mathrm{\&delta;})}^T(z_1-\mathrm{\&delta;})+\frac{{\mathrm{\&Gamma;}}_1^{-1}}{2}{\widetilde{\mathrm{\&tau;}}}_{w1}^T{\widetilde{\mathrm{\&tau;}}}_{w1}+\frac{1}{2}{z}_2^T{z}_2+\frac{{\mathrm{\&Gamma;}}_2^{-1}}{2}{\widetilde{\mathrm{\&tau;}}}_{w2}^T{\widetilde{\mathrm{\&tau;}}}_{w2}.$

4.根据权利1所述的六自由度欠驱动水下航行器的鲁棒跟踪控制器，其特征在于鲁棒控制器的设计中采用了函数映射技术。

5.根据权利1所述的六自由度欠驱动水下航行器的鲁棒跟踪控制器，其特征在于初始状态不受约束。

6.根据权利1所述的六自由度欠驱动水下航行器的鲁棒跟踪控制器，其特征在于轨迹的形状不受约束，只要求是光滑的，并且即使纵荡及转艏的速度皆为零，仍有良好跟踪效果。