﻿ 多约束模型预测控制在AUV深度控制中的应用
 舰船科学技术  2021, Vol. 43 Issue (3): 120-124    DOI: 10.3404/j.issn.1672-7649.2021.03.023 PDF

1. 海军装备部驻武汉地区军事代表局驻九江地区军事代表室，江西 九江 332007;
2. 天津航海仪器研究所九江分部，江西 九江 332007

Application research of multi-constrained model predictive control in AUV depth control
YANG De-cheng1, GUO Yi-ping2, REN Yuan-zhou2, WANG Yi-min2
1. Jiujiang Military Representative Office of Military Representative Bureau in Wuhan of the PLA Naval Equipment Department, Jiujiang 332007, China;
2. Jiujiang Branch of Tianjin Nautical Instrument Research Institute, Jiujiang 332007, China
Abstract: An algorithm that converts depth deviation into command pitch is proposed aiming at AUV depth control. Based on the model predictive control theory of Laguerre network approximation, the model predictive control of pitch motion under the conditions of hard constraints of rudder angle, rudder speed and soft constraints of pitch is designed. The simulation results indicate that the control method has good dynamic control performance, and can quickly handle the quadratic programming problems with a variety of constraints.
Key words: AUV     model predictive control     motion control     pitch     MPC
0 引　言

1 多约束条件下AUV深度运动控制算法设计 1.1 深度控制策略

AUV的深度控制可采用深度—纵倾控制策略[8]，实现深度机动和深度保持，其关键是AUV的纵倾控制。

AUV的深度—纵倾内外环控制策略如图1所示。

 图 1 深度—纵倾内外环控制策略框图 Fig. 1 Depth-Block diagram of inner and outer pitch control strategy
