栅格地图更新过程
首先来了一帧雷达数据,对应到每一个栅格点,即观测得到该栅格点是occupied或者是Free。
在cartographer中,使用CorrespondenceCostValue(整数表示的空闲概率)表示栅格状态,所以现在的目的就是如何通过传感器的测量值(occupied/free)对栅格的空闲概率进行更新。
栅格分为miss栅格和hit栅格
栅格更新过程:
栅格的当前状态CorrespondenceCostValue为[0, 1~32768]
变量定义:
probability: 栅格被占用的概率
odds:占用与空闲的比率
orrespondenceCost:栅格是free的概率(orrespondenceCost + probability = 1)
当上一状态为unknown时,来了一个新状态为occupied,概率默认为0.55
当上一状态为已知时,定义上一时刻的栅格空闲概率为M_old[0, 0.1~0.9],栅格的概率更新公式如下。
在代码中如此实现
CorrespondenceCostToValue(
ProbabilityToCorrespondenceCost(ProbabilityFromOdds(
odds * Odds(CorrespondenceCostToProbability(
(*kValueToCorrespondenceCost)[cell]))))) +
kUpdateMarker);
odds 为odds(p_hit)
kValueToCorrespondenceCost 将整数[1~32768]映射到浮点数[0.1~0.9]
CorrespondenceCostToProbability 将空闲概率转化为占用概率
ProbabilityFromOdds odds逆运算,将odds转化为占用概率
ProbabilityToCorrespondenceCost 将占用概率转化为空闲概率
CorrespondenceCostToValue 将浮点概率映射到整数空闲概率
在本段中,使用了公式进行了栅格概率的更新,无非就是使用上一时刻的空闲概率[0, 1~32768],结合当前的hit概率,得到当前的空闲概率[0, 1~32768],hit的概率是固定值(0.55),那么就可以建立一张表,当上一时刻的空间概率为[0, 1~32768]任意值时,能快速查到到当前空闲概率。
miss栅格的更新过程同理。
使用查找表加速栅格更新过程
在cartographer中,采用了时间换空间的策略,使用了两张查找表,hit_table_, miss_table.。这两张表也称概率更新表,概率更新表存储着更新后的值,而对应该行的索引就是更新前的值。
- hit_table_解决新来测量值是occupied时,如何更新该点的CorrespondenceCostValue
- miss_table_解决新来测量值是free时,如何更新该点的CorrespondenceCostValue
1. 查找表初始化
查找表的更新过程就是把上一步的栅格更新过程的所有情况都走一遍。
// 将栅格是未知状态与odds状态下, 将更新时的所有可能结果预先计算出来
std::vector<uint16> ComputeLookupTableToApplyCorrespondenceCostOdds(
float odds) {
std::vector<uint16> result;
result.reserve(kValueCount); // 32768
// 当前cell是unknown情况下直接把odds转成value存进来
result.push_back(CorrespondenceCostToValue(ProbabilityToCorrespondenceCost(
ProbabilityFromOdds(odds))) +
kUpdateMarker); // 加上kUpdateMarker作为一个标志, 代表这个栅格已经被更新了
// 计算更新时 从1到32768的所有可能的 更新后的结果
for (int cell = 1; cell != kValueCount; ++cell) {
result.push_back(CorrespondenceCostToValue(ProbabilityToCorrespondenceCost(
ProbabilityFromOdds(odds * Odds(CorrespondenceCostToProbability((*kValueToCorrespondenceCost)[cell]))))) +
kUpdateMarker);
}
return result;
}
2. 利用查找表更新