基于A*算法的驾驶地图路径规划实现

关泉珍，鲍 泓，史志坚

（北京联合大学北京市信息服务工程重点实验室，北京 100101）



基于A*算法的驾驶地图路径规划实现

关泉珍，鲍 泓，史志坚

（北京联合大学北京市信息服务工程重点实验室，北京 100101）

［摘 要］采用高精度地图构建技术还原路况信息，结合A*算法使智能车能够在导航不起作用的情况下按照规划路径进行无障碍行驶。将高精度地图用栅格数据模型表示，在标记为有障碍的栅格模型中，为机器人寻找一条恰当的从起始点到目标点的运动路径，且可以使机器人在运动过程中安全、无碰撞地绕过障碍物。通过在无人驾驶智能车平台上仿真实验表明，这种方法具有形式简单规范、一致性好并容易在计算机中实现的优点。

［关键词］路径规划；自主导航；高精度地图；A*算法；栅格数据模型

引言

无人驾驶汽车是一种智能汽车，也称之为轮式移动机器人［1］，是在没有人类参与的情况下，依靠车内的计算机系统，通过智能驾驶系统来实现无人驾驶的功能［2］。它是一个集环境信息感知、智能规划决策、辅助及自主驾驶等多种功能于一体的综合车辆系统［3］，路径规划是无人驾驶汽车导航的基本环节之一。它是按照某一性能指标搜索一条从起始点到目标点的最优或近似最优的无碰路径。对于路径规划来说，往往与无人驾驶汽车所处的空间位置和传感器所感知的周围环境信息密切相关。根据无人驾驶汽车传感器对周围环境信息知道的程度不同，可分为两种类型:第一种是环境信息完全知道的宏观路径规划也叫全局路径规划；第二种是环境信息完全未知或部分未知，通过雷达、摄像头等传感器对无人驾驶汽车的工作环境进行探测，获取障碍物的位置、形状和尺寸等信息的局部路径规划。全局路径规划可使所规划的路径达到最优，局部路径规划则可使机器人完成实时避障。目前常用的移动机器人全局路径规划方法很多，主要有蚁群算法［4］、粒子群算法［5］、遗传算法［6］等，此外GPS也是智能车辆进行全局规划的重要手段之一，目前主要采用GPS/INS组合的导航技术。对蚁群算法来说，它存在收敛速度慢，容易陷入局部最优等缺陷；粒子群优化算法虽然具有通用性强、不依赖于问题、原理简单、容易实现等优点，但粒子群算法计算效率偏低，且抗噪能力差；遗传算法存在编程实现比较复杂，且参数的选择大部分是依靠经验等缺点。对GPS来说，虽然无人驾驶汽车通过GPS/INS等传感器能全天候实时感知车辆在环境中的位置，提供准确的车辆行驶方向、速度、加速度等车辆自身状态信息，但由于GPS的工作依赖于卫星信号，因此，在卫星信号不佳或信号无法获取的情况下将会失效［7］，INS也会出现信号漂移等问题。

以上算法都存在编程实现较复杂等缺点。本文结合人类驾驶员驾驶经验，提出采用高精度地图构建技术还原路况信息，结合A*算法使智能车能够在导航不起作用的情况下按照规划路径进行无障碍行驶。将高精度地图用栅格数据模型表示，在标记为有障碍的栅格模型中，为机器人寻找一条恰当的从起始点到目标点的运动路径。高精度地图的获取可通过专业做地图的公司采集处理或者由人类驾驶员驾驶智能汽车遍历一块区域内的所有车道，将传感系统如64线激光雷达或摄像机等采集到的行驶轨迹、道路环境等信息经过滤波、融合等算法处理，就得到了这一区域的道路网络综合信息。

1　驾驶地图栅格化

