基于数字地图和惯性导航的车辆自主导航

文/汤爽 杨逸菲 张巧可 鄂旭

车辆自主导航系统是利用内置传感器来确定车辆自身所处的相对位置以及行驶方向，采用数学分析的方法来确定车辆行驶路径，并通过将该车辆行驶路径与内存电子地图上的道路进行对比，来确定车辆在地图上所处的位置及所要到达目的地的方向和所剩距离等信息，并在显示器上显示出来，从而起到导航和引导的功能。车辆自主导航是降低车辆突发事件概率及减少人类乘车危险的一项核心技术，将广泛应用于道路行驶。定位的精确度和控制的稳定性是实现车辆自主导航的关键，随着 GPS（Global position system）、GLONASS（Global navigation satellite system）、伽利略卫星系统和北斗卫星系统的逐步完善，车辆自主导航技术的实现已经成为研究重点。国内外对于自主导航的研究主要集中在如何提高定位控制精度，精度方面如采用 RTK-GPS（Real time kinematic-GPS）差分定位技术、多传感器信息融合技术、视觉导航技术等；控制方面如采用PID（Proportion integration differentiation） 控制方法、模糊控制、最优控制、纯追踪模型等参考文献[1]。严恭敏[2]等人采用惯导/里程仪的组合算法，可大大减小惯性导航随时间的积累误差逐渐变大这一缺点，来为车辆提供精准的定位信息。陈靓影[3]等人采用GPS与惯性导航系统融合的精确连续定位算法，克服独立的惯性导航系统无限制的位置误差。由于车辆行驶道路的多样性，算法的适应性和多样性都有很大的改善空间。惯性导航系统的误差还会随着时间的增长而不断积累增大,所以本文主要研究的是在数字地图和惯性导航二者的基础上提出的一种车辆自主导航算法。应用地图匹配技术将惯性导航系统所得到的精确的定位信息与数字地图进行对比和修正，从而提高定位的精度和导航的实时控制性，并根据行驶过程中各种情况的不同需要，建立了直线搜索路径与曲线搜索路径的方法，来实现车辆的自主导航行驶功能。

1 平台体系架构

1.1 车辆自主导航系统平台搭建

车辆自主导航系统主要由一辆小汽车、加速度计、陀螺仪、方向盘控制装置及前轮转角检测装置等构成，其中小汽车为试验平台，将加速度计和陀螺仪这两种惯性元件组成惯性导航系统设备，安置到试验平台上，去分别测出车辆相对于惯性空间的线运动信息，并在给定的初始条件下，由计算机推测出车辆的姿态、行驶方向、速度、位置等，系统组成如图1所示。

1.2 路径搜索与转向控制

车辆在自主导航的行驶中，系统会根据车辆状况设置出直线预设路线和曲线预设路线，然后用直线、曲线路径去定位当前车辆的所处位置，根据车辆当前位置与预设路径之间的横向偏差，控制车辆在道路上行驶。转弯路径作为一种转弯角度大的曲线路径，预设路径的生成与曲线路径生成相似。 在用户选择转弯方式后，系统根据车辆的行驶幅度和最小转弯半径生成预设路径，下文则分别介绍直线路径搜索方法、曲线路径搜索方法和车辆转向方式。

1.2.1 直线路径搜索方法

本文参考借鉴文献[1]，选用简化小车为运动模型，不考虑轮胎与地面摩擦和侧滑、以及空气阻力等因素，在直线导航路径上设一点A，在距A点十米处在设置个B点，连接A、B并延长，为导航的直线预测路线。如图2a所示，P点为车辆当前所处位置，通过惯性导航可以实时获取P点的位置坐标，将车辆位置和行驶方向信息与直线预测路线对比，便会得到横向偏差，通过P向AB做垂线，垂足为P0，PP0的距离d（cm）为车辆在行驶过程中的横向偏差，直线MN为AB的平行线。

图1

图2：直线、曲线搜索图

图3：转向路径搜索图

1.2.2 曲线路径搜索方法

曲线路径搜索是将一条曲线分成n个小段，再按照直线路径搜索方法进行横向偏差的计算，如图2b所示，在曲线上设置A1, B1, C1,D1四个导航点，P1点为车辆当前所处位置，通过P1点向曲线A1B1段的切线做垂线，垂足为P1`，M1N1为曲线A1D1的切线的平行线，P1P1`长为d（cm），然后求出车辆的当前位置点P分别与每个导航点距离，取最小值，得到与车辆最近的导航点，假设A1点为M，B1点为（M+1），那么就可以按照直线路径的搜索方法进行处理，同理可得，横向偏差为d（cm）。

