简介
这里总结整理了移动机器人一些常用的对位姿的描述方法,包括变换矩阵,旋转向量以及四元数。
变换矩阵 ——— 旋转矩阵和平移向量
移动机器人领域中有不同的坐标系,如:
- 世界坐标系 W
- IMU 坐标系 I
- 相机坐标系 C
坐标系之间的转换关系通常可以由一个变换矩阵(李群 SE(3) )表示,其包括一个旋转矩阵和平移向量,这里用 $\boldsymbol{T_{WI}}$
来表示 I 到 W 系之间的变换矩阵:
$$
\boldsymbol{T}_{WI} =
\begin{bmatrix}
\boldsymbol{R}_{WI} & \boldsymbol{t}_{WI} \\
\boldsymbol{0}^T & 1
\end{bmatrix}
\in \mathbb{R}^{4\times 4}
$$
$\boldsymbol{R}$
为$3\times 3$
的旋转矩阵,$\boldsymbol{t}_{WI}$
为平移向量(也是 IMU 在 世界坐标系 W )的位置坐标- 上式中,
$\boldsymbol{T}$
也表示了相机在世界坐标系中的位姿,用$\boldsymbol{p}^I$
表示在 I 系下的一个点,用$\boldsymbol{T}$
右乘上$\boldsymbol{p}^I$
就可以得到该点在 W 系下的齐次坐标, 即:$\boldsymbol{T}_{WI}\boldsymbol{p}^I = \boldsymbol{p}^W$
同理,不只是点,对于其他矢量(例如速度)也可以用同样的方法进行变换。要仔细分清楚 $\boldsymbol{T_{WI}}$
和 $\boldsymbol{T_{IW}}$
的区别(哪个系在哪个系下的位姿)以及联系(互为逆),在进行点的坐标转换时要注意使用哪个变换矩阵。
- 连续对坐标系进行两次变换可以直接对两次变换进行相乘,
$T = T_1T_2$
旋转矩阵常用的性质
- 伴随 (证明过程 TODO)
如下:
$$
\begin{aligned}
\boldsymbol{R^\mathrm{T}}\exp{(\boldsymbol{p}^\wedge)}\boldsymbol{R} &= \exp{((\boldsymbol{R^\mathrm{T}p})^\wedge)} \\
\Rightarrow\exp{(\boldsymbol{p}^\wedge)}\boldsymbol{R} &= \boldsymbol{R}\exp{((\boldsymbol{R^\mathrm{T}p})^\wedge)}
\end{aligned}
$$
四元数
对于旋转除了旋转矩阵 $\boldsymbol{R}$
以外另一种表达方式是四元数 $\boldsymbol{q}$
。顾名思义四元数由四个部分组成,其分别有一个实部和三个虚部,可以用以下几种形式表示:
$$
\begin{aligned}
\boldsymbol{q} &=
\begin{bmatrix}
q_0 & q_1 & q_2 & q_3
\end{bmatrix}^T \\
&=
\begin{bmatrix}
w & x & y & z
\end{bmatrix}^T \\
&=
\begin{bmatrix}
s & \boldsymbol{v}
\end{bmatrix}^T
\end{aligned}
$$
这里我们将实部放在前面,在不同情况下(不同代码,论文,教材等)也可能将实部放在后面,在使用的时候需要注意表示方式。由于虚部是矢量,所以在上述第三种表示方法也可以用一个标量和一个矢量来表示四元数。
四元数的运算
四元数支持各种运算方式,这里以乘法为例:
$$
\begin{aligned}
\boldsymbol{q}_a \otimes \boldsymbol{q}_b
=& \quad\, \ w_aw_b - x_ax_b - y_ay_b - z_az_b \\
&+ (w_ax_b + x_aw_b + y_az_b - z_ay_b)i \\
&+ (w_ay_b - x_az_b + y_aw_b + z_ax_b)j \\
&+ (w_az_b + x_ay_b - y_ax_b + z_aw_b)k \\
=&\begin{bmatrix}
s_as_b - \boldsymbol{v}_a^T\boldsymbol{v}_b^T & s_a\boldsymbol{v}_b + s_b\boldsymbol{v}_a + \boldsymbol{v}_a\times \boldsymbol{v}_b
\end{bmatrix}
\end{aligned}
$$
乘法运算中,向量的计算方法比较容易记忆,相乘后实部为两实部相乘减去两虚部正交,虚部为两个四元数和实部和虚部交叉相乘的和加上虚部的叉积。连续旋转两个角度可以用这两个旋转对应的四元数的乘积来表示,即 $\boldsymbol{q}(t_1+t_2) = \boldsymbol{q}(t_1)\boldsymbol{q}(t_2)$
四元数的运算性质
- 四元数对加法封闭,即
$\boldsymbol{q}_1 + \boldsymbol{q}_2$
也是四元数 - 四元数的逆为其共轭,即
$\boldsymbol{q}^{-1} = \boldsymbol{q}^*$
- 四元数服从乘法结合律,即
$\boldsymbol{q}\otimes(\boldsymbol{x}\otimes\boldsymbol{p}) = \boldsymbol{q}\otimes\boldsymbol{x}\otimes\boldsymbol{p}$
- 四元数服从乘法分配律,有:
$\boldsymbol{p}\otimes(\boldsymbol{x} + \boldsymbol{q}) = \boldsymbol{p}\otimes\boldsymbol{x} + \boldsymbol{p}\otimes\boldsymbol{q}$
- 四元数的乘法可以转换为矩阵乘法,如下所示:
有两种形式,可以将左边的操作数转为矩阵,即:
$$
\begin{aligned}
\boldsymbol{p}\otimes\boldsymbol{q} &= [\boldsymbol{p}]_L\boldsymbol{q}\\
&= \begin{bmatrix}
p_w & -p_x & -p_y & -p_z\\
p_x & p_w & -p_z & p_y\\
p_y & p_z & p_w & -p_x\\
p_z & -p_y & p_x & p_w
\end{bmatrix}\boldsymbol{q}\\
&= (\begin{bmatrix}
0 & -\boldsymbol{p}_v^T\\
\boldsymbol{p}_v & p_w\boldsymbol{I} +\boldsymbol{p}_v^\wedge
\end{bmatrix})\boldsymbol{q}
\end{aligned}
$$
也可以将右边的操作数转换为矩阵:
$$
\begin{aligned}
\boldsymbol{p}\otimes\boldsymbol{q} &= [\boldsymbol{q}]_R\boldsymbol{p}\\
&= \begin{bmatrix}
q_w & -q_x & -q_y & -q_z\\
q_x & q_w & q_z & -q_y\\
q_y & -q_z & q_w & q_x\\
q_z & q_y & -q_x & q_w
\end{bmatrix}\boldsymbol{p}\\
&= (\begin{bmatrix}
0 & -\boldsymbol{q}_v^T\\
\boldsymbol{q}_v & q_w\boldsymbol{I} -\boldsymbol{q}_v^\wedge
\end{bmatrix})\boldsymbol{p}
\end{aligned}
$$
由上述性质,可以推导出两个重要性质,在四元数求导时会经常用到:
$(\boldsymbol{q}\otimes\boldsymbol{x})\otimes\boldsymbol{p} = ([\boldsymbol{q}]_L\boldsymbol{x})\otimes\boldsymbol{p} = [\boldsymbol{p}]_R[\boldsymbol{q}]_L\boldsymbol{x}$
$\boldsymbol{q}\otimes(\boldsymbol{x}\otimes\boldsymbol{p}) = \boldsymbol{q}\otimes([\boldsymbol{p}]_R\boldsymbol{x}) = [\boldsymbol{q}]_L[\boldsymbol{p}]_R\boldsymbol{x}$
$[\boldsymbol{p}]_R[\boldsymbol{q}]_L=[\boldsymbol{q}]_L[\boldsymbol{p}]_R$
- 对于单位四元数,有:
$([\boldsymbol{p}^{-1}]_L)_{3\times 3} = ([\boldsymbol{p}]_R)_{3\times3}$
,下标表示右下角$3\times3$
的矩阵块
四元数对时间求导
我们说四元数是除了旋转矩阵以外的另一种对旋转表达方式,并且它不具备奇异性,可以表达任意三维旋转,因此有必要学习一下它对时间的求导方式。
首先来看四元数和角轴的转换关系,假设某个旋转运动的旋转轴为单位向量 $\boldsymbol{u}$
, 绕该轴旋转的角度为 $\theta$
那这个旋转的对应的四元数可表示为:
$$
\boldsymbol{q} = \begin{bmatrix}
\cos{\frac{\theta}{2}} \\
\boldsymbol{u}\sin{\frac{\theta}{2}}
\end{bmatrix}
$$
对时间求导:当旋转一段微小时间时,旋转经过角度可以视为趋于 $0$
,因此上述四元数为:
$$
\begin{aligned}
\Delta \boldsymbol{q}
&= \begin{bmatrix}
\cos{\frac{\theta}{2}} \\
\boldsymbol{u}\sin{\frac{\theta}{2}}
\end{bmatrix} \\
&\approx \begin{bmatrix}
1 \\
\boldsymbol{u}\frac{\delta \theta}{2}
\end{bmatrix} \\
&= \begin{bmatrix}
1 \\
\frac{1}{2} \delta\boldsymbol{\theta}
\end{bmatrix}
\end{aligned}
$$
其中,$\delta\boldsymbol{\theta}$
的方向为旋转轴,模长为(这段时间内非常微小的)旋转角度。
下面对时间求导:
$$
\begin{aligned}
\dot{\boldsymbol{q}} &= \lim_{\Delta t \rightarrow 0}\frac{\boldsymbol{q}(t+\Delta{t})- \boldsymbol{q}(t)}{\Delta{t}} \\
&= \lim_{\Delta t \rightarrow 0}\frac{\boldsymbol{q}(t)\otimes\boldsymbol{q}(\Delta{t})- \boldsymbol{q}(t)}{\Delta{t}} \\
&= \lim_{\Delta t \rightarrow 0}\frac{\boldsymbol{q}\otimes\Delta\boldsymbol{q}- \boldsymbol{q}}{\Delta{t}} \\
&= \lim_{\Delta t \rightarrow 0}\frac{\boldsymbol{q}\otimes(
\begin{bmatrix}
1 \\ \frac{1}{2}\delta\boldsymbol{\theta}
\end{bmatrix}
- \begin{bmatrix}
1 \\ \boldsymbol{0}
\end{bmatrix})}{\Delta{t}} \\
&= \boldsymbol{q}\otimes\lim_{\Delta t \rightarrow 0}\frac{(
\begin{bmatrix}
0 \\ \frac{1}{2}\delta\boldsymbol{\theta}
\end{bmatrix}
)}{\Delta{t}} \\
&= \boldsymbol{q}\otimes\begin{bmatrix}
0 \\
\frac{1}{2}\boldsymbol{\omega}
\end{bmatrix}
\end{aligned}
$$
上式中,$\boldsymbol{\omega} = \lim_{\Delta t \rightarrow 0}\frac{\delta\boldsymbol{\theta}}{\Delta{t}}$
是求导时间的角速度。
旋转向量
除了旋转向量以及四元数以外,旋转还可以用旋转向量来描述,即一个旋转轴(单位向量)$\boldsymbol{n}$
和旋转角 $\theta$
来表示,旋转可以表示成 $(\theta\boldsymbol{n})$
。旋转矩阵和旋转向量的转换关系可以由罗德里格斯公式表明:
$$
\boldsymbol{R} = \boldsymbol{\mathrm{\cos{\theta}}I + \mathrm{(1-\cos{\theta})}nn^\mathrm{T} + \mathrm{\sin{\theta}}n^\wedge}
$$
证明过程可以参考下一部分中证明李代数的指数映射推导过程。 而从旋转矩阵到旋转向量的过程可以利用矩阵迹(trace)的性质进行推导:
$$
\begin{aligned}
\mathrm{tr}(\boldsymbol{R}) &= \boldsymbol{\mathrm{\cos{\theta}\ tr}(I) + \mathrm{(1-\cos{\theta})\ tr}(nn)^\mathrm{T} + \mathrm{\sin{\theta}}\ n^\wedge} \\
&= 3\cos{\theta} + (1 - \cos{\theta}) \\
&= 1 + 2\cos{\theta} \\
\Rightarrow \theta &= \arccos{\frac{\mathrm{tr}(\boldsymbol{R}) - 1}{2}}
\end{aligned}
$$
李群和李代数
第一部分提到的变换矩阵 $\mathrm{SE}(3)$
和旋转矩阵 $\mathrm{SO}(3)$
都属于李群,满足李群的所有性质,关于李群的简单介绍可以参考维基百科。这里主要通过对旋转矩阵 $\mathrm{SO}(3)$
的求导来引出李代数 $\mathfrak{so}(3)$
的概念。
对于任意时间旋转矩阵 $\boldsymbol{R}$
满足:
$$
\boldsymbol{R}(t)\boldsymbol{R}(t)^T = \boldsymbol{R}(t)^T\boldsymbol{R}(t)= \boldsymbol{I}
$$
我们对等式两次对时间求导:
$$
\begin{aligned}
&\dot{\boldsymbol{R}(t)}\boldsymbol{R}(t)^T + \boldsymbol{R}(t)\dot{\boldsymbol{R}(t)^T} = 0\\
\Rightarrow&\dot{\boldsymbol{R}(t)}\boldsymbol{R}(t)^T = -(\dot{\boldsymbol{R}(t)}\boldsymbol{R}(t)^T)^T
\end{aligned}
$$
通过观察,可以看出 $\dot{\boldsymbol{R}(t)}\boldsymbol{R}(t)^T$
是一个反对称矩阵 $(A=-A^T)$
, 我们可以用一个向量 $\phi$
以及反对称算子 $\wedge{}$
来对其进行表示:$\phi^\wedge{} = \dot{\boldsymbol{R}(t)}\boldsymbol{R}(t)^T$
, 对于反对称算子,有:
$$
\omega^{\wedge{}} =
\begin{bmatrix}
0 & -\omega_3 & \omega_2 \\
\omega_3 & 0 & -\omega_1 \\
-\omega_2 & \omega_1 & 0
\end{bmatrix}
$$
对 $\phi^\wedge{} = \dot{\boldsymbol{R}(t)}\boldsymbol{R}(t)^T$
两边右乘 $\boldsymbol{R}$
, 有:
$$
\dot{\boldsymbol{R}}{(t)} = \phi(t)^\wedge{}\boldsymbol{R}(t)
$$
因此对旋转矩阵的求导,可以通过对旋转矩阵左乘一个 $\phi^\wedge{}$
来获得。
这里注意,上式中我们把 $\boldsymbol{R}(t)\boldsymbol{R}(t)^T = \boldsymbol{I}$
化简成 $\dot{\boldsymbol{R}(t)}\boldsymbol{R}(t)^T = -(\dot{\boldsymbol{R}(t)}\boldsymbol{R}(t)^T)^T$
同样的,对 $\boldsymbol{R}(t)^T\boldsymbol{R}(t) = \boldsymbol{I}$
进行求导并化简可以获得:
$$
\begin{aligned}
&\dot{\boldsymbol{R}}(t)^T\boldsymbol{R}(t) + \boldsymbol{R}(t)^T\dot{\boldsymbol{R}}(t) = 0 \\
\Rightarrow&\boldsymbol{R}(t)^T\dot{\boldsymbol{R}(t)} = -(\boldsymbol{R}(t)^T\dot{\boldsymbol{R}(t)})^T
\end{aligned}
$$
因此 $\boldsymbol{R}(t)^T\dot{\boldsymbol{R}(t)}$
也是一个反对称矩阵,也可以通过一个 $\phi'^\wedge{}$
来表示并化简:
$$
\begin{aligned}
&\boldsymbol{R}(t)^T\dot{\boldsymbol{R}(t)} = \phi'^\wedge{} \\
\Rightarrow&\dot{\boldsymbol{R}(t)} = \boldsymbol{R}\phi'^\wedge{}
\end{aligned}
$$
所以,对于旋转矩阵对时间的导数,我们也可以看成是对旋转矩阵右乘一个反对称矩阵 $\phi'^\wedge{}$
来求得,注意这里的反对称矩阵和上述中左乘的反对称矩阵 $\phi^\wedge{}$
是不同矩阵,不要混淆。
事实上,在这篇论文里面,作者推导了左乘和右乘两种情况下该向量分别对应的物理含义,简单来说:对于旋转矩阵 $\boldsymbol{R}_{AB}$
而言,如果用左乘 $\boldsymbol{\omega}_1^\wedge$
来表示导数的话,其物理含义为该时间下的角速度($B$
相对于 $A$
的旋转)在 $A$
系下的表示,如果用右乘 $\boldsymbol{\omega}_2^\wedge$
来表示导数的话,其物理含义为该角速度在 $B$
系下的表示,因此这两个向量(角速度)指代的是同一个旋转,即 $B$
相对于 $A$
的旋转角速度,这两者的区别就是对应单位旋转轴的坐标是在哪个坐标系下表示而已,因此我们同样可以对旋转轴进行坐标转换,即:$\boldsymbol{\omega}_A = \boldsymbol{R}_{AB}\boldsymbol{\omega}_B$
。
下面主要用右乘反对称矩阵来进行推导,假设 $t_0, \boldsymbol{R}(0) = \boldsymbol{I}$
,对 $\boldsymbol{R}$
在 $t = 0$
进行一阶泰勒展开得:
$$
\begin{aligned}
\boldsymbol{R}(t) &\approx\boldsymbol{R}(t_0) + \dot{\boldsymbol{R}}(t_0)(t-t_0) \\
&= \boldsymbol{R}(t_0) + \boldsymbol{R}(t_0)\boldsymbol{\phi}(t_0)^\wedge(t-t_0) \\
&= \boldsymbol{I} + \boldsymbol{I}\boldsymbol{\phi}(t_0)^\wedge(t-0) \\
&= \boldsymbol{I} + \boldsymbol{\phi}(t_0)^\wedge{}(t)
\end{aligned}
$$
在 $t_0$
附近,设 $\phi^\wedge{}(t_0) = \phi_0$
, 有:$\dot{\boldsymbol{R}}(t) = \phi_0^\wedge{}\boldsymbol{R}(t)$
, 通过对这个 $\boldsymbol{R}$
的微分方程求解可得:$\boldsymbol{R}(t) = \exp{(\phi_0^\wedge t)}$
,(求解过程参考 文献),因此我们可以发现在 $t_0$
时刻,$\boldsymbol{R}$
可以通过个反对称矩阵通过指数映射联系起来。这里,我们把这个反对称矩阵对应的向量 $\phi$
称为这个旋转矩阵对应的李代数 $\mathfrak{so}(3)$
。
李代数的简单介绍可以参考维基百科,其包含一个二元运算符(李括号)$[,]$
,这里我们关注的李代数是一类定义在 $\mathbb{R}^3$
的向量 $\phi$
,每个 $\phi$
都可以通过反对称算子生成一个反对称矩阵 $\phi^\wedge$
。李代数 $\mathfrak{so}(3)$
的二元运算符为:$[\phi_1, \phi_2] = (\boldsymbol{\Phi_1\phi_2 - \Phi_2\Phi_1})^\vee$
指数映射:$\mathfrak{so}(3) \Rightarrow$
SO(3)
矩阵的指数映射为:
$$
\exp{(\boldsymbol{A})} = \sum_{n=0}^\infty\frac{1}{n!}\boldsymbol{A}^n
$$
同理,李代数的指数映射为:
$$
\exp{(\phi^\wedge)}=\sum_{n=0}^\infty\frac{1}{n!}(\boldsymbol{\phi}^\wedge)^n
$$
从定义式没法进行具体计算,这里我们定义 $\boldsymbol{\phi}$
的模长和方向分别为:$\theta$
和 $\boldsymbol{a}$
, 即$\boldsymbol{\phi} = \theta \boldsymbol{a}$
。
对于 $\boldsymbol{a}$
(单位向量)的反对称矩阵 $\boldsymbol{a}^\wedge$
,有以下性质:
$$
\begin{aligned}
\boldsymbol{a^\wedge a^\wedge}
&= \begin{bmatrix}
-a_2^2-a_3^2 & a_1a_2 & a_1a_3 \\
a_1a_2 & -a_1^2-a_3^2 & a_2a_3 \\
a_1a_3 & a_2a_3 & -a_1^2-a_2^2
\end{bmatrix}\\
&= \boldsymbol{aa^\mathrm{T}-I}
\end{aligned}
$$
同理可证:$\boldsymbol{a^\wedge a^\wedge a^\wedge = a^\wedge(aa^\mathrm{T}-I)}=-a^\wedge$
。因此我们可以化简定义式中的高阶项了:
$$
\begin{aligned}
\exp{(\phi^\wedge)} &=\sum_{n=0}^\infty\frac{1}{n!}(\boldsymbol{\phi}^\wedge)^n \\
&=\sum_{n=0}^\infty\frac{1}{n!}(\theta \boldsymbol{a}^\wedge)^n \\
&= \boldsymbol{I + \mathrm{\theta}a^\wedge + \frac{1}{2!}\mathrm{\theta^2}(a^\wedge)^2 + \frac{1}{3!}\mathrm{\theta^3}(a^\wedge)^3 + \frac{1}{4!}\mathrm{\theta^4}(a^\wedge)^4 + ...} \\
&= \boldsymbol{aa^\mathrm{T}} + (\theta - \frac{1}{3!}\theta^3 + \frac{1}{5!}\theta^5 - ...)\boldsymbol{a}^\wedge - (1 - \frac{1}{2!}\theta^2+\frac{1}{4!}\theta^4 - ...) \boldsymbol{a^\wedge a^\wedge} \\
&= \boldsymbol{a^\wedge a^\wedge + I} + \sin{\theta}\boldsymbol{a}^\wedge - \cos{\theta}a^\wedge a^\wedge \\
&= \boldsymbol{(1-\mathrm{\cos{\theta}})a^\wedge a^\wedge + I + \mathrm{\sin{\theta}}a^\wedge}\\
&=\boldsymbol{(1-\mathrm{\cos{\theta}})(aa^\mathrm{T}-I) + I + \mathrm{\sin{\theta}}a^\wedge}\\
&=\boldsymbol{(1-\mathrm{\cos{\theta}})aa^\mathrm{T} + \mathrm{\cos{\theta}}I + \mathrm{\sin{\theta}}a^\wedge}\\
\Rightarrow\exp{(\theta\boldsymbol{a}^\wedge)}&= \mathrm{\cos{\theta}}I + \boldsymbol{(1-\mathrm{\cos{\theta}})aa^\mathrm{T} + \mathrm{\sin{\theta}}a^\wedge}
\end{aligned}
$$
从上式可以看出最后得出的公式实际上就是罗德里格斯公式,实际上 $\mathfrak{so}(3)$
就是用旋转向量组成的空间(因此李代数的模长和方向实际上就式旋转角和旋转轴)
并且当 $\boldsymbol{\phi}$
为一个很小旋转时,有一个常见的近似技巧:
$$
\begin{aligned}
\exp{(\boldsymbol{\phi}^\wedge)} &= \mathrm{\cos{\theta}}\boldsymbol{I} + (1-\mathrm{\cos{\theta}})\boldsymbol{aa^\mathrm{T} + \mathrm{\sin{\theta}}a^\wedge} \\
&\approx \boldsymbol{I} + \theta \boldsymbol{a}^\wedge \qquad \boldsymbol{\phi} 为微小旋转\\
&= \boldsymbol{I} + \boldsymbol{\phi}^\wedge
\end{aligned}
$$
这个化简在优化时经常会用到。
除此之外,指数映射还有以下性质:
$(\exp{\boldsymbol{A}})^{-1} = \exp{(-\boldsymbol{A})}$
对数映射:$SO(3) \Rightarrow \mathfrak{so}(3)$
对数映射定义为:
$$
\begin{aligned}
\boldsymbol{\phi} &= \ln{(\boldsymbol{R})}^\vee \\
&= (\sum_{n=0}^\infty\frac{(-1)^n}{n+1}\boldsymbol{(R-I)}^{n+1})^\vee
\end{aligned}
$$
因为我们已经知道李代数实际上就是旋转向量,因此我们可以不用这个定义式,而根据我闷推导由旋转矩阵获得旋转向量的公式:
$$
\begin{aligned}
\boldsymbol{\phi} &= \ln{(\boldsymbol{R})}^\vee \\
&= \arccos{\frac{\mathrm{tr}(\boldsymbol{R}) - 1}{2}}
\end{aligned}
$$
对变换矩阵的指数映射和对数映射由于比较复杂,这里不进行推导直接给出结论。
李代数求导和扰动模型
在进行优化过程中,我们经常需要寻找一个位姿 $T$
来使得某个误差项(观测和模型)最小化,就求解下式:
$$
\min_\boldsymbol{T}{J(\boldsymbol{T})} = \sum_{i=1}^N{||\boldsymbol{z}_i-\boldsymbol{Tp}_i||_2^2}
$$
而对上式求解 $T$
使其最小化时,不可避免地我们要对含有位姿 $\boldsymbol{T}$
的函数对位姿 $\boldsymbol{T}$
进行求导。这里比较直观的做法是我们直接将 $T$
作为 $SE(3)$
或者 $SO(3)$
来进行求导,这样有一个问题:由于并不是所有的 $\boldsymbol{A}^{3\times3}$
都是合法的李群,因此我们势必要加上额外的约束条件(例如限制 $\boldsymbol{A}\boldsymbol{A}^T = \boldsymbol{I}$
以及其他条件来保证 $\boldsymbol{A}$
是李群),这样并不是很方便,而由于所有三维向量都是合法的李代数(所有向量都可以作为旋转向量),因此如果我们将李群转化为李代数,通过求解李代数使得误差最小,最后再将李代数重新通过指数映射转化为李群就无须考虑这些额外的约束了。这就是为什么我们要引入李代数来作为优化问题的自变量,因此我们有必要来推导如何对李代数进行求导。
由于求解优化问题经常涉及李群的乘法,而李群和李代数是通过指数映射联系在一起的,因此我们会想知道两个李群相乘(对应的李群)再通过对数映射成李代数时和原来的两个李代数有什么关系,在标量里,指数函数有这样的关系:
$$
\begin{aligned}
e^ae^b&=e^{(a+b)}\\
\Rightarrow ln(e^ae^b) &= a+b
\end{aligned}
$$
即两个数的指数相乘对结果的对数对应这两数之和。但遗憾的是在矩阵形式下并没有这个性质,由 BCH 公式指出,两个矩阵$\boldsymbol{A, B}$
的指数相乘再求对数对应矩阵和原矩阵关系为:
$$
\ln{(\exp{\boldsymbol{(A)}}\exp{\boldsymbol{(B)}})} = \boldsymbol{A + B + \mathrm{\frac{1}{12}[A, B]} + \mathrm{\frac{1}{12}[A, [A,B]]} - \mathrm{\frac{1}{12}[B, [A,B]]}+...}
$$
BCH 对任意矩阵都满足,可以看出:两个矩阵的指数相乘再求对数时,结果除了包含两个原矩阵相加以外,还有一系列复杂的余项。这里我们主要研究一类特殊的矩阵:李群($SO(3)$
)对应的李代数$(\boldsymbol{\phi}_1, \boldsymbol{\phi}_2)$
的指数映射时。并且两个李代数其中有一个为小量时,二次项以上的小量都可以忽略。此时, BCH 公式有以下线性形式:
$$
\ln{(\exp(\boldsymbol{\phi}_1^\wedge)\exp{(\boldsymbol{\phi}_2^\wedge))}}^\vee \approx\begin{cases}
\boldsymbol{J}_l(\boldsymbol{\phi}_2)^{-1}\boldsymbol{\phi}_1 + \boldsymbol{\phi}_2\qquad \mathrm{当\phi_1 为小量} \\
\boldsymbol{J}_r(\boldsymbol{\phi}_1)^{-1}\boldsymbol{\phi}_2 + \boldsymbol{\phi}_1\qquad \mathrm{当\phi_2 为小量}
\end{cases}
$$
这个式子看似不是很直观,实际上表示的是:当一个旋转矩阵 $\boldsymbol{R}_1 = \exp{\boldsymbol{\phi}_1^\wedge}$
右乘一个微小旋转(扰动) $\boldsymbol{R}_2 = \exp{\boldsymbol{\phi}_2^\wedge}$
时,其对应的李代数可以看作是在原有的李代数 $\phi_1$
的基础上加上一项 $\boldsymbol{J_r}(\phi_\mathrm{1})^{-1}\boldsymbol{\phi_2}$
,对左乘一个扰动也可以得到相似的结果。
上式中的左/右乘 BCH 近似雅可比分别为 (将李代数分解成单位向量和模长 $\boldsymbol{\phi} = \theta\boldsymbol{a}$
):
$$
\begin{aligned}
\boldsymbol{J_l(\phi)} = \boldsymbol{J(\phi)} &= \boldsymbol{\mathrm{\frac{\sin{\theta}}{\theta}}I + \mathrm{(1-\frac{\sin\theta}{\theta})}aa^\mathrm{T} + \mathrm{\frac{1-\cos{\theta}}{\theta}}a^\wedge}\\
\boldsymbol{J_l(\phi)}^{-1} &= \boldsymbol{\mathrm{\frac{\theta}{2}\frac{\cot{\theta}}{2}}I +\mathrm{(1-\frac{\theta}{2}\frac{\cot{\theta}}{2})}aa^\mathrm{T} - \mathrm{\frac{\theta}{2}}a^\wedge } \\
\boldsymbol{J_r(\phi)} = (\exp{(\phi^\wedge)})^{-1}\boldsymbol{J_l(\phi)} = \boldsymbol{J_l(-\phi)} &= \boldsymbol{\mathrm{\frac{\sin{\theta}}{\theta}}I + \mathrm{(1-\frac{\sin\theta}{\theta})}aa^\mathrm{T} - \mathrm{\frac{1-\cos{\theta}}{\theta}}a^\wedge} \\
\boldsymbol{J_r(\phi)}^{-1} = \boldsymbol{J_l(-\phi)}^{-1} &= \boldsymbol{\mathrm{\frac{\theta}{2}\frac{\cot{\theta}}{2}}I +\mathrm{(1-\frac{\theta}{2}\frac{\cot{\theta}}{2})}aa^\mathrm{T} + \mathrm{\frac{\theta}{2}}a^\wedge }
\end{aligned}
$$
上述推导过程参考教材 231 页附近。
整理一下,当我们对一个旋转 $\boldsymbol{R}$
左乘或者右乘一个扰动$\Delta\boldsymbol{R}$
时,BCH 近似过程可以表示为:
$$
\begin{aligned}
\exp{(\Delta \boldsymbol{\phi}^\wedge)}\exp{(\boldsymbol{\phi}^\wedge))} &= \exp{((\boldsymbol{\phi}+J_l^{\mathrm{-1}}(\phi)\Delta\phi})^\wedge) \\
\exp{(\Delta \boldsymbol{\phi}^\wedge)}\exp{(\boldsymbol{\phi}^\wedge))} &= \exp{((\boldsymbol{\phi}+J_r^{\mathrm{-1}}(\phi)\Delta\phi})^\wedge)
\end{aligned}
$$
而反过来,当我们对一个李代数$\boldsymbol{\phi}$
加上一个微小扰动 $\Delta \boldsymbol{\phi}$
时,通过 BCH 可以近似为:
$$
\begin{aligned}
\exp{(\boldsymbol{(\phi + \Delta\phi)}^\wedge)} &= \exp{\boldsymbol{(J_l\Delta\phi)^\wedge}}\exp{(\boldsymbol{\phi}^\wedge)} \\
&= \exp{(\boldsymbol{\phi}^\wedge)}\exp{\boldsymbol{(J_r\Delta\phi)^\wedge}}
\end{aligned}
$$
对于变换矩阵 $SE(3)$
也有类似的结论,由于用的不多,这里先不进行推导。
旋转矩阵函数对自变量求导
我们为什么要关注在李群或者李代数上叠加微小量的情况呢?因为在求导过程中,我们常见的做法是对自变量添加一个微小值来进行:
$$
f'(x) = \lim_{\Delta x\rightarrow0}\frac{f(x+\Delta x)}{\Delta x}
$$
对于旋转矩阵 $SO(3)$
我们不能这么做,因为李群对加法不封闭,因此两个旋转矩阵相加不一定是旋转矩阵,但是利用李代数,根据上面两个方向的 BCH 近似不难看出我们有两种思路进行求导,分别是:
- 用李代数(旋转向量)来表示姿态,然后利用李代数加法叠加微小量并对该微小量进行求导
- 用李群(旋转矩阵)表示姿态,然后左/右乘上一个扰动,然后对该扰动求导,即左扰动模型和右扰动模型