在车辆导航系统中，电子地图数据库、路径规划和路径引导是3个相辅相成的重要模块。为了实现路径规划和路径引导，首先需要一个电子地图数据库的支持。对于无人驾驶汽车，除了需要电子地图上常见的道路形态和路边环境信息，还需要抽象的道路网络拓扑结构，用于宏观层面上合理的路径规划和微观层面上精确的路径引导。比之目前一般的导航设备，无人驾驶将使用完全不同的地图数据。精确到米的低端地图（如图1）只能给人类驾驶员领路，无人驾驶智能车要的是厘米级的精度（如图2）所示。应用差分定位技术的全球卫星定位系统和惯性导航组合可以为无人驾驶车的行驶提供厘米级的导航信息，但由于在发达城市越来越长的隧道兴建和连续高层的遮挡，全球卫星定位系统和惯性导航组合系统进入隧道不能满足一定精度的要求以及在大型隧道内不能长期定位的缺点［7］，使得采用高精度地图匹配技术［8］来克服以上缺点成为当务之急。

无人驾驶车辆的工作环境与一般机器人应用存在如下几点不同:不具备已知的环境地图，环境信息主要来自于车载传感器系统的实时感知结果，而各个传感器只能提供局部不完整的环境信息。因此，在车辆行驶过程中，即使环境是静态的，规划系统面对的局部环境信息也是在不断更新的；此外，环境中可能存在较多的动态障碍物，易造成规划结果不可行；这些特点对规划系统的实时性提出了更高的要求，而高精度地图无疑对智能车的行驶起到了很大的帮助作用。

对于高精度地图来说，道路网络的几何坐标信息为智能汽车提供了定位和路径引导的依据，利用高精度的道路网信息，将GPS/INS或其他传感器的定位结果匹配到高精度地图的道路网络上，实现车辆位置纠正。当推算定位指示车辆在地图上的某一位置时，车辆位置可以被调整到地图上的绝对位置，这样做会消除累积误差，直到下一次地图匹配步骤。在每一个连续的系统周期中完成这个过程，就能实时得到更加准确的车辆位置。地图匹配技术的应用有两个前提，即:用于匹配的数字地图包含高精度的道路位置坐标以及被定位的车辆正在道路网中行驶。对于前者而言，高精度地图无疑是满足这一要求的。当满足上述条件时，就可以把定位数据和车辆运行轨迹同高精度地图所提供的道路位置信息进行比较，通过适当的匹配过程确定出车辆最可能的行驶路段以及车辆在该路段的最大可能位置。［9］

1.1驾驶地图与栅格地图的关系

栅格地图对应投影坐标系统，高精度地图对应地理空间坐标系。

投影坐标系统（Projection coordinate system）:使用基于X，Y值的坐标系统来描述地球上某个点所处的位置。这个坐标系是从地球的近似椭球体投影得到的，它对应于某个地理坐标系（地理空间坐标系:使用基于经纬度坐标描述地球上某一点所处的位置。某一个地理坐标系是基于一个基准面来定义的。基准面是利用特定椭球体对特定地区地球表面的逼近，因此每个国家或地区均有各自的基准面，也称球面坐标。平面坐标系统地图单位通常为米，也称非地球投影坐标系统，或者是平面直角坐标。投影坐标系由以下两项参数确定:地理空间坐标系以及投影方法。下面用公式（1）来表示地理空间坐标系和投影坐标系统的关系:

式中（B，L）是椭球面上某一点的大地经纬度，而（X，Y）是该点投影到平面上的直角坐标。

把地球上的点位换算到平面上，称为地图投影。地图投影的方法有很多，本文中采用的是高斯—克吕格投影（又称高斯正形投影），简称高斯投影。它是由德国数学家高斯提出，由克吕格改进的一种分带投影方法。它成功解决了将椭球面转换为平面的问题。通过高斯投影，将中央子午线的投影作为纵坐标轴，用x表示，将赤道的投影作横坐标轴，用y表示，两轴的交点作为坐标原点，由此构成的平面直角坐标系称为高斯平面直角坐标系，［10］如图3所示。

1.2坐标转换算法

将GPS-84坐标转换成平面坐标的方法有两种:一是先将WGS-84的大地坐标转换为1954年北京大地坐标或1980年国家大地坐标，而后通过投影变换（如高斯投影变换）转换成平面直角坐标（如图4所示）；另一种方式是先将经纬度坐标以WGS-84的参考椭球为基准进行高斯投影，前一种方法计算过程较复杂，计算机耗时相对要长一些，但其计算精度要高于后一种方法，后一方法简化了公式［11-12］，提高了计算效率和可操作性。这种方法适用于实时性要求较高的情况，如实时车辆导航和监控系统。本文着重讨论后一种坐标转换算法。

WGS-84的经纬度（维度:B，经度:L）坐标高斯投影到平面坐标（x，y）上，高斯—克吕格投影的正算公式（高斯—克吕格投影正算公式是把大地坐标换算成高斯—克吕格投影平面上的直角坐标）如公式（2）、公式（3）、公式（4）、公式（5）、公式（6）所示:

式中C0、C1、C2、C3是与点位无关而仅与椭球参数有关的常系数，L0为中央子午线经度，X为自赤道量起的子午线弧长，N为卯酉圈曲率半径。

高斯—克吕格投影的反算公式（高斯—克吕格反算公式是把高斯—克吕格投影平面直角坐标换算到椭球面上的大地坐标）如公式（7）、公式（8）、公式（9）、公式（10）所示:

式中

式中Bf为底点纬度，K0、K1、K2、K3、K4、K5是与点位无关而仅与椭球参数有关的常系数。

事实证明，高斯—克吕格投影正反算公式能够进行任意带之间的坐标变化，并且转换精度高，迭代计算收敛速度快，效率高。由于地图投影变形是球面转化成平面的必然结果，没有变形的投影是不存在的。对某一地图投影来说，不存在这种变形，就必然存在另一种或两种变形。在制图时则可以做到:在有些投影图上没有角度或面积变形；在有些投影图上沿某一方向无长度变形。高斯—克吕格投影具有等角性质，比较适合用于测制地形图，且适用于多种比例尺，便于编制成套比例尺的地形图，并且由于投影带经差不大，经纬网同直角坐标网的偏差较小，阅读和使用较方便。此外，坐标和子午线收敛角数值，只要计算一带，其他带都可以通用，可以大大减少计算工作量。

1.3驾驶地图的栅格化表示

栅格数据结构又称为网格数据或者栅格数据，是以规则的阵列来表示空间地物或现象分布的数据组织。每个网格作为一个像元或像素，由行、列号定义，并包含一个代码，表示该像素的属性类型或量值。每个栅格都存储了它所覆盖的表面部分的值。一个指定像元包含一个值，因此，表面的详细程度取决于栅格像元的大小。同其他模型相比，栅格模型比较简单、高效。一般栅格模型多用于区域性的、小比例尺的应用，本文将高精度地图用栅格数据模型表达如图5所示。

简单说，栅格是单波段，任何栅格分析（处理）都是信息有损的处理。在定义栅格单元的大小时，需要平衡信息的精确性和数据量之间的矛盾。栅格单元代表的尺度越小，表达的信息就越精确。栅格单元代表的尺度越大，存储数据所需要的空间就更少，同时，表达的信息也就不精确。栅格数据集像元的位深度容量（像素深度）决定着特定栅格文件可以存储的值的范围，该范围可根据公式2n计算得出（其中，n表示位深度）。例如，一个8位的栅格可以具有256个不同的值（范围从0至255）。栅格数据的表达如图6所示。在本文中栅格数据只有两类信息:分别是障碍物和非障碍物两类。

2　基于A*算法的路径规划

A*算法是应用极为广泛的启发式搜索算法（Heuristic Search Algorithm），相对于其他盲目搜索算法如广度优先和深度优先等，A*算法主要依靠启发信息构造启发函数，在规划搜索路径时对访问节点进行代价评价，以满足某些指标要求，例如时间最短、空间最小等，从而形成代价最优的节点访问路径。它的基本思想是:把从起始点到节点的代价与从节点到目标点的代价结合起来对节点进行评价:f（n）=g（n）+h（n），在这个式子当中，h（n）是启发函数，是从n到目标节点最佳路径估价，f（n）是从初始点经由节点n到终点的评价函数，g（n）是从初始节点到n节点的实际代价。［13-14］

