无人车路径规划算法实现

A. 有哪些应用于移动机器人路径规划的算法

机器人家上了解到，在二维二值地图（FREE or OCCUPIED）场景下进行路径规划的方法。我看之前有同学在回答的时候配上了这幅图：

这幅图上的算法罗列的还是很全面的，体现了各个算法的出生顺序。但是并不能很好的对他们进行一个本质的分类。刚刚那位同学说的graph-based和sampling-based的分类方法我感觉有点概念重叠不能够对规划算法进行这样的分类，下面通过自己这一年多的研究和实践对规划算法进行一个简单的分类：

这幅图上的算法罗列的还是很全面的，体现了各个算法的出生顺序。但是并不能很好的对他们进行一个本质的分类。刚刚那位同学说的graph-based和sampling-based的分类方法我感觉有点概念重叠不能够对规划算法进行这样的分类，下面通过自己这一年多的研究和实践对规划算法进行一个简单的分类：

两大类：
1. 完备的（complete)
2. 基于采样的（sampling-based）又称为概率完备的

一 完备的规划算法

A*算法

所谓完备就是要达到一个systematic的标准，即：如果在起始点和目标点间有路径解存在那么一定可以得到解，如果得不到解那么一定说明没有解存在。
这一大类算法在移动机器人领域通常直接在occupancy grid网格地图上进行规划（可以简单理解成二值地图的像素矩阵）以深度优先寻路算法、广度优先寻路算法、Dijkstra(迪杰斯特拉)算法为始祖，以A*算法(Dijstra算法上以减少计算量为目的加上了一个启发式代价)最为常用，近期的Theta*算法是在A*算法的基础上增加了line-of-sight优化使得规划出来的路径不完全依赖于单步的栅格形状（答主以为这个算法意义不大，不就是规划了一条路径再简单平滑了一下么）。
完备的算法的优势在与它对于解的捕获能力是完全的，但是由此产生的缺点就是算法复杂度较大。这种缺点在二维小尺度栅格地图上并不明显，但是在大尺度，尤其是多维度规划问题上，比如机械臂、蛇形机器人的规划问题将带来巨大的计算代价。这样也直接促使了第二大类算法的产生。

二 基于采样的规划算法

RRT-connect算法
这种算法一般是不直接在grid地图进行最小栅格分辨率的规划，它们采用在地图上随机撒一定密度的粒子来抽象实际地图辅助规划。如PRM算法及其变种就是在原始地图上进行撒点，抽取roadmap在这样一个拓扑地图上进行规划；RRT以及其优秀的变种RRT-connect则是在地图上每步随机撒一个点，迭代生长树的方式，连接起止点为目的，最后在连接的图上进行规划。这些基于采样的算法速度较快，但是生成的路径代价（可理解为长度）较完备的算法高，而且会产生“有解求不出”的情况（PRM的逢Narrow space卒的情况）。这样的算法一般在高维度的规划问题中广泛运用。

三 其他规划算法
除了这两类之外还有间接的规划算法：Experience-based（Experience Graph经验图算法）算法：基于经验的规划算法，这是一种存储之前规划路径，建立知识库，依赖之进行规划的方法，题主有兴趣可以阅读相关文献。这种方法牺牲了一定的空间代价达到了速度与完备兼得的优势。此外还有基于广义Voronoi图的方法进行的Fast-marching规划，类似dijkstra规划和势场的融合，该方法能够完备地规划出位于道路中央，远离障碍物的路径。答主最近也在研究此类算法相关的工作。

APF（人工势场）算法

至于D* 、势场法、DWA(动态窗口法)、SR-PRM属于在动态环境下为躲避动态障碍物、考虑机器人动力学模型设计的规划算法。

B. A*算法介绍

姓名：车文扬 学号：16020199006

【嵌牛导读】：A*算法的逐步详解

【嵌牛鼻子】：启发式算法

【嵌牛提问】：A*算法的原理是什么？

【嵌牛正文】：

A*算法

路径规划是指的是机器人的最优路径规划问题，即依据某个或某些优化准则（如工作代价最小、行走路径最短、行走时间最短等），在工作空间中找到一个从起始状态到目标状态能避开障碍物的最优路径。机器人的路径规划应用场景极丰富，最常见如游戏中NPC及控制角色的位置移动，网络地图等导航问题，小到家庭扫地机器人、无人机大到各公司正争相开拓的无人驾驶汽车等。

