相机自运动估计

前言

自运动估计（Ego-Motion Estimation）是机器人领域和机器视觉领域基础问题，是解决后续GNC问题的基础。其核心的计算问题是，通过视觉传感器获取的一系列视觉特征的约束关系，解算出某个时刻处的传感器速度与位移等物理量，这里的视觉传感器包括但不限于普通光学相机、激光雷达、事件相机、声呐、超声雷达、毫米波雷达以及红外成像传感器等。

数学基础

自运动估计需要用到的预备数学知识有坐标系变换、科氏方程、旋转矩阵。

概念声明

由于坐标系变换与科氏方程有概念非常容易被混淆，首先声明以下两个概念：

• 表示：针对空间矢量而言，其使用某套坐标系基底，或是其在某个坐标系下的投影，称为该矢量在某个坐标系下表示。
• 观测：针对矢量端点而言，该端点在某个坐标系下的向量值，或称从不同坐标系的原点出发到达相同端点的视线向量，称为该矢量（端点）在某个坐标系下的观测。

为了便于区别概念，表示使用上标，在坐标系$m$下表示的矢量$\mathbf{X}$记为$\mathbf{X}^m$；观测使用竖线分隔的下标，在坐标系$m$下观测的矢量端点$\mathbf{X}$记为$\mathbf{X}_{|m}$。如图所示，其中任意一个矢量如$\mathbf{R}_i,\mathbf{R}_c,\mathbf{R}_{ic}$都可以投影至不同坐标系下，如将矢量$\mathbf{R}_i$投影至$i$系$\mathbf{R}_i^i$，投影至$c$系$\mathbf{R}_i^c$等；矢量端点$P$在$i$系下的观测为$\mathbf{R}_{i|i}$，在$c$系下的观测为$\mathbf{R}_{c|c}$，同理，矢量端点$O_c$在$i$系下的观测为$\mathbf{R}_{ic|i}$，在不引起混淆的前提下，可以忽略竖线分隔右侧的下标。

因此，表示是针对同一个矢量说明的，而观测则是针对同一个端点在不同坐标系下说明的，注意区分这两个概念。

坐标系变换

低速运动下，某个质点的位移在不同坐标系下服从伽利略变换，如上图，某个质点$P$在坐标系$i,c$下的不同观测位移值满足线性叠加关系：

$$\mathbf{R}_{i|i} = \mathbf{R}_{c|c} + \mathbf{R}_{ic|i} \Leftrightarrow \mathbf{R}_{i} = \mathbf{R}_{c} + \mathbf{R}_{ic}$$

同理，速度、加速度、角速度等物理量也满足伽利略变换（前提是各个矢量构成矢量三角形）。

注意角位置并不是矢量

科氏方程

科氏方程是描述同一矢量的变化量在不同坐标系下观测值关系，其形式为：

$$\dot{\mathbf{X}}_{|i}=\dot{\mathbf{X}}_{|c} + \bm{\omega}_{ic}\times{\mathbf{X}}$$

其中$\dot{\mathbf{X}}_{|i}$是矢量$\mathbf{X}$在$i$系下的观测变化率，$\dot{\mathbf{X}}_{|c}$是矢量$\mathbf{X}$在$c$系下的观测变化率，$\bm{\omega}_{ic}$是$i$系相对于$c$系的角速度。例如在上图中，点$P$位移的变化量$\dot{\mathbf{R}}$在$i,c$两个坐标系下的观测值关系为：

$$\dot{\mathbf{R}}_{|i}=\dot{\mathbf{R}}_{|c} + \bm{\omega}_{ic}\times{\mathbf{R}}$$

需要注意的是：$\dot{\mathbf{R}}_{|c}\ne\dot{\mathbf{R}}_c$。

旋转矩阵

旋转矩阵是描述同一矢量在不同坐标系下的表示值关系，其形式为：

$$\mathbf{X}_i^c = \mathbf{C}^{ci}\mathbf{X}_i^i$$

其中，$\mathbf{C}^{ci}$表示从$i$坐标系向$c$坐标系的旋转矩阵，旋转矩阵的概念比较通俗，此处不再赘述。

相机自运动数学基础

仍以图表示的情形为例，假设$O_i$坐标系为惯性参考系，$O_c$坐标系与相机固连，相机在空间中作6自由度运动，质点$P$在空间中作3轴平移，根据伽利略变换公式，在惯性坐标系$i$下对时间求导可得：

$$\left.\frac{d\mathbf{R}_i}{dt}\right|_i =\left. \frac{d\mathbf{R}_c}{dt}\right|_i + \left.\frac{d\mathbf{R}_{ic}}{dt}\right|_i$$