借鉴A*算法的基本思想，无人驾驶车进行路径规划时，首先进行一次路径规划（也叫宏观路径规划），规划出一条从起始点到目标点的路径，然后沿着此条路径移动，当无人驾驶车传感器探测到的环境信息与系统已知的环境信息不一致的时候，机器人重新规划从当前位置到目标点的路径，如此反复，直至到达目标点。A*算法的寻路步骤如下:

简易地图（如图7所示），其中绿色方块的是起点（用A表示），中间蓝色的是障碍物，红色的方块（用B表示）是目的地。用一个二维数组来表示地图，将地图划分成一个个的小方块。寻路步骤如下:

1）从起点A开始，把它作为待处理的方格存入一个开启列表（开启列表就是一个等待检查方格的列表），寻找起点A周围可以到达的方格，将它们放入开启列表，并设置它们的父方格为A。

2）从开启列表中删除起点A，并将起点A加入关闭列表（关闭列表中存放的都是不需要再次检查的方格），从开启列表中找出相对最靠谱的方块，通过公式F=G+H来计算。G表示从起点A移动到网格上指定方格的移动耗费（可沿斜方向移动），H表示从指定的方格移动到终点B的预计耗费，从开启列表中选择F值最低的方格C。

3）把它从开启列表中删除，并放到关闭列表中，检查它所有相邻并且可以到达（障碍物和关闭列表的方格都不考虑）的方格。如果这些方格还不在开启列表里的话，将它们加入开启列表，计算这些方格的G，H和F值各是多少，并设置它们的父方格C如图8。

4）如果某个相邻方格D已经在开启列表里了，检查如果用新的路径（就是经过C的路径）到达它的话，G值是否会更低一些。如果新的G值更低，那就把它的“父方格”改为目前选中的方格C，然后重新计算它的F值和G值（H值不需要重新计算，因为对于每个方块，H值是不变的）。如果新的G值比较高，就说明经过C再到达D不是一个明智的选择，因为它需要更远的路，这时什么也不做。就这样，从开启列表找出F值最小的，将它从开启列表中移掉，添加到关闭列表。再继续找出它周围可以到达的方块，如此循环下去。

5）当我们发现开始列表里出现了目标终点方块的时候，说明路径已经被找到。

3　实验与分析

3.1实验平台

实验是在北京联合大学无人驾驶智能车仿真平台上进行的，如图9所示，硬件使用平台为北汽C70智能车，软件为MATLAB R2013b，操作系统Windows 7。

3.2实验结果

1）选择所用的栅格地图（如图10）。

2）设置地图障碍物位置（如图11）。

3）A*算法的寻路实现（如图12）。

3.3实验分析

本实验将高精度地图以栅格数据结构表示，通过在栅格地图模型上设立障碍物，利用A*算法，为机器人寻找一条恰当的从起始点到目标点的运动路径，且可以使机器人在运动过程中安全、无碰撞地绕过所有的障碍物。与其他方式相比，虽然用栅格法规划地理空间，具有描述规范、形式简单、一致性好、容易实现、在计算机中存储方便等优点，但是利用栅格数据模型表示地图，对精度有很强的依赖性，且在复杂环境下，搜索空间变大，算法的效率可能降低。

4　结束语

在分析了智能汽车宏观路径导航和局部轨迹决策所需的道路网络信息之后，本文提出了基于高精度地图的道路网络综合信息表达方式，将高精度地图利用栅格数据结构表示，结合A*算法实现了从环境初始点寻找最优路径，绕过障碍物到达终点。论文所提方法目前还是仿真验证，环境假设是理想的静态条件，作为一个在真实城市半结构化道路上无人驾驶车辆的完整路径规划系统，如何进行更高效的动态建图和地图定位也是目前工作中正研究的重点

