前言
本文部分内容参考了深蓝学院的移动机器人运动规划，依此做相关的笔记与整理。
相关代码整理:
- https://gitee.com/lxyclara/motion-plan-homework/
- https://github.com/KailinTong/Motion-Planning-for-Mobile-Robots/blob/master
- https://gitee.com/aries-wu/Motion-plan/blob/main/
- 链接: https://pan.baidu.com/s/1UtVHRxDq771LfSGK_21wgQ?pwd=rhtp 提取码: rhtp
相关文章：
- 自动驾驶路径规划——Dijkstra算法
- 自动驾驶路径规划——A*（Astar）算法
- 【ROS-Navigation】—— Astar路径规划算法解析
- 【Apollo学习笔记】—— Routing模块
可视化网址：
常用地图基础
规划时需要对环境信息进行处理，构建相应的数学模型，依据不同的策略处理环境信息，以便于对环境进行分析和计算，对路径进行搜索和优化。合理的地图建模方法有利于减少路径规划的计算量，从而加快运算速度，减少存储空间。下面将会介绍常用的地图。
Occupancy grid map
占据栅格地图
GitHub地址： https://github.com/ANYbotics/grid_map
地图特点：
- Most Dense稠密
- Structural结构化
- Direct Index Query可以直接进行坐标索引查询
- 栅格划分越细，所占用的内存空间越大
在进行路径规划时采用栅格表示地图，处理障碍物的边界时，避免了复杂的计算。它具有表示不规则障碍物的能力，并适用于所有类型的传统或智能路径搜索算法。
具体细节可参考：自动驾驶路径规划——基于MATLAB的栅格地图
Octo-map
八叉树地图
github地址： https://octomap.github.io/
地图特点：
• Sparse 稀疏
• Structural 结构化
• Indirect Index Query 非直接的查询，递归查询
Voxel hashing
哈希建图
github： https://github.com/niessner/VoxelHashing
地图特点：
• Most Sparse
• Structural
• Indirect Index Query
具体细节详见：Voxel Hashing阅读笔记
Point cloud map
点云地图
地址：http://pointclouds.org/
地图特点：
• Un-ordered 无序
• No Index Query
TSDF map
Truncated Signed Distance Functions
截断符号距离
地址： https://github.com/personalrobotics/OpenChisel
地图特点：
- 障碍物之外距离场值为正,反之为负。
- 只关注视锥之内
可以参考这篇博客：截断符号距离 | TSDF, Truncated Signed Distance以及这篇论文《Truncated Signed Distance Function: Experiments on Voxel Size》
ESDF map
欧几里得符号距离场
Euclidean Signed Distance Functions Incremental Update, Global Map
参考地址:
voxblox https://github.com/ethz-asl/voxblox
地图特点：
与TSDF相比不仅仅只关注视锥内。
Free-space Roadmap
地址:https://github.com/HKUST-Aerial-Robotics/Teach-Repeat-Replan
Voronoi Diagram Map
地址:https://github.com/ethz-asl/mav_voxblox_planning
图搜索基础
配置空间
机器人配置：对机器人一系列点的位置的描述
自由度（DOF）：用最少的坐标数量 n n n去描述机器人配置
机器人配置空间：一个 n n n维的空间包括了所有机器人配置的可能，被表示为 C-space ，任何一个可能的位姿在C-space中表述为一个点。
为什么需要配置空间呢？如下图所示，不同的机器人有着不同的形状和大小，若在一般的工作空间中进行碰撞检测，需要知道机器人的几何信息，这是十分浪费时间、算力且复杂的。
在配置空间C-space中，机器人被描述为一个质点（位置 p o s i t i o n position position被描述为 R 3 R^3 R3空间中的一个点，位姿 p o s e pose pose被描述为 S O ( 3 ) SO(3) SO(3)空间中的一个点）。通过将机器人视为一个质点，同时对障碍物按照机器人的形状进行适当碰撞，可以得到配置障碍空间，称为C-obstacle。C-obstacle需要在规划之前完成设置，且是一次性的。其余没有障碍物的空间，描述为自由空间C-free。显然， C s p a c e = C o b s t a c l e ∪ C f r e e C_{space}=C_{obstacle} \cup C_{free} Cspace=Cobstacle∪Cfree。
因此规划可以直接在C-free空间中进行，而不用考虑机器人自身的几何信息。
图搜索基本概念
图、有向图、无向图等等概念在图论、数据结构中已有不少阐述，这里就不赘述了。
对于图搜索问题，首先便是要构造图用以状态描述。在栅格地图中，每个栅格可以用作节点，栅格之间的连接距离可以用作边；在采样图中，通常将采样点作为节点，节点之间的连接被认为边。
有了图之后，便可以进行搜索。对于图搜索问题，其实就是从起点到终点寻得一条代价值最小的路径的问题。图搜索可以产生一个搜索树，二者是等效的。通过回溯（back-tracing）终点到起点的节点，便可以得到相应的路径。但通常图搜索问题难以被完全描述，搜索树也难以被完全建立，因此如何快速、有效地搜索到路径便是图搜索算法所面临的问题。
图搜索算法大体有如下一个框架：设置一个集合用以存储待访问的节点，集合首先会初始化，将起点作为第一个节点，接着会进入以下循环：
- 移出节点：节点被访问过后，移出这个节点
- 扩展：搜索这个节点的邻居
- 放入节点：将这些邻居节点放入集合中
不断进行以上循环，直到达到目标或相应的判断条件。循环终止条件可以是当上述集合为空时；对于已访问过的节点，通常加入到closed集中，防止再次访问。
经典的搜索算法有BFS、DFS、Dijkstra等等，可以参考这篇博客自动驾驶路径规划——Dijkstra算法。Greedy Best First Search和AStar可以参考这篇博客自动驾驶路径规划——A*（Astar）算法
Dijkstra
算法伪代码
PS1：设置一个优先队列（小顶堆应该也行）
PS2：Cnm为从n到m的cost
示例：
优点：完备性且最优
缺点：1. 均匀扩散性地搜索；2. 没有目标点的信息，盲目性。
AStar
伪代码
示例：
PS：AStar的重点在于启发函数的设计，估计值需要满足小于等于实际值
| 启发函数 | 可行性 |
|---|---|
| Euclidean distance (L2 norm) | 可行 |
| Manhattan distance (L1 norm) | 不一定，依据机器人运动学具体情况 |
| L∞ norm distance | 可行 |
| 0 distanc | 可行 |
Astar的一些变种（次优算法）
Weighted AStar: 代价函数 f ( n ) = g （ n ） + ε h ( n ) , ε > 1 f(n)=g（n）+εh(n),ε>1 f(n)=g（n）+εh(n),ε>1
用最优换速度：得到次优解，但速度提升大
其代价值大致可估算为：cost(solution)<=εcost(optimal solution)
可视化的一个网站：http://qiao.github.io/PathFinding.js/visual/
AStar在工程实践中的注意点
- 如何将栅格地图描述为图（4邻域/8邻域）
- Priority queue in C++的构造技巧
• std::priority_queue
• std::make_heap
• std::multimap - 合适的启发函数(尽可能贴近实际值)
栅格地图高度结构化，可以计算出最近的距离，设计最tight的启发函数(对角启发函数)。
d x = a b s ( n o d e . x − g o a l . x ) dx=abs(node.x −goal.x) dx=abs(node.x−goal.x)
d y = a b s ( n o d e . y − g o a l . y ) dy=abs(node.y −goal.y) dy=abs(node.y−goal.y)
h = ( d x + d y ) + ( √ 2 − 2 ) ∗ m i n ( d x , d y ) h=(dx+dy)+(√2−2)∗min(dx,dy) h=(dx+dy)+(√2−2)∗min(dx,dy)
path具有对称性 - Tie Breaker（打破平衡）
轻微放大启发函数。理论上可能会出现非完备性，实际上轻微的放大几乎不会有影响。
相同f比较h/增加固定的随机cost，并用哈希表实现/增加倾向性
Jump Point Search(JPS)
找到对称性并打破它们
Look Ahead Rule
Jumping Rules
PS1:pruning rule就是 Look Ahead Rule
PS2:先走垂直方向，再走对角线
PS3:Jumping Straight情况下，移动到y时，z是y的force neighbor,可以认定y节点是关键节点，加入到openlist中；Jumping Diagonally情况下，z是一个特殊节点，具有force neighbor,返回到上一节点y，将y加入到openlist中。
示例
伪代码
与AStar的唯一的区别在于如何寻找邻居
结论
- 大多数情况下，JPS要比AStar快
- JPS减少了OpenList的节点，但增加了节点查询的代价，在部分场景会比AStar慢不少。
- JPS大多只能在结构化的地图上进行搜索。
一些问题与解决方案
本文基于ROS neotic进行相关实验（本来机子是Ubuntu18.04的，因为显卡驱动的问题，重新安装了Ubuntu20.04），和原教程相比会遇到一些问题，现给出可能遇到的问题以及相应的解决方案，也期待与大家一同解决棘手的问题，共同探讨与进步。
问题1 PCL requires C++14 or above
初次编译，遇到一系列报错，其中不乏类似于以下的内容。
/usr/include/pcl-1.10/pcl/........
error: #error PCL requires C++14 or above
问题原因：PCL版本产生的问题。对功能包CmakeLists中C++11的部分替换为C++14。主要替换的文件有 grid_path_searcher、waypoint_generator的CmakeLists.txt
# 如grid_path_searcher的CmakeLists中
set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS} -O3 -Wall") # -# # Wextra -Werror
# 替换为
set(CMAKE_CXX_FLAGS "-std=c++14 ${CMAKE_CXX_FLAGS} -O3 -Wall") # -# # Wextra -Werror
再次编译，抛了一个warning
不做更改，再次编译，无误
按照步骤加载地图，如下
问题2 Diagonal函数的三维推导
类比二维情况，首先选出最短的边，斜对角最快达到，然后类似于二维进行设计。为了便于编程，如下设计：
问题3 目标点的设置
3D New Goal是一个可以给出目标点四维信息的插件(x,y,z,raw)
使用方法:首先鼠标左键点击地图一个点，确定x,y，之后可以旋转鼠标，确定raw;接着同时按住鼠标右键滑动，可以确定z。
部分结果
红色JPS，绿色AStar
