CN105180955A - 机动车实时精准定位方法及装置 - Google Patents

Abstract

一种机动车实时精准定位方法及装置，其中方法包括如下步骤，机动位移更新、测量更新；其中机动位移更新包括如下步骤：以起始速度作为初始条件，对惯性加速度对于时间进行积分，得到系统惯性速率；以起始位置坐标作为初始条件积分得到惯性位置；重复上述步骤，得到连续的导航坐标系统；消除所述导航坐标系统与车辆坐标系统的偏差，得到后验概率；测量更新包括如下步骤：对环境进行激光扫描，得到反射数据，根据后验概率、反射数据和地图数据信息计算后验概率分布。达到了机动车定位精度高，更新快，抗干扰能力强的技术效果。

Description

机动车实时精准定位方法及装置

技术领域

本发明涉及机动车技术领域，尤其涉及一种基于概率地图的机动车实时精准定位方法及装置。

背景技术

机动车在高度复杂的城市环境中精确地确定位置是实现机动车自动驾驶的一个极其重要的环节，特别是对于主要依赖预先采集的电子地图实现环境感知的自动驾驶系统。

现有技术中实现定位的方法有很多种，比较常见有：

1.完全依靠车载传感器实现定位的：

a.基于全球定位系统，或者基于全球定位/惯性导航融合系统(GPS/INS)的。

b.基于其它传感器的，如立体摄像机、激光雷达的。

2.依靠车辆和行驶环境中的信息化网络实现双向信息交流的。

行驶环境也极大地影响定位所采取的方法，因为在各种情形下的定位的难度并不尽相同。显然，在动态的环境(即不断改变的环境，如繁忙城市的道路)中进行定位就要比静态的环境(如越野行驶时，没有什么其它车辆的环境)中进行定位要更困难，因为要考虑更多的动态障碍引起的不确定性。

常见的定位方法各有千秋。然而，要实现稳定可靠的厘米级的定位精度，难度依然很大。而这对于城市中实现自动驾驶又是不可或缺的。几十厘米的偏差往往决定了在拥挤的城市道路上，机动车是否会压线行驶、开上人行道等。

发明内容

为此，需要提供一种能够提供更高的定位精度(若传感器如GPS/INS和激光雷达能达到一定的精度，则能达到10厘米级)，能够学习环境，并随着时间推移提供定位的精度，能对环境的变化，特别是动态障碍物的变化有很好的抗干扰性的机动车定位方法及装置。

为实现上述目的，发明人提供一种机动车实时精准定位方法，包括如下步骤，机动位移更新步骤、测量更新步骤；

其中机动位移更新步骤包括如下步骤：以机动车起始速度作为初始条件，将机动车惯性加速度对于时间进行积分，得到系统惯性速率；以机动车起始位置坐标作为初始条件积分得到系统惯性位置；重复上述步骤，根据系统惯性速率和系统惯性位置得到连续的导航坐标系统；消除所述导航坐标系统与车辆坐标系统的偏差，得到后验概率；

测量更新包括如下步骤：对环境进行激光扫描，得到反射数据，根据后验概率、反射数据和地图数据信息计算后验概率分布，根据后验概率分布判断得到机动车位置。

进一步地，测量更新后还包括步骤，对后验概率分布的结果进行最大似然估计，选择最优化的坐标表示机动车坐标的估计值。

具体地，通过下列公式选择最优化的坐标表示估计值：

$\begin{array}{cc}x=\frac{{\Σ}_{x,y}P{(x,y)}^{\mathrm{\α}}\·x}{{\Σ}_{x,y}P{(x,y)}^{\mathrm{\α}}}& y=\frac{{\Σ}_{x,y}P{(x,y)}^{\mathrm{\α}}\·y}{{\Σ}_{x,y}P{(x,y)}^{\mathrm{\α}}}\end{array}$

其中P为后验概率分布，x，y为GPS方位坐标。

具体地，所述得到后验概率的公式为：

$\stackrel{\‾}{P}(x,y)=\mathrm{\η}\·\underset{i,j}{\Σ}P(i,j)\·\exp \left(-\frac{1}{2}{(i-x)}^2{(j-y)}^2/{\mathrm{\σ}}^2\right)$

其中代表在机动位移更新后车辆位置在(x,y)的后验概率，η代表了归一化常数，x，y为GPS方位坐标，i，j为单元格坐标。

