cartographer 论文

news/2024/10/30 17:29:47/

欢迎访问我的博客首页。


cartographer 论文

  • 4. 局部二维 SLAM
    • 4.1 扫描
    • 4.2 子地图
  • 5. 回环闭合
    • 5.1 优化问题
    • 5.2 搜索空间
    • 5.3 分支定界扫描匹配
  • 6. 参考

4. 局部二维 SLAM


  二维 SLAM 的局部算法和全局算法是分开的,但它们都优化雷达观测点的位姿 ξ = ( ξ x , ξ y , ξ θ ) \xi = (\xi_x, \xi_y, \xi_\theta) ξ=(ξx,ξy,ξθ)。如果传感器位于背包这种不稳定的平台上,我们会使用惯导估计重力方向,再把水平安置的雷达的扫描结果投影到二维世界。

  在局部算法中,每一段连续的扫描结果都与世界的一小块匹配。这样的小块称为子地图,匹配的方法是使用非线性优化算法使扫描结果与子地图对齐。该过程称为扫描匹配。扫描匹配会随着时间积累误差,全局算法就是用于去除积累误差的。

4.1 扫描


  子地图的构造过程是,不断进行扫描与子地图的坐标帧对齐。扫描帧包含二维空间中的 K K K 个二维点 H = { h k } , k = 1 , . . . , K H = \{h_k\}, k=1,...,K H={hk},k=1,...,K,它在子地图中的位姿称为 T ξ T_\xi Tξ,该位姿是把扫描帧中的点变换到子地图帧的刚体变换,定义如下

T ξ = ( c o s ξ θ − s i n ξ θ s i n ξ θ c o s ξ θ ) p + ( ξ x ξ y ) . T_\xi = \begin{pmatrix} cos\xi_\theta & -sin\xi_\theta \\ sin\xi_\theta & cos\xi_\theta \end{pmatrix} p + \begin{pmatrix} \xi_x \\ \xi_y \end{pmatrix}. Tξ=(cosξθsinξθsinξθcosξθ)p+(ξxξy).

其中,第一个大括号表示旋转矩阵 R ξ R_\xi Rξ,第二个大括号表示平移向量 t ξ t_\xi tξ

4.2 子地图


  子地图由雷达连续采集的若干扫描帧构建而成,以概率栅格的形式存在。每个格子的像素值代表该格子中有障碍物的概率。栅格点的像素值包含所有离该栅格点最近的雷达点。
在这里插入图片描述

图 4.1 分辨率为 r 的栅格地图

  一个扫描帧被插入栅格地图前,需要计算出哪些栅格点是该帧发现的 hits 点,哪些栅格点是该帧发现的 misses 点。对于每个 hit,与它最近的那个栅格点将被加入 hit 集合;对于每个 misses,与从扫描原点到扫描点这条射线相交且不在 hit 集合中的栅格点将被加入 mis 集合。
在这里插入图片描述

图 4.2 一个扫描帧及其对应的 hits(有叉号的阴影格子) 和 misses(无叉号的阴影格子)

  之前没有被扫描过的栅格点出现在这两个集合中,它将被赋予一个概率值 p h i t p_{hit} phit p m i s s p_{miss} pmiss。之前已经被扫描过的栅格点出现在这两个集合中,其概率值按下面的公式更新

o d d s ( p ) = p 1 − p , M n e w ( x ) = c l a m p ( o d d s − 1 ( o d d s ( M o l d ( x ) ) ⋅ o d d s ( p h i t ) ) ) . \begin{aligned} {\rm odds}(p) &= \frac{p}{1-p}, \\ M_{\rm new}(x) &= {\rm clamp}( {\rm odds^{-1}} ( {\rm odds}(M_{\rm old}(x)) \cdot {\rm odds}(p_{hit}) ) ). \end{aligned} odds(p)Mnew(x)=1pp,=clamp(odds1(odds(Mold(x))odds(phit))).

第一个公式实现在文件 cartographer/mapping/probability_values.h 的函数 Odds。第二个公式的实现在文件 cartographer/mapping/probability_values.cc 的函数 ComputeLookupTableToApplyCorrespondenceCostOdds。

5. 回环闭合


  每个扫描只与包含最近几个扫描的子地图匹配,所以上面的 realtime CSM 匹配算法会积累误差。cartographer 使用稀疏位姿调整算法(Sparse Pose Adjustmen) 优化所有扫描和子地图的位姿。当一个扫描被插入子地图时,它的相对位姿会被保存以用于回环闭合。此外,如果一个子地图不再改变,它和与它匹配的扫描都会被用于回环闭合。运行与后台的扫描匹配算法一旦发现一个满足条件的匹配,对应的相对位姿就会被添加到优化问题中。

