水下机器人涵推构型分析

1.1 电机混控

　　无人机（或水下机器人）每个旋翼提供的升力和反作用力矩如图所示。定义第 $i$ 个螺旋桨的转子坐标系为 $O_i$，转子坐标系的 $z$ 轴与电机轴重合。记第 $i$ 个螺旋桨提供的升力和反作用力矩分别 $F_i$ 和 $\tau_i$，对于逆时针 (CCW) 旋转的螺旋桨反作用力矩为负，对于顺时针 (CW) 旋转的桨反作用力矩为正。$F_i$ 和 $\tau_i$ 与螺旋桨转速 $\omega_i$ 的关系可以近似为：

\[\begin{equation} F_i = C_f \omega_i^2 \end{equation}\] \[\begin{equation} \tau_i = S_i C_\tau \omega_i^2 \label{eq:1-1} \end{equation}\]

　　其中 $C_f$ 和 $C_\tau$ 是与空气动力学参数相关的系数。$S_i$ 表示螺旋桨的旋向，当螺旋桨逆时针(CCW)旋转时，$S_i = -1$; 当螺旋桨顺时针(CW)旋转时，$S_i = 1$。因此可以将 $\tau_i$ 记作：

\[\begin{equation} \tau_i = k S_i F_i \label{eq:1-2} \end{equation}\]

　　其中 $k = C_\tau/C_f$。考虑到利用公式就可由升力求得对应的螺旋桨转速，为了简便书写，后续的推导将以螺旋桨的升力作为控制输入。

　　第 $i$ 个螺旋桨在作用点处的力和力矩在机体坐标系下的表示为：

\[\begin{equation} f_i = R_i [0, 0, F_i]^T \end{equation}\] \[\begin{equation} \tau_i = R_i [0, 0, S_i \tau_i]^T \label{eq:1-3} \end{equation}\]

　　将所有力和力矩向质心简化，$n$ 个螺旋桨提供的合力在机体坐标系下的表示为：

\[\begin{equation} F = \sum_{i=0}^{n-1} f_i \end{equation}\] \[\begin{equation} \tau = \sum_{i=0}^{n-1} (p_i \times f_i + \tau_i) \label{eq:1-4} \end{equation}\]

　　其中 $p_i$ 为机体质心到螺旋桨受力点的位置矢量。解上述方程，就可以由控制输入 $F$ 和 $\tau$ 计算每个螺旋桨的升力和倾转电机的角度。

1.2 四棱锥模型

1.2.1 动力分配矩阵

　　控制模型可以简化为底面为长方形的四棱锥构型。每个螺旋桨产生的力/力矩的作用点位于长方形的四个顶点，转子坐标系的 $z$ 轴和四棱锥的棱共线。机体坐标系原点位于长方形几何中心，xoy 平面和长方形共面。

　　转轴单位向量公式（右手系，XY 平面内）：

\[\begin{equation} \begin{aligned} v_0 &= [\frac{a}{\sqrt{a^2+b^2}}, \frac{b}{\sqrt{a^2+b^2}}, 0]^T \\ v_1 &= [-\frac{a}{\sqrt{a^2+b^2}}, \frac{b}{\sqrt{a^2+b^2}}, 0]^T \\ v_2 &= [-\frac{a}{\sqrt{a^2+b^2}}, -\frac{b}{\sqrt{a^2+b^2}}, 0]^T \\ v_3 &= [\frac{a}{\sqrt{a^2+b^2}}, -\frac{b}{\sqrt{a^2+b^2}}, 0]^T \end{aligned} \label{eq:1-5} \end{equation}\]

　　各转子坐标系为标准坐标系绕各转轴旋转 $\theta$ 后的结果，转轴转角的 Rodrigues 公式为：

\[\begin{equation} R(n, \theta) = I + \sin\theta [n]_\times + (1-\cos\theta)[n]_\times^2 \label{eq:1-6} \end{equation}\]

