机器人导航规划

阅读信息

约 2835 个字 9 分钟本页总访问量：加载中... 次

机器人导航规划

导航方式：

有人工标识导引的固定路线导航：磁条，磁感应线，磁钉，二维码……
有人工标识导引的无轨导航
无人工标识导引的无轨导航

基本概念：

工作空间：移动机器人上的参考点能达到的空间集合，机器人采用位置和姿态描述，并需考虑体积
位形空间：机器人成为一个可移动点，不考虑姿态、体积和运动学约束

完备性

路径规划方法需要具备完备性。

完备性：当解存在时，能够在有限的时间内找到解

路径规划算法挑战：在连续空间内搜索，难以保证时间

确保完备性的方法：

基本思路：对空间作离散化
分辨率完备：解析性离散化，确保获得可行解
概率完备：基于概率进行随机采样离散化，使获得解的概率趋近于 1

路径规划

连通图中搜索最优路径的方法：

精确最优搜索法：深度优先法、宽度优先法
近似最优搜索法
启发式搜索法： A*, D*
准启发式搜索算法：退火、进化和蚁群优化等

分辨率完备的路径规划方法：

行车图法

基于障碍物几何形状分解位形空间，将自由空间的连通性用一维曲线的网格表示，在加入起始点和目标点后，在该一维无向连通图中寻找一条无碰路径。

1. 可视图法：连接所有可见顶点对（可见顶点指中间无障碍物，初始位置和目标位置也作为顶点）

优点：
非常简单，特别是当环境地图用多边形描述物体时
可得到在路径长度上最优的解
缺点：
所得路径过于靠近障碍物，不够安全。

2. Voronoi diagram：取障碍物之间的中间点，以最大化机器人和障碍物之间的距离

优点：安全性高
缺点：计算复杂、路径长度较可视图法长、不适用于短距离定位传感器

单元分解法

首先，将位形空间中的自由空间分为若干小区域，每个区域作为一个单元，以单元为顶点、以单元之间的相邻关系为边构成一张连通图；
其次，在连通图中寻找包含初始姿态和目标姿态的单元，搜索连接初始单元和目标单元的路径；
最后，根据所得路径的单元序列生成单元内部的路径

1. 精确单元分解：单元边界严格基于环境几何形状分解，所得单元完全空闲或完全被占。

优点：
机器人不需要考虑它在每个空闲单元中的具体位置，只需要考虑如何从一个单元移动到相邻的空闲单元
单元数与环境大小无关
缺点：
计算效率极大地依赖于环境中物体的复杂度

2. 近似单元分解：栅格表示法，将环境分解成若干个大小相同的栅格

优点
非常简单，与环境的疏密和物体形状的复杂度无关
缺点：
对存储空间有要求

3. 人工势场法：目标点对机器人产生吸引力，障碍物对机器人产生排斥力，所有力的合成构成机器人的控制律

目标点吸引势定义为：

\[ U_{att}(x) = \frac{1}{2} K_a \cdot \rho_d^2(x) \]

其中：

\[ \rho_d(x) = \|x - x_d\| \]

$K_a$ 为系数
$x$ 为被评估点
$x_d$ 为目标点

障碍物：排斥势场

当机器人靠近障碍物时，排斥力须足够大，以避免碰撞；
当机器人远离障碍物时，排斥力必须不影响其运动。

A*算法

（待补充）

地图表示与构建

地图的表示：点云，栅格，特征，拓扑（点和线的关系）

点云地图

线扫激光得到二维的点，交叉线扫描得到三维的点

激光+视觉，得到包含距离和颜色纹理的点云

Cons：重建精度<<扫描精度

栅格地图

把环境划分成不同栅格，每个栅格有一个值，表示当前栅格被占用的情况。栅格划分越细，分辨率越大。

地图表示为 M={m0,m1...}，mi 表示第 i 个栅格被占用的情况，取 0 或 1

栅格划分更细后存储空间爆炸
降低存储空间的方法：不想要地方的栅格合并；如果想要提高分辨率，将大的栅格划分。通过分辨率可变来减少存储空间的要求。四叉树（二维）/八叉树（三维）

特征地图

标记导航路径有关的特征点（陆标）

发展趋势：二维特征+高度信息

Cons：无法精确表示

拓扑地图

把环境表示为带节点和相关连接线的拓扑结构

Pros：易于扩展，可以实现快速路径规划
Cons：由于信息的抽象性，使得机器人难以实现精确可靠的自定位

地图表示的研究趋势：

混合地图，语义地图，多重地图

里程估计

里程估计：输入传感器感知数据，输出移动机器人位姿变化

轮式编码器：记录轮子转动的圈数
惯性测量单元：测速度、加速度
多信息融合里程估计
……

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

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

累计误差随着时间逐渐增大
在特定位置放参照点，用传感器获取参照点并校正

惯性测量单元（IMU）里程估计

加速度计、陀螺仪（有时还有磁力计）
测量加速度和角速度，用于估算机器人的运动和姿态，为导航、平衡、运动控制提供关键数据

激光里程估计

求解两个相邻时刻的相对位姿，将 t2 的扫描地图融合到全局地图

迭代最近点算法（ICP）

最近点匹配
为源点云的每个点找到目标点云中最近的对应点。
计算变换
基于对应点对，计算最佳旋转矩阵和平移向量（最小化距离误差）。
应用变换
将源点云按照计算出的变换进行更新。
重复迭代
直到误差收敛或达到最大迭代次数。

R，t 求解（by gpt）：

R，t 分解算法主要用于从两个坐标系下的一组对应点中，求出刚体变换的旋转矩阵 R 和 平移向量 t，使两组点尽可能重合。这是 刚体配准 (Rigid Registration) 的核心步骤，也用于 相机外参估计、SLAM、3D 重建 等。

