多传感器融合定位GNSS、IMU、Lidar、Camera

概述

0.坐标系

为了求解速度分量，相当于local



自动驾驶如何应用这些坐标系：

坐标系直接的转换公式：


航向角：
北东地，北偏东某某度

1.纯激光雷达点云定位
2.融合GNSS的点云定位
3.融合IMU的点云定位

IMU介绍

https://blog.csdn.net/hbsyaaa/article/details/108186892
IMU，即惯性测量单元，一般包含三轴陀螺仪与三轴加速度计。
1：通过其内部的DMP处理单元直接得到姿态解算的四元数结果。
2：通过软件解算的方式，利用欧拉角与旋转矩阵来对陀螺仪与加速度计的原始数据进行姿态求解，并将两种姿态进行互补融合，最终得到IMU的实时姿态

我们使用IMU器件最终想要获取的是位姿数据，包含位置和姿态，而实际IMU直接测量的数据是加速度和角速度，需要对角速度进行一次积分计算角度，也就是姿态；对加速度进行二次积分获取距离，也就是位置。由于种种原因，所有的传感器测量数据，都有测量误差，而IMU是通过积分实现间接测量值，且其参考数据是上时刻数据，这就造成其误差也会随着时间传递下去，时间越久，其误差越大，这一特性成为数据漂移，所以IMU一般需要与其它传感器配合使用。

某些IMU只有线加速度和角加速度，没有四元素；
那么如何求解

根据加速度计解算姿态角，会有误差，因为靠重力加速度

方式二还可以利用重力分离求解roll pitch，但是yaw无法求解

// 订阅IMU
void imuHandler(const sensor_msgs::Imu::ConstPtr& imuIn)
{
    #ifdef INITIAL_BY_IMU
    if(!initialImu)
    {
        // float imupitch_ = atan2(-imuIn->linear_acceleration.x, 
        //                   sqrt(imuIn->linear_acceleration.z*imuIn->linear_acceleration.z + 
        //                   imuIn->linear_acceleration.y*imuIn->linear_acceleration.y));
        int signAccZ;
        if(imuIn->linear_acceleration.z >= 0) signAccZ = 1;
        else signAccZ = -1;
        float imupitch_ = -signAccZ * asin(imuIn->linear_acceleration.x);
        float imuroll_ = signAccZ * asin(imuIn->linear_acceleration.y);
        // float imuroll_ = atan2(imuIn->linear_acceleration.y, imuIn->linear_acceleration.z);
        Eigen::AngleAxisf imuPitch = Eigen::AngleAxisf(imupitch_, Eigen::Vector3f::UnitY());
        Eigen::AngleAxisf imuRoll = Eigen::AngleAxisf(imuroll_, Eigen::Vector3f::UnitX());
        Ext_Livox.rotate(imuRoll * imuPitch);
        // Ext_Livox.pretranslate();
        initialImu = true;
    }
    #endif
}

IMU预积分

https://maimai.cn/article/detail?fid=1658970578&efid=XAH0JEgstI0sq0-pwwnJUg

预积分问题引入

这种融合模型的核心问题在于，IMU的约束是通过惯性积分得到的，而惯性积分是以它前一时刻的位姿为初始值得到的，但是位姿每次优化后会发生变化，这导致其后的IMU惯性积分就要重新进行，显然运算量过大。

这个问题的解决思路就是直接计算两帧之间的相对位姿，而不依赖初始值影响，即所谓的预积分。本篇文章剩余的篇幅就几乎全在讨论预积分的问题。

1. 使用思路
   
2. 残差设计
   
   
   
   
   类似于kalman滤波

IMU与GPS

在自动驾驶定位算法中，IMU+GPS被称为黄金搭档，其主要原因是这两种器件互补性比较好，其数据有以下特征：

① 频率互补，GPS频率一般为10Hz，IMU频率一般为100Hz。
② 数据干扰互补，GPS数据易受环境影响，例如高楼、隧道等；IMU不易受外界干扰。
③ 数据误差互补，如上述第六点所述，IMU存在数据漂移，GPS每次测量都是独立的，即与上次测量无关，所以不存在误差累计。
④ IMU测量的是相对位置，GPS测量的是绝对位置。

    // add imu msg
    sensor_msgs::ImuConstPtr imu_msg = m.instantiate<sensor_msgs::Imu>();
    if (imu_msg) {
      ImuData data;
      data.acc = Eigen::Vector3d(imu_msg->linear_acceleration.x, //表示线加速度
                                 imu_msg->linear_acceleration.y,
                                 imu_msg->linear_acceleration.z);
      data.gyr = Eigen::Vector3d(imu_msg->angular_velocity.x, //角速度
                                 imu_msg->angular_velocity.y,
                                 imu_msg->angular_velocity.z);
      // orientation是由linear_acceleration和angular_velocity计算而得
      data.rot =
          Eigen::Quaterniond(imu_msg->orientation.w, //姿态，使用四元数表示
                             imu_msg->orientation.x, imu_msg->orientation.y,
                             imu_msg->orientation.z);
      data.stamp = imu_msg->header.stamp.toSec();

4.融合GNSS、IMU的点云定位
5.融合Camera的点云定位

参考AutoWare 或者 Apollo

AutoWare：
https://github.com/autowarefoundation/autoware_ai_perception
Apollo：
https://github.com/ApolloAuto/apollo/tree/master/modules/localization

1.纯激光雷达点云定位

LOAM系列算法配置及建图测试：LIO-SAM、LeGO、A-LOAM、LIO-mapping

2.融合GNSS的点云定位

3.融合IMU的点云定位

4.融合GNSS、IMU的点云定位

5.融合Camera的点云定位