导航教程 • ZeroHour's Site

导航#

本文介绍导航系统的基本结构。阅读完成后可继续查看导航补充教程，了解路径与轨迹、图搜索、轨迹优化、地图表示和动态避障。

零. 导航做什么#

你来到西交，学姐给你拿了一张地图，你要去 RoboMaster 笃行机器人社，现在有几个要做的事：

找自己的在地图中的位置（重定位问题：重定位算法）
边走边确定自己在地图中走到哪了（定位问题：里程计算法）
规划一条到社团的路线（路径规划问题：全局规划器）
你在路中间走遇见车怕被撞死于是绕过去躲开，你觉得走的太慢了于是跑了两步（路径规划问题：局部规划器/速度控制器）
地图怎么得到的（建图问题）

1 和 2 可以统称定位
2 和 5 可以统称同步定位与建图：SLAM
这篇教程我们把他们拆分的更细一点以便于理解

一. 重定位#

0. 基础#

重定位有时候也叫全局定位，有时候也简称定位
地图分为 2D 地图和 3D 地图/点云图。ROS 中的 2D 栅格地图一般由 .pgm/.png 图片和 .yaml 描述文件组成，3D 点云图常见格式为 .pcd
map系：举例来说就是学姐给你的地图上的一个固定的坐标系（初次接触导航可能觉得这个比较莫名其妙，因为大部分地图上都不会标记一个坐标系，或者说会直接在你当前所在位置标记一个坐标系即map系与odom系重合；为了方便理解，你可以选取地图的一个角作为map系原点）
odom系：通常是系统启动或里程计初始化时，以机器人初始位置附近为原点建立的局部坐标系；它不会随着机器人运动不断重置
map->odom 的 TF 变换：TF 变换就是 ROS 的叫法。变换这个词对于大一同学来说可能比较高级（因为没学线性代数），实际上就是odom系在map系中的位置（坐标）和姿态（旋转）。（视觉如果讲了 PnP，应该也讲了变换的 T 和 R，与这里含义相同）
点云匹配/配准算法：ICP / GICP / NDT 的区别

1. 概念#

把里程计输出的点云和预先构建好的点云图做匹配/配准，输出机器人在地图坐标系下的位姿，并进一步得到 map->odom 的 TF 变换。

重定位不一定只在程序启动时运行一次。当机器人被搬动、定位严重漂移或者匹配置信度过低时，也可能需要重新定位。

2. 特点#

重定位是低频的，不需要高频更新，但需要保证正确，因为错了的后果比较严重（例如这波你起点在仲英书院，结果重定位错了给自己搞文治书院去了，你的路径会同步更新，你会按照从文治到社团的路径走，但是你实际在仲英）
重定位可以关掉（例如没有地图的时候）
点云匹配算法输出了结果不代表结果一定正确。重复走廊、对称场景、点云重叠区域太小或者初值太差，都可能导致错误匹配

二. 定位#

0. 基础#

机器人底盘坐标系：一般记作 base_link，坐标系固定在机器人本体上，会跟着机器人一起移动
LiDAR：激光雷达。提供高精度的环境三维点云，但在快速运动、特征稀疏或重复场景（如长走廊）下容易匹配失败
IMU：惯性测量单元。提供高频的角速度和加速度，但误差会随时间快速累积（漂移）
里程计：一类算法，包括 LIO / VIO / LIVO / LIWO 等
LIO：LiDAR-Inertial Odometry（激光雷达惯性里程计）。Fast-LIO / Point-LIO 等
卡尔曼滤波相关基础：KF / EKF / IEKF / ESKF
李群与李代数基础：SO(3) / SE(3)，指数映射，扰动模型

1. 概念#

就是相对于起点走了多远，即当前在 odom 系下的位姿，通过里程计算法实现。除此之外，里程计通常还会输出速度、轨迹和 odom->base_link 的 TF 变换。

在比赛中，里程计同样可能输出去畸变后的点云、配准点云或局部地图，但是否输出点云取决于具体算法接口。

2. 特点#

里程计是高频的，因为更新慢了后果比较严重（这个跟你用手机导航的时候，突然手机卡死了一样）
里程计有误差：一种是累积误差类似于 IMU 的累积误差，但是没那么严重；一种是直接飞起来或者说飘了（例如受到剧烈撞击、传感器异常、雷达网线接触不良、IMU 坏了、IMU 饱和）
odom 系中的轨迹应该尽量连续。全局定位修正了累积误差时，通常修改 map->odom，而不是让 odom->base_link 突然跳变

三. 全局规划器#

0. 基础#

