航位推算和全球定位系统的组合定位方法

摘要

本发明公开了一种航位推算和全球定位系统的组合定位方法，包括初始化组合定位参量以待用于DR系统、GPS定位以判断是否启用DR系统以及DR和GPS组合定位。在DR和GPS组合定位中，DR系统输出的车辆的位置和速度与GPS接收机输出的位置和速度的差值被输入卡尔曼滤波器以获取校正值，通过该校正值和DR系统输出的位置和速度获取组合定位的位置和速度。本发明可以有效解决城市道路复杂环境下单GPS系统定位不连续不可靠的问题，本发明只需要使用两个里程计而不需要使用陀螺仪加速度计等惯性传感器辅助，就能实时校正DR系统的输出，使其不累积误差，本发明的实现方法简便可靠。

说明书

技术领域

本发明涉及车辆定位技术，尤其涉及一种航位推算和全球定位系统的组合定位 方法。

背景技术

车辆定位技术是实现行车路线引导的关键技术，GPS(全球定位系统)凭借其 定位迅速、准确，以及在无遮挡情况下可实现全天候定位的特点，成为当前首选的 车辆定位技术。但是，在当今的城市道路环境下，单独使用GPS导航定位系统的 车辆信号经常会受到高楼、树木、高架桥等物体的遮挡，导致其定位不连续以及定 位精度低等问题。因此GPS导航定位系统需要和其他导航系统进行组合，以获得 较高的导航定位质量。

在常与GPS导航定位系统组合的导航系统中，惯性导航系统由于惯性测量单 元价格较高，实现方法复杂，因此被较少应用。经常和GPS组合应用在车载组合 导航中的是DR(航道推算)导航定位方式，其通常的做法有三种：1、一个里程 计加一个单轴陀螺仪；2、一个单轴陀螺仪加一个单轴加速度计；3、两个里程计差 分。由于前两种方案都需要用到惯性传感器，而目前可用于车辆导航的惯性传感器 价格昂贵，且相应数据处理方法复杂，不适用于车载DR导航定位。另外，由于 DR导航系统是基于位置推测的，其误差会随时间累积。

因此，本领域的技术人员致力于开发一种航位推算和全球定位系统的组合定位 方法，无需使用惯性传感器，且能消除其中DR导航的随时间累积的误差。

发明内容

有鉴于现有技术的上述缺陷，本发明所要解决的技术问题是提供一种航位推算 和全球定位系统的组合定位方法，消除其中DR导航的随时间累积的误差。

为实现上述目的，本发明提供了一种航位推算和全球定位系统的组合定位方 法，用于对车辆导航定位，其特征在于，包括：

步骤100，初始化组合定位参量并将所述参量输入微处理器，所述参量包括所 述车辆的车轮的半径和后轮间距；

步骤200，GPS定位，包括：

启动GPS接收机；当所述GPS接收机的GPS定位成功，且当所述车辆的 速度大于设定值时，进入步骤300；

步骤300，DR和GPS组合定位，包括：

步骤310，所述微处理器将所述步骤100中获取的所述参量输入所述DR 系统，所述微处理器将完成所述步骤200后的所述GPS接收机输出的所述车辆的 位置和速度输入所述DR系统；

步骤320，所述微处理器将所述DR系统接收的所述车辆的位置和速度作 为所述DR系统采用的初始位置和初始速度，对所述DR系统进行初始化；所述微 处理器启动DR系统；

步骤330，所述微处理器指令所述DR系统工作，所述DR系统输出所述 车辆的位置和速度；

步骤340，所述微处理器将所述DR系统输出的所述车辆的位置和速度以 及所述GPS接收机输出的所述车辆的速度和位置输入卡尔曼滤波器以对所述DR 系统输出的所述位置和速度进行校正；

步骤350，所述微处理器根据所述卡尔曼滤波器输出的所述车辆的位置和 速度的校正量以及所述DR系统输出的所述车辆的位置和速度，计算获得所述车辆 的组合定位的位置和速度，并将所述的组合定位的位置和速度输出；

步骤360，所述微处理器将所述组合定位的位置和速度输入所述DR系统， 进入步骤320。

进一步地，所述DR系统和所述卡尔曼滤波器在所述微处理器中，所述GPS 接收机通过串口连接到所述微处理器，所述车辆包括左轮里程计和右轮里程计分别 连接到所述微处理器，所述左轮里程计与所述微处理器间的通信通过外部中断1 触发，所述右轮里程计与所述微处理器间的通信通过外部中断2触发。

进一步地，所述左轮是左后轮，所述右轮是右后轮。

进一步地，所述步骤200包括：

