第4章 李群李代数

一、概述

  1. 李群和李代数的核心思想

    1. 可以理解为专门用于矩阵旋转的东西,符合封结幺逆法则;

    2. 李群可以理解为旋转矩阵,李代数可以理解为旋转向量

    3. 李群是连续群,李代数可以表出李群的导数,所以李代数表示的是李群的局部性质;

    4. 进而我们可以理解为:旋转向量表达了旋转矩阵的局部(旋转发生那一瞬间的领域内)性质;

    5. 由拉格朗日中值定理可知:导数控制函数。李代数控制李群,\(\phi\)控制\(R\)【1】

      也就是说想要估计出函数值,我们可以研究该函数的导数,用来描述某个点领域内性质。故而我们需要建立对李群的求导模型,通过分析导数的性质来估计出相机在这一时刻(领域内)的位姿。

      但是我们知道是指只有一个运算的集合(我们选择矩阵乘法),所以李群不对加法封闭【2】,但是我们知道李代数是建立在向量空间上的,支持加法运算。所以我们需要一种让李群映射到李代数的机制,然后通过对李代数求导,求出李群的导数。不过,对李代数求导后的结果非常复杂,所以我们需要寻找另外一种求导方式【3】,这就是我们接下来所要介绍的内容。

      【注】

      【1】:某个名牌大学考研的复试题——你知道导数的作用是什么吗?

      【2】:李群也是一种群。甭跟我扯什么鳄鱼不是鱼、日本人不是人。

      【3】:对谁求导不重要,因为我们总可以通过这个导数控制相同的函数。

  2. 李群的两种求导模型(都是映射到了李代数空间)

    1. BCH公式线性化(将李群的变化与李代数的变化联系起来);

    2. 对李代数求导的求导模型;(复杂)

      1. 需要求出左右雅可比矩阵的逆;
    3. 对微扰动求导的扰动模型;(精简)

      1. 不需要求出左右雅可比矩阵的逆;
  3. 这两种求导模型都是会有误差存在的

  4. 李群和李代数的基础符号

    1. 特殊正交群\(SO(3)\),特殊欧式群\(SE(3)\)

    2. 特殊正交群上的李代数\(\mathfrak{so}(3)\),这里我们具象化为三维\(\phi\)向量或者反对称阵\(\widehat{\phi}\)

      李代数so(3)的定义
    3. 特殊欧式群上的李代数\(\mathfrak{se}(3)\),这里我们具象化为六维\(\xi\)向量或者四维方阵\(\widehat{\xi}\)

      李代数se(3)的定义

      \(\rho\)表示三维空间中的平移,\(\phi\)表示三维空间中的旋转。

    4. 反对称矩阵与相应的三维向量:\(\wedge\)\(\vee\)

      反对称矩阵与相应的三维向量
    5. 向量\(\overrightarrow{a}\)\(\overrightarrow{\omega}\)都表示旋转向量的单位方向向量,\(\theta\)表示旋转角,\(\overrightarrow{\phi}\)表示旋转向量,\(R\)表示旋转矩阵;

    6. 有时为了格式不显得过于复杂,会省略掉向量标识\(\overrightarrow{}\)

  5. Sophus库的使用

    1. \(SO(3)\)\(\mathfrak{so}(3)\)

      // 构造旋转向量、旋转矩阵、四元数
      Eigen::AngleAxisd RV = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(0,0,1));
      Eigen::Matrix3d RM = RV.toRotationMatrix();
      Eigen::Quaterniond QD(RM);
      // 通过上述旋转表述构造李群
      Sophus::SO3d SO3_RM(RM);
      Sophus::SO3d SO3_QD(QD);
      // 输出SO(3)时,以so(3)形式输出
      cout << "SO(3) from matrix    :" << SO3_RM.log().transpose() << endl;
      cout << "SO(3) from quaternion:" << SO3_QD.log().transpose() << endl;
      
      // 使用对数映射获得它的李代数
      Eigen::Vector3d so3 = SO3_RM.log();
      cout << "so3 = " << so3.transpose() << endl;
      // hat为向量到反对称矩阵
      cout << "so3 hat=" << endl << Sophus::SO3d::hat(so3) << endl;
      // vee为反对称矩阵到向量
      cout << "so3 hat vee= " << Sophus::SO3d::vee(Sophus::SO3d::hat(so3)).transpose() << endl; 
      
      // 增量扰动模型的更新
      Eigen::Vector3d update_so3(1e-4, 0, 0);
      Sophus::SO3d SO3_updated = Sophus::SO3d::exp(update_so3) * SO3_RM;
      cout << "SO3 updated = " << endl << SO3_updated.matrix() << endl;
      
    2. \(SE(3)\)\(\mathfrak{se}(3)\)

      // 构造旋转向量、旋转矩阵、四元数
      Eigen::AngleAxisd RV = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(0,0,1));
      Eigen::Matrix3d RM = RV.toRotationMatrix();
      Eigen::Quaterniond QD(RM);
      // 通过上述旋转表述构造李群
      Sophus::SO3d SO3_RM(RM);
      Sophus::SO3d SO3_QD(QD);
      // 对SE(3)操作大同小异
      Eigen::Vector3d t(1,0,0);           // 沿X轴平移1
      Sophus::SE3d SE3_RMt(RM, t);           // 从R,t构造SE(3)
      Sophus::SE3d SE3_QDt(QD, t);            // 从q,t构造SE(3)
      cout << "SE3 from RM,t = " << SE3_RMt.log().transpose() << endl;
      cout << "SE3 from QD,t = " << SE3_QDt.log().transpose() << endl;
      // 李代数se(3) 是一个六维向量,方便起见先typedef一下
      typedef Eigen::Matrix<double,6,1> Vector6d;
      Vector6d se3 = SE3_RMt.log();
      cout << "se3 = " << se3.transpose() << endl;
      // 观察输出,会发现在Sophus中,se(3)的平移在前,旋转在后.
      // 同样的,有hat和vee两个算符
      cout << "se3 hat = " << endl << Sophus::SE3d::hat(se3) << endl;
      cout << "se3 hat vee = " << Sophus::SE3d::vee(Sophus::SE3d::hat(se3)).transpose() << endl;
      
      // 最后,演示一下更新
      Vector6d update_se3; //更新量
      update_se3.setZero();
      update_se3(0,0) = 1e-4;
      Sophus::SE3d SE3_updated = Sophus::SE3d::exp(update_se3) * SE3_RMt;
      cout << "SE3 updated = " << endl << SE3_updated.matrix() << endl;
      

