当前位置： 首页 > article >正文

高翔【自动驾驶与机器人中的SLAM技术】学习笔记（十）高翔书中的细节：参考链接；卫星导航；ESKF

article 2024/11/19 8:37:03

一、 参考链接

我认真查找了好多地方：结果在最后一页。

作者GITHUB链接如下：
https://github.com/gaoxiang12/slam_in_autonomous_driving

全书所有参考链接 ：如下

1 https://www.sae.org/standards/content/j3016_202104
2 http://www.evinchina.com/articleshow-217.html
3 Arcgis: https://www.arcgis.com/
4 Autoware: https://github.com/autowarefoundation/autoware
5 工具地址见：https://github.com/hobu/mgrs
6 ÀEPFL雕像数据集：https://lgg.epfl.ch/statues_dataset.php
7 https://github.com/HKUST-Aerial-Robotics/A-LOAM
8 https://github.com/wh200720041/floam

二、卫星导航

全球卫星导航系统（Global Navigation Satellite System，GNSS），简称卫星导航。

GNSS 通过测量自身与地球周围各卫星的距离来确定自身的位置，而与卫星的距离主要是通过测量时间间隔来确定的。一个卫星信号从卫星上发出时，带有一个发送时间。而GNSS接收机接收到它时，又有一个接收时间。比较接收时间与卫星发送时间，就能估算各卫星离我们的距离。而各种 GNSS 和测量方法的主要差异，就是如何减少这个时间测量的误差。从这种角度来看，GNSS本质上可以看成一种高精度的授时系统。

目前，世界范围内，我们可以接收到的卫星信号主要来自四个系统：

  • 美国的全球定位系统Global Positioning System,GPS )、
  • 中国的北斗卫星导航系统(Beidou Navigation Satellite System，BDS)、
  • 俄罗斯的格洛纳斯系统(GLONASS)、
  • 欧盟的伽利略系统(GALILEO)。

对于自动驾驶车辆来说，最常用的卫星定位技术包括以下几种。

  1. 单点 GNSS 定位，即传统的米级精度卫星定位。这种定位方式价格低廉，应用广泛。大多数手机、车机等终端都具备单点卫星定位能力。在普通车辆的道路级导航中，单点定位的精度足以让驾驶人员辨认出车辆位于哪条道路，但在多条道路并排（车道级）时，它的精度又往往不足以区分车辆是在高速路上还是在辅路上，或者是在主路上还是在匝道上。
  2. RTK 定位。由于卫星定位信号在传输过程中可能产生误差，人们发展了差分定位技术，即通过地面上的一个已知精确位置的基站与车辆通信，校正车辆卫星接收机的信号。差分定位又可进一步分为位置、伪距及载波相位差分定位。其中最广泛使用的，是基于载波相位差分的 RTK 技术。RTK 通过与一个或多个基站进行通信，可以实时地获取校正后的卫星导航位置。

自动驾驶通常需要车道级导航而非道路级导航。车道级导航可以指出车辆位于道路当中哪个车道，比道路级导航更稳定。

世界坐标系

  • 地理坐标系：经纬度
  • UTM坐标系：米制坐标

GNSS代码：src/common/gnss.h

//
// Created by xiang on 2022/1/4.
//

#ifndef SLAM_IN_AUTO_DRIVING_GNSS_H
#define SLAM_IN_AUTO_DRIVING_GNSS_H

#include "common/eigen_types.h"
#include "common/message_def.h"

namespace sad {

/// GNSS状态位信息
/// 通常由GNSS厂商提供，这里使用千寻提供的状态位
enum class GpsStatusType {
    GNSS_FLOAT_SOLUTION = 5,         // 浮点解（cm到dm之间）
    GNSS_FIXED_SOLUTION = 4,         // 固定解（cm级）
    GNSS_PSEUDO_SOLUTION = 2,        // 伪距差分解（分米级）
    GNSS_SINGLE_POINT_SOLUTION = 1,  // 单点解（10m级）
    GNSS_NOT_EXIST = 0,              // GPS无信号
    GNSS_OTHER = -1,                 // 其他
};

/// UTM 坐标
struct UTMCoordinate {
    UTMCoordinate() = default;
    explicit UTMCoordinate(int zone, const Vec2d& xy = Vec2d::Zero(), bool north = true)
        : zone_(zone), xy_(xy), north_(north) {}

