Rotation Kinematics

问题来源是 IMU 中 Gyroscope 测量的角速度实际含义，从而帮助理解 IMU 预积分过程中 Rotation Matrix 的积分过程（即文献 [1] 中公式 (30) 的第一个等式）。

解决这个问题，参考文献 [2] State Estimation for Robotics 的 6.2.4 Rotational Kinematics 与 6.4.4 Inertial Measurement Unit 。

1. 角速度测量值含义

在对文献 [2] 6.2.4 有基本印象之后，阅读文献[2] 6.4.4。

在文献 [2] 6.4.4 中，图 Figure 6.14 有三个坐标系——惯性（世界）坐标系 (underrightarrow{mathcal{F}_i})、载具坐标系 (underrightarrow{mathcal{F}_v})、IMU 坐标系 (underrightarrow{mathcal{F}_s})。

IMU 坐标系与载具坐标系不重合，所以 IMU 的测量值不能直接在载具坐标系在使用，需要进行转换。

文献[2]公式 (6.149) 给出了 Gyroscope 测量值 (mathbf{omega}) 与所需要的载具角速度 (mathbf{omega}^{vi}_v) 的关系：

[mathbf{omega} = mathbf{C}_{sv}mathbf{omega}^{vi}_v， ]

由文献 [2] 公式 (6.3)

[underrightarrow{mathcal{F}_1}^T = underrightarrow{mathcal{F}_2}^T mathbf{C}_{21} ]

得

[mathbf{omega} = underrightarrow{mathcal{F}_s}underrightarrow{mathcal{F}_v}^Tmathbf{omega}^{vi}_v， ]

由文献 [2] 公式 (6.40)

[underrightarrow{omega}_{21} = underrightarrow{mathcal{F}_2}^T mathbf{omega}^{21}_2 ]

得

[egin{align} mathbf{omega} &= underrightarrow{mathcal{F}_s}underrightarrow{mathbf{omega}}_{vi} otag \ underrightarrow{mathcal{F}_s}^T mathbf{omega} &= underrightarrow{mathbf{omega}}_{vi} end{align}，]

因为 IMU 与载具之间形成刚体，不存在相对运动，所以从角速度的空间向量（注意，并不是向量在某基底下的向量坐标）角度看，IMU 与载具的角速度相同。即

[underrightarrow{mathbf{omega}}_{si} = underrightarrow{mathbf{omega}}_{vi}。 ]

所以，认为角速度测量值

[mathbf{omega} = mathbf{omega}^{si}_s。 ]

注意 (underrightarrow{omega}_{21}) 的含义，在 6.2.4 有一句话引用如下

The angular velocity of frame 2 with respect to frame 1 is denoted by (underrightarrow{omega}_{21}).

所以 (underrightarrow{mathbf{omega}}_{si}) 理解为 IMU 坐标系 (underrightarrow{mathcal{F}_s}) 相对惯性坐标系 (underrightarrow{mathcal{F}_i}) 的角速度向量，(mathbf{omega}^{si}_s) 符号理解为IMU 坐标系 (underrightarrow{mathcal{F}_s}) 相对惯性坐标系 (underrightarrow{mathcal{F}_i}) 的角速度向量在 IMU 坐标系 (underrightarrow{mathcal{F}_s}) 下的坐标。

2. 从角速度积分

参考 6.2.4 仔细理解公式 (6.36)。这是从向量的角度理解角速度，而不是从向量坐标角度理解。

将此处的 (underrightarrow{mathcal{F}_1}) 对应惯性坐标系 (underrightarrow{mathcal{F}_i})，(underrightarrow{mathcal{F}_2}) 对应 IMU 坐标系 (underrightarrow{mathcal{F}_s})。

角速度在空间中可以表示为一个向量，按照右手螺旋的习惯，此向量方向标明了旋转方向，此向量的长度标明了角速度标量。

在某一时刻 (underrightarrow{mathcal{F}_1}, underrightarrow{mathcal{F}_2}) 之间存在角速度 (underrightarrow{omega}_{21})，考虑 (underrightarrow{mathcal{F}_2}) 各轴（也就是该坐标系基底所在的向量） (underrightarrow{2_1}, underrightarrow{2_2}, underrightarrow{2_3}) 对时间的变化率（当然是在 (underrightarrow{mathcal{F}_1}) 下观察的结果，在 (underrightarrow{mathcal{F}_2}) 下结果为 0，所谓相对运动）。得到以下结果（参考图理解）：