二、详述

  1. 从物理角度引出

    从物理角度引出

    假设有一个三维的向量\(\overrightarrow{p(0)}\)绕着旋转轴\(\overrightarrow{\omega}\)旋转了\(\theta\)角度(旋转所需时间为\(t\))到达了\(\overrightarrow{p(t)}\)处,由小学二年级就学过的知识知道:\(\overrightarrow{速度}=\overrightarrow{角速度}\times\overrightarrow{(旋转中心,旋转点)}\)

    我们不妨设\(\overrightarrow{p(t)}\)领域处的速度为\(\overset{\bullet}{\overrightarrow{p(t)}}\),故而有公式:\(\overset{\bullet}{\overrightarrow{p(t)}}=\overrightarrow{\omega}\times\overrightarrow{p(t)}=\widehat{\overrightarrow{\omega}}\cdot\overrightarrow{p(t)}\)【1】。这其实是一个关于\(\overrightarrow{p(t)}\)的微分方程【2】,并且当\(\overrightarrow{\omega}\)是单位向量,即值为\(1\ \mathbf{rad/s}\)时有\(\theta=t\),所以不难解出:

    \[\overrightarrow{p(t)}=e^{\widehat{\overrightarrow{\omega}}\cdot{t}}\cdot{\overrightarrow{p(0)}},由此我们不难看出\overrightarrow{p(0)}到\overrightarrow{p(t)}的旋转矩阵:R(t)=e^{\widehat{\overrightarrow{\omega}}\cdot{t}}=e^{\widehat{\overrightarrow{\omega}}\cdot{\theta}}=e^{\widehat{\overrightarrow{\phi}}}; \]

    所以我们便可的出结论:旋转向量和旋转矩阵存在指数映射关系,且多对一对应(旋转周期性)。

    为了统一化表述,我们称旋转矩阵为李群\(SO(3)\),旋转向量为\(\mathfrak{so}(3)\),它们之间有指数映射关系。

    【1】:如果这个时候你仍然以向量为参考,那可能有点儿不太好理解,但要是将视线移至坐标系身上的化,也就是说是坐标系在旋转,那就不难理解:旋转轴是\(\overrightarrow{\omega}\),旋转中心是原点。

    【2】:标量微分方程与矢量微分方程如下所示:

    \[\begin{aligned} &\overset{\bullet}{x(t)}=a\cdot{x(t)},且x(0)=x_0&\Longrightarrow&\quad x(t)=e^{at}\cdot{x_0}; \\\\ &\overset{\bullet}{X(t)}=A\cdot{X(t)},且X(0)=X_0&\Longrightarrow&\quad X(t)=e^{At}\cdot{X_0}; \end{aligned} \]

  2. 指数与对数映射

    1. 矩阵的泰勒展开

      \[e^{\widehat{\overrightarrow{\omega}}\cdot{\theta}}=\sum_{n=0}^{\infty}{\frac{1}{n!}(\overrightarrow{\omega}}{\theta})^n=\sum_{n=0}^{\infty}{\frac{{\theta}^n}{n!}(\overrightarrow{\omega}})^n \]

    2. 反对称阵的性质

      \[\begin{cases} \widehat{\overrightarrow{\omega}}\cdot{\widehat{\overrightarrow{\omega}}}=\overrightarrow{\omega}\cdot{\overrightarrow{\omega}^T}-I \\\\ \widehat{\overrightarrow{\omega}}\cdot{\widehat{\overrightarrow{\omega}}\cdot{\widehat{\overrightarrow{\omega}}}}=-{\widehat{\overrightarrow{\omega}}} \end{cases} \]

    3. 指数映射的推导

      1. \(\mathfrak{so}(3) \Longrightarrow SO(3)\)的推导
        SO(3)的指数映射推导过程
      2. \(\mathfrak{se}(3) \Longrightarrow SE(3)\)的推导
        SE(3)的指数映射推导过程

        如果你尝试这将上面的\(J\)泰勒展开,将会得到下面的公式(请务必有印象!):

        SE(3)的指数映射推导过程2

        而旋转矩阵\(R\)通过上述的\(\mathfrak{so}(3) \Longrightarrow SO(3)\)已经给出。

    4. 对数映射的推导

      1. \(SO(3)\Longrightarrow\mathfrak{so}(3)\)的推导
        SO(3)的对数映射推导过程

        只有伞兵才会这样推导,之前在《矩阵旋转》中就提到了如何从旋转矩阵到旋转向量,我们只需知道旋转矩阵的迹\(\mathbf{tr}(R)\)解出\(\theta\),并解出特征值为1的特征向量并归一化得到\(\omega\)

      2. \(SE(3)\Longrightarrow\mathfrak{se}(3)\)的推导

        由上述\(SO(3)\Longrightarrow\mathfrak{so}(3)\)我们可以通过旋转\(R\)求出旋转向量\(\phi\),而\(\overrightarrow{t}=J\rho\)\(\rho=J^{-1}\overrightarrow{t}\)

  3. 李群和李代数的对应关系

    李群和李代数的对应关系