    int zone_ = 0;              // utm 区域
    Vec2d xy_ = Vec2d::Zero();  // utm xy
    double z_ = 0;              // z 高度（直接来自于gps）
    bool north_ = true;         // 是否在北半球
};

/// 一个GNSS读数结构
struct GNSS {
    GNSS() = default;
    GNSS(double unix_time, int status, const Vec3d& lat_lon_alt, double heading, bool heading_valid)
        : unix_time_(unix_time), lat_lon_alt_(lat_lon_alt), heading_(heading), heading_valid_(heading_valid) {
        status_ = GpsStatusType(status);
    }

    /// 从ros的NavSatFix进行转换
    /// NOTE 这个只有位置信息而没有朝向信息，UTM坐标请从ch3的代码进行转换
    GNSS(sensor_msgs::NavSatFix::Ptr msg) {
        unix_time_ = msg->header.stamp.toSec();
        // 状态位
        if (int(msg->status.status) >= int(sensor_msgs::NavSatStatus::STATUS_FIX)) {
            status_ = GpsStatusType::GNSS_FIXED_SOLUTION;
        } else {
            status_ = GpsStatusType::GNSS_OTHER;
        }
        // 经纬度
        lat_lon_alt_ << msg->latitude, msg->longitude, msg->altitude;
    }

    double unix_time_ = 0;                                  // unix系统时间
    GpsStatusType status_ = GpsStatusType::GNSS_NOT_EXIST;  // GNSS 状态位
    Vec3d lat_lon_alt_ = Vec3d::Zero();                     // 经度、纬度、高度，前二者单位为度
    double heading_ = 0.0;                                  // 双天线读到的方位角，单位为度
    bool heading_valid_ = false;                            // 方位角是否有效

    UTMCoordinate utm_;       // UTM 坐标（区域之类的也在内）
    bool utm_valid_ = false;  // UTM 坐标是否已经计算（若经纬度给出错误数值，此处也为false）

    SE3 utm_pose_;  // 用于后处理的6DoF Pose
};

}  // namespace sad

using GNSSPtr = std::shared_ptr<sad::GNSS>;

#endif  // SLAM_IN_AUTO_DRIVING_GNSS_H

给出了GNSS的状态枚举；以及UTM坐标的数据结构和GNSS的读数数据结构。

其中GNSS的数据结构与ros中的NavSatStatus消息可以转换。

注意GNSS中，有几个属性字段：一个是通过utm_convert转换之后的坐标。另一个是基于UTM坐标的6自由度pose。

antenna：天线

//
// Created by xiang on 2022/1/4.
//

#include <glog/logging.h>
#include <iomanip>
#include <memory>

#include "common/gnss.h"
#include "common/io_utils.h"
#include "tools/ui/pangolin_window.h"
#include "utm_convert.h"

DEFINE_string(txt_path, "./data/ch3/10.txt", "数据文件路径");

// 以下参数仅针对本书提供的数据
DEFINE_double(antenna_angle, 12.06, "RTK天线安装偏角（角度）");
DEFINE_double(antenna_pox_x, -0.17, "RTK天线安装偏移X");
DEFINE_double(antenna_pox_y, -0.20, "RTK天线安装偏移Y");
DEFINE_bool(with_ui, true, "是否显示图形界面");

/**
 * 本程序演示如何处理GNSS数据
 * 我们将GNSS原始读数处理成能够进行后续处理的6自由度Pose
 * 需要处理UTM转换、RTK天线外参、坐标系转换三个步骤
 *
 * 我们将结果保存在文件中，然后用python脚本进行可视化
 */