步骤210，GPS接收机接收卫星信号；

步骤220，所述微处理器判断GPS接收机是否定位成功，如果判断为是，则 进入步骤230，如判断为否，则进入步骤210；

步骤230，所述GPS接收机输出所述车辆的位置和速度，所述微处理器根据 所述GPS输出的所述车辆的速度以及所述左轮里程计和所述右轮里程计输出的脉 冲频率判断所述车辆的速度是否大于所述设定值，如果判断为是，则进入所述步骤 310，如果判断为否，则进入步骤210。

进一步地，所述GPS接收机输出的所述车辆的位置包括经度、纬度和高度， 所述DR系统输出的所述车辆的位置包括经度、纬度和高度，所述GPS接收机输 出的所述车辆的速度包括东向速度、北向速度和偏航角，所述DR系统输出的所述 车辆的速度包括东向速度、北向速度和偏航角。

进一步地，所述DR系统基于DR算法工作，所述DR算法根据所述车辆的车 轮的半径和后轮间距、初始位置和初始速度以及行驶中的所述车辆的所述左轮和所 述右轮在一个计算周期内通过的距离来获得所述车辆在所述一个计算周期末的位 置和速度。

进一步地，所述车辆的所述左轮在一个计算周期内通过的距离由所述左轮里程 计输出，所述车辆的所述右轮在一个计算周期内通过的距离由所述右轮里程计输 出。

进一步地，所述步骤340包括：

步骤341，所述微处理器判断GPS接收机是否定位成功，如果判断为是，则 进入步骤342，如判断为否，则进入步骤343；

步骤342，所述微处理器将所述DR系统输出的所述车辆的位置和速度与所述 GPS输出的所述车辆的位置和速度相减以获得差值，并将所述差值作为所述卡尔 曼滤波器的量测值；进入步骤344；

步骤343，所述微处理器将一个零向量作为所述卡尔曼滤波器的量测值；进入 步骤344；

步骤344，所述卡尔曼滤波器依次使用卡尔曼滤波的状态方程和卡尔曼滤波的 量测方程以对所述DR系统输出的所述位置和速度进行校正；

所述卡尔曼滤波的状态方程为：其中，t为时间，系统状 态转移矩阵F(t)是一个单位矩阵，系统过程噪声序列W(t)是一个均值为0、方差 为Q(t)、呈正态分布的函数，状态向量X(t)为所述量测值；当所述步骤341的判断 为是的时候，状态向量X＝[Δlon Δlat Δhed ΔVn ΔVe]^T中的⊿lon为所述GPS接收机 输出的所述车辆的经度和所述DR系统输出的所述车辆的经度之间的差值，⊿lat 为所述GPS接收机输出的所述车辆的纬度和所述DR系统输出的所述车辆的纬度 之间的差值，⊿hed为所述GPS接收机输出的所述车辆的偏航角和所述DR系统输 出的所述车辆的偏航角之间的差值，⊿Vn为所述GPS接收机输出的所述车辆的北 向速度和所述DR系统输出的所述车辆的北向速度之间的差值，⊿Ve为所述GPS 接收机输出的所述车辆的东向速度和所述DR系统输出的所述车辆的东向速度之 间的差值；当所述步骤341的判断为否的时候，状态向量X＝[0 0 0 0 0]^T；

所述卡尔曼滤波的观测方程为：其中，观测矩阵C(t) 为一个单位矩阵，测量噪声序列V(t)为一个均值为0、方差为R(t)、呈正态分布的 函数；当所述步骤341的判断为是的时候，观测向量Y(t)取为：

y_k＝[Δlon Δlat Δhed ΔVn ΔVe]^T；当所述步骤341的判断为否的时候，观测向量Y(t) 取为：y_k＝[0 0 0 0 0]^T；

所述为中间量，所述Y(t)为所述车辆的位置和速度的校正量。

进一步地，⊿lon等于所述DR系统输出的所述车辆的经度减去所述GPS接收 机输出的所述车辆的经度，⊿lat等于所述DR系统输出的所述车辆的纬度减去所 述GPS接收机输出的所述车辆的纬度，⊿hed等于所述DR系统输出的所述车辆的 偏航角减去所述GPS接收机输出的所述车辆的偏航角，⊿Vn等于所述DR系统输 出的所述车辆的北向速度减去所述GPS接收机输出的所述车辆的北向速度，⊿Ve 等于所述DR系统输出的所述车辆的东向速度减去所述GPS接收机输出的所述车 辆的东向速度。

进一步地，所述车辆的组合定位的位置和速度分别地等于所述DR系统输出的 所述车辆的位置和速度分别地减去所述卡尔曼滤波器输出的所述车辆的位置和速 度的校正量。