目前路径规划算法分为：

A*算法原理：

在计算机科学中，A*算法作为Dijkstra算法的扩展，因其高效性而被广泛应用于寻路及图的遍历，如星际争霸等游戏中就大量使用。在理解算法前，我们需要知道几个概念：

搜索区域（The Search Area）：图中的搜索区域被划分为了简单的二维数组，数组每个元素对应一个小方格，当然我们也可以将区域等分成是五角星，矩形等，通常将一个单位的中心点称之为搜索区域节点（Node）。

开放列表(Open List)：我们将路径规划过程中待检测的节点存放于Open List中，而已检测过的格子则存放于Close List中。

父节点（parent）：在路径规划中用于回溯的节点，开发时可考虑为双向链表结构中的父结点指针。

路径排序（Path Sorting）：具体往哪个节点移动由以下公式确定：F(n) = G + H 。G代表的是从初始位置A沿着已生成的路径到指定待检测格子的移动开销。H指定待测格子到目标节点B的估计移动开销。

启发函数（Heuristics Function）：H为启发函数，也被认为是一种试探，由于在找到唯一路径前，我们不确定在前面会出现什么障碍物，因此用了一种计算H的算法，具体根据实际场景决定。在我们简化的模型中，H采用的是传统的曼哈顿距离（Manhattan Distance），也就是横纵向走的距离之和。

如下图所示，绿色方块为机器人起始位置A，红色方块为目标位置B，蓝色为障碍物。

我们把要搜寻的区域划分成了正方形的格子。这是寻路的第一步，简化搜索区域。这个特殊的方法把我们的搜索区域简化为了2 维数组。数组的每一项代表一个格子，它的状态就是可走(walkalbe)或不可走(unwalkable) 。现用A*算法寻找出一条自A到B的最短路径，每个方格的边长为10，即垂直水平方向移动开销为10。因此沿对角移动开销约等于14。具体步骤如下：

从起点 A 开始，把它加入到一个由方格组成的open list(开放列表) 中，这个open list像是一个购物清单。Open list里的格子是可能会是沿途经过的，也有可能不经过。因此可以将其看成一个待检查的列表。查看与A相邻的8个方格 ，把其中可走的 (walkable) 或可到达的(reachable) 方格加入到open list中。并把起点 A 设置为这些方格的父节点 (parent node) 。然后把 A 从open list中移除，加入到close list(封闭列表) 中，close list中的每个方格都是不需要再关注的。

如下图所示，深绿色的方格为起点A，它的外框是亮蓝色，表示该方格被加入到了close list 。与它相邻的黑色方格是需要被检查的，他们的外框是亮绿色。每个黑方格都有一个灰色的指针指向他们的父节点A。

下一步，我们需要从open list中选一个与起点A相邻的方格。但是到底选择哪个方格好呢？选F值最小的那个。我们看看下图中的一些方格。在标有字母的方格中G = 10 。这是因为水平方向从起点到那里只有一个方格的距离。与起点直接相邻的上方，下方，左方的方格的G 值都是10 ，对角线的方格G 值都是14 。H值通过估算起点到终点( 红色方格) 的Manhattan 距离得到，仅作横向和纵向移动，并且忽略沿途的障碍。使用这种方式，起点右边的方格到终点有3 个方格的距离，因此H = 30 。这个方格上方的方格到终点有4 个方格的距离( 注意只计算横向和纵向距离) ，因此H = 40 。

比较open list中节点的F值后，发现起点A右侧节点的F=40，值最小。选作当前处理节点，并将这个点从Open List删除，移到Close List中。

对这个节点周围的8个格子进行判断，若是不可通过（比如墙，水，或是其他非法地形）或已经在Close List中，则忽略。否则执行以下步骤：

若当前处理节点的相邻格子已经在Open List中，则检查这条路径是否更优，即计算经由当前处理节点到达那个方格是否具有更小的 G值。如果没有，不做任何操作。相反，如果G值更小，则把那个方格的父节点设为当前处理节点 ( 我们选中的方格 ) ，然后重新计算那个方格的 F 值和 G 值。

若当前处理节点的相邻格子不在Open List中，那么把它加入，并将它的父节点设置为该节点。

