ROS导航源码包学习——AMCL
一、AMCL
1>算法原理：
AMCL算法根据订阅到的地图数据配合激光扫描特征，使用粒子滤波获取最佳定位点，该点称为Mp(point on map)，它是相对于地图map上的坐标，也就是base_link相对map上的坐标。
odom的原点是机器人启动时刻的位置（试想一下，在同一张地图上，你每次把机器人放的地方都不一样，所以odom的原点会在每次启动作业时都变一次的），而且因为传感器，打滑，空转等因素，会造成odom的数据有累计误差，所以还需要不停的修正odom坐标系在map中的位置。base_link->odom的tf转换信息是每时每刻都在发布的（因为里程计的数据在一直上报），所以它是已知的。
综合以上两点，AMCL 可以根据最佳粒子的位置以及base_link->odom的tf转换信息推算出odom->map的tf转换信息并发布到tf主题上。
m a p − > o d o m = m a p − > b a s e l i n k − b a s e l i n k − > o d o m map->odom = map->baselink - baselink->odom map−>odom=map−>baselink−baselink−>odom
自适应蒙特卡洛增强：
- 初始化粒子群
- 模拟粒子运动
- 计算粒子评分
- 粒子群重采样
2>源码解析：
i, map是处理地图的
/**************************************************************************
\* Basic map functions
**************************************************************************/
// Create a new (empty) map
map_t *map_alloc(void);
// Destroy a map
void map_free(map_t *map);
// Get the cell at the given point
map_cell_t *map_get_cell(map_t *map, double ox, double oy, double oa);
// Load an occupancy map
int map_load_occ(map_t *map, const char *filename, double scale, int negate);
// Load a wifi signal strength map
//int map_load_wifi(map_t *map, const char *filename, int index);
// Update the cspace distances
void map_update_cspace(map_t *map, double max_occ_dist);
/**************************************************************************
\* Range functions
**************************************************************************/
// Extract a single range reading from the map
double map_calc_range(map_t *map, double ox, double oy, double oa, double max_range);
/**************************************************************************
\* GUI/diagnostic functions
**************************************************************************/
// Draw the occupancy grid
void map_draw_occ(map_t *map, struct _rtk_fig_t *fig);
// Draw the cspace map
void map_draw_cspace(map_t *map, struct _rtk_fig_t *fig);
// Draw a wifi map
void map_draw_wifi(map_t *map, struct _rtk_fig_t *fig, int index);
/**************************************************************************
\* Map manipulation macros
**************************************************************************/
// Convert from map index to world coords
\#define MAP_WXGX(map, i) (map->origin_x + ((i) - map->size_x / 2) * map->scale)
\#define MAP_WYGY(map, j) (map->origin_y + ((j) - map->size_y / 2) * map->scale)
// Convert from world coords to map coords
\#define MAP_GXWX(map, x) (floor((x - map->origin_x) / map->scale + 0.5) + map->size_x / 2)
\#define MAP_GYWY(map, y) (floor((y - map->origin_y) / map->scale + 0.5) + map->size_y / 2)
// Test to see if the given map coords lie within the absolute map bounds.
\#define MAP_VALID(map, i, j) ((i >= 0) && (i < map->size_x) && (j >= 0) && (j < map->size_y))
// Compute the cell index for the given map coords.
\#define MAP_INDEX(map, i, j) ((i) + (j) * map->size_x)
ii, pf是算法的核心粒子滤波（particle filter）
-
kdtree
pf_kdtree将pose跟其权重保存在一个kdtree中，加速pose的查找跟一系列计算。kdtree是一个基本的数据结构，简单来说就是多个维度的二分法，作为对比，二叉树是单个维度的二分法。
-
pf
计算权重和重新采样
iii, sensors就是处理雷达跟里程计传感器的
iv, 在这些之上还有一个amcl_node.cpp，用来实现功能的聚合，对外提供简洁的接口。
- 构造时初始化,从参数服务器中获取数据初始化各类参数;(接收地图设置,gui显示发布频率,保存位姿到参数服务器频率,laser测距范围及其概率模型参数,odom概率模型参数,粒子滤波及kld重采样参数,从参数服务器获取初始位姿,然后初始化了订阅者,发布者,服务)
- 地图加载,两种方式(1.订阅/map话题2.请求服务得到地图),得到地图后也有个初始化过程(将消息类型的地图转换为定义的map类数据,统计free状态的栅格索引,从参数服务器获取位姿信息,并初始化粒子滤波器pf_,初始化odom模型参数,初始化laser模型参数)
- 粒子滤波,订阅laser_scan的回调函数中处理,得到结果后发布位姿和粒子集
- initialpose的回调,接收到初始位姿消息后,融入最新的里程改变,然后在该位姿附近重新生成粒子集
参考文献：
【1】一文搞懂HMM（隐马尔可夫模型）
【2】蒙特卡罗定位（Particle Filter Localization）