三、李代数的求导和扰动模型(左乘)

  1. BCH公式和它的线性近似式

    1. BCH公式的引出

      在高等数学中,我们研究的大部分问题都是标量(也就是纯数值)的问题,所以一定满足下面恒等式

      \[\ln(e^A\cdot{e^B})\equiv{A+B}\\ \ln(e^\widehat{a}\cdot{e^\widehat{b}})\equiv{(a+b)^{\wedge}} \]

      但是很不幸的是,我们在SLAM中研究的是向量(或者说矩阵),反正不是标量,所以不能满足上式。这个东西在向量空间中的计算很是复杂,但是这个世界上总有些神仙捣鼓这种玩意儿,比如Baker-Campbell-Hausdorff这哥仨就为这个东西的计算给出了BCH公式。

    2. BCH的完全展开

      \[\ln(e^A\cdot{e^B})=A+B+\frac{1}{2}[A,B]+\frac{1}{12}[A,[A,B]]-\frac{1}{12}[B,[A,B]]+\cdots \]

      其中\([,]\)表示李括号运算。也就是说,这天底下就没有干干净净的事,矩阵指数之积会产生一堆李括号余项。

    3. BCH线性近似式

      但凡事不能说得太绝对。虽说这BCH公式的完全展开很复杂,但是后面那一堆东西叫做余项啊!也就是说,当\(\overrightarrow{\phi_1}\)或者\(\overrightarrow{\phi_2}\)是小量的时候,它们的高阶就是高阶无穷小量了,可以被忽略的!这个时候BCH公式就变得非常‘和蔼可亲’了(近乎线性的表达)

      \[\ln(e^{\widehat{\phi_1}}\cdot{e^{\widehat{\phi_2}}})^{\vee}\approx \begin{cases} 左乘模型:J_l^{-1}(\phi_2)\cdot{\phi_1}+\phi_2,当\phi_1为小量时; \\\\ 右乘模型:J_r^{-1}(\phi_1)\cdot{\phi_2}+\phi_1,当\phi_2为小量时; \end{cases} \]

      至此,我们将李群\(SO(3)\)(旋转矩阵)和李代数\(\mathfrak{so}(3)\)(旋转向量)线性化了。

      可以理解为:旋转矩阵\(R_2\)左乘了一个微小旋转矩阵\(R_1\),相当于旋转向量\(\phi_2\)加上了经过左雅可比逆\(J_l^{-1}(\phi_2)\)旋转后的旋转向量\(\phi_1\)。右乘微小矩阵\(R_2\)亦可模仿理解。

    4. BCH公式的意义

      1. 李群 \(\Longrightarrow\) 李代数

        \[\begin{cases} 左乘模型:e^{\widehat{\Delta\phi}}\cdot{e^{\widehat{\phi}}}=e^{【J_l^{-1}(\phi)\cdot{\Delta\phi}+\phi】^{\wedge}} \\\\ 右乘模型:e^{\widehat{\phi}}\cdot{e^{\widehat{\Delta\phi}}}=e^{【J_r^{-1}(\phi)\cdot{\Delta\phi}+\phi】^{\wedge}} \end{cases} \]

      2. 李代数 \(\Longrightarrow\) 李群

        \[e^{【\phi+\Delta\phi】^{\wedge}}= \begin{cases} 左乘模型:e^{【\phi+J_l^{-1}(\phi)\cdot{J_l(\phi){\Delta\phi}}】^{\wedge}} =e^{【J_l(\phi){\Delta\phi}】^{\wedge}}\cdot{e^{\widehat{\phi}}} \\\\ 右乘模型:e^{【\phi+J_r^{-1}(\phi)\cdot{J_r(\phi){\Delta\phi}}】^{\wedge}} =e^{\widehat{\phi}}\cdot{e^{【J_r(\phi){\Delta\phi}】^{\wedge}}} \end{cases} \]

      3. 李代数解决李群的求导问题

        李群没有加法,所以很难用导数的一般定义去求导数,但是李代数允许加法【1】。所以如果想要对李群求导数,我们可以运用指数映射法则和BCH公式转换成李代数,然后用定义区求导数(乘除变加减)。

        【注】【1】:李群是群,不对加法封闭。李代数定义在向量空间,对加法封闭。

    5. 左右雅可比矩阵的计算

      左右雅可比矩阵的计算
    6. \(SE(3)\)上同样有线性化公式

      在SE(3)上同样有线性化公式

      其中的左右雅可比矩阵表述十分复杂,并且我们以后不会用到它来计算,所以就将其视为一个常数符号。

  2. 李代数\(SO(3)\)的求导

    假设一个向量\(\overrightarrow{p}\)经过了旋转矩阵\(R\)的作用,旋转矩阵对应的李代数(旋转向量)为\(\phi\)

    1. 对李代数求导(求导模型)

      \[公式 控制\begin{aligned} &\frac{\part{(Rp)}}{\part{\phi}} = \frac{\part{(Rp)}}{\part{\phi}} = \frac{\part{(e^{\widehat{\phi}}\cdot{p})}}{\part{\phi}} \\\\=&\underset{\Delta\phi\rightarrow0}{\lim} \frac{(e^{(\phi+\Delta\phi)^{\wedge}}-e^{\phi^{\wedge}})\cdot{p}}{\Delta\phi} ,(利用左乘模型得) \\\\=&\underset{\Delta\phi\rightarrow0}{\lim} \frac{(e^{【J_l(\phi){\Delta\phi}】^{\wedge}}\cdot{e^{\widehat{\phi}}}-e^{\phi^{\wedge}})\cdot{p}}{\Delta\phi} ,(利用矩阵的等价无穷小替换) \\\\=&\underset{\Delta\phi\rightarrow0}{\lim} \frac{【J_l(\phi){\Delta\phi}】^{\wedge}\cdot{e^{\widehat{\phi}}p}}{\Delta\phi} ,(将反对陈矩阵\wedge符号换成叉积\times,再交换顺序) \\\\=& -【e^{\widehat{\phi}}p】^{\wedge}J_l(\phi) = -(Rp)^{\wedge}J_l{(\phi)} \end{aligned} \]

    2. 对微扰动求导(扰动模型)

      假设这个时候对\(R\)有一个作用在左边的微小扰动\(\Delta R\),对应的李代数为\(\psi\)。(左右干扰模型不同!!)

      \[\begin{aligned} &\frac{\part{(Rp)}}{\part{\psi}} = \frac{\part{(Rp)}}{\part{\psi}} = \frac{\part{(e^{\widehat{\phi}}\cdot{p})}}{\part{\psi}} \\\\=&\underset{\Delta\phi\rightarrow0}{\lim} \frac{(e^{\psi^{\wedge}}e^{\phi^{\wedge}}-e^{\phi^{\wedge}})\cdot{p}}{\psi} ,(利用矩阵的等价无穷小替换) \\\\=&\underset{\Delta\phi\rightarrow0}{\lim} \frac{\psi^{\wedge}\cdot{e^{\phi^{\wedge}}p}}{\psi} ,(将反对陈矩阵\wedge符号换成叉积\times,再交换顺序) \\\\=& -【e^{\widehat{\phi}}p】^{\wedge}=-(Rp)^{\wedge} \end{aligned} \]

  3. 李代数\(SE(3)\)的求导

    李代数SE(3)的求导

    第2、3行是因为用矩阵的等价无穷小替换;


