0 引　言

水下机器人（Autonomous Underwater Vehicle, AUV）近年来在海洋工程领域发挥着越来越重要的作用，被广泛应用在水下设备安装监控、大范围海洋监测以及自然资源勘探等领域。由于水下机器人数学模型存在强耦合、强非线性、水动力学系数不确定以及不确定海流干扰等问题，为保证AUV在复杂环境下能够正常工作，迫切需要易用且稳定性与鲁棒性良好的运动控制方法^[1-6]。

AUV的鲁棒控制方法是当前的研究热点之一，大量的工作主要集中在低自由度AUV的线性系统控制。B.Clement通过非线性补偿与卡尔曼滤波实现了AUV单自由度首向角的H_∞鲁棒控制结构，对比传统PID控制具有低过冲、稳定快的优点^[7]。Juan C. Cutipa-Luque在解耦的横移-首向2DOF AUV模型上应用H_∞鲁棒控制策略，取得了较好的控制效果^[8]。Wei Zhang等^[9]提出插值型H_∞鲁棒控制器，分别实现了速度、首向及深度控制器并给出了相应的实验结果，验证了控制器设计思路的有效性。AUV单自由度的鲁棒控制虽然易于单个控制过程实现，但是忽略了AUV实际上各自由度间的耦合，在实际应用上容易造成控制器设计逻辑复杂，控制环节无法有效平滑过渡以及控制器参数过多不便校准等问题。为解决AUV多自由度系统相互耦合、强非线性等问题，本文基于六自由度AUV工作点处的线性化方程，构建适用于鲁棒控制的不确定性模型，并据此设计方便实际应用的六自由度H_∞鲁棒控制器，得到了对应的仿真结果。

1 AUV六自由度非线性模型

通过对AUV进行刚体运动及所受外力分析，可以得到AUV的动力学及运动学模型。为研究方便，本文采用Kambara外形AUV的模型进行仿真^[10]。AUV的坐标系定义如图1所示，在此坐标系上，随体坐标系 ${O_0} - {X_0}{Y_0}{Z_0}$ 到大地坐标系 $O - XYZ$ 的速度与角速度变换关系可定义为：

$ {{{\dot \eta }}_1} = {{{J}}_1}\left( {{{{\eta }}_2}} \right){{{v}}_1}{\text{，}} $

(1)

${{{\dot \eta }}_2} = {{{J}}_2}\left( {{{{\eta }}_2}} \right){{{v}}_2}{\text{。}} $

(2)

其中 ${\eta _1} {\rm{ = }}{\left[ x\;\;\;\;y\;\;\;\;z \right]^{\rm{T}}}$ ， ${\eta _2} ={\left[ \phi \;\;\;\;\theta\;\;\;\;\psi \right]^{\rm{T}}}$ ， ${v_1} = {\left[ u\;\;\;\;v\;\;\;\;w\right]^{\rm{T}}}$ ， ${v_2} = {\left[ p\quad q\quad r \right]^{\rm{T}}}$ 。 $\left( x\quad y\quad z\quad \phi \quad \theta \quad \psi \right)$ 分别为大地坐标系 ${X_0}$ 方向位置， ${Y_0}$ 方向位置， ${Z_0}$ 方向位置，横滚角，纵倾角，首向角； $\left( u\quad v\quad w\quad p\quad q\quad r \right)$ 分别为随体坐标系速度与角速度分量。 ${{ J}_1}\left( {{\eta _2}} \right)$ 和 ${{ J}_2}\left( {{\eta _2}} \right)$ 分别为速度与角速度转换矩阵。

${{A}}=-{{{M}}}^{-1}\left({F}_{c}+{F}_{d}\right)$ 、 ${{B}} = {{{M}}^{ - 1}}$ ：

全驱动形式AUV的六自由度动力学模型可表示为：

${{{M}}_1}{\dot v_1} = - {{{C}}_1}\left( {{v_1}} \right){v_2} - {{{D}}_1}\left( {{v_1}} \right){v_1} - {g_1}\left( {{\eta _2}} \right) + {\tau _1}{\text{，}}$

(3)

