跳转至

定位

约 2524 个字 预计阅读时间 10 分钟

导航任务 类比
定位问题 我是谁我在哪
我要去哪里
路径规划问题 我如何去

本质上是状态估计问题

  • 单一传感器的估计 视觉 激光
  • 多传感器融合的估计 卡尔曼滤波等

SLAM | Simultaneous Localization and Mapping

两大任务：定位 & 建图

机器人在 未知环境 中从一个 未知位置 开始移动 在移动过程中根据 位置估计 和 地图 进行自身定位. 同时在自身定位的基础上建造增量式地图实现机器人的自主定位和导航

多次重复，提高精度，实时更新

建模

SLAM 问题的建模

把 SLAM 问题建模成了一个 状态估计问题：如何通过带有噪声的测量数据，估计内部的、隐藏着的状态变量

  • 运动：机器人从 \(k-1\) 时刻到 \(k\) 时刻，位置 \(x\) 的变化。无论使用何种测量自身运动的传感器，我们都能使用一个抽象的数学模型来表达：

$$ x_k = f(x_{k-1}, u_k, omega_k) $$

该方程成为运动方程，其中 \(u_k\) 为传感器输入，\(\omega_k\) 为噪声。

  • 观测：机器人在 \(x_k\) 的位置看到某个路标点 \(y_j\)，产生一个观测数据，抽象数学模型为：

$$ z_{kj} = h(y_j, x_k, nu_{kj}) $$

该方程为观测方程，其中 \(\nu_{kj}\) 是观测存在的噪声。

从概率视角看状态估计

  • 从概率机器人的观点出发，我们希望从过去的观测数据，来估计现在的状态分布，那么就存在如下条件概率表达式：

$$ p(x_{1:T}, m | z_{1:T}, u_{1:T}, x_0) $$

其中 \(x_t\) 为时刻 \(t\) 机器人状态（位置和姿态），\(m\) 为观测到的路标，\(z_t\) 为外部传感器输入，\(u_t\) 为内部传感器输入。

滤波与优化

  • 若假设马尔可夫性，\(T\) 时刻状态只与 \(T-1\) 时刻状态有关，若考虑 \(T\) 时刻状态与之前所有状态的关系：

  • 滤波方法

  • 非线性优化方法

地标的提取

地标指的是环境中可以 较易重复识别的特征 。

地标在 SLAM 中主要是用于帮助移动机器人确定它在哪里 ( 自定位 )

数据关联

运动状态的估计

卡尔曼滤波

KF 给出了线性系统的无偏最优估计

卡尔曼滤波归为两个步骤：“预测”和“更新”。

  1. 预测：

$$ begin{aligned} bar{x}k &= A_k x + u_k + w_k \ z_k &= C_k bar{x}_k + v_k end{aligned} $$

$$ bar{P}k = A_k P A_k^T + R $$

  1. 更新：先计算 $ K $，它又称为卡尔曼增益：

$$ K = P_k C_k^T left( C_k P_k C_k^T + Q right)^{-1} $$

然后计算后验概率的分布：

$$ begin{aligned} hat{x}_k &= bar{x}_k + K (z_k - C_k bar{x}_k) \ hat{P}_k &= (I - K C_k) P_k end{aligned} $$

EKF

扩展卡尔曼滤波（ Extended Kalman Filter ） 扩展卡尔曼技术帮助准确估计含有噪声信息的机器人位置，通知准确估计含有噪声信息的环境地标位置信息。

预测（贝叶斯估计）+ 更新（反馈测量）

  1. ** 系统状态 $ mathbf{X} \(**：包含机器人的位置 \((x, y, z, \psi, \theta, \phi)\) 和路标位置 \((x_i, y_i, z_i)\)，\) i $ 为路标个数。

  2. 协方差矩阵 $ mathbf{P} \(，\) mathbf{Q} $：描述了两个变量的相关程度，误差协方差 $ mathbf{P} $ 是用来对状态估计准确度的一种测量，$ mathbf{Q} $ 是被称为过程噪声协方差矩阵，它体现对模型准确度的一种估计。

  3. 卡尔曼滤波增益 $ mathbf{K} $：表示对观测路标的信任度以及他们提供的数据中能获得的改正。

\[ \begin{aligned} \hat{x}_k &= F_k x_{k-1} + B_k u_k + w_k \\ z_k &= H_k x_k + v_k \end{aligned} \]

过程模型 (process model) 和测量模型 (observation model)

预测

  • 状态预测 (State Prediction): $$ hat{x}{k|k-1} = F_k hat{x} + B_k u_k $$

  • 误差协方差预测 (Error Covariance Prediction): $$ P_{k|k-1} = F_k P_{k-1|k-1} F_k^T + Q_k $$

