多传感器融合定位（3-惯性导航原理及误差分析）8-惯性导航解算验证
多传感器融合定位（3-惯性导航原理及误差分析）8-惯性导航误差分析
本次作业摘自 张松鹏大哥的优秀作业
eg.
在现实中 高精度惯导中用等效旋转矢量，低精度惯导中，用角增量；一般中低精度的导航里面，提高的比例都在10%以内，没有太高；实际系统中可以做个实验对比一下，看能提高多少，来决定要不要使用等效旋转矢量的方法解算
我们先来回顾一下姿态更新的框架图
两种姿态解算方法：
1.使用theta t 时间内角增量做积分中值处理
2.使用角增量求解等效旋转矢量的方法
注：表征姿态变化，一般使用四元数较为方便
张松鹏的优秀作业代码github地址
这里对松鹏大神的部分代码进行自己理解记录和一些修改
仿真数据集准备
这里使用的imu 生成仿真软件为 gnss-ins-sim
自定义运动场景 imu_def5.csv
自定义场景
生成仿真数据 sim_imu.py
运行 如下指令即可生成仿真数据，注意生成文件的路径
python sim_imu.py
import numpy as np
import os
import math
from gnss_ins_sim.sim import imu_model
from gnss_ins_sim.sim import ins_sim
# globals
D2R = math.pi/180
motion_def_path = os.path.abspath('.//demo_motion_def_files//')
fs = 10.0 # IMU sample frequency
fs_gps = 10.0 # GPS sample frequency
fs_mag = fs # magnetometer sample frequency, not used for now
def create_sim_imu(): #生成 imu 方针imu数据
### Customized IMU model parameters, typical for IMU381
imu_err = {'gyro_b': np.array([1e-2, 2e-2, 3e-2]) / D2R * 3600 * 0,
'gyro_arw': np.array([1e-5, 1e-5, 1e-5]) / D2R * 60,
'gyro_b_stability': np.array([5e-6, 5e-6, 5e-6]) / D2R * 3600,
'gyro_b_corr': np.array([100.0, 100.0, 100.0]),
'accel_b': np.array([2.0e-3, 1.0e-3, 5.0e-3]) * 0,
'accel_vrw': np.array([1e-4, 1e-4, 1e-4]) * 60.0,
'accel_b_stability': np.array([1e-5, 1e-5, 1e-5]) * 1.0,
'accel_b_corr': np.array([200.0, 200.0, 200.0]),
# 'mag_std': np.array([0.2, 0.2, 0.2]) * 1.0
}
# generate GPS and magnetometer data
imu = imu_model.IMU(accuracy=imu_err, axis=6, gps=True)
#### start simulation
sim = ins_sim.Sim([fs, fs_gps, fs_mag],
motion_def_path+"//imu_def6.csv",
ref_frame=1,
imu=imu,
mode=None,
env=None,
algorithm=None)
sim.run(1)
# generate simulation results, summary, and save data to files
sim.results('/home/kaho/chapter3/kaho_chapter3/gnss-ins-sim/imu_data/data6') # save data files
if __name__ == '__main__':
create_sim_imu()
resloving.cpp 姿态解算 部分代码片
读取gnss-ins-sim数据集
// 读取gnss-ins-sim生成的仿真数据
bool ReadData(const std::vector<std::string> &path,
std::vector<double> &stamps,
std::vector<Eigen::Vector3d> &accs,
std::vector<Eigen::Vector3d> &gyros,
std::vector<Eigen::Vector3d> &gpses,
std::vector<Eigen::Vector3d> &ref_poses,
std::vector<Eigen::Quaterniond> &ref_att_quats)
{
stamps.clear();
accs.clear();
gyros.clear();
gpses.clear();
ref_poses.clear();
ref_att_quats.clear();
std::vector<std::ifstream> reads;
// int count = 0;
for (int i = 0; i < 6; ++i)
{
reads.push_back(std::ifstream(path[i]));
}
bool init = false;
while (true)
{
if (!init)
{
init = true;
for (int i = 0; i < 6; ++i)
{
std::string strs;
std::getline(reads[i], strs);
}
}
else
{
double time;
{
std::string strs;
if (std::getline(reads[0], strs))
{
// count++;
// count = count % 2;
// if (count != 0)
// {
// continue;
// }
time = std::stod(strs);
}
else
{
break;
}
}
{
std::string strs;
std::string temp;
strs = "";
std::getline(reads[1], strs);
temp = "";
std::vector<double> acc;
for (int i = 0; i < strs.size(); ++i)
{
if (strs[i] == ',')
{
acc.push_back(std::stod(temp));
temp = "";
}
else
{
temp = temp + strs[i];
}
}
acc.push_back(std::stod(temp));
strs = "";
std::getline(reads[2], strs);
temp = "";
std::vector<double> gyro;
for (int i = 0; i < strs.size(); ++i)
{
if (strs[i] == ',')
{
gyro.push_back(std::stod(temp));
temp = "";
}
else
{
temp = temp + strs[i];
}
}
gyro.push_back(std::stod(temp));
strs = "";
std::getline(reads[3], strs);
temp = "";
std::vector<double> gps;
for (int i = 0; i < strs.size(); ++i)
{
if (strs[i] == ',')
{
gps.push_back(std::stod(temp));
temp = "";
}
else
{
temp = temp + strs[i];
}
}
gps.push_back(std::stod(temp));
strs = "";
std::getline(reads[4], strs);
temp = "";
std::vector<double> ref_pos;
for (int i = 0; i < strs.size(); ++i)
{
if (strs[i] == ',')
{
ref_pos.push_back(std::stod(temp));
temp = "";
}
else
{
temp = temp + strs[i];
}
}
ref_pos.push_back(std::stod(temp));
strs = "";
std::getline(reads[5], strs);
temp = "";
std::vector<double> ref_att_quat;
for (int i = 0; i < strs.size(); ++i)
{
if (strs[i] == ',')
{
ref_att_quat.push_back(std::stod(temp));
temp = "";
}
else
{
temp = temp + strs[i];
}
}
ref_att_quat.push_back(std::stod(temp));
// std::cout << time << std::endl;
// std::cout << (Eigen::Vector3d(acc[0], acc[1], acc[2])).transpose() << std::endl;
// std::cout << (Eigen::Vector3d(gyro[0], gyro[1], gyro[2])).transpose() << std::endl;
// std::cout << (Eigen::Vector3d(gps[0], gps[1], gps[2])).transpose() << std::endl;
// std::cout << (Eigen::Vector3d(ref_pos[0], ref_pos[1], ref_pos[2])).transpose() << std::endl;
// std::cout << (Eigen::Quaterniond(ref_att_quat[0], ref_att_quat[1], ref_att_quat[2], ref_att_quat[3])).coeffs().transpose() << std::endl;
// throw;
stamps.push_back(time);
accs.push_back(Eigen::Vector3d(acc[0], acc[1], acc[2]));
gyros.push_back(Eigen::Vector3d(gyro[0] * D2R, gyro[1] * D2R, gyro[2] * D2R));
// 四元数(4x1)(Eigen::Quaterniond 旋转向量(3x1):Eigen::AngleAxisd 平移向量(3x1):Eigen::Vector3d
Eigen::Quaterniond q = Eigen::AngleAxisd(90 * D2R, Eigen::Vector3d::UnitZ()) * //以(0,0,1)做旋转轴，旋转90度
Eigen::AngleAxisd(0, Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(180 * D2R, Eigen::Vector3d::UnitX());
q = q.inverse(); // 初始化四元数
gpses.push_back(Eigen::Vector3d(gps[0], gps[1], gps[2]));
// std::cout << Eigen::Vector3d(ref_pos[0], ref_pos[1], ref_pos[2]) << std::endl;
// std::cout << q * Eigen::Vector3d(ref_pos[0], ref_pos[1], ref_pos[2]) << std::endl;
// throw;
ref_poses.push_back(q * Eigen::Vector3d(ref_pos[0], ref_pos[1], ref_pos[2])); // 存储 reference pose
ref_att_quats.push_back(q * Eigen::Quaterniond(ref_att_quat[0], ref_att_quat[1], ref_att_quat[2], ref_att_quat[3])); // 存储 reference quaterniond
}
}
}
}
输入文件路径
// 输入路径，读取数据
path.push_back("../../gnss-ins-sim/imu_data/data6/time.csv");
path.push_back("../../gnss-ins-sim/imu_data/data6/accel-0.csv");
path.push_back("../../gnss-ins-sim/imu_data/data6/gyro-0.csv");
path.push_back("../../gnss-ins-sim/imu_data/data6/gps-0.csv");
path.push_back("../../gnss-ins-sim/imu_data/data6/ref_pos.csv");
path.push_back("../../gnss-ins-sim/imu_data/data6/ref_att_quat.csv");
ReadData(path, stamps, accs, gyros, gpses, ref_poses, ref_att_quats);
生成 gt.txt ins.txt 供使用kitti evo 评估工具进行数据评估
// groundtruth
std::ofstream gt;
gt.open("../../gnss-ins-sim/imu_data/data6/gt.txt", std::fstream::out);
// 解算后的姿态
std::ofstream pose;
pose.open("../../gnss-ins-sim/imu_data/data6/ins.txt", std::fstream::out);
初始化旋转量和一些地球参数常量
Eigen::Vector3d g = Eigen::Vector3d(0, 0, -9.79484197226504); // 地球重力矢量
/*旋转矩阵*/
Eigen::Quaterniond c_nm_bm(1, 0, 0, 0);
Eigen::Quaterniond c_nm_1_bm_1(1, 0, 0, 0);
Eigen::Quaterniond c_e_nm(1, 0, 0, 0);
Eigen::Quaterniond c_e_nm_1(1, 0, 0, 0);
Eigen::Quaterniond c_i_bm_1(1, 0, 0, 0);
Eigen::Quaterniond c_i_bm(1, 0, 0, 0);
Eigen::Quaterniond c_i_nm_1(1, 0, 0, 0);
Eigen::Quaterniond c_i_nm(1, 0, 0, 0);
Eigen::Quaterniond c_e_bm_1(1, 0, 0, 0);
Eigen::Quaterniond c_e_bm(1, 0, 0, 0);
Eigen::Vector3d p_e_nm_e(0, 0, 0);
Eigen::Vector3d p_e_nm_1_e(0, 0, 0);
Eigen::Vector3d v_e_nm_e(0, 0, 0);
Eigen::Vector3d v_e_nm_1_e(0, 0, 0);
Eigen::Vector3d v_e_nm_nm(0, 0, 0);
double w_ie = 360.0 /24.0 / 3600.0 * D2R ; // 地球自转角速度
double rm = 6353346.18315; //极半径 (短半轴)
double rn = 6384140.52699; //赤道半径(长半轴)
double L = 32 * D2R; //L为纬度
double h =0 ;
角增量姿态解算
/*角增量姿态解算*/
if (method == 0 )
{
phi = 0.5*dt *(gyros[i-1] + gyros[i]); //积分中值定理
angle = phi.norm(); //求解等效旋转角度
if (angle == 0)
{
direction = Eigen::Vector3d(0,0,0);
}else
{
direction = phi / angle ; //单位化取方向矢量
direction = direction * std::sin(angle/2.0); // 轴角 转换为 四元数表示
}
c_bm_1_bm = Eigen::Quaterniond(std::cos(angle / 2.0), direction[0], direction[1], direction[2]);
}
eg：轴角与四元数的转换公式
参考博客 ：
四元数和旋转轴及旋转角度之间的转换理解实例
四元数笔记（3）—— 轴角表示法与四元数表示的区别
x=ax∗sin(θ/2)
y = a y ∗ s i n ( θ / 2 ) y = ay * sin(\theta/2)y=ay∗sin(θ/2)
z = a z ∗ s i n ( θ / 2 ) z = az * sin(\theta/2)z=az∗sin(θ/2)
w = c o s ( θ / 2 ) w = cos(\theta/2)w=cos(θ/2)
其中(ax,ay,az)表示轴的矢量，theta表示绕此轴的旋转角度
等效旋转矢量姿态解算
公式先导
部分代码解析：
求解 T 和 2T时刻的等效旋转矢量
求解 当前时刻相对于上一时刻的旋转
求解旋转矩阵
速度解算，位置解算
eg：位置其实算的是相对于e系的，所以要用到相对于e系的旋转，不过机器人领域，忽略了e跟n系的相对旋转，直接看成一个坐标系了,忽略n相对于e的旋转
//速度解算
/*位置其实算的是相对于e系的，所以要用到相对于e系的旋转，不过机器人领域，忽略了e跟n系的相对旋转，直接看成一个坐标系了,忽略n相对于e的旋转***/
v_e_nm_e = v_e_nm_1_e + dt * (0.5 * (c_nm_1_bm_1 * accs[i - 1] + c_nm_bm * accs[i]) + g);
v_e_nm_nm = c_nm_bm.inverse()*v_e_nm_e;
p_e_nm_e = p_e_nm_1_e + 0.5 * dt * (v_e_nm_1_e + v_e_nm_e);
// 更新 旋转矩阵
c_i_bm_1 = c_i_bm;
c_i_nm_1 = c_i_nm;
/*********************/
c_e_nm_1 = c_e_nm;
c_e_bm_1 = c_e_bm;
/********************/
c_nm_1_bm_1 = c_nm_bm;
v_e_nm_1_e = v_e_nm_e;
p_e_nm_1_e = p_e_nm_e;
解算结果
在两个不同的终端，分别运行
./resolving 0
./resolving 1
使用evo 里程计评估工具进行评估
evo_ape tum gt.txt ins.txt --plot
角增量解算
等效旋转矢量解算
对比发现两种方法误差相同，运行1分钟后整体误差均达到30米左右。由于运动是绕定轴转动，计算结
果相同。
为了对比两者的区别尝试让运动更剧烈且更久一些角增量和旋转矢量的轨迹精度如下所示
ps：来自张松鹏总结
全部代码
代码下载地址https://github.com/rainbowrooster/Multi—sensor-fusion-and-positioning/tree/main/chapter3/Q4
resolving.cpp
#include CMakeLists.txt
project(ins)
cmake_minimum_required(VERSION 3.10)
include_directories(/usr/include/eigen3)
add_executable(resolving resolving.cpp)
#target_link_libraries( ins ${IMU_TK_LIBS})
sim_imu.py
import numpy as np
import os
import math
from gnss_ins_sim.sim import imu_model
from gnss_ins_sim.sim import ins_sim
# globals
D2R = math.pi/180
motion_def_path = os.path.abspath('.//demo_motion_def_files//')
fs = 10.0 # IMU sample frequency
fs_gps = 10.0 # GPS sample frequency
fs_mag = fs # magnetometer sample frequency, not used for now
def create_sim_imu(): #生成 imu 方针imu数据
### Customized IMU model parameters, typical for IMU381
imu_err = {'gyro_b': np.array([1e-2, 2e-2, 3e-2]) / D2R * 3600 * 0,
'gyro_arw': np.array([1e-5, 1e-5, 1e-5]) / D2R * 60,
'gyro_b_stability': np.array([5e-6, 5e-6, 5e-6]) / D2R * 3600,
'gyro_b_corr': np.array([100.0, 100.0, 100.0]),
'accel_b': np.array([2.0e-3, 1.0e-3, 5.0e-3]) * 0,
'accel_vrw': np.array([1e-4, 1e-4, 1e-4]) * 60.0,
'accel_b_stability': np.array([1e-5, 1e-5, 1e-5]) * 1.0,
'accel_b_corr': np.array([200.0, 200.0, 200.0]),
# 'mag_std': np.array([0.2, 0.2, 0.2]) * 1.0
}
# generate GPS and magnetometer data
imu = imu_model.IMU(accuracy=imu_err, axis=6, gps=True)
#### start simulation
sim = ins_sim.Sim([fs, fs_gps, fs_mag],
motion_def_path+"//imu_def6.csv",
ref_frame=1,
imu=imu,
mode=None,
env=None,
algorithm=None)
sim.run(1)
# generate simulation results, summary, and save data to files
sim.results('/home/kaho/chapter3/kaho_chapter3/gnss-ins-sim/imu_data/data6') # save data files
if __name__ == '__main__':
create_sim_imu()