$ {{{M}}_2}{\dot v_2} \!=\! - {{{C}}_1}\left( {{v_1}} \right){v_1} \!-\! {C_2}\left( {{v_2}} \right){v_2} \!-\! {{{D}}_2}\left( {{v_2}} \right){v_2} \!-\! {g_2}\left( {{\eta _2}} \right) \!+\! {\tau _2}{\text{。}}\!\!\!\!\!\! $

(4)

其中： ${{{M}}_1}$ ， ${{{M}}_2}$ 为惯性矩阵； ${{{C}}_1}$ ， ${{{C}}_2}$ 为科氏矩阵； ${{{D}}_1}$ ， ${{{D}}_2}$ 为阻尼矩阵； ${g_1}$ ， ${g_2}$ 分别为恢复力及恢复力矩； ${\tau _1} = $ ${\left[ {{\tau _u}}\quad {{\tau _v}}\quad {{\tau _w}} \right]^{\rm{T}}}$ ， ${\tau _2} = \left[ {{\tau _p}}\quad {{\tau _q}}\quad {{\tau _r}} \right]^{\rm{T}}$ 分别为各推进器产生推力及推力矩。

令 ${{v}} = {\left[ {v_1^T}\quad {v_2^T} \right]^{\rm{T}}}$ ， ${{\tau }} = {\left[ {\tau _1^T}\quad {\tau _2^T} \right]^{\rm{T}}}$ 以及

$ M= \left[\begin{array}{cc}{M}_{1}& O\\ O& {M}_{2}\end{array}\right]{\text{，}} $

(5)

$ C\left( {{v}} \right) = \left[ {\begin{array}{*{20}{c}} O&{{C_1} \left( {{v_1} } \right)} \\ {{C_1} \left( {{v_1} } \right)}&{{C_2} \left( {{v_2} } \right)} \end{array}} \right]{\text{，}} $

(6)

$ D\left( {{v}} \right) = \left[ {\begin{array}{*{20}{c}} {{D_1}\left( {{v_1}} \right)}&O \\ O&{{D_2}\left( {{v_2}} \right)} \end{array}} \right] {\text{，}} $

(7)

$ g\left( {{\eta _2}} \right) = \left[ {\begin{array}{*{20}{c}} {{g_1}\left( {{\eta _2}} \right)} \\ {{g_2}\left( {{\eta _2}} \right)} \end{array}} \right]{\text{，}} $

(8)

则AUV六自由度方程可以简化为Fossen形式^[11]，且 ${{M}}$ 可逆，AUV六自由度非线性模型为：

${{M}}{\dot v}{\rm{ = }} - {{{C}}}\left( {v} \right){v} - {{D}}\left( {v} \right){v + }g\left( {{\eta _2}} \right) + {\tau }{\text{。}} $

(9)

2 六自由度AUV工作点处不确定性模型

本文所提出的六自由度AUV不确定性模型建立在AUV工作点处的线性化方程以及鲁棒控制中的加性不确定性模型之上，并最终在频域上对其进行描述，得到耦合性未丢失且方便处理的多自由度控制模型。

为便于处理，定义模型控制输入为 ${{\tau }_{{e}}} = {\tau } - g\left( {{\eta _2}} \right)$ ，并且式（9）中 ${{C}}\left( {v} \right)$ ， ${{D}}\left( {v} \right)$ 分别具有如下形式的非线性项：

