蒙特卡洛粒子滤波定位算法_[PR-2] PF 粒子滤波/蒙特卡罗定位

Probabilistic Robotics《概率机器人》(PR)是一本非常经典的介绍移动机器人技术的书；但是这本书主要注重理论，而对于实现只给出了伪代码；在PR系列文章中，我根据自己的理解，对其中的一些方法进行了复现。

本文主要介绍如何在Grid Map/栅格地图中利用Particle Filter 定位。也就是说我们是已知地图去定位，本文中的地图是预先利用gmapping算法构建好的。先看一下定位效果。粒子滤波定位https://www.zhihu.com/video/1020748273266536448

1. 算法流程

基本理论可以参考《概率机器人》第8章。

算法有两个关键步骤：1)更新粒子，2)重采样。

2. 更新粒子

更新粒子涉及到两个模型，一个是运动模型，一个是观测模型。

运动模型

我们使用第5章的基于里程计的采样算法：sample_motion_model_odometry。

实现代码如下，函数的输入是 , 可由两次里程计位姿的差计算。需要注意的是，书上的算法没有考虑机器人纯旋转和后退的情况，必须做一些判断。

#include

MotionModel::MotionModel ( const double& alpha1, const double& alpha2, const double& alpha3, const double& alpha4 ):

alpha1_(alpha1), alpha2_(alpha2), alpha3_(alpha3), alpha4_(alpha4)

{

rng_ = cv::RNG(cv::getTickCount());

}

void MotionModel::sampleMotionModelOdometry ( const double& delta_rot1, const double& delta_trans, const double& delta_rot2, Pose2d& xt )

{

/* 处理后退的问题 */

double delta_rot1_PI = delta_rot1 - PI;

double delta_rot2_PI = delta_rot2 - PI;

Pose2d::NormAngle(delta_rot1_PI);

Pose2d::NormAngle(delta_rot2_PI);

double delta_rot1_noise = std::min(fabs(delta_rot1), fabs( delta_rot1_PI) );

double delta_rot2_noise = std::min(fabs(delta_rot2), fabs( delta_rot2_PI) );

double delta_rot1_2 = delta_rot1_noise*delta_rot1_noise;

double delta_rot2_2 = delta_rot2_noise * delta_rot2_noise;

double delta_trans_2 = delta_trans * delta_trans;

/* 采样 */

double delta_rot1_hat = delta_rot1 - rng_.gaussian(alpha1_ * delta_rot1_2 + alpha2_ * delta_trans_2);

double delta_trans_hat = delta_trans - rng_.gaussian(alpha3_ * delta_trans_2 + alpha4_ * delta_rot1_2 + alpha4_ * delta_rot2_2);

double delta_rot2_hat = delta_rot2 - rng_.gaussian(alpha1_ * delta_rot2_2 + alpha2_ * delta_trans_2);

xt.x_ += delta_trans_hat * cos( xt.theta_ + delta_rot1_hat );

xt.y_ += delta_trans_hat * sin( xt.theta_ + delta_rot1_hat );

xt.theta_ += (delta_rot1_hat + delta_rot2_hat);

xt.NormAngle(xt.theta_);

}

其中比较重要的是四个噪声参数 ，理论上应该通过统计获得，但是我们这里就直接设置了。设得越大，表明里程计越不精确，更新时越容易使粒子发散。

运动更新

void Localizer::motionUpdate (