专利名称：一种智能车辆中差分gps与惯性导航数据融合的系统和方法
技术领域：
本发明涉及一种智能车辆中差分GPS与惯性导航数据融合的系统和方法，属于汽车控制领域。
背景技术：
智能车辆，也称无人驾驶，是室外轮式移动机器人在交通领域中的重要应用。它使用车载传感器(激光雷达、超声传感器、惯性系统、视觉、GPS、里程计、磁罗盘等)感知车辆周围环境，获取自身位置和姿态。智能车辆具有高度的自规划、自组织和自适应能力，在无人干预的情况下，能够根据感知所获得的道路、车辆位姿和障碍物信息，控制车辆的转向和速度，从而使车辆能够安全、可靠地在道路上行驶，并完成预定的任务。应用视觉定位方法主要是根据摄像头实时拍摄的视频图像来计算运动物体的运动参数，从而提供定位信息。现阶段视觉定位方式主要有两种，一种是基于人工标志的视觉定位，即在可知的环境中，设置简单易识别的标志，摄像机拍摄到包含人工标志的图像后，视觉定位算法根据标志物在图片中成像的大小、位置及变化，就可以确定自身的位置和运动参数；第二种方法是同时定位与地图构建技术(Simultaneous localization andmapping, SLAM)，是指车辆在未知环境中从一个未知位置开始移动，在移动过程中根据位置估计和地图进行自身定位，同时在自身定位的基础上构建增量式地图，实现车辆的自主定位。但其受环境变化影响大，晴天时树木的阴影，光滑表面的反光，阴雨道路上的积水以及夜间光照不足等都会对图像信息的识别造成很大影响，大大的降低了智能车辆的环境适应性。惯性导航技术是利用陀螺仪和加速度计等传感器的信息自主地推算出车辆相对于起始点的位置。这种定位技术是一种依靠载体自身设备独立自主地进行定位，不受外界环境的干扰。虽然此种技术在短时间内的定位精度较高，但由于传感器本身存在随机漂移和随机误差，在推算过程中定位误差将随时间而累积。目前应用最广的是GPS定位导航系统，该系统具有全球、全天候、连续、实时提供高精度的三维位置和三维速度信息的能力，是实现全球定位的高新技术。但是在城市高楼区、林荫道、隧道等地方有可能导致GPS信号的遮挡，引起GPS信号的暂时中断以及多路径效应，从而造成车辆定位系统无法实时定位或者定位精度降低。综上所述，本发明采用了差分GPS与惯性系统组合的方式，一方面差分GPS能够提供的绝对位置精度很高，不受定位时间的影响，减少了多路径效应，并可以为惯性系统提供航位推算的初值，对惯性系统进行定位误差等系统参数的矫正；另一方面，惯性系统的连续推算具有很高的相对精度，可以补偿GPS系统定位信号更新频率慢以及GPS信号被遮挡时精度下降或无法定位等缺陷。通过组合导航定位的数据融合方案，可得到最优的组合定位结果，使智能车辆能够在多种复杂环境状态下准确的进行导航定位，并向规划决策计算机传递数据以控制车辆底层执行机构。
发明内容
本发明主要针对智能车辆的导航与定位和车辆运行的地图显示等问题，提出了一种智能车辆中差分GPS与惯性导航数据融合的系统和方法，用来解决无人驾驶和辅助驾驶中车辆的导航与定位问题，以提高车辆行驶的安全便捷性，为智能车辆的无人驾驶提供了有效地保障。为了实现上述目的本发明采用的技术方案为:一种智能车辆中差分GPS与惯性导航数据融合的系统，包括数据融合上位机、规划决策上位机、差分GPS、惯性导航系统以及路由器；所述差分GPS通过空间卫星信号获取经过基站电台修正的GPS数据包应用串口传输方式到所述数据融合上位机；所述惯性导航系统采集智能车辆的行驶状态信息同样通过串口传输到所述数据融合上位机；所述数据融合上位机依据NEMA-0183协议提取GPS数据包中速度、航向和经纬度这些信息并进行坐标变换到大地坐标系，同时提取出所述惯性导航系统的航向变化率和X、y轴加速度，并通过数据处理算法得到航向角和x、y轴速度及位移，同时对上述数据进行融合并将精确的航向、速度和位移这些数据信息转换到MapX控件中进行地图显示；所述路由器在搭建的局域网中利用UDP协议将打包后的包括上述经纬度及融合后的精确航向、速度和位移的车辆运行控制数据转发给所述规划决策上位机供其使用，所述规划决策上位机解析上述车辆运行控制数据后得出控制命令对车辆底层执行机构进行控制。一种智能车辆中差分GPS与惯性导航数据融合的方法，包括以下步骤:步骤一:车辆顶部的差分GPS从空间卫星信号获取经过基站电台修正的GPS数据包通过串口传输方式到数据融合上位机，并进行一定的解包和坐标转换的处理；步骤二:车身内中部的惯性导航系统采集智能车辆动态参数通过串口传输方式到所述数据融合上位机，并对数据解析后进行一定的滤波和多次积分处理；步骤三:在获取了步骤一和步骤二中的上述差分GPS以及惯性导航系统的导航定位信息后就进行数据融合，并将数据融合后的导航定位信息转换到MapX地图控件中从而描绘出车辆位置及行驶路径；步骤四:将包含有导航定位信息的车辆运行控制数据由路由器搭建的局域网中利用UDP协议转发给规划决策上位机供其使用；步骤五:规划决策上位机解析上述车辆运行控制数据后得到的控制命令来对底层执行机构进行控制，实施车辆转向、抬起油门或踩下刹车动作。所述数据融合后的导航定位信息包括融合后的精确航向、速度和位移。所述数据融合后的导航定位信息还包括经度、纬度、检验位以及ID号。有益效果通过对智能车辆中差分GPS和惯性导航系统的信息获取，便可以根据一定的算法来进行数据融合，实现了导航与定位的精确性和地图显示车辆运行轨迹的可视化效果。而将相应数据实时地传输给规划决策上位机，对智能车辆的执行机构进行了有效的控制，保障了其合理运行及安全性。
附图1为本发明智能车辆中差分GPS与惯性导航数据融合系统组成框图；附图2为本发明智能车辆中差分GPS与惯性导航数据融合系统总体设计图；附图3为惯性导航系统算法框图；附图4为智能车辆中差分GPS与惯性导航数据融合系统运行过程图；附图5为实车试验软件流程图。其中，1-数据融合上位机；2_规划决策上位机；3_差分GPS ;4-惯性导航系统；5-路由器。
具体实施例方式如图1-5所示，本发明的一种智能车辆中差分GPS与惯性导航数据融合的系统和方法，是由两台安装有VS2008程序的笔记本电脑构成的上位机1、2、一个差分GPS3、一个惯性导航系统4和网络路由器5组成。差分GPS3通过空间卫星信号获取经过基站电台修正的GPS数据包应用串口传输方式到数据融合上位机I ;惯性导航系统4采集智能车辆的行驶状态信息同样通过串口传输到数据融合上位机I ;数据融合上位机I依据NEMA-0183协议提取GPS数据包中速度、航向和位置这些信息并进行坐标变换到大地坐标系，同时提取出所述惯性导航系统4的航向变化率和x、y轴加速度，并通过数据处理算法得到航向角和x、y轴速度及位移，同时将上述航向角和x、y轴速度及位移这些数据信息在MapX控件中进行地图显示；路由器5在搭建的局域网中利用UDP协议将打包后的包括上述航向角和x、y轴速度及位移的车辆运行控制数据转发给所述规划决策上位机2供其使用，规划决策上位机2解析上述车辆运行控制数据后得出控制命令对车辆底层执行机构进行控制。其算法和工作流程如下:1.数据融合上位机I应用VS2008开发环境编写的差分GPS3采集程序实时地获取GPS移动站经过基站电台修正的差分原始数据，依照NEMA-0183协议将该数据进行解析以获取如经纬度、速度、航向(航向角)等信息。而GPS输出的坐标为ECEF坐标系统下的纬度、经度和高度，然而这样的定位数据不适用于做平面运动的车辆。因此，要将GPS输出的经讳度转化到平面坐标系下。本实施例优选通用横轴墨卡托坐标系(Universal TransverseMercator, UTM),此坐标系是一个基于栅格的二维笛卡尔坐标系，由60个基于正切横轴墨卡托投影的区域组成，即将北纬84°和南纬80°的地球表面，以经度6°作为宽度，分成60个区域。这使得坐标变换后的经纬度相较于其它算法更为精确。2.惯性导航系统4同样由数据融合上位机I应用VS2008开发环境编写其测量单元的实时采集程序。系统中光纤陀螺用来测量车体相对于惯性系的旋转角速度，加速度计用来测量车体质心的线加速度，它们的测量值中都包含很多毛刺脉冲和随机干扰，如果直接送入惯性导航算法将带来很大的额外累积误差，本实施例优选采用中值滤波的方法对测量数据进行预先滤波。中值滤波是一种非线性平滑技术，基于排序统计理论的一种能有效抑制噪声的非线性信号处理技术。基本原理是把数字序列中一点的值用该点的一个邻域中各点值的中值代替，使周围的像素值接近真实值，从而消除孤立的噪声点。而惯性导航算法采用的是一种纯数字积分方法，即对线加速度积分得到速度、位移，同时与角速率积分得到车体姿态后一起构成了完备的车辆状态信息。3.应用数据融合上位机I对差分GPS3的航向、速度和位移与惯性导航系统4的航向角、速度和位移进行融合，本实施例采用了扩展卡尔曼滤波。首先卡尔曼滤波是一种递推线性最小方差估计技术，它在以前时刻状态估值的基础上，根据当前时刻的测量值，递推得到当前时刻状态估值。其计算量小，计算速度快，而且能够在合适的条件下给出无偏估计量。然而实际中有很多系统是非线性的，处理这些系统就要采用扩展卡尔曼滤波，即将期望和方差线性化的卡尔曼滤波器。通过此算法可以有效地补偿GPS系统定位信号更新频率慢，以及在信号丢失时仍可单独依靠惯性系统进行数据输出，并能在GPS信号恢复后对惯性系统进行更新。保障了智能车辆导航定位的连续性与精确性。4.数据融合上位机基于MapX控件进行地图信息显示程序的编写。应用扩展卡尔曼滤波算法测算出的精确且连续的航向值，与行驶距离相结合来进行一系列的坐标转换和投影处理，并利用航向角将行车距离投影到经纬度方向上的方法可以计算出精确的经纬度坐标，有效地在MapX地图控件中描绘出了车辆位置及其运行轨迹。5.通过路由器5向控制决策计算机的数据传输采用了 UDP协议并应用多线程的方法进行实现，其有着并行处理及资源消耗小的特点，保证了数据传输的实时性要求。如图1所示的系统框图为智能车辆中差分GPS3与惯性导航系统4的数据融合系统的组成框图，介绍了系统组成部件及其连接关系。如图2所示为智能车辆中差分GPS3与惯性导航系统4的数据融合系统总体设计图。差分GPS3实时地获取移动站经过基站电台修正的满足NEMA-0183协议的差分原始数据(如:$GPGGA，UTM时间，纬度，...，*hh〈CRXLF>)，该数据经过坐标变换到导航坐标系下，并与由惯性测量单元采集到并经过预处理后送入惯性导航算法计算得到的导航数据一起送入扩展卡尔曼滤波，得到修正后的高精度汽车状态信息。通过MapX控件将车辆轨迹等信息显示在地图上，与此同时将相关的车辆控制信息通过UDP协议传递给智能车辆的规划决策上位机来对底层执行机构进行控制以实现车辆的前进与转向，并保障其安全性与合理运行。如图3所示为惯性导航系统算法框图。惯性导航系统4属于一种推算导航方式，即从一已知点的位置根据连续测得的运载体航向角和速度推算出其下一点的位置，因而可连续测出运动体的当前位置。采用纯数字积分方法的惯性导航算法，即由加速度计测得车体运动的线加速度，然后积分得到速度、位移，同时与陀螺仪测得的角速率以及积分得到的车体姿态一起构成完备的车辆状态信息。如图4所示为智能车辆中差分GPS3与惯性导航系统4的数据融合系统的运行过程图。依据差分GPS3与惯性导航系统4的数据传输方式连接数据传输线(串口线、USB线、网线)，连接符合要求的电源线并上电为其供电。以差分GPS3与惯性导航系统4设定好的配置参数来启动，根据差分GPS3与惯性导航系统4的使用场合来设置工作模式(如端口号和波特率等)。设置完成后通过软件同时启动差分GPS3与惯性导航系统4来连续的获取数据并传输到数据融合上位机1，在上位机I上把这些数据应用预处理(坐标变换、滤波和积分等)算法来获取智能车辆的导航定位信息并进行数据融合。应用持续输出且精确的导航定位信息在地图控件中描绘出车辆位置及行驶轨迹。将车辆控制数据通过UDP协议在局域网上传输给规划决策上位机2以控制执行机构。在实验完成后修改工作模式或对系统进行维护，并断开电源线和数据传输线。如图5所示为实车试验软件流程图。软件开始运行后将对差分GPS3与惯性导航系统4进行初始化并开始采集与处理相关数据，每当到达GPS采样时刻时会应用扩展卡尔曼滤波算法对惯性系统的数据进行校正。得到的精确导航定位信息会通过UDP协议传输给规划决策上位机2，与此同时也将应用MapX控件进行车辆运行情况的地图显示。下表I所示为定义的UDP通信协议格式，其中校验位采用的算法为(ID~RXHrRXH2……~RXH7) &07 ，有效地保证了数据的完整性。表权利要求
1.一种智能车辆中差分GPS与惯性导航数据融合的系统，其特征在于:包括数据融合上位机(I)、规划决策上位机(2)、差分GPS (3)、惯性导航系统(4)以及路由器(5)； 所述差分GPS (3)通过空间卫星信号获取经过基站电台修正的GPS数据包应用串口传输方式到所述数据融合上位机(I); 所述惯性导航系统(4)采集智能车辆的行驶状态信息同样通过串口传输到所述数据融合上位机(I); 所述数据融合上位机(I)依据NEMA-0183协议提取GPS数据包中速度、航向和经纬度这些信息并进行坐标变换到大地坐标系，同时提取出所述惯性导航系统(4)的航向变化率和x、y轴加速度，并通过数据处理算法得到航向角和x、y轴速度及位移，同时对上述数据进行融合并将精确的航向、速度和位移这些数据信息转换到MapX控件中进行地图显示； 所述路由器(5)在搭建的局域网中利用UDP协议将打包后的包括上述经纬度及融合后的精确航向、速度和位移的车辆运行控制数据转发给所述规划决策上位机(2 )供其使用，所述规划决策上位机(2)解析上述车辆运行控制数据后得出控制命令对车辆底层执行机构进行控制。
2.一种智能车辆中差分GPS与惯性导航数据融合的方法，其特征在于:包括以下步骤: 步骤一:车辆顶部的差分GPS (3)从空间卫星信号获取经过基站电台修正的GPS数据包通过串口传输方式到数据融合上位机(1)，并进行一定的解包和坐标转换的处理； 步骤二:车身内中部的惯性导航系统(4)采集智能车辆动态参数通过串口传输方式到所述数据融合上位机(I)，并对数据解析后进行一定的滤波和多次积分处理； 步骤三:在获取了步骤一和步骤二中的上述差分GPS (3)以及惯性导航系统(4)的导航定位信息后就进行数据融合，并将数据融合后的导航定位信息转换到MapX地图控件中从而描绘出车辆位置及行驶路径； 步骤四:将包含有导航定位信息的车辆运行控制数据由路由器(5)搭建的局域网中利用UDP协议转发给规划决策上位机(2)供其使用； 步骤五:规划决策上位机(2)解析上述车辆运行控制数据后得到的控制命令来对底层执行机构进行控制，实施车辆转向、抬起油门或踩下刹车动作。
3.根据权利要求2所述的方法，其特征在于:所述数据融合后的导航定位信息包括融合后的精确航向、速度和位移。
4.根据权利要求3所述的方法，其特征在于:所述数据融合后的导航定位信息还包括经度、纬度、检验位以及ID号。
全文摘要
本发明涉及一种智能车辆中差分GPS与惯性导航数据融合的系统和方法，其利用安装在智能车辆顶部的差分GPS获取空间卫星所提供的信息，并应用将要进行数据融合的上位机依据NEMA-0183协议提取GPS数据以得出航向、速度及位置等信息；利用置于车身内中部的惯性导航系统采集的车辆运行中航向变化率和x、y轴加速度的数据传输给将要进行数据融合的上位机得出航向角及x、y轴的速度和位移，通过算法融合，最终得出精确的导航定位信息并可在GPS信号丢失的情况下利用惯性系统持续的提供相关信息。同时，将导航定位数据在地图中显示并打包数据后通过路由器搭建的局域网络传输至规划决策上位机，再依据解析的信息和实际行驶情况对智能车辆进行控制。
文档编号G01C21/34GK103207634SQ201310089459
公开日2013年7月17日 申请日期2013年3月20日 优先权日2013年3月20日
发明者段建民, 王凡 申请人:北京工业大学