四、评估轨迹的误差

  1. 向量的长度(或大小)

    1. 欧式距离

      在三维及三维以下的向量空间中,向量的长度的平方是坐标值的平方和。

    2. 欧式推广

      在n维坐标系中,我们也可以用这一思想表示向量的长度(或者称之为大小)。

      \[\overrightarrow{\alpha}=[\alpha_1,\alpha_2,\cdots,\alpha_n]^T,则||\overrightarrow{\alpha}||_2^2=\alpha_1^2+\alpha_2^2+\cdots+\alpha_n^2 \]

  2. 矩阵的左右乘法区别

  3. 每个位姿李代数误差

    我们记第\(i\)时刻的真实轨迹为\(T_{(GT,i)}\)、观测(估计)轨迹为\(T_{(Esti,i)}\)、绝对轨迹误差\(T_{(ATE,i)}\)

    由于误差的作用使得真实轨迹变换到了估计轨迹,我们的关注点是坐标系的变换,所以有三者的转换公式:\(T_{(Esti,i)}=T_{(GT,i)}\cdot{T_{(ATE,i)}}\),即有\(T_{(ATE,i)}=T_{(GT,i)}^{-1}\cdot{T_{(Esti,i)}}\)

    得到了每个位姿的误差公式,那如何确定误差的大小呢?我们知道李群是很难估计大小的,所以我们可以借助其对应的李代数来确定大小,这就需要用到对数映射公式:\(\xi=【\log(T)】^{\vee}\)

    所以每个位姿的李代数上的误差为:\(【\log(T_{(GT,i)}^{-1}\cdot{T_{(Esti,i)}})】^{\vee}\),那么它的大小也就可以用二范数来计算。

    当然我们也可以计算第i时刻的邻域\(\Delta{t}\)时间内,真实轨迹(的变化量)与估计轨迹(的变化量)的相对误差:

    \[【\log(T_{(GT,i)}^{-1}\cdot{T_{(GT,i+\Delta{t})}})^{-1}(T_{(Esti,i)}^{-1}\cdot{T_{(Esti,i+\Delta{t})}})】^{\vee} \]

posted @ 2021-05-27 19:51  BNTU  阅读(1580)  评论(0编辑  收藏  举报