1.2 深度偏差—指令纵倾转换算法

 \left\{ \begin{aligned} {\theta _z} =& 0,\begin{array}{*{20}{c}} {}&{}&{\left| {\eta - {\eta _z}} \right| \leqslant a} \text{,} \end{array} \\ {\theta _z} =& 2{\rm{sign}}(\eta - {\eta _z}){\theta _z}_{\max }{\left(\dfrac{{\left| {\eta - {\eta _z}} \right| - a}}{{b - a}}\right)^2}\text{,}\\ & \begin{array}{*{20}{c}} {a < \left| {\eta - {\eta _z}} \right| \leqslant \dfrac{{(a + b)}}{2}} \text{,} \end{array} \\ {\theta _z} =& {\rm{sign}}(\eta - {\eta _z}){\theta _z}_{\max }\left(1 - 2{\left(\dfrac{{\left| {\eta - {\eta _z}} \right| - a}}{{b - a}}\right)^2}\right)\text{,}\\& \begin{array}{*{20}{c}} {\dfrac{{(a + b)}}{2} < \left| {\eta - {\eta _z}} \right| \leqslant b} \text{,} \end{array} \\ {\theta _z} =& {\rm{sign}}(\eta - {\eta _z}){\theta _z}_{\max }\begin{array}{*{20}{c}} {\left| {\eta - {\eta _z}} \right| > b} \text{。} \end{array} \\ \end{aligned} \right. (1)

${\theta _z}_{\max } = {20^ \circ }$ ，深度偏差变化范围 $[ - 45,45]$ 为例，则深度偏差转换为指令纵倾的曲线图如图2所示。

 图 2 深度偏差—指令纵倾转换曲线 Fig. 2 Depth deviation-commanded pitch conversion curve
2 多约束条件下的纵倾模型预测控制算法

AUV垂直面运动线性方程可用状态空间方程描述如下：

 $\left[ {\begin{array}{*{20}{c}} {\dot w} \\ {\dot q} \\ {\dot \theta } \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {{{\rm{a}}_{11}}}&{{a_{12}}}&{{a_{13}}} \\ {{a_{21}}}&{{a_{22}}}&{{a_{23}}} \\ 0&1&0 \end{array}} \right]\left[ {\begin{array}{*{20}{c}} w \\ q \\ \theta \end{array}} \right] + \left[ {\begin{array}{*{20}{c}} {{b_1}} \\ {{b_2}} \\ 0 \end{array}} \right]{\delta _s}\text{,}$ (2)
 $y = \left[ {\begin{array}{*{20}{c}} 0&0&1 \end{array}} \right]\left[ {\begin{array}{*{20}{c}} w \\ q \\ \theta \end{array}} \right]\text{,}$ (3)

 $\begin{split} \dot x &= Ax + B\delta\text{,} \\ y &= Cx\text{,} \\ \end{split}$ (4)

 $\begin{split} {x_m}(k + 1) &= {A_m}{x_m}(k) + {B_m}u(k) \text{,} \\ y(k) &= {C_m}{x_m}(k) \text{,} \\ \end{split}$ (5)

 $\begin{split} {x_m}\left( {k + 1} \right) - {x_m}\left( k \right) &= {A_m}\left( {{x_m}\left( k \right) - {x_m}\left( {k - 1} \right)} \right) +\\ &\quad{B_m}\left( {u\left( k \right) - u\left( {k - 1} \right)} \right)\text{,} \\ y\left( {k + 1} \right) - y\left( k \right) &= {C_m}\left( {{x_m}\left( {k + 1} \right) - {x_m}\left( k \right)} \right) =\\ & Cm\Delta {x_m}\left( {k + 1} \right) \text{。} \\ \end{split}$ (6)

 $\begin{split} {\Delta {x_m}\left( {k + 1} \right) =}&{ {x_m}\left( {k + 1} \right) - {x_m}\left( k \right)},\\ {\Delta {x_m}\left( k \right) }= &{x_m}\left( k \right) - {x_m}\left( {k - 1} \right)\text{,} \\ {\Delta u\left( k \right)} =&{ u\left( k \right) - u\left( {k - 1} \right)}\text{,} \end{split}$ (7)

 $\begin{split} & {\left[ {\begin{array}{*{20}{c}} {\Delta {x_m}\left( {k + 1} \right)} \\ {y\left( {k + 1} \right) } \end{array}} \right]} = \left[ {\begin{array}{*{20}{c}} {{A_m}}&0 \\ {{C_m}{A_m}}&1 \end{array}} \right] \left[ {\begin{array}{*{20}{c}} {\Delta {x_m}\left( k \right)} \\ {y\left( k \right) } \end{array}} \right] + \\ &\quad\quad\quad\quad\quad\quad\quad\;\;\left[ {\begin{array}{*{20}{c}} {{B_m} } \\ {{C_m}{B_m}} \end{array}} \right] \Delta u\left( k \right) \text{,}\\ & {y\left( k \right) = \left[ {\begin{array}{*{20}{c}} 0&0&1 \end{array}} \right]} \left[ {\begin{array}{*{20}{c}} {\Delta {x_m}\left( k \right)} \\ {y\left( k \right) } \end{array}} \right] \text{,} \end{split}$ (8)

 $\begin{split} {x\left( {k + 1} \right) }&{= Ax\left( k \right) + B\Delta u\left( k \right)}\text{,} \\ {y\left( k \right) }&{= Cx\left( k \right)} \text{,} \end{split}$ (9)

 $x(k) = {[\Delta x(m),y - r({k_i})]^{\rm{T}}}\text{,}$ (10)

 $\Delta u\left( {k + m} \right) = L{\left( m \right)^{\rm T}}\eta \text{,}$ (11)

Laguerre函数向量具有下述正交特性：

 $\begin{gathered} \sum\limits_{m = 0}^{{N_p}} {{l_i}(m){l_j}(m) = 0} \begin{array}{*{20}{c}} {}&{i \ne j} \text{,} \end{array} \\ \sum\limits_{m = 0}^{{N_p}} {{l_i}(m){l_j}(m) = 1} \begin{array}{*{20}{c}} {}&{i = j} \text{,} \end{array} \\ \end{gathered}$ (12)
 $L(k + 1) = {A_l}L(k)\text{。}$ (13)

 $\begin{split} &{A_l} = \left[ {\begin{array}{*{20}{c}} \alpha &0&0\\ \beta &\alpha &0\\ { - \alpha \beta }&\beta &\alpha \\ {{\alpha ^2}\beta }&{ - \alpha \beta }&\beta \\ { - {\alpha ^3}\beta }&{{\alpha ^2}\beta }&{ - \alpha \beta } \\ \vdots & \vdots & \vdots \\ {( - {1^{N - 2}}){\alpha ^{N - 2}}\beta }&{( - {1^{N - 3}}){\alpha ^{N - 3}}\beta }&{( - {1^{N - 4}}){\alpha ^{N - 4}}\beta } \end{array}} \right.\\ &\qquad\left. {\begin{array}{*{20}{c}}0&0& \cdots &0\\ 0&0& \cdots &0\\ 0&0& \cdots &0\\ \alpha &0& \cdots &0\\ \beta &\alpha & \cdots &{}\\ \vdots & \vdots & \ddots &{} \\ {( - {1^{N - 5}}){\alpha ^{N - 5}}\beta }& \cdots & \cdots &\alpha \end{array}}\right],\;\\ &L(0) =\sqrt \beta \left[ {\begin{array}{*{20}{c}} 1 \\ { - \alpha } \\ {{\alpha ^2}} \\ { - {\alpha ^3}} \\ {{\alpha ^4}} \\ \vdots \\ {{{( - 1)}^{N - 1}}\alpha } \end{array}} \right] ,\;\beta = (1 - {\alpha ^2})\text{。} \end{split}$

 $\begin{split} {x\left( {k + m/k} \right) }=&{ {A^m}x\left( k \right) + \sum\limits_{j = 0}^{m - 1} {{A^{m - j - 1}}} BL{{\left( j \right)}^{\rm{T}}}\eta }=\\& { {A^m}x\left( k \right) + \phi {{\left( m \right)}^{\rm{T}}}\eta }\text{,} \\ \phi {\left( m \right)^{\rm{T}}} = &\sum\limits_{j = 0}^{m - 1} {{A^{m - j - 1}}} BL{\left( j \right)^{\rm{T}}}\text{。} \end{split}$ (14)

 $J = \sum\limits_{m = 1}^{{N_p}} {x{{({k_i} + m|{k_i})}^{\rm{T}}}Q} x({k_i} + m|{k_i}) + \Delta {u^{\rm{T}}}R\Delta u\text{,}$ (15)

 $J = \sum\limits_{m = 1}^{{N_p}} {x{{({k_i} + m|{k_i})}^{\rm{T}}}Q} x({k_i} + m|{k_i}) + {\eta ^{\rm{T}}}R\eta\text{,}$ (16)

 $\begin{split} J =& {\eta ^{\rm{T}}}\left(\sum\limits_{m = 1}^{{N_p}} {\phi (m)} Q\phi {{(m)}^{\rm{T}}} + {R_L}\right)\eta + \\[-2pt] &2{\eta ^{\rm{T}}}\left(\sum\limits_{m = 1}^{{N_p}} {\phi (m)} Q{A^m}\right)x(k) + \sum\limits_{m = 1}^{{N_p}} {x{{(k)}^{\rm{T}}}} {{({A^{\rm{T}}})}^m}Q{A^m}x(k) =\\[-2pt] & {\eta ^{\rm{T}}}\Omega \eta {\kern 1pt} + {\kern 1pt} {\kern 1pt} 2{\eta ^{\rm{T}}}\Phi x(k) + \sum\limits_{m = 1}^{{N_p}} {x{{(k)}^{\rm{T}}}} {{({A^{\rm{T}}})}^m}Q{A^m}x(k) \text{。}\\[-20pt] \end{split}$ (17)

1）舵角限制

 ${u^{\min }} \leqslant u(k) \leqslant {u^{\max }}\text{。}$

 ${u^{\min }} \leqslant \sum\limits_{i = 0}^{m - 1} {{L_1}{{(i)}^{\rm T}}} \eta + u({k_i} - 1) \leqslant {u^{\max }}\text{,}$ (18)

$m = 1,2, \cdots ,{n_c}$ 对应的舵角约束整理成矩阵不等式，则可得：

 $\begin{split} &\underbrace {\left[ \begin{gathered} L{(0)^{\rm{T}}} \\ \sum\limits_{i = 0}^1 {L{{(i)}^{\rm{T}}}} \\ \vdots \\ \sum\limits_{i = 0}^{m - 1} {L{{(i)}^{\rm{T}}}} \\ \end{gathered} \right]}_{{M_u}}\eta \leqslant \underbrace {\left[ \begin{gathered} {u^{\max }} - u({k_i} - 1) \\ {u^{\max }} - u({k_i} - 1) \\ \vdots \\ {u^{\max }} - u({k_i} - 1) \\ \end{gathered} \right]}_{{U^{\max }} - \bar u({k_i} - 1)},\\ &- \underbrace {\left[ \begin{gathered} L{(0)^{\rm{T}}} \\ \sum\limits_{i = 0}^1 {L{{(i)}^{\rm{T}}}} \\ \vdots \\ \sum\limits_{i = 0}^{m - 1} {L{{(i)}^{\rm{T}}}} \\ \end{gathered} \right]}_{{M_u}}\;\;\eta \leqslant \underbrace {\left[ \begin{gathered} {u^{\min }} + u({k_i} - 1) \\ {u^{\min }} + u({k_i} - 1) \\ \vdots \\ {u^{\min }} + u({k_i} - 1) \\ \end{gathered} \right]}_{ - {U^{\min }} + \bar u({k_i} - 1)}, \end{split}$

 $\begin{gathered} {M_u}\eta \leqslant {U^{\max }} - \bar u({k_i} - 1)\text{,} \\ - {M_u}\eta \leqslant - {U^{\min }} + \bar u({k_i} - 1) \text{。} \\ \end{gathered}$ (19)

2）舵速限制

 $\Delta {U_{\min }} \leqslant \Delta U \leqslant \Delta {U_{\max }}\text{。}$

 $\Delta {u^{\min }} \leqslant L{(m)^{\rm{T}}}\eta \leqslant \Delta {u^{\max }}\text{,}$ (20)

 $\begin{split} \underbrace {\left[ \begin{gathered} L{(1)^{\rm{T}}} \\ L{(2)^{\rm{T}}} \\ \vdots \\ L{({n_c})^{\rm{T}}} \\ \end{gathered} \right]}_{{M_\Delta }}\eta \leqslant \underbrace {\left[ \begin{gathered} \Delta {u^{\max }} \\ \Delta {u^{\max }} \\ \vdots \\ \Delta {u^{\max }} \\ \end{gathered} \right]}_{\Delta {U^{\max }}}, \underbrace {\left[ \begin{gathered} - L{(1)^{\rm{T}}} \\ - L{(2)^{\rm{T}}} \\ \vdots \\ - L{({n_c})^{\rm{T}}} \\ \end{gathered} \right]}_{ - {M_\Delta }}\eta \leqslant \underbrace {\left[ \begin{gathered} - \Delta {u^{\min }} \\ - \Delta {u^{\min }} \\ \vdots \\ \Delta {u^{\min }} \\ \end{gathered} \right]}_{ - \Delta {U^{\min }}}, \end{split}$

 $\begin{gathered} {M_\Delta }\eta \leqslant \Delta {U^{\max }} \text{,} \\ - {M_\Delta }\eta \leqslant - \Delta {U^{\min }} \text{,} \\ \end{gathered}$ (21)

 ${M_H}\eta \leqslant {\gamma _H}\text{。}$ (22)

3）纵倾角限制

 ${\theta _{\min }} \leqslant \theta \leqslant {\theta _{\max }}\text{。}$

 $\begin{array}{l} x\left( {k + m/k} \right) = {A^m}x\left( k \right) + \phi {\left( m \right)^{\rm{T}}}\eta \text{,}\\ y\left( {k + m/k} \right) = C{A^m}x\left( k \right) + C\phi {\left( m \right)^{\rm{T}}}\eta \text{,} \end{array}$ (23)

 $\begin{split} &{\theta _{\min }} \leqslant y({k_i} + m|{k_i}) \leqslant {\theta _{\max }} , \\ &{\theta _{\min }} \leqslant C{A^m}x({k_i}) + C\phi {\left( m \right)^{\rm{T}}}\eta \leqslant {\theta _{\max }} ,\\ & - C\phi {\left( m \right)^{\rm{T}}}\eta \leqslant - {\theta _{\min }} + C{A^m}x({k_i}) ,\\ &C\phi {\left( m \right)^{\rm{T}}}\eta \leqslant {\theta _{\max }} - C{A^m}x({k_i}) \text{。} \end{split}$

 $\begin{split} -& \left[ \begin{gathered} C\phi {\left( 1 \right)^{\rm{T}}} \\ C\phi {\left( 2 \right)^{\rm{T}}} \\ \vdots \\ C\phi {\left( {Np} \right)^{\rm{T}}} \\ \end{gathered} \right]\eta \leqslant - {\left[ \begin{gathered} 1 \\ 1 \\ \vdots \\ 1 \\ \end{gathered} \right]_{Np}}{\theta ^{\min }} + \left[ \begin{gathered} CA \\ C{A^2} \\ \vdots \\ C{A^{Np}} \\ \end{gathered} \right]x({k_i}) ,\\& \left[ \begin{gathered} C\phi {\left( 1 \right)^{\rm{T}}} \\ C\phi {\left( 2 \right)^{\rm{T}}} \\ \vdots \\ C\phi {\left( {Np} \right)^{\rm{T}}} \\ \end{gathered} \right]\eta \leqslant {\left[ \begin{gathered} 1 \\ 1 \\ \vdots \\ 1 \\ \end{gathered} \right]_{Np}}{\theta ^{\max }} - \left[ \begin{gathered} CA \\ C{A^2} \\ \vdots \\ C{A^{Np}} \\ \end{gathered} \right]x({k_i}) \text{。} \end{split}$

 ${M_S}\eta \leqslant {\gamma _S}\text{,}$ (24)

 $\underbrace {\left[ {\begin{array}{*{20}{c}} {{M_H}} \\ {{M_S}} \end{array}} \right]}_M\eta \leqslant \underbrace {\left[ {\begin{array}{*{20}{c}} {{\gamma _H}} \\ {{\gamma _S}} \end{array}} \right]}_\gamma \text{,}$ (25)

 \left\{ \begin{aligned} &{J_1} = \dfrac{1}{2}{\mathit{\boldsymbol{\eta }}^{\rm{T}}}{\mathit{\boldsymbol{\Omega }}_\mathit{\boldsymbol{\Sigma }}}\mathit{\boldsymbol{\eta }} + {\mathit{\boldsymbol{\eta }}^{\rm{T}}}\mathit{\boldsymbol{\Phi x}}(k)\text{,}\\ &\mathit{\boldsymbol{M\eta }} \leqslant \mathit{\boldsymbol{\gamma }}\text{。} \end{aligned} \right. (26)

 $\Delta u(k + 1) = \mathit{\boldsymbol{L}}{\left( 0 \right)^{\rm{T}}}\mathit{\boldsymbol{\eta }}\text{,}$ (27)

 $\mathit{\boldsymbol{\hat x}}({k_i} + 1) = \mathit{\boldsymbol{A\hat x}}({k_i}) + \mathit{\boldsymbol{B\Delta u}}({k_i}) + {\mathit{\boldsymbol{K}}_{\mathit{\boldsymbol{ob}}}}\left(\mathit{\boldsymbol{y}}({k_i}) - \mathit{\boldsymbol{C\hat x}}\left({k_i}\right)\right)\text{。}$ (28)
3 仿真分析

 图 3 3kn航速深度变化曲线 Fig. 3 Speed depth curve of 3kn

 图 4 3kn航速纵倾变化曲线 Fig. 4 Speed pitch curve of 3kn

 图 5 3kn航速尾水平舵变化曲线 Fig. 5 Speed stern hydroplane curve of 3kn

 图 6 5kn航速深度变化曲线 Fig. 6 Speed depth curve of 5kn

 图 7 5kn航速纵倾变化曲线 Fig. 7 Speed pitch curve of 5kn

 图 8 5kn航速尾水平舵变化曲线 Fig. 8 Speed stern hydroplane curve of 5kn

4 结　语

 [1] 伍尙慧, 路静. 国外无人潜航器的发展现状与张望, 电科防务研究, 2016.8. 25. [2] SUR, JOO-NO. Design and investigation of a dive plane sliding mode compensator for an autonomous underwater vehicle[D]. Naval Postgraduate School. USA 1989. [3] MICHAEL F. HAJOSY. Six degree of freedom vehicle controller design for the operation of an unmanned underwater vehicle in a shallow water environment[D]. Naval Postgraduate School. USA 1994. [4] HYUN-Sik KIM, YONG-Ku SHIN. Design of adaptive fuzzy sliding mode controller based on fuzzy basis function expansion for UFV depth control[J]. International journal of control, Automation and Systems, 2005, 3(2): 217-224. [5] RAJA ROUT. Design and experimental realization of adaptive control schemes for an autonomous underwater vehicle[D]. National Institute of Technology Rourkela, India. 2017. [6] 高剑著. 无人水下航行器自适应非线性控制技术[M]. 西安: 西北工业大学出版社, 2016: 143−149. [7] 熊华胜, 边信黔, 施小成. 自治水下机器人深度的鲁棒H∞控制仿真[J]. 计算机仿真, 2007, 24(3). [8] CB/Z 259−96《潜艇空间机动控制规律》[R]. p5. [9] WANG Liuping. Model predictive control system design and implementation using Matlab®[M]. Verlag London Limited, 2009. [10] WANG Liuping, William R, CLUETT. From plant data to process control——Ideas for process identification and PID design[J]. Taylor & Francis, 2000: 10−11. [11] KATSUHIKO OGATA. Discrete-time controlsystems[M]. 北京: 机械工业出版社, 2004: 421−449. [12] PRESTERO T. Verification of a six-degree of freedom simulation model for the REMUS autonomous underwater vehicle[D]. Woods Hole Ocenographic Institu-tion, Departments of Ocean and Mechanical Engineering, MIT, MA, 2001