0 引　言

随着科技发展及海洋资源开发，水下机器人被广泛运用于水下勘探、作业及海上救援。部分水下机器人装载超过其自由度的推进器数量以保证其高效的运动及作业效率^[1-2]，由于推进器数量超过自由度数量，当控制器给出目标推力与力矩时，推力分配算法需要给出合适的每个推进器的推力值大小。目标推力与力矩较小时，运用推力分配算法能够实现各个推进器合成的实际推力与力矩和目标值相等；目标推力与力矩较大，运用推力分配算法后，部分推进的推力会超出限制范围，这类问题称为饱和问题。

推力分配算法可以分为直接法和伪逆法^[3]。直接法^[4-5]在水平面和垂直面能够分别实现控制，按照空间结构，分别从水平面和垂直面目标推力与力矩反推推进器推力大小，算法简洁易用，但是需要在水下机器人设计时考虑推进器的空间布局，联合控制较为复杂，且不能解决饱和问题。伪逆法^[6]计算推力转换矩阵的伪逆，通过目标控制推力和力矩伪逆矩阵计算各个推进器的推力，需要通过放缩或削峰的方法解决饱和问题，这样将会导致各个推进器合成的实际推力与力矩和目标值差距较大。

本文利用可行方向法解决推力分配的饱和问题。首先，利用水下机器人推进器的空间布置建立了通用的ROV推力转换矩阵。然后，通过最小化目标推力与力矩将推力分配问题转化为二次规划问题并利用可行方向法求解该二次规划问题。最后，利用仿真实验比较了伪逆法和可行方向法输出的推力和力矩及其合成后和目标值相比较的结果。

1 水下机器人推进系统数学建模 1.1 坐标系与水下机器人推进器空间布置

扁平式的水下机器人综合了遥控作业型潜水器（ROV）与无人自主潜水器（AUV）的特点，具有高效的响应速度和稳定性^[7]。本文以扁平式尾主推多螺旋桨的水下机器人为实验对象。

如图1所示，建立全局坐标系 ${O_n} - {x_n}{y_n}{z_n}$ (North-East-Down coordinate system, NED)和体坐标系 ${O_b} - {x_b}{y_b}{z_b}$ ^[8]。其中NED坐标系的原点 ${O_n}$ 固定在大地上的某一点，x轴指向正北方，y轴指向东方，z轴指向正下方，从地表指向地心。体坐标系随着水下机器人的运动而运动的坐标系，原点 ${O_b}$ 与水下机器人的重心重合，x轴指向水下机器人的正前方，y轴指向水下机器人的正右方，z轴指向水下机器人的正下方。

图2为水下机器人推进器8个推进器的空间布置。水下机器人的推进器与重心的相对位置、推力方向和推力最值如表1所示，其中推进器1、推进器2和推进器3是主推进器。

1.2 推力分配问题提出

水下机器人的动力学方程^[9]为：

$ {{{\boldsymbol{M}}{\boldsymbol{\nu}} + {\boldsymbol{C}}}}{\text{(}}{{{\boldsymbol{\nu}} }}{\text{)}}{{{\boldsymbol{\nu}} + {\boldsymbol{D}}}}{\text{(}}{{{\boldsymbol{\nu }}}}{\text{)}}{{{\boldsymbol{\nu}} + {\boldsymbol{g}}}}{\text{(}}{{{\boldsymbol{\eta}} }}{\text{)}}{{ + }}{{{\boldsymbol{{g}}}}_0}{{ = {\boldsymbol{\tau}} + {\boldsymbol{w}}}}。$

(1)

