SE(3)左扰动导数计算

本文推到$SE(3)$上的左扰动模型导数

扰动模型

假设$q_1$和$q_2$分别为空间的点在坐标系$1$和$2$下的三维坐标，$T_{21}\in SE(3)$是坐标系$1$到坐标系$2$之间的位姿转化矩阵。则有：

\[q_1=T_{21}*p_2\]

为了求解$q_1$关于$T_{21}$的导数，在公式$q_1=T_{21}*p_2$的两边添加微小的扰动$\xi_1$和$\xi_2$, 采用左乘模型可得

\[q_1+\xi_1=\exp(\xi_2)*T_{21}*p_2\]

由$\exp(\xi_2)= I+\xi_2^{\wedge}+o(\xi_2)$,可得：

\[\begin{eqnarray*} &&q_1+\xi_1=(I+\xi_2^{\wedge})*T_{21}*p_2 \\ \implies &&q_1+\xi_1=T_{21}*p_2+\xi_2^{\wedge}*T_{21}*p_2 \\ \implies &&\xi_1=\xi_2^{\wedge}*T_{21}*p_2 \end{eqnarray*}\]

因为在$SE(3)$上$a^{\wedge}b=\left[\begin{matrix} I , -b^{\wedge} \end{matrix}\right]$ 所以

\[\xi_1=\left[\begin{matrix} I , -p_2^{\wedge} \end{matrix}\right]*\xi_2+o(\xi_2)\]

所以$q_1$关于$T_{21}$的导数为$\left[\begin{matrix} I , -p_2^{\wedge} \end{matrix}\right]$

为了求解$T_{21}$关于$T_{1w}$的导数，在公式$ T_{21}=T_{2w}T_{1w}^{-1}$的 两边添加微小的扰动$\xi_1$和$\xi_2$,采用左乘模型可得

\[\exp(\xi_1)T_{21}=T_{2w}(\exp(\xi_2)(T_{1w}))^{-1}\]

展开可得

\[\begin{eqnarray*} &&\exp(\xi_1)T_{21}=T_{2w}(T_{1w})^{-1}*\exp(-\xi_2) \\ \implies &&\exp(\xi_1)T_{21}=T_{21}*\exp(-\xi_2) \end{eqnarray*}\]

因为$Texp(\xi)T^{-1}=\exp(Ad_{T}\xi)$,所以

\[\begin{eqnarray*} &&\exp(\xi_1)T_{21}=\exp(-Ad_{T_{21}}\xi_2)*T_{21} \\ \implies &&\exp(\xi_1)=\exp(-Ad_{T_{21}}\xi_2) \\ \implies &&\xi_1=-Ad_{T_{21}}\xi_2 \end{eqnarray*}\]

所以:

\[\begin{eqnarray*} \frac{\partial T_{21}}{ \partial T_{1w}}=-Ad_{T_{21}} \end{eqnarray*}\]

同理：

\[\begin{eqnarray*} \frac{\partial T_{21}}{ \partial T_{2w}}=I \end{eqnarray*}\]

伴随矩阵

前面提到的矩阵$Ad_{T}$称为$T$的伴随矩阵，在$SE(3)$下有：

\[Ad_{T}=\left[ \begin{matrix} R&t\times R\\ 0&R \end{matrix} \right]\]