5.1 优化问题


  回环闭合和扫描匹配一样是个非线性优化问题,每隔几秒,就会使用 Ceres 求解

arg ⁡ min ⁡ Ξ m , Ξ s 1 2 ∑ i j ρ ( E 2 ( ξ i m , ξ j s ; ∑ i j , ξ i j ) ) (SPA) \arg \mathop {\min} \limits_{\Xi^m, \Xi^s} \frac{1}{2} \sum_{ij} \rho (E^2 (\xi_i^m, \xi_j^s; \sum_{ij}, \xi_{ij}) ) \tag{SPA} argΞm,Ξsmin21ijρ(E2(ξim,ξjs;ij,ξij))(SPA)

   Ξ m = { ξ i m } , i = 1 , . . . , m \Xi^m = \{\xi_i^m\}, i=1,...,m Ξm={ξim},i=1,...,m m m m 个子地图在世界坐标系中的位姿, Ξ s = { ξ j s } , j = 1 , . . . , s \Xi^s = \{\xi_j^s\}, j=1,...,s Ξs={ξjs},j=1,...,s s s s 个扫描在世界坐标系中的位姿,这些位姿是需要被优化的对象。

  每个扫描 j j j 都有一个与之匹配的子地图 i i i,优化过程中会使用它们的相对位姿 ξ i j \xi_{ij} ξij。公式中的 ∑ i j \sum_{ij} ij 是一个协方差矩阵,可以按参考文献 15 计算,或按照公式 (CS) 计算Ceres 的协方差估计特征。

   ρ \rho ρ 是损失函数,可以使用 Huber 函数,用于降低外点的影响。残差 E E E 的计算公式如下:

E 2 ( ξ i m , ξ j s ; ∑ i j , ξ i j ) = e ( ξ i m , ξ j s ; ξ i j ) T ∑ i j − 1 e ( ξ i m , ξ j s ; ξ i j ) e ( ξ i m , ξ j s ; ξ i j ) = ξ i j − ( R ξ i m − 1 ( t ξ i m − t ξ j s ) ξ i ; θ m − ξ j ; θ s ) \begin{aligned} E^2(\xi_i^m, \xi_j^s; \sum_{ij}, \xi_{ij}) &= e(\xi_i^m, \xi_j^s; \xi_{ij})^T \sum_{ij}{}^{-1} e(\xi_i^m, \xi_j^s; \xi_{ij}) \\ e(\xi_i^m, \xi_j^s; \xi_{ij}) &= \xi_{ij} - \begin{pmatrix} R_{\xi_i^m}^{-1}(t_{\xi_i^m} - t_{\xi_j^s}) \\ \xi_{i;\theta}^m - \xi_{j;\theta}^s \end{pmatrix} \end{aligned} E2(ξim,ξjs;ij,ξij)e(ξim,ξjs;ξij)=e(ξim,ξjs;ξij)Tij1e(ξim,ξjs;ξij)=ξij(Rξim1(tξimtξjs)ξi;θmξj;θs)

5.2 搜索空间


  前端实时 CSN 算法和后端快速 CSM 算法,通过扰动旋转和扰动平移产生候选空间。

  对每个搜索空间打分非常耗时,因此只有前端实时 CSM 算法会遍历所有搜索空间。后端回环检测的快速 CSM 算法需要与所有子地图匹配,因此使用分支定界算法实现更高效的匹配。

ξ ⋆ = arg ⁡ min ⁡ ξ ∈ W ∑ k = 1 K M n e a r e s t ( T ξ h k ) (BBS) \xi^\star = \arg \mathop {\min} \limits_{\xi \in \mathcal W} \sum_{k=1}^{K} M_{\rm nearest} (T_\xi h_k) \tag{BBS} ξ=argξWmink=1KMnearest(Tξhk)(BBS)

   W \mathcal W W 是搜索窗口, M n e a r e s t M_{\rm nearest} Mnearest M M M 经过四舍五入得到的网格点。

  旋转步长 δ θ \delta_\theta δθ 是每次旋转的角度,确定旋转步长的原则是:以激光发射点为中心旋转一个步长时,最远的那个扫描点刚好旋转一个分辨率 r r r

d m a x = max ⁡ k = 1 , . . . , K ∣ ∣ h k ∣ ∣ , δ θ = a r c c o s ( 1 − r 2 2 d m a x 2 ) . \begin{aligned} d_{max} &= \max\limits_{k=1,...,K}||h_k||, \\ \delta_\theta &= \rm{arccos}(1 - \frac{r^2}{2d_{max}^2}). \end{aligned} dmaxδθ=k=1,...,Kmax∣∣hk∣∣,=arccos(12dmax2r2).