按照上述规则我们继续搜索，选择起点右边的方格作为当前处理节点。它的外框用蓝线打亮，被放入了close list 中。然后我们检查与它相邻的方格。它右侧的3个方格是墙壁，我们忽略。它左边的方格是起点，在close list 中，我们也忽略。其他4个相邻的方格均在open list 中，我们需要检查经由当前节点到达那里的路径是否更好。我们看看上面的方格，它现在的G值为14 ，如果经由当前方格到达那里，G值将会为20( 其中10为从起点到达当前方格的G值，此外还要加上从当前方格纵向移动到上面方格的G值10) ，因此这不是最优的路径。看图就会明白直接从起点沿对角线移动到那个方格比先横向移动再纵向移动要好。

当把4个已经在open list 中的相邻方格都检查后，没有发现经由当前节点的更好路径，因此不做任何改变。接下来要选择下一个待处理的节点。因此再次遍历open list ，现在open list中只有7 个方格了，我们需要选择F值最小的那个。这次有两个方格的F值都是54，选哪个呢？没什么关系。从速度上考虑，选择最后加入open list 的方格更快。因此选择起点右下方的方格，如下图所示。

接下来把起点右下角F值为54的方格作为当前处理节点，检查其相邻的方格。我们发现它右边是墙（墙下面的一格也忽略掉，假定墙角不能直接穿越)，忽略之。这样还剩下 5 个相邻的方格。当前方格下面的 2 个方格还没有加入 open list ，所以把它们加入，同时把当前方格设为他们的父亲。在剩下的 3 个方格中，有 2 个已经在 close list 中 ( 一个是起点，一个是当前方格上面的方格，外框被加亮的 ) ，我们忽略它们。最后一个方格，也就是当前方格左边的方格，检查经由当前方格到达那里是否具有更小的 G 值。没有，因此我们准备从 open list 中选择下一个待处理的方格。

不断重复这个过程，直到把终点也加入到了open list 中，此时如下图所示。注意在起点下方2 格处的方格的父亲已经与前面不同了。之前它的G值是28并且指向它右上方的方格。现在它的G 值为20 ，并且指向它正上方的方格。这是由于在寻路过程中的某处使用新路径时G值更小，因此父节点被重新设置，G和F值被重新计算。

那么我们怎样得到实际路径呢？很简单，如下图所示，从终点开始，沿着箭头向父节点移动，直至回到起点，这就是你的路径。

A*算法总结：

1. 把起点加入 open list 。

2. 重复如下过程：

a. 遍历open list ，查找F值最小的节点，把它作为当前要处理的节点，然后移到close list中

b. 对当前方格的 8 个相邻方格一一进行检查，如果它是不可抵达的或者它在close list中，忽略它。否则，做如下操作：

□  如果它不在open list中，把它加入open list，并且把当前方格设置为它的父亲

□  如果它已经在open list中，检查这条路径 ( 即经由当前方格到达它那里 ) 是否更近。如果更近，把它的父亲设置为当前方格，并重新计算它的G和F值。如果你的open list是按F值排序的话，改变后你可能需要重新排序。

c. 遇到下面情况停止搜索：

□  把终点加入到了 open list 中，此时路径已经找到了，或者

□  查找终点失败，并且open list 是空的，此时没有路径。

3. 从终点开始，每个方格沿着父节点移动直至起点，形成路径。

C. slam算法是什么

SLAM是Simultaneous localization and mapping缩写，意为“同步定位与建图”，主要用于解决机器人在未知环境运动时的定位与地图构建问题。

Simultaneous Localization and Mapping (SLAM)原本是Robotics领域用来做机器人定位的，最早的SLAM算法其实是没有用视觉camera的（Robotics领域一般用Laser Range Finder来做SLAM）。

SLAM对实时性要求比较高，而要做到比较精确、稳定、可靠、适合多种场景的方案一般计算量相对较大，目前移动式设备的计算能力还不足够支撑这么大的计算量，为了达到实时性能，往往需要在精确度和稳定性上做些牺牲。

因此在具体的应用中，往往需要根据移动设备所具有的传感器组合、计算能力、用户场景等，选择和深度定制合适的SLAM算法。比如，无人驾驶汽车和手机端AR类应用的SLAM算法就非常不同。

SLAM的典型应用领域

机器人定位导航领域：地图建模。SLAM可以辅助机器人执行路径规划、自主探索、导航等任务。国内的科沃斯、塔米以及最新面世的岚豹扫地机器人都可以通过用SLAM算法结合激光雷达或者摄像头的方法，让扫地机高效绘制室内地图，智能分析和规划扫地环境，从而成功让自己步入了智能导航的阵列。