［参考文献］

［1］ 乔维高，徐学进.无人驾驶汽车的发展现状及方向［J］.上海汽车，2007，40（1）:40-43.

［2］ 冯学强，张良旭，刘志宗.无人驾驶汽车的发展综述［J］.山东工业技术，2015，51（1）:1-1.

［3］ 徐友春，王荣本.世界智能车辆近况综述［J］.汽车工程，2001，23（5）:289-295.

［4］ 张银玲，牛小梅.蚁群算法在移动机器人路径规划中的仿真研究［J］.计算机仿真，2011，28（6）:231-234.

［5］ 李擎，徐银，梅张德政，等.基于粒子群算法的移动机器人全局路径规划策略［J］.北京科技大学学报，2010，32（3）:397 -402.

［6］ 石铁峰.改进遗传算法在移动机器人路径规划中的应用［J］.计算机仿真，2011，28（4）:193-195，303.

［7］ 方鹏.GPS/INS组合导航与定位系统研究［D］.上海:同济大学，2008:1-11.

［8］ 潘吴，胡飞，杨立国.GPS车载导航系统的地图匹配算法研究［J］.中国水运，2007，7（11）:174-176.

［9］ 王卫华.未知环境中移动机器人创建地图的研究［D］.上海:上海交通大学，2000.

［10］ 姜岩，王琦，龚建伟，等.无人驾驶车辆局部路径规划的时间一致性与鲁棒性研究［J］.自动化学报，2015，41（3）:518-527.

［11］ 罗飞，杨泽超，伯鲁江.大地坐标换带计算在定向井轨迹设计中的应用［J］.内蒙古石油化工，2007，95（6）:1-3.

［12］ 刘飞，周琳琳，益建芳.GPS大地坐标向地方坐标转换的实用方法研究［J］.华东师范大学学报，2005，75（1）:1-2.

［13］ 陆州.移动机器人路径规划与路径跟踪研究［D］.广州:华南理工大学，2012:19-22.

［14］ Pamosoaji A K，Hong K.A path-planning algorithm using vector potential functions in triangular regions［J］.IEEE Trans Syst Man CybernSyst，2013，43（4）:832-842.

［15］ Jaillet L，Porta J M.Path planning under kinematic constraints by rapidly exploring manifolds［J］.IEEE Trans Robot，2013，29（1）:105-117.

［16］ 黄健生.移动机器人的路径规划研究［D］.杭州:浙江大学，2008:3-8.

（责任编辑 李亚青）

The Achievement of Path Planning Based on A*Algorithm

GUAN Quan-zhen，BAO Hong，SHI Zhi-jian
（Beijing Key Laboratory of Information Service Engineering，Beijing Union University，Beijing 100101，China）

Abstract:This study uses high-precision map to restore traffic information，combining with A*algorithm to make the driverless car travel according to the planned path without GPS.The high-precision map is presented in the form of raster model.A*algorithm can find a proper path from the starting point to end point for mobile robots in the environment which is presented in the form of raster and full of barriers.Through simulation experiments，it turns out that this method has the advantage of simplification，consistency and easy implementation on computer.

Key words:Path planning；Autonomous navigation；High-precision map；A*algorithm；Raster data model

［中图分类号］U 491.2

［文献标志码］A

［文章编号］1005-0310（2016）02-0031-09

DOI：10.16255/j.cnki.ldxbz.2016.02.006

［收稿日期］2016-03-07

［基金项目］国家自然科学基金重大研究计划项目（91420202）。

［作者简介］关泉珍（1990-），女，河南省许昌市人，北京联合大学北京市信息服务工程重点实验室硕士，主要研究方向为路径规划。

［通讯作者］鲍泓（1958-），男，北京市人，博士，北京联合大学副校长、教授，博士生导师，主要研究方向为图像处理、智能控制。E-mail:baohong@buu.edu.cn