粒子滤波用于机器人定位

0.引言
1.里程计
- 1.1.里程计模型
- 1.2.里程计运动模型
2.二维激光雷达的观测模型
- 2.1.光束模型
- 2.2.似然场模型
3.重采样
4.滤波结果
5.数据集

0.引言

参考链接。基本是基于概率机器人进行实现的，是一个很好的学习材料。本文只是自己根据参考博客一级相关资料的学习记录。

目标：已知地图去定位。

使用里程计运动模型进行粒子状态传播(驱动粒子运动)，使用激光测量评估粒子的权重(决定保留哪些粒子)，然后重采样。依次循环进行。

1.里程计

1.1.里程计模型

通过编码器信息计算里程计并发布。
[xyθ]t=[xyθ]t−1+[Δscos⁡(θt−1+Δθ/2)Δssin⁡(θt−1+Δθ/2)Δθ]\left[\begin{array}{l} x \\ y \\ \theta \end{array}\right]_{t}=\left[\begin{array}{l} x \\ y \\ \theta \end{array}\right]_{t-1}+\left[\begin{array}{c} \Delta s \cos (\theta_{t-1}+\Delta \theta / 2) \\ \Delta s \sin (\theta_{t-1}+\Delta \theta / 2) \\ \Delta \theta \end{array}\right] ⎣⎡xyθ⎦⎤t=⎣⎡xyθ⎦⎤t−1+⎣⎡Δscos(θt−1+Δθ/2)Δssin(θt−1+Δθ/2)Δθ⎦⎤{Δθ=Δsr−ΔslbΔs=Δsr+Δsl2,Δsl/r=kl/r⋅Δel/r\left\{\begin{array}{l} \Delta \theta=\frac{\Delta s_{r}-\Delta s_{l}}{b} \\ \Delta s=\frac{\Delta s_{r}+\Delta s_{l}}{2} \end{array}, \Delta s_{l / r}=k_{l / r} \cdot \Delta e_{l / r}\right. {Δθ=bΔsr−ΔslΔs=2Δsr+Δsl,Δsl/r=kl/r⋅Δel/rΔsl/r∼N(Δsl/r^,∥kΔsl/r^∥2)\Delta s_{l / r} \sim N\left(\widehat{\Delta s_{l / r}}, \quad\left\|k \widehat{\Delta s_{l / r}}\right\|^{2}\right) Δsl/r∼N(Δsl/r,∥∥∥kΔsl/r∥∥∥2)
kl/rk_{l / r}kl/r为左右轮系数，把编码器增量Δel/r\Delta e_{l/r}Δel/r转化为左右轮的位移， bbb 是轮间距。

//cal odometry
double delta_theta = (KR*delta_en_r - KL*delta_en_l) / B;
double delta_s = 0.5 *(KR*delta_en_r + KL*delta_en_l);
double delta_x = delta_s * cos(theta + 0.5*delta_theta);
double delta_y  = delta_s * sin(theta + 0.5*delta_theta);x = x + delta_x;
y = y + delta_y;
theta = theta + delta_theta;

1.2.里程计运动模型

void Localizer::motionUpdate ( const Pose2d& odom )
{const double MIN_DIST = 0.02; // TODO param 如果运动太近的话就不更新了，防止出现角度计算错误if ( !is_init_ ) //首次{last_odom_pose_ = odom;is_init_ = true;return;}/* 计算ut */// 根据里程计计算，运动模型中的（t-1,t]之间的运动参数double dx = odom.x_ - last_odom_pose_.x_;double dy = odom.y_ - last_odom_pose_.y_;double delta_trans = sqrt ( dx * dx + dy * dy );double delta_rot1 = atan2 ( dy,  dx ) - last_odom_pose_.theta_;Pose2d::NormAngle ( delta_rot1 );/* 处理纯旋转 */if(delta_trans < 0.01) delta_rot1 = 0;double delta_rot2 = odom.theta_ - last_odom_pose_.theta_ - delta_rot1;Pose2d::NormAngle ( delta_rot2 );for ( size_t i = 0; i < nParticles_; i ++ ){motion_model_->sampleMotionModelOdometry ( delta_rot1, delta_trans,delta_rot2, particles_.at ( i ).pose_ ); // for each particle}last_odom_pose_ = odom;// 以 odom 作为 ut 驱动粒子运动
}

实际上是一个航迹推算的过程：

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) );// 最后一个2表示平方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;/* 采样 */// rng_.gaussianN(0,sigma)函数 生成 均值为0，方差为sigma的值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);// Pose2d属性是publicxt.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_);
}

2.二维激光雷达的观测模型

这部分主要参考曾书格大佬PPT。

2.1.光束模型

