Ros navigation功能包集源码详解(一) amcl

Navigation stack总体结构

  本文主要解读amcl包对蒙特卡洛定位算法的具体实现过程,主要关注源码中定义的数据类型,及程序的具体实现流程,对于详细的算法原理不作过多解释(主要是我不太懂,讲不好),想了解涉及到的理论知识推荐阅读<< Probabilistic Robotics >>

1.定位包amcl的 功能

  输入:地图,激光数据,里程计
  输出:位姿pose(x,y,θ),粒子集

2.源码文件结构

最为核心的是include目录和src目录,一共包含三个大类,map,pf,以及sensors,还有一个重要文件amcl_node.cpp
 1). map处理地图，pf是算法的核心粒子滤波(particle filter),sensors就是处理雷达跟里程计传感器的
 2). amcl_node.cpp，定义了一个可运行节点,定位过程主要在此文件中组织实现

3.主要数据类型与算法

3.1 pf

”
a).eig3(指头文件和源文件,下同)实现的是一个3x3对称矩阵的特征值与特征向量的计算,首先用Householder矩阵将矩阵变换为三对角矩阵,然后使用ql分解迭代计算
b).pf_kdtree定义了一个kdtree以及维护方法来管理所有粒子
c).pf_pdf主要定义了一个从给定pdf中采样粒子的方法
d).pf_vector定义了三维列向量和三维矩阵和基本的运算方法
e).pf定义了粒子单元pf_sample_t,粒子集pf_sample_set_t,粒子滤波pf_t的数据类型,还有一个 pf_cluster_t表示粒子集的聚类信息,关键函数主要包含如下三个,分别对应粒子滤波中的运动更新,观测更新,重采样三个过程

// Update the filter with some new action
void pf_update_action(pf_t *pf, pf_action_model_fn_t action_fn, void *action_data);

// Update the filter with some new sensor observation
void pf_update_sensor(pf_t *pf, pf_sensor_model_fn_t sensor_fn, void *sensor_data);

// Resample the distribution
void pf_update_resample(pf_t *pf);

3.2 sensors

a) amcl_sencor定义了基类,其成员函数都为虚函数,主要有如下两个,为两个派生类amcl_laser,amcl_odom提供统一接口

// Update the filter based on the action model.  Returns true if the filter
// has been updated.
public: virtual bool UpdateAction(pf_t *pf, AMCLSensorData *data);

// Update the filter based on the sensor model.  Returns true if the
// filter has been updated.
public: virtual bool UpdateSensor(pf_t *pf, AMCLSensorData *data);     

b) amcl_laser定义了激光数据类型,三种观测更新模型(详细见<<概率机器人>>),具体实现了UpdateSensor,用于计算粒子权值

//amcl_laser.h   
// Laser sensor data   
clas