VR/AR方面：辅助增强视觉效果。SLAM技术能够构建视觉效果更为真实的地图，从而针对当前视角渲染虚拟物体的叠加效果，使之更真实没有违和感。VR/AR代表性产品中微软Hololens、谷歌ProjectTango以及MagicLeap都应用了SLAM作为视觉增强手段。

无人机领域：地图建模。SLAM可以快速构建局部3D地图，并与地理信息系统（GIS）、视觉对象识别技术相结合，可以辅助无人机识别路障并自动避障规划路径，曾经刷爆美国朋友圈的Hovercamera无人机，就应用到了SLAM技术。

无人驾驶领域：视觉里程计。SLAM技术可以提供视觉里程计功能，并与GPS等其他定位方式相融合，从而满足无人驾驶精准定位的需求。例如，应用了基于激光雷达技术Google无人驾驶车以及牛津大学MobileRoboticsGroup11年改装的无人驾驶汽车野猫(Wildcat)均已成功路测。

以上内容参考：slam路径规划算法 - CSDN

D. 基于激光雷达的SLAM和路径规划算法研究与实现

本文仅供学习使用，并非商业用途，全文是针对哈尔滨工业大学刘文之的论文《移动机器人的路径规划与定位技术研究》进行提炼与学习。论文来源中国知网，引用格式如下：
[1]刘文之. 基于激光雷达的SLAM和路径规划算法研究与实现[D].哈尔滨工业大学,2018.

相关坐标系转换原理已经在前一篇文章写完了，直接上转换方程。

这里他的运动模型选择的是基于里程计的运动模型，还有一种基于速度的运动模型，其实都差不多，整体思想都一样。里程计是通过计算一定时间内光电编码器输出脉冲数来估计机器人运动位移的装置，主要是使用光电码盘。根据光电码盘计算出此时轮子的速度，然后通过已知的轮子半径来获得单位时间 每个轮子 的位移增量。

高等数学可知单位时间位移增量就是速度，对速度在一定时间上进行积分就得到这一段时间所走过的路程。

根据上图，我们可以求出来机器人航向角角速度、圆弧运动半径和机器人角度变化量，由此可以解的机器人在当前时刻的位姿。

实际上也是有误差，所以单独依靠里程计会与实际结果产生较大误差，所以必须引入其他的外部传感器对外部环境的观测来修正这些误差，从而提高定位精度。

首先肯定需要将激光雷达所测得的端点坐标从极坐标、机器人坐标中转换到世界坐标中。

这张略过，暂时不需要看这个

路径规划算法介绍：

因为该算法会产生大量的无用临时途径，简单说就是很慢，所以有了其他算法。

了解两种代价之后，对于每一个方块我们采用预估代价与当前路径代价相加的方法，这样可以表示每一个路径点距离终点的距离。在BFS搜索过程的基础上，优先挑选总代价最低的那个路径进行搜索，就可以少走不少弯路。（算法讲解 https://www.bilibili.com/video/BV1bv411y79P?from=search&seid=3623681329596549549 ）

在局部路径规划算法之中，我们选用DWA算法（dynamic window approach），又叫动态窗口法。动态窗口法主要是在速度(v, w)空间中采样多组速度，并模拟机器人在这些速度下一定时间内的轨迹。在得到多组轨迹后，对这些轨迹进行评价，选取最优的轨迹所对应的速度来驱动机器人运动。
state sampling就是按照之前给出的全局路径规划，无论是Dijkstra还是A* 都可以方便的得到state sampling，DWA算法所需要提前建立的action sampling有两种：

但是无论是什么情况，上述所做的工作就是把机器人的位移转化到世界坐标中来，而不是机器人坐标系。速度采样结束之后，只需要对小车的轨迹进行评判，就可以得到最优解了。下面介绍速度采样的办法。

对速度进行采样一般有以下三个限制：

当确定了速度范围之后，就需要根据速度分辨率来对小车速度离散化，在每一时刻将小车在不同直线速度角速度组合下所即将要行驶的距离都可视化出来。

其中每一条轨迹都是很多小直线连接起来的。

需要用评价函数来对上述轨迹进行选择，选择最适合的轨迹

最后为了让三个参数在评价函数里所发挥的作用均等，我们使用归一化处理来计算权重。

算法流程整体如下：