具体地，所述根据后验概率、反射数据和地图数据信息计算后验概率分布具体为：

P(x,y|z,m)＝η·P(z|x,y,m)·P(x,y)

其中P(x,y|z,m)为后验概率分布，z为反射数据，m为地图数据信息，x，y是GPS方位坐标，η代表归一化常数。

一种机动车实时精准定位装置，包括机动位移更新模块、测量更新模块；

其中机动位移更新模块用于以起始速度作为初始条件，对惯性加速度对于时间进行积分，得到系统惯性速率；还用于以起始位置坐标作为初始条件积分得到惯性位置；还用于重复上述步骤，得到连续的导航坐标系统；消除所述导航坐标系统与车辆坐标系统的偏差，得到后验概率；

测量更新模块用于对环境进行激光扫描，得到反射数据；还用于根据后验概率、反射数据和地图数据信息计算后验概率分布，根据后验概率分布判断得到机动车位置。

进一步地，还包括最大似然估计模块，所述最大似然估计模块用于对后验概率分布的结果进行最大似然估计，选择最优化的坐标表示机动车坐标的估计值。

具体地，所述最大似然估计模块通过下列公式选择最优化的坐标表示估计值：

$\begin{array}{cc}x=\frac{{\Σ}_{x,y}P{(x,y)}^{\mathrm{\α}}\·x}{{\Σ}_{x,y}P{(x,y)}^{\mathrm{\α}}}& y=\frac{{\Σ}_{x,y}P{(x,y)}^{\mathrm{\α}}\·y}{{\Σ}_{x,y}P{(x,y)}^{\mathrm{\α}}}\end{array}$

其中P为后验概率分布，x，y为GPS方位坐标。

具体地，所述机动位移模块用于得到后验概率的公式为：

$\stackrel{\‾}{P}(x,y)=\mathrm{\η}\·\underset{i,j}{\Σ}P(i,j)\·\exp \left(-\frac{1}{2}{(i-x)}^2{(j-y)}^2/{\mathrm{\σ}}^2\right)$

其中代表在机动位移更新后车辆位置在(x,y)的后验概率，η代表了归一化常数，x，y为GPS方位坐标，i，j为单元格坐标。

具体地，所述测量更新模块用于根据后验概率、反射数据和地图数据信息计算后验概率分布具体为：

P(x,y|z,m)＝η·P(z|x,y,m)·P(x,y)

其中P(x,y|z,m)为后验概率分布，z为反射数据，m为地图数据信息，x，y是GPS方位坐标，η代表归一化常数。

区别于现有技术，上述技术方案采用贝叶斯推理的方法，随着机动车的移动不断更新状态变量的分布，达到了精度高，更新快，抗干扰能力强的技术效果。

附图说明

图1为本发明具体实施方式所述的机动车实时精准定位方法流程示意图；

图2为本发明具体实施方式所述的一维条件下机动车移动与位置概率分布估计示意图；

图3为本发明具体实施方式所述的机动车测量更新效果示意图；

图4为本发明具体实施方式所述的机动车实时精准定位装置模块示意图。

附图标记说明：

400、机动位移更新模块；

402、测量更新模块；

404、最大似然估计模块。

具体实施方式

为详细说明技术方案的技术内容、构造特征、所实现目的及效果，以下结合具体实施例并配合附图详予说明。

请参阅图1，为本发明所述的一种机动车实时精准定位方法流程示意图，包括如下步骤，S1机动位移更新、S2测量更新；

其中机动位移更新包括如下步骤：S101以起始速度作为初始条件，对惯性加速度对于时间进行积分，得到系统惯性速率；S103以起始位置坐标作为初始条件积分得到惯性位置；重复上述步骤S101、S103，得到连续的导航坐标系统；S105消除所述导航坐标系统与车辆坐标系统的偏差，得到后验概率；测量更新S2包括如下步骤：S201对环境进行激光扫描，得到反射数据；S203根据后验概率、反射数据和地图数据信息计算后验概率分布，S205根据后验概率分布判断得到机动车位置。通过上述方法，达到了机动车定位精度高，更新快，抗干扰能力强的技术效果。

在某些实施例中，实验车辆载有激光雷达，GPS/INS传感器。GPS/INS能够以很高的频率(例如，高于100赫兹)输出全球定位坐标、当前角速度与加速度(以下二者简称惯性加速度)。激光雷达经过校准。且所述地图数据信息为预先采集、预处理并且存储于车载计算机中的目标行驶环境的基于概率的地图。

