Chapter6-基于图优化的激光SLAM方法
如有错误望不吝赐教!!!
Graph-based SLAM
- 前端:帧间匹配/回环检测
- 后端:用一个图来表示SLAM过程
图中节点表示机器人的位姿
两个节点之间的边表示两个位姿的空间约束(出现环才表示误差)
Graph-based SLAM:构建图,并且找到一个最优的配置(各节点的位姿),让预测与观测的误差最小
非线性最小二乘求解
非线性最小二乘
解决问题:给定一个系统,状态方程 f(x)=z
· x:状态向量,需要估计的值 f(x):非线性映射函数
· z:系统观测值,通过传感器进行直接测量(预测的观测量)
给定混有噪声的观测值,估计x,使其经过f(x)映射后预测值和观测值的误差最小
非线性问题->线性问题->求解
- x为机器人的位置
f(x)为传感器的观测模型(似然场模块或重投影模型)
z为传感器的观测值,激光数据或图像特征点
目标:找到最优的x,让预测和观测的误差最小 - 误差函数
误差为预测和观测之差ei=fi(x)-zi
假设服从高斯分布,对应的信息矩阵为∑i(观测值可靠性,或者是权重矩阵),误差的平方定义:Ei(x)=ei(x)T2∑iei(x)
非线性最小二乘的目标函数:F(x)=∑Ei(x)=∑ ei(x)T∑iei(x) - 将非线性方程展开成关于x的线性方程
线性化:泰勒展开 - 直接对误差函数ei(x)进行线性化即可
ei(x+Δx)=ei(x)+Ji(x)Δx
雅可比矩阵:Ji(x)为泰勒展开后余项 - F(x)化解:
F(x+Δx)
=∑ eiT(x+Δx)∑iei(x+Δx)
=∑ ci+2biΔx+ΔxTHiΔx
=c+2bT+ΔxTHΔx
F(x+Δx)变换关于Δx二次函数
求导 - 对Δx求导得
2b+2HΔx=0
Δx*=-H-1b
令x=x+Δx*,进行不断迭代,直至收敛为止 - 线性化
构建线性系统
求解线性系统
更新解不断迭代
非线性最小二乘在SLAM中的应用
-
图的构建
里程计测量、回环检测
-
误差函数
观测值zij表示经过匹配计算得到的节点xi和节点xj的相对位姿
预测值为里程计的测量值。
z’ij=Xi-1Xj,Xi为xi对应的转换矩阵
误差函数eij(x)=t2v(Zij-1Z’ij),t2v表示把转换矩阵转换到对应的位姿
稀疏矩阵 -
固定坐标系
观测值观测到的是两个位姿之间的相对位姿
满足相对位姿约束的解有无穷多组
为求唯一解必须加入一个约束条件
让某一个位姿固定,一般选择第一个位姿,Δx1=0
加入约束
求解线性系统
setfixed -
构建线性系统
已知误差项和Jacobian矩阵Aij和Bij
更新向量b
更新矩阵H -
求解
已知矩阵H和向量b
求解线性方程组
不断迭代直至收敛
Catorgrapher介绍
(基于图优化)
比较完善的匹配系统,包含建图和定位
目前最好的开源激光SLAM系统
专人维护,不断增加新特性