1.程序运行
cd ~/OB_GINS
#编译好的可执行文件:./bin/ob_gins, 参数文件./dataset/ob_gins.yaml
./bin/ob_gins ./dataset/ob_gins.yaml
2.参数文件详解
# 数据文件,参考数据给的时间都是GPS周内秒,并未给定具体的GPS周。
imufile: "dataset/ADIS16465.txt"#惯导数据,惯导精度为战术级别。
gnssfile: "dataset/GNSS_RTK.pos"#GNSS数据,RTK定位结果。
outputpath: "dataset/"#文件输出路径
#滑窗大小设定为30s,这个参数有一篇英文论文进行过验证30s是精度和效率的最优选择。
#但是这个参数应该与载体运动的速度、IMU级别相关。之后博客将进行详细验证。
windows: 30
starttime: 357473#数据处理起始时间
endtime: 359090#数据处理结束时间
# 迭代次数
num_iterations: 20
# 进行GNSS粗差检测
is_outlier_culling: false
# 初始化信息
initvel: [ 0, 0, 0 ] # 北东地速度 (NED velocity), m/s
initatt: [ 0, 0, 276 ] # 横滚俯仰航向 (RPY attitude), deg
# IMU初始误差
initgb: [ 0, 0, 0 ] # 初始陀螺零偏 (gyroscope biases), deg/hr
initab: [ 0, 0, 0 ] # 初始加表零偏 (accelerometer biases), mGal
# IMU文件数据列数 (7-imu only, 8-with odometer),
#测试数据8列包含里程计数据,设置为7列则为纯惯导,并且原始数据是增量的形式。
imudatalen: 8
# IMU原始数据频率, Hz
imudatarate: 200
# 考虑地球自转补偿项
# consider the Earth's rotation
isearth: true
# 安装参数
# Installation parameters
antlever: [ -0.073, 0.302, 0.087 ] # 天线杆臂 (antenna lever), IMU前右下方向, m
odolever: [ -0.64, -0.628, 2.122 ] # 里程计杆臂 (odometer lever), IMU前右下方向, m
bodyangle: [ 0, -0.30, -1.09 ] # IMU到载体的旋转角 (mouting angles to construct C_b^v), deg
#里程计参数
odometer:
isuseodo: true # 是否使用里程计
std: [ 0.05, 0.05, 0.05 ] # 里程标准差 m/s
srw: 100 # 比例因子误差 , PPM
# IMU噪声参数
imumodel:
arw: 0.1 # deg/sqrt(hr)
vrw: 0.1 # m/s/sqrt(hr)
gbstd: 25.0 # deg/hr
abstd: 200.0 # mGal
corrtime: 1.0 # hr
# GNSS仿真中断配置
# GNSS outage parameters
isuseoutage: true
outagetime: 357900
outagelen: 60
outageperiod: 150
# 固定阈值GNSS抗差 (m)
gnssthreshold: 0.2
3.主程序
3.1 看主程序之前首先看编译文件(cmakelist),其他的先不用看主要看可执行文件的生成以及包含的相关代码文件。
file(GLOB_RECURSE SOURCE
src/ob_gins.cc
src/fileio/fileloader.cc
src/fileio/filesaver.cc
src/preintegration/preintegration_base.cc
src/preintegration/preintegration_normal.cc
src/preintegration/preintegration_earth.cc
src/preintegration/preintegration_odo.cc
src/preintegration/preintegration_earth_odo.cc)
include_directories(${PROJECT_SOURCE_DIR})
#生成可执行文件名字:${PROJECT_NAME}在本程序中为ob_gins
#相关代码文件: ${SOURCE}为上边的file中的.cc文件,一般主程序为第一个.cc文件
add_executable(${PROJECT_NAME} ${SOURCE})
注:如果想查看具体的变量名字可以使用下述语句:
message("变量名" ${PROJECT_NAME} )
3.2主程序ob_gins.cc
OB_GINS代码结构清晰,注释详细,因此本节主要对代码整体结构进行阐述,随后在此基础讲解具体程序实现思路。
主程序包含上述四个函数,具体含义如注释所示。代码的阅读直接从主函数开始,顺序阅读即可。
//main函数分为四部分,几乎主程序都可以如此划分
main()
{
1.数据参数预处理
主要功能:读取参数文件,在OB_GINS中读取了,滑动窗口次数、处理数据时间段、优化迭代次数、初始速度、位置、IMU相关参数等。
2.程序初始化
主要作用:通过上一步读取的数据进行系统初始化。这个初始化详细讲一下
(1)设置站心坐标系(北东地)原点,更新当地重力参数。
(2)设置预积分选项:是否添加里程计,是否考虑地球自转
(3)初始状态(当前状态)设置:北东地位置、姿态、速度、加速度计陀螺仪偏置、安装角。
3.循环处理数据
(1)imu预积分
(2)利用GNSS数据进行优化
4.结果输出
}
3.3IMU预积分
//加入IMU数据并进行预积分
preintegrationlist.back()->addNewImu(imu_cur);
void PreintegrationBase::addNewImu(const IMU &imu) {
imu_buffer_.push_back(imu);
integrationProcess(imu_buffer_.size() - 1);
}
integrationProcess()函数有很多个,具体选用与预积分选项设置相关,分为一下四种:
//imu预积分
if (options == PREINTEGRATION_NORMAL) {
preintegration = std::make_shared<PreintegrationNormal>(parameters, imu0, state);
}
// imu和里程计预积分
else if (options == PREINTEGRATION_ODO) {
preintegration = std::make_shared<PreintegrationOdo>(parameters, imu0, state);
}
// 考虑地球自转的imu预积分
else if (options == PREINTEGRATION_EARTH) {
preintegration = std::make_shared<PreintegrationEarth>(parameters, imu0, state);
}
// 考虑地球自转的IMU和里程计的预积分
else if (options == PREINTEGRATION_EARTH_ODO) {
preintegration = std::make_shared<PreintegrationEarthOdo>(parameters, imu0, state);
}
不考虑地球自转的预积分
// imu预积分
void PreintegrationNormal::integrationProcess(unsigned long index) {
IMU imu_pre = compensationBias(imu_buffer_[index - 1]);//获取零偏补偿后的速度,姿态增量
IMU imu_cur = compensationBias(imu_buffer_[index]);
// 预积分
integration(imu_pre, imu_cur);
// 更新系统状态雅克比和协方差矩阵
updateJacobianAndCovariance(imu_pre, imu_cur);
}
// 积分
void PreintegrationBase::integration(const IMU &imu_pre, const IMU &imu_cur) {
//!机械编排
// 区间时间累积
double dt = imu_cur.dt;
delta_time_ += dt;
end_time_ = imu_cur.time;
current_state_.time = imu_cur.time;
// 连续状态积分, 先位置速度再姿态
// 速度增量考虑旋转和划桨误差
Vector3d dvfb = imu_cur.dvel + 0.5 * imu_cur.dtheta.cross(imu_cur.dvel) +
1.0 / 12.0 * (imu_pre.dtheta.cross(imu_cur.dvel) + imu_pre.dvel.cross(imu_cur.dtheta));
Vector3d dvel = current_state_.q.toRotationMatrix() * dvfb + gravity_ * dt;//速度增量
current_state_.p += dt * current_state_.v + 0.5 * dt * dvel;
current_state_.v += dvel;
// 姿态圆锥误差补偿
Vector3d dtheta = imu_cur.dtheta + 1.0 / 12.0 * imu_pre.dtheta.cross(imu_cur.dtheta);
current_state_.q *= Rotation::rotvec2quaternion(dtheta);
current_state_.q.normalize();
//! 预积分
dvel = delta_state_.q.toRotationMatrix() * dvfb;
//位置速度增量
delta_state_.p += dt * delta_state_.v + 0.5 * dt * dvel;
delta_state_.v += dvel;
// 姿态增量
delta_state_.q *= Rotation::rotvec2quaternion(dtheta);
delta_state_.q.normalize();
}
机械编排,OB_GINS对于旋转误差和划桨误差处理方法是:“单子样+前一周期的方法”:
速度增量:
注:速度增量计算中,有害加速度可以分为两项1.载体运动和地球自转引起的哥氏加速度、载体运动造成的对地向心加速度和重力加速度。不考虑地球自转的预计积分中只考虑了重力加速度,(补充考虑地球自转的预积分中考虑了在此基础上考虑了哥氏加速度)
姿态增量:
注意这个并未考虑地球自转的影响,因此代码姿态更新时并未加入C^nn。
考虑地球自转的IMU预积分:
// 零偏补偿和积分
void PreintegrationEarth::integrationProcess(unsigned long index) {
//零偏补偿
IMU imu_pre = compensationBias(imu_buffer_[index - 1]);
IMU imu_cur = compensationBias(imu_buffer_[index]);
// 区间时间累积
double dt = imu_cur.dt;
delta_time_ += dt;
end_time_ = imu_cur.time;
current_state_.time = imu_cur.time;
//b系速度增量=速度增量+旋转误差+划桨误差 对应公式(6)速度积分,b系速度增量detv^b_fm
Vector3d dvfb = imu_cur.dvel + 0.5 * imu_cur.dtheta.cross(imu_cur.dvel) +
1.0 / 12.0 * (imu_pre.dtheta.cross(imu_cur.dvel) + imu_pre.dvel.cross(imu_cur.dtheta));
// 哥氏项和重力项对应公式(6)速度积分(g^w-2w^w_ie x v^w_bm-1)dt
Vector3d dv_cor_g = (gravity_ - 2.0 * iewn_.cross(current_state_.v)) * dt;
// 地球自转补偿项, 省去了enwn项
Vector3d dnn = -iewn_ * dt;//对应公式(7)
Quaterniond qnn = Rotation::rotvec2quaternion(dnn);
//对应公式(6)n系速度增量
Vector3d dvel = 0.5 * (Matrix3d::Identity() + qnn.toRotationMatrix()) * current_state_.q.toRotationMatrix() * dvfb + dv_cor_g;
// 前后历元平均速度计算当前位置
current_state_.p += dt * current_state_.v + 0.5 * dt * dvel;//相较于起点的当前位置
current_state_.v += dvel;//当前速度
// 缓存IMU时刻位置, 时间间隔为两个历元的间隔
pn_.emplace_back(std::make_pair(dt, current_state_.p));
// 姿态圆锥误差补偿
Vector3d dtheta = imu_cur.dtheta + 1.0 / 12.0 * imu_pre.dtheta.cross(imu_cur.dtheta);
//地球自转补偿
current_state_.q = qnn * current_state_.q * Rotation::rotvec2quaternion(dtheta);
current_state_.q.normalize();//当前姿态
// 预积分
// 起始时刻到当前时刻的地球自转等效旋转矢量
dnn = -(delta_time_ - 0.5 * dt) * iewn_;
// 公式(11)中的R^bk-1_w,
//q0_.inverse() * Rotation::rotvec2quaternion(dnn) * q0_地球自转补偿
// * delta_state_.q上一时刻到当前时刻的速度增量,转换到起点坐标系。
dvel = (q0_.inverse() * Rotation::rotvec2quaternion(dnn) * q0_ * delta_state_.q).toRotationMatrix()
* dvfb;//当前b系速度增量(去除划桨误差和旋转误差的速度增量)
// 到预积分起点的位置和速度增量
delta_state_.p += dt * delta_state_.v + 0.5 * dt * dvel;
delta_state_.v += dvel;
// 到预积分起点的姿态增量
delta_state_.q *= Rotation::rotvec2quaternion(dtheta);
delta_state_.q.normalize();
// 更新系统状态雅克比和协方差矩阵
updateJacobianAndCovariance(imu_pre, imu_cur);
}
机械编排:
预积分: