CN103453913B - 停车场中车辆的组合定位方法 - Google Patents

Abstract

一种室内停车场精确的组合定位方法，由DR（Dead？Reckoning，航位推算）模块和室内停车场电子地图匹配模块构成，DR模块使用车载的传感器，包括加速计、里程计、陀螺仪，坡度检测仪等，进行车辆位置和状态的推断，室内停车场电子地图进过道路分段和折线化处理，能够更精确的描述停车场内的行驶路线，将航位推算得到的估计位置，送到地图匹配器进行位置匹配，得到的匹配位置和实时车辆速度、位置信息输入卡尔曼滤波器，再次得到车辆的估计位置，输入到地图匹配器中进行匹配，在地图匹配器和卡尔曼滤波器之间的增益回路中循环定位过程，滤波器的估计位置和地图匹配器的匹配位置重合，地图分段和折线化提高了地图的精确度，循环定位提高了室内定位的准确性。

Description

停车场中车辆的组合定位方法

技术领域

本发明属于车辆导航领域，具体涉及航位推算和地图匹配领域。

背景技术

随着经济的高速发展，近年来，我国汽车保有量急剧增长，2012年底，达到1.25亿辆，正式进入汽车时代。导致了传统的中小型停车场已无法满足实际泊车需求，因此，很多地方修建了大型停车场，比如飞机场、大型展览中心、大型购物中心等地。尽管大型停车场有效地缓解了汽车对泊位的需求，但是，人们却产生了新的苦恼，即难以获取停车场内空闲泊位的信息，取车时难以记起车辆的准确停靠位置，取车后难以找到准确的停车场出口。

所以室内车辆定位系统越来越受到关注，为了提高室内车辆管理的效率，精确的室内定位方法就显得尤为重要。

传统的车辆定位导航系统一般包括两个步骤，第一步，将车载传感器（如加速计、陀螺仪、里程计等）收集的数据和GNSS（GlobalNavigationSatelliteSystem，全球卫星导航系统）接收器收集的数据通过卡尔曼滤波器算法进行融合，鉴于卡尔曼滤波器的迭代特性，它很容易在实时环境中实现，输出的位置估计信息包含了车辆的经度、纬度、高度和航向角等。

第二步，地图匹配器将第一步中估计的车辆位置通过地图匹配方法投影到数字地图中的路段，匹配的结果会显示给驾驶员，这将成为导航和定位的基础。

但是GNSS接收器容易受到道路和环境的影响，在高大建筑物密布的市区，GNSS的可用性急剧下降到60%，GNSS估计位置的准确性和精度受到严重的影响，需要提出新的室内定位算法，为室内车辆提供准确的定位服务。

目前的地图匹配算法主要有基于几何信息的匹配和基于网络拓扑关系的匹配两种思想，具体算法有基于卡尔曼滤波、基于曲线拟合和基于权重度量值等匹配算法。基于曲线拟合的匹配算法，能充分利用历史数据，稳定性好，但是比较复杂；基于权重度量值的匹配算法，实时性好，但在复杂道路网络下匹配准确度较低；基于卡尔曼滤波的匹配算法，能迭代预测，修正处理结果，并且在求解时不需要存储大量的观测数据，但是要求观测方程具有线性特性，同时要求噪声是白噪声。

传统的车辆定位导航技术一般包括两个步骤，首先，将车载传感器（如加速计、陀螺仪、里程计等）收集的数据和GNSS（全球卫星导航系统）接收器收集的数据通过卡尔曼滤波器算法进行融合，鉴于卡尔曼滤波器的迭代特性，卡尔曼滤波器方法算法很容易在实时环境中实现，产生的位置估计信息包括经度、纬度、高度和航向角等。

然后，地图匹配器将第一步中估计的位置通过地图匹配方法投影到道路交通网络的数字地图上，匹配的结果会显示给驾驶员，匹配结果也将成为定位和导航的基础。

该方案中GNSS定位精确性较高，但是GNSS接收器要求能够同时看到四颗卫星，才能准确的计算出接收器的估计位置，而在城市环境中，高大的建筑物遮蔽了大部分的天空，构成了密集的“城市峡谷”，这种环境下，由于GNSS信号无法穿透墙壁等障碍物，信号就会因为障碍物的阻挡而受到严重的损害，使GNSS信号的可用性严重下降。在室内停车场环境下，信号衰减的更厉害，无法为室内停车场中的车辆定位提供可靠的位置信息。

而通过车牌识别技术进行车辆定位的方案，需要为停车场内的所有车位都安装摄像头，通过摄像头获取泊位的相关信息，如是否有车辆停泊、停泊车辆的车牌。停泊车辆的车牌信息通关对视频信息进行车牌识别算法获取，将获取的车牌号码输入到数据库，停车场的管理人员可以通过搜索数据库，对目标车辆进行定位，统计空闲泊车位，车辆停靠时间，计费以及车辆导航等。

该方案需要在每个车位上安装获取视频信息的摄像头，这些摄像头的安装导致了停车场需要支付较高的管理成本，且相应的能耗高，体积大，布线复杂等问题也随之而来。从大量的视频信息中获取车辆信息，需要处理大量的数据，设计高效的算法以及使用高性能的计算机等。

发明内容

发明目的：本发明提出了一种基于DR（DeadReckoning，航位推算）和停车场精确地图的自主循环定位方法——DR（DeadReckoning，航位推算）使用车载的传感器，包括加速计、里程计、陀螺仪，坡度检测仪等，进行车辆位置和状态的推断。由于DR具有处处可用，测量误差小，稳定性高的优点，它成为运动车辆定位的首选；本发明使用的停车场地图，由路线和节点组成，路线代表了道路数量和车辆行驶方向等信息，节点表示线路终点的连接点，并且曲线型的线路可以进一步划分为一系列的直线路段，路段除了含有道路数量和行驶方向的信息，还包括了道路宽度以及道路限速的信息。

技术方案：原理为采用道路分段和地图投影算法在候选路段中选择最匹配的路段进行地图匹配，利用误差概率准则计算地图匹配观测噪声，使得沿路段纵向的地图匹配噪声可观测，在此基础上建立卡尔曼滤波方程，将停车场电子地图匹配器输出的匹配位置为X_map、车辆的实时速度V以及偏航速度Ψ输入到卡尔曼滤波器Ⅱ中，根据输入中的车速V和偏航速度Ψ进行DR位置估计得到然后以输入的匹配位置x_map和DR估计位置在东向、北向的位置偏差和航向偏差为观测值，估计车辆的定位误差，提高DR位置估计的精确度。获取的精确地车辆估计位置再次输入到地图匹配器中进行位置匹配，得到新的地图匹配位置，然后重复上述定位过程，直到车辆估计位置定位到地图上的选定路段为止。本发明力求在复杂环境下，如大型交通路口、飞机场、展览中心、大型购物中心等，为驾驶员提供更加精确的室内车辆定位方法。

一种室内停车场精确的组合定位方法，DR即航位推算模块和室内停车场电子地图匹配模块两者组合运用，结合地图匹配器和卡尔曼滤波器，进行DR估计位置和地图匹配位置的循环匹配，从而实现室内停车场环境下对车辆的精确定位；其中：

停车场电子地图模块中，将路线划分为链路和节点，链路中包含了某一段道路的静态信息；节点是链路的终点，也是链路之间的连接点；电子地图中有弧度的链路可以进一步划分为更短的直线路段，每一条直线路段都含有相应的道路信息；例子不要在权利要求中列明，应放在具体实施方式的实施例中。

DR模块中，使用车载的传感器即DR，进行车辆位置和状态的推断；

地图匹配位置以及DR模块获取的车辆实时速度和偏航速度，三者共同输入到卡尔曼滤波器中，计算车辆在东向、北向的位置误差和航向误差，以误差作为观测值，经卡尔曼滤波器计算出更加精确地车辆估计位置，从而提高DR位置估计的准确性；

本发明中的卡尔曼滤波器和地图匹配器构成了一个反馈回路，回路中的正反馈引起了链路亲和（LinkAffinity）的现象：一旦地图匹配器选择了某一路段，接下来循环发生的车辆位置估计和地图匹配过程，将会持续地把车辆估计位置“拉”向选定路段；这种亲和现象逐渐将车辆的估计位置和行驶方向角逼近选定路段，增大选定路段在下一次车辆位置估计的循环中被选中的概率；所以，链路匹配一旦建立后，就会保持稳定，并且位置估计会跟选定的链路保持亲和，这种正反馈循环会一直持续，直到车辆估计位置到达选定链路。

