多传感器融合定位 第八章 基于滤波的融合方法进阶
参考博客:深蓝学院-多传感器融合定位-第8章作业
代码下载:/kahowang/sensor-fusion-for-localization-and-mapping/tree/main/%E7%AC%AC%E5%85%AB%E7%AB%A0%20%E5%9F%BA%E4%BA%8E%E6%BB%A4%E6%B3%A2%E7%9A%84%E8%9E%8D%E5%90%88%E6%96%B9%E6%B3%952
1.环境配置
1.1 protoc 版本问题
前几章使用的protoc 的版本为3.14, 第七章使用的proto版本为3.15
解决方法:需要安装新版本的proto 3.15x,按照第四章的方式生成对应的文件。
按照GeYao README中的方法,重新生成基于自己基环境protobuf的proto:
打开lidar_localization/config/scan_context文件夹,输入如下命令,生成pb文件
protoc --cpp_out=./ key_frames.protoprotoc --cpp_out=./ ring_keys.protoprotoc --cpp_out=./ scan_contexts.protomv key_frames.pb.cppmv ring_keys.pb.cppmv scan_contexts.pb.cpp
分别修改生成的三个.pb.cpp文件。如下,以ring_keys.pb.cpp为例。
// Generated by the protocol buffer compiler. DO NOT EDIT!// source: ring_keys.proto#define INTERNAL_SUPPRESS_PROTOBUF_FIELD_DEPRECATION#include "ring_keys.pb.h" 替换为 #include "lidar_localization/models/scan_context_manager/ring_keys.pb.h"#include <algorithm>
之后,用以上步骤生成的的.pb.h文件替换lidar_localization/include/lidar_localization/models/scan_context_manager
中的同名文件。
将.pb.cpp文件替换(注意:需要剪切,确保config文件中新生成的文件都转移到对应目录下,不能重复出现)lidar_localization/src/models/scan_context_manager中的同名文件。
1.2 缺少 fmt 库
git clone /fmtlib/fmtcd fmt/mkdir buildcd buildcmake ..makesudo make install
编译过程中出现:error_state_kalman_filter.cpp:(.text.unlikely+0x84):对‘fmt::v8::detail::assert_fail(char const, int, char const)’未定义的引用**
参考网址: undefined reference to `vtable for fmt::v7::format_error‘
cd catkin_ws/src/lidar_localization/cmake/sophus.cmake
list append 添加 fmt
# sophus.cmakefind_package (Sophus REQUIRED)include_directories(${Sophus_INCLUDE_DIRS})list(APPEND ALL_TARGET_LIBRARIES ${Sophus_LIBRARIES} fmt)
1.3 glog缺少gflag的依赖
:(.text+0x6961):对‘google::FlagRegisterer::FlagRegisterer(char const*, char const*, char const*, bool*, bool*)’未定义的引用
#解决办法: 打开glog.cmake , 末尾改为list(APPEND ALL_TARGET_LIBRARIES ${GLOG_LIBRARIES} libgflags.a libglog.a)
2.增加运模型的 error state kalmam filter
FILE: lidar_localization/src/model/kalman_filter/error_state_kalman_filter.cpp
2.1 融合编码器模型
编码器可以额外提供一个轮速里程计,在不是很高频的编码器情况下,可以做为一个约束边,通过提供线速度(b系) 到上一章节的观测方程中,进行kalman 融合。
注意:1. 编码器提供的线速度是比较准确的,但是角速度不太准确(转弯存在打滑现象),角速度不宜用作观测边。2.编码器参与的融合,还有另外一种融合方式,即编码器不当做观测使用,而是和IMU一起进行状态预测,然后再与其他传感器提供的观测进行滤波融合。具体思路为IMU提供角速度,编码器提供线速度,假设二者频率相同、时间戳已对齐,且外参已标定,那么它们可以直接认为是一个可以通过解算得到姿态、位置的新传感器。
2.2 添加运动约束模型(伪观测)
车子坐标系(前左上),在没有编码器硬件的基础上,可以使用四轮底盘本身的运动属性进行约束,正常情况下,车子只有前向(x)的速度,观测上,y 和 z 向的观测都为0。所以可在观测中添加 Vy 、 Vz 两个约束边。
2.3 代码编写
FILE:catkin_ws/src/lidar_localization/src/models/kalman_filter/error_state_kalman_filter.cpp
2.3.1 调用 CorrectErrorEstimation
case MeasurementType::POSE_VEL:CorrectErrorEstimationPoseVel(measurement.T_nb, measurement.v_b, measurement.w_b,Y, G, K);break;
2.3.2 CorrectErrorEstimationPoseVel 计算 Y ,G ,K
注意:
CorrectErrorEstimationPoseVel 函数中输入的v_b 取自 惯导,因为KITTI数据集并没有轮速里程计信息,所以,如果在做融合运动约 束模型时,输入的measurment velocity 为v_b ,可以认为观测是取自ins的。
用车子的运动约束(vy = 0;vz = 0)做伪观测约束, 如下所示:
// Eigen::Vector3d v_b_ = {v_b[0], 0, 0}; // measurment velocity (body 系) , 伪观测 (vy 、vz = 0)Eigen::Vector3d v_b_ = v_b; // measurment velocity (body 系) , 融入速度 (vx 取自 惯导)
void ErrorStateKalmanFilter::CorrectErrorEstimationPoseVel(// 计算 Y ,G ,Kconst Eigen::Matrix4d &T_nb, const Eigen::Vector3d &v_b, const Eigen::Vector3d &w_b,Eigen::VectorXd &Y, Eigen::MatrixXd &G, Eigen::MatrixXd &K) {//// TODO: set measurement:计算观测 delta pos 、 delta ori//// Eigen::Vector3d v_b_ = {v_b[0], 0, 0}; // measurment velocity (body 系) , 伪观测 (vy 、vz = 0)Eigen::Vector3d v_b_ = v_b; // measurment velocity (body 系) , 融入速度 (vx 取自 惯导)Eigen::Vector3d dp = pose_.block<3, 1>(0, 3) - T_nb.block<3, 1>(0, 3);Eigen::Matrix3d dR = T_nb.block<3, 3>(0, 0).transpose() * pose_.block<3, 3>(0, 0) ;Eigen::Vector3d dv = T_nb.block<3, 3>(0, 0).transpose() *vel_ - v_b_ ; // delta v 严格意义上来说,这里的观测是,惯导给的vx// TODO: set measurement equation:Eigen::Vector3d dtheta = Sophus::SO3d::vee(dR - Eigen::Matrix3d::Identity() );YPoseVel_.block<3, 1>(0, 0) = dp;// delta position YPoseVel_.block<3, 1>(3, 0) = dv; // delta velocity sYPoseVel_.block<3, 1>(6, 0) = dtheta;// 失准角Y = YPoseVel_;// set measurement GGPoseVel_.setZero();GPoseVel_.block<3, 3>(0, kIndexErrorPos) = Eigen::Matrix3d::Identity();GPoseVel_.block<3, 3>(3, kIndexErrorVel) = T_nb.block<3, 3>(0, 0).transpose();GPoseVel_.block<3, 3>(3, kIndexErrorOri) = Sophus::SO3d::hat( T_nb.block<3, 3>(0, 0).transpose() *vel_ ) ;GPoseVel_.block<3 ,3>(6, kIndexErrorOri) = Eigen::Matrix3d::Identity(); G = GPoseVel_;// set measurement CCPoseVel_.setIdentity();Eigen::MatrixXd C = CPoseVel_;// TODO: set Kalman gain:Eigen::MatrixXd R = RPoseVel_; // 观测噪声K = P_ * G.transpose() * ( G * P_ * G.transpose( ) + C * RPoseVel_* C.transpose() ).inverse() ;}
2.3.2 将更新后的vel_b 写到txt 中,进行比较
/*********************write data to txt********************/#include <list>#include <sstream>#include <fstream>#include <iomanip>#define DEBUG_PRINT std::ofstream fused;std::ofstream fused_vel;std::ofstream fused_cons;bool CreateFile(std::ofstream& ofs, std::string file_path) {ofs.open(file_path, std::ios::out);// 使用std::ios::out 可实现覆盖if(!ofs){std::cout << "open csv file error " << std::endl;return false;}return true;}/* write2txt */void WriteText(std::ofstream& ofs, Eigen::Vector3d data){ofs << std::fixed << data[0] << "\t" << data[1] << "\t " << data[2] << "\t" << std::endl;}
调用
/*init debug print file */#ifdef DEBUG_PRINTchar fused_path[] = "/home/kaho/shenlan_ws/src/lidar_localization/slam_data/trajectory/fused.txt";char fused_vel_path[] = "/home/kaho/shenlan_ws/src/lidar_localization/slam_data/trajectory/fused_vel.txt";char fused_cons_path[] = "/home/kaho/shenlan_ws/src/lidar_localization/slam_data/trajectory/fused_cons.txt";CreateFile(fused, fused_path );#endif ------------------------------------------------------------------------------------/*print vel(body)*/#ifdef DEBUG_PRINT Eigen::Vector3d vel_print_ = pose_.block<3, 3>(0, 0).transpose() * vel_; // convert kalman filter velocity to body axisWriteText(fused, vel_print_ ); // 写进文件夹#endif
2.4 EVO 评估 及 update后的v_b 比较
evo评估指令
# set up session:source devel/setup.bash# save odometry:rosservice call /save_odometry "{}"# run evo evaluation:# a. fused 没有输入运动模型 输出评估结果,并以zip的格式存储:evo_ape kitti ground_truth.txt fused.txt -r full --plot --plot_mode xy --save_results ./fused.zip# b. fused_vel 速度观测 输出评估结果,并以zip的格式存储:evo_ape kitti ground_truth.txt fused.txt -r full --plot --plot_mode xy --save_results ./fused_vel.zip# c. fused_cons 运动约束伪观测 输出评估结果,并以zip的格式存储:evo_ape kitti ground_truth.txt fused.txt -r full --plot --plot_mode xy --save_results ./fused_cons.zip#e. 比较 laser fused 一并比较评估evo_res *.zip --use_filenames -p
2.4.1 ape 比较
fused 为无运动约束, fused_vel 为加入惯导速度观测,fused_cons为加入运动约束(伪观测)
通过观察ape曲线,觉得并无太大差别
2.4.2 使用运动模型后 vel_y vel_z (body系) 比较
fused 为无运动约束, fused_vel 为加入惯导速度观测,fused_cons为加入运动约束(伪观测)
2.4.1 matplotlib 可视化数据
为了方便可视化v_x、v_z ,这里使用matplotlib 可视化数据
FILE: /home/kaho/shenlan_ws/visual/ main.py
# import necessary modulefrom mpl_toolkits.mplot3d import axes3dimport matplotlib.pyplot as pltimport numpy as npfused = np.loadtxt("fused_withoust_cons.txt")fused_vel_data = np.loadtxt("fused_vel.txt")fused_cons_data = np.loadtxt("fused_cons.txt")# x = fused_vel_data[:,0]y_fused = fused[:,1]z_fused = fused[:,2]y_fused_vel = fused_vel_data[:,1]z_fused_vel = fused_vel_data[:,2]y_fused_cons_data = fused_cons_data[:,1]z_fused_cons_data = fused_cons_data[:,2]plt.plot(y_fused, c='r', label ="y_fused")plt.plot(y_fused_vel, c='g', label ="Y_fused_vel")# plt.plot(y_fused_cons_data, c='b', label ="y_fused_cons")plt.legend();plt.title('y velocity ',fontsize=18,color='y')plt.show()plt.plot(z_fused, c='r', label ="z_fused")# plt.plot(z_fused_vel, c='g', label ="z_fused_vel")plt.plot(y_fused_cons_data, c='b', label ="y_fused_cons")plt.legend();plt.title('z velocity ',fontsize=18,color='y')plt.show()
2.4.2 vel_y 比较
红色为无运动约束,绿色为fused 加入速度观测,蓝色为fused_cons为加入运动约束,可明显看出蓝色曲线大部分时间处于红色曲线之下,说明使用运动约束后,数据的波动性更小。
2.4.3 vel_z 比较
红色为无运动约束,绿色为fused 加入速度观测,蓝色为fused_cons为加入运动约束,可明显看出蓝色曲线大部分时间处于红色曲线之下,说明使用运动约束后,数据的波动性更小。
2.4.4 结论
实验结果大致和理论的一致,添加了运动模型后,整体效果有了轻微的改善,但是速度的波动得到了大幅度的抑制!!!
3. GNSS + Oodm 观测模型
参照 (Estimate: imu, Measurment: lidar、odom ) 模型,推导出(Estimate: imu, Measurment: gnss、odom ),观测方程中,gnss 提供position、odom
3.1 公式推导
状态量
δx=[δpδvδθδbaδbω]\delta \boldsymbol{x}=\left[\begin{array}{l} \delta \boldsymbol{p} \\ \delta \boldsymbol{v} \\ \delta \boldsymbol{\theta} \\ \delta \boldsymbol{b}_{a} \\ \delta \boldsymbol{b}_{\omega} \end{array}\right] δx=⎣⎢⎢⎢⎢⎡δpδvδθδbaδbω⎦⎥⎥⎥⎥⎤
观测量 GPS + Encoder 做观测, GPS 提供position, Encoder 提供velocity
y=[δp‾δv‾b]\boldsymbol{y}=\left[\begin{array}{c} \delta \overline{\boldsymbol{p}} \\ \delta \overline{\boldsymbol{v}}^{b} \\ \end{array}\right] y=[δpδvb]
观测值的获取
δv‾b=v~b−vb=R~bwv~w−[vm00]\delta \overline{\boldsymbol{v}}_{b}=\tilde{\boldsymbol{v}}^{b}-\boldsymbol{v}^{b}=\tilde{\boldsymbol{R}}_{b w} \tilde{\boldsymbol{v}}^{w}-\left[\begin{array}{c} \boldsymbol{v}_{m} \\ 0 \\ 0 \end{array}\right] δvb=v~b−vb=R~bwv~w−⎣⎡vm00⎦⎤
观测方程
y=Gtδx+Ctn\boldsymbol{y}=\boldsymbol{G}_{t} \delta \boldsymbol{x}+\boldsymbol{C}_{t} \boldsymbol{n} y=Gtδx+Ctn
Gt=[I300000Rbw[vb]×00]Ct=[I300I3]\begin{aligned} \boldsymbol{G}_{t} &=\left[\begin{array}{ccccc} \boldsymbol{I}_{3} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \boldsymbol{R}_{b w} & {\left[\boldsymbol{v}^{b}\right]_{\times}} & \mathbf{0} & \mathbf{0} \\ \end{array}\right] \\\\\\ \boldsymbol{C}_{t} &=\left[\begin{array}{ccc} \boldsymbol{I}_{3} & \mathbf{0} \\ \mathbf{0} & \boldsymbol{I}_{3} \\ \end{array}\right] \end{aligned} GtCt=[I300Rbw0[vb]×0000]=[I300I3]
n=[nδpˉxnδpˉynδpˉznδvˉxbnδvˉybnδvˉzb]T\boldsymbol{n}=\left[\begin{array}{lllllllll} n_{\delta \bar{p}_{x}} & n_{\delta \bar{p}_{y}} & n_{\delta \bar{p}_{z}} & n_{\delta \bar{v}_{x}^{b}} & n_{\delta \bar{v}_{y}^{b}} & n_{\delta \bar{v}_{z}^{b}} \end{array}\right]^{T} n=[nδpˉxnδpˉynδpˉznδvˉxbnδvˉybnδvˉzb]T
3.2 CorrectErrorEstimationPosiVel , 因为只是少了ori这个观测量,所以在(lidar + encoder)的观测上删掉orii这一行的观测就可以。
void ErrorStateKalmanFilter::CorrectErrorEstimationPosiVel( // position + velocityconst Eigen::Matrix4d &T_nb, const Eigen::Vector3d &v_b, const Eigen::Vector3d &w_b,Eigen::VectorXd &Y, Eigen::MatrixXd &G, Eigen::MatrixXd &K) {//// TODO: set measurement: 计算观测 delta pos 、 delta velocity//Eigen::Vector3d v_b_ = {v_b[0], 0, 0}; // measurment velocity (body 系) , 伪观测 (vy 、vz = 0)Eigen::Vector3d dp = pose_.block<3, 1>(0, 3) - T_nb.block<3, 1>(0, 3);Eigen::Vector3d dv = pose_.block<3, 3>(0, 0).transpose() *vel_ - v_b ; // delta v , v_x 来自轮速里程计// TODO: set measurement equation:YPosiVel_.block<3, 1>(0, 0) = dp;// delta position YPosiVel_.block<3, 1>(3, 0) = dv; // delta velocity Y = YPosiVel_;// set measurement GGPosiVel_.setZero();GPosiVel_.block<3, 3>(0, kIndexErrorPos) = Eigen::Matrix3d::Identity();GPosiVel_.block<3, 3>(3, kIndexErrorVel) = pose_.block<3, 3>(0, 0).transpose();GPosiVel_.block<3, 3>(3, kIndexErrorOri) = Sophus::SO3d::hat( pose_.block<3, 3>(0, 0).transpose() *vel_ ) ;G = GPosiVel_;// set measurement CCPosiVel_.setIdentity();Eigen::MatrixXd C = CPosiVel_;// TODO: set Kalman gain:Eigen::MatrixXd R = RPosiVel_; // 观测噪声K = P_ * G.transpose() * ( G * P_ * G.transpose( ) + C * R* C.transpose() ).inverse() ;}
3.3 参数调整,及重力参数修改
3.3.1 pos vel 观测误差参数调整
FILE: catkin_ws/src/lidar_localization/config/filtering/gnss_ins_sim_filtering.yaml
按照GeYao 助教所说,
最后这里在运行的时候需要提醒一下大家, 左边的两张图是gnss-ins-sim生成测量数据时候的误差等级, 课程中默认使用的都是中等精度, 也就是说gps位置的协方差应该在2.5e-1左右, 而编码器应该在2.5e-3左右, 而右边是我们的作业使用的默认配置文件中的误差等级. 可以看出gps的实际误差与我们给的先验值的差距比较巨大, 这就是为什么有些同学在程序正确的情况下rviz中显示的姿态抖动非常厉害的原因. 建议大家在左边这两个值的基础上进行调参.
所以,我们把 pos 和 vel 的measurment cov 调整为
measurement:pose:pos: 1.0e-4ori: 1.0e-4pos: 2.5e-1 # 1.0-4vel: 2.5e-3
3.3.2 重力参数调整
这里生成的仿真数据的重力加速度和kitti的重力加速度方向不太一致,原因,仿真的数据集中传感器的Z轴正方向是朝上的,所以imu_sim_ins accel_z 读出来的数值是 -g。通过 指令rqt_bag 查看传感器读取的重力加速度,写到yaml中。
FILE: catkin_ws/src/lidar_localization/config/filtering/gnss_ins_sim_filtering.yaml
gravity_magnitude: -9.794216
3.3.3 evo评估
# set up session:source devel/setup.bash# save odometry:rosservice call /save_odometry "{}"# run evo evaluation:# a. fused 没有输入运动模型 输出评估结果,并以zip的格式存储:evo_ape kitti ground_truth.txt fused.txt -r full --plot --plot_mode xyz --save_results ./fused.zip# b. fused_vel 速度观测 输出评估结果,并以zip的格式存储:evo_ape kitti ground_truth.txt gnss.txt -r full --plot --plot_mode xyz --save_results ./gnss.zip#e. 比较 laser fused 一并比较评估evo_res *.zip --use_filenames -p
3.4 结论通过对比上述实验
可得知,融合了odom轮速里程计后,数据波动得到大幅度抑制。
edit by kaho .10.22