深蓝激光slam

  svd分解的意思是将矩阵携程由三个部分相乘的形式:

PIC方法：

 

VICP方法介绍

• ICP方法在激光匹配中的缺点

  • 没有考虑激光的运动畸变

  • 当前激光数据是错误的

    ICP默认给定的点云数据是正确的，给错数据他也认为是对的

  • 所以提出了VICP

• VICP是ICP算法的变种，听名字就知道他在ICP的基础上提供了速度估计因为他是考虑了机器人的运动过程和激光的测量过程所产生的运动畸变。

• 他的速度估计是认为机器人运动过程中是匀速运动的，虽然这也不一定是匀速运动，但总比在ICP算法中认为他没动要好得多。进行匹配的同时估计机器人的速度。

 VICP公式如下：其中的ti表示时间，Ti为当前时刻的位姿矩阵。▲t为两帧数据之间的间隔时间。Vi为机器人速度（计算速度公式在下面）。

所以校正过程可为：（运动校正），根据每两帧之间的时间差得到激光数据和位姿数据，随后通过计算得到机器人速度，来用每个点的速度来校准激光数据

 最后得到的是一个数据。

总结轮式里程计去除运动畸变（常用的）

• 已知数据：1.已知当前 帧激光的起始时间 𝑡𝑠,𝑡𝑒

  2.两个激光束间 的时Δ𝑡

  3.里程 计数据按照时间顺序存储在一个队列 中

  4.最早的里程计数据时间戳 <𝑡𝑠

  5.最早的里程计数据时间戳 >𝑡𝑒。

• 他要做的就是1.求解当前帧激光数据中每一个点对应的机器人位姿，即求解｛ 𝑡𝑠,𝑡𝑠+Δ𝑡,⋯,𝑡𝑒｝时刻的机器人位姿。2.根据求解的位姿把所有的激光点转换到同一坐标系下。3.重新封装成一帧激光数据，发布出去。

• 过程：

  • 1.求解ts，te时刻的位姿ps，pe

    • 里程计队列中正好和激光数据同步，假设第 i和第 j跟数据是时刻分别为 𝑡𝑠,𝑡𝑒:

      𝑝𝑠=𝑂𝑑𝑜𝑚𝐿𝑖𝑠𝑡[𝑖]
      𝑝𝑒=𝑂𝑑𝑜𝑚𝐿𝑖𝑠𝑡[𝑗]

    • 在𝑡𝑠时刻没有对应的里程计位姿，则进行线性插值设在 l,k时刻有位姿，且 l<s<k,则：

      𝑝𝑙=𝑂𝑑𝑜𝑚𝐿𝑖𝑠𝑡[𝑙]
      𝑝𝑘=𝑂𝑑𝑜𝑚𝐿𝑖𝑠𝑡[𝑘]
      𝑝𝑠=𝐿inar𝐼𝑛𝑡𝑒𝑟𝑝(𝑝𝑙,𝑝𝑘,𝑠−𝑙𝑘−𝑙)

  • 2.进行二次插值

    • 在一帧激光数据之间，认为机器人做匀加速运动。
    • 机器人的位姿是关于时间 t的二次函数。
    • 设𝑡𝑚=（𝑡𝑠+𝑡𝑒）/2，且 l<m<k 则：
      𝑝𝑚=𝐿inar𝐼𝑛𝑡𝑒𝑟𝑝(𝑝𝑙,𝑝𝑘,𝑚−𝑙 / 𝑘−𝑙)
    • 已知 𝑝𝑠,𝑝𝑚,𝑝𝑒，可以插值一条二次曲线：
      𝑃𝑡=𝐴𝑡2+𝐵𝑡+𝐶
      𝑡𝑠≤𝑡≤𝑡𝑒

  • 3.二次曲线的近似

    • 用分段线性函数对二次曲进行近似
    • 分段数大于 3时，近似误差可以忽略不计
    • 在𝑡𝑠和𝑡𝑒时间段内，一共取 k个位姿 𝑝𝑠,𝑝𝑠+1,⋯,𝑝𝑠+𝑘−2,𝑝𝑒
    • 位姿通过线性插值获取，在这 K个位姿之间，进行线性插值：
      设𝑝𝑠和𝑝𝑠+1之间有 N个位姿 ｛𝑝𝑠,𝑝𝑠1,⋯,𝑝𝑠(𝑛−2),𝑝𝑠+1｝
      则：
      𝑝𝑠𝑖=𝐿inar𝐼𝑛𝑡𝑒𝑟𝑝(𝑝𝑙,𝑝𝑘,𝑠𝑖−𝑠 / Δ𝑡)

  • 4.坐标系统和激光数据的发布

    • 一帧激光数据 n个激光点，每对应的位姿 𝑝1,𝑝2,⋯,𝑝𝑛通过上述介绍的方法插 值得到

    • 𝑥𝑖为转化之前的坐标， 𝑥𝑖′为转换之后的坐标，则：

      𝑥𝑖′=𝑝𝑖𝑇𝑥𝑖

    • 把转换之后的坐标为激光数据发布出去：

      𝑥𝑖′=（𝑝𝑥,𝑝𝑦）

      𝑟𝑎𝑛𝑔𝑒=sqrt（𝑝𝑥∗𝑝𝑥+𝑝𝑦∗𝑝𝑦）

      𝑎𝑛𝑔𝑙𝑒=𝑎𝑡𝑎𝑛2(𝑝𝑦,𝑝𝑥)

