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主题上。
$$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）