$ {{C}}\left( {{v}} \right) \!=\! \left[\!\!\!\! {\begin{array}{*{20}{c}} 0\!\!&\!\!0\!\!&\!\!0\!\!&\!\!0\!\!&\!\!{{m_{33}}w}\!\!&\!\!{ - {m_{22}}v} \\ 0\!\!&\!\!0\!\!&\!\!0\!\!&\!\!{ - {m_{33}}w}\!\!&\!\!0\!\!&\!\!{{m_{11}}u} \\ 0\!\!&\!\!0\!\!&\!\!0\!\!&\!\!{{m_{22}}v}\!\!&\!\!{ - {m_{11}}u}\!\!&\!\!0 \\ 0\!\!&\!\!{{m_{33}}w}\!\!&\!\!{ - {m_{22}}v}\!\!&\!\!0\!\!&\!\!{{m_{66}}r}\!\!&\!\!{ - {m_{55}}q} \\ { - {m_{22}}v}\!\!&\!\!0\!\!&\!\!{{m_{11}}u}\!\!&\!\!{ - {m_{66}}r}\!\!&\!\!0\!\!&\!\!{{m_{44}}p} \\ {{m_{22}}v}\!\!&\!\!{ - {m_{11}}u}\!\!&\!\!0\!\!&\!\!{{m_{55}}q}\!\!&\!\!{ - {m_{44}}p}\!\!&\!\!0 \end{array}} \!\!\!\!\right]{\text{，}} $

(10)

$ \begin{split} {{D}}\left( {{v}} \right) =& {\bf{diag}}\left(\!\! {\begin{array}{*{20}{c}} {{d_u}}&{{d_v}}&{{d_w}}&{{d_p}}&{{d_q}}&{{d_r}} \end{array}}\!\! \right) + \\ &{\bf{diag}}\left( \!\!{\begin{array}{*{20}{c}} {{d_{u1}}\left| u \right|}\!\!&\!\!{{d_{v1}}\left| v \right|}\!\!\!&\!\!\!{{d_{w1}}\left| w \right|}\!\!\!&\!\!\!{{d_{p1}}\left| p \right|}\!\!\!&\!\!\!{{d_{q1}}\left| q \right|}\!\!\!&\!\!\!{{d_{r1}}\left| r \right|} \end{array}} \!\!\right) =\\ & {{{D}}_l}\left( {{v}} \right) + {{{D}}_n}\left( {{v}} \right){\text{。}}\\[-15pt] \end{split} $

(11)

式中： ${m_{ii}}$ ， ${d_j}$ 均为模型常数。对于 ${{D}}\left( {{v}} \right)$ 中的非线性阻尼项 ${{{D}}_n}\left( {{v}} \right)$ ，考虑到AUV最大速度小于 $0.8\;{\rm{m/s}}$ ，最大转动角速度小于 $0.5\;{\rm{rad/s}}$ ，通过最大速度界函数 ${{{D}}_n}\left( {{{{v}}_{max}}} \right)$ 近似非线性阻尼项使其满足范数意义下 $\left\| {{{{D}}_n}\left( {{{{v}}_{\max }}} \right)} \right\| \geqslant$ $\left\| {{{{D}}_n}\left( {{v}} \right)} \right\|$ 。

模型的线性化方程由文献[8]给出，在给定工作点 ${{{v}}_0} = \left( {\begin{array}{*{20}{c}} {{u_0}}&{{v_0}}&{{w_0}}&{{p_0}}&{{q_0}}&{{r_0}} \end{array}} \right)$ 处，令

$ {{v}_{{e}}} = {v}\left( t \right) - {{v}_{{0}}}\left( t \right){\text{，}} $

(12)

$ {f_c}\left( {{v}} \right) = {{C}}\left( {{v}} \right){{v}}\left( t \right){\text{，}} $

(13)

$ {f_d}\left( {{v}} \right) = {{{D}}_l}\left( {{v}} \right){{v}}\left( t \right){\text{，}} $

(14)

$ {{{K}}_d} = \left\| {{{{D}}_n}\left( {{{{v}}_{\max }}} \right)} \right\|{\text{，}} $

(15)

则式（9）在 ${{v}_0}$ 处的线性化方程可表示为：

$ {{M}}{{\dot v}_{{e}}} + {\left. {\frac{{\partial {f_c}\left( {v} \right)}}{{\partial {v}}}} \right|_{{{v}_{{0}}}}}{{v}_{{e}}} + {\left. {\frac{{\partial {f_d}\left( {v} \right)}}{{\partial {v}}}} \right|_{{{v}_{{0}}}}}{{v}_{{e}}} + {{{K}}_d}{{v}_{{e}}} = {{\tau }_{{e}}}{\text{。}} $

(16)

