【移动机器人运动规划】01 —— 常见地图基础 |图搜索基础

前言

本文部分内容参考了深蓝学院的移动机器人运动规划，依此做相关的笔记与整理。

相关代码整理:

1. https://gitee.com/lxyclara/motion-plan-homework/
2. https://github.com/KailinTong/Motion-Planning-for-Mobile-Robots/blob/master

相关文章：

1. 自动驾驶路径规划——Dijkstra算法
2. 自动驾驶路径规划——A*（Astar）算法
3. 【ROS-Navigation】—— Astar路径规划算法解析
4. 【Apollo学习笔记】—— Routing模块

可视化网址：

1. http://qiao.github.io/PathFinding.js/visual/
2. https://github.com/daohu527/osm-pathfinding或https://daohu527.github.io/

常用地图基础

规划时需要对环境信息进行处理，构建相应的数学模型，依据不同的策略处理环境信息，以便于对环境进行分析和计算，对路径进行搜索和优化。合理的地图建模方法有利于减少路径规划的计算量，从而加快运算速度，减少存储空间。下面将会介绍常用的地图。

Occupancy grid map

占据栅格地图
GitHub地址： https://github.com/ANYbotics/grid_map
地图特点：

1. Most Dense稠密
2. Structural结构化
3. Direct Index Query可以直接进行坐标索引查询
4. 栅格划分越细，所占用的内存空间越大

在进行路径规划时采用栅格表示地图，处理障碍物的边界时，避免了复杂的计算。它具有表示不规则障碍物的能力，并适用于所有类型的传统或智能路径搜索算法。

具体细节可参考：自动驾驶路径规划——基于MATLAB的栅格地图

2D栅格地图
2.5D栅格地图（海拔地图）

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
地图特点：

1. 障碍物之外距离场值为正,反之为负。
2. 只关注视锥之内

可以参考这篇博客：截断符号距离 | 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$维的空间包括了所有机器人配置的可能，被表示为 C-space ，任何一个可能的位姿在C-space中表述为一个点。

为什么需要配置空间呢？如下图所示，不同的机器人有着不同的形状和大小，若在一般的工作空间中进行碰撞检测，需要知道机器人的几何信息，这是十分浪费时间、算力且复杂的。

在配置空间C-space中，机器人被描述为一个质点（位置$position$被描述为$R^3$空间中的一个点，位姿$pose$被描述为$SO(3)$空间中的一个点）。通过将机器人视为一个质点，同时对障碍物按照机器人的形状进行适当碰撞，可以得到配置障碍空间，称为C-obstacle。C-obstacle需要在规划之前完成设置，且是一次性的。其余没有障碍物的空间，描述为自由空间C-free。显然，$C_{space}=C_{obstacle}\cup C_{free}$。

因此规划可以直接在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）+\epsilon h(n),\epsilon >1$
用最优换速度：得到次优解，但速度提升大
其代价值大致可估算为：cost(solution)<=εcost(optimal solution)
可视化的一个网站：http://qiao.github.io/PathFinding.js/visual/

AStar在工程实践中的注意点

1. 如何将栅格地图描述为图（4邻域/8邻域）
   
2. Priority queue in C++的构造技巧
   • std::priority_queue
   • std::make_heap
   • std::multimap
3. 合适的启发函数(尽可能贴近实际值)
   栅格地图高度结构化，可以计算出最近的距离，设计最tight的启发函数(对角启发函数)。
   $dx=abs(node.x-goal.x)$
   $dy=abs(node.y-goal.y)$
   $h=(dx+dy)+(\surd 2-2)\ast min(dx,dy)$
   
   path具有对称性
4. 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的唯一的区别在于如何寻找邻居

结论

1. 大多数情况下，JPS要比AStar快
2. JPS减少了OpenList的节点，但增加了节点查询的代价，在部分场景会比AStar慢不少。
3. 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

不做更改，再次编译，无误

按照步骤加载地图，如下