基于上述假设，可以构件如下概率模型：

·机动车在某一时刻的状态由一个随机变量x_t表示，即状态变量。状态变量可以包括位置坐标、欧拉角(Eulerangles,包括pitch,roll,yaw)等。

·状态变量的概率分布，代表了机动车对于自身位置的估计。

·采用贝叶斯推理的办法，随着机动车的移动，不断更新状态变量的概率分布：

ο每一次新的观测(比如激光雷达一次360度完整的扫描)，机动车都会结合预先采集的电子地图改变对于自身位置的估计：

p(x|z,m)

这里z代表观测数据，m是电子地图数据。预先采集的电子地图是个二维的、网格化的地图。网格中的每一个单元都存储了环境中对应的表面对于激光雷达照射的反射信息。这个信息可以是一个向量如反射强度的绝对值，也可以具有更复杂的结构：比如用一个概率分布(比如高斯分布)记录每个单元激光强度的分布。

ο每一个新的机动动作，也会改变对于自身位置的估计：p(x|u)

这里u代表由机动车传感器(如惯性导航仪)测出的位置的变化。下面用一个实施例演示随着机动车的移动状态变量的概率分布的变化：

这里请看图2所示的实施例中，小机器人代表机动车。机动车只在一维的方向(x轴)上移动。bel(x)即机动车对于自身位置的概率分布的估计值，也可以表示为p(x|z,m,u)，假定一开始，如图2a所示机动车并没有任何自己位置的信息，因此bel(x)对于所有x的值都相同。

接下来请看图2b，机动车往前移动到第一扇门前，传感器观察到一扇门。由于事先知道地图上有三扇门以及门的位置，所以对于自身位置的估计bel(x)变为图中所示。现在机动车对于自身位置的估计一下子缩小到几个位置：地图上所有有门的地方。再接下来，机动车继续往前机动一段距离，当到了图2c所示的位置时这段机动的结果就是机动车对于自身位置分布的估计也前移了。值得注意的是，对于每一个位置的信息却有所下降，这反映了机动本身具有不确定性(比如惯性导航仪的误差)。再接下来请看图2d，机动车传感器又侦测到一扇门。结合机动位移的信息，机动车对于自身位置的分布一下子有了很强的偏向(图下最高的波峰)。这是因为，在地图上移动一段特定的距离后又看到一扇门，当前的位置很可能就是在第二扇门前。

当理解了基本构思假设后，可以理解本发明的基本步骤即对于状态变量的过滤(即消除噪信号推断真实的概率的过程)分为两步：机动位移更新。这一步根据机动位移传感器测量的位移值，降低对于自身位置估值的信心，以反映仪器中和机动车运动存在的误差。测量更新(图三)。这一步通过匹配观测的结果和电子地图，增加对于自身位置估值的信心。在图3所示的某些实施例中，激光雷达的测量用明暗相间的线条表示。可以在图3a中看出，车辆前方两个方向箭头，测量的箭头和地图上的箭头的偏差。在图3b中经过测量更新，两个箭头基本重合。

在另一些实施例中，虽然位置坐标x,y为置为连续的，但因为传感器、机动车控制器本身的误差，同时为了提高计算的效率，本方法采用直方图将连续x,y分割个一个一个连续的区间。每个区间有一个固定的值，为原来连续值的平均值。使用这种方法的过滤器以下简称直方图过滤器。定位中常用的另一种过滤器是粒子过滤器。但GPS可以让我们把搜索范围限制在估计点附近一定的范围(米)内，并可以直接计算在10＊10单元格内所有坐标(x，y)的概率。在这种情况下将x，y坐标表示成直方图过滤器的好处在于就算在实际位置周围的粒子有缺失，也不会影响我们对x，y坐标进行有效的表达。

机动位移更新

要准确地做出机动位移更新首先要建立一个尽可能接近现实的运动模型。使用传感器的数据我们可以得到一个连续的导航座标系统，具体的步骤如下：

1)以起始速度作为初始条件，运用正确的运动学方程，对惯性加速度相对于时间进行积分，就可得到系统惯性速率。

2)然后以起始位置座标作为初始条件再次积分就可得到惯性位置。