1.2.3 车辆转向控制

首先通过利用车辆运动学模型的几何方法。根据车辆行驶速度来确定预设点的位置，得到车辆到达指定位置的圆弧段路径，并用运动模型来计算车辆的转角。由图3c所示，A2E2为车辆行驶转弯的预设路径，由一系列点组成，生成方法与曲线路径生成方法类似，根据车辆行驶速度等间距的取A2, B2, C2, D2, E2五个点为例，开始导航时，车辆当前所处位置P2，搜索落在以P2为圆心，R2为半径的圆内预设路径上的点。半径R2的确定与车辆行驶的速度和曲线的弯曲度有关，选择车速为0.4M/s，R2为1.6M，恰好A2, B2, C2, D2四个点落在搜索圆内。点B2为距离P2点最近的点，点D2为搜索范围内的最后一个预测点，故P2D2圆弧为车辆的行驶路径，点O，为行驶路径的圆弧的圆心，线段 P2P2`为曲线 A2E2切线的垂线，点 P2`为垂足，线段 P2P2`为横向偏差（cm）。

在XOY坐标系下，利用惯性导航的定位功能来获取当前位置坐标P2（X1, Y1），并由车辆行驶弧度和最小转弯半径来推算出预设路径，根据车辆行驶速度和各导航点的间距得出预算点D2的坐标（X2,Y2），将坐标轴旋转，以P2O`为X轴，P2F2为Y轴，P2（0，0）为车辆当前位置的坐标系F2P2O`的原点，建立直角坐标系，可得新坐标系下F2P2O`预瞄点坐标D2（x,y），XOY坐标系下点P2坐标为(x1,y1)，点 D2坐标为 (x2, y2)，F2P2O,坐标系下D2坐标为(x,y)。

通过D2点向P2O,做垂线，垂足为Q，连接D2O，由图3c几何关系建立方程组：

圆弧P2D2的圆心为O`，由几何关系结合（2）得：

P2O`为圆弧P2D2的半径，即转弯半径。

2 自主导航路径的搜索方式

2.1 设计思路

如图4所示，车辆自主导航系统共有四个模块，由数据收发、数据处理、数据存储以及显示界面组成，数据收发模块负责接收惯性导航系统提供的车辆实时定位坐标，数据处理模块将INS接到的数据进行坐标系的转换和参数的设定，根据行驶需求利用直线，曲线路径搜索算法完成路径跟踪，数据存储模块负责车辆位置，导航状态等信息的存储，显示界面模块负责车辆状态，路径以及坐标等信息的显示。

2.2 实验与测试

本实验参考借鉴文献[4]，车辆分别在0.4m/s、0.6m/s、0.8 m/s 的恒定速度下直线行驶，横向误差是导致车辆在行驶过程中造成误差的主要原因，根据实验数据，建立了数学模型，当车辆分别在 0.4m/s、0.6m/s、0.8 m/s 时，数学模型如（4）（5）（6）所示，由于模型的二次项系数均大于0，故一阶导数均为递增函数，即横向误差的变化呈线性递增。通过计算，线性递增的斜率分别为 0.0018、0.0034、0.0050，表明车辆行驶速度越快，横向误差就越大。采用MATLAB作为实验环境，CPU采用Intel（R）Core(TM)i7-8550U@1.80GHz 1.99GHz,系统类型为64位操作系统画出试验测试图。

图4：系统功能结构图

图5：实验效果图

h为横向偏差的累积值，t为时间。

试验结果如图5（d),（e）,（f）所示：

实验结果表明：在相同时间内，忽略摩擦力等因素的影响，随着车辆行驶速度的不断增大，横向偏差也会越来越大；随着车辆行驶速度的不断减小，横向偏差也会越来越小。

3 结论

本文通过利用车辆运动学模型的几何等方法，计算并试验得出“汽车随着速度的增大，偏差也会越来越大；随着车辆行驶速度的不断减小，横向偏差也会越来越小”的结论。我们应用地图匹配技术将惯性导航系统所得到的精确的定位信息与数字地图的结果进行对比，修正，从而提高定位精度和导航实时控制性，对减小汽车自主行驶的误差有一定的帮助，并根据行驶过程中的不同环境，参考借鉴文献[1]建立了直线搜索路径与曲线搜索路径的方法。同时在数字地图和惯性导航结合的基础上，会更加提高定位精度和导航实时控制性。该方法具有更好的导航效果。