此时，由于$\mathbf{R}_i$和$\mathbf{R}_ic$均为惯性坐标系下的观测值，与求导的观测坐标系保持一致，它们可以直接计算出速度值：

$$\begin{aligned} \left.\frac{d\mathbf{R}_i}{dt}\right|_i &= \mathbf{v}_i \\ \left.\frac{d\mathbf{R}_{ic}}{dt}\right|_i &= \mathbf{v}_{ic} \end{aligned}$$

其中，$\mathbf{v}_i$表示点$P$的惯性速度，$\mathbf{v}_{ic}$表示坐标系$O_c$的惯性平动速度。对于右式第一项，由于在$c$系下观测的点$P$位置对$i$系求导，观测坐标系与求导坐标系并不一致，此时需要使用科氏方程公式进行转换：

$$\left.\frac{d\mathbf{R}_c}{dt}\right|_i = \left.\frac{d\mathbf{R}_c}{dt}\right|_c + \bm{\omega}_{ic}\times{\mathbf{R}}_c= \mathbf{v}_c + \bm{\omega}_{ic}\times{\mathbf{R}}_c$$

这里，$\mathbf{v}_c$表示点$P$在$c$系下的观测线速度，即相对$c$系的速度，后一项表示由于$c$系角运动产生的牵连转动线速度。将以上两式代入公式可得：

$$\mathbf{v}_i = \mathbf{v}_c + \bm{\omega}_{ic}\times{\mathbf{R}}_c + \mathbf{v}_{ic}$$

即求得了点$P$在$i$坐标系下的惯性速度，该公式是相机自运动估计的数学基础。通常讨论的是以相机坐标系$c$作为投影坐标系的相机自运动估计，将以上各个矢量投影至相机坐标系下，可进一步写为：

$$\mathbf{v}_i^c = \mathbf{v}_c^c + \bm{\omega}_{ic}^c\times{\mathbf{R}}_c^c + \mathbf{v}_{ic}^c$$

这里用到了向量积的双线性公式$\mathbf{(Ra)\times b}=(\mathbf{Ma})\times(\mathbf{Mb})$，其中$\mathbf{M}$为旋转矩阵

相机自运动情形

根据点$P$与$O_c$坐标系的运动情况，可以将相机自运动分为以下几种情形：

相机纯平移，点静止

相机纯平移即$\bm{\omega}_{ic}=\mathbf{0}$，由于点静止，则$\mathbf{v}_i=\mathbf{0}$，由此可得：

$$\mathbf{v}_{ic}^c = -\mathbf{v}_c^c$$

即相机中观测到的点运动速度的相反数就是相机自运动速度。

相机纯平移，点运动

此时仍然有$\bm{\omega}_{ic}=\mathbf{0}$，则有：

$$\mathbf{v}_{ic}^c = \mathbf{v}_i^c - \mathbf{v}_c^c$$

即求解相机自运动的速度需要在已知当前点的惯性运动条件下，减去相机坐标系下的的点运动相对速度。

相机纯旋转，点静止

此时有$\mathbf{v}_i=\mathbf{0}$，$\mathbf{v}_{ic}=\mathbf{0}$，则有：

$$\bm{\omega}_{ic}^c\times{\mathbf{R}}_c^c = -\mathbf{v}_c^c$$

相机自运动的速度，可由点在相机坐标系下的观测速度与点的位置矢量的建立最小二乘方程求解。

相机纯旋转，点运动

此时仅有$\mathbf{v}_{ic}=\mathbf{0}$，则有：

$$\bm{\omega}_{ic}^c\times{\mathbf{R}}_c^c = \mathbf{v}_i^c - \mathbf{v}_c^c$$

具体情况与相机纯旋转，点静止类似，需要额外已知点的运动场后才能求解相机自运动的速度。

相机平移+旋转，点静止

这种情况适用于相机在静止的场景内六轴自由运动，此时有$\mathbf{v}_i=\mathbf{0}$，则有：

$$\mathbf{v}_{ic}^c + \bm{\omega}_{ic}^c\times{\mathbf{R}}_c^c = -\mathbf{v}_c^c$$

此时相机的线速度和角速度都需要通过点的观测速度与位置矢量建立方程求解。

相机平移+旋转，点运动

这是最一般的运动情况，通常适用于航空航天领域的导航与控制，此时的质点大多取为运载体本身，而相机坐标系则通常取为地球坐标系或其他星球的坐标系，有：

$$\mathbf{v}_{ic}^c + \bm{\omega}_{ic}^c\times{\mathbf{R}}_c^c = \mathbf{v}_i^c - \mathbf{v}_c^c$$

此时需要已知星球的运动场，才能求解载体自运动的速度，通常该情况在相机自运动领域应用较少。