int main(int argc, char** argv) {
    google::InitGoogleLogging(argv[0]);
    FLAGS_stderrthreshold = google::INFO;
    FLAGS_colorlogtostderr = true;
    google::ParseCommandLineFlags(&argc, &argv, true);

    if (fLS::FLAGS_txt_path.empty()) {
        return -1;
    }

    sad::TxtIO io(fLS::FLAGS_txt_path);

    std::ofstream fout("./data/ch3/gnss_output.txt");
    Vec2d antenna_pos(FLAGS_antenna_pox_x, FLAGS_antenna_pox_y);

    auto save_result = [](std::ofstream& fout, double timestamp, const SE3& pose) {
        auto save_vec3 = [](std::ofstream& fout, const Vec3d& v) { fout << v[0] << " " << v[1] << " " << v[2] << " "; };
        auto save_quat = [](std::ofstream& fout, const Quatd& q) {
            fout << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << " ";
        };

        fout << std::setprecision(18) << timestamp << " " << std::setprecision(9);
        save_vec3(fout, pose.translation());
        save_quat(fout, pose.unit_quaternion());
        fout << std::endl;
    };

    std::shared_ptr<sad::ui::PangolinWindow> ui = nullptr;
    if (FLAGS_with_ui) {
        ui = std::make_shared<sad::ui::PangolinWindow>();
        ui->Init();
    }

    bool first_gnss_set = false;
    Vec3d origin = Vec3d::Zero();
    io.SetGNSSProcessFunc([&](const sad::GNSS& gnss) {
          sad::GNSS gnss_out = gnss;
          if (sad::ConvertGps2UTM(gnss_out, antenna_pos, FLAGS_antenna_angle)) {
              if (!first_gnss_set) {
                  origin = gnss_out.utm_pose_.translation();
                  first_gnss_set = true;
              }

              /// 减掉一个原点
              gnss_out.utm_pose_.translation() -= origin;

              save_result(fout, gnss_out.unix_time_, gnss_out.utm_pose_);

              if (ui) {
                  ui->UpdateNavState(
                      sad::NavStated(gnss_out.unix_time_, gnss_out.utm_pose_.so3(), gnss_out.utm_pose_.translation()));
                  usleep(1e3);
              }
          }
      }).Go();

    if (ui) {
        while (!ui->ShouldQuit()) {
            usleep(1e5);
        }
        ui->Quit();
    }

    return 0;
}

正如代码注释中所写的：将txt中的GNSS数据读取，然后转换成UTM数据，基于天线外参，坐标转换为6自由度的pose。将pose记录gnss_out。最后用gnss_out转换成导航状态量NavStated类。

此处是从txt读取数据，真实应用更可能订阅某个ROS话题节点。

三、误差状态卡尔曼滤波器（Error State Kalman Filter,ESKF）

RTK 设备为我们提供了一个不太稳定的位姿观测源。我们可以将它视为定位滤波器的一种观测。本节将RTK与IMU 结合，使用拓展卡尔曼滤波器形成传统的组合导航算法，以供后续的算法对比。严格来说，笔者向读者介绍的是误差状态卡尔曼滤波器(Error State Kalman Filter，ESKF)。ESKF 的应用十分广泛，从GINS组合导航到视觉SLAM[58-60]、外参自标定[61-62]等任务中都有应用。

这里提到了一个外参自动标定。

  • Visual-inertial sensor fusion: Localization, mapping and sensor-to-sensor self-calibration. 
  • A kalman flter-based algorithm for imu-camera calibration：Observability analysis and performance evaluation.

1、ESKF的数学推导演变

之前是把IMU视为观测模型。现在我们IMU视为运动模型，并把GNSS观测视为观测模型，推导整个滤波器。

状态估计问题嘛，先明确状态变量。

状态变量为：

\mathbf{x} = [\mathbf{p}, \mathbf{v},\mathbf{R},\mathbf{b}_g, \mathbf{b}_a, \mathbf{g}]^\top

所有变量都默认取下标()_{WB}，其中p为平移，v为速度，R为旋转，\mathbf{b}_g,\mathbf{b}_a为零偏，g为重力。

将IMU测量值带入公式3.1。状态变量在连续时间下的运动方程为公式3.22：

公式3.1：

公式3.22

直白点说：搞不定了，或者说不好搞了。得想个其他法子。

于是，能否避免直接使用x和P来表达状态的均值和协方差，推导运动和观察方程呢？能否使用原先卡尔曼滤波器中的更新量来推导这两个方程？

说直白点：状态量用x和P来表示均值和协方差，来推导运动方程和观察方程这个方式有困难推不动了，改用更新量来重新推导运动方程和观察方程。

回忆卡尔曼滤波器中的观测部分。咱们把运动方程和观察方程，以及“黄金五公式”一起回忆下。

在卡尔曼滤波器观测部分，我们看到在用卡尔曼增益更新状态时，（C和H看做等效）。

\boldsymbol{x}_k=\boldsymbol{x}_{k,\mathrm{pred}}+\boldsymbol{K}_k\underbrace{(\boldsymbol{z}_k-\boldsymbol{H}_k\boldsymbol{x}_{k,\mathrm{pred}})}_\text{Updates Value}.

