Navigation源码阅读之robot_pose_ekf

卡尔曼滤波器是传感器融合的利器,而navigation源码中的对于小车感知这一块的处理也是依靠EKF来实现,我们来阅读一下这个包的实现方法,以及网上的贝叶斯滤波的官方API,在实际使用时,可以更快地定位错误与改写。

一、首先看看cmakelists,这个包只生成一个节点,主函数在odom_estimation_node.cpp中,定位到OdomEstimationNode类的构造函数,这里有一溜的param,我们在做小车时一般用里程计、IMU、视觉里程计进行定位,这三个默认是true,另外滤波器的刷新频率设为30Hz,也绰绰有余了,一般传感器的信息频率在100~1000Hz左右。

my_filter_.setOutputFrame(output_frame_);
my_filter_.setBaseFootprintFrame(base_footprint_frame_);

timer_ = nh_private.createTimer(ros::Duration(1.0/max(freq,1.0)),&OdomEstimationNode::spin, this);

我们一般将这两个frame设为odom与base_link,这样就可以通过EKF代替base_controller发送odom的tf变换。另外,用手写一个函数OdomEstimationNode::spin而不是一般见到的ros自带的spin函数,用来检查各个传感器的使用状态,以及发布我们需要的topic和tf,这个函数将以30Hz的频率运行。

二、OdomEstimationNode::odomCallback,这一系列的传感器回调函数,不同的传感器的数据维度与协方差矩阵尺寸不同,就拿里程计来说,协方差矩阵为6*6。

Quaternion q;
tf::quaternionMsgToTF(odom->pose.pose.orientation, q);
odom_meas_  = Transform(q, Vector3(odom->pose.pose.position.x, odom->pose.pose.position.y, 0));
for (unsigned int i=0; i<6; i++)
 for (unsigned int j=0; j<6; j++)
   odom_covariance_(i+1, j+1) = odom->pose.covariance[6*i+j];

my_filter_.addMeasurement(StampedTransform(odom_meas_.inverse(), odom_stamp_, base_footprint_frame_, "wheelodom"), odom_covariance_);

在这个代码块它调用了OdomEstimation::addMeasurement函数,它位于odom_estimation.cpp中: 

1.OdomEstimation::addMeasurement这个函数其实就是把组装好的transform发出去,它将在之后被OdomEstimation::update捕获并进行下一次的状态估计。

三、OdomEstimationNode::spin函数,这是整个包的输出信息的函数,一开始需要对时间戳进行检查:

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);
}

这个错误的出现概率不算太大,除非把传感器频率设得太低,或者直接断开连接了。接着直接对小车的状态进行估计并发送tf变换:

if ( my_filter_.isInitialized() )
{
  bool diagnostics = true;
  if (my_filter_.update(odom_active_, imu_active_,gps_active_, vo_active_,  filter_stamp_, diagnostics))
  {
    my_filter_.getEstimate(output_);
    pose_pub_.publish(output_);
    ekf_sent_counter_++;

    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_));

...

 上面的代码段调用了OdomEstimation类的多个函数,我们依次进行分析:

1.先看看OdomEstimation的构造函数,下面的代码段是系统的概率模型。各变量的意义是:pdf是条件概率密度函数,即probability density function,model是概率密度模型,sysNoise_Mu是系统的噪声均值,sysNoise_Cov是协方差矩阵。在这里将协方差矩阵的对角线都赋成10^-6。

ColumnVector sysNoise_Mu(6);  sysNoise_Mu = 0;
SymmetricMatrix sysNoise_Cov(6); sysNoise_Cov = 0;
for (unsigned int i=1; i<=6; i++) sysNoise_Cov(i,i) = pow(1000.0,2);
Gaussian system_Uncertainty(sysNoise_Mu, sysNoise_Cov);
sys_pdf_   = new NonLinearAnalyticConditionalGaussianOdo(system_Uncertainty);
sys_model_ = new AnalyticSystemModelGaussianUncertainty(sys_pdf_);

相比系统模型,传感器模型直接使用线性模型进行估计, 首先创建一个高斯分布的观测噪声,再加入到里程计的概率密度函数中,在这里的噪声协方差比系统初始值要大很多。另外,Hodom指的是里程计自身的噪声。接下来在各个传感器模型中流程均相同。

ColumnVector measNoiseOdom_Mu(6);  measNoiseOdom_Mu = 0;
SymmetricMatrix measNoiseOdom_Cov(6);  measNoiseOdom_Cov = 0;
for (unsigned int i=1; i<=6; i++) measNoiseOdom_Cov(i,i) = 1;
Gaussian measurement_Uncertainty_Odom(measNoiseOdom_Mu, measNoiseOdom_Cov);
Matrix Hodom(6,6);  Hodom = 0;
Hodom(1,1) = 1;    Hodom(2,2) = 1;    Hodom(6,6) = 1;
odom_meas_pdf_   = new LinearAnalyticConditionalGaussian(Hodom, measurement_Uncertainty_Odom);
odom_meas_model_ = new LinearAnalyticMeasurementModelGaussianUncertainty(odom_meas_pdf_);

2.OdomEstimation::initialize函数建立了先验概率模型。prior_Mu是先验概率的均值,prior_Cov是协方差矩阵,那么一开始的先验概率均值由odom_meas_提供。

ColumnVector prior_Mu(6);
decomposeTransform(prior, prior_Mu(1), prior_Mu(2), prior_Mu(3), prior_Mu(4), prior_Mu(5), prior_Mu(6));

 用这个先验概率新初始化一个EKF模型,将先验概率记下来,在update函数中会用于下一次的更新。

