五. 激光雷达建图和定位方案-基础概念

前面内容：

一. 器件选型心得（系统设计）--1_goldqiu的博客-CSDN博客

一. 器件选型心得（系统设计）--2_goldqiu的博客-CSDN博客

二. 多传感器时间同步方案（时序闭环）--1

三. 多传感器标定方案（空间同步）--1_goldqiu的博客-CSDN博客

三. 多传感器标定方案（空间同步）--2

三. 多传感器标定方案（空间同步）--3

四. 点云着色（真彩点云）方案

五. 激光雷达建图和定位方案-引言-CSDN博客

五. 激光雷达建图和定位方案-基础概念

点与坐标系

定义世界坐标系w和机体坐标系b，那么坐标转换由旋转矩阵和平移矢量来描述：

添加图片注释，不超过 140 字（可选）

李群和李代数

• 旋转矩阵称为SO3群，变换矩阵称为SE3群 • SO3群的李代数记为so3，实际上就是旋转矢量 • SO3到so3的映射称为指数和对数映射，实际即为旋转矢量到旋转矩阵的转换公式

添加图片注释，不超过 140 字（可选）

BCH公式给出了李群上（左右）乘小量和李代数上的关系：

添加图片注释，不超过 140 字（可选）

解释：

李群左乘小量（用左乘李代数小量指数变换到的李群来表示）等于李代数加上李代数小量左乘左雅可比矩阵，然后再加反对称符号，指数变换到李群。

李代数加小量（李代数直接加小量，然后加反对称符号，再指数变换到李群）等于原来的李群左乘一个新的李群，即李代数小量左乘左雅可比矩阵再取反对称符号，指数变换到李群；或右乘一个新的李群，即李代数小量右乘右雅可比矩阵再取反对称符号，指数变换到李群。

SO3的左右雅可比阵：

添加图片注释，不超过 140 字（可选）

左右之间的关系：

添加图片注释，不超过 140 字（可选）

运动学

首先R是正交阵

添加图片注释，不超过 140 字（可选）

添加图片注释，不超过 140 字（可选）

于是：

添加图片注释，不超过 140 字（可选）

添加图片注释，不超过 140 字（可选）

是反对称矩阵。

那么：

添加图片注释，不超过 140 字（可选）

称为泊松方程。

意义是对李群求导等于右乘或者左乘一个反对称矩阵。

如果仅考虑瞬时变化，在固定时间t，可以认为w不变 • 称为瞬时角速度 • 瞬时角速度同时也是旋转向量，模长代表旋转轴，长度代表旋转速度。

将泊松方程视为常微分方程，可得：

添加图片注释，不超过 140 字（可选）

意义为：t0时刻车辆的姿态为R(t0)，经过∆t时间的角速度变换后，车辆的姿态为原来的姿态右乘一个李群，即对角速度乘∆t求指数变换。

于是，得到连续形式和离散形式上的运动学：

添加图片注释，不超过 140 字（可选）

这是处理IMU角速度的基础。

线速度与加速度

考虑带有旋转关系的两个坐标系，某个点p在两个坐标系下满足：

添加图片注释，不超过 140 字（可选）

添加图片注释，不超过 140 字（可选）

该式为速度矢量的变换关系。

注意：

• 速度矢量的变换，并不是速度矢量的坐标转换；

• 由于p在运动，同时两个坐标系也在转动，所以p在两个坐标系中的速度本身就不一样；

• 该式描述了p在1系中的速度与2系中速度之间的转换关系，而不是某个固定的速度矢量在两个系中取坐标（这个有时候也会用）。

继续求时间导数：

添加图片注释，不超过 140 字（可选）

添加图片注释，不超过 140 字（可选）

同样，这是两个系中的加速度矢量变换，而非坐标变换！

实际例子：

车辆在世界系中运动，我们关心车辆的运动速度，请问应该如何表示？

• 车的运动速度是指车身坐标系原点在世界坐标系中的速度矢量，记作Vw ；

• 该点在车体系下一直为原点，速度为零，所以不谈论这个点的速度；

• 我们谈论Vw在车体系下的坐标表示，即：

添加图片注释，不超过 140 字（可选）

这是坐标变换，并非前文的矢量变换；

• 该速度可以通过轮子、电机等设备测量到。

SO3的求导

将李群和李代数引入SO3后，求导相关的操作变得十分简单，而且全部都可以写成矩阵式操作 算例（右扰动模型）：

添加图片注释，不超过 140 字（可选）

李代数为小量。