[egin{align} underrightarrow{2^{ullet}_1} = underrightarrow{omega}_{21} imes underrightarrow{2_1} \ underrightarrow{2^{ullet}_2} = underrightarrow{omega}_{21} imes underrightarrow{2_2} \ underrightarrow{2^{ullet}_3} = underrightarrow{omega}_{21} imes underrightarrow{2_3} end{align}]

现在考虑经过足够短的时间 (Delta t)，坐标系 (underrightarrow{mathcal{F}_2}) 经过 (Delta t) 时间移动到 (underrightarrow{mathcal{F}_2}^{prime}) 。

[egin{align} underrightarrow{2^{prime}_1} &= underrightarrow{2^{ullet}_1} Delta t + underrightarrow{2_1} otag \ &= (underrightarrow{omega}_{21} Delta t) imes underrightarrow{2_1} + underrightarrow{2_1} \ underrightarrow{2^{prime}_2} &= underrightarrow{2^{ullet}_2} Delta t + underrightarrow{2_2} otag \ &= (underrightarrow{omega}_{21} Delta t) imes underrightarrow{2_2} + underrightarrow{2_2} \ underrightarrow{2^{prime}_3} &= underrightarrow{2^{ullet}_3} Delta t + underrightarrow{2_3} otag \ &= (underrightarrow{omega}_{21} Delta t) imes underrightarrow{2_3} + underrightarrow{2_3} end{align}]

合并，写作

[underrightarrow{mathcal{F}^T_2}^{prime} = (underrightarrow{omega}_{21} Delta t) imes underrightarrow{mathcal{F}^T_2} + underrightarrow{mathcal{F}^T_2} ]

其中

[egin{align} underrightarrow{omega}_{21} &= underrightarrow{mathcal{F}^T_2} mathbf{omega}^{21}_2 \ underrightarrow{omega}_{21} &= underrightarrow{mathcal{F}^T_1} mathbf{omega}^{21}_1 end{align}]

考虑 (underrightarrow{mathcal{F}_1}, underrightarrow{mathcal{F}_2}) 之间的旋转矩阵 (mathbf{C}_{12})，在经过时间 (Delta t) 之后变化成 (mathbf{C}_{12}^{prime})。

则有

[egin{align} mathbf{C}_{12} &= underrightarrow{mathcal{F}_1}underrightarrow{mathcal{F}^T_2} \ {mathbf{C}_{12}^{prime}} &= underrightarrow{mathcal{F}_1}underrightarrow{mathcal{F}^T_2}^{prime} end{align}]

对于 ({mathbf{C}_{21}^{prime}}^T) 的计算比较困难。假设在这个空间中有一组单位正交基，为了简单起见将这组基设定在 (underrightarrow{mathcal{F}_1}) 的各条轴上（同时也是在 (underrightarrow{mathcal{F}_1}) 上求对时间的变化率），长度都为 1 。将以上这些向量都用它们在这组单位正交基下的坐标表示，考虑各个向量的坐标维度：

1. (underrightarrow{mathcal{F}_1}, underrightarrow{mathcal{F}_2})，(3 imes 3)，(underrightarrow{mathcal{F}_1}=egin{bmatrix} underrightarrow{1_1} \ underrightarrow{1_2} \ underrightarrow{1_3} end{bmatrix}, underrightarrow{mathcal{F}_2}=egin{bmatrix} underrightarrow{2_1} \ underrightarrow{2_2} \ underrightarrow{2_3} end{bmatrix})，矩阵的每一行都是对应坐标系基底在该坐标系下的坐标；
2. (underrightarrow{mathcal{F}^T_1}, underrightarrow{mathcal{F}^T_2})，(3 imes 3)，(underrightarrow{mathcal{F}^T_1}=egin{bmatrix} underrightarrow{1_1} & underrightarrow{1_2} & underrightarrow{1_3} end{bmatrix}, underrightarrow{mathcal{F}^T_2}=egin{bmatrix} underrightarrow{2_1} & underrightarrow{2_2} & underrightarrow{2_3} end{bmatrix})，矩阵的每一列都是对应坐标系基底在该坐标系下的坐标；
3. (underrightarrow{omega}_{21})，(3 imes 1)，并且 ({underrightarrow{omega}_{21}}_{3 imes 1} = underrightarrow{mathcal{F}^T_2}_{3 imes 3} {mathbf{omega}^{21}_2}_{3 imes 1}) 。

[egin{align} mathbf{C}_{12} &= underrightarrow{mathcal{F}_1}underrightarrow{mathcal{F}^T_2} otag \ &= mathbf{I} underrightarrow{mathcal{F}^T_2} otag \ &= underrightarrow{mathcal{F}^T_2} end{align}]