prior_  = new Gaussian(prior_Mu,prior_Cov);
filter_ = new ExtendedKalmanFilter(prior_);

addMeasurement(StampedTransform(prior, time, output_frame_, base_footprint_frame_));
filter_estimate_old_vec_ = prior_Mu;
filter_estimate_old_ = prior;
filter_time_old_     = time;

 3.OdomEstimation::update函数用于滤波器的实际更新。首先加上系统噪声,再依次将传感器的数据传入滤波器中予以状态估计。下面这一段指的是将里程计的数据转化为在水平面上的相对数据,加入传感器噪声之后更新协方差矩阵,最终给里程计模型更新这次的测量值。

if (odom_initialized_)
{
  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));
      	
  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);
}

在这里有一句 filter_->Update(odom_meas_model_, odom_rel);查看头文件就可以知道这是EKF提供的接口,我们跳到贝叶斯滤波库的BFL::ExtendedKalmanFilter中看看程序是怎么写的。网页版链接http://docs.ros.org/jade/api/bfl/html/annotated.html

 (1)在这里对sysmodel进行了强制转换,转到AnalyticSystemModelGaussianUncertainty这个类就可以知道,我们用AnalyticSystemModelGaussianUncertainty的成员函数把扩展卡尔曼滤波模型在当前状态下进行线性化:

\[ F = \frac{df}{dx} \mid_{u,x} \]

 那么接下来就是KF模型的估计了。

void ExtendedKalmanFilter::SysUpdate(SystemModel<ColumnVector>* const sysmodel,const ColumnVector& u)
{
   _x = _post->ExpectedValueGet();
   _J = ((AnalyticSys*)sysmodel)->PredictionGet(u,_x);
   _F = ((AnalyticSys*)sysmodel)->df_dxGet(u,_x);
   _Q = ((AnalyticSys*)sysmodel)->CovarianceGet(u,_x);
 
   CalculateSysUpdate(_J, _F, _Q);
}

(2)这一段实际上就是用状态转移矩阵F,后验估计的协方差矩阵,系统噪声的协方差矩阵Q计算得到预测值和真实值之间误差的协方差矩阵。

void KalmanFilter::CalculateSysUpdate(const ColumnVector& J, const Matrix& F, const SymmetricMatrix& Q)
{
  _Sigma_temp = F * ( (Matrix)_post->CovarianceGet() * F.transpose());
  _Sigma_temp += (Matrix)Q;
  _Sigma_temp.convertToSymmetricMatrix(_Sigma_new);

  PostMuSet   ( J );
  PostSigmaSet( _Sigma_new );
}

4.再回到OdomEstimation::update,当各传感器的数据均在滤波器内更新后,开始获取本轮的状态估计。

filter_estimate_old_vec_ = filter_->PostGet()->ExpectedValueGet();
tf::Quaternion q;
q.setRPY(filter_estimate_old_vec_(4), filter_estimate_old_vec_(5),filter_estimate_old_vec_(6));
filter_estimate_old_ = Transform(q,Vector3(filter_estimate_old_vec_(1), filter_estimate_old_vec_(2), filter_estimate_old_vec_(3)));
filter_time_old_ = filter_time;
addMeasurement(StampedTransform(filter_estimate_old_, filter_time,output_frame_, base_footprint_frame_));

重新覆盖filter_estimate_old_vec_作为下一次的先验,并且把估计的系统状态用transform的形式传给getEstimate,那么OdomEstimationNode::spin函数便可接收到并发出本次估计的tf变换。

用robot_pose_ekf包进行融合之后,tf树应该是由robot_pose_ekf发出的odom指向base_link。

 

  • 6
    点赞
  • 44
    收藏
    觉得还不错? 一键收藏
  • 6
    评论
navigation2/smac_planner_lattice是ROS2的一个插件,用于路径规划和导航。下面是对其码的简要解析。 首先介绍一下码的目录结构。smac_planner_lattice包含了几个主要的文件夹和文件。config文件夹包含了一些配置文件,可以在其中进行一些参数的设置。include文件夹包含了一些头文件,这些头文件定义了插件的一些类和函数。src文件夹包含了插件的代码文件,其中包括了插件的主要逻辑。launch文件夹包含了一些launch文件,用于启动插件。scripts文件夹包含了一些辅助的脚本文件。test文件夹包含了一些测试文件和测试用例。 在代码的主要逻辑部分,主要包含了几个类和函数。其中的Planner类是插件的核心类,它实现了路径规划器的主要功能。首先,它会根据收到的地图、起点和终点等信息进行初始化。然后,它会使用一些算法来搜索最佳路径,其中包括了离散Lattice规划算法。在搜索过程中,它会考虑一些约束,例如机器人的最大速度、转弯半径等。最后,它会生成一条可行的路径,并将其发布出去。 除了Planner类之外,还有一些辅助的类和函数。例如,CollisionChecker类用于检测路径上是否有障碍物。Costmap类用于处理和更新地图信息。MotionValidator类用于验证运动的合法性。这些类和函数共同协作,实现了路径规划和导航的功能。 总结来说,navigation2/smac_planner_lattice是一个用于路径规划和导航的ROS2插件。它的码包含了一些关键的类和函数,通过使用一些算法和约束来计算并生成一条可行的路径。这个插件在ROS2导航堆栈中起到了重要的作用,可以帮助机器人在复杂环境中完成自主导航。
评论 6
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值