令 ${F_c} = {\left. {\dfrac{{\partial {f_c}\left( {v} \right)}}{{\partial {v}}}} \right|_{{{v}_0}}}$ ， ${F_d} = {\left. {\dfrac{{\partial {f_d}\left( {v} \right)}}{{\partial {v}}}} \right|_{{{v}_0}}}$ ，则不考虑非线性阻尼项的线性标称系统 $ G $ 可表示为：

$ M{{\dot v}_{{e}}} + {F_c}{{v}_{{e}}} + {F_d}{{v}_{{e}}} = {{\tau }_{{e}}}{\text{，}} $

(17)

式中：

$\begin{split} &{{A}}=-{{{M}}}^{-1}\left({F}_{c}+{F}_{d}\right){\text{，}}\\ &{{B}} = {{{M}}^{ - 1}}{\text{，}}\\ &{{\dot v}_{{e}}} = {{A}}{{v}_{{e}}} + {{B}}{{\tau }_{{e}}}{\text{。}} \end{split} $

(18)

将非线性阻尼项 ${K_d}$ 视为模型不确定性因素，不确定性系统 ${G_0}$ 可表示为：

$ M{{\dot v}_{{e}}} + {F_c}{{v}_{{e}}} + {F_d}{{v}_{{e}}} + {K_d}{{v}_{{e}}} = {{\tau }_{{e}}}{\text{，}} $

(19)

将 $ - {M^{ - 1}}{K_d}$ 视为 $\Delta A$ ，则不确定性系统可表示为：

$ {{\dot v}_{{e}}} = \left( {A + \Delta A} \right){{v}_{{e}}} + B{{\tau }_{{e}}}{\text{，}} $

(20)

将不确定性因素考虑为加性不确定性，则不确定性系统 ${G_0}$ 与标称系统 $G$ 满足如下关系：

$ {G}_{0}\left(s\right)=G\left(s\right)+\Delta \left(s\right){W}_{d}\left(s\right) ,{\Vert \Delta \Vert }_{\infty }\leqslant 1 {\text{。}} $

(21)

其中 ${W_d}\left( s \right)$ 为描述系统不确定性的界函数， $\Delta \left( s \right)$ 为系统未知摄动。系统加性摄动模型如图2所示。

由于本系统为六自由度高阶系统，保持结果准确并且降低系统模型复杂性，可通过传递函数差值的频率响应得到满足要求的2阶加性摄动界函数 ${W_d}\left( s \right)$ ，使得该界函数满足：

$\left| {{G_0}\left( {j\omega } \right) - G\left( {j\omega } \right)} \right| = \left| {\Delta \left( s \right){W_d}\left( s \right)} \right| \leqslant \left| {{W_d}\left( s \right)} \right|{\text{，}}$

(22)

根据文献[10]中的六自由度非线性AUV模型，可得到在工作点 ${{v}_0}$ 处展开的系统参数如下：

$ \begin{split}&{{{F}}_c} = \\ &\left[\!\!\!\!\! {\begin{array}{*{20}{c}} 0\!\!\!\!\!&\!\!\!\!\!{ - 140.8{r_0}}\!\!\!\!\!&\!\!\!\!\!{140.8{q_0}}\!\!\!\!\!&\!\!\!\!\!0\!\!\!\!\!&\!\!\!\!\!{140.8{w_0}}\!\!\!\!\!&\!\!\!\!\!{ - 140.8{v_0}} \\ {175.4{r_0}}\!\!\!\!\!&\!\!\!\!\!0\!\!\!\!\!&\!\!\!\!\!{ - 140.8{p_0}}\!\!\!\!\!&\!\!\!\!\!{ - 140.8{w_0}}\!\!\!\!\!&\!\!\!\!\!0\!\!\!\!\!&\!\!\!\!\!{175.4{u_0}} \\ { - 175.4{q_0}}\!\!\!\!\!&\!\!\!\!\!{140.8{p_0}}\!\!\!\!\!&\!\!\!\!\!0\!\!\!\!\!&\!\!\!\!\!{140.8{v_0}}\!\!\!\!\!&\!\!\!\!\!{ - 175.4{u_0}}\!\!\!\!\!&\!\!\!\!\!0 \\ 0\!\!\!\!\!&\!\!\!\!\!0\!\!\!\!\!&\!\!\!\!\!0\!\!\!\!\!&\!\!\!\!\!0\!\!\!\!\!&\!\!\!\!\!{3.09{r_0}}\!\!\!\!\!&\!\!\!\!\!{3.09{q_0}} \\ {34.6{w_0}}\!\!\!\!\!&\!\!\!\!\!0\!\!\!\!\!&\!\!\!\!\!{34.6{u_0}}\!\!\!\!\!&\!\!\!\!\!{ - 1.99{r_0}}\!\!\!\!\!&\!\!\!\!\!0\!\!\!\!\!&\!\!\!\!\!{ - 1.99{p_0}} \\ { - 34.6{v_0}}\!\!\!\!\!&\!\!\!\!\!{ - 34.6{u_0}}\!\!\!\!\!&\!\!\!\!\!0\!\!\!\!\!&\!\!\!\!\!{ - 1.1{q_0}}\!\!\!\!\!&\!\!\!\!\!{ - 1.1{p_0}}\!\!\!\!\!&\!\!\!\!\!0 \end{array}} \!\!\!\!\!\right]{\text{，}} \end{split}$

