网站栏目名称,做营销网站视频,搜索引擎入口,天津短视频seo文章目录 前言相关代码整理:相关文章#xff1a; 可视化网址#xff1a;常用地图基础Occupancy grid mapOcto-mapVoxel hashingPoint cloud mapTSDF mapESDF mapFree-space RoadmapVoronoi Diagram Map 图搜索基础配置空间图搜索基本概念DijkstraAStarAstar的一些变种#x… 文章目录 前言相关代码整理:相关文章 可视化网址常用地图基础Occupancy grid mapOcto-mapVoxel hashingPoint cloud mapTSDF mapESDF mapFree-space RoadmapVoronoi Diagram Map 图搜索基础配置空间图搜索基本概念DijkstraAStarAstar的一些变种次优算法AStar在工程实践中的注意点 Jump Point Search(JPS)Look Ahead RuleJumping Rules示例伪代码结论 一些问题与解决方案问题1 PCL requires C14 or above 前言 本文部分内容参考了深蓝学院的移动机器人运动规划依此做相关的笔记与整理。 相关代码整理: https://gitee.com/lxyclara/motion-plan-homework/https://github.com/KailinTong/Motion-Planning-for-Mobile-Robots/blob/master 相关文章 自动驾驶路径规划——Dijkstra算法自动驾驶路径规划——A*Astar算法【ROS-Navigation】—— Astar路径规划算法解析【Apollo学习笔记】—— Routing模块 可视化网址 http://qiao.github.io/PathFinding.js/visual/https://github.com/daohu527/osm-pathfinding或https://daohu527.github.io/ 常用地图基础
规划时需要对环境信息进行处理构建相应的数学模型依据不同的策略处理环境信息以便于对环境进行分析和计算对路径进行搜索和优化。合理的地图建模方法有利于减少路径规划的计算量从而加快运算速度减少存储空间。下面将会介绍常用的地图。
Occupancy grid map
占据栅格地图 GitHub地址 https://github.com/ANYbotics/grid_map 地图特点
Most Dense稠密Structural结构化Direct Index Query可以直接进行坐标索引查询栅格划分越细所占用的内存空间越大
在进行路径规划时采用栅格表示地图处理障碍物的边界时避免了复杂的计算。它具有表示不规则障碍物的能力并适用于所有类型的传统或智能路径搜索算法。
具体细节可参考自动驾驶路径规划——基于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 地图特点
障碍物之外距离场值为正,反之为负。只关注视锥之内
可以参考这篇博客截断符号距离 | 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} CspaceCobstacle∪Cfree。
因此规划可以直接在C-free空间中进行而不用考虑机器人自身的几何信息。
图搜索基本概念
图、有向图、无向图等等概念在图论、数据结构中已有不少阐述这里就不赘述了。
对于图搜索问题首先便是要构造图用以状态描述。在栅格地图中每个栅格可以用作节点栅格之间的连接距离可以用作边在采样图中通常将采样点作为节点节点之间的连接被认为边。
有了图之后便可以进行搜索。对于图搜索问题其实就是从起点到终点寻得一条代价值最小的路径的问题。图搜索可以产生一个搜索树二者是等效的。通过回溯back-tracing终点到起点的节点便可以得到相应的路径。但通常图搜索问题难以被完全描述搜索树也难以被完全建立因此如何快速、有效地搜索到路径便是图搜索算法所面临的问题。 图搜索算法大体有如下一个框架设置一个集合用以存储待访问的节点集合首先会初始化将起点作为第一个节点接着会进入以下循环
移出节点节点被访问过后移出这个节点扩展搜索这个节点的邻居放入节点将这些邻居节点放入集合中
不断进行以上循环直到达到目标或相应的判断条件。循环终止条件可以是当上述集合为空时对于已访问过的节点通常加入到closed集中防止再次访问。
经典的搜索算法有BFS、DFS、Dijkstra等等可以参考这篇博客自动驾驶路径规划——Dijkstra算法。Greedy Best First Search和AStar可以参考这篇博客自动驾驶路径规划——A*Astar算法
Dijkstra
算法伪代码 PS1设置一个优先队列小顶堆应该也行 PS2Cnm为从n到m的cost
示例 优点完备性且最优 缺点1. 均匀扩散性地搜索2. 没有目标点的信息盲目性。
AStar 伪代码
示例 PSAStar的重点在于启发函数的设计估计值需要满足小于等于实际值
启发函数可行性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)gnεh(n),ε1 f(n)gnε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 ) dxabs(node.x −goal.x) dxabs(node.x−goal.x) d y a b s ( n o d e . y − g o a l . y ) dyabs(node.y −goal.y) dyabs(node.y−goal.y) h ( d x d y ) ( √ 2 − 2 ) ∗ m i n ( d x , d y ) h(dxdy)(√2−2)∗min(dx,dy) h(dxdy)(√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 C14 or above
初次编译遇到一系列报错其中不乏类似于以下的内容。
/usr/include/pcl-1.10/pcl/........
error: #error PCL requires C14 or above问题原因PCL版本产生的问题。对功能包CmakeLists中C11的部分替换为C14。主要替换的文件有 grid_path_searcher、waypoint_generator的CmakeLists.txt
# 如grid_path_searcher的CmakeLists中
set(CMAKE_CXX_FLAGS -stdc11 ${CMAKE_CXX_FLAGS} -O3 -Wall) # -# # Wextra -Werror
# 替换为
set(CMAKE_CXX_FLAGS -stdc14 ${CMAKE_CXX_FLAGS} -O3 -Wall) # -# # Wextra -Werror再次编译抛了一个warning 不做更改再次编译无误 按照步骤加载地图如下
