路径规划和避障算法

基于A*的路径搜索算法
A*搜索是基于启发函数的搜索，也称有信息搜索，是人工智能的 一个典型算法。我们采用的基于A*的路径规划算法，主要有以下 三个步骤：
(1)根据栅格地图生成图 (2)在图上搜索最优路径 (3)对路径进行优化
基于A*的路径搜索算法
根据栅格地图生成拓扑图 我们的路径规划算法都是基于确定障碍物信息的，也就是根据地
路径规划和避障算法
徐海军 2009-12-15
大纲
路径规划算法
➢ A*路径搜索算法 ➢ RRT算法 ➢ 人工势场法
实时避障算法
➢ VFF算法
路径规划算法
路径规划是移动机器人导航控制中的一个重要问题，它主要是指 在存在障碍物的情况下，找到一条从起点到终点的最优(无碰撞、 最短)路径。
主要的路径规划算法： (1)基于启发式的路径搜索，如A*算法 (2)快速随即生成树算法 Rapidly-exploring Random Tree (3)人工势场法 Artificial Potential Field Method
最后得到如图中线段A,B,C组成的路径, 可见 该路径不会穿越障碍物, 只会更贴着障碍物, 优化 原路径;
RRT算法
RRT是快速随机扩展树算法，主要分三中，基本RRT，扩展RRT， 双向RRT，这里介绍最基本的RRT。算法包含以下这些元素：
Cfree 可选点集 Cobs 障碍点集 Root 或 q-init 起始点 Step 步长 Target扩增目标点 Nearset树上距离target取近的点
(2)四分法的特点是在障碍物较多区域会多分布一些点，障碍物 较少区域点也较少；这样的好处是，只要设置合理的分辨率，一 定能找到通路;但是它有一个特点，对于一个空旷的房间，规划 的路径总是从中间走而不是沿着墙壁，这就是四分法的局限性。
基于+h(n)
a) 寻找开启列表中F值最低的格子。我们称它为当前格。 b) 把它切换到关闭列表。 c) 对相邻的8格中的每一个
* 如果它不可通过或者已经在关闭列表中，略过它。反之如下。 * 如果它不在开启列表中，把它添加进去。把当前格作为这一格 的父节点。记录这一格的F,G,和H值。 * 如果它已经在开启列表中，用G值为参考检查新的路径是否更好。 更低的G值意味着更好的路径。如果是这样，就把这一格的父节点改成当 前格，并且重新计算这一格的G和F值。如果你保持你的开启列表按F值 排序，改变之后你可能需要重新对开启列表排序。 d) 停止，当你 * 把目标格添加进了关闭列表，这时候路径被找到，或者 * 没有找到目标格，开启列表已经空了。这时候，路径不存在。 3.保存路径。从目标格开始，沿着每一格的父节点移动直到回到起始 格。这就是你的路径。
g(n)是从初始节点到该节点n的路径耗散； h(n)是从节点n到目标节点的最低耗散路径的估计耗散值，称为 启发式或启发函数； 因此，f(n)=经过节点n、具有最低耗散值的解的估计耗散 搜索的过程就是找到g(n)+h(n)值最小的节点。
基于A*的路径搜索算法
A*算法步骤： 1，把起始格添加到开启列表。 2，重复如下的工作：
线段1 A
障碍物
随机点2 线段3
C 终点
起点
图3 路径优化方法
假设点S从起点开始, 点E从线段2起点某一点 开始, 若线段SE之间无碰, E继续在线段2上以一定 步长前进(若一直无碰, 则可以一直走到线段3, 及 之后的线段上), 直到SE碰撞为止, 记录SE作为新的 线段1(即类似图中的线段A),除去原先位于断点SE 之间的原路径点(如随机点1), 并使S重新从线段1起 点开始, E从线段2起点后一点开始, 重复上述优化 过程; 若E刚开始时, SE便已碰撞, 则使S沿着线段1 前进一定步长, 继续计算; 若S一直到线段1最后点, SE仍然产生碰撞, 则S点跳到线段2开始, E点跳到线 段3, 重新开始优化, 直到E到达终点为止.
图信息要得到一张拓扑图。主要思想是，在栅格地图的自由空间 上随机产生一定数量的种子点，通过这些点，判断连线是否碰撞， 再根据起点和终点，就可以确定需要搜索的拓扑图。 这里的关键是拓扑图顶点的生成。如果选的点少有可能找不到路 径，点多又会增加计算量。
基于A*的路径搜索算法
拓扑图点生成方法 (1)在障碍物周围随机生成一定数量的点，在离初始或目标点近的
基于A*的路径搜索算法
图2 是基于网格的A*搜索的示意图，绿色代表起点、 红色代表终点、红点是搜索得到的路径。
基于A*的路径搜索算法
路径优化 通过以上方式得到的路径是通过连接已有随机点生成的, 因此往
往较为曲折, 不够优化; 需要对已有路径进行近一步优化.
基于A*的路径搜索算法
随机点1
线段2 B
障碍物的点，记录中点；如果全是障碍
物的点，扔掉不处理；否则继续四分下
去，直至到最小单元栅格。
D1
D2
C
D
D3 D4
图1 四分法示意图
基于A*的路径搜索算法
两种撒点方法优缺点： (1)基于随机点的生成方法算法相对简单，适合栅格地图等非结 构化地图，能通过随机点的产生处理复杂障碍物形状，然而在某 些特殊的初始和目标点分布下，还是会出现找不到路径的情况； 虽然解决办法可以通过多撒点，但无疑会增加计算量。
RRT算法
将空间点集C分成可选点集Cfree和障碍点集Cobs；起始点称作根 root或q-init，rrt这棵树每节树枝的长度称作步长step；
当RRT在空间内没有目标随机扩展时，首先在空间全集C中随机选 取一点，作为这一轮扩增的目标点target；之后在已有的“树” 的节点中，找到距离target的最近点nearest；在nearest与target 的连线上，找到与nearest距离为step的点作为待选新节点new， 如果new属于可选点集Cfree，则把new点，作为新节点加入树中。 否则放弃本轮，开始下一轮扩增。如图4、图5。
区域多撒点，远的区域少撒点，可避免在弯折狭窄的的通道里找 不到路径。 (2)四分树法生成点，思想是对栅格地图不断四分，对每一块不全 是障碍物的区域继续四分，否则记录，逐步得到全部的点。
基于A*的路径搜索算法
算法步骤：
(a)记录地图的中点、长度、宽度；
A
B
(b)求每一块区域的中点、 长和宽； (c)遍历四分的每块区域，如果这块不包含