腿足机器人之九- SLAM基础

• 未知环境：无法提前预知障碍物、路径和地形。
• 动态环境：无法实时更新地图（如移动的障碍物）。
• 定位依赖：无法确定自身位置，导致路径规划失效。

因此，SLAM为运动控制提供了环境感知和定位信息，两者结合才能实现真正的自主导航。

SLAM基础知识

SLAM指的是机器人在未知环境中，一边构建地图（Mapping），一边确定自身位置（Localization）。这离不开传感器数据、精细处理前端数据、强力后端优化以及闭环检测等环节。
对于腿足式机器人，SLAM的实现在某些方面与常规车轮式或飞行型机器人有所不同。这是因为腿足机器人的运动模式较为复杂，它们可以在非常不规则的地形上行走，如岩石、瓦砾或者是台阶等，这些环境为SLAM系统的实现提出了新的挑战。

Boston Dynamics的Spot机器人：这是一种四足行走机器人，配备有多种传感器，包括LiDAR和摄像头等，能够在各种复杂地形中进行探索和监测任务。Spot机器人利用高级SLAM技术进行环境感知、障碍物避免和路径规划，已经被应用于工业检查、数据收集等多个领域。

ANYmal：由苏黎世联邦理工学院开发的四足机器人，同样装备了先进的视觉和IMU传感器。ANYmal通过专门为四足机器人设计的SLAM算法能够在复杂、动态且未知的環境中稳定行走。

传感器数据采集

由于腿足式机器人需要在各种复杂地形上行走，因此相较于车轮式机器人，它们通常需要更多种类的传感器来获得足够的环境信息。常见的传感器配置包括：

• 激光雷达（LiDAR）：用于进行高精度的距离测量（目前成本较高），直接生成2D/3D点云，能帮助机器人进行障碍物检测和避障。
• 摄像头（单目或立体）：便宜且能提供丰富的视觉信息（缺点是对光照敏感），通过特征点（如ORB、SIFT）构建稀疏/稠密地图。
• IMU（惯性测量单元）：由于腿足机器人在行走时可能会产生较大的震动，IMU 可以提供实时连续的运动状态信息（缺点是长时间使用会有积累误差），帮助机器人更准确地估计自身的姿态和加速度，辅助定位。
• 触觉传感器：腿足机器人的足部可能安装有触觉传感器，用于感知地面属性，如硬度、滑腻等，这对于机器人的步态规划和稳定行走非常关键。

前端处理

腿足式机器人在复杂地形上的行走要求SLAM系统能够实时且准确地处理大量的传感器信息，SLAM的前端处理主要包含特征提取和数据关联。特征提取是指从不同传感器（尤其是视觉传感器）中提取稳定可靠的特征点，如边缘、角点等，这些特征需具备易于跟踪和重复识别的特性。在数据关联步骤是指SLAM系统需要将当前帧数据中提取的特征与之前的帧进行比对，实现帧间的匹配，这要求算法必须具有高效和鲁棒性，以应对机器人动态运动带来的影响。

后端优化

