8.紧耦合LIO系统
自动驾驶对SLAM的应用主要集中在定位与建图这两大模块。在定位模块中，也常常需要使用DR或LIO进行局部的位置估计。
8.1紧耦合的原理和优点
在进行学习之前，我们先来回顾一下什么是松耦合，松耦合和紧耦合的区别。
首先什么是松耦合，松耦合是将IMU得到的位姿估计以及点云匹配输出的位姿进行优化，将IMU和轮速计提供惯性和速度方面的观测源，点云匹配的结果为这个系统提供位姿数据的观测源，是对IMU和轮速计的位姿匹配结果进行优化。
什么是紧耦合呢？所谓紧耦合LIO系统，就是把点云的残差方程直接作为观测方程，写入观测模型中，这种做法相当于在滤波器或者优化算法中内置了一个ICP或NDT。因为ICP和NDT需要迭代来更新它们的最近邻，所以相应的滤波器也应该使用可以迭代的版本。可以看做直接将IMU得到的数据与点云数据进行融合。
紧耦合比松耦合好在哪里？以松耦合LIO系统来说，如果车辆经过了某段退化区域，那么单独以点云匹配方式推算自身运动的雷达里程计容易失效，它的解空间存在额外的自由度，给出错误的位姿估计，融合之后就会带偏整个系统。而紧耦合LIO系统中的状态还会受到其他传感器的约束，这些自由度会被别的模块约束在一个固定范围内，使系统仍然工作在有效状态中。简而言之，紧耦合对结果进行约束，使其固定好在一个范围内，即使遇到意外情况也能正常工作。
紧耦合和松耦合的核心区别：
8.2基于IEKF的LIO系统
8.2.1IEKF状态变量与运动方程
在紧耦合LIO系统中，IMU与点云模型使用相同的状态模型、运动方程与观测方程。最直接的方式是将它们全都写到EKF模型中，IMU提供运动过程的约束，点云提供观测方程的约束。由于点云匹配需要多次最近邻迭代之后才能得到正确的解，也需要将传统的EKF滤波器改成它的迭代版本IEKF。
运动过程包括以下两个部分。
由于雷达频率通常比IMU频率低，所以两个雷达数据之间会存在多个IMU读数方便起见，笔者不再使用上下标来记录多次递推的过程，而是把递推之后的状态估计记为。在使用观测方程进行修正之前，得到的估计均值和协方差就是这两个量。
8.2.2观测方程中的迭代方程
在紧耦合LIO系统中，把雷达的ICP、NDT残差作为观测方程，写入EKF的模型中。然而，
由于ICP和NDT都需要迭代才会收敛，因此应该为EKF的观测过程增加迭代过程。这个迭代
过程有以下几个要点。
1.迭代的核心逻辑：IEKF 是给 "基准状态"（从初始预测值开始）加 "状态修正量"，每次修正量都不同；一开始修正量是 0，状态的 "靠谱程度" 直接用初始预测值的靠谱程度。
2.观测与增益的更新：因为要反复迭代，每次都得重算卡尔曼增益、观测关系；当前的基准状态，是从初始点累加每次修正量得到的 ------ 可以理解为："这次迭代的基准状态是 ，算完修正后误差就清零，下次从新基准继续"。
3.状态靠谱程度（用协方差矩阵来表示）的小技巧：迭代没结束时，不用每次都重算状态的 "靠谱程度"，只需要把初始的 "靠谱程度" 转到当前的计算维度就行；等最后一次迭代，再统一更新这个 "靠谱程度"，并转到新基准对应的维度。
4.IEKF和EKF的核心区别：EKF 只在 "初始预测的基准状态" 处简化一次非线性关系；IEKF 每次迭代都要在 "当前的基准状态" 处重新简化，更新流程和 EKF 不一样，代码写起来也有明显差别。
5.LIO的特殊点：紧耦合 LIO 中，每次迭代都要重新找激光点的匹配对象，导致观测数据的数量会变（不像普通 EKF 是固定的）；观测数据太多时，得把 EKF 的公式变形，才能处理数据量太大的问题。
当我们遇到实际环境问题时如在弯曲的状态空间里，准确地一点点调准机器人的位置和朝向。下面是例子以及解决方案。
但这里有个麻烦：机器人的 "朝向（旋转）" 不是 "平坦的平面"（比如朝东→朝东南不是简单的加法），而是 "弯曲的空间"（像地球表面，走直线其实是曲线）。所以 "状态的误差范围" 不能直接算，得做特殊处理 ------ 这就是下面要解决的核心问题。（好难看不懂）
从初始状态开始调 ，在计算过程中，首先是需要进行微调，即：从初始状态开始调，从出发不断迭代修正量
，更新
直至收敛。
迭代更新，累加修正量：
**计算误差范围：**由于我们这里是要对弯曲空间进行计算，需要将弯曲空间转为平面近似。
计算修正量，让预测匹配观测：
示意图的作用：
8.2.3高维观测的等效处理
上面就是利用公式进行降维，降低计算量。
NDT和卡尔曼滤波器的联系：
考虑一组点云的NDT配准。设点云一共有N个点，当考虑第j个点的配准情况时，它应该
落在被配准的目标点云中的一个体素内，并产生一个与NDT相关的残差
主要是在于将点云的残差带入到卡尔曼增益公式的误差部分，以及协方差更新中，构成观测方程。
8.3实现基于IEKF的LIO系统
基于IEKF的LIO系统的整个实现流程：
1.传感器数据输入和预处理：输入IMU原始数据(角速度、加速度)以及激光雷达原始点云，由于IMU和激光雷达的频率不一样，需要通过硬件触发 / 软件插值，将 IMU 数据对齐到激光点云的每个采样时刻，之后进行IMU校准，比如去零漂、补偿重力得到真实角速度，最后进行点云去畸变，利用 IMU 的高频姿态信息，修正激光扫描过程中因机器人运动导致的点云变形，得到 "帧起始时刻静止坐标系下的无畸变点云"。
2.IMU预积分计算：相邻激光帧（）间的校准后IMU数据，对其进行预积分，计算相对运动增量。
3.IEKF预测步（IMU驱动状态预测）
4.激光观测（生成观测残差）
输入：无畸变激光点云 + 局部地图（由历史帧点云拼接而成，或者上一帧点云）
其中第一步的先验位姿是：用 IMU 预测的位姿（先验状态），把激光自己坐标系的点，换算成全局世界系的坐标。
5.IEKF迭代更新步（消除IMU漂移）
6.最优状态与局部地图更新
上述步骤会随每帧激光数据重复执行，形成"预处理→预积分→预测→观测→迭代修正→地图更新"的循环，最终实现"IMU高频跟踪+激光高精度修正"的紧耦合LIO位姿与地图估计。
代码实现：
我们希望从NDT中直接获取雅可比矩阵和信息矩阵，所以在IEKF实
NDT观测函数：
cpp
/**
* NDT观测函数，输入一个SE3 Pose, 返回本书(8.10)中的几个项
* HT V^{-1} H
* H^T V{-1} r
* 二者都可以用求和的形式来做
*/
using CustomObsFunc = std::function<void(const SE3& input_pose, Eigen::Matrix<S, 18, 18>& HT_Vinv_H,
Eigen::Matrix<S, 18, 1>& HT_Vinv_r)>;
/// 使用自定义观测函数更新滤波器
bool UpdateUsingCustomObserve(CustomObsFunc obs);
/// accessors
/// 全量状态
NavStateT GetNominalState() const { return NavStateT(current_time_, R_, p_, v_, bg_, ba_); }
/// SE3 状态
SE3 GetNominalSE3() const { return SE3(R_, p_); }
void SetX(const NavStated& x) {
current_time_ = x.timestamp_;
R_ = x.R_;
p_ = x.p_;
v_ = x.v_;
bg_ = x.bg_;
ba_ = x.ba_;
}
void SetCov(const Mat18T& cov) { cov_ = cov; }
Vec3d GetGravity() const { return g_; }
private:
void BuildNoise(const Options& options) {
double ev = options.acce_var_;
double et = options.gyro_var_;
double eg = options.bias_gyro_var_;
double ea = options.bias_acce_var_;
double ev2 = ev; // * ev;
double et2 = et; // * et;
double eg2 = eg; // * eg;
double ea2 = ea; // * ea;
// set Q
Q_.diagonal() << 0, 0, 0, ev2, ev2, ev2, et2, et2, et2, eg2, eg2, eg2, ea2, ea2, ea2, 0, 0, 0;
double gp2 = options.gnss_pos_noise_ * options.gnss_pos_noise_;
double gh2 = options.gnss_height_noise_ * options.gnss_height_noise_;
double ga2 = options.gnss_ang_noise_ * options.gnss_ang_noise_;
gnss_noise_.diagonal() << gp2, gp2, gh2, ga2, ga2, ga2;
}
/// 更新名义状态变量
void Update() {
p_ += dx_.template block<3, 1>(0, 0);
v_ += dx_.template block<3, 1>(3, 0);
R_ = R_ * SO3::exp(dx_.template block<3, 1>(6, 0));
if (options_.update_bias_gyro_) {
bg_ += dx_.template block<3, 1>(9, 0);
}
if (options_.update_bias_acce_) {
ba_ += dx_.template block<3, 1>(12, 0);
}
g_ += dx_.template block<3, 1>(15, 0);
}
double current_time_ = 0.0;
// nominal state
SO3 R_;
VecT p_ = VecT::Zero();
VecT v_ = VecT::Zero();
VecT bg_ = VecT::Zero();
VecT ba_ = VecT::Zero();
VecT g_{0, 0, -9.8};
// error state
Vec18T dx_ = Vec18T::Zero();
// covariance
Mat18T cov_ = Mat18T::Identity();
// noise
MotionNoiseT Q_ = MotionNoiseT::Zero();
GnssNoiseT gnss_noise_ = GnssNoiseT::Zero();
Options options_;
};
using IESKFD = IESKF<double>;
using IESKFF = IESKF<float>;
template <typename S>
bool IESKF<S>::Predict(const IMU& imu) {
/// Predict 部分与ESKF完全一样，不再解释
assert(imu.timestamp_ >= current_time_);
double dt = imu.timestamp_ - current_time_;
if (dt > (5 * options_.imu_dt_) || dt < 0) {
LOG(INFO) << "skip this imu because dt_ = " << dt;
current_time_ = imu.timestamp_;
return false;
}
VecT new_p = p_ + v_ * dt + 0.5 * (R_ * (imu.acce_ - ba_)) * dt * dt + 0.5 * g_ * dt * dt;
VecT new_v = v_ + R_ * (imu.acce_ - ba_) * dt + g_ * dt;
SO3 new_R = R_ * SO3::exp((imu.gyro_ - bg_) * dt);
R_ = new_R;
v_ = new_v;
p_ = new_p;
Mat18T F = Mat18T::Identity();
F.template block<3, 3>(0, 3) = Mat3T::Identity() * dt;
F.template block<3, 3>(3, 6) = -R_.matrix() * SO3::hat(imu.acce_ - ba_) * dt;
F.template block<3, 3>(3, 12) = -R_.matrix() * dt;
F.template block<3, 3>(3, 15) = Mat3T::Identity() * dt;
F.template block<3, 3>(6, 6) = SO3::exp(-(imu.gyro_ - bg_) * dt).matrix();
F.template block<3, 3>(6, 9) = -Mat3T::Identity() * dt;
cov_ = F * cov_ * F.transpose() + Q_;
current_time_ = imu.timestamp_;
return true;
}
template <typename S>
bool IESKF<S>::UpdateUsingCustomObserve(IESKF::CustomObsFunc obs) {
// H阵由用户给定
SO3 start_R = R_;
Eigen::Matrix<S, 18, 1> HTVr;
Eigen::Matrix<S, 18, 18> HTVH;
Eigen::Matrix<S, 18, Eigen::Dynamic> K;
Mat18T Pk, Qk;
for (int iter = 0; iter < options_.num_iterations_; ++iter) {
// 调用obs function
obs(GetNominalSE3(), HTVH, HTVr);
// 投影P
Mat18T J = Mat18T::Identity();
J.template block<3, 3>(6, 6) = Mat3T::Identity() - 0.5 * SO3::hat((R_.inverse() * start_R).log());
Pk = J * cov_ * J.transpose();
// 卡尔曼更新
Qk = (Pk.inverse() + HTVH).inverse(); // 这个记作中间变量，最后更新时可以用
dx_ = Qk * HTVr;
// LOG(INFO) << "iter " << iter << " dx = " << dx_.transpose() << ", dxn: " << dx_.norm();
// dx合入名义变量
Update();
if (dx_.norm() < options_.quit_eps_) {
break;
}
}
// update P
cov_ = (Mat18T::Identity() - Qk * HTVH) * Pk;
// project P
Mat18T J = Mat18T::Identity();
Vec3d dtheta = (R_.inverse() * start_R).log();
J.template block<3, 3>(6, 6) = Mat3T::Identity() - 0.5 * SO3::hat(dtheta);
cov_ = J * cov_ * J.inverse();
dx_.setZero();
return true;
}
cpp
void IncNdt3d::ComputeResidualAndJacobians(const SE3& input_pose, Mat18d& HTVH, Vec18d& HTVr) {
assert(grids_.empty() == false);
SE3 pose = input_pose;
// 大部分流程和前面的Align是一样的，只是会把z, H, R三者抛出去而非自己处理
int num_residual_per_point = 1;
if (options_.nearby_type_ == NearbyType::NEARBY6) {
num_residual_per_point = 7;
}
std::vector<int> index(source_->points.size());
for (int i = 0; i < index.size(); ++i) {
index[i] = i;
}
int total_size = index.size() * num_residual_per_point;
std::vector<bool> effect_pts(total_size, false);
std::vector<Eigen::Matrix<double, 3, 18>> jacobians(total_size);
std::vector<Vec3d> errors(total_size);
std::vector<Mat3d> infos(total_size);
// gauss-newton 迭代
// 最近邻，可以并发
std::for_each(std::execution::par_unseq, index.begin(), index.end(), [&](int idx) {
auto q = ToVec3d(source_->points[idx]);
Vec3d qs = pose * q; // 转换之后的q
// 计算qs所在的栅格以及它的最近邻栅格
Vec3i key = CastToInt(Vec3d(qs * options_.inv_voxel_size_));
for (int i = 0; i < nearby_grids_.size(); ++i) {
Vec3i real_key = key + nearby_grids_[i];
auto it = grids_.find(real_key);
int real_idx = idx * num_residual_per_point + i;
/// 这里要检查高斯分布是否已经估计
if (it != grids_.end() && it->second->second.ndt_estimated_) {
auto& v = it->second->second; // voxel
Vec3d e = qs - v.mu_;
// check chi2 th
double res = e.transpose() * v.info_ * e;
if (std::isnan(res) || res > options_.res_outlier_th_) {
effect_pts[real_idx] = false;
continue;
}
// build residual
Eigen::Matrix<double, 3, 18> J;
J.setZero();
J.block<3, 3>(0, 0) = Mat3d::Identity(); // 对p
J.block<3, 3>(0, 6) = -pose.so3().matrix() * SO3::hat(q); // 对R
jacobians[real_idx] = J;
errors[real_idx] = e;
infos[real_idx] = v.info_;
effect_pts[real_idx] = true;
} else {
effect_pts[real_idx] = false;
}
}
});
// 累加Hessian和error,计算dx
double total_res = 0;
int effective_num = 0;
HTVH.setZero();
HTVr.setZero();
const double info_ratio = 0.01; // 每个点反馈的info因子
for (int idx = 0; idx < effect_pts.size(); ++idx) {
if (!effect_pts[idx]) {
continue;
}
total_res += errors[idx].transpose() * infos[idx] * errors[idx];
effective_num++;
HTVH += jacobians[idx].transpose() * infos[idx] * jacobians[idx] * info_ratio;
HTVr += -jacobians[idx].transpose() * infos[idx] * errors[idx] * info_ratio;
}
LOG(INFO) << "effective: " << effective_num;
}
cpp
// 后续的scan，使用NDT配合pose进行更新
LOG(INFO) << "=== frame " << frame_num_;
ndt_.SetSource(current_scan_filter);
ieskf_.UpdateUsingCustomObserve([this](const SE3 &input_pose, Mat18d &HTVH, Vec18d &HTVr) {
ndt_.ComputeResidualAndJacobians(input_pose, HTVH, HTVr);
});
auto current_nav_state = ieskf_.GetNominalState();
// 若运动了一定范围，则把点云放入地图中
SE3 current_pose = ieskf_.GetNominalSE3();
SE3 delta_pose = last_pose_.inverse() * current_pose;
if (delta_pose.translation().norm() > 1.0 || delta_pose.so3().log().norm() > math::deg2rad(10.0)) {
// 将地图合入NDT中
CloudPtr current_scan_world(new PointCloudType);
pcl::transformPointCloud(*current_scan_filter, *current_scan_world, current_pose.matrix());
ndt_.AddCloud(current_scan_world);
last_pose_ = current_pose;
}IEKF会为NDT提供当前的状态估计值，然后NDT会计算前文所述的两个矩阵，返回给IEKF，最后IEKF再去更新自己内部的状态估计。这样就得到了本次激光扫描对应的位姿。另外，如果车辆运动超过1米或角度超过10°，就把当前的激光扫描数据合入NDT地图中，这样可以防止车辆在静止时一直花时间更新地图。
ps：感觉这个很难，需要把前面的知识串起来，还有好多数学运算，好难好难，幸好有豆包帮我，现在理解了。，我真的哭死。