以下所有內(nèi)容均為高翔大神所注的《自動(dòng)駕駛與機(jī)器人中的SLAM技術(shù)》中的內(nèi)容
2D SLAM
- 作者實(shí)現(xiàn)了一個(gè)2D 的ICP，包含了點(diǎn)到線的處理方式
- 實(shí)現(xiàn)了一個(gè)似然場(chǎng)法的配準(zhǔn)，介紹了相關(guān)公式，使用了高斯牛頓法和g2o進(jìn)行求解，其中g(shù)2o中有對(duì)核函數(shù)的使用
3D SLAM
ICP
- 實(shí)現(xiàn)了一個(gè)并發(fā)的ICP配準(zhǔn)
- 實(shí)現(xiàn)了點(diǎn)到面的ICP
- 實(shí)現(xiàn)了點(diǎn)到線的ICP
- 點(diǎn)到線的ICP的結(jié)果與點(diǎn)到點(diǎn)的ICP相當(dāng)，略差于點(diǎn)到面的、在三中算法中，點(diǎn)到面的ICP在精度和效率上都具有一定優(yōu)勢(shì)，明顯快于PCL的內(nèi)置版本，單其單次迭代中計(jì)算量明顯大于原始ICP
NDT
本書(shū)各配準(zhǔn)算法與PCL的對(duì)比
增量式NDT
需要解決兩個(gè)問(wèn)題：
- 每個(gè)體素內(nèi)的高斯參數(shù)如何改變
- 如何維護(hù)不斷增加的體素
高斯分布的增量更新
體素的增量維護(hù)
融合導(dǎo)航
1. EKF和優(yōu)化的關(guān)系
2. 組合導(dǎo)航eskf中的預(yù)測(cè)部分，主要是F矩陣的構(gòu)建
template <typename S>
bool ESKF<S>::Predict(const IMU& imu) {
assert(imu.timestamp_ >= current_time_);
double dt = imu.timestamp_ - current_time_;
if (dt > (5 * options_.imu_dt_) || dt < 0) {
// 時(shí)間間隔不對(duì)，可能是第一個(gè)IMU數(shù)據(jù)，沒(méi)有歷史信息
LOG(INFO) << "skip this imu because dt_ = " << dt;
current_time_ = imu.timestamp_;
return false;
}
// nominal state 遞推
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;
// 其余狀態(tài)維度不變
// error state 遞推
// 計(jì)算運(yùn)動(dòng)過(guò)程雅可比矩陣 F，見(jiàn)(3.47)
// F實(shí)際上是稀疏矩陣，也可以不用矩陣形式進(jìn)行相乘而是寫(xiě)成散裝形式，這里為了教學(xué)方便，使用矩陣形式
Mat18T F = Mat18T::Identity(); // 主對(duì)角線
F.template block<3, 3>(0, 3) = Mat3T::Identity() * dt; // p 對(duì) v
F.template block<3, 3>(3, 6) = -R_.matrix() * SO3::hat(imu.acce_ - ba_) * dt; // v對(duì)theta
F.template block<3, 3>(3, 12) = -R_.matrix() * dt; // v 對(duì) ba
F.template block<3, 3>(3, 15) = Mat3T::Identity() * dt; // v 對(duì) g
F.template block<3, 3>(6, 6) = SO3::exp(-(imu.gyro_ - bg_) * dt).matrix(); // theta 對(duì) theta
F.template block<3, 3>(6, 9) = -Mat3T::Identity() * dt; // theta 對(duì) bg
// mean and cov prediction
dx_ = F * dx_; // 這行其實(shí)沒(méi)必要算，dx_在重置之后應(yīng)該為零，因此這步可以跳過(guò)，但F需要參與Cov部分計(jì)算，所以保留
cov_ = F * cov_.eval() * F.transpose() + Q_;
current_time_ = imu.timestamp_;
return true;
}3. 以下是速度量測(cè)，主要是H矩陣的構(gòu)建
template <typename S>
bool ESKF<S>::ObserveWheelSpeed(const Odom& odom) {
assert(odom.timestamp_ >= current_time_);
// odom 修正以及雅可比
// 使用三維的輪速觀測(cè)，H為3x18，大部分為零
Eigen::Matrix<S, 3, 18> H = Eigen::Matrix<S, 3, 18>::Zero();
H.template block<3, 3>(0, 3) = Mat3T::Identity();
// 卡爾曼增益
Eigen::Matrix<S, 18, 3> K = cov_ * H.transpose() * (H * cov_ * H.transpose() + odom_noise_).inverse();
// velocity obs
double velo_l = options_.wheel_radius_ * odom.left_pulse_ / options_.circle_pulse_ * 2 * M_PI / options_.odom_span_;
double velo_r = options_.wheel_radius_ * odom.right_pulse_ / options_.circle_pulse_ * 2 * M_PI / options_.odom_span_;
double average_vel = 0.5 * (velo_l + velo_r);
VecT vel_odom(average_vel, 0.0, 0.0);
VecT vel_world = R_ * vel_odom;
dx_ = K * (vel_world - v_);//v_是狀態(tài)遞推后的速度
// update cov
cov_ = (Mat18T::Identity() - K * H) * cov_;
UpdateAndReset();
return true;
}4. 以下是GPS的量測(cè)，主要任然是H矩陣的構(gòu)建文章來(lái)源：http://www.zghlxwxcb.cn/news/detail-787804.html
template <typename S>
bool ESKF<S>::ObserveGps(const GNSS& gnss) {
/// GNSS 觀測(cè)的修正
assert(gnss.unix_time_ >= current_time_);
if (first_gnss_) {
R_ = gnss.utm_pose_.so3();
p_ = gnss.utm_pose_.translation();
first_gnss_ = false;
current_time_ = gnss.unix_time_;
return true;
}
assert(gnss.heading_valid_);
ObserveSE3(gnss.utm_pose_, options_.gnss_pos_noise_, options_.gnss_ang_noise_);
current_time_ = gnss.unix_time_;
return true;
}
template <typename S>
bool ESKF<S>::ObserveSE3(const SE3& pose, double trans_noise, double ang_noise) {
/// 既有旋轉(zhuǎn)，也有平移
/// 觀測(cè)狀態(tài)變量中的p, R，H為6x18，其余為零
Eigen::Matrix<S, 6, 18> H = Eigen::Matrix<S, 6, 18>::Zero();
H.template block<3, 3>(0, 0) = Mat3T::Identity(); // P部分
H.template block<3, 3>(3, 6) = Mat3T::Identity(); // R部分（3.66)
// 卡爾曼增益和更新過(guò)程
Vec6d noise_vec;
noise_vec << trans_noise, trans_noise, trans_noise, ang_noise, ang_noise, ang_noise;
Mat6d V = noise_vec.asDiagonal();
Eigen::Matrix<S, 18, 6> K = cov_ * H.transpose() * (H * cov_ * H.transpose() + V).inverse();
// 更新x和cov
Vec6d innov = Vec6d::Zero();
innov.template head<3>() = (pose.translation() - p_); // 平移部分
innov.template tail<3>() = (R_.inverse() * pose.so3()).log(); // 旋轉(zhuǎn)部分(3.67)
dx_ = K * innov;
cov_ = (Mat18T::Identity() - K * H) * cov_;
UpdateAndReset();
return true;
}IMU預(yù)積分
- 書(shū)中有IMU預(yù)積分所有的公式推導(dǎo)，包括了預(yù)積分計(jì)算公式，預(yù)積分相較于狀態(tài)量的雅克比矩陣公式，誤差傳播公式等等
文章來(lái)源地址http://www.zghlxwxcb.cn/news/detail-787804.html
/**
* IMU 預(yù)積分器
*
* 調(diào)用Integrate來(lái)插入新的IMU讀數(shù)，然后通過(guò)Get函數(shù)得到預(yù)積分的值
* 雅可比也可以通過(guò)本類(lèi)獲得，可用于構(gòu)建g2o的邊類(lèi)
*/
class IMUPreintegration {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/// 參數(shù)配置項(xiàng)
/// 初始的零偏需要設(shè)置，其他可以不改
struct Options {
Options() {}
Vec3d init_bg_ = Vec3d::Zero(); // 初始零偏
Vec3d init_ba_ = Vec3d::Zero(); // 初始零偏
double noise_gyro_ = 1e-2; // 陀螺噪聲，標(biāo)準(zhǔn)差
double noise_acce_ = 1e-1; // 加計(jì)噪聲，標(biāo)準(zhǔn)差
};
IMUPreintegration(Options options = Options());
/**
* 插入新的IMU數(shù)據(jù)
* @param imu imu 讀數(shù)
* @param dt 時(shí)間差
*/
void Integrate(const IMU &imu, double dt);
/**
* 從某個(gè)起始點(diǎn)開(kāi)始預(yù)測(cè)積分之后的狀態(tài)
* @param start 起始時(shí)時(shí)刻狀態(tài)
* @return 預(yù)測(cè)的狀態(tài)
*/
NavStated Predict(const NavStated &start, const Vec3d &grav = Vec3d(0, 0, -9.81)) const;
/// 獲取修正之后的觀測(cè)量，bias可以與預(yù)積分時(shí)期的不同，會(huì)有一階修正
SO3 GetDeltaRotation(const Vec3d &bg);
Vec3d GetDeltaVelocity(const Vec3d &bg, const Vec3d &ba);
Vec3d GetDeltaPosition(const Vec3d &bg, const Vec3d &ba);
public:
double dt_ = 0; // 整體預(yù)積分時(shí)間
Mat9d cov_ = Mat9d::Zero(); // 累計(jì)噪聲矩陣
Mat6d noise_gyro_acce_ = Mat6d::Zero(); // 測(cè)量噪聲矩陣
// 零偏
Vec3d bg_ = Vec3d::Zero();
Vec3d ba_ = Vec3d::Zero();
// 預(yù)積分觀測(cè)量
SO3 dR_;
Vec3d dv_ = Vec3d::Zero();
Vec3d dp_ = Vec3d::Zero();
// 雅可比矩陣
Mat3d dR_dbg_ = Mat3d::Zero();
Mat3d dV_dbg_ = Mat3d::Zero();
Mat3d dV_dba_ = Mat3d::Zero();
Mat3d dP_dbg_ = Mat3d::Zero();
Mat3d dP_dba_ = Mat3d::Zero();
};到了這里，關(guān)于學(xué)習(xí)記錄-自動(dòng)駕駛與機(jī)器人中的SLAM技術(shù)的文章就介紹完了。如果您還想了解更多內(nèi)容，請(qǐng)?jiān)谟疑辖撬阉鱐OY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！
 KUKA 的仿真工具](https://imgs.yssmx.com/Uploads/2024/04/860059-1.jpeg)
