姿态解算与一个一般应用。

点与向量

区分点与向量：

• 点：位置信息，有大小，无方向。
• 向量：位移信息，有大小，有方向。由两个点连接起来。

空间中的点和向量是实际存在的实体，不会因为坐标系的改变而改变其本质。但是，我们需要用坐标系来描述这些实体。

对于一个 $n$ 维空间，我们用 $n$ 个线性不相关的向量来描述这个空间，这组向量称为一组基，有了基我们可以描述空间中的任意一个向量。规定了坐标系的原点后，我们将向量的起点平移到原点，这样我们就可以用一个 $n$ 维向量来表示空间中的任意一点。

如下是三维空间内的一组基，来表示空间中的任意向量。

$$\boldsymbol a= \begin{bmatrix} \boldsymbol e_1 & \boldsymbol e_2 & \boldsymbol e_3 \end{bmatrix} \begin{bmatrix} a_1\\ a_2\\ a_3 \end{bmatrix} =a_1\boldsymbol e_1+a_2\boldsymbol e_2+a_3\boldsymbol e_3$$

其中 $\boldsymbol e_1,\boldsymbol e_2,\boldsymbol e_3$ 是一组基，$[a_1,a_2,a_3]^\text T$ 是向量 $\boldsymbol a$ 在这组基下的坐标。

坐标系间的欧式变换

两个坐标系之间的变换，可以用一个旋转矩阵和一个平移向量来表示。

旋转变换

旋转矩阵

前面我们可以知道，对于同一个向量，他不会随着坐标系的选择而改变。因此，在坐标变换过程中有下面的等式。