　　其中，$n = [n_x, n_y, n_z]^T$，是单位向量。$\theta$ 为转角。

　　首先，利用第一章中的公式来推导混控矩阵，可以列出以下关系：

\[\begin{equation} \begin{aligned} p_0 &= [ \frac{a}{2}, -\frac{b}{2}, 0 ]^T \\ p_1 &= [ \frac{a}{2}, \frac{b}{2}, 0 ]^T \\ p_2 &= [ -\frac{a}{2}, \frac{b}{2}, 0 ]^T \\ p_3 &= [ -\frac{a}{2}, -\frac{b}{2}, 0 ]^T \end{aligned} \label{eq:1-7} \end{equation}\]

　　(注：原文公式可能略有不同，此处根据常见的四旋翼布局修正)

　　带入 \eqref{eq:1-4} 可得：

\[\begin{equation} F = \sum_{i=0}^{n-1} R_i [0, 0, 1]^T F_i \end{equation}\] \[\begin{equation} \tau = \sum_{i=0}^{n-1} (p_i \times R_i [0, 0, 1]^T + R_i [0, 0, 1]^T k S_i) F_i \label{eq:1-8} \end{equation}\]

　　写成矩阵形式：

\[\begin{equation} \begin{bmatrix} F \\ \tau \end{bmatrix} = M \begin{bmatrix} f_0 \\ f_1 \\ f_2 \\ f_3 \end{bmatrix} \label{eq:1-9} \end{equation}\]

　　对上式求伪逆可得到：

\[\begin{equation} \begin{bmatrix} f_0 \\ f_1 \\ f_2 \\ f_3 \end{bmatrix} = M^+ \begin{bmatrix} F \\ \tau \end{bmatrix} \label{eq:1-10} \end{equation}\] \[\begin{equation} M = \begin{bmatrix} \frac{a}{\sqrt{a^2+b^2}}s\theta & -\frac{b}{\sqrt{a^2+b^2}}s\theta & -\frac{a}{\sqrt{a^2+b^2}}s\theta & \frac{b}{\sqrt{a^2+b^2}}s\theta \\ -\frac{b}{\sqrt{a^2+b^2}}s\theta & \frac{a}{\sqrt{a^2+b^2}}s\theta & \frac{b}{\sqrt{a^2+b^2}}s\theta & -\frac{a}{\sqrt{a^2+b^2}}s\theta \\ c\theta & c\theta & c\theta & c\theta \\ -\frac{b}{2}c\theta + S_0 k \frac{a}{\sqrt{a^2+b^2}}s\theta & \frac{b}{2}c\theta + S_1 k \frac{a}{\sqrt{a^2+b^2}}s\theta & \frac{b}{2}c\theta - S_2 k \frac{a}{\sqrt{a^2+b^2}}s\theta & -\frac{b}{2}c\theta - S_3 k \frac{a}{\sqrt{a^2+b^2}}s\theta \\ -\frac{a}{2}c\theta - S_0 k \frac{b}{\sqrt{a^2+b^2}}s\theta & -\frac{a}{2}c\theta + S_1 k \frac{b}{\sqrt{a^2+b^2}}s\theta & \frac{a}{2}c\theta + S_2 k \frac{b}{\sqrt{a^2+b^2}}s\theta & \frac{a}{2}c\theta - S_3 k \frac{b}{\sqrt{a^2+b^2}}s\theta \\ S_0 k c\theta & S_1 k c\theta & S_2 k c\theta & S_3 k c\theta \end{bmatrix} \label{eq:1-11} \end{equation}\]

　　为了简化，假设螺旋桨旋转方向顺序为 $[+1, -1, +1, -1]$（即 $S_0=1, S_1=-1, S_2=1, S_3=-1$）。

