Chapter6-基于图优化的激光SLAM方法

Chapter6-基于图优化的激光SLAM方法

---

---

如有错误望不吝赐教！！！

---

---

Graph-based SLAM

• 前端：帧间匹配/回环检测
• 后端：用一个图来表示SLAM过程
  图中节点表示机器人的位姿
  两个节点之间的边表示两个位姿的空间约束（出现环才表示误差）
  Graph-based SLAM：构建图，并且找到一个最优的配置（各节点的位姿），让预测与观测的误差最小
  非线性最小二乘求解

非线性最小二乘

解决问题：给定一个系统，状态方程 f(x)=z
· x：状态向量，需要估计的值 f(x)：非线性映射函数
· z：系统观测值，通过传感器进行直接测量（预测的观测量）
给定混有噪声的观测值，估计x，使其经过f(x)映射后预测值和观测值的误差最小
非线性问题->线性问题->求解

• x为机器人的位置
  f(x)为传感器的观测模型（似然场模块或重投影模型）
  z为传感器的观测值，激光数据或图像特征点
  目标：找到最优的x，让预测和观测的误差最小
• 误差函数
  误差为预测和观测之差e_i=f_i(x)-z_i
  假设服从高斯分布，对应的信息矩阵为∑i（观测值可靠性，或者是权重矩阵），误差的平方定义：Ei(x)=e_i(x)T²∑_ie_i(x)
  非线性最小二乘的目标函数：F(x)=∑E_i(x)=∑ e_i(x)^T∑_ie_i(x)
• 将非线性方程展开成关于x的线性方程
  线性化：泰勒展开
• 直接对误差函数e_i(x)进行线性化即可
  e_i(x+Δx)=e_i(x)+J_i(x)Δx
  雅可比矩阵：J_i(x)为泰勒展开后余项
• F(x)化解：
  F(x+Δx)
  =∑ e_i^T(x+Δx)∑_ie_i(x+Δx)
  =∑ c_i+2b_iΔx+Δx^TH_iΔx
  =c+2b^T+Δx^THΔx
  F(x+Δx)变换关于Δx二次函数
  求导
• 对Δx求导得
  2b+2HΔx=0
  Δx*=-H^-1b
  令x=x+Δx*,进行不断迭代，直至收敛为止
• 线性化
  构建线性系统
  求解线性系统
  更新解不断迭代

非线性最小二乘在SLAM中的应用

• 图的构建
  里程计测量、回环检测
  

• 误差函数
  观测值z_ij表示经过匹配计算得到的节点x_i和节点x_j的相对位姿
  预测值为里程计的测量值。
  z’_ij=X_i^-1X_j，X_i为x_i对应的转换矩阵
  误差函数e_ij(x)=t2v(Z_ij-1Z’_ij)，t2v表示把转换矩阵转换到对应的位姿
  稀疏矩阵

• 固定坐标系
  观测值观测到的是两个位姿之间的相对位姿
  满足相对位姿约束的解有无穷多组
  为求唯一解必须加入一个约束条件
  让某一个位姿固定，一般选择第一个位姿，Δx1=0
  加入约束
  求解线性系统
  setfixed

• 构建线性系统
  已知误差项和Jacobian矩阵Aij和Bij
  更新向量b
  更新矩阵H

• 求解
  已知矩阵H和向量b
  求解线性方程组
  不断迭代直至收敛

Catorgrapher介绍

（基于图优化）
比较完善的匹配系统，包含建图和定位
目前最好的开源激光SLAM系统
专人维护，不断增加新特性