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
自适应蒙特卡洛增强:
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,用来实现功能的聚合,对外提供简洁的接口。
参考文献:
【1】一文搞懂HMM(隐马尔可夫模型)
【2】蒙特卡罗定位(Particle Filter Localization)