3)每得到新的数据就重复以上两步，通过不断对新的惯性加速度数据的积分，得到一个连续的导航坐标系统。

现在我们通过对GPS和惯性加速度数据积分得到的这个导航座标系统，是与车辆自身位置的坐标系统有偏差的。如果能够消除这个导航坐标系统与车辆自身位置坐标系统之间的偏差，我们就能得到更为精准的全球定位。现在我们的重点就在于，如何去模拟估算这个偏差。由于惯性位置是由对速率进行积分得到的，我们可以用期望值为0的高斯噪声下的随机游动去精确地模拟惯性位置相对于实际位置的偏移量。在这个模型下，我们用以下公式对每个单元格的概率进行更新：

$\stackrel{\‾}{P}(x,y)=\mathrm{\η}\·\underset{i,j}{\Σ}P(i,j)\·\exp \left(-\frac{1}{2}{(i-x)}^2{(j-y)}^2/{\mathrm{\σ}}^2\right)$

其中代表了在移动更新后车辆位置在(x,y)的后验概率，η代表了归一化常数(归一化常数是对于任何非负函数的任意区间所含有之常数使得该函数对于一特定区间之积分恰好等于1)。σ这个参数则是用来描述偏移量的比率，由于更新的频率很高，这个比率通常很小。虽然理论上来说，这个方程是关于单元数量的二次方程，也就是关于搜索半径的四次方程，运算量并不算大。但在实际运用上，我们完全可以只搜索距离(x，y)两或三个单位内的单元格来进行优化。这是由于我们可以以任意高的频率进行更新，而且偏移辆的比率相对很小，所以在很短的时间内，车辆传感器的偏移是非常有限的。从统计学的角度解释，偏移量在一定范围内的概率是极高的，具体范围多大就看对这个概率的具体要求了。举个例子来说，惯性导航坐标系在0.1秒内产生大于45cm偏移量的概率是趋近于0的(尽管对GPS来说这样一个跳移比较正常)。即使出现大偏移事件，很快下一次的更新也会把它纠正回来。由于这个偏移量的期待值与车辆速度是大致成正比的，所以我们也以一个与车辆速度成正比的比率去进行移动更新。车辆移动速度缓慢时我们就可以相应减小更新的频率

测量更新

直方图过滤器的第二部分就是测量更新，可以通过不断对环境进行新的激光扫描然后用新得到的反射数据不断对车辆的方位估计进行更新。随着被扫描环境的测量数据不断增多，我们对车辆方位的估计也会越来越精准。这其实就体现了一种对环境的学习和适应能力。

为了处理不断更新的测量数据，我们根据将累积的传感器数据网格化，建立一个对应的滚动网格。每得到一个新的激光反射数据，我们要对它进行及时的处理，把它编码进这个既存的滚动网络中，更新相应的单元格。经过这样的处理我们可以直接把传感器数据的单元格与地图上的单元格进行比对，而不会对那些接到很多反应数据的单元格，比如说树或者比较大的动态障碍对应的单元格，过分加权。

在某些实施例中，如果z是我们的传感器数据，m是地图，x和y是可能的GPS方位坐标，那么利用贝叶斯准则：

P(x,y|z,m)＝η·P(z|x,y,m)·P(x,y)

我们可以用方差为的高斯分布去模拟GPS/IMU方位估计的不准确性，所以P(x,y)其实就是GPS高斯与后验概率的乘积

$P(x,y)=\mathrm{\η}\·\exp \left(\frac{x^2+y^2}{-2{\mathrm{\σ}}_{GPS}^2}\right)\·\stackrel{\‾}{P}(x,y)---\left(1\right)$

接下来我们需要计算P(z|x,y,m)，也就是在已知(x,y)和m的情况下，传感器数据等于z的概率。

$P\left(z\right|x,y,m)={\Π}_{i,j}\exp {\left(\frac{-{(m_{r(i-x,j-y)}-z_{r(i,j)})}^2}{2{\left(m_{{\mathrm{\σ}}_{\left(i-x,j-y\right)}+z_{{\mathrm{\σ}}_{\left(i,j\right)}}}\right)}^2}\right)}^{\mathrm{\α}}---\left(2\right)$