算法步骤（经典方法：基于 SVD 分解）

给定两组对应点：

源点集 $P = \{p_i\}_{i=1}^N$
目标点集 $Q = \{q_i\}_{i=1}^N$

目标：找到 $R, t$ 使得：

\[ q_i \approx R p_i + t \]

1. 计算质心

\[ \bar{p} = \frac{1}{N} \sum_{i=1}^N p_i, \quad \bar{q} = \frac{1}{N} \sum_{i=1}^N q_i \]

2. 去中心化

将两组点平移到以质心为原点：

\[ p'_i = p_i - \bar{p}, \quad q'_i = q_i - \bar{q} \]

3. 计算协方差矩阵

\[ H = \sum_{i=1}^N p'_i {q'_i}^\top \]

4. 奇异值分解（SVD）

对 $H$ 进行 SVD 分解：

\[ H = U \Sigma V^\top \]

5. 计算旋转矩阵 R

\[ R = V U^\top \]

若 $\det(R) < 0$，则修正：

\[ V[:, -1] = -V[:, -1], \quad R = V U^\top \]

6. 计算平移向量 t

\[ t = \bar{q} - R \bar{p} \]

特点

鲁棒性好：在无噪声时能精确求解
计算效率高：主要计算量在 SVD
该方法是 ICP（迭代最近点） 中每轮更新 R,t 的标准步骤

视觉里程估计

检测由运动导致的图像变化

VO 问题求解

基于图像：

利用两幅图像中所有限速的亮度信息
计算量大，精度低

基于特征：

从图像中提取醒目的、可重复的特征
要求两帧之间鲁棒匹配或跟踪特征

特征匹配 (by gpt)

什么是光流（Optical Flow）？

光流就是物体在相邻两帧图像中的移动轨迹。简单说：同一个点，从这一帧到下一帧，跑到哪去了？

光流法的特征匹配是什么？

光流并不是“凭空算出来的”，它是基于特征点（明显的小角落、亮点）来跟踪的。
特征匹配就是：找到第一帧中的特征点，然后在第二帧中找到对应的位置。

常见方法：稀疏光流（以 Lucas–Kanade 为例）

选取特征点
先用像 Harris 或 Shi-Tomasi 算法找“角点”（这些点在图像中最明显、最稳定）。
在下一帧寻找这些点
假设两帧时间很近，点的运动很小。
在该点附近找一小块图像窗口，计算亮度变化。
计算位移（dx, dy）
假设亮度不变（亮的还是亮，暗的还是暗），通过线性方程求解位移。
得到光流向量
每个特征点就有一个箭头：从第一帧的位置，指向第二帧找到的位置。

特征匹配和光流的关系

光流法是一种特征跟踪方式。
它不一定计算整张图（那叫稠密光流），而是只对特征点做匹配（稀疏光流）。
和 SIFT/ORB 等特征匹配的区别是：
SIFT/ORB：找关键点+描述子+匹配 → 适合大尺度变化
光流：跟踪局部小范围运动 → 更快，适合视频逐帧跟踪

同时定位及建图（SLAM）

基于最大后验估计（MAP）

(by gpt)

1. 什么是 MAP-SLAM？

SLAM 的目标：机器人在未知环境中 一边移动，一边同时构建地图和确定自身位置。
MAP（Maximum a Posteriori）：最大后验估计，意思是在所有可能的地图和轨迹中，选择最可能的那个。
换句话说：我们希望找到一组 机器人位置轨迹 X 和 地图 M，使得它们 最符合观测数据 Z 和 控制输入 U。

数学上：

\[ (X^*, M^*) = \arg\max_{X,M} P(X, M \mid Z, U) \]

$X$ ：机器人各时刻位姿
$M$ ：地图（特征点、栅格等）
$Z$ ：观测数据（如激光、相机）
$U$ ：控制量（如轮速、舵角）

2. MAP-SLAM 的核心思想

建立概率模型
用贝叶斯公式描述 SLAM：

\[ P(X, M | Z, U) \propto P(Z | X, M) \, P(X | U) \, P(M) \]

$P(Z|X,M)$：观测模型（传感器测量概率）
$P(X|U)$：运动模型（机器人如何移动）
$P(M)$：地图先验（通常假设均匀分布）
最大后验估计
找出最可能的地图和轨迹，使整个系统的似然最大化：

$$ (X^, M^) = \arg\max P(Z|X,M) P(X|U) $$

迭代优化
初始位姿和地图可能不准确。
通过 非线性优化（如高斯-牛顿、Levenberg-Marquardt） 不断调整 X 和 M。
目标是让预测观测值和实际观测值尽可能接近。

3. MAP-SLAM 的流程（直观版）

初始化：机器人起点位置已知或假设为原点，地图为空。
预测：根据控制输入 $U$，预测下一个位姿。
观测更新：获取传感器数据 $Z$，计算预测位置和实际观测的差异。
构建后验概率：把运动模型和观测模型结合，形成最大后验概率。
优化：调整机器人轨迹和地图，使后验概率最大。
迭代：重复预测 → 观测 → 优化，逐步生成精确轨迹和地图。

4. 常用实现方法

图优化方法（Graph-based SLAM）
把每个位姿当作节点，观测约束当作边。
用非线性优化求解整个图的最优布局 → 等价于 MAP。
扩展卡尔曼滤波（EKF-SLAM）
在线递推估计地图和位姿。
也可以看作对 MAP 的高斯近似。

5. 总结

MAP-SLAM 的核心是：在所有可能的机器人轨迹和地图中，选择最可能解释观测数据的一组。它将 SLAM 问题转化为 概率建模 + 优化问题，通过迭代不断修正位姿和地图，使预测与观测尽量一致。