式中： ${{{\boldsymbol{\nu}} }}$ 为水下机器人在体坐标系下的速度（包括线速度和角速度）； ${{{\boldsymbol{\eta}} }}$ 为水下机器人在NED坐标系下的坐标； ${{{\boldsymbol{M}}}}$ 为系统惯性矩阵（包括附加质量）； ${{{\boldsymbol{C}}}}{\text{(}}{{{\boldsymbol{\nu}} }}{\text{)}}$ 为科氏向心力矩阵（包括附加质量）； ${{{\boldsymbol{D}}}}{\text{(}}{{{\boldsymbol{\nu }}}}{\text{)}}$ 为阻尼矩阵； ${{{\boldsymbol{g}}}}{\text{(}}{{{\boldsymbol{\eta }}}}{\text{)}}$ 为重力和浮力的差值； ${{{{\boldsymbol{g}}}}_0}$ 为压载物产生的力； ${{{\boldsymbol{w}}}}$ 为环境扰动（包括风浪流）； ${{{\boldsymbol{\tau }}}}$ 为推力与力矩，由所有主动控制器产生的合力和合力矩组成， $ {\mathbf{\tau }} $ 的具体形式为体坐标系下3个方向的力和对体坐标系原点的力矩。

$ {{{\boldsymbol{\tau}} }} =[\,X \,\,\,Y \,\,\,Z\,\,\, K\,\,\,M\,\,\, N ]^{\text{T}}。$

(2)

式中： $[X\,\,\,Y\,\,\, Z ]^{\text{T}}$ 表示主动控制力在体坐标系下的3个分量，分别表示纵向推力、横向推力和垂向推力； $[K\,\,\,M\,\,\, N ]^{\text{T}}$ 为对体坐标系原点的主动控制力矩在体坐标系下的3个分量，分别表示横倾力矩、纵倾力矩和首摇力矩。

当控制器在控制过程中，在每个控制时刻产生一个目标推力与力矩：

$ {{{\boldsymbol{\bar \tau}} }} = [{\bar X}\,\,\,{\bar Y}\,\,\, {\bar Z}\,\,\, {\bar K} \,\,\,{\bar M} \,\,\,{\bar N} ]^{\text{T}}。$

(3)

式中： $[{\bar X}\,\,\,{\bar Y}\,\,\, {\bar Z} ]^{\text{T}}$ 表示主动控制力在体坐标系下的3个分量，分别表示目标纵向推力、目标横向推力和目标垂向推力； $[{\bar K}\,\,\,{\bar M}\,\,\, {\bar N} ]^{\text{T}}$ 为对体坐标系原点的主动控制力矩在体坐标系下的3个分量，分别表示目标横倾力矩、目标纵倾力矩和目标首摇力矩。

然而水下机器人安装的主动控制器无法直接给出推力与力矩 ${{{\boldsymbol{\tau}} }}$ ，需要通过各个主动控制器的推力合成之后才能得到。推力分配问题主要解决如何利用各个推进器的推力合成推力与力矩 ${{{\boldsymbol{\tau}} }}$ ，使得推力与力矩 ${{{\boldsymbol{\tau}} }}$ 与目标推力与力矩 ${{{\boldsymbol{\bar \tau}} }}$ 尽可能相等。

1.3 水下机器人推进系统数学建模

水下推进系统数学模型主要与水下机器人的构造有关，设水下机器人主动控制器有n个推进器。

如图3所示，设第k个推进器中心的位置为 ${P_k}$ ，重心C到第k个推进器中心的位置 ${P_k}$ 的方向向量为 ${{{{\boldsymbol{r}}}}_k}$ ，第k个推进器的推力正方向为 ${{\boldsymbol{e}}_k}$ ，且满足 $\left| {{{\boldsymbol{e}}_k}} \right| = 1$ ，推力大小为 ${T_k}$ ，则第k个推进器对重心产生的力矩 ${{\boldsymbol{M}}_k}$ 可用下式表示：

$ {{\boldsymbol{M}}_k} = {T_k}{{\boldsymbol{r}}_k} \times {{\boldsymbol{e}}_k}。$

(4)

将每个推力和力矩合成，可以用推力矢量和力矩矢量相加得到。主动控制力可用下式表示：

$ [X\,\,\,Y \,\,\,Z]^{\text{T}} = \sum\limits_{k = 1}^n {{T_k}{{\boldsymbol{e}}_k}} , $