在上面这个公式里，m_σ和z_σ是两个二维的数组，分别代表了地图单元和传感器数据的强度值标准差。举个例子来说,在地图m上，表示的是以车辆的全球估计位置为原点，往东0.45米和往北1.2米的单元格的强度值标准差。同样，我们也用m_r和z_r来分别代表地图单元和传感器数据的平均(反射)强度。

举同样的例子来说，表示的是在地图m上，以车辆的全球估计位置为原点，往东0.45米和往北1.2米的单元格的平均(反射)强度。

注意，我们之所以要将这个乘积升到α(α>1)次幂，是因为考虑到由于可能的系统校准错误或者环境的细微变化，数据之间不一定是完全独立的。

现在我们有了(1)和(2)，便可以得到最终的后验概率分布：

$P\left(x,y|z,m\right)=\mathrm{\η}\·\underset{i,j}{\Π}\exp {\left(\frac{-{(m_{r(i-x,j-y)}-z_{r(i,j)})}^2}{2{\left(m_{{\mathrm{\σ}}_{\left(i-x,j-y\right)}+z_{{\mathrm{\σ}}_{\left(i,j\right)}}}\right)}^2}\right)}^{\mathrm{\α}}\·\exp \left(\frac{x^2+y^2}{-2{\mathrm{\σ}}_{GOS}^2}\right)\·\stackrel{\‾}{P}\left(x,y\right)$

根据后验概率分布，能够使得P(x,y|z,m)取最大值的x，y即判断为机动车位置的坐标。尽管传感器数据和地图单元的方差都很小，我们还是可以进一步的去最小化传感器数据和地图单元的合并标准差，以求得更强大的稳定性。另外，为了方便计算，我们可以只使用车辆的全球估计位置附近几米内的单元格去计算P(x,y|z,m)。这个搜索范围可以根据GPS系统的精确度灵活变动，也就是说，GPS系统越精确，我们的搜索范围就可以定的越小。反之如果GPS系统的定位不够精确，我们就可以将搜索范围扩大。

最大似然估计

在某些进一步的实施例中，测量更新后还包括步骤S3最大似然估计，包括步骤S301对后验概率分布的结果进行最大似然估计，选择最优化的坐标表示机动车坐标的估计值。具体地，有了最终的后验分布后，最后一个步骤就是选择一个从可能性上最优化的(x,y)来表示我们的估计值，也就是最终被输出到车辆导航计划模块的坐标点。这里并非直接取max_x,yP(x,y)就可以，因为作为一个多峰分布的最大值，max_x,yP(x,y)很容易会在各峰值间跳来跳去，导致我们给车辆导航计划模块反馈的坐标点也会跳来跳去。这显然不利于实现精准的车辆导航。

为了既可以从概率上优化坐标估计值又可以保证更好的连续性，我们通过以下公式进行选择：

$\begin{array}{cc}x=\frac{{\Σ}_{x,y}P{(x,y)}^{\mathrm{\α}}\·x}{{\Σ}_{x,y}P{(x,y)}^{\mathrm{\α}}}& y=\frac{{\Σ}_{x,y}P{(x,y)}^{\mathrm{\α}}\·y}{{\Σ}_{x,y}P{(x,y)}^{\mathrm{\α}}}\end{array}$

上面两个公式，从定义上来说，就是把后验分布升到了α(α>1)次幂然后取其质点。这里升幂也是由于P(x,y)不一定是完全互相独立的。

这个坐标点估计值(x，y)会以10赫兹的频率被计算并输出到车辆导航计划模块。这样车辆就可以利用这个最优化的方位估计值在全球坐标系统中来做路径规划了。通过最大似然估计步骤，保证了坐标优化后的连续性，使得本方法具有更高的实用价值。

图4为一种机动车实时精准定位装置模块示意图，所述装置包括机动位移更新模块400、测量更新模块402；

其中机动位移更新模块400用于以起始速度作为初始条件，对惯性加速度对于时间进行积分，得到系统惯性速率；还用于以起始位置坐标作为初始条件积分得到惯性位置；还用于重复上述步骤，得到连续的导航坐标系统；消除所述导航坐标系统与车辆坐标系统的偏差，得到后验概率；

测量更新模块402用于对环境进行激光扫描，得到反射数据；还用于根据后验概率、反射数据和地图数据信息计算后验概率分布，根据后验概率分布判断得到机动车位置。通过上述模块设计，达到了机动车定位精度高，更新快，抗干扰能力强的技术效果。