在本发明的较佳实施方式中，应用了上述的组合定位方法，其在GPS定位无 效的时候，依然能较好的体现运行中的车辆的真实位置。

本发明的航位推算和全球定位系统的组合定位方法具有以下优点：

1)相比于单独的GPS导航定位系统，本方法只额外使用了两个里程计(左 轮里程计和右轮里程计)，用来作为DR系统的信号输入；相比于常与GPS导航定 位系统组合的导航系统，本发明无需用到陀螺仪加速度计等惯性器件，处理方法简 洁高效可靠；

2)本发明中启动DR系统的方式能确保DR系统初始化对准快速、正确；

3)本发明的工作机制清晰，便于维护。在GPS定位不正常的情况下，其 仍然可以在较长时间内保持有效定位；

4)本发明能实时校正DR系统，使其随时间产生的误差不被累积；

5)本发明成本较低。

以下将结合附图对本发明的构思、具体结构及产生的技术效果作进一步说明， 以充分地了解本发明的目的、特征和效果。

附图说明

图1是本发明的航位推算和全球定位系统的组合定位方法的流程图。

图2是本发明中的卡尔曼滤波器的工作原理框图。

图3是在一个较佳实施例中，实现本发明的航位推算和全球定位系统的组合定 位方法的系统结构框图。

图4显示了图3所示的系统实施本发明的航位推算和全球定位系统的组合定位 方法的仿真结果。

具体实施方式

图1示出了本发明的航位推算和全球定位系统的组合定位方法的流程图，在本 实施例中，其应用于如图3所示的系统。该系统包括微处理器、GPS接收机和两 个里程计(左轮里程计及右轮里程计)，其中微处理器中有DR系统和所述卡尔曼 滤波器，GPS接收机通过串口连接到微处理器，左轮里程计和右轮里程计分别连 接到微处理器，左轮里程计与微处理器间的通信通过外部中断1触发，右轮里程计 与微处理器间的通信通过外部中断2触发。其中左轮是车辆的左后轮，右轮是车辆 的右后轮，左轮里程计及右轮里程计分别计量左轮和右轮经过的里程。

本发明的航位推算和全球定位系统的组合定位方法具体地为：

步骤100，初始化组合定位参量并将这些参量输入微处理器。这些参量是与 DR系统的工作相关的常数，包括车辆的车轮的半径和后轮间距。

本实施例中，车辆的车轮的半径为0.33米，后轮间距为1.55米。

步骤200，GPS定位。

首先不启动DR系统，而进行GPS独立的定位，具体地包括步骤210、220和 230：

步骤210，GPS接收机接收卫星信号。这是GPS接收机的工作方式，在此不 赘述。

步骤220，微处理器判断GPS接收机是否定位成功，如果判断为是，则进入 步骤230，如判断为否，则进入步骤210；

步骤230，GPS接收机输出车辆的位置和速度，微处理器根据GPS输出的车 辆的速度以及左轮里程计和右轮里程计输出的脉冲频率判断该车辆的速度是否大 于设定值，如果判断为是，则进入所述步骤310，如果判断为否，则进入步骤210。

这里的设定值，是启动DR系统的最小车速，即只有当车辆的速度到达一定值 时，才启动DR系统，否则还是保持GPS的独立定位。一般地，该设定值取为 10Km/h。

步骤300，DR和GPS组合定位，其具体地包括步骤310、320、330、340、350 和360。

步骤310，微处理器将步骤100中获取的那些参量输入DR系统，微处理器将 完成步骤200后的GPS接收机输出的车辆的位置和速度输入DR系统。

GPS接收机输出的车辆的位置包括经度、纬度和高度，GPS接收机输出的车 辆的速度包括东向速度、北向速度和偏航角。

步骤320，微处理器将DR系统接收的车辆的位置和速度作为DR系统采用的 初始位置和初始速度，对DR系统进行初始化；微处理器启动DR系统。

步骤330，微处理器指令DR系统工作，DR系统输出车辆的位置和速度。

本步骤中，DR系统基于DR算法工作，其根据车辆的车轮的半径和后轮间距、 初始位置和初始速度以及行驶中的车辆的左轮和右轮在一个计算周期内通过的距 离来获得所述车辆在这个计算周期末的位置和速度并将该位置和速度输出。其中， 车辆的左轮在一个计算周期内通过的距离由左轮里程计输出，车辆的右轮在一个计 算周期内通过的距离由右轮里程计输出。

计算周期可以设定，本实施例中选用的计算周期为1秒。