\[\begin{equation} \begin{bmatrix} f_x \\ f_y \\ f_z \\ \tau_x \\ \tau_y \\ \tau_z \end{bmatrix} = \begin{bmatrix} A s\theta & -B s\theta & -A s\theta & B s\theta \\ -B s\theta & -A s\theta & B s\theta & A s\theta \\ c\theta & c\theta & c\theta & c\theta \\ -\frac{b}{2} c\theta + k A s\theta & \frac{b}{2} c\theta - k A s\theta & \frac{b}{2} c\theta + k A s\theta & -\frac{b}{2} c\theta - k A s\theta \\ -\frac{a}{2} c\theta - k B s\theta & -\frac{a}{2} c\theta - k B s\theta & \frac{a}{2} c\theta + k B s\theta & \frac{a}{2} c\theta + k B s\theta \\ k c\theta & -k c\theta & k c\theta & -k c\theta \end{bmatrix} \begin{bmatrix} f_0 \\ f_1 \\ f_2 \\ f_3 \end{bmatrix} \label{eq:1-12} \end{equation}\] \[\begin{equation} \begin{aligned} f_x &= A s\theta (f_0 - f_2) - B s\theta (f_1 - f_3) \\ f_y &= -B s\theta (f_0 - f_2) - A s\theta (f_1 - f_3) \\ f_z &= c\theta (f_0 + f_1 + f_2 + f_3) \\ \tau_x &= (-\frac{b}{2} c\theta + k A s\theta) f_0 + (\frac{b}{2} c\theta - k A s\theta) f_1 + (\frac{b}{2} c\theta + k A s\theta) f_2 + (-\frac{b}{2} c\theta - k A s\theta) f_3 \\ \tau_y &= (-\frac{a}{2} c\theta - k B s\theta) f_0 + (-\frac{a}{2} c\theta - k B s\theta) f_1 + (\frac{a}{2} c\theta + k B s\theta) f_2 + (\frac{a}{2} c\theta + k B s\theta) f_3 \\ \tau_z &= k c\theta (f_0 - f_1 + f_2 - f_3) \end{aligned} \label{eq:1-13} \end{equation}\]

　　其中 $A = \frac{a}{\sqrt{a^2+b^2}}$, $B = \frac{b}{\sqrt{a^2+b^2}}$, $s\theta = \sin\theta$, $c\theta = \cos\theta$。

1.2.2 耦合证明

　　基于公式 \eqref{eq:1-13} 推导（此处略去详细推导过程）。

　　定义 4 个与力和力矩有关的组合量：

\[\begin{equation} \begin{aligned} \Gamma &= f_0 + f_1 + f_2 + f_3 \\ \Gamma_1 &= f_0 + f_1 - f_2 - f_3 \\ \Gamma_2 &= -f_0 + f_1 + f_2 - f_3 \\ \Gamma_3 &= f_0 - f_1 + f_2 - f_3 \end{aligned} \label{eq:1-14} \end{equation}\]

　　对于 $\tau_x$ 和 $\tau_y$：

\[\begin{equation} \tau_x = (\frac{b}{2} \cos\theta - k \frac{a}{\sqrt{a^2+b^2}} \sin\theta) \Gamma_2 \end{equation}\] \[\begin{equation} \tau_y = (-\frac{a}{2} \cos\theta - k \frac{b}{\sqrt{a^2+b^2}} \sin\theta) \Gamma_1 \label{eq:1-15} \end{equation}\]

1.3 长方形模型

1.3.1 动力分配矩阵

　　令 $\theta = 0$：

\[\begin{equation} \begin{bmatrix} f_x \\ f_y \\ f_z \\ \tau_x \\ \tau_y \\ \tau_z \end{bmatrix} = \begin{bmatrix} 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 1 & 1 & 1 & 1 \\ -\frac{b}{2} & \frac{b}{2} & \frac{b}{2} & -\frac{b}{2} \\ -\frac{a}{2} & -\frac{a}{2} & \frac{a}{2} & \frac{a}{2} \\ k & -k & k & -k \end{bmatrix} \begin{bmatrix} f_0 \\ f_1 \\ f_2 \\ f_3 \end{bmatrix} \label{eq:1-16} \end{equation}\]