(5)

主动控制力矩可用下式表示：

$ [K\,\,\,M \,\,\,N]^{\text{T}} = \sum\limits_{k = 1}^n {{T_k}{{\boldsymbol{r}}_k} \times {{\boldsymbol{e}}_k}} 。$

(6)

用 ${\boldsymbol{T}} = [{{T_1}}\,\,\,{{T_2}}\,\,\, \cdots \,\,\,{{T_n}} ]^{\text{T}}$ 来表示n个推进器输出的推力向量，那么推力与力矩 $ {\mathbf{\tau }} $ 可以用下式表示：

$ {\boldsymbol{\tau }} = {\boldsymbol{B}} \cdot {\boldsymbol{T}}。$

(7)

式中： ${\boldsymbol{B}}$ 为推进器的推力转换矩阵，由水下机器人推进器的空间布置决定。推力转换矩阵 ${\boldsymbol{B}}$ 的表达式为：

$ {\boldsymbol{B}} = {\left[ {\begin{array}{*{20}{c}} {{{\boldsymbol{e}}_1}}& \cdots &{{{\boldsymbol{e}}_n}} \\ {{{\boldsymbol{r}}_1} \times {{\boldsymbol{e}}_1}}& \cdots &{{{\boldsymbol{r}}_n} \times {{\boldsymbol{e}}_n}} \end{array}} \right]_{n \times 6}}。$

(8)

2 推力分配 2.1 问题提出

在控制过程中，推力与力矩 ${\boldsymbol{\tau }}$ 应尽可能接近目标推力与力矩 ${\boldsymbol{\bar \tau }}$ 。水下机器人的目标推力与力矩 ${\boldsymbol{\bar \tau }}$ 与当前推进器合力产生的推力与力矩 ${\boldsymbol{\tau }}$ 并不一定相同，由于推进器合成的实际推力与力矩的范围是一个六维凸集，边界难以确定，可能所有推进器的推力组合都不能达到目标推力与力矩 ${\boldsymbol{\bar \tau }}$ ，设目标推力与力矩 ${\boldsymbol{\bar \tau }}$ 与当前推进器产生的推力与力矩 ${\boldsymbol{\tau }}$ 的差值为 ${\boldsymbol{s}}$ 。那么可以通过最小化目标推力的差值的绝对值来寻找最优解。水下运载器推力分配问题可转化为二次规划问题：

$ \begin{gathered} \min \qquad f({[{{\boldsymbol{T}}^{\text{T}}}\;{{\boldsymbol{s}}^{\text{T}}}]^{\text{T}}}) = \frac{1}{2}{{\boldsymbol{s}}^{\text{T}}}{\boldsymbol{Hs}}，\\ {\text{s}}{\text{. t}}{\text{.}}\qquad {\boldsymbol{T}} \geqslant {{\boldsymbol{T}}_{\min }}, \\ \qquad \quad - {\boldsymbol{T}} \geqslant - {{\boldsymbol{T}}_{\max }}, \\ \qquad \quad {\boldsymbol{BT}} + {\boldsymbol{s}} = \overline {\boldsymbol{\tau }} 。\\ \end{gathered} $

(9)

式中： ${\boldsymbol{H}}$ 为对角矩阵，表示目标推力与力矩 ${\boldsymbol{\bar \tau }}$ 和推力与力矩 $ {\mathbf{\tau }} $ 各个分量接近程度的权重。 ${{\boldsymbol{T}}_{\min }}$ 为各个推进器所能达到的最小推力，如果推进器可以反向运动，那么 ${{\boldsymbol{T}}_{\min }}$ 表示反向最大推力，用负数表示。 ${{\boldsymbol{T}}_{\max }}$ 表示推进器正向所能达到的最大推力。

将式 (9) 约束条件中的2个不等式合并，改写为：

