type
status
date
slug
summary
tags
category
icon
password
一、Introduction
1. autonomous robot
- Estimation（状态估计/定位）
- 低延时
- 高精度，高前后一致性
- Planning（规划）
- 复杂和未知的环境
- 安全性、动态可行性
- 有限的传感和计算
- Perception（感知）
- 3D建图
- 融合和集成
- Control（控制）
- 产生实时控制信号
2. Motion planning
- Basic requirements
- Old-school pipeline（通用规划算法）
- 前端路径搜索，首先在前端利用图搜索等方法搜索出一条初始无碰撞的安全路径（低维，离散空间）
- 后端轨迹生成（优化），再在后端对已生成路径进行优化以获得一条可执行的优化轨迹（高维，连续空间）
二、Course Outline
motion planning
- 路径规划的前端、路径搜索（基于采样/搜索/满足动力学约束的一些搜索）
- 后端轨迹优化、轨迹生成
1. Front-end: Path Finding
- Search-based methods
- Sampling-based methods
2. Search-based Method
基于搜索的算法
- Dijkstra’s算法
- A*算法
- JPS
3. Sampling-based Method
基于采样的路径搜索算法d
4. Kinodynamic Path Finding
都是建立一个状态空间模型（或者非线性的模型），接下来给一定离散化之后的控制量，驱动状态转移
1. State Lattice Search（代价十分高昂）
（图上面的搜索）建立一个移动机器人的动力学模型，将控制量进行离散化（固定时间，固定油门，不同方向的路径合一起形成的图），每一个结点包含状态，实现最短路径。
2. 混合A*算法（复杂度降低，可以比较快的找到path）
- 维护一个栅格网络地图。
- 会根据格子大小对树进行暴力剪枝，每一个网格中只保留一个状态点，该状态点不一定像普通的网格搜索一样落在格子中间，而是可能落在格子任何一个地方。
- 当格子里出现了另一个的状态，它可以维护起点开始到此处代价最小，则会取代掉该网格里原状态。
3. Kino dynamic RRT*
- 采样位置和状态
- 求解两状态边界最优控制问题
5. Back-end: Trajectory Optimization
后端轨迹优化、轨迹生成
Typical Planning Methods Overview
三、Map Representation
Map
- Data Structure 数据结构
- Fusion Method 用什么方法去做地图融合
1. Occupancy grid map
Map以数据结构为基础
2. octo-map：
- 八叉树（Octree），查询时需要根据树的结构进行递归索引的查询
- 基于3D Occupancy Grid Map的开源库：octo-map
3. Voxel hashing
- 用哈希表的方式存储起来
4. Point cloud map
- 用于表示比较稠密的障碍物的信息
- 开源库：PCL http://pointclous.org/
5. TSDF map
Truncated（截断） Signed Distance Functions
- 开源软件OpenChisel
TSDF map 用于三维重建
- TSDF 只关注靠近表面的区域，忽略远处的距离信息。这使得计算和存储更加高效，因为只需要处理靠近表面的数据。
- TSDF 通过截断距离值，可以减少噪声的影响，提供更平滑的表面重建。
- 由于只处理局部区域，TSDF 可以更快地更新和重建，适合实时应用。
6. ESDF map
Euclidean Signed Distance Functions Incremental Update, Global Map
- 通过计算每个点到最近障碍物的有符号距离来提供环境的详细信息。
- 生成：通常需要一个占据栅格地图（Occupancy Grid Map），通过算法（如快速行进法 Fast Marching Method 或 Dijkstra 算法）计算每个栅格到最近障碍物的欧几里得距离，生成一个与输入地图分辨率相同的 ESDF 地图，每个栅格包含有符号的距离值。
- VoxBlox, FIESTA, TRR’s Local Map
欧几里得距离
计算两点之间的直线距离
More
- Free-space Roadmap
- Voronoi Diagram Map 拓扑地图
Lecture 2
SEARCH-BASED PATH FINDING
一. Graph Search Basis
1. Configuration Space
- Robot configuration（机器人配置）
- Robot degree of freedom(DOF)自由度，如无人机轨迹规划有四个自由度：x轴，y轴，z轴和偏航角
- Robot configuration space （机器人配置空间）
n（自由度数）个维度的空间（包含了所有机器人可能存在的配置）
C-space
- Each robot pose is a point in the C-space
机器人任何一个可能存在的位置在C-space中都表示唯一一个点
2. Configuration Space Obstacle
- Planning in workspace（通过做碰撞检测，费时费力）
- Planning in configuration space
- 将机器人表示为点
- 将工作空间的障碍物转化为控制空间的障碍物
- 将障碍物按照无人机大小膨胀
3. Workspace and Configuration Space Obstacle
- Graph and Search method
Search-based Method
- Graph Search Overview
对于图进行图搜索，我们总是能产生一棵搜索树——搜索起始节点到终止节点的一条路径——快且最优
4. Graph Search Overview
Maintain a container to store all the nodes to be visited
container，容器，是一个数据结构，用于存储待访问的节点
- 如果是广度优先搜索（BFS），容器通常是一个队列（先进先出）。
- 如果是深度优先搜索（DFS），容器通常是一个栈（后进先出）。
- 如果是A*算法，容器通常是一个优先队列（按某种评分函数排序）。
The container is initialized with the start state XS
容器初始时只包含一个节点，即起始状态，这是搜索的起点
Loop
算法不断从容器中取出节点并处理，直到满足终止条件（找到目标节点或容器为空）
- Remove a node from the container according to some pre-defined score function —— visit a node
- 如果是BFS，评分函数是“最早进入容器的节点优先”。
- 如果是DFS，评分函数是“最晚进入容器的节点优先”。
- 如果是A*算法，评分函数是“代价最小的节点优先”。
- 对取出的节点进行“访问”，即检查该节点是否满足目标条件。
- 如果满足目标条件，算法可以终止并返回结果。
- 如果不满足目标条件，继续下一步。
移除的规则由评分函数（score function）决定：
visit a node：
- Expansion: Obtain all neighbors of the node —— discover all its neighbors
- 邻居节点的定义取决于具体问题：
- 在路径规划中，邻居节点可能是相邻的网格点。
- 在状态空间搜索中，邻居节点可能是通过某种操作（例如移动、旋转等）得到的新状态。
- 发现当前节点的所有邻居节点。
- 这一步通常包括检查邻居节点是否合法（例如是否在边界内、是否满足约束条件等）。
获取当前节点的所有邻居节点。
discover all its neighbors
- Push them (neighbors) into the container
- 加入的顺序和容器的类型有关：
- 如果是BFS，邻居节点会被加入队列的末尾。
- 如果是DFS，邻居节点会被加入栈的顶部。
- 如果是A*算法，邻居节点会根据评分函数插入优先队列的合适位置。
将所有合法的邻居节点加入容器中，等待后续访问。
End Loop
最基本的图的遍历
Breadth First Search 广度优先搜索（队列，先进先出）
二. Dijkstra and A*
Dijkstra算法
Dijkstra和之前搜索路径时用的广度优先搜索BFS算法来说最大的不同就是从容器中弹出接下来要访问的节点不同。D每次弹出的节点具有最小的Gn（起点到n节点的cost总和最小）
图搜索主要的循环三大部分：弹出-扩展-新节点压入堆栈
判断从n走到m是否m的cost会下降
最优性保证：图里面每个每扩展过的节点的g是最小的——找最小的累计cost的g值
启发式函数帮助下的搜索算法A*
g(n) 累计cost
h(n) 到终点的预估
f(n) = g(n) + h(n)
A*算法策略：将所有已经扩展过的节点塞进容器，每次弹出n节点——弹出节点依据的准则时f(n)，更新依然是更新g值，而排序依据是f，因此会重新计算f并排序
A*算法如果要保持最优性，它每个节点估计出来的可能花费的cost应该比实际到达的cost小
将Grid Map转换成Graph
其核心在于将离散的网格单元抽象为图的节点，并通过连接规则定义节点间的边
How to get the truly theoretical optimal solution?
Tie Breaker
对于两个cost相同的path，找到一个倾向性去倾向某一条路
当很多paths有相同的f值的时候，稍微干涉一下h
三. Jump Point Search
A*可能会做很多无谓的探索
JPS如何工作的？
它会智能判断哪些节点是需要扩展的，哪些节点是不需要扩展的（如果其他节点可以由x节点父亲直接到达且距离小于等于（对角线移动则是小于）经过x，则x不扩展这些节点
先执行直线的移动，再执行对角线的移动（首先考虑直线跳跃，其次再考虑对角线跳跃）
为什么 JPS 先进行直线移动，再执行对角线移动？JPS 的扩展策略是：
- 沿直线方向尽可能远地跳跃（跳过所有非 Jump Point 节点）。
- 到达 Jump Point 后，再检查对角线移动方向。
- 对角线方向也是尽可能跳跃，直到遇到障碍或强制邻居。
直线移动优先的原因
- 减少冗余搜索
- 先沿 水平/垂直方向 跳跃可以快速到达潜在的 Jump Point，避免 A* 中逐步扩展每个格子。
- 如果优先进行对角线搜索，可能会遇到一些不必要的拐弯情况，从而引入额外的计算量。
- 保证最优路径
- 直线搜索可以保证当前路径的
g 值是最小的，确保启发式计算 (f=g+h) 正确。
- 对角线移动有更复杂的邻居检查（因为可能要绕开障碍物），所以放到后面处理更合理。
跳跃的规则：
- 不断的迭代的去往前推进
- 遇见“Forced Neighbor”或者“终点”时，停止跳跃并将当前点标记为 Jump Point 进行扩展。（从x节点直接跳到有forced neighbor的节点y上）
- 对角线跳跃：直线跳跃停止在障碍物（或边界上），跳跃失败，进行对角线上的跳跃到达y节点
- 进行水平和垂直方向的跳跃到z节点（z有forced neighbor），返回到上一层y节点，将y加入open list（优先级队列）
- 同样的，x还会天然具备一个forced neighbor 即w节点，则需要将w也加入open list
- x的分析完毕，从openlist弹出，加入close list
- 作者:spark
- 链接:http://sparkleaf.cn/article/1293c231-7c52-80a7-b851-f9c43fc6dbaa
- 声明:本文采用 CC BY-NC-SA 4.0 许可协议，转载请注明出处。