使用车载的传感器，包括加速计、里程计、陀螺仪，坡度检测仪等，进行车辆位置和状态的推断，结合精确地室内停车场电子地图，通过地图匹配器和卡尔曼滤波器循环定位。

DR定位模块采用惯性件和里程传感器。使用加速计、陀螺仪、坡度检测仪实时测量和记录车辆的移动速度和移动方向，使用里程传感器可以实时获取车辆的位移量，通过这些信息可以计算出任意时刻车辆的位置，从而在停车场内对车辆进行DR定位。

有益效果

1）本发明提出了行车路线折线化和节点化的地图结构，将室内停车场的路线划分为不同的链路段，在链路的两端用节点表示，节点又是不同链路的连接点。曲线的链路内部采用直线路段，多个直线路段共同构成曲线链路。

2）本发明采用DR和电子地图组合定位的方法，避免了在停车场内进行车辆定位时，使用额外的硬件或者基础设施，节省开支。

3）采用循环定位方法，将车辆传感器获取的车辆速度、偏航速度和地图匹配位置共同作为卡尔曼滤波器Ⅱ的输入，进行循环定位，直到获取稳定的、准确的车辆估计位置。

4）本发明中的卡尔曼滤波器和地图匹配器构成了一个反馈回路，回路中的正反馈引起了链路亲和（LinkAffinity）的现象：链路匹配一旦建立后，就会保持稳定，并且位置估计会跟选定的链路保持亲和，这种正反馈循环会一直持续，直到车辆估计位置到达选定链路。

附图说明

图1是停车场电子地图结构化和航位推算位置投影的示意图

图2是地图匹配的观测噪声

图3是航位推算和停车场地图组合定位的示意图

具体实施方式：

本发明的车辆定位算法分为两步：第一步、车轮转速传感器获取的车辆重心的速度V通过CAN总线输出；转向角传感器和陀螺仪传感器的数据，经卡尔曼滤波器Ⅰ整合在一起，得到车辆的航向速度Ψ。这一步骤中获取的两个速度共同实现DR（DeadReckoning，航位推算）初始位置估计（即图3中的）。第二步、将停车场内的路线折线化和点化处理，根据第一步得到的DR初始位置估计（即图3中的）以及地图匹配位置X_map经卡尔曼滤波器Ⅱ得到更加精确地车辆估计位置然后，根据上文提到的投影定位算法，将得到的车辆估计位置输入到地图匹配器中，获得地图测定位置X_map。注意到地图匹配器和卡尔曼滤波器Ⅱ构成一个反馈回路，该回路中，卡尔曼滤波器Ⅱ不断的接收实时更新的Ψ，V，和X_map，根据这些参数计算得到地图匹配点（即图3中的）与DR定位点（即图3中的）在东向、北向的位置偏差和航向偏差，以误差作为观测值，估计定位偏差，不断将车辆估计位置进行修正，得到精确地车辆估计位置反馈回路不断的循环，直到车辆的估计位置和选定的路段重合。

系统模型

停车场电子地图的结构和DR估计位置在路段上的投影方案，如图1所示，该图为停车场的地图结构和估计位置投影的示意图，地图由链路和节点组成，链路中包含了某一段道路的静态信息，如车道数量、行驶方向等；节点是链路的终点，也是链路之间的连接点。地图中有弧度的链路可以进一步划分为更短的直线路段，每一条直线路段都含有相应的道路信息，如车道数量，行驶方向，车道宽度以及限速等。

本发明中使用的地图匹配算法，可以划分为两个步骤：1、确定误差区域范围，搜索候选路段，并进行路段选择。2、使用迭代定位的方法将车辆定位到选定路段上的某一点。

在步骤1中，地图匹配首先确定候选道路的搜索范围，选择适当大小的区域很重要，过大的误差区域会增加道路搜索和道路匹配的计算量，而区域过小则可能遗漏正确的道路。一般按照概率准则定义误差区域，即误差区域以一定的概率包括车辆的实际位置。假设定位系统的协方差矩阵定义为

