本文对robot_pose_ekf中的odom和IMU融合部分作review，主要关注以下几点：
- 时间戳如何同步
- odom和IMU是如何融合的
- 代码如何设置协方差矩阵
- 代码如何让处理IMU和base的外参
- 代码最终没有输出加速度和角速度信息，如何修改代码输出加速度角速度？
声明：本文中的代码只复制了源码中与上述问题相关的部分，想了解更多可以去看看源码
1. 时间戳如何同步
总体来说，假设只使用Odom和IMU两种传感器，在程序运行时候，会有三个线程：
- odomcallback
- imucallback
- 数据融合线程（filter）
声明如下所示：
//odomcallback
odom_sub_ = nh.subscribe("odom", 10, &OdomEstimationNode::odomCallback, this);
//imucallback
imu_sub_ = nh.subscribe("imu_data", 10, &OdomEstimationNode::imuCallback, this);
//spin线程，以freq的频率进行融合
timer_ = nh_private.createTimer(ros::Duration(1.0/max(freq,1.0)), &OdomEstimationNode::spin, this);
callback线程接受并尝试激活传感器数据，数据融合线程使用已经激活数据对估计器进行更新
1.1先来看odomcallback
总体来说，同步的过程如下:
- 记录odom接收时间
- 向估计器放入odom的估计
- 如果odom没被激活，尝试激活
- 记录初始化的时间
- 当filter的时间戳晚于初始化odom的时间，初始化成功，odom被激活
void OdomEstimationNode::odomCallback(const OdomConstPtr& odom)
{
odom_callback_counter_++;
ROS_DEBUG("Odom callback at time %f ", ros::Time::now().toSec());
assert(odom_used_);
// receive data
odom_stamp_ = odom->header.stamp;
//1.记录odom接收时间
odom_time_ = Time::now();
//2.向估计器放入odom的估计
my_filter_.addMeasurement(StampedTransform(odom_meas_.inverse(), odom_stamp_, base_footprint_frame_, "wheelodom"), odom_covariance_);
// activate odom
if (!odom_active_) {
//3.如果odom没被激活，尝试激活
if (!odom_initializing_){
odom_initializing_ = true;
//4.记录初始化的时间
odom_init_stamp_ = odom_stamp_;
ROS_INFO("Initializing Odom sensor");
}
//5.当filter的时间戳晚于初始化odom的时间，初始化成功，odom被激活
if ( filter_stamp_ >= odom_init_stamp_){
odom_active_ = true;
odom_initializing_ = false;
ROS_INFO("Odom sensor activated");
}
else ROS_DEBUG("Waiting to activate Odom, because Odom measurements are still %f sec in the future.",
(odom_init_stamp_ - filter_stamp_).toSec());
}
}
};
1.2再来看imuCallback
总体流程如下：
6. 记录odom接收时间
7. 检查是否设定了base到imu的外参
8. 向估计器放入odom的估计
9. 如果odom没被激活，尝试激活
10. 记录初始化的时间
11. 当filter的时间戳晚于初始化odom的时间，初始化成功，odom被激活
void OdomEstimationNode::imuCallback(const ImuConstPtr& imu)
{
imu_callback_counter_++;
ROS_INFO("Odom callback at time %f ", ros::Time::now().toSec());
assert(imu_used_);
// receive data
imu_stamp_ = imu->header.stamp;
tf::Quaternion orientation;
quaternionMsgToTF(imu->orientation, orientation);
imu_meas_ = tf::Transform(orientation, tf::Vector3(0,0,0));
for (unsigned int i=0; i<3; i++)
for (unsigned int j=0; j<3; j++)
imu_covariance_(i+1, j+1) = imu->orientation_covariance[3*i+j];
// Transforms imu data to base_footprint frame
if (!robot_state_.waitForTransform(base_footprint_frame_, imu->header.frame_id, imu_stamp_, ros::Duration(0.5))){
// warn when imu was already activated, not when imu is not active yet
if (imu_active_)
ROS_ERROR("Could not transform imu message from %s to %s", imu->header.frame_id.c_str(), base_footprint_frame_.c_str());
else if (my_filter_.isInitialized())
ROS_WARN("Could not transform imu message from %s to %s. Imu will not be activated yet.", imu->header.frame_id.c_str(), base_footprint_frame_.c_str());
else
ROS_DEBUG("Could not transform imu message from %s to %s. Imu will not be activated yet.", imu->header.frame_id.c_str(), base_footprint_frame_.c_str());
return;
}
StampedTransform base_imu_offset;
robot_state_.lookupTransform(base_footprint_frame_, imu->header.frame_id, imu_stamp_, base_imu_offset);
imu_meas_ = imu_meas_ * base_imu_offset;
imu_time_ = Time::now();
// manually set covariance untile imu sends covariance
if (imu_covariance_(1,1) == 0.0){
SymmetricMatrix measNoiseImu_Cov(3); measNoiseImu_Cov = 0;
measNoiseImu_Cov(1,1) = pow(0.00017,2); // = 0.01 degrees / sec
measNoiseImu_Cov(2,2) = pow(0.00017,2); // = 0.01 degrees / sec
measNoiseImu_Cov(3,3) = pow(0.00017,2); // = 0.01 degrees / sec
imu_covariance_ = measNoiseImu_Cov;
}
my_filter_.addMeasurement(StampedTransform(imu_meas_.inverse(), imu_stamp_, base_footprint_frame_, "imu"), imu_covariance_);
// activate imu
if (!imu_active_) {
if (!imu_initializing_){
imu_initializing_ = true;
imu_init_stamp_ = imu_stamp_;
ROS_INFO("Initializing Imu sensor");
}
if ( filter_stamp_ >= imu_init_stamp_){
imu_active_ = true;
imu_initializing_ = false;
ROS_INFO("Imu sensor activated");
}
else ROS_DEBUG("Waiting to activate IMU, because IMU measurements are still %f sec in the future.",
(imu_init_stamp_ - filter_stamp_).toSec());
}
if (debug_){
// write to file
double tmp, yaw;
imu_meas_.getBasis().getEulerYPR(yaw, tmp, tmp);
imu_file_ <<fixed<<setprecision(5)<<ros::Time::now().toSec()<<" "<< yaw << endl;
}
};
##1.3最后看数据融合线程（spin）
主要流程如下:
- 记录filter的时间戳
- 判断：如果现在的时间和传感器当前时间戳相差太远，就使传感器休眠（相应的active置为0）。
- 如果有传感器被激活，就把当前filter时间置为传感器时间
void OdomEstimationNode::spin(const ros::TimerEvent& e)
{
ROS_DEBUG("Spin function at time %f", ros::Time::now().toSec());
// check for timing problems
if ( (odom_initializing_ || odom_active_) && (imu_initializing_ || imu_active_) ){
double diff = fabs( Duration(odom_stamp_ - imu_stamp_).toSec() );
if (diff > 1.0) ROS_ERROR("Timestamps of odometry and imu are %f seconds apart.", diff);
}
// initial value for filter stamp; keep this stamp when no sensors are active
filter_stamp_ = Time::now();
// check which sensors are still active
if ((odom_active_ || odom_initializing_) &&
(Time::now() - odom_time_).toSec() > timeout_){
odom_active_ = false; odom_initializing_ = false;
ROS_INFO("Odom sensor not active any more");
}
if ((imu_active_ || imu_initializing_) &&
(Time::now() - imu_time_).toSec() > timeout_){
imu_active_ = false; imu_initializing_ = false;
ROS_INFO("Imu sensor not active any more");
}
if ((vo_active_ || vo_initializing_) &&
(Time::now() - vo_time_).toSec() > timeout_){
vo_active_ = false; vo_initializing_ = false;
ROS_INFO("VO sensor not active any more");
}
if ((gps_active_ || gps_initializing_) &&
(Time::now() - gps_time_).toSec() > timeout_){
gps_active_ = false; gps_initializing_ = false;
ROS_INFO("GPS sensor not active any more");
}
// only update filter when one of the sensors is active
if (odom_active_ || imu_active_ || vo_active_ || gps_active_){
// update filter at time where all sensor measurements are available
if (odom_active_) filter_stamp_ = min(filter_stamp_, odom_stamp_);
if (imu_active_) filter_stamp_ = min(filter_stamp_, imu_stamp_);
if (vo_active_) filter_stamp_ = min(filter_stamp_, vo_stamp_);
if (gps_active_) filter_stamp_ = min(filter_stamp_, gps_stamp_);
// update filter
if ( my_filter_.isInitialized() ) {
bool diagnostics = true;
if (my_filter_.update(odom_active_, imu_active_,gps_active_, vo_active_, filter_stamp_, diagnostics)){
// output most recent estimate and relative covariance
my_filter_.getEstimate(output_);
pose_pub_.publish(output_);
ekf_sent_counter_++;
// broadcast most recent estimate to TransformArray
StampedTransform tmp;
my_filter_.getEstimate(ros::Time(), tmp);
if(!vo_active_ && !gps_active_)
tmp.getOrigin().setZ(0.0);
odom_broadcaster_.sendTransform(StampedTransform(tmp, tmp.stamp_, output_frame_, base_footprint_frame_));
};
2. odom和IMU是如何融合的
在内部有一个估计器OdomEstimation my_filter_，这是一个卡尔曼滤波的估计器，我们知道卡尔曼滤波有预测和更新两个部分，我们分这两个部分来看。
2.1预测
预测值在初始化时给定，内部有好几种初始化方式，在这里着重看odom的初始化方式，可以看到在一开始就给定了预测初始值
else if ( odom_active_ && !gps_used_ && !my_filter_.isInitialized()){
my_filter_.initialize(odom_meas_, odom_stamp_);
ROS_INFO("Kalman filter initialized with odom measurement");
}
void OdomEstimation::initialize(const Transform& prior, const Time& time)
{
// set prior of filter
ColumnVector prior_Mu(6);
decomposeTransform(prior, prior_Mu(1), prior_Mu(2), prior_Mu(3), prior_Mu(4), prior_Mu(5), prior_Mu(6));
SymmetricMatrix prior_Cov(6);
for (unsigned int i=1; i<=6; i++) {
for (unsigned int j=1; j<=6; j++){
if (i==j) prior_Cov(i,j) = pow(0.001,2);
else prior_Cov(i,j) = 0;
}
}
prior_ = new Gaussian(prior_Mu,prior_Cov);
filter_ = new ExtendedKalmanFilter(prior_);
// remember prior
addMeasurement(StampedTransform(prior, time, output_frame_, base_footprint_frame_));
filter_estimate_old_vec_ = prior_Mu;
filter_estimate_old_ = prior;
filter_time_old_ = time;
// filter initialized
filter_initialized_ = true;
}
2.2 更新
// process odom measurement
// ------------------------
ROS_DEBUG("Process odom meas");
if (odom_active){
if (!transformer_.canTransform(base_footprint_frame_,"wheelodom", filter_time)){
ROS_ERROR("filter time older than odom message buffer");
return false;
}
transformer_.lookupTransform("wheelodom", base_footprint_frame_, filter_time, odom_meas_);
if (odom_initialized_){
// convert absolute odom measurements to relative odom measurements in horizontal plane
Transform odom_rel_frame = Transform(tf::createQuaternionFromYaw(filter_estimate_old_vec_(6)),
filter_estimate_old_.getOrigin()) * odom_meas_old_.inverse() * odom_meas_;
ColumnVector odom_rel(6);
decomposeTransform(odom_rel_frame, odom_rel(1), odom_rel(2), odom_rel(3), odom_rel(4), odom_rel(5), odom_rel(6));
angleOverflowCorrect(odom_rel(6), filter_estimate_old_vec_(6));
// update filter
odom_meas_pdf_->AdditiveNoiseSigmaSet(odom_covariance_ * pow(dt,2));
ROS_DEBUG("Update filter with odom measurement %f %f %f %f %f %f",
odom_rel(1), odom_rel(2), odom_rel(3), odom_rel(4), odom_rel(5), odom_rel(6));
filter_->Update(odom_meas_model_, odom_rel);
diagnostics_odom_rot_rel_ = odom_rel(6);
}
else{
odom_initialized_ = true;
diagnostics_odom_rot_rel_ = 0;
}
odom_meas_old_ = odom_meas_;
}
// sensor not active
else odom_initialized_ = false;
3. 如何设置协方差矩阵
在传感器各自的callback里设定，以imu为例
void OdomEstimationNode::imuCallback(const ImuConstPtr& imu)
{
imu_time_ = Time::now();
// manually set covariance untile imu sends covariance
if (imu_covariance_(1,1) == 0.0){
SymmetricMatrix measNoiseImu_Cov(3); measNoiseImu_Cov = 0;
measNoiseImu_Cov(1,1) = pow(0.00017,2); // = 0.01 degrees / sec
measNoiseImu_Cov(2,2) = pow(0.00017,2); // = 0.01 degrees / sec
measNoiseImu_Cov(3,3) = pow(0.00017,2); // = 0.01 degrees / sec
imu_covariance_ = measNoiseImu_Cov;
}
};
4. 如何让处理外参
该项目是默认小车坐标中心是和odom一致的，因此odom的外参不需要处理。而IMU的外参在使用这个代码的时候需要自己传入。
5. 最终没有输出加速度和角速度信息，如何修改代码输出加速度角速度？
加速度角速度可以使用imu的加速度和角速度，z值则可由imu的积分结果指定。
扫一扫
3万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
