分支定界简述

发布时间:2023年12月27日

分支定界法本身是优化中的一种方法,这里不做研究,我主要看cartographer中的分支定界法,在cartographer中分支定界用来做scan to map的匹配,用于回环。

在2维SLAM中,激光雷达扫描得到的结果如何和当前的map去做匹配呢?首先要明确,当前的map是一个2维栅格地图,每一个格子都有自己的占用概率,而拿到的激光雷达扫描结果就是2维点云,是包含XY在内的一系列点,如果能将扫描结果和当前地图匹配起来,就能够确定当前的机器人在地图中的位姿(x,y,theta)。

那实际操作中呢,自然想到可以先假设一个机器人位姿,然后根据这个位姿将激光雷达点云转换到地图坐标系中,也就是映射到地图上,然后计算是否匹配,如何衡量匹配程度呢?如果每一个激光点都打在了占据概率大的栅格上,就证明匹配程度高,因此可以定义一个匹配度=所有激光点所在栅格的概率值之和,然后我们把所有地图上的机器人位姿等间隔分开(三层循环,x,y,theta),计算匹配度,匹配度最高的那个位姿就是机器人的当前位姿。这种方法即所谓暴力匹配。

地图很大时,这种方法显然不现实,怎么优化匹配速度呢?

一个很朴素的想法是,把搜索的间隔搞大一点,这样就比较快的得出结果,然后在最优结果附近再去按小间隔搜索一遍(所谓多分辨率)。

这样有一个明显的问题,就是可能会漏掉全局最优,而找到了局部最优。

那有没有可能先把搜索的间隔搞大一点,然后得出每个矩形区域内的最优可能结果?然后再抛弃其他区域,去这个最优的区域里面找?

分支定界的思路就是这样,它使用了树模型(为什么是树模型?可能是因为树模型适合搜索?),做了分支(先按大区域划分,因为是树模型,所以叫分支,每个区域是一个树的节点,越靠近根节点的区域也就越大,很符合树模型),定界(确定这个区域内的最优可能匹配度,即确定上界,这个“计算区域匹配度”的方法叫贪心得分,是上面的计算匹配度的拓展,所谓贪心得分,就是计算这个激光点的得分时,给它往右下角拓展,用这个拓展后的区域里面的栅格的最大概率值去计算,越大的区域拓展的范围越大,其造成的效果就是子区域计算的最高分必然不高于父区域计算的最高分,这里的区域也可以理解为节点),然后抛弃不行的区域,在可能的区域里面继续划分。

详细流程如下:

再来看这张图假设我们需要去计算检测匹配的点为如图所示16个
则我们第一层搜索精度最低,只列举其中两个,并优先考虑靠左(优先考虑可能性最高的)。
对其继续分层,将其精度提高一倍,又可以列举出两个,并优先考虑靠左。
这样直至最底层,计算出该情况下的平凡得分(也就是直接计算所在栅格的那种方式),最左的底层有两个值A和B,我们求出最大值,并将其视为best_score
然后我们返回上一层还未来得及展开的C,计算C的贪心得分并让它与best_score比较,若best_score依旧最大,则不再考虑C,即不对其进行分层讨论。
若C的贪心得分比best_score更大,则对其进行分层,计算D和E的值,我们假设D值大于E,则将D与best_score对比
若D最大,则将D视为best_score,否则继续返回搜索。

文章来源:https://blog.csdn.net/wenningshine/article/details/135241028
本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。