降阶扩张状态观测器(LESO)在AUV运动控制中的运用

前言

      扩张状态观测器是处理复杂环境和干扰问题较好的工具，也是自抗扰控制（ADRC）中的重要手段，在解决不确定非线性的问题时得到了广泛的应用。其核心思路为：基于整个系统的输入信号和输出信号，对系统状态方程进行化简，得到积分串联型，将标准形式之外的项视为总的扰动项，然后用观测器对总的扰动项进行估计，随后反馈给控制系统。扩张状态观测器只需系统的输入和输出就可以对整个系统的全部状态变量进行估计，视总的干扰为系统状态的一部分，便可将总扰动估计并反馈给系统，从而增强整个系统的控制性能。
本文借助降阶扩张观测器对处于复杂环境下的水下机器人的运动进行分析，旨在抵抗外界干扰和模型的不确定摄动产生的影响。

一、状态观测器典型例子

      下面介绍一个典型的单输入单输出的二阶系统的观测器设计过程进行简单说明，对于一个带有不确定干扰的二阶系统，其状态方程表示为：
$$\{\begin{array}{ll}{\dot{x}}_1=x_2& \\ {\dot{x}}_2=f(x_1,x_2,w)+bu& \\ y=x_1& \end{array}$$其中$u$表示系统输入，整个$f(x_1,x_2,w)$视为总的扰动部分，既包含系统的非线性成分，又包含外界扰动，$w$为外界扰动，$y$为输出。由此建立的三阶观测器表达式为：
$$\{\begin{array}{ll}{\dot{z}}_1=z_2-l_1{e}_1& \\ {\dot{z}}_2=z_3+bu-l_2{e}_1& \\ {\dot{z}}_3=-l_3{e}_1& \end{array}$$上式中，$z_1$、$z_2$和$z_3$分别表示对$x_1$、$x_2$和$f(x_1,x_2,w)$的估计，$e_1=x_1-z_1$表示$x_1$的估计误差。

二、线性扩张状态观测器设计

      为处理欠驱动AUV所受到的复合干扰，通过相关知识设计了线性扩张状态观测器，将其纳入控制系统中，从使整个系统鲁棒性和对环境的适应能力提高。设计过程如下：
      首先定义干扰为：
$$\chi =\left[\begin{array}{c}{\chi }_u\\ {\chi }_v\\ {\chi }_w\\ {\chi }_q\\ {\chi }_r\end{array}\right]$$其中${\chi }_i$表示各个方向上的干扰，是欠驱动AUV模型摄动和海流干扰的总的不确定项，现假设干扰$\chi$是连续可微的。
      考虑复杂环境，运动学和动力学模型可以用矩阵表示为：
$$\{\begin{array}{ll}\dot{\eta }=S(\eta )v& \\ M\dot{v}+C(v)v+D(v)v+g(\eta )=\tau +{\tau }_w& \end{array}$$其中$\eta =[x,y,z,\theta ,\psi {]}^T$是位置与姿态向量，$S(\eta )$为定系和动系的转换矩阵，且$S^{-1}(\eta )=S^T(\eta ),v=[u,v,w,q,r{]}^T$为速度向量，$C(v)$为科氏向心力矩阵，$\tau$和${\tau }_w$分别表示控制输入和干扰项。
      将上式化简为：
$$\{\begin{array}{ll}\dot{\eta }=S(\eta )v& \\ \dot{v}=-M^{-1}(C(v)v+D(v)v+g(\eta ))+Bu+B\chi & \end{array}$$其中$B=M^{-1}$为控制输入矩阵。由上式所设计的线性干扰观测器的表达式为：
$$\{\begin{array}{ll}\dot{\hat{\eta }}=S(\eta )\hat{v}+k_1(\eta -\hat{\eta })& \\ \dot{\hat{v}}=-M^{-1}(C(\hat{v}))\hat{v}+D(\hat{v})\hat{v}+g(\eta ))+Bu+B\hat{\chi }+S^T(\eta )k_2(\eta -\hat{\eta })& \\ \dot{\hat{\eta }}=S^T（\eta )k_3(\eta -\hat{\eta })& \end{array}$$其中$\hat{\eta }、\hat{v}、\hat{\chi }$分别是对$\eta 、v、\chi$的估计，$k_1、k_2$和$k_3$为参数矩阵，即观测器的增益。以下将更加详尽地给出所设计地LESO，下面分别给出给矩阵等式的具体展开式：
$$\{\begin{array}{ll}\dot{\hat{x}}=\hat{u}cos\psi cos\theta +\hat{v}(cos\psi sin\theta sin\varphi -sin\psi cos\varphi )& \\ +\hat{w}(cos\psi sin\theta cos\varphi +sin\psi sin\varphi )+k_1(x-\hat{x})& \\ \dot{\hat{y}}=\hat{u}sin\psi cos\theta +\hat{v}(sin\psi sin\theta sin\varphi +cos\psi cos\varphi )& \\ +\hat{w}(sin\psi sin\theta cos\varphi -cos\psi sin\varphi )+k_2(z-\hat{z})& \\ \hat{\dot{z}}=-\hat{u}sin\theta +\hat{v}cos\theta sin\varphi +\hat{w}cos\theta cos\varphi +k_1(z-\hat{z})& \\ \dot{\hat{\theta }}=\hat{q}cos\varphi -\hat{r}sin\varphi +k_1(\theta -\hat{\theta })& \\ \dot{\hat{\psi }}=\hat{q}sin\varphi sec\theta +\hat{r}sin\varphi sec\theta +k_1(\psi -\hat{\psi })& \end{array}$$
$$\{\begin{array}{ll}\dot{\hat{u}}=\frac{1}{m_u}(F_u+{\chi }_u-\widehat{d_u})+k_2[(x-\hat{x})sin\psi cos\theta +(y-\hat{y})sin\psi cos\theta -(z-\hat{z})sin\theta ]& \\ \dot{\hat{v}}=\frac{1}{m_v}(\widehat{{\chi }_v}-m_{ur}\hat{u}\hat{r}-\widehat{d_v})& \\ +k_2[-(x-\hat{x})sin\psi +(y-\hat{y})cos\psi +(z-\hat{z})(cos\psi sin\theta cos\varphi -cos\psi sin\varphi )]& \\ \dot{\hat{w}}=\frac{1}{m_w}(\widehat{{\chi }_w}-m_{uq}\hat{u}\hat{q}-\widehat{d_w})+k_2[(x-\hat{x})sin\theta cos\psi +(y-\hat{y})sin\psi sin\theta +(z-\hat{z})cos\theta ]& \\ \dot{\hat{q}}=\frac{1}{m_q}(\widehat{{\chi }_q}-\widehat{d_q})+k_2(\theta -\hat{\theta })cos\varphi & \\ \dot{\hat{r}}=\frac{1}{m_r}(\widehat{{\chi }_r}-\widehat{d_r})+k_2(\psi -\hat{\psi })sec\psi & \end{array}$$
$$\{\begin{array}{ll}\dot{{\hat{\chi }}_u}=k_3[(x-\hat{x}sin\psi cos\theta +(y-\hat{y}sin\psi cos\theta -(z-\hat{z})sin\theta ]& \\ \dot{{\hat{\chi }}_v}=k_3[-(x-\hat{x})sin\psi +(y-\hat{y})cos\psi +(z-\hat{z})(cos\psi sin\theta cos\varphi -cos\psi sin\varphi )]& \\ \dot{{\hat{\chi }}_w}=k_3[(x-\hat{x})sin\theta cos\psi +(y-\hat{y})sin\psi sin\theta +(z-\hat{z})cos\theta ]& \\ \dot{{\hat{\chi }}_q}=k_3(\theta -\hat{\theta })cos\varphi & \\ \dot{{\hat{\chi }}_r}=k_3(\psi -\hat{\psi })sec\psi & \end{array}$$上式中${\hat{d}}_{()}$表示将$d_{()}$中的速度用估计速度代替后的项。