定位与地图构建过程中会积累误差，后端优化的主要任务是对这些误差进行修正，确保地图和器人轨迹的全局一致性。常见的后端优化方法包括图优化技术和机器人的状态估计。对于图优化技术，可以利用因子图（factor graph）和非线性最小二乘方法求解优化问题：
$$x^{\ast }=\arg \underset{x}{\min }{\Sigma }_{(i,j)\in c}\rho (||f(x_i,x_j,z_{ij}|{|}^2)$$
其中$x$表示所有位姿的集合，$z_{ij}$是从位姿$x_i$到$x_j$的测量值，$\rho$是一个鲁棒核函数，用于减轻离群值的影响。

对于状态估计是指实时更新机器人的状态估计，包括位置、姿态和速度等，这对于机器人动态平衡和路径规划至关重要。

闭环检测

在长时间的导航过程中，机器人可能会再次进入之前已经探索过的区域。闭环检测的任务是识别这种情况，即检测机器人是否回到了先前的位置。这对于减少全局定位误差和增强地图的一致性至关重要。通过闭环检测，系统可以在地图中加入一个新的约束，从而优化整个地图的精度和质量。

闭环经常通过寻找数据库中与当前帧相似的图像或激光雷达扫描数据实现。这可以通过不同的图像描述符或者特征匹配算法来完成，例如ORB、SIFT特征匹配。

SLAM算法分类

对于腿足式机器人而言，选择合适的SLAM算法至关重要，因为它们面临的环境比较复杂，比如不均匀的地形、不稳定的支撑面等。下面是几种常见的SLAM算法及其在腿足机器人上的应用情况。

在介绍具体算法之前，先用数学化公式描述SLAM问题：

• 机器人控制命令$u_i$（如前进、转向等），控制命令是一个连续过程 u 1 : T = { u 1 , u 2 , u 3 , ⋯ u T } u_{1:T}=\{u_1,u_2,u_3, \cdots u_T\} u1:T​={u1​,u2​,u3​,⋯uT​}
• 机器人位姿（如位置和方向）定义：$x_i$表示$i$时刻位姿， x 0 : T = { x 0 , x 1 , x 2 , ⋯   , x T } x_{0:T}=\{x_0, x_1, x_2, \cdots , x_T\} x0:T​={x0​,x1​,x2​,⋯,xT​}表示位姿从0时刻到$T$的过程。其运动建模可为：
  $$x_{i+1}=g(x_i,u_i)+v_i$$
  其中g是运动函数(对应于前面系列博客，正运动学、逆运动学等)，$v_i$是与运动相关的噪声，通常假设是高斯分布的。
• 整个环境的地图：m表示
• 观测值$z_i$，该数据通常是传感器（如激光雷达、摄像头）获取， z 1 : T = { z 1 , z 2 , z 3 , ⋯   , z T } z_{1:T}=\{z_1, z_2, z_3, \cdots , z_T\} z1:T​={z1​,z2​,z3​,⋯,zT​}，$i$时刻观测值依赖于$z_i$由当前机器人的位姿和地图，其数学关系可建模为：
  $$z_i=h(x_i,m)+{\omega }_i$$
  其中$h$是观测函数，${\omega }_i$是观测噪声，通常假设它符合某种分布，如高斯分布。

实时SLAM算法要解决的问题是，通过观测值$z_i$和控制命令$u_i$，得到机器人的位姿$x_i$和全局地图$m$：
$$p(x_i,m|z_{1:T},u_{1:T})$$

基于滤波的SLAM：扩展卡尔曼滤波（EKF-SLAM）

EKF-SLAM是应用较早且较为经典的SLAM技术之一。它是基于卡尔曼滤波理论的一种非线性拓展。卡尔曼滤【介绍可见】波适用于线性高斯模型，而扩展卡尔曼滤波通过对非线性系统的线性化处理，使传统卡尔曼滤波理论得以应用于非线性系统。

扩展卡尔曼滤波适用于环境相对静态，传感器误差可被适当模型化为高斯噪声时的情况，如工厂环境。该算法的核心公式如下，$u_i$控制命令、$z_i$观测值，$x_i$是位姿，$P_{i|i-1}$是预测协方差，$G_i$是过程模型相对于状态的雅可比矩阵，$R_i$是过程噪声协方差矩阵，其它扩展卡尔曼的流程见【腿足机器人之四- 卡尔曼滤波】：

接下来可以看一个扫地机器人的例子。
1.EKF-SLAM的状态向量
在EKF-SLAM中，状态向量通常包含机器人的位姿和地图中所有特征的位置，其位姿由其在二维空间中的位置和朝向$x,y,\theta$表示，而地图中有N个特征，每个特征的位置也由二维坐标$(m_x,m_y)$表示，那么状态向量$x_i$将是：
$$x_i=[x,y,\theta ,m_{x1},m_{y1},m_{x2},m_{y2},\cdots ,m_{xN},m_{yN}]$$

2.过程模型
过程模型描述了如何根据控制输入（如速度和转向角）预测机器人的新位置。对于机器人位姿，假设控制输入$u_i=[v,\omega {]}^T$包括了线速度$v$和角速度$\omega$，位姿的运动可通过如下方程预测：

$x_{i+1}=x_i+v_i\times t\times \cos ({\theta }_i)$
$y_{i+1}=y_i+v_i\times t\times \sin ({\theta }_i)$
${\theta }_{i+1}={\theta }_i+{\omega }_i\times t$
其中的$t$表示的是从$i$到$i+1$经过的时间间隔（如单位为毫秒等），因时间间隔比较短，可假设地图特征不变，因此其预测模型只是简单地将它们保持不变。

3.观测模型
观测模型描述了如何从状态向量中计算预期的观测值。这些观测通常是与机器人与地图特征之间的相对位置有关的量度，比如距离和角度。如果假设观测到的是特征$j$的位置，则观测模型$h(x_i)$可以表示为：
$r_i=\sqrt{((m_{xj}-x{)}^2+(m_{yj}-y{)}^2}$
${\varphi }_i=\arctan 2(m_{yj}-y,m_{xj}-x)-\theta$
$r_i$和${\varphi }_i$分别是观测到的特征相对于机器人的距离和角度。

4.EKF 算法流程
预测阶段：
使用过程模型和当前状态估计$x_{i-1}$及控制输入$u_i$来预测新的状态估计${\overline{x}}_i$：
$${\overline{x}}_i=g(u_i,x_{i-1})$$
计算预测协方差${\overline{\Sigma }}_i$:
$${\overline{\Sigma }}_i=G_i{\Sigma }_{i-1}{G}_i^T+R_i$$
其中$G_i$是过程模型相对于状态的雅可比矩阵，$R_i$是过程噪声协方差矩阵。

更新阶段：
使用观测$z_i$更新状态估计：
$K_i={\overline{\Sigma }}_i{H}_i^T(H_i{\overline{\Sigma }}_i{H}_i^T+Q_i{)}^{-1}$
$x_i={\overline{x}}_i+K_i(z_i-h({\overline{x}}_i))$
${\Sigma }_i=(I-K_i{H}_i){\overline{\Sigma }}_i$
其中$H_i$是观测模型相对于状态的雅可比矩阵，$Q_i$是观测噪声协方差矩阵，$K_i$是卡尔曼增益。

基于图优化的SLAM

基于图优化的SLAM是一种高级的SLAM技术，其中环境和机器人的路径被表示为一个图（graph），节点代表机器人的位姿，边代表位姿之间的相对关系，这些关系可能来自移动信息或传感器测量。Google的Cartographer就是这类SLAM的一个典型代表，它能够实时地为2D和3D环境生成高质量的地图。

Cartographer的核心是利用激光雷达数据构建一个全局一致的地图，其处理过程主要包括两部分：实时定位及局部地图构建和周期性的全局优化。

当腿足机器人移动时，Cartographer首先对激光雷达扫描进行实时处理，与局部子地图进行匹配，从而估计机器人的当前位姿。这一步的关键是有效地将当前的感知数据与已有地图进行对齐（scan matching）：
$$x_t^{\ast }=\arg \underset{x_t}{\min }\sum _k||z_k-h(x_t,m_k)|{|}^2$$

其中$z_k$是当前扫描中的一个观测点，$m_k$是子地图中的对应点，$h(x_t,m_k)$是计算在位姿$x_t$状态下观测点$z_k$的预期位置。

为了校正路径中累积的漂移和闭环时的误差，Cartographer 周期性地执行全局优化。在这个步骤中，算法尝试找到环路闭合（即机器人返回到之前访问过的位置），并调整整个路径和地图的一致性。

这通过构造和优化一个因子图实现。每个节点代表一个时刻的机器人位姿，边则代表位姿间的约束：
$${\chi }^{\ast }=\arg \underset{\chi }{\min }(\sum _{(i,j)\in c_{odom}}||x_j-g(x_i,u_{ij})|{|}_{{\Sigma }_{ij}}^2+\sum _{(i,j)\in c_{loop}}||x_j-g(x_i,u_{ij})|{|}_{{\Lambda }_{ij}}^2)$$
其中：

• KaTeX parse error: Unexpected end of input in a macro argument, expected '}' at end of input: …thcal {c_{odom}是来自里程计的约束，$g(x_i,u_{ij})$计算从位姿$x_i$到$x_j$的变换，
• KaTeX parse error: Unexpected end of input in a macro argument, expected '}' at end of input: …thcal {c_{loop}是闭环检测时发现的约束，
• ${\Sigma }_{ij}$和${\Lambda }_{ij}$分别是里程计与闭环约束的信息矩阵。

视觉SLAM（V-SLAM）

视觉SLAM是利用摄像头为主要传感器输入，通过分析图像的序列来定位机器人的位置并同时构建环境地图的技术。与依赖激光雷达或其他传感器的SLAM相比，视觉SLAM往往成本更低，且能提供更丰富的环境信息。下面将重点介绍两种常见的视觉SLAM算法：ORB-SLAM和LSD-SLAM。
ORB-SLAM
ORB-SLAM使用ORB特征（Oriented FAST and Rotated BRIEF）来从视频图像中提取稳定的特征点，用于后续的追踪、定位和地图构建。

1. 特征提取与匹配
   对于每一帧图像$I_t$，ORB-SLAM首先提取ORB特征。每个特征包含了位置信息和一个独特的描述符。假设当前帧与前一帧$I_{t-1}$之间进行特征匹配。我们可以通过匹配描述符来找到两帧图像中对应的特征点对$(P_i,P_i^{{}^{\prime }})$

2. 位姿估计
   使用特征点匹配信息，ORB-SLAM估计相机位姿$(R,t)$，即相机在空间中的旋转和平移。这通过解决以下优化问题实现：
   $$\underset{R,t}{\min }=\sum _i\rho (||P_i^{{}^{\prime }}-\pi (R,t,P_i)|{|}^2)$$
   其中：

• $P_i^{{}^{\prime }}$是当前帧中匹配的特征点的位置，
• $P_i$是上一帧中的位置，
• $\pi (R,t,P_i)$表现为将上一帧的3D点$P_i$通过位姿$R,t$投影到当前帧的计算位置，
• $\rho$是一个鲁棒性函数（如Huber函数），帮助减少异常点的影响。

3. 地图构建与更新
   一旦新帧的相机位姿被估计出来，新观测到的特征点被用来更新和扩展地图。这包括将新特征点添加到地图中以及更新现有特征点的位置信息。
4. 闭环检测
   ORB-SLAM 设计有闭环检测机制来识别机器人是否访问了先前探索的区域。这通过比较当前图像的特征描述符与数据库中存储的早期图像的描述符来实现。如果检测到闭环，则通过图优化来调整所有关键帧和地图点的位置，以减少累积的漂移误差：
   $x^{\ast }=\arg {\min }_x\sum _{alli,j}{e}_{ij}(x{)}^T{\Omega }_{ij}{e}_{ij}(x)$
   其中$e_{ij}(x)$表示帧间的约束误差，${\Omega }_{ij}$是信息矩阵。

一个腿足机器人在未知复杂地形中行走，如岩石区或瓦砾堆。这种环境中，传统的激光雷达可能受限于不规则形状的表面反射问题。利用集成的摄像头，机器人通过ORB-SLAM呈现上述过程，不断采集图像、提取和匹配ORB特征、估计运动和更新其内部的3D地图。停留在一个角落或绕过障碍物时，ORB-SLAM的闭环检测能够识别出该区域是否之前已探索过，并据此调整其位姿估计，以减小任何可能的导航误差。这确保了机器人在复杂环境中的导航与定位精度，使其能安全有效地完成任务。

工具与框架

ROS导航栈：集成SLAM（gmapping/cartographer）、定位（AMCL）、路径规划（move_base）。

仿真环境：Gazebo + TurtleBot3（模拟SLAM和导航）。

下一篇博客讨论《SLAM地图如何用于运动控制》，主要是机器人全局、局部路径规划内容。