更新 (Update)

  • 卡尔曼增益 (Kalman Gain): $$ K_k = P_{k|k-1} H_k^T left( H_k P_{k|k-1} H_k^T + R_k right)^{-1} $$

  • 状态更新 (State Update): $$ hat{x}{k|k} = hat{x}) $$} + K_k (z_k - H_k hat{x}_{k|k-1

  • 误差协方差更新 (Error Covariance Update): $$ P_{k|k} = (I - K_k H_k) P_{k|k-1} $$

运动状态的更新

地标的更新

常见方法

激光传感器测距 GPS测量 视觉传感器

里程估计

  • 根据传感器感知信息推导机器人位姿（位置和角度）变化
  • 用途：
  • 航位推算 (Dead-reckoning)：基于已知位置， 利用里程估计，推算现在位置

里程估计方法

  • 基于机器人运动感知信息，结合运动学模型
  • 电机码盘
  • IMU（惯性单元，加速度计 + 陀螺仪）
  • 基于环境感知传感器信息，通过最佳匹配估计
  • 激光里程计
  • 视觉里程计（VO）

基于运动感知的里程估计

基于电机码盘的轮式移动机器人里程估计

方式

根据电机码盘获得轮子转速

\[ 角速度计算公式 \\ \omega = \Phi ' = \frac{2 \pi n}{η} \]

对于速度

  • 考虑一个轮子固定，则另一个轮子的速度就是 \(r\omega\)
  • 相对于质心 P 点速度就是 \(r\omega/2\)
  • 相加即可

对于角速度

  • 同样考虑一个轮子固定，则另一个轮子角速度就是 ω
  • 故质心角速度也是 \(\frac{r\omega}{2l}\)
  • 由于两个轮子角速度相反，故为减法

缺点

累计误差变大：在航位推算时，里程计误差被累加，推算随着时间而增长

各种误差太大

基于惯性单元的里程估计

惯性单元 IMU

  • 一般含有三轴的加速度计和三轴的陀螺仪

  • yaw 不准

  • 优点

  • 全天候
  • 采样频率高
  • 短时精度较好
  • 缺点
  • 随着时间的增长累积误差较大，无法满足移动机器人长距离精确定位的要求，需要融合其它传感器进行组合导航

激光里程计

采用 ICP(Iterative Closest Point) 算法

  • 估计 P’集合点与 P 集合点的初始位姿关系
  • P' 是 P 点相对于里程计的坐标系下位置改变后的坐标
  • 根据最近邻域规则建立 P’集合点与 P 集合点的关联
  • 构建旋转矩阵，使得 P' 旋转之后成为 P 点
  • 利用线性代数 / 非线性优化的方式估计旋转平移量
  • 优化旋转矩阵
  • 对点集合 P’的点进行旋转平移
  • 如果旋转平移后重新关联的均方差小于阈值， 结束
  • 否则迭代重复上述步骤

激光里程计的实现可以看做是点云的匹配和激光数据校正 ICP(Iterative Closest Point, 迭代最近点 )

视觉里程计

各类相机主要区别：有没有深度信息

单目：没有深度，必须通过移动相机产生深度 Moving View Stereo

双目：通过视差计算深度 Stereo

RGBD ：通过物理方法测量深度

相机位姿和空间观测特征点的优化方法 BA(Bundle光束平差法) BA的本质是一个优化模型，其目的是最小化重投影误差

定位问题

确定机器人在世界（全局）坐标系中的位置 / 位姿

分为三种解决方法：

  • 基于外部设备感知的定位
  • 基于里程估计的航位推算
  • 基于本体设备感知的定位

基于外部设备感知的定位

全球定位系统 GPS

由空间端、 控制端和用户端三部分组成，也称为 GNSS

  • 空间端由卫星组成
  • 控制端由地面天线站等构成，主要作用监测控制卫星的运行，并对卫星进行时间同步
  • 用户端，采用三点法进行定位

存在问题

遮挡问题：室内难以使用

多路径问题：楼房太高导致最短路径变成反射后的一条路径

全局视觉观测定位

搭建一套外部视觉系统，识别机器人，确定其位置

与 GPS 相同，在内部搭建多个相机实现定位

应用约束：

  • 摄像头有视野范围约束，当环境较大时需要多个摄像头
  • 机器人上也需要有一定的标识方便图像识别定位

基于本体感知的定位

  • 需要预先构建地图，地图坐标系在构建地图时确定
  • 对于大规模环境，路标需具备可鉴别性，否则需要正确的数据关联
  • 存储大规模环境的图片需要较大的存储空间
  • 对于实际环境中存在路标被破坏 / 污染、更换、稀疏等风险

基于环境人工标识的定位

在环境中部署特殊标签，降低成本，确保可靠性，如

  • 地面二维码：适合仓库运输机器人
  • 高反射物质：适合叉车

基于环境自然标识的定位

路标

已知三个星星的坐标，并通过传感器测得 r 或者 θ，进行求解坐标

基本思想：利用本体传感器获得的信息与地图中存储的各个位置上的信息进行匹配，实现位置识别

应用约束：

  • 环境的动态变化和不同位置的相似性，对位置辨别带来挑战
  • 为适应环境的动态变化，可以考虑对每个位置存储多张图片，但增加存储空间需求
  • 图像匹配是一个耗时的工作

控制感知信息融合的自定位

定义：在给定环境地图 ( 对环境的已有知识 ) 的条件下，根据机器人的运动控制 / 里程估计信息和传感器感知数据 ( 当前认知 ) 估计机器人相对于环境地图的坐标