(23)

$ {{{F}}}_{d}=\left[\begin{array}{cccccc}120& 0& 0& 0& 0& 0\\ 0& 90& 0& 0& 0& 0\\ 0& 0& 150& 0& 0& 0\\ 0& 0& 0& 15& 0& 0\\ 0& 0& 0& 0& 15& 0\\ 0& 0& 0& 0& 0& 18\end{array}\right] {\text{。}} $

(24)

为不失一般性，在 ${{v}_0} \!=\! ( {0.4}\;\;\,{0.4}\;\;\,{0.4}\;\;\,{0.25}\;\;\,{0.25}\;\;\,{0.25} )$ 工作点处展开，通过式（22）可求得加性摄动界函数 ${W_d}\left( s \right)$ 。不确定系统在 ${{v}_0}$ 处的奇异值如图3所示。

3 AUV线性模型的H_∞控制器

本文采用回路成形设计方法得到六自由度AUV不确定性模型H_∞控制器，设计时采用经典的闭环控制结构如图4所示。其中r为参考输入，d与n分别是输出扰动与传感器噪声，y为系统输出。

开环传递传递函数 $L\left( s \right) \!=\! {G_0}\left( s \right)K\left( s \right)$ ，定义敏感函数 $S\left( s \right) \!=\! {\left( {I \!+\! L\left( s \right)} \right)^{ - 1}}$ 及补敏感函数 $T\left( s \right) = L\left( s \right){\left( {I + L\left( s \right)} \right)^{ - 1}}$ 。敏感函数与补敏感函数满足 $ S\left(s\right)+ T\left(s\right)=I$ ，则闭环输入输出结构中的传递函数关系满足：

$ y\left( s \right) = T\left( s \right)r\left( s \right) + S\left( s \right)d\left( s \right) - T\left( s \right)n\left( s \right) {\text{。}} $

(25)

得到的控制器 $K\left( s \right)$ 在满足将系统 ${G_0}\left( s \right)$ 镇定的同时，根据公式：

1）若系统有较强的抗干扰能力，应当使 $\bar \sigma \left( {S\left( {j\omega } \right)} \right)$ 较小；

2）若系统具有较强的噪声消除能力，应当使 $\bar \sigma \left( {T\left( {j\omega } \right)} \right)$ 较小；

3）若系统具有较强的信号跟踪能力，应当使 $\bar \sigma \left( {T\left( {j\omega } \right)} \right) \approx 1$ 。

其中 $\bar \sigma \left( \bullet \right)$ 为系统最大奇异值。由上述条件可知为保证系统性能，各条件不能同时满足，需要折中处理。回路成形的控制器设计即是通过开环传递函数 $L\left( s \right)$ 来找到控制器 $K$ ，从而使得受到开环传递函数影响的敏感函数与补敏感函数分别在低频段满足控制性能要求，在高频段满足鲁棒稳定性要求^[12]，即

