一种无人车位置预测方法及装置

1.本发明涉及无人车技术领域，尤其涉及一种无人车位置预测方法及装置。


背景技术：

2.无人驾驶车辆(以下简称无人车)是一种不需要人工控制即可对其周围环境进行感知，对场景进行决策判断，对其进行控制的现代车辆。无人车在行驶过程中，无人车的gps(英文全称：global positioning system，中文全称：全球定位系统)信号可能会被非高斯噪声干扰，甚至会丢失信号，这种情况下，无人车无法定位位置，很容易影响无人车的正常行驶。


技术实现要素：

3.有鉴于此，本实施例中，通过离子滤波和马尔可夫模型分别预测车辆未来位置信息，这样即使车辆gps信号不佳的情况下，也能保障无人车的正常行驶。
4.本发明实施例公开了一种无人车位置预测方法，包括：
5.在车辆周围存在参照物的情况下，获取参照物的位置信息和车辆的第一历史行驶状态信息；
6.基于参照物与车辆的空间关系构建车辆的观测方程；
7.通过观测方程和车辆的第一历史行驶状态信息进行粒子滤波，预测车辆的未来位置信息；
8.在车辆周围不存在参照物的情况下，获取车辆在不同历史时刻的第二行驶状态信息，并通过马尔可夫模型和车辆在不同历史时刻的第二行驶状态信息，预测车辆未来位置。
9.可选的，所述基于参照物与车辆的空间关系构建车辆的观测方程，包括：
10.根据参照物的位置信息，构建车辆与参照物距离的方程，以及车辆与参照物航向角的方程；
11.通过所述车辆的预期车速与实际车速的偏差，构建车速的观测值方程；
12.基于车辆与参照物距离的方程、车辆与参照物航向角的方程以及车速的观测值方程以及非高斯噪声，构建车辆的观测方程。
13.可选的，所述通过观测方程和车辆的第一历史行驶状态信息进行粒子滤波，预测车辆的未来位置信息，包括：
14.构建车辆的第一运动学模型；所述第一运动学模型表征车辆质心的坐标、偏航角和期望车速、前轮角以及车辆轴距之间的关系；
15.通过所述运动学模型和车辆的第一历史行驶状态预测粒子的下一步行驶状态，得到每个粒子的行驶状态预测值；
16.将每个粒子的行驶状态预测值输入到观测方程中，得到每个粒子的观测值；
17.获取车辆当前的观测值；所述观测值包括：当前车辆与周围物体的距离、当前车辆与周围物体的角度以及实际车速和预期车速的偏差；
18.通过车辆当前的观测值和粒子的预测值，计算粒子的权重；
19.基于粒子的权重对粒子进行筛选；
20.对于筛选后的粒子，返回执行通过所述运动学模型预测粒子的下一步状态，得到每个粒子的状态预测值。
21.可选的，所述车辆周围不存在参照物的情况下，获取车辆在不同历史时刻的第二行驶状态信息，包括：
22.构建车辆的第二运动学模型；所述第二运动学模型表征车辆质心的坐标、偏航角和偏航角速度、车辆的轴距、车辆速度之间的关系；
23.通过所述第二运动学模型，获取车辆在不同历史时刻的第二行驶状态信息；所述不同历史时刻的行驶状态信息包括：车辆的纵向车速和偏航角速度。
24.可选的，所述通过马尔可夫模型和车辆在不同历史时刻的第二行驶状态信息，预测车辆未来位置，包括：
25.通过所述车辆在不同历史时刻的第二行驶状态信息，构建变阶、变预测步长的马尔可夫模型；
26.通过变阶变预测步长的马尔可夫模型构建转移概率，并且确定约束条件；
27.依据所述转移概率以及约束条件以及动态滑动窗口理论，构建动态转移概率；
28.根据动态转移概率确定预测方程；
29.根据所述预测方程预测车辆的未来位置。
30.本发明实施例还公开了一种无人车位置预测装置，包括：
31.获取单元，用于在车辆周围存在参照物的情况下，获取参照物的位置信息和车辆的第一历史行驶状态信息；
32.观测方程构建单元，用于基于参照物与车辆的空间关系构建车辆的观测方程；
33.第一未来位置预测单元，用于通过观测方程和车辆的第一历史行驶状态信息进行粒子滤波，预测车辆的未来位置信息；
34.第二未来位置预测单元，用于在车辆周围不存在参照物的情况下，获取车辆在不同历史时刻的第二行驶状态信息，并通过马尔可夫模型和车辆在不同历史时刻的第二行驶状态信息，预测车辆未来位置。
35.可选的，所述观测方程构建单元，包括：
36.距离和航向角方程构建子单元，用于根据参照物的位置信息，构建车辆与参照物距离的方程，以及车辆与参照物航向角的方程；
37.车速观测值方程构建子单元，用于通过所述车辆的预期车速与实际车速的偏差，构建车速的观测值方程；
38.车辆观测方程构建子单元，用于基于车辆与参照物距离的方程、车辆与参照物航向角的方程以及车速的观测值方程以及非高斯噪声，构建车辆的观测方程。
39.可选的，所述第一未来位置预测单元，包括：
40.第一运动学模型构建子单元，用于构建车辆的第一运动学模型；所述第一运动学模型表征车辆质心的坐标、偏航角和期望车速、前轮角以及车辆轴距之间的关系；
41.第一预测子单元，用于通过所述运动学模型和车辆的第一历史行驶状态预测粒子的下一步行驶状态，得到每个粒子的行驶状态预测值；
42.第二预测子单元，用于将每个粒子的行驶状态预测值输入到观测方程中，得到每个粒子的观测值；
43.第一获取子单元，用于获取车辆当前的观测值；所述观测值包括：当前车辆与周围物体的距离、当前车辆与周围物体的角度以及实际车速和预期车速的偏差；
44.粒子权重计算子单元，用于通过车辆当前的观测值和粒子的预测值，计算粒子的权重；
45.粒子筛选子单元，用于基于粒子的权重对粒子进行筛选；
46.迭代子单元，用于对于筛选后的粒子，返回执行通过所述运动学模型预测粒子的下一步状态，得到每个粒子的状态预测值。
47.可选的，所述第二未来位置预测单元，包括：
48.第二运动学模型构建子单元，用于构建车辆的第二运动学模型；所述第二运动学模型表征车辆质心的坐标、偏航角和偏航角速度、车辆的轴距、车辆速度之间的关系；
49.第二获取子单元，用于通过所述第二运动学模型，获取车辆在不同历史时刻的第二行驶状态信息；所述不同历史时刻的行驶状态信息包括：车辆的纵向车速和偏航角速度。
50.可选的，所述第二未来位置预测单元，包括：
51.马尔可夫模型构建子单元，用于通过所述车辆在不同历史时刻的第二行驶状态信息，构建变阶、变预测步长的马尔可夫模型；
52.转移概率构建子单元，用于通过变阶变预测步长的马尔可夫模型构建转移概率，并且确定约束条件；
53.动态转移概率构建子单元，用于依据所述转移概率以及约束条件以及动态滑动窗口理论，构建动态转移概率；
54.预测方程构建子单元，用于根据动态转移概率确定预测方程；
55.预测子单元，用于根据所述预测方程预测车辆的未来位置。
56.本发明实施例公开了一种无人车位置预测方法及装置，包括：在检测到车辆周围存在参照物的情况下，获取参照物的位置信息和车辆的第一历史行驶状态信息；基于参照物与车辆的空间关系构建车辆的观测方程；通过观测方程和车辆的第一运动状态信息进行粒子滤波，预测车辆的未来位置信息；在车辆周围不存在参照物的情况下，获取车辆在不同历史时刻的第二行驶状态信息，并通过马尔可夫模型和车辆在不同历史时刻的第二行驶状态信息，预测车辆未来位置。由此，即使gps信号不佳的情况下，也可以通过对车辆位置进行预测的方法，对车辆进行定位，保障了无人车的正常行驶。
附图说明
57.为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据提供的附图获得其他的附图。
58.图1示出了本发明实施例提供的一种无人车位置预测方法的流程示意图；
59.图2示出了本发明实施例公开的一种无人车位置预测装置的结构示意图。
具体实施方式
60.下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
61.参考图1，示出了本发明实施例提供的一种无人车位置预测方法的流程示意图，在本实施例中，该方法包括:
62.s101:在车辆周围存在参照物的情况下，获取参照物的位置信息和车辆的第一历史行驶状态信息；
63.本实施例中，无人车上安装有传感器，可以用来探测周围的物体，将探测到的周围的物体表示为无人车的参照物，例如可以为车辆周围的建筑物，或者车辆周围的树木。其中，在探测到车辆周围的参照物后，可以根据预设的世界地图，或者预设的某区域的地图以及参照物的相关信息(例如参照物的图片)获取参照物的位置信息。
64.其中，第一历史行驶状态信息例如包括：车辆的位置和偏航角。本实施例中，可以通过多种方式获取车辆的第一历史行驶状态信息，本实施例中不进行限定。
65.s102：基于参照物与车辆的空间关系构建车辆的观测方程；
66.在车辆信号不佳，或者gps失灵的情况下，难以确定车辆当前位置。在参照物位置已知的情况下，本实施例中，可以根据车辆与参照物的空间关系预测车辆位置，其中，车辆与参照物的位置关系可以包括：车辆与参照物的距离、车辆与参照物的航向角等。除此之外，车辆的位置也与车辆的速度有关，因此预测车辆位置的参数还可以包括车辆车速的观测值，本实施例中通过车辆的期望速度和实际速度的偏差表示车辆的观测值。
67.其中，根据周围环境的信息，无人驾驶车辆可以通过计算其与周围参考对象的相对距离来实现实时定位。车辆与周围物体的距离方程如下公式1)所示：
[0068][0069]
其中，d为传感器测量到的车辆与周围参考物体的距离，(x0,y0)为参考物体的坐标，可在构建的地图中确定。s(t)为实时测量噪声。
[0070]
其中，车辆位置与周围参考物体位置之间的航向角方程如下公式2)所示：
[0071][0072]
其中，θ为传感器测得的车辆与周围参考物体之间的航向角，同时也受到测量噪声的干扰。
[0073]
为了满足高速无人驾驶车辆实时定位的要求，需要考虑车辆行驶状态信息。实际纵向速度与预期纵向速度的偏差作为观测值，其观测方程如下公式3)所示：
[0074]
△
v(t)＝[v
r
(t)
‑
v(t)]+s(t)
ꢀꢀꢀꢀꢀꢀꢀ
3)
[0075]
通过上述可知，车辆的观测方程可以表示为如下的公式4)：
[0076]
z(t)＝h(x(t))+s(t)
ꢀꢀꢀꢀꢀꢀꢀ
4)；
[0077]
其中，h(x(t))通过如下的公式5)表示：
[0078][0079]
如上所示，车辆观测方程的构建方法包括：
[0080]
根据参照物的位置信息，构建车辆与参照物距离的方程，以及车辆与参照物航向角的方程；
[0081]
通过所述车辆的预期车速与实际车速的偏差，构建车速的观测值方程；
[0082]
基于车辆与参照物距离的方程、车辆与参照物航向角的方程以及车速的观测值方程以及非高斯噪声，构建车辆的观测方程。
[0083]
其中，非高斯噪声可以包括多种，本实施例中不进行限定，例如可以包括：色噪声或闪烁噪声。
[0084]
s103：通过观测方程和车辆的第一运动状态信息进行粒子滤波，预测车辆的未来位置信息；
[0085]
本实施例中，通过粒子滤波预测车辆未来位置的过程中，可以通过观测方程不断更新粒子的位置，并剔除劣质粒子，更新新的粒子，从而指导粒子空间分布。
[0086]
具体的,粒子滤波的过程包括如下的s201
‑
s207：，包括：
[0087]
s201：构建车辆的第一运动学模型；所述第一运动学模型表征车辆质心的坐标、偏航角和期望车速、前轮角以及车辆轴距之间的关系；
[0088]
举例说明：假设无人车的状态矩阵为控制量矩阵为[v,δ
f
]
t
，车辆的第一运动学模型如下公式5)所示：
[0089][0090]
式中，(x,y)为车辆质心在惯性坐标系中的坐标，为偏航角，v为期望车速，δ
f
为前轮角，l为车辆轴距。
[0091]
s202：通过所述运动学模型和车辆的第一历史行驶状态预测粒子的下一步行驶状态，得到每个粒子的行驶状态预测值；
[0092]
其中，在粒子更新的过程中，通过t
‑
1时刻的历史行驶状态以及车辆的运动学方程预测车辆下一时刻t的状态，具体的，将例如包括：车辆位置与偏航角。
[0093]
s203：将每个粒子的行驶状态预测值输入到观测方程中，得到每个粒子的观测值；
[0094]
s204：获取车辆当前的观测值；所述观测值包括：当前车辆与周围物体的距离、当前车辆与周围物体的角度以及实际车速和预期车速的偏差；
[0095]
s205：通过车辆当前的观测值和粒子的预测值，计算粒子的权重；
[0096]
s206：基于粒子的权重对粒子进行筛选；
[0097]
s207：对于筛选后的粒子，返回执行s201。
[0098]
举例说明：粒子滤波过程中，粒子滤波过程包括：
[0099]
(1)首先将表示车辆t
‑
1时刻的状态x
i
(t)的每一个粒子带入车辆运动学状态方
程，得到一步预测值其中i＝1,2,
…
,n。
[0100]
(2)将得到的集合中的每一个值代入观测方程中，计算得到观测值的预测集合
[0101]
(3)在当前t时刻，测量系统能够唯一采集到的一个观测值zc(t)(车辆的距离)，可以用于计算每个粒子的权值。根据权值，控制器可以复制大量的优质粒子，剔除劣质粒子，从而指导粒子空间的分布，权值的计算公式可以表示为如下的公式6)
[0102][0103]
其归一化权值可以通过如下的公式7)表示：
[0104][0105]
在对粒子进行重采样阶段，根据归一化权值的大小重新采样得到新的n个粒子的集合并重新分配粒子权值复制一部分权值高的粒子，同时淘汰一部分权值低的粒子。
[0106]
s104：在车辆周围不存在参照物的情况下，获取车辆在不同历史时刻的第二行驶状态信息，并通过马尔可夫模型和车辆在不同历史时刻的第二行驶状态信息，预测车辆未来位置。
[0107]
本实施例中，车辆在不同历史时刻的第二行驶状态信息，例如可以包括：车辆的纵向车速和偏航角，其中车辆的第二行驶状态，例如可以通过车辆的第二运动学模型确定，具体的，包括：
[0108]
构建车辆的第二运动学模型；所述第二运动学模型表征车辆质心的坐标、偏航角和偏航角速度、车辆的轴距、车辆速度之间的关系；
[0109]
通过所述第二运动学模型，获取车辆在不同历史时刻的行驶状态信息；所述不同历史时刻的行驶状态信息包括：车辆的纵向车速和偏航角速度。
[0110]
其中，车辆的第二运动学模型可以通过如下的公式8)表示：
[0111][0112]
其中，w为偏航角速度，l为车辆的轴距，δ
f
为前轮角。
[0113]
其中，通过车辆的第二运动学模型可以获得车辆在不同时刻的状态，例如，在不同时刻车辆的纵向车速和偏航角速度。
[0114]
其中，获取的车辆在不同历史时刻的行驶状态，并且车辆在不同时刻的状态构建马尔可夫过程的离散状态集。
[0115]
举例说明：假设随机过程{xn,n∈t}的参数集t属于离散时间集，t＝{0,1,2，
…
}，其对应的xn属于马尔可夫过程的离散状态集i＝{i0,i1,i2,
…
,i
n
}。
[0116]
本实施例中，通过马尔可夫模型和车辆在不同历史时刻的第二行驶状态信息，预测车辆未来位置的过程包括如下的：s301
‑
s305，包括：
[0117]
s301：通过所述车辆在不同历史时刻的第二行驶状态信息，构建变阶、变预测步长的马尔可夫模型；
[0118]
s302：通过变阶变预测步长的马尔可夫模型构建转移概率，并且确定约束条件；
[0119]
s303：依据所述转移概率以及约束条件以及动态滑动窗口理论，构建动态转移概率；
[0120]
s304：根据动态转移概率确定预测方程；
[0121]
s305：根据所述预测方程预测车辆的未来位置。
[0122]
其中，在构建马尔可夫模型的过程中，条件概率公式可以通过如下的公式9)表示：
[0123][0124]
通过该条件概率公式构建变阶、变预测步长的马尔可夫模型，其中，马尔可夫模型可以通过如下的公式10)表示：
[0125][0126]
需要说明的是，构建的变阶、变预测步长的马尔可夫模型为高阶的马尔可夫模型。
[0127]
其中，s为马尔可夫模型的阶数，n为预测步长，m为状态数，根据数据库中车辆的速度和角速度的取值范围来设定状态数量。
[0128]
本实施例中，高阶马尔可夫模型的转移概率通过如下的公式11)表示：
[0129][0130]
其中，表示状态i到状态j的转移概率，k∈{1,2,....,m}，λ
k
表示遗忘因子，满足以下方程12)：
[0131][0132]
基于动态滑动窗口理论，在传感器数据采集过程中，窗口随着数据流向前移动，实时更新动态转移概率矩阵。动态转移概率矩阵p
(t)
的表达形式如下所示：
[0133][0134]
最后，预测方程可以通过如下的公式14)表示：
[0135][0136]
其中，x
t
为t时刻的概率分布，p
k
为k阶的转移概率矩阵，s为可调节的阶数。
[0137]
本实施例中，通过粒子滤波或者马尔可夫模型预测车辆未来的状态，包括：在检测到车辆周围存在参照物的情况下，获取参照物的位置信息和车辆的第一历史行驶状态信息；基于参照物与车辆的空间关系构建车辆的观测方程；通过观测方程和车辆的第一运动状态信息进行粒子滤波，预测车辆的未来位置信息；在车辆周围不存在参照物的情况下，获取车辆在不同历史时刻的第二行驶状态信息，并通过马尔可夫模型和车辆在不同历史时刻的第二行驶状态信息，预测车辆未来位置。由此，即使gps信号不佳的情况下，也可以通过对车辆位置进行预测的方法，对车辆进行定位，保障了无人车的正常行驶。
[0138]
参考图2，示出了本发明实施例公开的一种无人车位置预测装置的结构示意图，在本实施例中，该装置包括：
[0139]
获取单元201，用于在车辆周围存在参照物的情况下，获取参照物的位置信息和车辆的第一历史行驶状态信息；
[0140]
观测方程构建单元202，用于基于参照物与车辆的空间关系构建车辆的观测方程；
[0141]
第一未来位置预测单元203，用于通过观测方程和车辆的第一运动状态信息进行粒子滤波，预测车辆的未来位置信息；
[0142]
第二未来位置预测单元，用于在车辆周围不存在参照物的情况下，获取车辆在不同历史时刻的第二行驶状态信息，并通过马尔可夫模型和车辆在不同历史时刻的第二行驶状态信息，预测车辆未来位置。
[0143]
可选的，所述观测方程构建单元，包括：
[0144]
距离和航向角方程构建子单元，用于根据参照物的位置信息，构建车辆与参照物距离的方程，以及车辆与参照物航向角的方程；
[0145]
车速观测值方程构建子单元，用于通过所述车辆的预期车速与实际车速的偏差，构建车速的观测值方程；
[0146]
车辆观测方程构建子单元，用于基于车辆与参照物距离的方程、车辆与参照物航向角的方程以及车速的观测值方程以及非高斯噪声，构建车辆的观测方程。
[0147]
可选的，所述第一未来位置预测单元，包括：
[0148]
第一运动学模型构建子单元，用于构建车辆的第一运动学模型；所述第一运动学模型表征车辆质心的坐标、偏航角和期望车速、前轮角以及车辆轴距之间的关系；
[0149]
第一预测子单元，用于通过所述运动学模型和车辆的第一历史行驶状态预测粒子的下一步行驶状态，得到每个粒子的行驶状态预测值；
[0150]
第二预测子单元，用于将每个粒子的行驶状态预测值输入到观测方程中，得到每个粒子的观测值；
[0151]
第一获取子单元，用于获取车辆当前的观测值；所述观测值包括：当前车辆与周围物体的距离、当前车辆与周围物体的角度以及实际车速和预期车速的偏差；
[0152]
粒子权重计算子单元，用于通过车辆当前的观测值和粒子的预测值，计算粒子的权重；
[0153]
粒子筛选子单元，用于基于粒子的权重对粒子进行筛选；
[0154]
迭代子单元，用于对于筛选后的粒子，返回执行通过所述运动学模型预测粒子的下一步状态，得到每个粒子的状态预测值。
[0155]
可选的，所述第二未来位置预测单元，包括：
[0156]
第二运动学模型构建子单元，用于构建车辆的第二运动学模型；所述第二运动学模型表征车辆质心的坐标、偏航角和偏航角速度、车辆的轴距、车辆速度之间的关系；
[0157]
第二获取子单元，用于通过所述第二运动学模型，获取车辆在不同历史时刻的第二行驶状态信息；所述不同历史时刻的行驶状态信息包括：车辆的纵向车速和偏航角速度。
[0158]
可选的，所述第二未来位置预测单元，包括：
[0159]
马尔可夫模型构建子单元，用于通过所述车辆在不同历史时刻的第二行驶状态信息，构建变阶、变预测步长的马尔可夫模型；
[0160]
转移概率构建子单元，用于通过变阶变预测步长的马尔可夫模型构建转移概率，并且确定约束条件；
[0161]
动态转移概率构建子单元，用于依据所述转移概率以及约束条件以及动态滑动窗口理论，构建动态转移概率；
[0162]
预测方程构建子单元，用于根据动态转移概率确定预测方程；
[0163]
预测子单元，用于根据所述预测方程预测车辆的未来位置。
[0164]
通过本实施例的装置，在检测到车辆周围存在参照物的情况下，获取参照物的位置信息和车辆的第一历史行驶状态信息；基于参照物与车辆的空间关系构建车辆的观测方程；通过观测方程和车辆的第一运动状态信息进行粒子滤波，预测车辆的未来位置信息；在车辆周围不存在参照物的情况下，获取车辆在不同历史时刻的第二行驶状态信息，并通过马尔可夫模型和车辆在不同历史时刻的第二行驶状态信息，预测车辆未来位置。由此，即使gps信号不佳的情况下，也可以通过对车辆位置进行预测的方法，对车辆进行定位，保障了无人车的正常行驶。
[0165]
需要说明的是，本说明书中的各个实施例均采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似的部分互相参见即可。
[0166]
对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。