-
2021-08-08 10:56:16
前言
本文参考文献1整理几篇有关智能车路径规划与控制算法的文章,便于后续研究。
路径规划分为局部路径规划和全局路径规划,全局路径规划根据起点,终点和地图生成可行驶路线;局部路径规划则需要考虑动态障碍物,解决避障问题。
一、路径规划
1.全局路径规划
全局路径规划算法可以分为
- 传统算法2
- 智能算法
2、局部路径规划
局部路径规划算法有
二、轨迹跟踪
轨迹跟踪控制器分为基于控制理论的控制器和基于几何运动学的控制器。32
车辆运动学模型的论文见35
总结
本文主要列出无人车路径规划与控制的几种常用算法,以及参考文献。
学习笔记
特定环境下低速无人车的路径规划与控制研究_朱景璐 ↩︎
李永丹,马天力,陈超波,韦宏利,杨琼楠. 无人驾驶车辆路径规划算法综述[J]. 国外电子测量技术,2019,38(06):72-79. ↩︎
W. Dijkstra. A note on two problems in connexion with graphs[J]. Numersische Mathematik,1959,1:269-271. ↩︎
Z. Liu,J. Chen,C. Song and L. Ding. An Improved Algorithm for Searching Local Optimal Path in Intelligent Transport System[C]. 2008 Second International Symposium on Intelligent Information Technology Application,Shanghai,2008: 157-161. ↩︎
Kang-le Wang,Shu-wen Dang,Fa-jiang He,Peng-zhan Cheng. A Path Planning Method for Indoor Robots Based on Partial & Global A-Star Algorithm[C]. 2017 5th International Conference on Frontiers of Manufacturing Science and Measuring Technology (FMSMT 2017),2017:385-389. ↩︎
陈伟华,林颖,文宗明,等.基于双重A*算法的移动机器人动态环境路径规划[J]. 组合机床与自动化加工技术,2018(4): 127-130. ↩︎
梁昭阳,蓝茂俊,陈正铭.A*算法在Shortest-Path方面的优化研究[J]. 计算机系统应用,2018, 27(7): 255-259. ↩︎
赵鑫,胡 广 地.平 滑ARA*算法 在 智 能 车 辆 路 径 规划的 应 用 [J].机械科学与技术 ↩︎
任玉洁,付丽霞,张勇,等.拓展搜索邻域的平滑A*算法机器人路 径 规 划[J].电子科技. ↩︎
WAGNER G, CHOSET H. M* : A complete multi-robot path planning algorithm with .optimality bounds[J]. Redundancy in Robot Manipulators and Multi- Robot Systems,2011, 57(1) .3260-3267. ↩︎
ISLAM F, NARAYANAN V,LIKHACHEV M. Dynamic
multi heuristic A [ C]. IEEE International Conference on Robotics and Automation, 2015:2376-2382. ↩︎Anthony Stentz. The Focussed D* Algorithm for Real-Time Replanning[C]. International Joint Conference on Artificial Intelligence,1995. ↩︎
刘军,冯硕,任建华. 移动机器人路径动态规划有向 D~*算法[J]. 浙江大学学报(工学版),2020,54(02):291-300. ↩︎
陈秋莲,蒋环宇,郑以君. 机器人路径规划的快速扩展随机树算法综述[J]. 计算机工程与应用,2019,55(16):10-17. ↩︎
Iram Noreen,Amna Khan,Zulfiqar Habib. Optimal Path Planning using RRT* based Approaches: A Survey and Future Directions[J]. International Journal of Advanced Computer Science and Applications. 2016,7(11):97-107. ↩︎
S. Karaman,E. Frazzoli. Sampling-based algorithms for optimal motion planning[J]. Computer Science,2011,30:846-894. ↩︎
S. Karaman,M. Walter,A. Perez,E. Frazzoli and S. Teller. Anytime motion planning using the RRT*[C]. 2011 IEEE International Conference on Robotics and Automation (ICRA),2011:1478-1483. ↩︎
郭聪.基于RRT的无人机三维航迹规划算法研究[D].沈阳:沈阳航空航天大学,2015. ↩︎
T. Arora,Y. Gigras,and V. Arora. Robotic path planning using genetic algorithm in dynamic environment[J]. International Journal of Computer Applications.2011,89: 8-12. ↩︎
Y. Wang,P. Chen and Y. Jin. Trajectory planning for an unmanned ground vehicle group using augmented particle swarm optimization in a dynamic environment[C]. 2009 IEEE International Conference on Systems, Man and Cybernetics. 2009:4341-4346. ↩︎
Khatib O. Real-time obstacle avoidance for manipulators and mobile robots[J]. International Journal of Robotics Research,1986,5(1):90-98. ↩︎
唐小洁,丁一航,申勤,贾成芬. 改进人工势场法的移动小车动态避障路径规划[J]. 软件导刊,2019,18(10):152-156+225. ↩︎
陈浩,喻厚宇. 基于势场搜索的无人车动态避障路径规划算法研究[J]. 北京汽车,2019(04):1-5+31. ↩︎
Surmann H , Huser J , and Wehking J. Path planning for a fuzzy controlled autonomous mobile robot[C]. IEEE International Conference On Fuzzy Systems,1996,2:1660-1665. ↩︎
申燕凯. 轮式机器人路径规划及控制方法研究[D].河北科技大学,2016. ↩︎
胡永仕,张阳.基于遗传模糊算法的智能车辆避障路径规划研究[J]. 福州大学学报(自然科学版),2015,43(02):219-224. ↩︎
李擎,张超,韩彩卫,张婷,张维存. 动态环境下基于模糊逻辑算法的移动机器人路径规划[J].中南大学学报(自然科学版),2013,44(S2):104-108. ↩︎
李擎,张超,韩彩卫,徐银梅,尹怡欣,张维存.静态环境下基于模糊逻辑算法的移动机器人路径规划[C]. 第 25 届中国控制与决策会议论文集,2013:2867-2872. ↩︎
Boujelben M, Ayedi D, Rekik C, et al. Fuzzy logic controller for mobile robot navigation to avoid dynamic and static obstacles[C]. 2017 14th International Multi-Conference on Systems, Signals and Devices( SSD) ,2017:293-298. ↩︎ ↩︎
吴国鹏. 基于 2D 激光传感器的移动机器人室内环境建模与自主导航研究[D]. 华南理工大学,2019. ↩︎
屈盼让,薛建儒,朱耀国,肖鹏. 面向无人车运动规划问题的 VFH 算法[J]. 计算机仿真,2018,35(12):245-251. ↩︎
单云霄. 城市无人驾驶规划与控制系统的关键技术研究[D]. 武汉大学,2018. ↩︎
THRUN S,MONTEMERLO M,DAHLKAMP H. Stanley: the robot that won the DARPA grand challenge[J]. Journal of Field Robotics,2006,23 (9):661-692. ↩︎
张梦巍. 智能无人驾驶车辆路径跟踪及底层控制方法研究[D]. 沈阳理工大学,2018. ↩︎
Wilko Schwarting, Javier Alonso-Mora, and Daniela Ru. Planning and Decision-Making for Autonomous Vehicles[J]. Annual Review of Control, Robotics, and Autonomous Systems,2018(1):187-210. ↩︎
更多相关内容 -
无人车路径规划算法matlab+python代码.rar
2020-07-09 15:48:34无人车路径规划算法matlab+python代码.rar -
基于Astar算法的无人车路径规划的MATLAB仿真,matlab2021a测试。
2022-05-01 05:03:04基于Astar算法的无人车路径规划的MATLAB仿真,matlab2021a测试。 -
【无人驾驶——路径规划】第二章 无人车路径规划
2021-07-11 15:26:18无人车路径规划的定义: A点到B点 ,构建一个车辆运动轨迹,结合HDMap,Localization 和Prediction 输出:可行驶轨迹,有一系列点组成 两个层面:导航层面; 运动轨迹层面 Routing routing的目标是导航一条A到B的...系列文章目录
文章目录
定义
无人车路径规划的定义:
- A点到B点 ,构建一个车辆运动轨迹,结合HDMap,Localization 和Prediction
- 输出:可行驶轨迹,有一系列点组成
- 两个层面:导航层面; 运动轨迹层面
Routing
routing的目标是导航一条A到B的全局路径,一条cost最小的路径。
输入:地图(网络信息,交通信息等)、当前位置、目的地(乘客决定)
输出:可行驶道路的连线
搜索:地图数据转化成图网络
* 节点表示道路
* 边表示路口
什么情况下cost高?:
权重规则:例如左转的权重相较于直行的权重更高,所以Node1到Node4的边权重大,Node1到Node3权重小。
拥堵情况:比如说Node1到Node3的道路很拥堵,那么它的cost就高;Node4的道路更堵,那么Node1到Node4的cost更高。
A*算法
在rounting中目前A*算法的应用还是非常广泛的。
公式:F(n) = G(n) + H(n)F(n)表示道路的routing的总cost
G(n)表示起始点到候选点的cost
H(n)表示候选点通过启发函数得到的目标点costMotion Planning
导航信息相当于给了粗略的路径信息,而Planning相当于一个高精度,低级别的search
规划的约束条件
- Collision 碰撞(躲避任何障碍物)
- Comfortable 舒适 (路径点必须平滑,速度也要平滑)
- 运动学约束(高速转弯,掉头曲率角度)
- Illegal合法 (交通法规)
Cost Function
- cost function由许多部分组成
- 道路偏离中心线
- 碰撞或者靠得太近
- 速度太大,超速
- Curvature太大,方向盘太急
针对不同的场景,我们可能有多个不同得cost
Frenet坐标系
一般情况下我们会用笛卡尔坐标系(世界坐标系),但是表征的东西并不全面。因此在道路形式方面,我们采用Frenet坐标系,能够更好地表征偏离道路中心线的距离。
【注】Frenet坐标系和Track坐标系的区别
* L方向不同
* Track是基于Road级别
* Frenet是基于Lane级别Path 和 Speed解耦
- Path和Speed解耦能够让我们将motion planning问题转化为多个凸优化问题。
Path Planning
步骤
- 先生成道路网格
- 每个网格单元中随机采样
- 每个网格选一个点
- 组成多条候选Path
Cost Function
需要cost最低的path,也就是最优path,cost的设计往往是planning的重点
- 中心线距离 l*a0
- 障碍物距离 d*a1
- 速度变化率 acc * a2
- 曲率 kappa * a3
- F(x) = la0 + da1 + acc * a2 + acc * a3 + a4
【思考】为什么线性加权可以在一定程度上解决所有问题呢?
Speed Planning
ST图
S-T图表示在path上的速度规划,S表示Path上的纵向距离,T表示运动时间。
斜率越大,表示速度越快。
如何规划ST轨迹
-
连续空间离散化(grid map)
-
单元格内速度不变
-
把障碍物投影进来
- 将挡住我们Path轨迹的部分画进ST图中
- 因此必须要有良好的轨迹预测
- 例如下图中,to-t1时刻障碍物会在我们的Path轨迹中挡住s0,s1部分,(如何理解黄色部分? 相当于t0-t1时刻,s0-s1这块区域是不能有车通行的)
-
速度曲线不能碰到这个区域
- 有多个车的情况
如何优化
由于折线并不平滑,我们需要将不平滑的折线优化成平滑的线性曲线。
- QP (Quadratic Programming) 二次规划
- 这个方法很大程度上依赖于线性空间离散化
- 将平滑的非线性曲线与这些线段进行拟合
- 现成的工具:qpQASES
轨迹规划
实例
-
遇到一辆速度很慢的车我们如何超车?
-
生成很多轨迹(撒点采样)
-
用cost function对其进行评估,选择cost最小的一条
-
生成一个ST图来表述速度规划
-
生成多条速度曲线
-
使用优化工具对多条速度采样进行最优化求解
- 让整个线路变得平滑
- 让整个线路变得平滑
-
最后将每个轨迹点(跟我们自己定义的轨迹点Resolution)的Path,Speed合并得到最终结果。
-
Coverage_path_planning-master_无人车路径规划_源码
2021-10-04 00:42:41在matlab中实现路径规划,部分源代码供参考。 -
无人车路径规划算法—(3)基于搜索的路径规划算法 (BFS/DFS/Dijkstra)
2021-11-23 23:05:26无人车路径算法—(3)基于搜索的路径规划算法 I 标签(空格分隔): Planning_Algorithm 1.BFS(广度优先搜索) && DFS(深度优先搜索) 广度优先遍历图的方式为,一次性访问当前顶点的所有未访问状态相邻...1.BFS(广度优先搜索) && DFS(深度优先搜索)
- 广度优先遍历图的方式为,一次性访问当前顶点的所有未访问状态相邻顶点,并依次对每个相邻顶点执行同样处理。因为要依次对每个相邻顶点执行同样的广度优先访问操作,所以需要借助队列结构来存储当前顶点的相邻顶点。(队结构,先入先出)
- 深度优先遍历图的方式,同样会访问一个顶点的所有相邻顶点,不过深度优先的方式为,首先访问一个相邻顶点,并继续访问该相邻顶点的一个相邻顶点,重复执行直到当前正在被访问的顶点出度为零,或者不存在未访问状态的相邻顶点,则回退到上一个顶点继续按照该深度优先方式访问。因为存在回溯行为,所以需要借助栈结构保存顶点,或者直接利用递归调用产生的方法栈帧来完成回溯。(栈结构,先入后出)
- 当前路径规划相关的图搜索方法大多是基于广度优先搜索的原理来进行拓展的
1.1 BFS 实现方式
- 选择起点放入队列中,并标记为已访问
- 判断队列是否为空,当队列不为空时,从队列中取出顶点作为拓展点,随后将拓展点的所有相邻且未被访问过的节点放入队列,并标记为已访问;
- 重复步骤2直至队列为空或搜索到目标点
1.2 DFS 实现方式
- 选择起点放入栈中,并标记为已访问
- 判断栈是否为空,当栈不为空时,将栈顶元素作为拓展点,随后搜索拓展点的相邻且未被访问过的节点,若存在,则将该节点入栈,否则则将拓展点出栈
- 重复步骤2 直至栈为空或者搜索到目标点
由于BFS以及DFS都是较为基础的搜索算法,因此是较难满足无人车或者机器人的路径规划需求的。因此在此思想基础上,衍生出了众多基于栅格地图(grid map或costmap)的搜索算法。其中,包括了大家都熟知的Dijkstra算法以及A*算法。
2.Dijkstra算法
2.1 常规的Dijkstra方法介绍:
通常,在进行路径规划时,我们的所规划的栅格地图内可能存在栅格距离起点的move_cost不同的情况。譬如,地图内如果某片区域是草地,或者窄道,或者其他不易通行的区域,那么我们会自然将此区域距离起点的move_cost增加。当地图内所有的move_cost都相同的时候,此时的Dijkstra方法可以认为与BFS相同。
具体效果区别如下图所示:
- 其中,绿色区域为可行驶但是move_cost较高区域,深色区域为不可行驶区域
- 左图为BFS的搜索效果
- 右图为Dijkstra的搜索效果
- 可以看出Dijkstra更加考虑距离起点的cost(cost_so_far)
因此,在涉及具体存储的数据结构时,Dijkstra引入了优先队列用以存储节点的搜索信息。
Dijkstra算法步骤如下:维护一个优先队列,用以存储所有需要被扩展的节点
2. 初始化起点,并将起点放入优先队列中
3. 设置起点的move_cost g( n s n_s ns) = 0, 其余节点的g(n) = infinite
4. 执行以下循环:- 如果优先队列为空,则返回false,终止搜索
- 弹出优先队列中 g值最小的节点n并将此节点标记为已拓展
- 如果该节点为目标点,return true,终止搜索
- 对于节点 n 的所有未拓展的邻节点m:
- 如果g(m) = infinite
- g(m) = g(n) + Cnm
- 将node m 放入优先队列中
- 如果g(m) > g(n) + Cnm :
- g(m) = g(n) + Cnm
2.2 ROS Navigation中Dijkstra源码解析
与常规Dijkstra的区别:
- 利用势场的方法来计算每个栅格的分数
- 采用倒叙(从终点向起点)的方法进行拓展
- 优点:生成的路径更加平滑
- 缺点:容易产生S型路径,且障碍物过多时会出现不必要的折线
- 缺点: 容易陷入局部最优无法脱困
costmap范围标记
/** * @实际的costmap坐标系可能原点坐标在左下角 * @brief outline the boundary in the costmap * @param costarr: one dimension cost value array of costmap * @param nx: size of x in index coordinate * @param ny: size of y in index coordinate * @param value : in this function is 255 or lethal value = 254 */ void OutlineMap(unsigned char* costarr, int nx, int ny, unsigned char value) { unsigned char* pc = costarr; for (int i = 0; i < nx; i++) *pc++ = value; // in x direction in first column (from (0,0) to (10,0)) pc = costarr + (ny - 1) * nx; // move to last row first position((0,7)) for (int i = 0; i < nx; i++) *pc++ = value; // in x direction in last column (from (0,7) to (10,7) ) pc = costarr; // (0,0) for (int i = 0; i < ny; i++, pc += nx) *pc = value; // in y direction in first row (from (0,0) to (0,7)) pc = costarr + nx - 1; // in y direction in last row (10,0) for (int i = 0; i < ny; i++, pc += nx) *pc = value; // in y direction in last row (from (10,0) to (10,7)) }
calculate potential/ 势场的计算
此处主要是基于ETH的一篇文献进行的优化,优化后的potential 扩散会由原来的矩形扩散优化为圆形扩散。
文献名称如下:- A Light Formulation of the E∗Interpolated Path Replanner
最终的理论公式:
v = − 0.2301 d d + 0.5307 d + 0.7040 p o t = t a + h f ∗ v v = -0.2301dd + 0.5307d + 0.7040 \\ pot = ta + hf * v v=−0.2301dd+0.5307d+0.7040pot=ta+hf∗v
其中,变量的物理意义可以参考以下代码,代码中有对应的推导及说明float CalculatePotential(int cell_index) { // get neighbors float u, d, l, r; l = potential_[cell_index - 1]; r = potential_[cell_index + 1]; u = potential_[cell_index - cell_x_]; d = potential_[cell_index + cell_x_]; // find lowest, and its lowest neighbor // ta lowest cost in same column, tc lowest cost in same row float ta, tc; if (l < r) tc = l; else tc = r; if (u < d) ta = u; else ta = d; float hf = getCost(charmap_, cell_index); // traversability factor float dc = tc - ta; // relative cost between ta,tc if (dc < 0) // tc is lowest { dc = -dc; ta = tc; } // calculate new potential if (dc >= hf) // if too large, use ta-only update return ta + hf; else // two-neighbor interpolation update { // use quadratic approximation float d = dc / hf; float v = -0.2301 * d * d + 0.5307 * d + 0.7040; return ta + hf * v; } }
双线性差值进行梯度计算
从终点开始逆向搜索,为了减少栅格对于路径精度的影响,这里采用了利用双线性差值的的方法来计算梯度从而达到搜索对应路径节点而不是栅格的目的。
具体步骤为:- 将终点设置为当前计算节点 node_cur,并更新world坐标与取整后坐标的dx,dy差距
- 如果当前坐标与起点坐标相差小于预设阈值,则认为搜索成功
- 计算node_cur的梯度,node_cur右侧相邻节点梯度,node_cur上侧相邻节点梯度,node_cur右上相邻节点梯度
- 利用双线性差值原理计算实际计算节点world坐标对应的梯度值
- 计算沿梯度搜索对应的实际坐标并更新node_cur以及dx,dy值
- 重复第二步
补充知识点:
bilinear interpolation method
参考链接: https://blog.csdn.net/xbinworld/article/details/65660665
首先,参考线性插值原理:
已知坐标 ( x 0 , y 0 ) , ( x 1 , y 1 ) (x_0, y_0), (x_1, y_1) (x0,y0),(x1,y1), 计算区间 [ x 0 , x 1 ] [x_0, x_1] [x0,x1]内某一坐标 x x x在直线上的$y $值, 可得公式:
y − y 0 x − x 0 = y 1 − y 0 x 1 − x 0 \frac{y-y_0}{x-x_0} = \frac{y_1 - y_0}{x_1 - x_0} x−x0y−y0=x1−x0y1−y0
推导上述公式可得:
y = x 1 − x x 1 − x 0 y 0 + x − x 0 x 1 − x 0 y 1 y = \frac{x_1 - x}{x_1 - x_0}y_0 + \frac{x-x_0}{x_1 - x_0}y_1 y=x1−x0x1−xy0+x1−x0x−x0y1
由此类比,推出双线性插值原理:
双线性插值是有两个变量的插值函数的线性插值扩展,其核心思想是在两个方向分别进行一次线性插值,图例见下图:
由上图, 已知四点 a , b , c , d {a, b, c, d} a,b,c,d及函数 f ( x , y ) f(x,y) f(x,y),首先在x方向上进行插值
f ( f ) = x 2 − x x 2 − x 1 f ( a ) + x − x 1 x 2 − x 1 f ( d ) f(f) = \frac{x_2 - x}{x_2 - x_1}f(a) + \frac{x - x_1}{x_2 - x_1}f(d) f(f)=x2−x1x2−xf(a)+x2−x1x−x1f(d)
f ( e ) = x 2 − x x 2 − x 1 f ( b ) + x − x 1 x 2 − x 1 f ( c ) f(e) = \frac{x_2 - x}{x_2 - x_1}f(b) + \frac{x - x_1}{x_2 - x_1}f(c) f(e)=x2−x1x2−xf(b)+x2−x1x−x1f(c)接着在y方向上进行插值
f ( g ) = y 2 − y y 2 − y 1 f ( f ) + y − y 1 y 2 − y 1 f ( e ) f(g) = \frac{y_2-y}{y_2-y_1}f(f) + \frac{y-y_1}{y_2-y_1}f(e) f(g)=y2−y1y2−yf(f)+y2−y1y−y1f(e)f ( g ) = f ( x , y ) = ( x 2 − x ) ( y 2 − y ) ( x 2 − x 1 ) ( y 2 − y 1 ) f ( a ) + ( x − x 1 ) ( y 2 − y ) ( x 2 − x 1 ) ( y 2 − y 1 ) f ( d ) + ( x 2 − x ) ( y − y 1 ) ( x 2 − x 1 ) ( y 2 − y 1 ) f ( b ) + ( x − x 1 ) ( y − y 1 ) ( x 2 − x 1 ) ( y 2 − y 1 ) f ( c ) f(g) = f (x,y) = \frac{(x_2 - x)(y_2 -y)}{(x_2-x_1)(y_2- y_1)} f(a) + \frac{(x - x_1)(y_2 -y)}{(x_2-x_1)(y_2- y_1)} f(d) + \frac{(x_2 - x)(y - y_1)}{(x_2-x_1)(y_2- y_1)} f(b) + \frac{(x - x_1)(y -y_1)}{(x_2-x_1)(y_2- y_1)} f(c) f(g)=f(x,y)=(x2−x1)(y2−y1)(x2−x)(y2−y)f(a)+(x2−x1)(y2−y1)(x−x1)(y2−y)f(d)+(x2−x1)(y2−y1)(x2−x)(y−y1)f(b)+(x2−x1)(y2−y1)(x−x1)(y−y1)f(c)
下一期将介绍图搜索算法中具有heuristic function的相关算法
-
无人车路径规划算法---(2)地图
2020-01-20 17:32:05上一篇博客 "无人车路径规划算法—(1)算法体系综述 " 中介绍了无人车路径规划通常采用的"前端生成路径->后端平滑优化"的pipeline,本文将为大家介绍路径规划算法常用地图格式,文章主要分为以下三个模块: ...上一篇博客 "无人车路径规划算法—(1)算法体系综述 " 中介绍了无人车路径规划通常采用的"前端生成路径->后端平滑优化"的pipeline,本文将为大家介绍路径规划算法常用地图格式,文章主要分为以下三个模块:
- 常用地图格式
- 栅格地图
- 拓扑地图与高精度地图
生成一个随机二维栅格地图方法的代码将开源在博主github主页
地图(MAP)
无人车的规划模块是联通感知模块,定位模块与控制模块的中间模块(架构图见图一),因此,基于搜索的规划算法通常是在能够包含无人车自身定位信息以及周边环境信息的数据结构体中进行运算,这种能够体现无人车周边环境信息的数据载体统称为地图(MAP),在介绍搜索具体的搜索算法之前,需要先引入搜索地图的概念以及常用的地图类型。
本部分将重点说明以下四种常见形式:
- Point Cloud Map(点云地图)
- Metric Map(度量地图)
- Topological Map(拓扑地图)
- Semantic Map(语义地图)
1. Point Cloud Map(点云地图)
说到自动驾驶的地图,大家常最先想起的就是激光雷达给出的点云地图,但是原始点云地图通常不会被拿来进行路径规划使用,主要是由于以下原因:
- 点云地图通常规模很大,需要大量的存储空间,同时点云地图提供了很多不必要的细节
- 处理多个点云重叠处的效果不好
因此,需要利用costmap或者octomap进一步的处理(见第二部分)。
2. Metric Map(度量地图)
度量地图框架是最常见的框架,它考虑的是目标对象的二维空间。用精确的坐标表示目标对象以及各个目标之间的相对关系。这种表示方法非常有效,但其对噪声敏感,并且很难计算距离精度。譬如在地图中,北京可以用固定的经纬度来表示,上海可以用另一个固定的经纬度来表示,两者之间的距离可以通过各自的经纬度推算出来。
通常我们用稀疏(Sparse)与稠密(Dense)对它们进行分类:1- 稀疏地图:对地图信息进行一定程度的抽象,只保留需要关注的信息,对次要信息进行简化
- 稠密地图:对所有地图中所有见到的信息进行描述,对于无人车的路径规划模块来说,稠密地图是十分必要的,通常我们将所有的有效信息通过设定的分辨率,分割到每个固定的栅格中(二维的栅格称为Grid,三维的栅格称为Voxel)。每个栅格由占有,空闲,与未知三种状态构成以表述对应栅格的障碍物占据情况。但是我们也看到,这种地图需要存储每一个格点的状态,耗费大量的存储空间,而且多数情况下地图的许多细节部分是无用的。
在度量地图中,与无人车规划模块或机器人导航模块应用最广泛的则是占据栅格地图(Occupancy Grid Map)与代价地图(CostMap)
2.1 Occupancy Grid Map (占据栅格地图)
占据栅格地图是把环境划分成一系列栅格,其中每一栅格给定一个可能值,表示该栅格被占据的概率。占据栅格的构建主要是基于二值贝叶斯滤波原理利用多传感器(激光雷达,相机等)的输入判断地图内每个栅格的障碍物占据概率。
由于占据栅格地图通常是单层结构,更适用于简单的路径规划功能,当机器人或无人车对于环境信息精确度要求不高时可以利用此地图进行规划,当规划模块对精度与安全性要求较高时,更好的选择是利用代价地图(CostMap)进行规划。2.2 CostMap (代价地图)
Costmap是无人车或者机器人融合多传感器信息后建立维护的二维或三维栅格地图,在地图中障碍物信息会被转换到对应的栅格中并且利用相应的规则对障碍物进行膨胀。ROS的官方Navigation库中最早利用costmap进行障碍物的位置判断以及在此基础上利用DWA(原理可参见我之前的系列博客)算法进行了局部规划,后来在日本的开源框架Autoware中也基于costmap实现了乘用车的局部规划功能(HybridAstar算法)。
ROS官方Navigation库中对于costmap说明文档如下:http://wiki.ros.org/costmap_2d/
官方的说明文档中有以下两幅图需要重点关注:- 地图结构说明图 (见图一)
- 障碍物膨胀(inflation)与栅格分数关系图(见图二)
下图(图一)可以大致描述出costmap地图中的三个主要组成要素:
- Footprint (红色多边形)
- 障碍物的膨胀层 (蓝色栅格)
- 障碍物位置 (红色栅格)
下图(图二)详细的解释了障碍物膨胀区栅格分数的取值依据,其中红色多边形为车辆轮廓:
- Lethal:机器人的中心与障碍物所占据栅格重合,此时机器人必然与障碍物冲突
- Inscribed:障碍物所占据栅格与机器人footprint内切圆重合,此时机器人必然与障碍物冲突
- Possibly circumscribed:机器人外接圆与障碍物占据栅格相切或接近,因此是否碰撞取决于机器人的朝向等因素
- Freespace:自由空间,无碰撞风险
- Unknow:未知区域
- 其他的栅格的分数可以根据与Lethal以及Freespace的距离来计算,也可以根据自身产品设计相应的衰减函数来推算
介绍完costmap自身构建原理之后,另外一个值得注意的重点则是栅格地图坐标系与world坐标系转换问题:
单层地图由许多个cell组成,一般可以认为单层地图有两个坐标系:位置坐标系(以m为单位)和索引坐标系(以cell为单位)。在地图构造完成后,会有索引坐标系到位置坐标系的转换关系,所以在定位某个cell的时候可以使用任意坐标系,不存在本质性的区别。
转换关系如下图所示2:
- 地图坐标系(map frame,坐标系原点即map center,其在world中的位置就是mapPosition,X正向上,Y正向左)
- 内存坐标系(原点为内存块的左上角,即第一个存储的cell,可以近似理解为图像坐标系,X正向下,Y正向右)
样例代码片段如下:
/** *@ This Section Is For Transform pose from world frame to index frame *@ This Is The First Method */ gemoetometry_msgs::Pose transformPose(const geometry_msgs::Pose& pose, const tf::Transform& tf) { // convert ROS pose to TF pose tf::Pose tf_pose; tf::poseMsgToTF(pose, tf_pose); // transform pose tf_pose = tf * tf_pose; // convert TF pose to ROS pose geometry_msgs::Pose ros_pose; tf::poseTFToMsg(tf_pose, ros_pose); return ros_pose; } void poseToIndex(const geometry_msgs::Pose& pose, int* index_x, int* index_y) { tf::Transform orig_tf; tf::poseMsgToTF(costmap_info.origin, orig_tf); geometry_msgs::Pose pose_new = transformPose(pose, orig_tf.inverse()); *index_x = pose_new.position.x / costmap_info.resolution; *index_y = pose_new.position.y / costmap_info.resolution; }
/** *@ This Section Is For Transform pose from world frame to index frame *@ This Is The Second Method */ void poseToIndex(const geometry_msgs::Pose& pose, int* index_x, int* index_y) { *index_x = (pose.position.x - costmap_.info.origin.position.x) / costmap_.info.resolution; *index_y = (pose.position.y - costmap_.info.origin.position.y) / costmap_.info.resolution; }
/** *@ This Section Is For Transform pose from index frame to world frame */ void indexToPose(int index_x , int index_y , geometry_msgs::Pose& pose) { double px, py; px = costmap_.info.origin.x + (*index_x + 0.5) * costmap_.info.resolution; py = costmap_.info.origin.y + (*index_y + 0.5) * costmap_.info.resolution; pose.position.x = px; pose.position.y = py; }
2.3 开源库分享
grid_map 开源库:
- grid_map库是设计有ROS接口的C++库,用于管理具有多个数据层的二维栅格地图。它是为移动机器人地图设计的,用于存储(海拔)高度、方差、颜色、摩擦系数、可通过性等数据。它用于为粗糙地形导航设计的以机器人为中心的高程地图包中。
- 链接:https://github.com/ANYbotics/grid_map
Octomap 开源库:
- Octomap是SLAM领域经常用到的地图结构,它以八叉树数据结构来存储三维环境的概率占据地图,采用此结构可以节省大量的存储空间同时可以实现地图的压缩与更新,并且地图的分辨率可调。
- 链接:https://github.com/OctoMap/octomap
2.4 式例代码
编写了一个随机生成障碍物的二维地图 (跳票中。。。。最近活太多。。。正在努力填坑中,后面也会找时间把这段时间做的东西整理一下分享出来)
3 Topological Map (拓扑地图)
拓扑地图则更强调地图元素之间的关系。拓扑地图是一个图(Graph),由节点和边组成,只考虑节点间的连通性。它放松了地图对精确位置的需要,去掉地图的细节问题,是一种更为紧凑的表达方式。这种地图更适用于低纬度的路径规划,因此接触的不多,有经验的网友欢迎补充。
4 Semantic Map (语义地图)
语义地图个人理解可以分为ADAS MAP和高精度地图(HD MAP),ADAS地图是级别低于高精度地图的一种包含语义信息的地图,我们这里还是主要介绍一下高精度地图。
高精度地图主要服务与自动驾驶车辆(用于机器人太奢侈,高精度地图成本太高),通过一套独特的导航体系,帮助自动驾驶解决系统性能问题,扩展传感器检测边界。它可以帮助车辆找到合适的行车空间,还可以帮助Planner确定不同的路线选择。如APOLLO中,高精度地图可以帮助车辆识别准确的道路中心线,这样在规划时就可以让车辆尽可能沿道路中心线行驶。同时在十字路口等交叉路口也可以帮助车辆缩小规划范围,以便节省计算成本与时间,在最有范围内选择出最佳路线。参考文献:
1《视觉SLAM十四讲:从理论到实践》
2 https://github.com/FleverX/grid_map -
无人车路径规划算法---(4)基于搜索的路径规划算法 II(贪心/Astar)
2021-12-09 00:16:57Dijkstra算法搜索时间较长,搜索范围较大,但是可以搜索出最短路径 Greedy算法搜索时间较短,搜索范围较小,但是路径不是最短路径 Astar算法搜索时间较短,同时搜索范围较小,路径是最短路径(部分情况下) 2.4 ... -
基于新型蚁群算法的无人驾驶路径规划
2021-04-08 08:46:56蚁群优化算法是解决无人驾驶汽车路径规划问题的有效途径。 首先,建立无人车道规划的环境模型,处理和描述环境信息,最后实现问题空间的划分。 接下来,描述蚁群算法的仿生行为。 蚁群算法已通过添加惩罚策略进行了... -
基于Matlab的汽车无人驾驶路径规划.pdf
2021-06-05 18:23:03采用人工势场法进行路径规划和避障,建立新的汽车无人驾驶路径规划环境模型。汽车以自身的圆周势场进行探测,每次移动都会使势力场发生改变,在进行信息收集后判断避开障碍物的所有路径,最终在规划的路径中找出最佳路径... -
无人驾驶汽车路径规划仿真分析
2020-10-16 23:17:45本文介绍了应用于无人驾驶汽车路径规划中全局路径规划的A*算法,从规划结果出发,分析传统A*的缺陷,并提出16邻域改进算法。为提高规划效率,减少路径规划时间提出双向16邻域改进算法。并与24邻域及48邻域算法进行比较,... -
无人驾驶汽车路径规划概述
2019-07-30 17:18:42无人驾驶汽车路径规划概述 原地址:http://imgtec.eetrend.com/blog/2019/100018447.html 无人驾驶汽车路径规划是指在一定的环境模型基础上,给定无人驾驶汽车起始点和目标点后,按照性能指标规划出一条无碰撞、能... -
论文研究 - 室内危险环境下遥感无人地面车辆的最优路径规划
2020-05-24 23:19:32本文提出了一种路径规划方法,该方法可在先验信息最少的情况下在室内危险区域内导航无人地面车辆。 该算法可以推广到任何给定的地图,并且基于概率路线图路径规划方法和螺旋动力学优化算法来获得最佳导航路径。 ... -
基于Matlab的汽车无人驾驶路径规划
2022-04-20 12:56:25基于Matlab的汽车无人驾驶路径规划基于Matlab的汽车无人驾驶路径规划基于Matlab的汽车无人驾驶路径规划基于Matlab的汽车无人驾驶路径规划基于Matlab的汽车无人驾驶路径规划基于Matlab的汽车无人驾驶路径规划基于... -
无人驾驶汽车局部路径规划研究综述
2020-10-16 23:16:29文章针对近年来的无人驾驶汽车路径规划算法进行总结和归纳。首先对目前主流的环境建模方法进行阐述;其次对路径规划算法进行介绍,通过分析其优缺点,指出融合轨迹规划算法具有最好的适用性;最后总结当前研究挑战并提出... -
无人驾驶汽车的路径规划与跟随控制算法研究
2017-10-26 14:38:38运用多点预瞄与滚动优化相结合的模型预测控制算法设计了汽车的跟随...在双移线工况下进行了多组速度的跟随实验,结果表明该控制器跟随路径的误差小,对速度的适应性强。与Carsim控制器的跟随结果相比,其跟随效果更好。 -
基于Frenet优化轨迹的无人车动作规划实例.zip
2020-07-12 15:38:37基于Frenet优化轨迹的无人车动作规划实例,使用Python实现,高速场景。 无人车 动作规划 自动驾驶 辅助驾驶 优化轨迹 -
无人车路径规划算法---(1)算法体系综述
2019-12-01 17:28:23***写此系列博客的目的,是希望将无人车路径规划算法成体系的总结展述出来,同时分享一些常用的算法库,源代码,以及与规划层联系紧密的感知层,定位层,控制层的基础知识,可能编写跨越周期较长,自身水平有限,若... -
无人驾驶路径规划(一)全局路径规划 - RRT算法原理及实现
2022-04-02 15:45:05前言:由于后续可能要做一些无人驾驶相关的项目和实验,所以这段时间学习一些路径规划算法并自己编写了matlab程序进行仿真。开启这个系列是对自己学习内容的一个总结,也希望能够和优秀的前辈们多学习经验。 一、... -
基于多智能体强化学习的无人车分布式路径规划方法.pdf
2021-08-08 19:41:26#资源达人分享计划# -
基于Frenet优化轨迹的无人车动作规划实例
2020-07-17 10:59:41基于Frenet优化轨迹的无人车动作规划实例,使用Python实现,高速场景。 无人车 动作规划 自动驾驶 辅助驾驶 优化轨迹 -
无人驾驶常用路径规划
2019-07-30 16:56:47无人驾驶系统介绍行为决策运动轨迹规划路径规划的不同之处路径规划的定义路径规划方法全局路径规划Dubins路径方法Dubins路径改良算法局部路径规划模糊逻辑算法基于行为的路径规划算法基于再励学习的路径规划算法基于... -
乘用车自动泊车系统路径规划与仿真分析
2021-05-02 10:28:54MATLAB 路径规划仿真,验证路径规划方法的合理性。接着,结合非时间参考路径跟踪控制和终端滑模控制方法,提出基于趋近律的非时间参考终端滑模路径跟踪控制方法:应用非时间量参考方法,避免车速控制量对车辆路径... -
基于Floyd算法的无人驾驶汽车路径规划模型_吴梓乔
2018-07-05 17:09:57在各类产业智能化发展的21世纪中,无人驾驶技术作为IT产业与传统汽车紧密相结合的代表,得到了高速发展,无人驾驶的路径规划问题一直是其发展过程中的一个难关。 -
综述(六)无人驾驶系统的路径规划算法介绍
2021-11-04 09:24:31作为无人驾驶汽车顺利运行的重要环节,路径规划是指无人驾驶汽车在具有障碍物的环境中,能够规划出一条从起始位置状态到目标位置状态无碰撞的最优路径或次优路径,并满足所有约束条件,是实现汽车智能化的关键技术之... -
B样条曲线规划小车路径.rar
2020-05-26 22:45:27内含matlab程序文件及实验报告,主要解决的是用B样条曲线实现无人车的路径规划,规划一条由起点出发到达终点的光滑曲线,为了避开这些障碍点,给定一组控制点,由这些控制点牵引小车躲避障碍,从而得到一条光滑的... -
3、无人驾驶--路径规划算法:Floyd算法
2022-01-20 21:46:16Floyd算法是一种动态规划算法,稠密图效果最佳,节点间的连接权值可正可负。此算法简单有效,由于三重循坏结构紧凑,对于稠密图,效果要高于Dijkstra算法。 -
CubicSpline_驾驶_路径规划_三次样条_无人驾驶_汽车
2021-09-11 06:31:51无人驾驶汽车的三次样条路径规划和拟合,python程序编写,效果很好 -
第5章_轨迹跟踪_路径规划_轨迹跟踪控制_无人驾驶车辆_无人驾驶
2021-09-10 22:07:56实现无人驾驶车辆基于车辆动力学模型的轨迹跟踪及路径规划