在某些进一步地实施例中，还包括最大似然估计模块404，所述最大似然估计模块用于选择最优化的坐标表示估计值。

具体地，所述最大似然估计模块通过下列公式选择最优化的坐标表示估计值：

$\begin{array}{cc}x=\frac{{\Σ}_{x,y}P{(x,y)}^{\mathrm{\α}}\·x}{{\Σ}_{x,y}P{(x,y)}^{\mathrm{\α}}}& y=\frac{{\Σ}_{x,y}P{(x,y)}^{\mathrm{\α}}\·y}{{\Σ}_{x,y}P{(x,y)}^{\mathrm{\α}}}\end{array}$

其中P为后验概率分布，x，y为GPS方位坐标。最大似然估计模块保证了坐标优化后的连续性，使得本方法具有更高的实用价值。

具体地，所述机动位移模块用于重复上述步骤，得到连续的导航坐标系统；消除所述导航坐标系统与车辆坐标系统的偏差，得到后验概率的公式为：

$\stackrel{\‾}{P}(x,y)=\mathrm{\η}\·\underset{i,j}{\Σ}P(i,j)\·\exp \left(-\frac{1}{2}{(i-x)}^2{(j-y)}^2/{\mathrm{\σ}}^2\right)$

其中代表在机动位移更新后车辆位置在(x,y)的后验概率，η代表了归一化常数，x，y为GPS方位坐标，i，j为单元格坐标。更好地达到了机动车定位精度高，更新快，抗干扰能力强的技术效果。

具体地，所述测量更新模块用于根据后验概率、反射数据和地图数据信息计算后验概率分布具体为：

P(x,y|z,m)＝η·P(z|x,y,m)·P(x,y)

其中P(x,y|z,m)为后验概率分布，z为反射数据，m为地图数据信息，x，y是GPS方位坐标，η代表归一化常数。更好地达到了机动车定位精度高，更新快，抗干扰能力强的技术效果。

需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者终端设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者终端设备所固有的要素。在没有更多限制的情况下，由语句“包括……”或“包含……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者终端设备中还存在另外的要素。此外，在本文中，“大于”、“小于”、“超过”等理解为不包括本数；“以上”、“以下”、“以内”等理解为包括本数。

本领域内的技术人员应明白，上述各实施例可提供为方法、装置、或计算机程序产品。这些实施例可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。上述各实施例涉及的方法中的全部或部分步骤可以通过程序来指令相关的硬件来完成，所述的程序可以存储于计算机设备可读取的存储介质中，用于执行上述各实施例方法所述的全部或部分步骤。所述计算机设备，包括但不限于：个人计算机、服务器、通用计算机、专用计算机、网络设备、嵌入式设备、可编程设备、智能移动终端、智能家居设备、穿戴式智能设备、车载智能设备等；所述的存储介质，包括但不限于：RAM、ROM、磁碟、磁带、光盘、闪存、U盘、移动硬盘、存储卡、记忆棒、网络服务器存储、网络云存储等。

上述各实施例是参照根据实施例所述的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到计算机设备的处理器以产生一个机器，使得通过计算机设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机设备以特定方式工作的计算机设备可读存储器中，使得存储在该计算机设备可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机设备上，使得在计算机设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

尽管已经对上述各实施例进行了描述，但本领域内的技术人员一旦得知了基本创造性概念，则可对这些实施例做出另外的变更和修改，所以以上所述仅为本发明的实施例，并非因此限制本发明的专利保护范围，凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本发明的专利保护范围之内。

Claims (10)

1.一种机动车实时精准定位方法，其特征在于，包括如下步骤，机动位移更新步骤、测量更新步骤；

其中机动位移更新步骤包括如下步骤：以机动车起始速度作为初始条件，将机动车惯性加速度对于时间进行积分，得到系统惯性速率；以机动车起始位置坐标作为初始条件积分得到系统惯性位置；重复上述步骤，根据系统惯性速率和系统惯性位置得到连续的导航坐标系统；消除所述导航坐标系统与车辆坐标系统的偏差，得到后验概率；

测量更新包括如下步骤：对环境进行激光扫描，得到反射数据，根据后验概率、反射数据和地图数据信息计算后验概率分布，根据后验概率分布判断得到机动车位置。

2.根据权利要求1所述的一种机动车实时精准定位方法，其特征在于，测量更新后还包括步骤，对后验概率分布的结果进行最大似然估计，选择最优化的坐标表示机动车坐标的估计值。