DR系统输出的车辆的位置包括经度、纬度和高度，DR系统输出的车辆的速 度包括东向速度、北向速度和偏航角。

步骤340，微处理器将DR系统输出的车辆的位置和速度以及GPS接收机输出 的车辆的速度和位置输入卡尔曼滤波器以对DR系统输出的位置和速度进行校正， 具体地包括步骤341、342、343和344。

步骤341，微处理器判断GPS接收机是否定位成功，如果判断为是，则进入 步骤342，如判断为否，则进入步骤343。

步骤342，微处理器将DR系统输出的车辆的位置和速度与GPS输出的车辆的 位置和速度相减以获得差值，并将该差值作为卡尔曼滤波器的量测值；进入步骤 344。

参见图2所示的卡尔曼滤波器的工作原理框图，本实施例中是用DR系统输出 的车辆的位置和速度分别地减去GPS输出的车辆的位置和速度，由此获得差值 ⊿lon、⊿lat、⊿hed、⊿Vn和⊿Ve，其中，⊿lon等于DR系统输出的车辆的经度 减去GPS接收机输出的车辆的经度，⊿lat等于DR系统输出的车辆的纬度减去GPS 接收机输出的车辆的纬度，⊿hed等于DR系统输出的车辆的偏航角减去GPS接收 机输出的车辆的偏航角，⊿Vn等于DR系统输出的车辆的北向速度减去GPS接收 机输出的车辆的北向速度，⊿Ve等于DR系统输出的车辆的东向速度减去GPS接 收机输出的车辆的东向速度。由此，卡尔曼滤波器的量测值为状态向量 X＝[Δlon Δlat Δhed ΔVn ΔVe]^T。

步骤343，微处理器将一个零向量作为卡尔曼滤波器的量测值；进入步骤344。 即认为GPS接收机与DR系统具有相同的输出，由此卡尔曼滤波器的量测值为状 态向量X＝[Δlon Δlat Δhed ΔVn ΔVe]^T＝[0 0 0 0 0]^T。

步骤344，卡尔曼滤波器依次使用卡尔曼滤波的状态方程和卡尔曼滤波的量测 方程以对所述DR系统输出的所述位置和速度进行校正。

首先使用卡尔曼滤波的状态方程：式中，t为时间，系 统状态转移矩阵F(t)是一个单位矩阵，系统过程噪声序列W(t)是一个均值为 0、方差为Q(t)、呈正态分布的函数。这里为中间量，可以认为它是对X(t) 的估值。本实施例中，方差Q(t)被设置为0.2乘以单位矩阵。

然后使用卡尔曼滤波的观测方程为：式中，观测 向量Y(t)取为：y_k＝[Δlon Δlat Δhed ΔVn ΔVe]^T，观测矩阵C(t)为一个单位矩阵， 测量噪声序列V(t)为一个均值为0、方差为R(t)、呈正态分布的函数。本实施例 中，方差R(t)被设置为0.4乘以单位矩阵。

卡尔曼滤波器输出的Y(t)为车辆的位置和速度的校正量。

步骤350，微处理器根据卡尔曼滤波器输出的车辆的位置和速度的校正量以及 DR系统输出的车辆的位置和速度，计算获得车辆的组合定位的位置和速度，并将 该组合定位的位置和速度输出。

如图2所示，本实施例中车辆的组合定位的位置和速度分别地等于DR系统 输出的车辆的位置和速度分别地减去卡尔曼滤波器输出的车辆的位置和速度的校 正量。

步骤360，微处理器将上述组合定位的位置和速度输入DR系统，进入步骤320。 由此DR系统开始对下一个计算周期末的车辆的位置和速度进行推算。

本实施例通过使用Spirent公司生产的模拟器并编辑一段车辆行驶轨迹来模 拟车辆运行，模拟器在运行编辑好的轨迹时可以给出每一时刻标准的经、纬度， 同时输出相对应的卫星信号以及车辆左右后轮的里程计信号。在运行场景时， 人为地在一些时间段切断GPS信号，使用本发明的航位推算和全球定位系统的 组合定位方法进行组合导航计算，同时将标准位置信息、GPS接收机输出的位 置信息和本发明组合导航计算得到的位置信息绘制于图4。由图4可以看出在 GPS定位无效的时候，本发明依然能较好的体现车辆的真实位置。

以上详细描述了本发明的较佳具体实施例。应当理解，本领域的普通技术人员 无需创造性劳动就可以根据本发明的构思做出诸多修改和变化。因此，凡本技术领 域的技术人员依本发明的构思在现有技术的基础上通过逻辑分析、推理或者有限的 实验可以得到的技术方案，皆应在由权利要求书所确定的保护范围内。