公式的实现在文件 correlative_scan_matcher_2d.cc 中的函数 SearchParameters::SearchParameters。

  假设水平搜索范围、竖直搜索范围和旋转范围分别是 W x W_x Wx, W y W_y Wy, θ \theta θ,则搜索空间的数量等于下面三者的乘积。

w x = ⌈ W x r ⌉ , w y = ⌈ W y r ⌉ , w θ = ⌈ W θ δ θ ⌉ . w_x = \lceil \frac{W_x}{r} \rceil, w_y = \lceil \frac{W_y}{r} \rceil, w_\theta = \lceil \frac{W_\theta}{\delta_\theta} \rceil. wx=rWx,wy=rWy,wθ=δθWθ.

对应的算法如算法 1,这是一个三重循环。

在这里插入图片描述

前端实时 CSM 算法划分搜索空间的函数是 RealTimeCorrelativeScanMatcher2D::GenerateExhaustiveSearchCandidates,配置平移搜索范围和旋转搜索范围的参数在文件 cartographer/configuration_files/trajectory_builder_2d.lua。后端快速 CSM 算法划分搜索空间的函数是 FastCorrelativeScanMatcher2D::GenerateLowestResolutionCandidates,配置平移搜索范围和旋转搜索范围的参数在文件cartographer/configuration_files/pose_graph.lua。

5.3 分支定界扫描匹配


  对于大搜索窗口,我们使用算法 2 更高效地求解 ξ ⋆ \xi^\star ξ,该算法最初用于混合整数线性规划。算法主要思想是把所有可能组织成一棵树,根结点代表所有可能的解。在 Cartographer 中,所有的可能解是 W \mathcal W W。子结点是对其父结点的划分。每个叶结点代表一个可行解。

在这里插入图片描述

6. 参考


  1. cartographer 论文,sci-hub,2016。
  2. cartographer 论文翻译,知乎专栏,2021。

http://www.ppmy.cn/news/58679.html

相关文章

基于ssm的论坛系统的设计与实现【附源码】

基于ssm的论坛系统的设计与实现 摘 要 早期的网络论坛系统已经诞生一段时间,随着互联网技术的发展,它已经从最初的简单电子公告板系统变成了一种丰富的论坛系统社区模型。人们通过论坛系统进行信息的获取、发布和交流已经成为一种普遍的社交方式&#x…

【java】`PosixFilePermission` 类设置 POSIX 文件权限

当您在使用 Java 8 或更高版本时,可以使用 PosixFilePermission 类设置 POSIX 文件权限。这是一个枚举类型,包含了常见的文件访问权限,例如读取、写入和执行等。以下是一些示例,演示如何使用 PosixFilePermission 类设置文件权限。…

weak_ptr特点

(Owed by: 春夜喜雨 http://blog.csdn.net/chunyexiyu) 1. 关于weak_ptr: weak_ptr也是一个很有意思的智能指针,感觉它不太像是一个智能指针,更像是一个shared_ptr的经纪人; weak_ptr指向一个由shared_ptr管理的对象,注意一定是…

STL及常用容器vector、list和deque的介绍

vector和built-in数组类似,它拥有一段连续的内存空间,并且起始地址不变,因此它能非常好的支持随机存取,即[]操作符,即可以以数组下标的方式来访问或遍历。但由于它的内存空间是连续的,所以在中间进行插入和删除会造成内存块的拷贝,另外,当该数组后的内存空间不够时,需…

unity中,常用的ShaderGraph节点介绍

文章目录 一、 介绍color颜色节点。Split节点combine节点Texture2DAsset节点SampleTexture2D节点Add节点Multiply节点Float节点TilingAndOffset节点Time节点Position节点Step节点Fraction节点。FresnelEffect节点NormalVector节点DotProduct节点Remap节点2.Subtract节点 一、 介…

美国大学特别重视的批判性思维,是如何培养出来的?

美国大学非常注重培养学生的批判性思维,爱因斯坦有一句名言:“大学教育的价值,不在于学习很多事实,而在于训练大脑会思考。” 批判性思维有两个典型特征:(1)批判性思维善于对通常被接受的结论提…

软件设计师笔记

软件设计师笔记 计算机组成与体系结构 数据的表示、计算机结构、Flynn分类法、CISC与RISC、流水线技术、存储系统、总线系统、可靠性、校验码 1. 数据的表示 (一)进制转换 R进制转十进制使用按权展开法: 十进制转R进制使用短除法 二进制…

Leetcode292. Nim 游戏

Every day a leetcode 题目来源:292. Nim 游戏 解法1:数学推理 让我们考虑一些小例子。 显而易见的是,如果石头堆中只有一块、两块、或是三块石头,那么在你的回合,你就可以把全部石子拿走,从而在游戏中…