之前提到这个公式有大用，基于此可以做一些操作了。

在流形意义下，右侧的更新量（Updates Value）（CSDN富文本编辑模式下，公式不支持中文，此处用了Updates Value）。原文如下：

应是位于切空间中的矢量，中间的加法应为流形与切空间指数映射的广义加法。但也可以将更新量(或者称为误差状态)视为滤波器的状态变量，来推导运动和观测模型。

这就引出了误差状态卡尔曼滤波器。

到这意思就是说，对于我们的状态估计问题，我们要估计的状态变为了误差状态。

这个更新量：其实就是观测结果与递推预测结果的差异。

  • A有一个数据，B有一个数据，真实数据应该是多少。
  • 这俩数据之间的差异，通过其协方差（可信度）来分配更倾向于A还是B。

这个之前理解KF时提及过。如何分配这个差异。

  • 矩阵H是从预测空间转换到观测空间的变换矩阵。
  • Hx就将递推预测的数据转换到观测空间，
  • 与观测数据进行比较。获取差异。

更进一步，不光是平移和旋转，把所有的状态都用误差状态来表达，这就是典型ESKF的做法。
ESKF是许多传统的、现代的系统里都广泛使用的状态估计方法，既可以作为组合导航的滤波器，也可以用来实现LIO、VIO 等复杂系统。

相比于传统KF，ESKF的优点可以总结如下：

  1. 在旋转的处理上，ESKF的状态变量可以采用最小化的参数表达，也就是使用三维变量来表达旋转的增量。该变量位于切空间中，而切空间是一个矢量空间。传统KF需要用到四元数(4维)或者更高维的变量来表达状态(旋转矩阵，9维)，要不就得采用带有奇异性的表达方式(欧拉角)。
  2. ESKF总是在原点附近，离奇异点较远，数值方面更稳定，并且不会产生离工作点太远而导致线性化近似不够的问题。
  3. ESKF的状态量为小量，其二阶变量相对来说可以忽略。同时，大多数雅可比矩阵在小量
    情况下变得非常简单，甚至可以用单位阵代替。
  4. 误差状态的运动学相比原状态变量更小(小量的运动学)，因此可以把更新部分归入原状
    态变量中。

个人理解哈：只考虑扰动模型的情况下，有各种好处。直白点：累计误差不会在更新中因累计放大而影响彼此。只考虑观察和预测的扰动模型即可。

在ESKF中，通常把原状态


http://www.kler.cn/a/322659.html

相关文章：

  • 【开源免费】基于SpringBoot+Vue.JS购物推荐网站（JAVA毕业设计）
  • 记录配置ubuntu18.04下运行ORBSLAM3的ros接口的过程及执行单目imu模式遇到的问题（详细说明防止忘记）
  • Java基础-I/O流
  • Linux dpkg命令详解
  • 【动手学深度学习Pytorch】1. 线性回归代码
  • 云速搭助力用友 BIP 平台快速接入阿里云产品
  • Redis篇（数据类型）
  • 桥接模式
  • 【区块链快速概览】了解区块链的基本原理、共识机制（如PoW, PoS）、加密技术基础。
  • uWsgi找不到新安装的python模块问题【亲测好用】
  • 通过 Xshell 无法连接到 Ubuntu
  • HarmonyOS鸿蒙开发实战（ Beta5.0）Web组件预览PDF文件实现案例
  • 简历信息提取系统源码分享
  • Postman如何测试WebSocket接口！
  • 企业内训｜大模型/智算行业发展机会深度剖析-某数据中心厂商
  • 设备管理系统-TPM（PC+APP/PDA全流程）高保真Axure原型 源文件分享
  • keepalived+nginx实现高可用的案例详解（主主模式）
  • 以题为例浅谈反序列化漏洞
  • 点餐小程序实战教程12菜品展示
  • 记一次 RabbitMQ 消费者莫名消失问题的排查
  • 【洛谷】AT_abc178_d [ABC178D] Redistribution 的题解
  • 摒弃“流量思维”，以精准流量驱动企业发展——基于开源 AI 智能名片、链动 2+1 模式及 O2O 商城小程序的思考
  • 【JavaScript】尾递归优化
  • en造数据结构与算法C# 之 二叉排序树的删除
  • 哪个快？用300万个图斑测试ArcGIS Pro的成对叠加与经典叠加
  • Spring Task快速入门