基于g2o图优化slam方案（1）

最新推荐文章于 2023-05-19 17:59:40 发布

jiajiading

最新推荐文章于 2023-05-19 17:59:40 发布

阅读量1.5k

点赞数

分类专栏： slam 文章标签： slam 闭环图优化

本文链接：https://blog.csdn.net/jiajiading/article/details/101457927

版权

slam 专栏收录该内容

29 篇文章 89 订阅

订阅专栏

基于g2o图优化slam方案（1）

基于图优化的SLAM主要内容组成
位姿预测
位姿修正
构建图优化所使用的顶点与边
闭环检测
图优化
地图构建并发布
总结

基于图优化的SLAM主要内容组成

slam主要包括2d slam 和 3d slam，本文以激光2d slam为背景进行描述

位姿预测
位姿修正
构建图优化所使用的顶点与边
闭环检测
图优化
地图构建并发布

位姿预测

slam是实时建图与定位，实际在算法实现上，是相互交替完成，而所创地图也为增量地图。即先已知初始位姿（x₀,y₀,yaw₀)，并依据当前位姿采集的数据点云构建一帧局部地图。通过一定方法预测下一刻位姿（x₁,y₁,yaw₁）,然后再将此刻位姿的激光点云再次构建一帧局部地图。如此往复，则实现了slam最基本的流程。其中机器人移动后，如何根据当前位姿获取下一位姿过程，则称为位姿预测。
已知当前时刻t位姿（x_t,y_t,yaw_t,），可通过里程计和陀螺仪等传感器获取机器人位移与旋转量（L,w）。则下一刻预测位姿为。

$x_{t+1} = x_t + L*\cos(w)$
$y_{t+1} = y_t + L*\sin(w)$
$yaw_{t+1} = yaw_t + w$

因此已知初始时刻，则可预测接下来每刻位姿。现已知预测位姿状态X，和当前位姿对应点云PCL，构建激光点云状态scan（X,PCL）。

位姿修正

由于里程计和陀螺仪等传感器会存在一定误差，因此采用激光点云匹配进行进一步修正，使其预测位姿更加精准。
将激光点云状态scan从初始时刻开始记录按顺序依次放入队列scan_queue。并将scan_queue中最近的n帧激光数据（scan_queue[size-n: size]）构建局部栅格地图submap。依据当前激光状态scan_curr，在submap中进行点云匹配，可采用高斯-牛顿迭代法或粒子滤波等方法进行匹配，获取最佳位姿X_match。
将X_match替换scan_queue中当前scan_curr中的位姿状态X。

构建图优化所使用的顶点与边

在2d 激光slam算法中，所需优化和修正的仍是机器人所经过的位姿。由于里程计存在累计误差，同时经过scan_to_Map中的map也是由局部位姿与其对应点云构建增量地图。因此所记录的激光状态队列scan_queue中位姿存在累计误差，因此需要进行优化。如此可看出，优化中顶点为机器人走过的位姿（x,y,yaw）。因此需要将scan_queue中每个状态的位姿放入g2o顶点中。
而图优化中的边为顶点的约束条件，由于顶点为位姿，则边应为位姿之间的误差（delta_x_t_t-1,delta_y_t_t-1,delta_yaw_t_t-1）。
在未闭环前，则优化并无意义，因此未闭环前，边可为相临两顶点位姿误差；

闭环检测

所谓闭环检测，是机器人回到曾经走过的位姿附近，并能识别。由于编码器构建里程计和激光里程计均存在累计误差，因此当机器人行走一段距离重新回到曾经经过的附近，机器人预测记录的位姿并不一定能够重合。因此构建的增量地图，在回到曾经的位置时，会出现重叠，错位，而不能实现完美匹配，我们称为不闭合现象。因此闭环检测在slam中的举足轻重。
闭环检测的目的就是当前机器人通过前面位姿预测当前状态scan_curr(X_curr,PCL_curr)。而当前scan_curr曾经经过此位置附近。因此采用当前scan_curr在曾经的scan_old(从scan_queue队列中提取)构建的submap中进行激光匹配，计算最佳位姿X_best。理论上X_best应与X_curr一致，实际并非如此。因此增加当前位姿与曾经经过附近的顶点位姿之间的约束，即X_best与scan_old中的X_old之间的误差。
而图优化思想则是根据预测连续顶点的约束条件和闭环检测增加约束条件，进行优化。

图优化

可采用g2o进行优化，前面已说明优化器中顶点与边。因此创建优化器如下：

typedef g2o::BlockSolver<g2o::BlockSolverTraits<-1, -1>> SlamBlockSolver;
typedef g2o::LinearSolverCholmod<SlamBlockSolver::PoseMatrixType> SlamLinearSolver;

    SlamBlockSolver::LinearSolverType* linear_solver = new SlamLinearSolver; 
    SlamBlockSolver* solver_ptr = new SlamBlockSolver(linear_solver);       
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); 
    optimizer_.setAlgorithm(solver);

添加顶点：

    g2o::VertexSE2* vertex = new g2o::VertexSE2;                     // 构建节点（VertexSE2 表示2维平面，包括x，y，theta）
    vertex->setEstimate(g2o::SE2(x, y, yaw));                              // 添加预测值
    vertex->setId(Id);                                                                     // 每个节点都必须有固定id，即激光队列中index
    optimizer_.addVertex(vertex);

添加边：

    g2o::EdgeSE2* edge = new g2o::EdgeSE2;
    edge->vertices()[0] = optimizer_.vertex(source_id);                                      // 指定边的两个顶点
    edge->vertices()[1] = optimizer_.vertex(target_id);

    g2o::SE2 measurement(delta_pose[0], delta_pose[1], delta_pose[2]);         //指定两个顶点差值
    edge->setId(edge_count);
    edge->setMeasurement(measurement);                                         
    edge->setInformation(information);
    edge_count++;
    optimizer_.addEdge(edge);

进行图优化：

optimizer_.initializeOptimization();         
int iter = optimizer_.optimize(100);

获取优化结果：

    g2o::SparseOptimizer::VertexContainer nodes = optimizer_.activeVertices();

    for(g2o::SparseOptimizer::VertexContainer::const_iterator n = nodes.begin(); n != nodes.end(); ++n) {
        double estimate[3];
        if((*n)->getEstimateData(estimate)) {
         std::cout << "result:" << estimate<< std::endl;     // 调试使用
        }
    }