$ \begin{gathered} \min \qquad f({\boldsymbol{x}}) = \frac{1}{2}{{\boldsymbol{s}}^{\text{T}}}{\boldsymbol{Hs}}，\\ {\text{s}}{\text{. t}}{\text{.}}\qquad {\boldsymbol{A}}\left[ {\begin{array}{*{20}{c}} {\boldsymbol{T}} \\ {\boldsymbol{T}} \end{array}} \right] \geqslant {\boldsymbol{b}}, \\ \qquad \quad \,\,\, \left[{\boldsymbol{B}}\,\,\,{{{\boldsymbol{I}}_6}} \right]{\boldsymbol{x}} = \overline {\boldsymbol{\tau }} 。\\ \end{gathered} $

(10)

式中： ${\boldsymbol{x}} = {\left[ {{{\boldsymbol{T}}^{\text{T}}}}\,\,\,{{{\boldsymbol{s}}^{\text{T}}}} \right]^{\text{T}}}$ ， ${\boldsymbol{b}} = {\left[{{\boldsymbol{T}}_{\min }^{\text{T}}}\,\,\,{ - {\boldsymbol{T}}_{\max }^{\text{T}}} \right]^{\text{T}}}$ , $ {\boldsymbol{A}} = \left[ {\begin{array}{*{20}{c}} {{{\boldsymbol{I}}_n}}&{\mathbf{0}} \\ {\mathbf{0}}&{ - {{\boldsymbol{I}}_n}} \end{array}} \right] $ , 其中 ${{\boldsymbol{I}}_n}$ 表示n维单位矩阵， ${\boldsymbol{0}}$ 表示零矩阵。

2.2 可行方向法

由于推进器的推力是有约束的，可采用可行方向法求取式 (10) 的最优解。可行方向法的主要思想是从可行点出发，沿着下降的可行方向搜索，求出使得目标函数下降的新的可行点，求解流程如图4 所示。

具体计算步骤^[10]如下：

1）确定初始可行点 ${{\boldsymbol{x}}^{(1)}}$ ，置 $u = 1$ ；在该问题中，初始可行点可以取为 ${\boldsymbol{T}} = {\boldsymbol{{\rm{0}}}}$ ， ${\boldsymbol{s}} = \overline {\boldsymbol{\tau }}$ 。

2）在点 ${{\boldsymbol{x}}^{(u)}}$ 处将 ${\boldsymbol{A}}$ 和 ${\boldsymbol{b}}$ 按照起作用约束和不起作用约束分为 $\left[ {\begin{array}{*{20}{c}} {{{\boldsymbol{A}}_1}} \\ {{{\boldsymbol{A}}_2}} \end{array}} \right]$ 和 $\left[ {\begin{array}{*{20}{c}} {{{\boldsymbol{b}}_1}} \\ {{{\boldsymbol{b}}_2}} \end{array}} \right]$ ，使得 ${{\boldsymbol{A}}_1}{\boldsymbol{x}} = {{\boldsymbol{b}}_1}$ ， ${{\boldsymbol{A}}_2}{\boldsymbol{x}} > {{\boldsymbol{b}}_2}$ ；并计算 $\nabla f({{\boldsymbol{x}}^{(u)}})$ 。

3）求下降的可行方向 ${\boldsymbol{d}}$ 。下降方向由目标函数的梯度 $\nabla f({{\boldsymbol{x}}^{(u)}})$ 来控制；可行方向的不等式约束由起作用约束矩阵 ${{\boldsymbol{A}}_1}$ 来控制；等式约束中，在可行方向上，变量值应保持不变。为了求得下降的可行方向 ${\boldsymbol{d}}$ 是个有限值，增加约束条件 $\left| {\boldsymbol{d}} \right| \leqslant 1$ ，算法表达如下式：