1）在低频段范围使 $\left| {L\left( {j\omega } \right)} \right| > > 1$ ，以保证控制器对低频干扰不敏感且具有良好信号跟踪能力；

2）在高频段范围使 $\left| {L\left( {j\omega } \right)} \right| < < 1$ ，以保证控制器有良好的高频噪声抑制能力。

满足条件的控制器由如下定理^[13]确定：

定理　设 $G$ 与 $K$ 分别为控制对象与控制器， ${\rm{\zeta }}$ 为全体稳定实有理传递函数集合， $G\left( s \right) = \dfrac{{N\left( s \right)}}{{M\left( s \right)}}$ 为 $G$ 在 ${\rm{\zeta }}$ 中的一个互质分解。 $X,Y \in \zeta $ ，满足 $XN + YM = 1$ ，则使闭环系统内稳定的所有控制器 $K$ 的集合为：

$ \xi = \left\{ {\left. {\begin{array}{*{20}{c}} {C = \dfrac{{X + MQ}}{{Y - NQ}}} \end{array}} \right|Q \in \zeta } \right\}{\text{。}} $

其中 $Q$ 为控制器参数。

本文计算得到的控制器用于在 ${{{v}}_0} = ( {0.4}\;\;{0.4}\;\;{0.4}\;\; {0.25}\;\;{0.25}\;\;{0.25} )$ 工作点的六自由度AUV系统，该控制器在回路成形计算时选取了 ${G_d} = 100/s$ 作为目标曲线进行成形计算，得到控制器 $K\left( s \right)$ 的奇异值如图5所示。该六自由度控制器在低频段具有更大的控制作用以增强系统的跟踪性能。该控制器求解得到的开环传递函数 $L\left( s \right)$ 的结果如图6所示，可以看出该曲线很好满足了开环传递函数目标曲线。在低频段开环传递函数的奇异值远大于1，在频率大于 $100\;{\rm rad/s}$ 后开环传递函数奇异值开始快速减小以提高对高频干扰噪声的抑制能力。

4 H_∞控制器仿真结果

将控制器作用于给定工作点 ${{{v}}_0}$ 后的六自由度AUV系统，图7与图8分别给出了作用控制器后闭环系统的敏感函数曲线以及补敏感函数曲线。得到的闭环系统敏感函数在高频奇异值减小，体现了鲁棒控制器在高频对干扰信号的抑制能力已经得到保证。补敏感函数曲线中低频段曲线集中在分贝值0 dB附近，表示系统在低频段具有良好的参考信号跟踪能力。而且，低频段的补敏感函数频带较宽，表示控制器具有较好的性能鲁棒性。

图9～图11给出了闭环系统在工作点 ${{{v}}_0}$ 时，在u方向输入0.15 m/s正弦信号，w方向输入−0.15 m/s，同时在u，v，w方向均施加频率为30 rad/s，幅值为0.015 m/s的正弦干扰信号，其余参考信号均为0的情况下鲁棒控制器与PID控制器六自由度响应对比曲线。从图中可以看出，鲁棒控制器在更短的时间内满足了阶跃信号的输入要求，并在10%幅值的正弦扰动下仍旧能够保持稳定性，表示该鲁棒控制器具有良好的信号跟踪能力。通过干扰抑制的数据对比，鲁棒控制器相比PID控制器对高频干扰有更强的抑制能力。同时，由于本系统为耦合系统，从图中可以看出非参考信号的自由度之间耦合影响不大，说明该鲁棒控制器具有优良的抗干扰能力。

5 结　语

本文通过对自由度间相互耦合、强非线性的AUV模型在工作点附近进行了线性化，给出了该线性化模型的操作过程及结果，并据此建立了适合应用鲁棒控制方法的加性不确定性模型。使用回路成形方法得到了对应工作点的鲁棒控制器，对闭环系统的仿真试验取得了良好的控制效果，验证了回路成形方法设计的鲁棒控制器在多自由度AUV系统中使用的有效性，未来可以针对多个工作点的鲁棒控制器连续工作做进一步的研究。