多传感器融合定位 第六章 惯性导航结算及误差模型
时间：2022-11-16 18:30:00
多传感器集成定位 第六章 惯性导航结算和误差模型
参考博客：深蓝学院-多传感器集成定位-第六章操作
代码下载：https://github.com/kahowang/sensor-fusion-for-localization-and-mapping/tree/main/第六章 惯性导航解算及误差分析/catkin_ws
1.姿态更新-基于中值法的解算
1.1 基于四元数的姿态更新
获得等效旋转矢量
// get deltas: size_t index_curr_ = 1; size_t index_prev_ =0; Eigen::Vector3d angular_delta = Eigen::Vector3d::Zero(); if(! (GetAngularDelta(index_curr_, index_prev_, angular_delta)) ){
std::cout << "GetAngularDelta(): index error" << std::endl; // 获得等效旋转矢量 } /**********************************************************************************************************************/ bool Activity::GetAngularDelta( const size_t index_curr, const size_t index_prev, Eigen::Vector3d &angular_delta ) {
// // TODO: this could be a helper routine for your own implementation // if ( index_curr <= index_prev || imu_data_buff_.size() <= index_curr ) {
return false; } const IMUData &imu_data_curr = imu_data_buff_.at(index_curr); const IMUData &imu_data_prev = imu_data_uff_.at(index_prev);
double delta_t = imu_data_curr.time - imu_data_prev.time;
Eigen::Vector3d angular_vel_curr = GetUnbiasedAngularVel(imu_data_curr.angular_velocity); // omega_k
Eigen::Vector3d angular_vel_prev = GetUnbiasedAngularVel(imu_data_prev.angular_velocity); // omega_k-1
angular_delta = 0.5*delta_t*(angular_vel_curr + angular_vel_prev); // 中值法计算angular
return true;
}
更新四元数
// update orientation:
Eigen::Matrix3d R_curr_ = Eigen::Matrix3d::Identity();
Eigen::Matrix3d R_prev_ = Eigen::Matrix3d::Identity();
UpdateOrientation(angular_delta,R_curr_, R_prev_); // 更新四元数
/*********************************************************************************************************************/
void Activity::UpdateOrientation(
const Eigen::Vector3d &angular_delta,
Eigen::Matrix3d &R_curr, Eigen::Matrix3d &R_prev
) {
//
// TODO: this could be a helper routine for your own implementation
//
// magnitude:
double angular_delta_mag = angular_delta.norm();
// direction:
Eigen::Vector3d angular_delta_dir = angular_delta.normalized();
// build delta q:
double angular_delta_cos = cos(angular_delta_mag/2.0);
double angular_delta_sin = sin(angular_delta_mag/2.0);
Eigen::Quaterniond dq(
angular_delta_cos,
angular_delta_sin*angular_delta_dir.x(),
angular_delta_sin*angular_delta_dir.y(),
angular_delta_sin*angular_delta_dir.z()
);
Eigen::Quaterniond q(pose_.block<3, 3>(0, 0));
// update:
q = q*dq;
// write back:
R_prev = pose_.block<3, 3>(0, 0);
pose_.block<3, 3>(0, 0) = q.normalized().toRotationMatrix();
R_curr = pose_.block<3, 3>(0, 0);
}
1.2 位置更新
更新速度
// get velocity delta:
double delta_t_;
Eigen::Vector3d velocity_delta_;
if(! (GetVelocityDelta(index_curr_, index_prev_, R_curr_, R_prev_, delta_t_, velocity_delta_)) ){
std::cout << "GetVelocityDelta(): index error" << std::endl; // 获取速度差值
}
/**********************************************************************************************************************/
bool Activity::GetVelocityDelta(
const size_t index_curr, const size_t index_prev,
const Eigen::Matrix3d &R_curr, const Eigen::Matrix3d &R_prev,
double &delta_t, Eigen::Vector3d &velocity_delta
) {
//
// TODO: this could be a helper routine for your own implementation
//
if (
index_curr <= index_prev ||
imu_data_buff_.size() <= index_curr
) {
return false;
}
const IMUData &imu_data_curr = imu_data_buff_.at(index_curr);
const IMUData &imu_data_prev = imu_data_buff_.at(index_prev);
delta_t = imu_data_curr.time - imu_data_prev.time;
Eigen::Vector3d linear_acc_curr = GetUnbiasedLinearAcc(imu_data_curr.linear_acceleration, R_curr);
Eigen::Vector3d linear_acc_prev = GetUnbiasedLinearAcc(imu_data_prev.linear_acceleration, R_prev);
velocity_delta = 0.5*delta_t*(linear_acc_curr + linear_acc_prev);
return true;
}
更新位置
// update position:
UpdatePosition(delta_t_, velocity_delta_);
/****************************************************************************************************/
void Activity::UpdatePosition(const double &delta_t, const Eigen::Vector3d &velocity_delta) {
//
// TODO: this could be a helper routine for your own implementation
//
pose_.block<3, 1>(0, 3) += delta_t*vel_ + 0.5*delta_t*velocity_delta;
vel_ += velocity_delta;
}
1.3 结果
roslaunch imu_integration imu_integration.launch
2.姿态更新-基于欧拉法解算
bool Activity::GetAngularDelta( )
angular_delta = delta_t*angular_vel_prev; // 欧拉法
bool Activity::GetVelocityDelta( )
velocity_delta = delta_t*linear_acc_prev; // 欧拉法
结果：
roslaunch imu_integration imu_integration.launch
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-RYFIS1pJ-1653025778659)(https://kaho-pic-1307106074.cos.ap-guangzhou.myqcloud.com/CSDN_Pictures/深蓝多传感器融合定位/第二章激光里程计1%E7%AC%AC%E4%BA%8C%E7%AB%A0%E6%BF%80%E5%85%89%E9%87%8C%E7%A8%8B%E8%AE%A11euler.png)]
3.gnss-ins-sim仿真数据数据集，对比中值法和欧拉法得解算精度
3.1 gnss-ins-sim 使用
gnss-ins-sim github下载链接
使用示例教程, 基于Python的开源GNSS/INS仿真
使用要点：
1.定义仿真IMU的误差模型
可以选择 ‘low-accuracy’, ‘mid-accuracy’ and ‘high accuracy’ 三种不同精度的IMU模型，或自定义IMU模型
imu = imu_model.IMU(accuracy=imu_err, axis=6, gps=False)
imu = imu_model.IMU(accuracy='low accuracy', axis=9, gps=True)
imu_err = {
# gyro bias, deg/hr
'gyro_b': np.array([0.0, 0.0, 0.0]),
# gyro angle random walk, deg/rt-hr
'gyro_arw': np.array([0.25, 0.25, 0.25]),
# gyro bias instability, deg/hr
'gyro_b_stability': np.array([3.5, 3.5, 3.5]),
# gyro bias instability correlation, sec.
# set this to 'inf' to use a random walk model
# set this to a positive real number to use a first-order Gauss-Markkov model
'gyro_b_corr': np.array([100.0, 100.0, 100.0]),
# accelerometer bias, m/s^2
'accel_b': np.array([0.0e-3, 0.0e-3, 0.0e-3]),
# accelerometer velocity random walk, m/s/rt-hr
'accel_vrw': np.array([0.03119, 0.03009, 0.04779]),
# accelerometer bias instability, m/s^2
'accel_b_stability': np.array([4.29e-5, 5.72e-5, 8.02e-5]),
# accelerometer bias instability correlation, sec. Similar to gyro_b_corr
'accel_b_corr': np.array([200.0, 200.0, 200.0]),
# magnetometer noise std, uT
'mag_std': np.array([0.2, 0.2, 0.2])
}
2.运动定义 command type
通过写入到csv中，进行运动定义，主要使用到两种指令格式， command type 1 和 command type 2
command type 1 定义在command duration 时间内的速率和角速率变化，可用于加速，匀速运动
command type 2 定义在command duration 时间内达到预设的角度(绝对) 和 速度
| Command type | Comment |
|---|---|
| 1 | Directly define the Euler angles change rate and body frame velocity change rate. The change rates are given by column 2~7. The units are deg/s and m/s/s. Column 8 gives how long the command will last. If you want to fully control execution time of each command by your own, you should always choose the motion type to be 1 |
| 2 | Define the absolute attitude and absolute velocity to reach. The target attitude and velocity are given by column 2~7. The units are deg and m/s. Column 8 defines the maximum time to execute the command. If actual executing time is less than max time, the remaining time will not be used and the next command will be executed immediately. If the command cannot be finished within max time, the next command will be executed after max time. |
3.2 prepare sim dataset
3.2.1 生成rosbag仿真数据集
通过参考recorder_node_allan_variance_analysis.py 和 Teamo 助教的GitHub写法，仿写生成dataset的代码 ,gnss-ins-sim 源码保存数据集的方式是csv，这里为了方便可视化，转为rosbag的方式保存，保存仿真的数据有，imu : gyro accel ; groundtruth : orientation(四元数)、position 、velocity。
FILE: catkin_ws/src/gnss_ins_sim/src/recorder_node_sim.py
#!/usr/bin/python import os import rospkg import rospy import rosbag import math import numpy as np import pandas as pd from gnss_ins_sim.sim import imu_model from gnss_ins_sim.sim import ins_sim # from gnss_ins_sim.geoparams import geoparams from std_msgs import msg from std_msgs.msg import String from sensor_msgs.msg import Imu from nav_msgs.msg import Odometry def get_gnss_ins_sim(motion_def_file, fs_imu, fs_gps): # set origin x y z origin_x = 2849886.61825 origin_y = -4656214.27294 origin_z = -3287190.60046 ''' Generate simulated GNSS/IMU data using specified trajectory. ''' # set IMU model: D2R = math.pi/180.0 # imu_err = 'low-accuracy' imu_err = { # 1. gyro: # a. random noise: # gyro angle random walk, deg/rt-hr 'gyro_arw': np.array([0., 0., 0.]), # gyro bias instability, deg/hr 'gyro_b_stability': np.array([0.0, 0.0, 0.0]), # gyro bias isntability correlation time, sec 'gyro_b_corr': np.array([100.0, 100.0, 100.0]), # b. deterministic error: 'gyro_b': np.array([0.0, 0.0, 0.0]), 'gyro_k': np.array([1.0, 1.0, 1.0]), 'gyro_s': np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]), # 2. accel: # a. random noise: # accel velocity random walk, m/s/rt-hr 'accel_vrw': np.array([0., 0., 0.]), # accel bias instability, m/s2 'accel_b_stability': np.array([0., 0., 0.]), # accel bias isntability correlation time, sec 'accel_b_corr': np.array([100.0, 100.0, 100.0]), # b. deterministic error: 'accel_b': np.array([0.0, 0.0, 0.0]), 'accel_k': np.array([1.0, 1.0, 1.0]), 'accel_s': np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]), # 3. mag: 'mag_si': np.eye(3) + np.random.randn(3, 3)*0.0, 'mag_hi': np.array([10.0, 10.0, 10.0])*0.0, 'mag_std': np.array([0.1, 0.1, 0.1]) } # generate GPS and magnetometer data: imu = imu_model.IMU(accuracy=imu_err, axis=9, gps=True) # init simulation: sim = ins_sim.Sim( # here sync GPS with other measurements as marker: [fs_imu, fs_imu, fs_imu], motion_def_file, ref_frame=1, imu=imu, mode=None, env=None, algorithm=None ) # run: sim.run(1) # get simulated data: rospy.logwarn( 'Simulated data size: Gyro-{}, Accel-{}, pos-{}'.format( len(sim.dmgr.get_data_all('gyro').data[0]), len(sim.dmgr.get_data_all('accel').data[0]), len(sim.dmgr.get_data_all('ref_pos').data) ) ) # calibration stages: step_size = 1.0 / fs_imu for i, (gyro, accel, ref_q, ref_pos, ref_vel) in enumerate( zip( # a. gyro: sim.dmgr.get_data_all('gyro').data[0], # b. accel: sim.dmgr.get_data_all('accel').data[0], # c. gt_pose: sim.dmgr.get_data_all('ref_att_quat').data, # groundtruth sim.dmgr.get_data_all('ref_pos').data, # d. true_vel : sim.dmgr.get_data_all('ref_vel').data ) ): yield { 'stamp': i * step_size, 'data': { # a. gyro: 'gyro_x': gyro[0], 'gyro_y': gyro[1], 'gyro_z': gyro[2], # b. accel: 'accel_x': accel[0], 'accel_y': accel[1], 'accel_z': accel[2], # c. true orientation: 'gt_quat_w': ref_q[0], 'gt_quat_x': ref_q[1], 'gt_quat_y': ref_q[2], 'gt_quat_z': ref_q[3], # d. true position: 'gt_pos_x': ref_pos[0] + origin_x, 'gt_pos_y': ref_pos[1] + origin_y, 'gt_pos_z': ref_pos[2] + origin_z, # d. true velocity: 'gt_vel_x': ref_vel[0], 'gt_vel_y': ref_vel[1], 'gt_vel_z': ref_vel[2] } } sim.results() sim.plot(['ref_pos', 'ref_vel'], opt={ 'ref_pos': '3d'}) def gnss_ins_sim_recorder(): """ Record simulated GNSS/IMU data as ROS bag """ # ensure gnss_ins_sim_node is unique: rospy.init_node('gnss_ins_sim_recorder_node') # parse params: motion_def_name = rospy.get_param('/gnss_ins_sim_recorder_node/motion_file') sample_freq_imu = rospy.get_param('/gnss_ins_sim_recorder_node/sample_frequency/imu') sample_freq_gps = rospy.get_param('/gnss_ins_sim_recorder_node/sample_frequency/gps') topic_name_imu = rospy.get_param('/gnss_ins_sim_recorder_node/topic_name_imu') topic_name_gt = rospy.get_param('/gnss_ins_sim_recorder_node/topic_name_gt') ## save scv output_path = rospy.get_param('/gnss_ins_sim_recorder_node/output_path') output_name = rospy.get_param('/gnss_ins_sim_recorder_node/output_name') ## save rosbag rosbag_output_path = rospy.get_param('/gnss_ins_sim_recorder_node/output_path') rosbag_output_name = rospy.get_param('/gnss_ins_sim_recorder_node/output_name') # generate simulated data: motion_def_path = os.path.join( rospkg.RosPack().get_path('gnss_ins_sim'), 'config', 'motion_def', motion_def_name ) imu_simulator = get_gnss_ins_sim( # motion def file: motion_def_path, # gyro-accel/gyro-accel-mag sample rate:
