Fast-lio个人总结_fastlio重力对齐

Lidar第一帧作为基坐标

1、lidar原始数据预处理默认不提取特征，对原始数据间隔式（间隔3个点）降采样提取。

2、imu初始化、惯性解算、误差分析、状态、协方差预测

3、Lidar与imu时间状态对齐

4、Lidar去畸变

5、构建局部地图，提供最近点索引，最小二乘方法拟合平面，构建点面残差

6、误差卡尔曼滤波

一、IMU

IMU初始化：计算加速度、角速度均值，利用均值计算方差。

IMU惯性解算、误差分析：状态、协方差预测

惯性解算：

利用相邻两个时刻的imu数据，采用离散中值法作为运动方程输入的加速度和角速度，并去除零偏。

Fast-lio认为加速度计方向是准确的，将已知当地的重力大小做先验，利用比例和先验重力微调重力大小。3自由度变为2个自由度。

计算旋转、加速度（转全局坐标系下并去重力和零偏）、位置传播、速度传播进行状态预测。

两种传播情况：当imu的head和tail在lidar帧前时，imu从head传播到tail，当lidar帧出现在imu两帧之间时，imu积分传播从lidar帧开始。

误差分析：

构建误差微分方程，得到状态转移矩阵，进行协方差传递。

二、lidar与Imu时间和状态对齐

计算lidar与imu最后时刻的时间差

通过Imu的加速度与角速度预估lidar最后时刻的速度、旋转和位置，此时仍是在imu坐标系下

通过外参计算出Lidar最后时刻世界坐标系下的状态。imu积分结果给lidar位姿提供初值。

三、Lidar点云去畸变

目的：将一帧中每个激光点云投影到最后时刻

获得每个激光点云的位姿，两帧imu之间会有许多lidar点，通过插值法，即得lidar与imu头帧的时间差，积分得lidar在imu下的位姿，再积分指lidar末尾，再通过外参转至lidar系下。

四、构建局部地图

以lidar帧最后一个状态构建立方体，立方体长宽高为48

地图管理：在ikdtree上保留当前lidar位置周围得局部地图，根据去畸变后的点云构造三维体素栅格，对体素栅格下采样达到滤波得目的（一个体素用中心点来近似，减少点的数量，保持点云形状）。

第一帧lidar数据用来构造ikdtree，其他帧加入树进行索引变换构建局部地图，当前点利用最近邻搜索找最近点5个（最远距离小于5m），最小二乘法将最近点拟合平面，获得平面方程系数，检验计算平面的有效性：将最近点集合带入平面方程得残差，残差与阈值（0.05）做比较，误差超过阈值，拟合得平面有问题，小于阈值，拟合得平面有效。计算点到平面的残差（当前点带入平面方程），构造观测方程和雅可比矩阵（残差关于优化变量求导）得到H阵。链式法则求偏导，观测方程先对状态求导，状态量对误差状态量求导。

五、误差卡尔曼滤波

名义值变量（8个、23维）--位置、姿态、旋转外参、平移外参、速度、加速度零偏、角速度零偏、重力

P阵：状态协方差矩阵（23*23维）

过程噪声维数Q（4个、12维）--加速度、角速度白噪声、加速度角速度零偏白噪声。斜对角阵

状态转移矩阵Fx（24*23维），噪声转移矩阵Fw（24*12维）。

R阵：测量噪声协方差矩阵（n*n维）一帧中有n个点云，斜对角阵

观测方程的雅可比矩阵H阵（n*12维）一帧中有n个点云，残差与位置、姿态、旋转外参、平移外参这四个优化变量有关，共12维。

计算卡尔曼滤波增益：如果量测维度小于状态维度，用传统公式（n*n维），否则用新公式（12*12维）

误差值会随着迭代次数增加而减小，当相邻两次增量之差小于一定阈值就认为是收敛，或者达到最大迭代次数也将停止迭代。用误差的后验去更新位姿并且更新P阵（状态协方差矩阵）