李代数求导是SLAM的基础由于SLAM待求解的状态量包含旋转而旋转状态量为了避免歧义往往采用更高维的四元素或者旋转矩阵表示。为了通过优化的方法更新旋转状态量需要求出旋转后的点关于旋转状态量的雅可比。1. 李群李代数部分整理1.1. 群的概念1.2. SO(3)推导在附近假设不变可解微分方程可以得到即该式说明任意t都可以找到R与ϕ对应关系该关系称为指数映射ϕ称为李代数1.3.李代数注不同书中对于se(3)中的顺序不一定都一样1.4. 指数映射与对数映射1.5. 伴随SE(3)伴随1.6. 求导与扰动1.6.1. BCH公式与左右雅克比李群对于加法不封闭需要重新定义导数解决方法两种利用李代数加法定义李群元素的导数利用指数映射和对数映射完成变换关系对于李群不成立于是利用BCH公式左雅克比右雅克比对于SE3形式更为复杂对于旋转后的点坐标关于旋转的导数由于R没有加法导数无法定义于是如下解决方法对R对应的李代数加上小量求相对于小量的变化率导数模型对R左乘或者右乘一个小量求相对于小量的李代数的变化率扰动模型导数模型:1.6.2. 需要计算雅克比矩阵扰动模型左扰动右扰动的推导类似SE(3)扰动模型1.7. 补充SO(3)重要性质与证明1.8. 导数2. 四元数的两种不同形式2.1. 分类通常四元数分为JPL四元数和Hamilton四元数两种JPL在航空中常用而Hamilton则在机器人领域较为常用ROS、Eigen、Ceres中使用的都是Hamilton四元数。实部顺序:实际使用中需要注意一下即可仅在计算中有些差别Hamilton四元数实部在前虚部在后JPL四元数实部是最后一个元素左手系与右手系两种定义下的四元数是共轭关系旋转操作坐标轴(frame)不变旋转的是向量类似相机不动相机所观察的目标在运动称为Active Rotation向量不变坐标轴在变化类似相机视角在变目标不动一个向量v可以用不同坐标系下的坐标表示即Passive RotationJPL和Hamilton都是Passive表示旋转变换:从数值上来看JPL四元数和Hamilton四元数是相等的Local-to-Global将旋转后坐标系下的坐标变换到旋转前坐标系下Global-to-Local将旋转前坐标系下的坐标变换到旋转后坐标系下对于添加扰动的区别Local-to-Global 是右扰动Global-to-Local 是左扰动2.2. 扰动2.2.1. 局部扰动Local-to-Global的形式下局部扰动表示为:2.2.2. 全局扰动Global-to-Local形式下全局扰动表示为2.3. 角速度关于Barfoot中的四元数Barfoot书中使用的四元数实部在后右手系Passive方式左扰动3. 旋转角速度3.1. 扰动量的定义局部扰动之所以乘在右边是由Hamilton形式四元数决定的。从局部扰动可以获得对应在局部切空间的等效向量再利用指数映射:可以得到如果扰动很小那么可以进行近似有如下结果扰动量为对应的SO(3)流形处切空间的元素方便去定义协方差矩阵.全局扰动扰动值对应原点处的切空间元素。3.2. 对时间的导数局部扰动求导初始状态扰动状态为求导公式可得为q所在的局部坐标系的角速度向量对应的的局部扰动量具体求导形式如下定义可以得到可以看到角速度是在特定参考系中的量可以从IMU传感器直接获取全局扰动求导其中对应全局坐标系下的角速度.3.3. 局部与全局之间的变换关系3.4. 利用Adjoint来变换利用性质也可以得到对应的结果对于4. 流形上的优化参考文献slam/liegroup/README.md · Zixin-YANG/SummerCamp - Gitee.comg2o中 EdgeSE3Expmap类型Jacobian的计算_白巧克力亦唯心的博客-CSDN博客A micro Lie theory for state estimation in robotics