本篇文章延續前文「李群、李代數與三維空間旋轉」，想要討論的是現在有一個李代數的函數，我們要如何對其微分求導數，而這個問題的答案就會是實務上要怎麼解決機器人學問題的演算法。
先回到李群與李代數的關係
我們用 \(\phi_1\) 來表示李代數上的向量，用 \(R_1\) 表是李群上的旋轉矩陣，而兩者的關係為：
\[
R_1 = exp(\phi_1^{\wedge})
\]
現在問題來了，假設有另外一組旋轉向量 \(\phi_2\) 及對應的旋轉矩陣 \(R_2\)，則他們在李代數上面相加並不等於在李群上面相乘，也就是：
\[
R_1 R_2 = exp(\phi_1^{\wedge}) exp(\phi_2^{\wedge}) \neq exp((\phi_1 + \phi_2)^{\wedge})
\]
BCH 公式給了我們兩者之間的關係式：
\[
ln(exp(A) exp(B)) = A + B + \frac{1}{2}[A,B] + \cdots
\]
這件事的物理意義解釋是因為之前提到李代數代表的是李群在單位元素附近的 tangent space，也就是說在李群上是個曲面，而李代數上是個平面，從平面換算回曲面時本來就會有一些誤差，而這個誤差有多大呢？BCH 公式給了我們答案。
該如何更新李群與李代數上的值？
科學家們定義了一組新的運算符號 \(\oplus \)（註：這個符號叫做 boxplus，它的外圈應該是個方塊，但這邊為了輸入方便用圓代替）。此運算符號代表的是直接在流形空間，也就是直接在李群上面進行運算 [1]：
\[
\varepsilon^* \leftarrow \frac{\partial F(x \oplus \varepsilon))}{\partial \varepsilon}\mid _{\varepsilon=0}\ = 0
\\
x' = x \oplus \varepsilon^* = x\ e^{\varepsilon^*}
\]
因此下一個問題就是這個偏微分怎麼計算了。
計算單位四元數至李代數的 Jacobian
上一篇文章我們提到了李代數對於單位四元數 \(q\) 的指數映射： \[
q =
\left\{\begin{matrix}
(1,0,0,0)^T\ \ \ \ \mathrm{,if}\ \mathbf{n} = \mathbf{0}
\\
(cos\frac{\theta}{2}, sin\frac{\theta}{2}\mathbf{n}) \ \ \ \mathrm{,otherwise}
\end{matrix}\right.
\]
在接下來的推導我們與參考資料 [1] 使用相同的符號，也就是 \(\theta \mathbf{n} = \omega\)：
\[
\left\{\begin{matrix}
q_1 = cos\frac{|\omega|}{2}\\
\ q_2 = \frac{sin\frac{|\omega|}{2}}{|\omega|} \omega_x \\
\ q_3 = \frac{sin\frac{|\omega|}{2}}{|\omega|} \omega_y \\
\ q_4 = \frac{sin\frac{|\omega|}{2}}{|\omega|} \omega_z
\end{matrix}\right.
\]
因此對於 \(\omega\) 在 0 附近的 Jacobian matrix 為：
\[
\frac{\partial e_q^\omega}{\partial \omega} \mid _{\omega = 0}
=
\frac{\partial e_q^\omega(\omega, \theta)}{\partial \{\omega_x, \omega_y, \omega_z, \theta\}}
\frac{\partial \{\omega_x, \omega_y, \omega_z, \theta\}}{\partial \{\omega_x, \omega_y, \omega_z\}}
\\
\\
\\
=
\begin{bmatrix}
0 & 0 & 0 & -\frac{sin(\frac{|\omega|}{2})}{2}\\
\frac{sin(\frac{|\omega|}{2})}{|\omega|} & 0 & 0 & \omega_x(C)\\
0 & \frac{sin(\frac{|\omega|}{2})}{|\omega|} & 0 & \omega_y(C)\\
0 & 0 & \frac{sin(\frac{|\omega|}{2})}{|\omega|} & \omega_z(C)
\end{bmatrix}
\begin{bmatrix}
1 & 0 & 0\\
0 & 1 & 0\\
0 & 0 & 1\\
\frac{\omega_x}{|\omega|} & \frac{\omega_y}{|\omega|} & \frac{\omega_z}{|\omega|}
\end{bmatrix}
\\
\\
\\
C = \frac{cos(\frac{|\omega|}{2})}{2|\omega|}- \frac{sin(\frac{|\omega|}{2})}{|\omega|^2}
\]
我們想要計算的是 \(\omega\) 為 0 附近的導數，但是從上面矩陣中可以看到有一些項是不定型的，也就是 0 除以 0，這時候我們就要拿出大一微積分學過的 L'Hôpital's rule 來解：
\[
\lim_{x=0}\frac{sin(\frac{x}{2})}{x} = \lim_{x=0}\frac{\frac{1}{2}cos(\frac{x}{2})}{1}=\frac{1}{2}
\]
同理我們也可以算出來 \(\lim_{\omega=0} C = 0\)，因此我們就得到了\(\omega\) 為 0 附近的 Jacobian 矩陣：
\[
\begin{bmatrix}
0 & 0 & 0\\
\frac{1}{2} & 0 & 0 \\
0 & \frac{1}{2} & 0\\
0 & 0 & \frac{1}{2}
\end{bmatrix}
\]
參考資料
[1] A tutorial on SE(3) transformation parameterizations and on-manifold optimization, Jose Luis Blanco Claraco
沒有留言:
張貼留言