$P=\left[\begin{array}{cc}{\mathrm{\σ}}_e^2& {\mathrm{\σ}}_{\mathrm{en}}\\ {\mathrm{\σ}}_{\mathrm{en}}& {\mathrm{\σ}}_n^2\end{array}\right]$ 式（1）

式中，和分别为传感器在东向和北向的均方差，σ_on为协方差，定义定位误差椭圆（未在图1中画出）为

$\left\{\begin{array}{c}a={\widehat{\mathrm{\σ}}}_0\sqrt{0.5({\mathrm{\σ}}_e^2+{\mathrm{\σ}}_n^2+\sqrt{{({\mathrm{\σ}}_e^2-{\mathrm{\σ}}_n^2)}^2+4{\mathrm{\σ}}_{\mathrm{en}}^2})}\\ b={\widehat{\mathrm{\σ}}}_0\sqrt{0.5({\mathrm{\σ}}_e^2+{\mathrm{\σ}}_n^2-\sqrt{{({\mathrm{\σ}}_e^2-{\mathrm{\σ}}_n^2)}^2+4{\mathrm{\σ}}_{\mathrm{en}}^2})}\\ \mathrm{\Φ}=\frac{1}{2}{\tan }^{-1}\left(\frac{2{\mathrm{\σ}}_{\mathrm{en}}}{{\mathrm{\σ}}_e^2-{\mathrm{\σ}}_n^2}\right)\end{array}\right.$ 式（2）

式中a为椭圆长半轴；b为椭圆短半轴；Φ为椭圆长半轴与正东方向的夹角，椭圆中心是当前传感器测得的车辆估计位置（即图1中的），为单位权值的后验方差，改变的值以调整误差椭圆的大小，可以获得不同的可信度。假设定位误差服从正态分布，可以获得95%的可信度。

定位误差椭圆确定后，搜索数据库，查找地图中落在误差椭圆范围内的路段，将其作为候选路段，然后从候选路段中匹配车辆的当前行驶路段。道路匹配会参考下面几个方面，为候选路段分配概率因子p作为车辆定位到该链路的度量值。

●距离：车辆估计位置和候选链路最近路段之间的距离d。

●角度：车辆行驶方向和路段方向之间的角度差值。

●路段连通性：后继路段与先前匹配路段之间的连通性

●其他：其他有关路段选择的因素，如行驶方向和速度。

概率因子p的计算方法是对上述各个参考方面进行加权求和。p的值越大，说明该候选路线与车辆行驶路线的相关性越高。所有的候选路段中，具有最大p的路段会被选择作为估计位置的投影路段。如果定位误差椭圆区域内没有搜索到任何路段，则认为车辆没有行驶在道路中。此时，取初始的车辆估计位置（即图1中的）作为车辆位置。

第二步，确定车辆在路段中的位置，本发明采用的方法是将车辆估计位置在选定路段上面的投影位置作为车辆在路段中的位置。具体的投影算法如下：

如图1所示，若选定路段两端点的坐标分别为A(x₁，，B（x₂，，地图匹配初始确定的车辆估计位置则在选定路段AB上的投影点的坐标为x_map(e₂，，计算公式如下：

$e_2=\frac{(x_2-x_1)[e_s(x_2-x_1)+n_s(y_2-y_1)]+(y_2-y_1)(x_1{y}_2-x_2{y}_1)}{{(x_2-x_1)}^2+{(y_2-y_1)}^2}$ 式（3）

$n_2=\frac{(y_2-y_1)[e_s(x_2-x_1)+n_s(y_2-y_1)]-(x_2-x_1)(x_1{y}_2-x_2{y}_1)}{{(x_2-x_1)}^2+{(y_2-y_1)}^2}$

本发明的地图匹配噪声算法

地图匹配过程中，为了将DR估计的车辆位置准确的匹配到选定路段，需要考虑匹配噪声。理论上，垂直于道路方向的定位误差是可观的，沿道路纵向误差不可观，采用误差概率准则可改变其可观性。图2为地图匹配的观测噪声。观测噪声σ_xm和σ_ym爪用道路局部坐标系x_mo_my_m描述，假设观测噪声服从零均值高斯白噪声分布，阴影椭圆为观测噪声椭圆。noe为局部南北坐标系，P及P′在noe中的坐标分别为(P_e，P_n)和(P′_e，P′_n)。道路直线长度L大于设定阈值L_th时，σ_xm取一个相对大值，否则将由路段长度和地图精度确定，即

式（4）

横向观测噪声σ_ym爪需要考虑实际的道路宽度以及地图精度，即

${\mathrm{\σ}}_{\mathrm{ym}}=\frac{W}{2k_p}+{\mathrm{\σ}}_m$ 式（5）

式（6）中，W为道路宽度，k_p与误差概率有关，取95%的可信度时，k_p=2.15；σ_m为电子地图误差，城市道路中取3m。

发明中卡尔曼滤波器设计

取状态为DR，定位在东向、北向的位置误差和航向误差为

X＝{λ_e，λ_n，λ_θ}^T式（6）

误差用一阶马尔科夫过程描述为

$\left\{\begin{array}{c}{\stackrel{\·}{\mathrm{\λ}}}_e=-{\mathrm{\τ}}_{{\mathrm{\λ}}_e}{\mathrm{\λ}}_e+{\mathrm{\ω}}_{{\mathrm{\λ}}_e}\\ {\stackrel{\·}{\mathrm{\λ}}}_n=-{\mathrm{\τ}}_{{\mathrm{\λ}}_n}{\mathrm{\λ}}_n+{\mathrm{\ω}}_{{\mathrm{\λ}}_n}\\ {\stackrel{\·}{\mathrm{\λ}}}_{\mathrm{\θ}}=-{\mathrm{\τ}}_{{\mathrm{\λ}}_{\mathrm{\θ}}}{\mathrm{\λ}}_{\mathrm{\θ}}+{\mathrm{\ω}}_{{\mathrm{\λ}}_{\mathrm{\θ}}}\end{array}\right.$ 式（7）

式中分别为噪声过程时间常数的倒数，为零均值的白噪声，它们的均方根分别为

离散状态方程为

$\left\{\begin{array}{c}X(k+1)=\mathrm{\ΦX}\left(k\right)+W_X\left(k\right)\\ \mathrm{\Φ}=\mathrm{diag}(e^{{-\mathrm{\τ}}_{{{\mathrm{\λ}}_e}^T}},e^{-{\mathrm{\τ}}_{{{\mathrm{\λ}}_n}^T}},e^{-{\mathrm{\τ}}_{{{\mathrm{\λ}}_{\mathrm{\θ}}}^T}})\end{array}\right.$ 式（8）

式中，T为采样周期，W_H(k)为白噪声，假设各状态噪声互不相关，得到状态协方差阵为

Q＝diag(q₁₁，q₂₂，q₃₃)式（7）

式中：

$q_{11}={\mathrm{\σ}}_{{\mathrm{\λ}}_e}^2[1-\exp (-2{\mathrm{\τ}}_{{\mathrm{\λ}}_e}T)]$

$q_{22}={\mathrm{\σ}}_{{\mathrm{\λ}}_n}^2[1-\exp (-2{\mathrm{\τ}}_{{\mathrm{\λ}}_n}T)]$

$q_{33}={\mathrm{\σ}}_{{\mathrm{\λ}}_{\mathrm{\θ}}}^2[1-\exp (-2{\mathrm{\τ}}_{{\mathrm{\λ}}_{\mathrm{\θ}}}T)]$

以地图匹配点P′(即图1中的X_map）与DR定位点P（即图1中的）在东向、北向的位置偏差和航向偏差为观测值，即

$Z=\left\{\begin{array}{c}{Z}_1\\ Z_2\\ Z_3\end{array}\right\}=\left\{\begin{array}{cc}{P}_e^{\′}& -P_e\\ P_n^{\′}& -P_n\\ {\mathrm{\θ}}^{\′}& -\mathrm{\θ}\end{array}\right\}$ 式（8）

离散观测方程为

Z(k)＝HX(k)+W_Z(k)式（9）

其中H＝diag(1，1，1)，W_Z(k)为观测噪声。

假设电子地图上位置噪声与航向噪声不相关，观测噪声协方差阵R为

$R=\left[\begin{array}{ccc}{\mathrm{\σ}}_{\mathrm{em}}^2& {\mathrm{\σ}}_{\mathrm{enm}}^2& 0\\ {\mathrm{\σ}}_{\mathrm{enm}}^2& {\mathrm{\σ}}_{\mathrm{en}}^2& 0\\ 0& 0& {\mathrm{\σ}}_{\mathrm{\θ}}^2\end{array}\right]$ 式（10）

式中

${\mathrm{\σ}}_{\mathrm{em}}^2={\mathrm{\σ}}_{\mathrm{xm}}^2{\left(\cos \mathrm{\α}\right)}^2+{\mathrm{\σ}}_{\mathrm{ym}}^2{\left(\sin \mathrm{\α}\right)}^2$

${\mathrm{\σ}}_{\mathrm{nm}}^2={\mathrm{\σ}}_{\mathrm{ym}}^2{\left(\cos \mathrm{\α}\right)}^2+{\mathrm{\σ}}_{\mathrm{xm}}^2{\left(\sin \mathrm{\α}\right)}^2$

${\mathrm{\σ}}_{\mathrm{enm}}^2=({\mathrm{\σ}}_{\mathrm{xm}}^2+{\mathrm{\σ}}_{\mathrm{ym}}^2)\cos \mathrm{\α}\sin \mathrm{\α}$

其中，为道路与东向的夹角。

DR/MM组合定位

DR（DeadReckon，航位推算）主要有定位传感器（陀螺仪、里程计、加速度计等）组成，通过测量和记录任意时刻的车辆的位移量和移动方向，来计算任意时刻车辆的位置。在短时间内能保持较高的精度，且其有效性不受外界影响，但是该方法仅能确定相对位置，且存在距离累积误差。GNSS信号容易受卫星的几何分布和建筑物遮挡的影响，经常发生中断，在室内停车场环境下，GNSS信号受到严重的削弱，无法对车辆的绝对位置进行定位。所以，在GNSS信号不可达的室内停车场环境中，使用电子地图校正DR对车辆位置估计的误差具有更大的现实意义。本发明采用DR/MM组合定位的方法，DR对车辆的相对位置和速度进行估计，结合精确的地图匹配算法，将车辆的估计位置映射到正确的路段上，从而在室内停车场中准确的确定车辆的位置。

从图3中可以看出，本发明使用的滤波器具有级联结构，DR（DeadReckoning，航位推算）需要的两个信号分别是车辆速度V和偏航速度Ψ。一方面，车辆速度V主要是通过车辆CAN总线获得，该速度信号是基于车辆前轮的角速度，能够准确地反映车辆重心的速度，该速度信号已经具有较高的精度，且其有效性不受外界影响；另一方面，偏航速度Ψ是从卡尔曼滤波器Ⅰ中获得的，该滤波器采用单车道模型，以汽车的转向角作为输入信号，结合陀螺仪的感知数据，可以输出准确的车辆偏航速度Ψ。卡尔曼滤波器Ⅰ所采用的单车道模型包含了车辆基本的几何参数和运动参数，能够充分描述车辆的动态，所以被广泛用于简化车辆模型。在车辆时速小于40km/h的情况下，单车道模型精确，即单车道模型适合于对停车场的车辆运动情况进行分析。对于陀螺仪传感器存在漂移现象，仍然会引起航向测量的误差，所以需要另外使用卡尔曼滤波器来测量陀螺仪在车辆静止时的偏移情况，来消除陀螺仪偏移量。这样，卡尔曼滤波器Ⅰ输出的结果就是平滑、准确的偏航速度Ψ。然而，仅仅使用DR估计的方法只能短时间内确定车辆的相对位置，并且这个位置估计存在累计误差，所以我们需要利用精确的停车场地图进行匹配，来弥补DR的不足。图3中，卡尔曼滤波器Ⅱ采用2.2.3节中的设计方法，将偏航速度Ψ，车辆速度V，地图匹配器输出的地图匹配位置：作为输入，计算得到地图匹配点（即图3中的：）与DR初始定位点（即图3中的）在东向、北向的位置偏差和航向偏差，以误差作为观测值，得到更加精确地车辆估计位置，提高DR位置估计的准确性。注意到，卡尔曼滤波器Ⅱ和地图匹配器构成了一个反馈回路，回路中的正反馈引起了链路亲和（LinkAffinity）的现象：一旦地图匹配器选择具有最高概率因子p的路段，接下来循环发生的车辆位置估计和地图匹配过程，将会持续地把车辆估计位置“拉”向选定路段。这种亲和现象逐渐将车辆的估计位置和行驶方向角逼近选定路段，增大选定路段在下一次车辆位置估计的循环中被选中的概率。所以，链路匹配一旦建立后，就会保持稳定，并且位置估计会跟选定的链路保持亲和，这种正反馈循环会一直持续，直到车辆估计位置到达选定链路。链路亲和是一个令人感到满意的现象，他防止了模糊情况下地图匹配器在链路之间的跳越匹配。

Claims (3)

1.一种室内停车场精确的组合定位方法，其特征在于，DR即航位推算模块和室内停车场电子地图匹配模块两者组合运用，结合地图匹配器和卡尔曼滤波器，进行DR估计位置和地图匹配位置的循环匹配，从而实现室内停车场环境下对车辆的精确定位；其中：

停车场电子地图模块中，将路线划分为链路和节点，链路中包含了某一段道路的静态信息；节点是链路的终点，也是链路之间的连接点；电子地图中有弧度的链路可以进一步划分为更短的直线路段，每一条直线路段都含有相应的道路信息，在进行地图匹配的时候，采用投影算法：

若选定路段两端点的坐标分别为A(x1，y1)，B(x2，y2)，地图匹配初始确定的车辆估计位置X(es，ns)，则在选定路段AB上的投影点的坐标为Xmap(e2，n2)，计算公式如下：

式(1)

DR模块中，使用车载的传感器即DR，进行车辆位置和状态的推断；

地图匹配位置以及DR模块获取的车辆实时速度和偏航速度，三者共同输入到卡尔曼滤波器中，计算车辆在东向、北向的位置误差和航向误差，

X＝{λ_e，λ_n，λ_θ}T式(6)

以误差作为观测值

式(10)

式中，地图匹配点P′(即图1中的X_map)与DR定位点P(即图1中的)

离散观测方程为

Z(k)＝HX(k)+W_z(k)式(21)

其中H＝diag(1，1，1)，W_z(k)为观测噪声；

假设电子地图上位置噪声与航向噪声不相关，观测噪声协方差阵R为

式(32)

式中

其中，为道路与东向的夹角；

卡尔曼滤波器结合观测噪声，计算出更加精确地车辆估计位置，从而提高DR位置估计的准确性；

所述方法分为两步：第一步，车轮转速传感器获取的车辆重心的速度V通过CAN总线输出；转向角传感器和陀螺仪传感器的数据，经卡尔曼滤波器Ⅰ整合在一起，得到车辆的航向速度Ψ，所述步骤中获取的两个速度共同实现DR(DeadReckoning，航位推算)初始位置估计；第二步，将停车场内的路线折线化和点化处理，根据第一步得到的DR初始位置估计以及地图匹配位置X_map经卡尔曼滤波器Ⅱ得到更加精确地车辆估计位置然后，根据投影定位算法，将得到的车辆估计位置输入到地图匹，其中，获得地图测定位置X_map，注意到地图匹配器和卡尔曼滤波器Ⅱ构成一个反馈回路，所述回路中，卡尔曼滤波器Ⅱ不断的接收实时更新的Ψ，V，和X_map，根据这些参数计算得到地图匹配点，与DR定位点在东向、北向的位置偏差和航向偏差，以误差作为观测值，估计定位偏差，不断将车辆估计位置进行修正，得到精确地车辆估计位置反馈回路不断的循环，直到车辆的估计位置和选定的路段重合。

2.根据权利要求1所述的室内停车场精确的组合定位方法，其特征在于，使用车载的传感器，包括加速计、里程计、陀螺仪，坡度检测仪，进行车辆位置和状态的推断，结合精确地室内停车场电子地图，通过地图匹配器和卡尔曼滤波器循环定位。

3.根据权利要求1所述的室内停车场精确的组合定位方法，其特征在于，DR定位模块采用惯性件和里程传感器。