全局地图/静态地图：描述整个任务区域中的固定障碍物和可通行区域
全局代价地图：在地图的基础上加入障碍物代价、膨胀区域、禁行区等信息
路径：从起点到终点的一串有顺序的位姿点
常见的全局规划算法：Dijkstra、A*、Theta*、Hybrid A* 等。不同算法是否合适，取决于地图形式和底盘的运动学约束。
图搜索家族：Dijkstra 是无启发的最短路基线；A* 使用启发函数加速搜索；D* Lite 适合地图代价变化后的增量重规划；JPS 通过对称性剪枝加速均匀规则栅格上的 A*
采样式规划：RRT / RRT* / PRM 不预先枚举完整栅格，而是在连续空间中采样并建立连通关系；本仓库没有使用，了解与图搜索的区别即可
路径平滑：LOS 可以删除栅格路径中能直接连通的中间点；B 样条等参数曲线可以提高连续性，但平滑后必须重新检查碰撞
带状态搜索：Kinodynamic A* 进一步把速度、朝向或控制输入放进搜索，用来处理“几何路径存在但机器人执行不了”的问题
轨迹优化：MINCO、B 样条或分段多项式可把搜索种子变成带时间、速度和加速度的轨迹；相关的数值基础包括 QP、L-BFGS 和线搜索

1. 概念#

global planner：根据机器人当前位置、目标点和全局代价地图，规划一条从起点到终点的总体路线

全局规划器负责告诉你从仲英书院到社团应该走哪条路，但不会具体告诉你下一步迈多大、遇到车以后向左还是向右躲

四. 局部规划器/速度控制器#

0. 基础#

局部代价地图/动态地图：一般只维护机器人附近的一块区域，使用雷达等传感器实时加入和清除障碍物
机器人轮廓 footprint：机器人在地面上的实际占用范围（往地面上的投影）。规划时机器人不能被当成一个没有体积的点
速度和加速度限制：机器人可以达到的最大线速度、角速度、线加速度和角加速度
底盘模型：差速、阿克曼、全向和舵轮底盘能够执行的运动不同，控制器必须与实际底盘匹配
距离场：SDF / ESDF 可以快速查询机器人到最近障碍物的距离和梯度，常用于膨胀代价与轨迹优化
动态障碍物处理链：点云分割或目标检测得到观测，经过数据关联和卡尔曼滤波形成航迹，再预测障碍物未来位置
动态避障：可以采用反应式速度规划、加入障碍预测的 MPC/MPPI，或在 $(x,y,t)$ 等时空状态中搜索；不能只把动态物体当前位置当成永久静态障碍

常见方法包括 DWA/DWB、Pure Pursuit/RPP、MPC、MPPI 等

1. 概念#

local planner / controller：根据全局路径、机器人当前位姿和速度、局部障碍物以及底盘约束，计算机器人接下来一小段时间应该怎么走，并输出底盘能够执行的速度指令

你按照全局路线往社团走，局部规划器负责在车辆突然出现时决定怎样绕开，决定你应该走还是跑、向哪个方向转以及何时减速

2. 特点#

局部规划器/控制器需要高频运行，因为障碍物和机器人状态会不断变化
它既要尽量跟随全局路径，又要避开障碍物，还要保证输出不超过底盘的速度和加速度能力
机器人左右振荡、贴墙、转不过弯，不一定是局部规划算法本身的问题，也可能来自定位延迟、底盘模型错误、机器人轮廓错误或全局路径不合理

五. 建图与 SLAM#

0. 基础#

建图：把机器人在不同位置看到的点云或激光扫描变换到同一个坐标系中，形成地图
SLAM：机器人不知道准确位姿、也没有完整地图时，一边估计自己的位置，一边构建地图
前端：一般负责相邻帧匹配、运动估计、关键帧或局部地图构建
后端：一般负责维护全局约束和优化整条轨迹
回环检测：机器人再次经过以前到过的位置时，判断“这里之前来过”，用这个约束修正累计误差
占据栅格地图：使用概率描述每个栅格为空闲、占据或未知，常用 log-odds 形式增量融合传感器观测
ROG-Map：一种占据栅格地图，一般只维护机器人附近的高分辨率区域就行，支持增量障碍物膨胀，它需要里程计或 SLAM 提供位姿，本身不负责定位
距离场地图：距离变换、SDF / ESDF 在占据信息上进一步提供到障碍物的距离，方便规划器计算安全代价和梯度
体素地图：OctoMap 使用概率八叉树表示三维占据；TSDF / ESDF 体素地图侧重表面融合或距离查询；iVox 一类结构常用于高频点云近邻查询和局部地图维护
语义地图：在几何占据信息之外记录区域或对象类别，但不能代替几何碰撞检查

1. 概念#

建图不是简单地把每一帧点云叠加起来。如果机器人位姿有误差，叠出来的墙面会重影；走得越远，地图可能错得越严重

SLAM 需要同时处理运动估计、数据关联和全局一致性

六. 导航系统怎样串起来#

数据流：

LiDAR / IMU
     ↓
   里程计 ─────────→ odom->base_link
     ↓
重定位或 SLAM ───────→ map->odom
     ↓
地图与代价地图 → 全局规划器 → 全局路径
     ↓
实时障碍物 ─────→ 局部规划器/控制器 → 速度指令 → 底盘

text