1.4 加 2 垂直推进器 4, 5 模型

1.4.1 动力分配矩阵

　　位于 $4-[l, m, n], 5-[l, -m, n]$。

\[\begin{equation} F = \sum_{i=0}^{n-1} f_i \end{equation}\] \[\begin{equation} \tau = \sum_{i=0}^{n-1} (p_i \times f_i + \tau_i) \label{eq:1-17} \end{equation}\] \[\begin{equation} R_{0-3} = I, \quad R_{4-5} = Rot_y(90^\circ) \label{eq:1-18} \end{equation}\] \[\begin{equation} \begin{aligned} p_0 &= [ \frac{a}{2}, -\frac{b}{2}, 0 ]^T \\ p_1 &= [ \frac{a}{2}, \frac{b}{2}, 0 ]^T \\ p_2 &= [ -\frac{a}{2}, \frac{b}{2}, 0 ]^T \\ p_3 &= [ -\frac{a}{2}, -\frac{b}{2}, 0 ]^T \\ p_4 &= [ l, m, n ]^T \\ p_5 &= [ l, -m, n ]^T \end{aligned} \label{eq:1-19} \end{equation}\] \[\begin{equation} \begin{bmatrix} F \\ \tau \end{bmatrix} = \sum_{i=0}^{5} \begin{bmatrix} R_i [0,0,1]^T f_i \\ p_i \times R_i [0,0,1]^T f_i + R_i [0,0,1]^T \tau_i \end{bmatrix} \label{eq:1-20} \end{equation}\]

　　（注：$Rot_y(90)$ 表示绕 Y 轴旋转 90 度）

　　增加垂直推进器后的混合矩阵 $M$：

\[\begin{equation} M = \begin{bmatrix} 0 & 0 & 0 & 0 & 1 & 1 \\ 0 & 0 & 0 & 0 & 0 & 0 \\ 1 & 1 & 1 & 1 & 0 & 0 \\ -\frac{b}{2} & \frac{b}{2} & \frac{b}{2} & -\frac{b}{2} & S_4 k & S_5 k \\ -\frac{a}{2} & -\frac{a}{2} & \frac{a}{2} & \frac{a}{2} & n & n \\ S_0 k & S_1 k & S_2 k & S_3 k & -m & m \end{bmatrix} \label{eq:1-21} \end{equation}\]

1.5 SO3 控制器

　　对于机器人位置：

\[\begin{equation} F_T - F_{floating} + G = F_d \label{eq:1-22} \end{equation}\]

　　其中，$F_T$ 为世界系下作用到 base_link 的力，$F_{floating}$ 为浮力，$G$ 为重力，$F_d$ 为世界系下推进器期望的合力。

　　设计 PD + 前馈控制律为：

\(\begin{equation} e_p = x - x_{ref} \end{equation}\) \(\begin{equation} e_v = v - v_{ref} \end{equation}\) \(\begin{equation} F_d = -k_p e_p - k_d e_v + m a_{ref} - F_{floating} + m g e_z \label{eq:1-23} \end{equation}\)

　　对于机器人姿态动力学：

\(\begin{equation} \dot{R} = R [\omega]_\times \end{equation}\) \(\begin{equation} J \dot{\omega} + \omega \times J \omega = \tau \label{eq:1-24} \end{equation}\)

　　姿态误差:

\[\begin{equation} e_R = \frac{1}{2} (R_d^T R - R^T R_d)^\vee \label{eq:1-25} \end{equation}\]

　　角速度误差:

\[\begin{equation} e_\omega = \omega - R^T R_d \omega_d \label{eq:1-26} \end{equation}\]

　　控制律:

\[\begin{equation} \tau = -k_R e_R - k_\omega e_\omega + \omega \times J \omega - J ([\omega]_\times R^T R_d \omega_d - R^T R_d \dot{\omega}_d) \label{eq:1-27} \end{equation}\]

　　所有物理量均在机体坐标系下表示。