上一篇文章分享了论文“基于车轮安装MEMS IMU的航迹推算算法研究”。
基于车轮安装MEMS IMU的航迹推算算法研究
纸上得来终觉浅，绝知此事要躬行。看再多的论文，不用代码实现一下，看看效果总觉得不得劲，刚好论文作者写有相关源码并提供了数据集，所以我们详细解读下，尽量做到公式与代码对应起来，完整呈现，真正做到看完就会的效果，哈哈。有问题请批评指正~
阅前知悉：
源码与论文的主要区别：
1.状态向量为pvacur(pos、vel、att(q, R, euler), acc/gyro bias acc/gyro scale)，没有考虑里程计比例因子误差和杠臂误差；
2.没有利用载体运动约束：零速更新和零速航向更新ZUPT/ZIHR。
源码地址：https://github.com/i2Nav-WHU/Wheel-INS
配套源码程序支持ubuntu/window，为了省去配置麻烦，这里只解读ubuntu环境下版本。
程序结构：
可以看到Wheel-INS包括：配置文件config，并提供配置文件相对应的数据dataset，第三方依赖库ThirdParty，常有工具文件夹utils、如画图，源码部分在src中。文件结构简单、清晰。
程序入口main
主要分为几个部分：导入相应yaml配置文件，读取数据，newImuProcess，存储结果。
一、导入相应yaml配置文件
文件导入通过yaml文件格式导入，源码中有点值得借鉴的是，导入yaml文件时，通过try catch做异常处理。这样做很友好，因为很多同学跑一些源码工程莫名跑不起来，有一部分就是yaml部分参数格式设置不对的，有这个异常跑出就可以很容易定位问题做在了。如下：
YAML::Node config;
try {
config = YAML::LoadFile(argv[1]);
} catch (YAML::Exception &exception) {
std::cout << "Failed to read configuration file: " << exception.what()
<< std::endl;
return -1;
}
从上面程序片段可以看出，如果运行代码时，如果第二个参数没有yaml文件就直接跑出异常，退出程序。
接下来重点看下loadConfig函数
inline bool loadConfig(YAML::Node &config, Paras ¶s) {
std::vector<double> initposstd_vec, initvelstd_vec, initattstd_vec;
try {
initposstd_vec = config["initposstd"].as<std::vector<double>>();
initvelstd_vec = config["initvelstd"].as<std::vector<double>>();
initattstd_vec = config["initattstd"].as<std::vector<double>>();
} catch (YAML::Exception &exception) {
std::cout << "Failed when loading configuration. Please check initial std "
"of position, velocity, and attitude!"
<< std::endl;
return false;
}
for (int i = 0; i < 3; i++) {
paras.initstate_std.pos[i] = initposstd_vec[i];
paras.initstate_std.vel[i] = initvelstd_vec[i];
paras.initstate_std.euler[i] = initattstd_vec[i] * D2R;
}
double arw, vrw, gbstd, abstd, gsstd, asstd;
try {
arw = config["imunoise"]["arw"].as<double>();
vrw = config["imunoise"]["vrw"].as<double>();
gbstd = config["imunoise"]["gbstd"].as<double>();
abstd = config["imunoise"]["abstd"].as<double>();
gsstd = config["imunoise"]["gsstd"].as<double>();
asstd = config["imunoise"]["asstd"].as<double>();
paras.imunoise.corr_time = config["imunoise"]["corrtime"].as<double>();
} catch (YAML::Exception &exception) {
std::cout << "Failed when loading configuration. Please check IMU noise!"
<< std::endl;
return false;
}
for (int i = 0; i < 3; i++) {
paras.imunoise.gyr_arw[i] = arw * (D2R / 60.0);
paras.imunoise.acc_vrw[i] = vrw / 60.0;
paras.imunoise.gyrbias_std[i] = gbstd * (D2R / 3600.0);
paras.imunoise.accbias_std[i] = abstd * 1e-5;
paras.imunoise.gyrscale_std[i] = gsstd * 1e-6;
paras.imunoise.accscale_std[i] = asstd * 1e-6;
paras.initstate_std.imuerror.gyrbias[i] = gbstd * (D2R / 3600.0);
paras.initstate_std.imuerror.accbias[i] = abstd * 1e-5;
paras.initstate_std.imuerror.gyrscale[i] = gsstd * 1e-6;
paras.initstate_std.imuerror.accscale[i] = asstd * 1e-6;
}
paras.imunoise.corr_time *= 3600;
std::vector<double> mountAngle, leverArm, odo_measurement_std;
double odo_update_interval, wheelradius;
bool ifCompensateVelocity;
mountAngle = config["MisalignAngle"].as<std::vector<double>>();
leverArm = config["WheelLA"].as<std::vector<double>>();
odo_measurement_std = config["ODO_std"].as<std::vector<double>>();
paras.mountAngle = Eigen::Vector3d(mountAngle.data());
paras.leverArm = Eigen::Vector3d(leverArm.data());
paras.odo_measurement_std = Eigen::Vector3d(odo_measurement_std.data());
paras.odo_update_interval = config["ODO_dt"].as<double>();
paras.wheelradius = config["Wheel_Radius"].as<double>();
paras.ifCompensateVelocity = config["ifCompVel"].as<bool>();
paras.starttime = config["starttime"].as<double>();
return true;
}
从程序片段可以看出，将yaml中配置参数往Paras这个数据结构中装，就干了这一件事。
所以接下来就重点看下Paras数据结构构成。
typedef struct Paras {
// initial state and state standard deviation
NavState initstate;
NavState initstate_std;
// imu noise parameters
ImuNoise imunoise;
// install parameters
Eigen::Vector3d mountAngle = {0, 0, 0};
Eigen::Vector3d leverArm = {0, 0, 0};
Eigen::Vector3d odo_measurement_std = {0, 0, 0};
double odo_update_interval;
double wheelradius;
bool ifCompensateVelocity;
double starttime;
} Paras;
Paras数据结构中又包含，NavState、ImuNoise，继续展开看就行（为了不让篇幅过于长，让人阅读不适，点到为止，这是一贯宗旨）。
NavState包括三维位置、速度、欧拉角和imu误差数据结构ImuError，ImuError包括acc、gyro，bias和scale。
ImuNoise包括三维acc、gyro随机游走，bias和scale标准差。
mountAngle 对应论文中安装角误差；
leverArm 对应论文中杠臂误差。
只要在一张白纸上将数据结构构成、意义搞清楚，看后续程序会变得更加简单，也能更好的理解。
所以，看一些开源写的好代码，数据定义都非常清晰、相互包含关系逻辑性很强，看这种代码就像看艺术品一样。。。
二、读取数据
数据配置文件中包含数据长度、数据采样频率、开始时间和结束时间。根据数据路径和配置参数进行数据读取，如下：
ImuFileLoader imufile(imupath, imudatalen, imudatarate);
ImuFileLoader(const string &filename, int columns, int rate = 200) {
open(filename, columns, FileLoader::BINARY);
dt_ = 1.0 / (double)rate;
imu_.timestamp = 0;
}
文件使用标准文件方式读取open，关于数据的读取全部在对象imufile中进行了封装实现，如next，starttime，endtime。如下：
const IMU &next() {
imu_pre_ = imu_;
data_ = load();
imu_.timestamp = data_[0];
memcpy(imu_.angular_velocity.data(), &data_[1], 3 * sizeof(double));
memcpy(imu_.acceleration.data(), &data_[4], 3 * sizeof(double));
double dt = imu_.timestamp - imu_pre_.timestamp;
if (dt < 0.1) {
imu_.dt = dt;
} else {
imu_.dt = dt_;
}
return imu_;
}
double starttime() {
double starttime;
std::streampos sp = filefp_.tellg();
filefp_.seekg(0, std::ios_base::beg);
starttime = load().front();
filefp_.seekg(sp, std::ios_base::beg);
return starttime;
}
double endtime() {
double endtime = -1;
std::streampos sp = filefp_.tellg();
if (filetype_ == TEXT) {
filefp_.seekg(-2, std::ios_base::end);
char byte = 0;
auto pos = filefp_.tellg();
do {
pos -= 1;
filefp_.seekg(pos);
filefp_.read(&byte, 1);
} while (byte != '\n');
} else {
filefp_.seekg(-columns_ * sizeof(double), std::ios_base::end);
}
endtime = load().front();
filefp_.seekg(sp, std::ios_base::beg);
return endtime;
}
从上述程序片段可以看出：
每调用一次next()返回一帧imu数据(timestamp，dt(与前一帧数据时间差)，gyro，acc)；
每调用一次starttime返回当前帧时间戳；
调用一次endtime返回最后一帧时间戳。
seekg()与tellg()用法，详见seekg()与tellg()详解
三、newImuProcess
1.从yaml配置starttime读取第一帧imu数据；
将读取的数据添加到imuBuff_(类型std::deque)
wheelins.addImuData(imu_cur);
2.循环入口
while (true) {
imu_cur = imufile.next();
if (imu_cur.timestamp > endtime || imufile.isEof()) {
break;
}
wheelins.addImuData(imu_cur);
wheelins.newImuProcess();
timestamp = wheelins.timestamp();
navstate = wheelins.getNavState();
cov = wheelins.getCovariance();
writeNavResult(timestamp, navstate, navfile, imuerrfile);
writeSTD(timestamp, cov, stdfile);
percent = int((imu_cur.timestamp - starttime) / interval * 100);
if (percent - lastpercent >= 1) {
std::cout << "Processing: " << std::setw(3) << percent << "%\r"
<< std::flush;
lastpercent = percent;
}
}
（1）依次读取下一帧数据
wheelins.addImuData(imu_cur);
（2）处理wheelins.newImuProcess();
A.insPropagation(imupre_, imucur_)
a.补偿imu：imuCompensate
根据imu零偏和比例因子误差acc/gyro数据
b.insMesh
void INSMech::insMech(const PVA &pvapre, PVA &pvacur, const IMU &imupre,
const IMU &imucur) {
Eigen::Vector3d d_vfb, d_vfn, d_vgn, gl;
Eigen::Vector3d temp1, temp2, temp3;
Eigen::Vector3d imucur_dvel, imucur_dtheta, imupre_dvel, imupre_dtheta;
imucur_dvel = imucur.acceleration * imucur.dt;
imucur_dtheta = imucur.angular_velocity * imucur.dt;
imupre_dvel = imupre.acceleration * imupre.dt;
imupre_dtheta = imupre.angular_velocity * imupre.dt;
temp1 = imucur_dtheta.cross(imucur_dvel) / 2;
temp2 = imupre_dtheta.cross(imucur_dvel) / 12;
temp3 = imupre_dvel.cross(imucur_dtheta) / 12;
d_vfb = imucur_dvel + temp1 + temp2 + temp3;
d_vfn = pvapre.att.cbn * d_vfb;
gl << 0, 0, NormG;
d_vgn = gl * imucur.dt;
pvacur.vel = pvapre.vel + d_vfn + d_vgn;
Eigen::Vector3d midvel;
midvel = (pvacur.vel + pvapre.vel) / 2;
pvacur.pos = pvapre.pos + midvel * imucur.dt;
Eigen::Quaterniond qbb;
Eigen::Vector3d rot_bframe;
rot_bframe = imucur_dtheta + imupre_dtheta.cross(imucur_dtheta) / 12;
qbb = Rotation::rotvec2quaternion(rot_bframe);
pvacur.att.qbn = pvapre.att.qbn * qbb;
pvacur.att.cbn = Rotation::quaternion2matrix(pvacur.att.qbn);
pvacur.att.euler = Rotation::matrix2euler(pvacur.att.cbn);
}
从上述程序片段看出，基于中值积分，完成惯性导航位置、速度、姿态的求解，分别赋值为pvacurr.pos，pvacurr.vel，pvacurr.att（q,R,euler）。
c.根据论文中基于车轮中心速度约束算法推导的状态向量误差方程，完成卡尔曼状态预测
源码中状态向量没有考虑里程计比例因子误差和杠臂误差。
...
F.block(BG_ID, BG_ID, 3, 3) =
-1 / paras_.imunoise.corr_time * Eigen::Matrix3d::Identity();
F.block(BA_ID, BA_ID, 3, 3) =
-1 / paras_.imunoise.corr_time * Eigen::Matrix3d::Identity();
F.block(SG_ID, SG_ID, 3, 3) =
-1 / paras_.imunoise.corr_time * Eigen::Matrix3d::Identity();
F.block(SA_ID, SA_ID, 3, 3) =
-1 / paras_.imunoise.corr_time * Eigen::Matrix3d::Identity();
G.block(V_ID, VRW_ID, 3, 3) = pvapre_.att.cbn;
G.block(PHI_ID, ARW_ID, 3, 3) = pvapre_.att.cbn;
G.block(BG_ID, BGSTD_ID, 3, 3) = Eigen::Matrix3d::Identity();
G.block(BA_ID, BASTD_ID, 3, 3) = Eigen::Matrix3d::Identity();
G.block(SG_ID, SGSTD_ID, 3, 3) = Eigen::Matrix3d::Identity();
G.block(SA_ID, SASTD_ID, 3, 3) = Eigen::Matrix3d::Identity();
//对F(t)/G(t)离散化
Phi.setIdentity();
//F(k)= F(k)+ F(t)*dt
Phi = Phi + F * imucur.dt;
//G(k)= G(t)w*G(t)^t*dt
Qd = G * Qc_ * G.transpose() * imucur.dt;
Qd = (Phi * Qd * Phi.transpose() + Qd) / 2;
EKFPredict(Phi, Qd);
结合程序片段看出，Qc_为w(t)，先求出F(t)/G(t)，Phi，Qd是对F(t)/G(t)离散化。
void WheelINS::EKFPredict(Eigen::MatrixXd &Phi, Eigen::MatrixXd &Qd) {
assert(Phi.rows() == Cov_.rows());
assert(Qd.rows() == Cov_.rows());
Cov_ = Phi * Cov_ * Phi.transpose() + Qd;
delta_x_ = Phi * delta_x_;
}
更新状态delta_x_ 和协方差Cov_ 。
B.里程计更新ODOUpdate()
a.计算轮子速度wheelVelocity
void WheelINS::getWheelVelocity() {
double gyro_x_mean = 0.0;
int i = 0;
for (auto it = imuBuff_.begin(); it != imuBuff_.end(); ++it) {
gyro_x_mean += it->angular_velocity[0];
}
gyro_x_mean /= imuBuffsize;
wheelVelocity = -paras_.wheelradius * (gyro_x_mean);
}
imuBuff_尺度，长度为imuBuffsize = 0.1*IMU_RATE。
用imuBuff_角速度平均值和轮子半径计算轮子速度。
b.计算由杠臂投影计算得到轮速观测velocity_leverarm
Matrix3d C_nv = computeVehicleRotmat().transpose();
Matrix3d C_bv = C_nv * pvacur_.att.cbn;
Vector3d velocity_vframe = C_nv * pvacur_.vel;
Matrix3d angularVelocity_skew =
Rotation::skewSymmetric(imucur_.angular_velocity);
Matrix3d leverarm_skew = Rotation::skewSymmetric(paras_.leverArm);
Matrix3d velocitySkew_nframe = C_nv * Rotation::skewSymmetric(pvacur_.vel);
Vector3d velocity_leverarm = C_bv * angularVelocity_skew * paras_.leverArm;
Matrix3d WheelINS::computeVehicleRotmat() {
Vector3d vehicle_euler = Vector3d::Zero();
vehicle_euler[2] = pvacur_.att.euler[2] - M_PI / 2.0;
Matrix3d C_vn = Rotation::euler2matrix(vehicle_euler);
return C_vn;
}
如论文所述，认定imu与载体之间的偏航角偏差固定为90°。由杠臂投影计算得到杠臂投影到轮子中心的速度velocity_leverarm，由速度状态计算得到机体坐标系的速度velocity_vframe 。
则轮子速度观测：
Eigen::MatrixXd Zv = velocity_vframe + velocity_leverarm;
如论文公式：
c.计算观测矩阵H
Hv.resize(3, RANK);
Hv.setZero();
Hv.block<3, 3>(0, V_ID) = C_nv;
Hv.block<3, 3>(0, PHI_ID) =
C_nv * Rotation::skewSymmetric(pvacur_.att.cbn * angularVelocity_skew *
paras_.leverArm);
Hv.block<3, 3>(0, BG_ID) = -C_bv * leverarm_skew;
Hv.block<3, 3>(0, SG_ID) =
-C_bv * leverarm_skew * imucur_.angular_velocity.asDiagonal();
Hv.block<3, 1>(0, 8) = -velocitySkew_nframe.block<3, 1>(0, 2);
d.计算观测噪声R
Eigen::MatrixXd Hv;
Eigen::MatrixXd Rv =
paras_.odo_measurement_std.cwiseProduct(paras_.odo_measurement_std)
.asDiagonal();
e.进行ekf更新
计算速度误差状态 = Zv - wheelVelocity（状态 - 观测）
EKFUpdate(Zv, Hv, Rv)
void WheelINS::EKFUpdate(Eigen::MatrixXd &dz, Eigen::MatrixXd &H,
Eigen::MatrixXd &R) {
assert(H.cols() == Cov_.rows());
assert(dz.rows() == H.rows());
assert(dz.rows() == R.rows());
assert(dz.cols() == 1);
auto temp = H * Cov_ * H.transpose() + R;
Eigen::MatrixXd K = Cov_ * H.transpose() * temp.inverse();
Eigen::MatrixXd I;
I.resizeLike(Cov_);
I.setIdentity();
I = I - K * H;
delta_x_ = delta_x_ + K * (dz - H * delta_x_);
Cov_ = I * Cov_ * I.transpose() + K * R * K.transpose();
}
公式如下：
对照上述代码片段，更新后验协方差和常规ekf有点区别。其它都是按照ekf五公式完全套下来即可。
f.更新后验状态向量
因为使用的误差卡尔曼，所以每次进行误差更新后，都要对状态进行更新。
stateFeedback();
void WheelINS::stateFeedback() {
pvacur_.pos -= delta_x_.block(P_ID, 0, 3, 1);
pvacur_.vel -= delta_x_.block(V_ID, 0, 3, 1);
Vector3d delta_att = delta_x_.block(PHI_ID, 0, 3, 1);
Eigen::Quaterniond qpn = Rotation::rotvec2quaternion(delta_att);
pvacur_.att.qbn = qpn * pvacur_.att.qbn;
pvacur_.att.cbn = Rotation::quaternion2matrix(pvacur_.att.qbn);
pvacur_.att.euler = Rotation::matrix2euler(pvacur_.att.cbn);
imuerror_.gyrbias += delta_x_.block(BG_ID, 0, 3, 1);
imuerror_.accbias += delta_x_.block(BA_ID, 0, 3, 1);
imuerror_.gyrscale += delta_x_.block(SG_ID, 0, 3, 1);
imuerror_.accscale += delta_x_.block(SA_ID, 0, 3, 1);
delta_x_.setZero();
}
如代码片段，更新完后验状态pvacur(pos、vel、att(q, R, euler), acc/gyro bias acc/gyro scale)。再对误差状态清零delta_x_.setZero()。
C.核验协方差checkCov()
void checkCov() {
for (int i = 0; i < RANK; i++) {
if (Cov_(i, i) < 0) {
std::cout << "Covariance is negative at " << std::setprecision(10)
<< timestamp_ << " !" << std::endl;
std::exit(EXIT_FAILURE);
}
}
}
从代码片段可以看出，这里核验协方差判断Cov_对角线元数若小于0，直接结束进程。
处理方式也比较粗暴，没有做异常后，系统重启处理，我想可能是因为系统只有轮子上安装了imu，系统中没有其它传感器可做异常接管的处理操作。
（3）更新状态存储
处理wheelins.newImuProcess()完，将最新状态和协方差写入到yaml指定的存储路径文本中。
timestamp = wheelins.timestamp();
navstate = wheelins.getNavState();
cov = wheelins.getCovariance();
writeNavResult(timestamp, navstate, navfile, imuerrfile);
writeSTD(timestamp, cov, stdfile);
最后同步实时输出当前处理进度：
percent = int((imu_cur.timestamp - starttime) / interval * 100);
if (percent - lastpercent >= 1) {
std::cout << "Processing: " << std::setw(3) << percent << "%\r"
<< std::flush;
lastpercent = percent;
}
四、后处理工作
就是对之前打开的文件进行关闭处理，非常简单。
imufile.close();
navfile.close();
imuerrfile.close();
stdfile.close();
五、总结
这样整个代码过程我们就全部看完了，综合下来，代码写的还是很不错的，简单、明了。代码部分与论文的主要区别：
1.状态向量为pvacur(pos、vel、att(q, R, euler), acc/gyro bias acc/gyro scale)，没有考虑里程计比例因子误差和杠臂误差；
2.没有利用载体运动约束：零速更新和零速航向更新ZUPT/ZIHR。
参考文献
http://i2nav.cn/ueditor/jsp/upload/file/20210905/1630804325780076093.pdf
509
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