$ \begin{gathered} \min \qquad \nabla f{({{\boldsymbol{x}}^{(u)}})^{\text{T}}}{\boldsymbol{d}} = [{{\boldsymbol{0}}_{n \times 1}}\;{{\boldsymbol{s}}^{\text{T}}}]\left[ {\begin{array}{*{20}{c}} {\boldsymbol{0}}&{\boldsymbol{0}} \\ {\boldsymbol{0}}&{\boldsymbol{H}} \end{array}} \right]{\boldsymbol{d}} ，\\ {\text{s}}{\text{. t}}{\text{.}}\qquad {{\boldsymbol{A}}_1}{\boldsymbol{d}} \geqslant {\boldsymbol{0}}, \\ \qquad \quad \,\,\, \left[{\boldsymbol{B}}\,\,\,{{{\boldsymbol{I}}_6}} \right]{\boldsymbol{d}} = {\boldsymbol{0}}, \\ \qquad \quad \,\,\, {- 1} \leqslant {d_j} \leqslant 1,j = 1, \cdots ,n + 6 。\\ \end{gathered} $

(11)

求解线性规划即得最优下降的可行方向 ${{\boldsymbol{d}}^{(u)}} $ 。

4）如果 $\nabla f{({{\boldsymbol{x}}^{(u)}})^{\text{T}}}{{\boldsymbol{d}}^{(u)}} = 0$ ，说明已经没有下降方向，停止计算； ${{\boldsymbol{x}}^{(u)}}$ 点为最优解，否则继续下一步；

5）找到下降的可行方向后，计算前进的步长 $\lambda $ 。计算 ${\boldsymbol{\hat b}} = {{\boldsymbol{b}}_2} - {{\boldsymbol{A}}_2}{{\boldsymbol{x}}^{(u)}}$ ， ${\boldsymbol{\hat d}} = {{\boldsymbol{A}}_2}{{\boldsymbol{d}}^{(u)}}$ ，可以得到步长 $\lambda $ 的上限为：