• 在单片机上处理，直接用单片机接雷达，直接在单片机上消除运动畸变（因为单片机测量的里程计数据和雷达数据是一起上发的，所以他两时间同步，就不会产生运动畸变），无需考虑时间同步的问题

• 在处理器上处理，用USB接单片机和激光雷达，但他两的数据需要进行时间同步才能消除运动畸变。

 void Lidar_MotionCalibration(
        tf::Stamped<tf::Pose> frame_base_pose,
        tf::Stamped<tf::Pose> frame_start_pose,
        tf::Stamped<tf::Pose> frame_end_pose,
        std::vector<double>& ranges,
        std::vector<double>& angles,
        int startIndex,
        int& beam_number)
    {
        //TODO
        // frame_base_pose.inverse;
        tf::Quaternion start_q = frame_start_pose.getRotation();
        tf::Quaternion end_q = frame_end_pose.getRotation();
        tf::Vector3 start_xy(frame_start_pose.getOrigin().getX(), frame_start_pose.getOrigin().getY(), 1);
        tf::Vector3 end_xy(frame_end_pose.getOrigin().getX(), frame_end_pose.getOrigin().getY(), 1);
 
        for (size_t i = startIndex; i < startIndex + beam_number; i++) {
            tf::Vector3 mid_xy = start_xy.lerp(end_xy, (i - startIndex) / (beam_number - 1));
            tf::Quaternion mid_q = start_q.slerp(end_q, (i - startIndex) / (beam_number - 1));
            tf::Transform mid_frame(mid_q, mid_xy);
            double x = ranges[i] * cos(angles[i]);
            double y = ranges[i] * sin(angles[i]);
            tf::Vector3 calib_point = frame_base_pose.inverse() * mid_frame * tf::Vector3(x, y, 1);
            ranges[i] = sqrt(calib_point[0] * calib_point[0] + calib_point[1] * calib_point[1]);
            angles[i] = atan2(calib_point[1], calib_point[0]);
        }
        //end of TODO
    }

原文链接：https://blog.csdn.net/p942005405/article/details/119494918

------------------------------------------------------------------------------------------- 

基于图优化：

图优化理论

        图优化又被称为Graph-based SLAM，它的基本思想是将机器人不同时刻的位姿抽象为点（pose），机器人在不同位置上的观测所产生的约束被抽象为点之间的边，或者叫约束（constraint）。地图的边是指地图中不同特征之间的关联关系或者约束。这些边可以是来自传感器测量的信息，也可以是通过运动模型或环境特性推断出来的。地图的边在图优化中起着重要作用，用于优化机器人在环境中的定位和地图构建。

非线性最小二乘在图优化SLAM中的应用
SLAM前端：图的构建

​ 

构建地图的边方式分为两种：

第一种是里程计测量构建边

机器人从节点xi运动到节点xi+1，里程计测量得到此运动信息。并在对应的节点中连上一条边，边为里程计测量值。

第二种是回环检测构建边

节点xi和节点xj观测到同样的环境信息（比较多的重合），那么两者进行匹配得到相对位姿。并在对应的节点中连一条边，边为匹配的相对位姿。用信息矩阵来描述本次匹配的可靠性。

 2.1 回环检测

       后端： 现在，回环边有了，节点和边也有了，就可以构建观测值（匹配计算得到的节点𝑖和节点𝑗的相对位姿）与预测值（里程积分得到的当前节点𝑖和节点𝑗的相对位姿）的误差函数来优化各个节点的位姿了，误差函数如下图所示。

       

误差函数去预测值都知道了，现在可以正式开始对图中的位姿进行优化了。由于器人位姿之间的变化函数是不连续性，也就是说误差函数是非线性的，图优化问题是一个非线性最小二乘问题。非线性最小二乘求解流程如下：

 使用上述方法对误差函数求解，有：

         对于每一个位姿，都需要求解一次Jacobian矩阵，非常耗时、耗资源，这是不可取的。后来大牛通过分析，发现Jacobian矩阵具有稀疏性，这一发现，大大简化了Jacobian矩阵的求解，具体如下：

 注意：
       由于观测值观测到的是两个位姿之间的相对位姿，所以在满足相对位姿约束的情况下，在任意坐标系，都能找到一个解满足约束，这也意味着解有无穷多的。为了让解唯一，必须加入一个约束条件让某一个位姿固定，一般选择第一个位姿，即：

等价于：

------------------------------------------------------------------------------