于是 (mathbf{C}_{12}) 的每一列都是 (underrightarrow{mathcal{F}_2}) 对应坐标系基底在 (underrightarrow{mathcal{F}_1}) 坐标系下的坐标。

[egin{align} mathbf{C}_{12}^{prime} &= underrightarrow{mathcal{F}_1}underrightarrow{mathcal{F}^T_2}^{prime} otag \ &= underrightarrow{mathcal{F}_1}((underrightarrow{omega}_{21} Delta t) imes underrightarrow{mathcal{F}^T_2} + underrightarrow{mathcal{F}^T_2}) otag \ &= mathbf{I}((mathbf{omega}^{21}_1Delta t)^{wedge} mathbf{C}_{12} + mathbf{C}_{12}) otag \ &= mathbf{C}_{12} + (mathbf{omega}^{21}_1 Delta t)^{wedge} mathbf{C}_{12} otag \ &= mathbf{C}_{12} + mathbf{C}_{12} mathbf{C}_{12}^T (mathbf{omega}^{21}_1Delta t)^{wedge} mathbf{C}_{12} otag \ &= mathbf{C}_{12} + mathbf{C}_{12} (mathbf{C}_{12}^T mathbf{omega}^{21}_1 Delta t)^{wedge} otag \ &= mathbf{C}_{12} + mathbf{C}_{12} (mathbf{omega}^{21}_2 Delta t)^{wedge} otag \ &= mathbf{C}_{12} (mathbf{I} + (mathbf{omega}^{21}_2 Delta t)^{wedge}) otag \ &= mathbf{C}_{12} ext{Exp}(mathbf{omega}^{21}_2 Delta t) end{align}]

以上第三个等号，可以看做是将所有向量都投影到 (underrightarrow{mathcal{F}_1}) 坐标系下进行后续的运算。因为旋转矩阵不是坐标系中的坐标，所以上式的第一个等号没有错。

变换一下坐标系的表示方式，并且将 ({mathbf{C}_{12}^{prime}}) 写作 (mathbf{C}_{12}(t+Delta t))。(mathbf{omega}^{si}_s) 对应 (mathbf{omega}^{21}_2)，所以上式可以写作：

[egin{align} {mathbf{C}_{is}(t+Delta t)} &= mathbf{C}_{is}(t) ext{Exp}(mathbf{omega}^{si}_s Delta t)label{eq1:R} end{align}]

至此，得到 IMU 姿态积分公式。

3. 微分方程

3.1. 旋转矩阵的微分

旋转矩阵微分 (dot{mathbf{C}_{is}}(t)) 的定义是：

[egin{align} {mathbf{C}_{is}(t+Delta t)} &= mathbf{C}_{is}(t) + dot{mathbf{C}_{is}}(t) Delta t end{align}]

所以现在从公式 (( ef{eq1:R})) 出发，凑出以上公式，所以需要分解 ( ext{Exp}(mathbf{omega}^{si}_s Delta t))，要求 (mathbf{omega}^{si}_s Delta t) 很小，可以做以下分解：

[egin{align} {mathbf{C}_{is}(t+Delta t)} &= mathbf{C}_{is}(t) ext{Exp}(mathbf{omega}^{si}_s Delta t) otag \ &= mathbf{C}_{is}(t) (mathbf{I} + {mathbf{omega}^{si}_s}^{wedge} Delta t) otag \ &= mathbf{C}_{is}(t) + mathbf{C}_{is}(t) {mathbf{omega}^{si}_s}^{wedge} Delta t end{align}]

得到微分 (dot{mathbf{C}_{is}}(t))：

[egin{align} dot{mathbf{C}_{is}}(t) = mathbf{C}_{is}(t) {mathbf{omega}^{si}_s}^{wedge} label{eq3.1.3} end{align}]

3.2. 四元数的微分

以下使用 Hamilton 形式四元数。

从语义上理解，公式 (( ef{eq1:R})) 表示 Rotation Matrix (mathbf{C}_{is}) 在时刻 (t) 的值为 (mathbf{C}_{is}(t))，在此基础上经过 (Delta t) 的时间，在时刻 (t + Delta t) 的值为 (mathbf{C}_{is}(t) ext{Exp}(mathbf{omega}^{si}_s(t) Delta t))，即在 (mathbf{C}_{is}(t)) 的基础上被右乘了一个矩阵 ( ext{Exp}(mathbf{omega}^{si}_s Delta t))，这个矩阵表示的旋转轴角为 (mathbf{omega}^{si}_s(t) Delta t)。（注意，不是在 (mathbf{C}_{is}(t)) 的基础上继续旋转 (mathbf{omega}^{si}_s(t) Delta t)，因为此处 (mathbf{C}_{is}(t)) 是被右乘。）按照这个旋转的含义，将公式中的 Rotation Matrix 换成 quaternion 。