把一条激光看做一个激光束，射到墙上时会有一个期望距离，在这个期望值的附近会形成一个高斯分布，这是正常的情况。ztk∗{z_t^{k}}^*ztk∗ 是期望值，如图中(a)所示。

如果有障碍物挡住了的话，也就是d>d1d>d_1d>d1时，这样会形成一个指数分布，如图中（b）所示。（c）(d)就对应freespace，没有接收到激光的反射，或只接收到一小部分。

上述情况基本就涵盖了激光的测量可能性。激光雷达的光束模型就是给定一个机器人的位置xtx_txt，和地图mmm，来获得这一帧观测值ztz_tzt的概率。默认认为每一个激光束都是线性无关的，是独立的。得到一个累计乘。

等式左边是这束激光的概率（之后都叫得分），反应的是当前这束激光与地图的重合程度，重合程度越好，得分也就越高，如果正好落在期望值的上面，那么就说这个得分就是1。

激光发出的一帧激光数据与地图的重合程度这个东西就是我们的光束模型，光束模型评估的就是这个。光束模型的缺点（基本没人用光束模型，因为有缺点）

机器人在非结构化环境中，位姿发生微小的变化，就会造成期望值发生巨大的变化，从而导致得分突变。

比如当前机器人正前方1m有障碍物，障碍物后面3m是一堵墙，激光发出激光束可以正常射到这个障碍物上，但是如果此时机器人的位姿发生微小变化，很有可能这个光束就射到障碍物后面的墙上去了，从而导致这个期望值发生突变。他是病态的，并且是无法进行优化的。所以说在复杂环境中不会使用这种

2.2.似然场模型

将所有的噪声，例如传感器噪声，机器人位姿噪声，等等…，得到一个高斯分布，用这个高斯分布对整个图像进行模糊，对障碍物进行高斯平滑，越白表示得分越高，越黑代表得分越低，不需要计算期望值，在对图像进行模糊之后，已经存在障碍物和模糊过的图像了，也就是说把之前计算期望值，然后用高斯分布算出一个得分换成查表的形式，查表的话，似然场从程序一开始就是在进行离线计算，所以对一帧激光来说，只需要查360次表，查表的计算量可以忽略不计。他是同时适合结构化环境和非结构化环境的。因为就算位姿发生一点改变，他的得分也不会太低。

似然域是预先计算好的。参数sigma非常重要，会影响到算法收敛效率，可以根据激光雷达的精度和建图的精度来定。原博主这里设为了0.25m。

这个demo中似然场计算结果：

σ=0.2\sigma = 0.2σ=0.2 (配置文件里面是设置的0.2)

σ=0.5\sigma = 0.5σ=0.5

相当于把分布给拉宽了。

MeasurementModel::MeasurementModel ( GridMap* map, const double& sigma, const double& rand ):
map_(map), sigma_(sigma), rand_(rand)
{/* 初始化似然域模型, 预先计算的似然域模型的格子大小比占有栅格小1倍 */// ?没看懂这里的地图尺寸为啥要扩大一倍？// size_x_ = 2 * map->size_x_;// size_y_ = 2 * map->size_y_;// init_x_ = 2 * map->init_x_;// init_y_ = 2 * map->init_y_; // cell_size_ = 0.5 * map->cell_size_;//网格大小size_x_ = map->size_x_;size_y_ = map->size_y_;init_x_ = map->init_x_;init_y_ = map->init_y_;cell_size_ =  map->cell_size_;//网格大小likelihood_data_.resize(size_x_ , size_y_); likelihood_data_.setZero();/* 构造障碍物的KD-Tree */// ref.blog: https://www.guyuehome.com/34095pcl::PointCloud<pcl::PointXY>::Ptr cloud (new pcl::PointCloud<pcl::PointXY>);for(int i = 0; i < map->size_x_;  i ++)for(int j = 0; j < map->size_y_; j ++){if(map->bel_data_(i, j) == 1.0) //是障碍物就加到KD数中{pcl::PointXY pt;pt.x = (i - map->init_x_) * map->cell_size_;pt.y = (j - map->init_y_) * map->cell_size_;cloud->push_back(pt);}}// 利用障碍物 cloud 构建kd-treekd_tree_.setInputCloud(cloud); /* 对每个格子计算likelihood */for(double i = 0; i < size_x_; i += 0.9)for(double j = 0; j < size_y_; j +=0.9){/* 计算double x, y */double x = ( i - init_x_)* cell_size_;double y = ( j- init_y_ ) * cell_size_;double likelihood =  likelihoodFieldRangFinderModel(x, y);setGridLikelihood(x, y, likelihood);}
}

