不变扩展卡尔曼滤波(一):李群与李代数

本文同步更新与github pages欢迎收藏关注。

群的定义

设有一个集合 G G G,集合中的元素为 g g g,元素之间存在乘法运算 ⋅ \cdot ,称 G G G是一个群如果其满足以下条件:

  • 封闭性 ∀ g 1 , g 2 ∈ G ,    g 1 ⋅ g 2 ∈ G \forall g_1, g_2\in G,\ \ g_1\cdot g_2\in G g1,g2G,  g1g2G
  • 结合律 g 1 ⋅ ( g 2 ⋅ g 3 ) = ( g 1 ⋅ g 2 ) ⋅ g 3 g_1\cdot(g_2\cdot g_3)=(g_1\cdot g_2)\cdot g_3 g1(g2g3)=(g1g2)g3
  • 存在幺元 ∀ g ∈ G ,    e ⋅ g = g ⋅ e = g \forall g\in G,\ \ e\cdot g=g\cdot e=g gG,  eg=ge=g
  • 存在逆元 ∀ g ∈ G ,    ∃ g − 1 ∈ G ,    g ⋅ g − 1 = g − 1 ⋅ g = e \forall g\in G,\ \ \exists g^{-1}\in G,\ \ g\cdot g^{-1}=g^{-1}\cdot g=e gG,  g1G,  gg1=g1g=e

SLAM中常用的群

只考虑实数域。所有 n × n n\times n n×n的正交矩阵 R R R构成了一个群,称为正交群,记为 O ( n ) O(n) O(n),在这些正交矩阵中,行列式为正1的矩阵本身也构成一个群,称为特殊正交群,记为 S O ( n ) SO(n) SO(n),也就是说 S O ( n ) SO(n) SO(n) O ( n ) O(n) O(n)的一个子群(准确的说是正规子群)。

在SLAM以及很多机器人技术中,最常用到的就是 S O ( 3 ) SO(3) SO(3),也就是所有三维旋转矩阵构成的群,可以用来表示机器人或者飞行器的姿态。

当需要描述一个刚体变换(旋转加平移)时,光靠一个旋转矩阵是不够的,当我们使用齐次坐标表示空间中的点时,对该点的刚体变换可用下面的 4 × 4 4\times 4 4×4矩阵表示

T = ( R t 0 1 ) T= \begin{pmatrix} R & t \\ 0 & 1 \end{pmatrix} T=(R0t1)

其中 R ∈ R 3 × 3 R\in \mathbb{R}^{3\times 3} RR3×3是旋转矩阵, t ∈ R 3 t\in \mathbb{R}^3 tR3是平移矢量。容易验证,矩阵 T T T也是满足构成群的四个条件的,其中逆元为

T − 1 = ( R T − R T t 0 1 ) T^{-1}= \begin{pmatrix} R^T & -R^Tt \\ 0 & 1 \end{pmatrix} T1=(RT0RTt1)

幺元就是单位阵,因此所有刚体变换矩阵也构成一个群,称为特殊欧式群,记为 S E ( 3 ) SE(3) SE(3)。当然也存在 S E ( n ) SE(n) SE(n),看做是任意维度下的刚体变换。如果不要求 T T T里面的 R R R必须是一个旋转矩阵,甚至不要求是正交矩阵,只要可逆就行,那么我们就得到了更一般的仿射群,在这就不深入讨论了。

为了方便之后介绍不变卡尔曼滤波,这里对 S E ( 3 ) SE(3) SE(3)做一个扩展,我们发现型如

χ = ( R t p 1 p 2 . . . p k 0 1 0 0 . . . 0 0 0 1 0 . . . 0 0 0 0 1 . . . 0 ⋮ 0 0 0 0 . . . 1 ) \chi= \begin{pmatrix} R & t & p_1 & p_2 & ... & p_k \\ 0 & 1 & 0 & 0 & ... & 0 \\ 0 & 0 & 1 & 0 & ... & 0 \\ 0 & 0 & 0 & 1 & ... & 0 \\ \vdots \\ 0 & 0 & 0 & 0 & ... & 1 \end{pmatrix} χ=R0000t1000p10100p20010...............pk0001

的矩阵,大小为 ( 4 + k ) × ( 4 + k ) (4+k)\times(4+k) (4+k)×(4+k) p k ∈ R 3 p_k\in \mathbb{R}^3 pkR3,也满足群定义,逆元为

χ − 1 = ( R T − R T t − R T p 1 − R T p 2 . . . − R T p k 0 1 0 0 . . . 0 0 0 1 0 . . . 0 0 0 0 1 . . . 0 ⋮ 0 0 0 0 . . . 1 ) \chi^{-1}= \begin{pmatrix} R^T & -R^Tt & -R^Tp_1 & -R^Tp_2 & ... & -R^Tp_k \\ 0 & 1 & 0 & 0 & ... & 0 \\ 0 & 0 & 1 & 0 & ... & 0 \\ 0 & 0 & 0 & 1 & ... & 0 \\ \vdots \\ 0 & 0 & 0 & 0 & ... & 1 \end{pmatrix} χ1=RT0000RTt1000RTp10100RTp20010...............RTpk0001

幺元也是单位阵,我们将这种矩阵构成的群记为 S E k + 1 ( 3 ) SE_{k+1}(3) SEk+1(3)

切空间与李代数

S O ( 3 ) SO(3) SO(3) S E ( 3 ) SE(3) SE(3)这类群,还有一个特点是平滑连续,即他们是一个平滑的流形,这类群称为李群。在流形上微分可以得到流形上某一点的切空间(类比于对曲线微分得到切线)。其中在幺元处的切空间最重要(与后面的指数映射有关)。下面分别来求一下 S O ( 3 ) SO(3) SO(3) S E ( 3 ) SE(3) SE(3),以及 S E k ( 3 ) SE_k(3) SEk(3)在幺元处的切空间。

S O ( 3 ) SO(3) SO(3)的切空间

具体写出 S O ( 3 ) SO(3) SO(3)的定义为

S O ( 3 ) = { R ∈ R 3 × 3 ∣ R T R = I , d e t ( R ) = 1 } SO(3)=\{R\in\mathbb{R}^{3\times 3}|R^TR=I, det(R)=1\} SO(3)={ RR3×3RTR=I,det(R)=1}

现在假设 R R R是一个随时间变化的量 R ( t ) R(t) R(t),则 R ( t ) T R ( t ) = I R(t)^TR(t)=I R(t)TR(t)=I两边对时间求导,并令 ∂ R / ∂ t = R ˙ \partial R/\partial t=\dot{R} R/

  • 17
    点赞
  • 37
    收藏
    觉得还不错? 一键收藏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值