[egin{align} mathbf{q}_{is}(t + Delta t) &= mathbf{q}_{is}(t) otimes q{ mathbf{omega}^{si}_s(t) Delta t } otag \ &= mathbf{q}_{is}(t) otimes egin{bmatrix} 1 \ {1 over 2}mathbf{omega}^{si}_s(t) Delta t end{bmatrix} otag \ &= left(mathbf{I} + egin{bmatrix} 0 & - {1 over 2}{mathbf{omega}^{si}_s(t)}^T Delta t \ {1 over 2}mathbf{omega}^{si}_s(t) Delta t & -{1 over 2}{mathbf{omega}^{si}_s(t)}^{wedge} Delta t end{bmatrix} ight) mathbf{q}_{is}(t) phantom{ } phantom{ } phantom{ } (论文 [3] 公式 (19)) otag \ &= mathbf{q}_{is}(t) + {1 over 2}egin{bmatrix} 0 & - {mathbf{omega}^{si}_s(t)}^T \ mathbf{omega}^{si}_s(t) & -{mathbf{omega}^{si}_s(t)}^{wedge} end{bmatrix} mathbf{q}_{is}(t) Delta t end{align}]

由定义可以得到 quaternion 的微分方程。

[egin{align} dot{mathbf{q}_{is}}(t) &= {1 over 2}egin{bmatrix} 0 & - {mathbf{omega}^{si}_s(t)}^T \ mathbf{omega}^{si}_s(t) & -{mathbf{omega}^{si}_s(t)}^{wedge} end{bmatrix} mathbf{q}_{is}(t) otag \ &= {1 over 2} mathbf{q}_{is}(t) otimes egin{bmatrix} 0 \ mathbf{omega}^{si}_s(t) end{bmatrix} otag \ &= {1 over 2} mathbf{q}_{is}(t) otimes mathbf{omega}^{si}_s(t) end{align}]

4. 附录：角速度的方向

上文中使用到了角速度 (mathbf{omega}^{si}_s)，理解为IMU 坐标系 (underrightarrow{mathcal{F}_s}) 相对惯性坐标系 (underrightarrow{mathcal{F}_i}) 的角速度向量在 IMU 坐标系 (underrightarrow{mathcal{F}_s}) 下的坐标。

(mathbf{omega}^{is}_s) 与 (mathbf{omega}^{si}_s) 都是在 (underrightarrow{mathcal{F}_s}) 下的坐标，只不过相对关系相反。相对关系相反，表示它们所表示的这个向量在空间中反向，而在量值上他们相等，所以这两个向量是反向量关系。反向量在同一个坐标系下的坐标是相反数，(mathbf{omega}^{is}_s = -mathbf{omega}^{si}_s) 。

角速度的推导参考 [4] 4.1.2 李代数的引出。( ef{eq3.1.3}) 省略时间 (t)，变形得到以下。

[egin{align} mathbf{C}_{is}^Tdot{mathbf{C}_{is}} = {mathbf{omega}^{si}_s}^{wedge} end{align}]

然而上式并不能与 [4] 的公式对应。但是它能与 [3] 的公式 (67) 对应，所以通过上式我们能够得到 [3] 中旋转向量的方向。

正常情况下一般如同 [4] 一样，从 (mathbf{C}mathbf{C}^T=mathbf{I}) 两边对时间取微分做；而不是同 [3] 一样，从 (mathbf{C}^Tmathbf{C}=mathbf{I}) 做。

参考文献

[1] Forster, Christian, Luca Carlone, Frank Dellaert, and Davide Scaramuzza. "On-Manifold Preintegration for Real-Time Visual--Inertial Odometry." IEEE Transactions on Robotics 33, no. 1 (2016): 1-21.

[2] Barfoot, Timothy D. State Estimation for Robotics. Cambridge University Press, 2017.

[3] Sola, Joan. "Quaternion kinematics for the error-state KF." Laboratoire dAnalyse et dArchitecture des Systemes-Centre national de la recherche scientifique (LAAS-CNRS), Toulouse, France, Tech. Rep (2012).

[4] 视觉SLAM十四讲.