$$[\boldsymbol e_1, \boldsymbol e_2, \boldsymbol e_3] \begin{bmatrix} \boldsymbol a_1\\ \boldsymbol a_2\\ \boldsymbol a_3 \end{bmatrix} = [\boldsymbol e'_1, \boldsymbol e'_2, \boldsymbol e'_3] \begin{bmatrix} \boldsymbol a'_1\\ \boldsymbol a'_2\\ \boldsymbol a'_3 \end{bmatrix}$$

两边同时左乘 $[\boldsymbol e_1, \boldsymbol e_2, \boldsymbol e_3]^\text T$ ，得到

$$\begin{bmatrix} a_1\\ a_2\\ a_3 \end{bmatrix} = \begin{bmatrix} \boldsymbol e_1^\text T\boldsymbol e'_1 & \boldsymbol e_1^\text T\boldsymbol e'_2 & \boldsymbol e_1^\text T\boldsymbol e'_3\\ \boldsymbol e_2^\text T\boldsymbol e'_1 & \boldsymbol e_2^\text T\boldsymbol e'_2 & \boldsymbol e_2^\text T\boldsymbol e'_3\\ \boldsymbol e_3^\text T\boldsymbol e'_1 & \boldsymbol e_3^\text T\boldsymbol e'_2 & \boldsymbol e_3^\text T\boldsymbol e'_3 \end{bmatrix} \begin{bmatrix} a'_1\\ a'_2\\ a'_3 \end{bmatrix} =\boldsymbol R\boldsymbol a'$$

其中 $\boldsymbol R$ 是旋转矩阵。由两组积的内积组成，刻画了旋转前后坐标变换的关系。

n 维旋转矩阵的集合是一个特殊的李群，称为特殊正交群 $\mathrm {SO}(n)$ 。

$$\mathrm{SO}(n)=\{\boldsymbol R\in\mathbb R^{n\times n}|\boldsymbol R^\text T\boldsymbol R=\boldsymbol I, \det(\boldsymbol R)=1\}$$

旋转向量和罗德里格斯公式

用上面的旋转矩阵旋转非常抽象，我们不能从中直接得到一个具体的旋转信息。也很难通过一个已知的旋转得到上面的旋转矩阵。

一个旋转可以通过一个旋转轴和旋转角度来描述。我们由右手定则确定旋转向量的方向，用一个单位向量 $\boldsymbol k$ 表示。由旋转角度的大小来确定旋转向量的大小，用 $\theta$ 表示。有了旋转向量我们可以描述空间中任意一个定轴的旋转。这种旋转与前面的旋转矩阵存在一定的联系，这种联系由罗德里格旋转公式给出。

$$\boldsymbol P_\text{rot}=\cos\theta\boldsymbol P+(1-\cos\theta)(\boldsymbol k\cdot\boldsymbol P)\boldsymbol k+\sin\theta\boldsymbol k\times\boldsymbol P$$

例如，我如果希望对一个向量 $[1\ 0\ 0]^\text T$ 绕 z 轴旋转 $\theta$ 角度，我们可以用罗德里格斯公式来描述这个旋转。旋转向量为 $\boldsymbol k=[0\ 0\ 1]^\text T$ 。代入公式，我们可以得到旋转后的向量。

$$\begin{aligned} \boldsymbol P_\text{rot}&=\cos\theta[1\ 0\ 0]^\text T+(1-\cos\theta)([0\ 0\ 1]^\text T\cdot[1\ 0\ 0]^\text T)[0\ 0\ 1]^\text T+\sin\theta[0\ 0\ 1]^\text T\times[1\ 0\ 0]^\text T\\ &=[\cos\theta\ \sin\theta\ 0]^\text T \end{aligned}$$

这就是一个在 xy 平面内的极坐标表达形式，与我们的期待结果吻合得很好。

罗德里格公式只给出了对一个向量进行旋转的方式，但是我们需要将其变化成一个旋转矩阵。

$$\boldsymbol P_\text{rot}=\boldsymbol R\boldsymbol P= (\cos\theta\boldsymbol I+(1-\cos\theta)\boldsymbol k\boldsymbol k^\text T+\sin\theta\boldsymbol K)\boldsymbol P$$

其中 $\boldsymbol K$ 是由 $\boldsymbol k$ 生成的反对称矩阵。反对称矩阵的特点是 $\boldsymbol K^\text T=-\boldsymbol K$ ，其实现了将向量的叉乘运算转化为矩阵乘法运算。

$$\boldsymbol k\times\boldsymbol P=\boldsymbol K\boldsymbol P$$

证明：

$$\boldsymbol k\times\boldsymbol P= \begin{bmatrix} k1\\ k2 \\ k3 \end{bmatrix} \times \begin{bmatrix} P1\\ P2 \\ P3 \end{bmatrix} = \begin{bmatrix} -P_3k_2+P_2k_3\\ P_3k_1-P_1k_3\\ -P_2k_1+P_1k_2 \end{bmatrix}$$

$$\begin{aligned} \boldsymbol K\boldsymbol P&= \begin{bmatrix} 0 & -k_3 & k_2\\ k_3 & 0 & -k_1\\ -k_2 & k_1 & 0 \end{bmatrix} \begin{bmatrix} P_1\\ P_2\\ P_3 \end{bmatrix}\\ &= \begin{bmatrix} -P_3k_2+P_2k_3\\ P_3k_1-P_1k_3\\ -P_2k_1+P_1k_2 \end{bmatrix} \end{aligned}$$

欧拉角描述旋转

欧拉角是一种常用的描述旋转的方式。欧拉角描述旋转的方式是，将一个复杂的旋转分解为三个简单的旋转。在三维空间中，我们通常使用 yaw-pitch-roll 来描述旋转。即绕 x 轴旋转 $\psi$ 角度，绕 y 轴旋转 $\theta$ 角度，绕 z 轴旋转 $\phi$ 角度。

习惯上，我们将绕 $x$ 轴的旋转称为 roll 或者滚转角，绕 $y$ 轴的旋转称为 pitch 或者俯仰角，绕 $z$ 轴的旋转称为 yaw 或者偏航角。

$$\boldsymbol R=\boldsymbol R_x(\psi)\boldsymbol R_y(\theta)\boldsymbol R_z(\phi)$$

值得注意的是，用欧拉角描述旋转时，旋转的顺序相当重要，不同的旋转顺序会得到不同的旋转矩阵，会产生不同的旋转效果。旋转矩阵的乘法遵循向量积的右结合律，即先进行右边的旋转。在上面的式子中，旋转顺序应当从右往左读，即为 yaw-pitch-roll 。

平移变换

平移变换是将坐标系的原点平移到另一个位置。平移变换可以用一个向量来表示。平移变换是一个线性变换，不改变向量的方向，只改变向量的位置。

$$\boldsymbol P'=\boldsymbol R\boldsymbol P+\boldsymbol t$$

其中 $\boldsymbol t$ 是平移向量。

变换矩阵与齐次坐标

在三维向量末尾加上一个 1 ，得到四维向量，称为齐次坐标。这样，旋转和平移可以统一表示为一个矩阵乘法。

$$\begin{bmatrix} \boldsymbol a'\\ 1 \end{bmatrix} = \begin{bmatrix} \boldsymbol R & \boldsymbol t\\ \boldsymbol 0^\text T & 1 \end{bmatrix} \begin{bmatrix} \boldsymbol a\\ 1 \end{bmatrix} =\boldsymbol T \begin{bmatrix} \boldsymbol a\\ 1 \end{bmatrix}$$

其中 $\boldsymbol R$ 是旋转矩阵，$\boldsymbol t$ 是平移向量。

n 维空间之间的变换矩阵的集和是一个特殊的李群，记为 $\mathrm{SE}(n)$ 。后文中，为了方便描述，坐标左乘 $\mathrm{SE}(n)$ 变换的时候不再将坐标先转成齐次坐标。

坐标系多次的转换

在实际应用中，我们可能需要多次的坐标系转换。这时，我们可以将多次的转换合并成一个转换矩阵。这样，我们可以用一个变换矩阵来描述从一个坐标系到另一个坐标系的变换。

$$\boldsymbol P_b=\boldsymbol T_{ba}\boldsymbol P_a$$

其中 $\boldsymbol T_{ab}=\boldsymbol T_{ac}\boldsymbol T_{cb}$ 。

具体实例

在一种常见自动瞄准发射云台结构中，我们通常定义了四个坐标系。

• 装甲板坐标系：建立在目标装甲板上，原点为击打中心， xy 平面张成装甲板平面， z 轴垂直于装甲板平面向外。
• 相机坐标系：建立在相机上，原点为光心， z 轴向上， x 轴向右， y 轴向前。
• 云台坐标系：建立在云台上，原点为云台 yaw 、 pitch 运动交点， z 轴垂直 yaw 平面向上， y 轴与枪口方向重合，垂直枪口向右。
• 世界坐标系：建立在世界中，原点与云台坐标系原点重合， z 轴竖直向上。xy 平面构成水平面。 xy的具体方向由陀螺仪零点时的位置决定。

本文规定当用下标表示相互的变换矩阵时，下标的右边表示变换前的坐标系，左边边表示变换后的坐标系。
例如 $\boldsymbol T_{wa}$ 表示从 armor 坐标变换到 world 坐标。即 $\boldsymbol T_{wa}\boldsymbol P_a=\boldsymbol P_w$ 。这种表达方式也与变换的右乘性质吻合得很好。

装甲板坐标系到相机坐标系

装甲板坐标系与相机坐标系的关系只与装甲板的位姿有关。装甲板的位姿通常采用 PnP 进行计算。

通过PnP解算可以得到装甲板坐标系到相机坐标系。rvec 和 tvec 。两者组合可以构成一个变换矩阵。但是 opencv 默认的相机坐标系为 z 轴向前， x 轴向右， y 轴向下。因此，我们需要对 rvec 和 tvec 进行变换。只需要左乘一个对应坐标轴的变换矩阵即可。

$$\boldsymbol T_{wa}= \text{SE3}\left( \begin{bmatrix} 1 & 0 & 0\\ 0 & 0 & 1\\ 0 & -1 & 0 \end{bmatrix},\boldsymbol 0 \right) \text{SE3}(\boldsymbol{R},\boldsymbol{t})$$

其中 $\boldsymbol{R}$ 和 $\boldsymbol{t}$ 分别是 PnP 解算得到的 rvec 和 tvec 。

相机坐标系到云台坐标系

相机坐标系与云台坐标系只与相机的安装方式有关。一般通过机械装配图纸得出，另外需要通过微调来修正装配过程中存在的误差，实践中可以发现，很多远距离的解算结果偏差都来自于相机装配过程中存在的微小角度误差。

同样可以有下面的式子。

$$\boldsymbol P_g=\boldsymbol T_{gc}\boldsymbol P_c$$

$\boldsymbol T_{gc}$ 同样可以拆分为旋转矩阵和平移向量。考虑如何确定旋转矩阵和平移向量。

我们首先从简单的问题开始考虑，现假定在机械设计上，相机与枪口存在在 pitch 角度的旋转 $\theta$ 和 在 z 轴上的一个平移 $t$ 。 $\theta$ 和 $t$ 定义在云台坐标系下，在下图中，应当是一个负值。

我们现在需要确定这一过程的转移矩阵。有一种很直观的方法是，现假定在相机坐标系下有一个向量 $[1\ 0\ 0]^\text T$ ，根据我们之前的定义，这个向量指向相机的正前方。如果需要在云台坐标系下表示同样的这个向量，我们需要将该向量向下旋转 pitch 角度。旋转矩阵由罗德里格斯公式给出， $\boldsymbol R=\boldsymbol R_y(\theta)$ 。

如果需要表达相机坐标系下 $[1\ 0\ 0]^\text T$ 这个点，我们还需要将其平移。平移向量是从云台坐标系原点，指向相机坐标系原点的向量。由于我们是先旋转再平移，故该向量需要用云台坐标系下的坐标来表示。平移向量为 $[0\ 0\ t]^\text T$ 。

综合上面两步，相机坐标系下的 $[1\ 0\ 0]^\text T$ 在云台坐标系下的表示即为 $R_y(\theta)[1\ 0\ 0]^\text T+[0\ 0\ t]^\text T$。

考虑该问题的完整形式，现假设相机和云台存在一个完整欧拉角的变换和一个完整的平移。相机和云台的欧拉角关系通常从云台出发，遵循 yaw-pitch-roll 的旋转顺序来描述从云台到相机的旋转。可是当我们需要从相机回到云台时，需要调换顺序，即 roll-pitch-yaw 。对应的旋转向量即为， $\boldsymbol R=\boldsymbol R_z(\phi)\boldsymbol R_y(\theta)\boldsymbol R_x(\psi)$ 。平移向量是从云台坐标系原点，指向相机坐标系原点的向量，需要在云台坐标系下表达这个向量。

通过上面两个例子，我们可以对这个过程进一步抽象。在求解从坐标系 A 到 B 的变换结果时，我们通常已知的是 B 的坐标系下 A 的姿态。通过前面的分析，我们发现旋转矩阵和平移矩阵在形式上存在一致性，我们虽然是需要将一个坐标从 A 转换到 B ，但我们在求解这一过程中的变换矩阵时带入的都是使用在 B 坐标系下到 A 的变换值。

综合上面两步，相机坐标系下的 $\boldsymbol P_c$ ，变换到云台坐标系下的结果即为 $\boldsymbol P_g=\boldsymbol P_c\boldsymbol R_z(\phi)\boldsymbol R_y(\theta)\boldsymbol R_x(\psi)+\boldsymbol t$ 。对应的变换为 $\text{SE3}(\boldsymbol R_z(\phi)\boldsymbol R_y(\theta)\boldsymbol R_x(\psi),\boldsymbol t)$ 。

云台坐标系到世界坐标系

云台坐标系到世界坐标系的变换是通过陀螺仪得到的。陀螺仪的零点时刻即为世界坐标系的原点。陀螺仪的输出即为云台坐标系到世界坐标系的变换。

一般来说我们将云台与世界坐标系的原点重合，不需要设计额外的平移。因此仅需将陀螺仪数据转换为旋转矩阵即可。过程与相机坐标系到云台坐标系的推导一致，请读者自行思考。

弹道方程

数学建模

弹丸的运动方程为：

$$\frac{\mathrm d\boldsymbol{v_w}}{\mathrm dt} = \boldsymbol{a} = -\frac{B}{m}\boldsymbol{v_w} + \boldsymbol{g}$$

其中 $\boldsymbol{v}$ 为弹丸速度， $\boldsymbol{a}$ 为加速度， $B$ 为弹丸空气阻力系数， $m$ 为弹丸质量， $\boldsymbol{g}$ 为重力加速度。

在云台坐标系下弹丸的初始条件为：

$$\boldsymbol{v_g}(0)= \begin{bmatrix} 0\\ v_0\\ 0 \end{bmatrix}$$

其中 $v_0$ 为弹丸初速度。

需要注意的是，在运动方程中，我们使用的是世界坐标系下的速度。因此，我们需要将速度从云台坐标系转换到世界坐标系。

$$\boldsymbol v_w = \boldsymbol T_{wg}\boldsymbol v_g$$

飞行路径的方程为：

$$\boldsymbol P_w(t) = \int_0^t \boldsymbol v_w(\tau)\mathrm d\tau$$

击打目标点约束：

$$\boldsymbol P(t_\text{hit}) = \boldsymbol P_{\text{target}}$$

综合上面的式子，我们可以求得唯一确定的 pitch 角 $\theta$ 和 yaw 角 $\phi$ 。

求解

无 roll 角的情况

在无 roll 角的情况下，我们可以直接控制 yaw 使得弹丸飞行平面与目标点重合。这样， yaw 的值可以直接确定。我们只需要最优化求解 pitch 角度即可。

首先将目标点投影到 xy 平面内，计算目标点的方位角，即为 yaw 角。

$$\phi = \arctan\left(\frac{P_{\text{target},y}}{P_{\text{target},x}}\right)$$

然后，我们需要求解 pitch 角度。由于考虑了空气阻力，我们需要求解上面的弹道微分方程。可以采用牛顿迭代法得到一个近似的数值解。

分别考虑弹丸在水平方向和竖直方向的飞行。

在水平面上，弹丸的初速度是 $v_0\cos\theta$ ，加速度是 $a=-\frac{B}{m}v_{xy}$ 。飞行距离是 $l$ 。列写弹丸在水平面上的运动方程：

$$\begin{aligned} \frac{\mathrm dv_{xy}}{\mathrm dt} &= -\frac{B}{m}v_{xy}\\ \frac{\mathrm dx}{\mathrm dt} &= v_{xy}\\ \end{aligned}$$

求解得：

$$\begin{aligned} v_{xy} &= v_0\cos\theta e^{-\frac{B}{m}t}\\ x &= \frac{m}{B}v_0\cos\theta(1-e^{-\frac{B}{m}t})\\ \end{aligned}$$

令 $x=l$ 可解得 $t$

$$t = \frac{m}{Bv_0\cos\theta}\ln\left(1-\frac{Bv_0\cos\theta}{m}l\right)$$

$$t=\frac{e^{B\cdot l}-1}{B\cdot v\cdot\cos\theta}$$

$$\begin{aligned} \theta &= \theta - \frac{\Delta z}{-\frac{v\cdot t}{\cos^2\theta}+\frac{g\cdot t^2}{v^2}\cdot\frac{\sin\theta}{\cos^3\theta}} \end{aligned}$$

其中 $\Delta z$ 为目标点与子弹飞行路径的高度差， $v$ 为子弹速度， $t$ 为子弹飞行时间， $g$ 为重力加速度， $l$ 为子弹投影到水平面的飞行距离， $B$ 为子弹空气阻力系数。

有 roll 角的情况

有 roll 角的情况与无 roll 角的不同，因为 roll 会使重力的方向不再与 yaw 平面垂直，也就不能直接采用前面的方法。

在无 roll 角的情况下，我们已经可以解出世界坐标系下云台的位姿。可以求解出弹丸的初速度

$$\boldsymbol v_{w0}= v_0 \begin{bmatrix} \cos\theta\cos\phi\\ \cos\theta\sin\phi\\ \sin\theta \end{bmatrix}$$

可以理解为，以这个初速度发射弹丸，一定可以击打中目标点。那么我们只需要将该速度转换到云台坐标系下，求解得到需要偏转的 yaw 和 pitch 即可。

$$\boldsymbol v_{g0}= \boldsymbol T_{gw}\boldsymbol v_{w0}$$

重新解出云台需要偏转的 yaw 和 pitch

$$\Delta\theta=\arctan\left(\frac{v_{g0,z}}{v_{g0,x}}\right)\\ \Delta\phi=\arctan\left(\frac{v_{g0,y}}{v_{g0,x}}\right)$$

最终云台需要到达的 yaw 和 pitch 就是

$$\theta=\theta_0+\Delta\theta\\ \phi=\phi_0+\Delta\phi$$