double MeasurementModel::likelihoodFieldRangFinderModel ( const double& x, const double& y )
{/* 找到最近的距离 */pcl::PointXY search_point;search_point.x = x;search_point.y = y;// 两个向量 存储搜索到的k紧邻 一个存点的索引 一个存点的距离平方// 这里的 k=1 找最近的一个点就是std::vector<int> k_indices;std::vector<float> k_sqr_distances;int nFound =  kd_tree_.nearestKSearch(search_point, 1, k_indices, k_sqr_distances);double dist = k_sqr_distances.at(0);/* 高斯 + random */return gaussion(0.0, sigma_, dist) + rand_;
}
// 二维高斯分布
double MeasurementModel::gaussion ( const double& mu, const double& sigma, double x)
{return (1.0 / (sqrt( 2 * PI ) * sigma) ) * exp( -0.5 * (x-mu) * (x-mu) / (sigma* sigma) );
}

通过kdtree存储障碍物位置，然后利用高斯分布计算似然场(概率)，越白表示是障碍物的可能性越高。

构造好似然场模型后，对激光测量就进行查表获取观测值。

原文：

每个粒子都用所有的激光束计算权重，这里我们用的加法，没用乘法，因为乘法收敛太快了。另外直接把重采样写在了观测更新里，就是观测更新后直接重采样。这里并没有在每次观测更新后都执行重采样。因为，重采样频率太高容易导致粒子退化。这里我们采取了降频措施。其实最好是使用书中的Augmented + KLD方法，这里我偷了个懒，只用了最简单的重采样方法。

即是程序8.2的第二个for循环。也是下图中的第二个步骤。

 /* 获取激光的信息 */const double& ang_min = scan->angle_min;const double& ang_max = scan->angle_max;const double& ang_inc = scan->angle_increment;const double& range_max = scan->range_max;const double& range_min = scan->range_min;// 每个粒子都用当前所有的激光束计算权重for ( size_t np = 0; np < nParticles_; np ++ ){Particle& pt = particles_.at ( np );/* for every laser beam */for ( size_t i = 0; i < scan->ranges.size(); i ++ ){/* 获取当前beam的距离 */const double& R = scan->ranges.at ( i );if ( R > range_max || R < range_min )continue;double angle = ang_inc * i + ang_min;double cangle = cos ( angle );double sangle = sin ( angle );Eigen::Vector2d p_l (R * cangle,R * sangle); //在激光雷达坐标系下的坐标/* 转换到世界坐标系下 */Pose2d laser_pose = pt.pose_ * robot_->T_r_l_;Eigen::Vector2d p_w = laser_pose * p_l;/* 更新weight ： 在预先计算好的似然场中查表 */double likelihood = measurement_model_->getGridLikelihood ( p_w ( 0 ), p_w ( 1 ) );/* 整合多个激光beam用的加法， 用乘法收敛的太快 */// 所有光束的概率相加作为当前粒子的权重pt.weight_ += likelihood;}// for every laser beam} // for every particle/* 权重归一化 */normalization();

3.重采样

原文：
这里，我只实现了一个最基本的低方差重采样，必须采用大量的粒子才能保证正常的定位。有兴趣，大家最好实现一下书中Augmented MCL和KLD sampling MCL。

可以看出，低方差采样只在第三行进行了一次随机抽取，随后便等间隔（ M−1M^{-1}M−1）的抽取样本。类似于上图所示。就好像发扑克牌的过程，刚开始切牌那一下决定了你可以拿到什么牌，后面只是按照固定顺序抽取而已。

void Localizer::reSample()
{if ( !is_init_ )return;/* 重采样 */std::vector<Particle> new_particles;cv::RNG rng ( cv::getTickCount() );long double inc = 1.0 / nParticles_;long double r = rng.uniform ( 0.0, inc );long double c = particles_.at ( 0 ).weight_;int i = 0;for ( size_t m = 0; m < nParticles_; m ++ ){long double U = r + m * inc;while ( U > c ){i = i+1;c = c + particles_.at ( i ).weight_;}particles_.at ( i ).weight_ = inc;/* 新采样的粒子加了高斯，稍微增加一下多样性 */Particle new_pt = particles_.at(i);new_pt.pose_.x_ += rng.gaussian(0.02);new_pt.pose_.y_ += rng.gaussian(0.02);new_pt.pose_.theta_ += rng.gaussian(0.02);Pose2d::NormAngle(new_pt.pose_.theta_);new_particles.push_back ( new_pt );}particles_ = new_particles;
}

4.滤波结果

自己添加了滤波后的轨迹显示功能，前期容易跳变，可以在刚开始的时候采用里程计发布，滤波稳定后再使用滤波后的path进行显示。

5.数据集

看到数据集说明文档最后一张图片中的箭头表示，以为TlrT_{lr}Tlr误写为了TrlT_{rl}Trl，实际上没有。只是箭头表示和自己的习惯不一样，其他是一样的，具体体现在Localizer::measurementUpdate()函数求激光点在世界坐标系下的坐标：