$ {\lambda _{\max }} = \left\{ {\begin{array}{*{20}{l}} {\min \left\{ {\frac{{\widehat {{b_i}}}}{{\widehat {{d_i}}}}|\widehat {{d_i}} < 0} \right\},}&{\widehat {\boldsymbol{d}}\not \geqslant 0} ，\\ {\infty ,}&{\widehat {\boldsymbol{d}} \geqslant 0} 。\end{array}} \right. $

(12)

在 $ \left[ {0,{\lambda _{\max }}} \right] $ 上进行一维搜索：

$ \begin{gathered} \min \qquad f({{\boldsymbol{x}}^{(u)}} + \lambda {{\boldsymbol{d}}^{(u)}}) ，\\ {\text{s}}{\text{. t}}{\text{.}}\qquad 0 \leqslant \lambda \leqslant {\lambda _{\max }} 。\\ \end{gathered} $

(13)

得到最优解 ${\lambda _u}$ ，令 ${{\boldsymbol{x}}^{(u + 1)}} = {{\boldsymbol{x}}^{(u)}} + {\lambda _u}{{\boldsymbol{d}}^{(u)}}$ 。

6）置 $u = u + 1$ ，返回步骤2。

对于式 (11) 所示的 $n + 6$ 个变量的线性优化问题可以采用单纯形方法进行求解。由于单纯形方法的标准形式要求变量都大于0，对式 (11) 做一个变换 ${\boldsymbol{\delta }} = {\boldsymbol{d}} + 1$ ，那么 ${\boldsymbol{d}} = {\boldsymbol{\delta }} - 1$ 。设起作用约束的个数为 $ l(0 \leqslant l \leqslant n) $ ，即 ${{\boldsymbol{A}}_1}$ 的行数为l，式 (11) 所示的线性优化问题可转换为：

$ \begin{gathered} \min \qquad [{{\boldsymbol{0}}_{n \times 1}}\;{{\boldsymbol{s}}^{\text{T}}}]\left[ {\begin{array}{*{20}{c}} {\boldsymbol{0}}&{\boldsymbol{0}} \\ {\boldsymbol{0}}&{\boldsymbol{H}} \end{array}} \right]{\boldsymbol{\delta }} ，\\ {\text{s}}{\text{. t}}{\text{.}}\qquad - {{\boldsymbol{A}}_1}{\boldsymbol{\delta }} \leqslant - {{\boldsymbol{A}}_1} \cdot {{\boldsymbol{I}}_{(n + 6) \times 1}}, \\ \qquad \quad \,\,\, \left[{\boldsymbol{B}}\,\,\,{{{\boldsymbol{I}}_6}} \right]{\boldsymbol{\delta }} = \left[ {\boldsymbol{B}}\,\,\,{{{\boldsymbol{I}}_6}} \right] \cdot {{\boldsymbol{I}}_{(n + 6) \times 1}}, \\ \qquad \quad \,\,\, 0 \leqslant {\delta _j} \leqslant 2,j = 1, \cdots ,14。\\ \end{gathered} $

(14)

式中： ${\boldsymbol{\delta }}$ 表示 $(n + 6)$ 个变量， ${{\boldsymbol{I}}_{(n + 6) \times 1}}$ 表示 $n + 6$ 行1列值为1的矩阵。

引进 $q = l + n + 6$ 个松弛变量，将式 (14) 的线性优化问题中的不等式约束转换为等式约束：

$ \begin{gathered} \min \qquad \left[ {\begin{array}{*{20}{c}} {{{\boldsymbol{0}}_{n \times 1}}}&{{{\boldsymbol{s}}^{\text{T}}}}&{{{\boldsymbol{0}}_{q \times 1}}} \end{array}} \right]\left[ {\begin{array}{*{20}{c}} {\boldsymbol{0}}&{\boldsymbol{0}}&{\boldsymbol{0}} \\ {\boldsymbol{0}}&{\boldsymbol{H}}&{\boldsymbol{0}} \\ {\boldsymbol{0}}&{\boldsymbol{0}}&{\boldsymbol{0}} \end{array}} \right]{\boldsymbol{\delta }} ，\\ {\text{s}}{\text{. t}}{\text{.}}\qquad \left[ {\begin{array}{*{20}{c}} {\begin{array}{*{20}{c}} { - {{\boldsymbol{A}}_1}} \\ {{{\boldsymbol{I}}_{(n + 6)}}} \end{array}}&{{{\boldsymbol{I}}_q}} \end{array}} \right]{\boldsymbol{\delta }} = \left[ {\begin{array}{*{20}{c}} { - {{\boldsymbol{A}}_1} \cdot {{\boldsymbol{I}}_{(n + 6) \times 1}}} \\ {2 \cdot {{\boldsymbol{I}}_{(n + 6) \times 1}}} \end{array}} \right], \\ \qquad \quad \,\,\, {\text{ }}\left[ {\begin{array}{*{20}{c}} {\boldsymbol{B}}&{{{\boldsymbol{I}}_6}}&{{{\boldsymbol{0}}_{q \times 6}}} \end{array}} \right]{\boldsymbol{\delta }} = \left[ {\begin{array}{*{20}{c}} {\boldsymbol{B}}&{{{\boldsymbol{I}}_6}} \end{array}} \right] \cdot {{\boldsymbol{I}}_{(n + 6) \times 1}}, \\ \qquad \quad \,\,\, {\text{ }}{\boldsymbol{\delta }} \geqslant {\boldsymbol{0}}。\\ \end{gathered} $

(15)

式中， ${\boldsymbol{\delta }}$ 表示 $(n + 6) + l + (n + 6)$ 个变量。

式 (15) 已经化为单纯形方法的标准形式。然而，使用单纯性法还有一个前提：找到初始基本可行解。为了寻找初始基本可行解，先寻找一个可行解，然后根据可行解通过变换得到基本可行解。

将式 (15) 改写为

$ \begin{gathered} \min \qquad {\boldsymbol{c\delta }} ，\\ {\text{s}}{\text{. t}}{\text{.}}\qquad {{\boldsymbol{A}}_s}{\boldsymbol{\delta }} = {{\boldsymbol{b}}_s}, \\ \qquad \quad \,\,\,{\text{ }}{\boldsymbol{\delta }} \geqslant {\boldsymbol{0}}。\\ \end{gathered} $

(16)

式中：

$ {{\boldsymbol{A}}_s} = \left[ {\begin{array}{*{20}{c}} {\begin{array}{*{20}{c}} { - {{\boldsymbol{A}}_1}} \\ {{{\boldsymbol{I}}_{(n + 6)}}} \end{array}}&{{{\boldsymbol{I}}_q}} \\ {\begin{array}{*{20}{c}} {\boldsymbol{B}}\,\,\,\,\,\,{{{\boldsymbol{I}}_6}} \end{array}}&{{{\boldsymbol{0}}_{q \times 6}}} \end{array}} \right], $

(17)

$ {{\boldsymbol{b}}_s} = \left[ {\begin{array}{*{20}{c}} { - {{\boldsymbol{A}}_1} \cdot {{\boldsymbol{I}}_{(n + 6) \times 1}}} \\ {2 \cdot {{\boldsymbol{I}}_{(n + 6) \times 1}}} \\ {\left[ {\begin{array}{*{20}{c}} {\boldsymbol{B}}&{{{\boldsymbol{I}}_6}} \end{array}} \right] \cdot {{\boldsymbol{I}}_{(n + 6) \times 1}}} \end{array}} \right]。$

(18)

将 $[l + (n + 6) + 6] \times [(n + 6) + 6 + (n + 6)]$ 阶矩阵写为：

$ {{\boldsymbol{A}}_s} = \left[ {{{\boldsymbol{p}}_1},{{\boldsymbol{p}}_1}, \ldots ,{{\boldsymbol{p}}_{(n + l) + 6 + (n + l)}}} \right]。$

(19)

设 $(n + l) + 6 + (n + l)$ 维的向量 ${\boldsymbol{\delta }} = {\left[ {{\delta _1},{\delta _2}, \ldots ,{\delta _s},0, \ldots ,0} \right]^{\text{T}}}$ 是式 (16) 的一个可行解，如果 ${\mathbf{\delta }}$ 的 $s$ 个正分量所对应的列 ${{\boldsymbol{p}}_1}, \ldots ,{{\boldsymbol{p}}_s}$ 线性无关，那么 ${\mathbf{\delta }}$ 是基本可行解。否则，需要对 ${\mathbf{\delta }}$ 进行变换得到基本可行解。设 ${{\boldsymbol{p}}_1}, \ldots ,{{\boldsymbol{p}}_s}$ 线性相关，则存在不全为零的数 $ {\gamma _1}, \ldots ,{\gamma _s} $ (其中至少有一个正数)，使得

$ \sum\limits_{i = 1}^s {{\gamma _i}{{\boldsymbol{p}}_i} = 0} ，$

(20)

构造一个点 ${\boldsymbol{\hat \delta }}$ ：

$ \left\{ {\begin{array}{*{20}{l}} {{{\hat \delta }_i} = {x_i} - \alpha {\gamma _i},}&{i = 1, \ldots ,s,} \\ {{{\hat \delta }_i} = 0,}&{i = s + 1, \ldots ,(n + l) + 6 + (n + l)。} \end{array}} \right. $

(21)

式中， $\alpha = \min \left\{ {\frac{{{x_i}}}{{{\gamma _i}}}|{\gamma _i} > 0} \right\}$ 。易知验证 ${\boldsymbol{\hat \delta }}$ 满足 ${{\boldsymbol{A}}_s}{\boldsymbol{\hat \delta }} = {{\boldsymbol{b}}_s}$ ， ${\boldsymbol{\hat \delta }} \geqslant {\boldsymbol{0}}$ 。故而 ${\boldsymbol{\hat \delta }}$ 也是一个可行解，但是 ${\boldsymbol{\hat \delta }}$ 大于零的分量比 ${\boldsymbol{\delta }}$ 至少少一个，重复上述步骤可以求得一个基本可行解。

通过观察式 (15), 可以发现

$ {\boldsymbol{\delta}} ={[\stackrel{(n+6)个1}{\overbrace{1,1,\dots ,1}},\stackrel{l个0}{\overbrace{0,0,\dots ,0}},\stackrel{(n+6)个1}{\overbrace{1,1,\dots ,1}}]}^{\text{T}} $

(22)

是一个可行解。利用式 (22) 所示的可行解，通过上述变换方法可以获得一个基本可行解。然后就可以利用单纯形法求解。式 (13) 所示的一元二次优化问题可以直接用求导的方法求解。

3 仿真验证 3.1 参数选取及目标值

推力转换矩阵 ${\boldsymbol{B}}$ 由水下机器人推进器的空间布置决定，利用表1和式 (8) 可以计算得出推力转换矩阵 ${\boldsymbol{B}}$ 。可行方向法的目标函数中的对角矩阵 ${\boldsymbol{H}}$ 取为单位矩阵。仿真时，设置目标推力与力矩 $\bar X = 150\sin (0.1{\text π} t)({\text{N)}}$ , $\bar Y = 150\sin (0.2{\text π} t)({\text{N)}}$ , $\bar Z = 150\sin (0.3{\text π} t)({\text{N)}}$ , $\bar K = 150\sin (0.4{\text π} t)({\text{N}} \cdot {\text{m)}}$ , $\bar M = 150\sin (0.5{\text π} t)({\text{N}} \cdot {\text{m)}}$ , $\bar N = 150\sin (0.6{\text π} t)({\text{N}} \cdot {\text{m)}}$ 。其中t从0开始每隔0.1取一次值，取到20，共201次仿真实验。

3.2 仿真实验结果与分析

根据目标推力与力矩，分别采用伪逆法和可行方向法求解每个推进器推力的大小，然后根据式 (7) 可以计算得到推力与力矩。

图5 为通过伪逆法和可行方向法计算得到的推进器推力。通过原始伪逆法计算得到的推进器推力会超出推进器的推力上下限，这里采用放缩法处理伪逆法的饱和问题。可以看出2种方法计算得到所有的推进器的推力都未超过其上下限。

图6为用伪逆法与可行方向计算得到推进器推力合成得到推力与力矩 $ {\mathbf{\tau }} $ 与目标推力与力矩 $ {\mathbf{\bar \tau }} $ 的差值的6个分量。其中， $ \Delta X = X - \bar X $ , $ \Delta Y = Y - \bar Y $ , $ \Delta Z = Z - \bar Z $ , $ \Delta K = K - \bar K $ , $ \Delta M = M - \bar M $ , $ \Delta N = N - \bar N $ 。从图中可以看出，通过伪逆法得到的推进器推力合成的推力与力矩 ${\boldsymbol{\tau }}$ 与目标推力与力矩 $ {\mathbf{\bar \tau }} $ 的差距较大，而通过可行方向法得到的推进器推力合成的推力与力矩 ${\boldsymbol{\tau }}$ 与目标推力与力矩 ${\boldsymbol{\bar \tau }}$ 的差距几乎为0。此时，推进器的利用可行方向法得到的推力组合合成的推力与力矩能够和目标推力与力矩相等。

4 结　语

采用矩阵运算的方式，建立推进系统数学模型。在推力分配中，利用二次规划约束条件控制的优势，以最小化推力与力矩与目标值的差值建立二次规划问题，并以可行方向法进行求解。利用单纯形法求取可行的下降方向，并通过可行解转化为初始基本可行解作为单纯形法的输入，再通过一维搜索求取步长。为了验证本方法的有效性，模拟2组目标推力与力矩，比较了经过放缩处理的伪逆法和可行方向法推力分配的结果。结果表明，经过放缩处理的伪逆法和可行方向法得到的各个推进器的推力都未超出其上下限，可行方向法得到的推力与力矩更接近目标值，可行方向法可以克服饱和问题。