《多传感器融合定位》惯性导航基础（二）
续： 《多传感器融合定位》惯性导航基础（一）.
惯性导航基础习题二
- 四、一组对自定义数据进行惯性导航解算验证
-
- 1.使用gnss-ins-sim仿真imu运动数据
- 2.对自定义运动惯性导航解算
-
- （1）数据读取
- （2）航迹解算
- （3）实验对比
- 五、遇到的问题
-
- 1.imu_tk编译问题
- 2.Ceres问题
四、一组对自定义数据进行惯性导航解算验证
要求:
- 数据可以使用仿真数据,也可以使用kitti数据集中的数据,只不过使用kitti数据时要修改
kitti2bag的代码,把imu的输出频率恢复成100hz,它的原版程序在生成bag时对数据做了降频。 - 由于mems惯导误差发散快,导航时间不用太长,几分钟即可。如果想验证高精度惯导随时
间发散的现象,可用仿真数据生成。 - 解算时尽量对比验证角增量方法和旋转矢量方法的区别,由于定轴转动下,二者没有区别,
因此若要验证该现象,数据要运动剧烈些。
参考链接: https://blog.csdn.net/weixin_41281151/article/details/110313225.
1.使用gnss-ins-sim仿真imu运动数据
该运动场景描述了物体先向东走30s，再向北走30s，然后再向东走30s最后向北走30s的轨迹.与之前生成仿真数据类似，调用以下代码产生仿真数据.
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+"//motion_def_my_imu2.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('.//my_sim_imu2//') # save data files
if __name__ == '__main__':
create_sim_imu()
2.对自定义运动惯性导航解算
（1）数据读取
首先明确我们进行解算时需要哪些数据：time、accel、gyro、gps、pos、quat等，在读取时参考我们之前的方法。
#include （2）航迹解算
航迹解算主要包括三个方面：姿态解算，速度解算，位置解算，其中最重要的就是姿态解算。无论是使用四元数还是旋转矩阵表示姿态变换，都必须先对等效的旋转矢量进行计算。值得说明的是，等效的旋转矢量并不等于角增量（特殊情况：在始终绕定轴转动的时候两者相等），所以在进行解算之前必须先计算等效旋转矢量。
a.等效旋转矢量的计算
假设角速度的线性模型: ω ( τ ) = a + 2 b τ \omega \left ( \tau \right )= a+2b\tau ω(τ)=a+2bτ
得到角增量的计算: Δ Θ ( τ ) = a τ + b τ 2 \Delta \Theta \left ( \tau \right )= a\tau +b\tau ^2 ΔΘ(τ)=aτ+bτ2
为了在不降低输出频率的情况下提高精度，使用前一时刻角增量对当前时刻进行补偿，得到前后两个周期的角增量:
Δ Θ 1 = ∫ 0 T ω ( τ ) d τ = T a + T 2 b \Delta \Theta_1= \int_{0}^{T}\omega \left ( \tau \right )d\tau = Ta+T^2b ΔΘ1=∫0Tω(τ)dτ=Ta+T2b
Δ Θ 2 = ∫ T 2 T ω ( τ ) d τ = T a + 3 T 2 b \Delta \Theta_2= \int_{T}^{2T}\omega \left ( \tau \right )d\tau = Ta+3T^2b ΔΘ2=∫T2Tω(τ)dτ=Ta+3T2b
解得：
a = ( 3 Δ Θ 1 − Δ Θ 2 ) / 2 T a=(3\Delta \Theta _1- \Delta \Theta _2)/2T a=(3ΔΘ1−ΔΘ2)/2T
b = ( Δ Θ 2 − Δ Θ 1 ) / ( 2 T 2 ) b=(\Delta \Theta _2-\Delta \Theta _1)/(2T^2) b=(ΔΘ2−ΔΘ1)/(2T2)
带入到等效旋转矢量导数计算公式：
φ ˙ ( τ ) = ω ( τ ) + 1 2 Δ Θ ( τ ) × ω ( τ ) = a + 2 b τ + 1 2 τ 2 a × b \dot{\varphi }(\tau )=\omega (\tau )+\frac{1}{2}\Delta \Theta (\tau )\times\omega \left ( \tau \right )=a+2b\tau +\frac{1}{2}\tau ^2a\times b φ˙(τ)=ω(τ)+21ΔΘ(τ)×ω(τ)=a+2bτ+21τ2a×b
积分可以得到：
φ ( T ) = Δ Θ 1 + 1 12 Δ Θ 1 × Δ Θ 2 \varphi \left ( T \right )=\Delta \Theta _1+\frac{1}{12}\Delta \Theta _1\times \Delta \Theta _2 φ(T)=ΔΘ1+121ΔΘ1×ΔΘ2
φ ( 2 T ) = Δ Θ 1 + Δ Θ 2 + 2 3 Δ Θ 1 × Δ Θ 2 \varphi \left ( 2T \right )=\Delta \Theta _1+\Delta \Theta _2+ \frac{2}{3}\Delta \Theta _1\times \Delta \Theta _2 φ(2T)=ΔΘ1+ΔΘ2+32ΔΘ1×ΔΘ2
所以我们就可以得到当前时刻相对于上一时刻的旋转矩阵：
C b , m b , m − 1 = e x p ( φ ( T ) ) − 1 e x p ( φ ( 2 T ) ) C_{b,m}^{b,m-1}=exp(\varphi (T))^{-1}exp(\varphi (2T)) Cb,mb,m−1=exp(φ(T))−1exp(φ(2T))
b.姿态解算
得到等效旋转矢量以后，姿态解算就变成了一个简单问题，在b系（运动系）相对于i系（惯性系）来看：
C b , m i = C b , m − 1 i C b , m b , m − 1 C_{b,m}^{i}=C_{b,m-1}^{i}C_{b,m}^{b,m-1} Cb,mi=Cb,m−1iCb,mb,m−1
当然我们平时讨论的相对运动是相对于n系（导航系）而言的，所以我们需要知道n系和i系的相对变换关系，他们之间的旋转角速度可以表示为：
ω i n n = ω i e n + ω e n n \omega _{in}^{n}=\omega _{ie}^{n} +\omega _{en}^{n} ωinn=ωien+ωenn
其中 ω i e n \omega _{ie}^{n} ωien表示地球自转引起的角速度, ω e n n \omega _{en}^{n} ωenn表示载体在地球表面运动时,绕地球旋转形成的角速度。其表达式如下:：
ω i e n = [ 0 ω i e cos L ω i e sin L ] T \omega _{ie}^{n}=\begin{bmatrix} 0& \omega_{ie}\cos L & \omega_{ie} \sin L \end{bmatrix} ^T ωien=[0ωiecosLωiesinL]T
ω e n n = [ − V N R M + h V E R N + h V E R N + h tan L ] T \omega _{en}^{n}=\begin{bmatrix} -\frac{V_N}{R_M+h}& \frac{V_E}{R_N+h}& \frac{V_E}{R_N+h}\tan L \end{bmatrix} ^T ωenn=[−RM+hVNRN+hVERN+hVEtanL]T
因为这两个量都比较小，且地球自转相当于定轴转动，所以在代码处理时，就直接使用角增量作为变换矩阵了，当然也可以使用等效变换矢量，效果应该会更好，最后的到的姿态解算为：
C b , m n , m = C n , m − 1 n , m C i n , m − 1 C b , m − 1 i C b , m b , m − 1 C_{b,m}^{n,m}=C_{n,m-1}^{n,m}C_{i}^{n,m-1}C_{b,m-1}^{i}C_{b,m}^{b,m-1} Cb,mn,m=Cn,m−1n,mCin,m−1Cb,m−1iCb,mb,m−1
c.速度解算
在这里其实有一个问题，对于速度而言，我们一般是在e系（地球）中考虑的。当然，我们也可以粗略的认为n系和e系时重合的，但实际上，他们之间是有一个比较小的相对运动 ω e n n \omega _{en}^{n} ωenn。如果我们同样使用角增量考虑就会有：
C n , m e = C n , m − 1 e C e n n , m − 1 C_{n,m}^{e}=C_{n,m-1}^{e}C_{en}^{n,m-1} Cn,me=Cn,m−1eCenn,m−1
速度解算为：
V m = V m − 1 + T ∗ ( 1 / 2 ∗ ( C b , m e a m + C b , m − 1 e a m − 1 ) − g n ) V_m=V_{m-1}+T*(1/2*(C_{b,m}^{e}a_{m}+C_{b,m-1}^{e}a_{m-1})-g^n) Vm=Vm−1+T∗(1/2∗(Cb,meam+Cb,m−1eam−1)−gn)
d.位置解算
P m = P m − 1 + 1 / 2 ∗ T ∗ ( V m + V m − 1 ) P_m=P_{m-1}+1/2*T*(V_m+V_{m-1}) Pm=Pm−1+1/2∗T∗(Vm+Vm−1)
代码实现如下：
void Solution()
{
Eigen::Quaterniond C_nm_to_bm(1,0,0,0);m时刻b系相对于n系的旋转矩阵
Eigen::Quaterniond C_e_to_bm(1,0,0,0);m时刻b系相对于e系的旋转矩阵
Eigen::Quaterniond C_e_to_bm_1(1,0,0,0);m-1时刻b系相对于e系的旋转矩阵
Eigen::Quaterniond C_i_to_bm_1(1,0,0,0);//m-1时刻b系相对于i系的旋转矩阵
Eigen::Quaterniond C_i_to_bm(1,0,0,0);//m时刻b系相对于i系的旋转矩阵
Eigen::Quaterniond C_i_to_nm_1(1,0,0,0);//m-1时刻n系相对于i系的旋转矩阵
Eigen::Quaterniond C_i_to_nm(1,0,0,0);//m时刻n系相对于i系的旋转矩阵
Eigen::Quaterniond C_e_to_nm_1(1,0,0,0);//m-1时刻n系相对于e系的旋转矩阵
Eigen::Quaterniond C_e_to_nm(1,0,0,0);//m时刻n系相对于e系的旋转矩阵
Eigen::Vector3d Venu_nm(0, 0, 0);//在n系下观测的m时刻载体速度
Eigen::Vector3d Venu_em(0, 0, 0);//在e系下观测的m时刻载体速度
Eigen::Vector3d Venu_em_1(0, 0, 0);//在e系下观测的m时刻载体速度
Eigen::Vector3d P_e_m(0, 0, 0);//在e系下观测的m时刻位置
Eigen::Vector3d P_e_m_1(0, 0, 0);//在e系下观测的m时刻位置
double w_ie = 360.0 /24.0 / 3600.0 * D2R ; //地球自传角速度
double L = 32 * D2R; //L为纬度
double rm = 6353346.18315; //极半径 (短半轴)
double rn = 6384140.52699; //赤道半径(长半轴)
double h =0 ; //海拔
//从第二个数据开始，等效旋转矢量求解角增量需要两个周期
for (size_t i = 2; i < stamps.size(); i++)
{
double dt1=stamps[i-1]-stamps[i-2];
double dt2=stamps[i]-stamps[i-1];
//w_ie_in_n w_en_in_n分别为在n系下观测到的 地球自传速度 和 载体在地球表面运动时,绕地球旋转形成的角速度
Eigen::Vector3d w_ie_in_n=Eigen::Vector3d(0, w_ie * std::cos(L), w_ie * std::sin(L));
Eigen::Vector3d w_en_in_n = Eigen::Vector3d(-Venu_nm[1] / (rm + h), Venu_nm[0] / (rn + h), Venu_nm[0] / (rn + h) * std::tan(L));
Eigen::Vector3d deltaTheta;
double equalAngle;
Eigen::Vector3d equalDirection;
//b系下m-1时刻到m时刻的旋转矩阵
Eigen::Quaterniond C_bm_1_to_bm;
/***********************************************************计算等效旋转矢量*************************************************************/
#if 0
//角增量解算
deltaTheta=0.5*dt2*(gyros[i]+gyros[i-1]);
equalAngle=deltaTheta.norm();
equalDirection= equalAngle==0 ? Eigen::Vector3d(0,0,0) : deltaTheta/equalAngle;
C_bm_1_to_bm=Eigen::Quaterniond(std::cos(equalAngle/2),
equalDirection[0]*std::sin(equalAngle/2),
equalDirection[1]*std::sin(equalAngle/2),
equalDirection[2]*std::sin(equalAngle/2));
#endif
#if 1
//等效转换矢量解算
Eigen::Vector3d deltaTheta1=0.5*dt1*(gyros[i-2]+gyros[i-1]);
Eigen::Vector3d deltaTheta2=0.5*dt2*(gyros[i]+gyros[i-1]);
double equalAngle1=deltaTheta1.norm();
double equalAngle2=deltaTheta2.norm();
Eigen::Vector3d equalDirection1;
Eigen::Vector3d equalDirection2;
equalDirection1= equalAngle1==0 ? Eigen::Vector3d(0,0,0) : deltaTheta1/equalAngle1;
equalDirection2= equalAngle2==0 ? Eigen::Vector3d(0,0,0) : deltaTheta2/equalAngle2;
Eigen::Quaterniond Quat1=Eigen::Quaterniond(std::cos(equalAngle1/2),
equalDirection1[0]*std::sin(equalAngle1/2),
equalDirection1[1]*std::sin(equalAngle1/2),
equalDirection1[2]*std::sin(equalAngle1/2));
Eigen::Quaterniond Quat2=Eigen::Quaterniond(std::cos(equalAngle2/2),
equalDirection2[0]*std::sin(equalAngle2/2),
equalDirection2[1]*std::sin(equalAngle2/2),
equalDirection2[2]*std::sin(equalAngle2/2));
C_bm_1_to_bm=Quat1.inverse()*Quat2;
#endif
/***********************************************************姿态解算***********************************************************************/
//b系相对于i系的m时刻旋转矩阵
C_i_to_bm=C_i_to_bm_1*C_bm_1_to_bm;
//求解b系相对于n系的m时刻旋转矩阵
//这里直接用角增量代替旋转矩阵，因为都是小量
deltaTheta=(w_ie_in_n+w_en_in_n)*dt2;
equalAngle=deltaTheta.norm();
equalDirection= equalAngle==0 ? Eigen::Vector3d(0,0,0) : deltaTheta/equalAngle;
//n系下m-1时刻到m时刻的旋转矩阵
Eigen::Quaterniond C_nm_1_to_nm=Eigen::Quaterniond(std::cos(equalAngle/2),
equalDirection[0]*std::sin(equalAngle/2),
equalDirection[1]*std::sin(equalAngle/2),
equalDirection[2]*std::sin(equalAngle/2));
C_i_to_nm=C_i_to_nm_1*C_nm_1_to_nm;
C_nm_to_bm=C_i_to_nm.inverse()*C_i_to_bm;
/***********************************************************速度解算***********************************************************************/
//速度和位置都是基于e系的，一般情况下考虑e系和n系重合,即C_e_to_nm=1
deltaTheta=w_en_in_n*dt2;
equalAngle=deltaTheta.norm();
equalDirection= equalAngle==0 ? Eigen::Vector3d(0,0,0) : deltaTheta/equalAngle;
Eigen::Quaterniond C_en_in_n=Eigen::Quaterniond(std::cos(equalAngle/2),
equalDirection[0]*std::sin(equalAngle/2),
equalDirection[1]*std::sin(equalAngle/2),
equalDirection[2]*std::sin(equalAngle/2));
C_e_to_nm = C_e_to_nm_1 * C_en_in_n;
C_e_to_bm = C_e_to_nm * C_nm_to_bm;
//解算得到的在e系下的速度
Venu_em = Venu_em_1 + dt2 * (0.5 * (C_e_to_bm_1 * accs[i - 1] + C_e_to_bm_1 * accs[i]) + Eigen::Vector3d(0,0,-g));
Venu_nm=C_e_to_nm.inverse()*Venu_em;
/***********************************************************位置解算***********************************************************************/
P_e_m = P_e_m_1+ 0.5 * dt2 * (Venu_em + Venu_em);
/***********************************************************更新***********************************************************************/
C_i_to_nm_1=C_i_to_nm;
C_i_to_bm_1=C_i_to_bm;
C_e_to_bm_1=C_e_to_bm;
C_e_to_nm_1=C_e_to_nm;
Venu_em_1=Venu_em;
P_e_m_1=P_e_m;
/**************************************************************评估*********************************************************/
std::ofstream gt;
gt.open("../Trajectory/gt.txt", std::fstream::out);
std::ofstream pose;
pose.open("../Trajectory/ins.txt", std::fstream::out);
gt << std::fixed << stamps[i] << " " << ref_poses[i][0] - ref_poses[2][0] << " "
<< ref_poses[i](1) - ref_poses[2][1] << " " << ref_poses[i](2) - ref_poses[2][2] << " "
<< ref_quats[i].coeffs()(0) << " " << ref_quats[i].coeffs()(1) << " "
<< ref_quats[i].coeffs()(2) << " " << ref_quats[i].coeffs()(3) << std::endl;
pose << std::fixed << stamps[i] << " " << P_e_m_1(0) << " "
<< P_e_m_1(1) << " " << P_e_m_1(2) << " "
<< C_nm_to_bm.coeffs()(0) << " " << C_nm_to_bm.coeffs()(1) << " "
<< C_nm_to_bm.coeffs()(2) << " " << C_nm_to_bm.coeffs()(3) << std::endl;
}
}
int main(int agrc ,char** argv)
{
ReadData(time_csv_path,times);
ReadData(accel_csv_path,accel);
ReadData(gyro_csv_path,gyro);
ReadData(gps_csv_path,gps);
ReadData(ref_pos_csv_path,ref_pos);
ReadData(ref_att_quat_csv_path,ref_att_quat);
Solution();
}
（3）实验对比
evo_ape tum gt.txt ins.txt --plot
a.使用等效旋转矢量，imu采样10hz
仿真数据一分钟的情况下误差在37m左右，这个数据算是正常把～
APE w.r.t. translation part (m)
(not aligned)
max 37.228732
mean 10.425789
median 4.848312
min 0.000095
rmse 15.072917
sse 158353.402388
std 10.885576
b.使用角增量，imu采样10hz
仿真数据一分钟的情况下误差在904m左右（？？？？？？）这个数据让我怀疑我是不是做错了唉～，有没有人能够耐心的看到这里，救救孩子把！
APE w.r.t. translation part (m)
(not aligned)
max 904.253556
mean 295.980268
median 315.221614
min 0.000095
rmse 384.671414
sse 103136551.292775
std 245.698550
c.使用等效旋转矢量，imu采样1000hz
相比于10hz的采样，频率的提高，误差稍微降了一点。
APE w.r.t. translation part (m)
(not aligned)
max 32.014005
mean 8.498650
median 4.700389
min 0.000000
rmse 12.460310
sse 10414330.123972
std 9.112205
c.使用角增量，imu采样1000hz
我觉得错啦错啦错啦，应该是角增量的求解出错了，我看了半天也没发现什么错误啊，有时间再改改把，等效旋转矢量应该是没问题的（2021.5.30）
APE w.r.t. translation part (m)
(not aligned)
max 980.592993
mean 316.567616
median 324.498340
min 0.000000
rmse 413.078011
sse 11445579454.425201
std 265.364631
五、遇到的问题
1.imu_tk编译问题
make[2]: *** No rule to make target '/usr/lib/x86_64-linux-gnu/libglog.so', needed by '../bin/test_integration'. Stop.
CMakeFiles/Makefile2:67: recipe for target 'CMakeFiles/test_integration.dir/all' failed
make[2]: *** No rule to make target '/usr/lib/x86_64-linux-gnu/libgflags.so', needed by '../bin/test_integration'. Stop.
CMakeFiles/Makefile2:67: recipe for target 'CMakeFiles/test_integration.dir/all' failed
这就是链接库的时候没有找到，可以先尝试locate一下报错的库文件，要是在其他地方能找到这个库文件，那就映射到报错的文件夹下面即可，如果locate没有找到，那么就要自己下载了～，很幸运，我的两个都在其他地方找到了
sudo ln -s /usr/local/lib/libgflags.so.2.2 /usr/lib/x86_64-linux-gnu/libgflags.so
2.Ceres问题
/usr/local/lib/libceres.a(callbacks.cc.o): In function `ceres::internal::LoggingCallback::operator()(ceres::IterationSummary const&)':
callbacks.cc:(.text+0x1ce): undefined reference to `google::kLogSiteUninitialized'
/usr/local/lib/libceres.a(callbacks.cc.o):(.data.rel+0x0): undefined reference to `google::kLogSiteUninitialized'
/usr/local/lib/libceres.a(line_search.cc.o): In function `ceres::internal::WolfeLineSearch::ZoomPhase(ceres::internal::FunctionSample const&, ceres::internal::FunctionSample, ceres::internal::FunctionSample, ceres::internal::FunctionSample*, ceres::internal::LineSearch::Summary*) const':
line_search.cc:(.text+0x216d): undefined reference to `google::kLogSiteUninitialized'
line_search.cc:(.text+0x3069): undefined reference to `google::kLogSiteUninitialized'
/usr/local/lib/libceres.a(line_search.cc.o): In function `ceres::internal::WolfeLineSearch::BracketingPhase(ceres::internal::FunctionSample const&, double, ceres::internal::FunctionSample*, ceres::internal::FunctionSample*, bool*, ceres::internal::LineSearch::Summary*) const':
line_search.cc:(.text+0x4d2b): undefined reference to `google::kLogSiteUninitialized'
/usr/local/lib/libceres.a(line_search.cc.o):line_search.cc:(.text+0x520a): more undefined references to `google::kLogSiteUninitialized' follow
collect2: error: ld returned 1 exit status
CMakeFiles/ceresCurveFitting.dir/build.make:160: recipe for target 'ceresCurveFitting' failed
make[2]: *** [ceresCurveFitting] Error 1
CMakeFiles/Makefile2:141: recipe for target 'CMakeFiles/ceresCurveFitting.dir/all' failed
make[1]: *** [CMakeFiles/ceresCurveFitting.dir/all] Error 2
Makefile:83: recipe for target 'all' failed
看到这个吓我一跳，于是我果断把系统里的有关于ceres的文件卸载的干干净净，找到以前下的安装包重新编译安装，问题解决，鬼知道它经历了什么罢工了～