3.根据权利要求2所述的一种机动车实时精准定位方法，其特征在于，通过下列公式选择最优化的坐标表示估计值：

$\begin{array}{cc}x=\frac{{\mathrm{\Σ}}_{x,y}P{(x,y)}^{\mathrm{\α}}\·x}{{\mathrm{\Σ}}_{x,y}P{(x,y)}^{\mathrm{\α}}}& y=\frac{{\mathrm{\Σ}}_{x,y}P{(x,y)}^{\mathrm{\α}}\·y}{{\mathrm{\Σ}}_{x,y}P{(x,y)}^{\mathrm{\α}}}\end{array}$

其中P为后验概率分布，x，y为GPS方位坐标。

4.根据权利要求1所述的一种机动车实时精准定位方法，其特征在于，所述得到后验概率的公式为：

$\stackrel{\‾}{P}(x,y)=\mathrm{\η}\·\underset{i,j}{\Σ}P(i,j)\·\exp (-\frac{1}{2}{\left(i-x\right)}^2{\left(j-y\right)}^2/{\mathrm{\σ}}^2)$

其中代表在机动位移更新后车辆位置在(x，y)的后验概率，η代表了归一化常数，x，y为GPS方位坐标，i，j为单元格坐标。

5.根据权利要求1所述的一种机动车实时精准定位方法，其特征在于，所述根据后验概率、反射数据和地图数据信息计算后验概率分布具体为：

P(x,y|z,m)＝η·P(z|x,y,m)·P(x,y)

其中P(x,y|z,m)为后验概率分布，z为反射数据，m为地图数据信息，x，y是GPS方位坐标，η代表归一化常数。

6.一种机动车实时精准定位装置，其特征在于，包括机动位移更新模块、测量更新模块；

其中机动位移更新模块用于以起始速度作为初始条件，对惯性加速度对于时间进行积分，得到系统惯性速率；还用于以起始位置坐标作为初始条件积分得到惯性位置；还用于重复上述步骤，得到连续的导航坐标系统；消除所述导航坐标系统与车辆坐标系统的偏差，得到后验概率；

测量更新模块用于对环境进行激光扫描，得到反射数据；还用于根据后验概率、反射数据和地图数据信息计算后验概率分布，根据后验概率分布判断得到机动车位置。

7.根据权利要求6所述的一种机动车实时精准定位装置，其特征在于，还包括最大似然估计模块，所述最大似然估计模块用于对后验概率分布的结果进行最大似然估计，选择最优化的坐标表示机动车坐标的估计值。

8.根据权利要求7所述的一种机动车实时精准定位装置，其特征在于，所述最大似然估计模块通过下列公式选择最优化的坐标表示估计值：

$\begin{array}{cc}x=\frac{{\mathrm{\Σ}}_{x,y}P{(x,y)}^{\mathrm{\α}}\·x}{{\mathrm{\Σ}}_{x,y}P{(x,y)}^{\mathrm{\α}}}& y=\frac{{\mathrm{\Σ}}_{x,y}P{(x,y)}^{\mathrm{\α}}\·y}{{\mathrm{\Σ}}_{x,y}P{(x,y)}^{\mathrm{\α}}}\end{array}$

其中P为后验概率分布，x，y为GPS方位坐标。

9.根据权利要求6所述的一种机动车实时精准定位装置，其特征在于，所述机动位移模块用于得到后验概率的公式为：

$\stackrel{\‾}{P}(x,y)=\mathrm{\η}\·\underset{i,j}{\Σ}P(i,j)\·\exp (-\frac{1}{2}{\left(i-x\right)}^2{\left(j-y\right)}^2/{\mathrm{\σ}}^2)$

其中代表在机动位移更新后车辆位置在(x，y)的后验概率，η代表了归一化常数，x，y为GPS方位坐标，i，j为单元格坐标。

10.根据权利要求6所述的一种机动车实时精准定位装置，其特征在于，所述测量更新模块用于根据后验概率、反射数据和地图数据信息计算后验概率分布具体为：

P(x,y|z,m)＝η·P(z|x,y,m)·P(x,y)

其中P(x,y|z,m)为后验概率分布，z为反射数据，m为地图数据信息，x，y是GPS方位坐标，η代表归一化常数。