一种融合定位方法、装置、系统和计算机可读存储介质与流程

1.本发明涉及移动通信技术领域，尤其涉及一种融合定位方法、装置、系统和计算机可读存储介质。


背景技术：

2.定位是车联网应用的一个重要基础，车联网安全、交通效率等场景都对定位提出要求。目前的定位方法有：基于gnss卫星导航系统进行的定位和基于传感器进行的定位，前者利用卫星单点定位或rtk技术完成定位，后者利用激光雷达、摄像头、惯导等实现传感定位。由于现有的定位方案均采用车载系统完成定位，存在定位成本高的问题；部分受成本影响的车辆还存在定位精度低的问题。


技术实现要素：

3.有鉴于此，本发明实施例期望提供一种融合定位方法、装置、系统和计算机可读存储介质。
4.为达到上述目的，本发明实施例的技术方案是这样实现的：
5.本发明实施例提供了一种融合定位方法，该方法应用于路侧计算单元，包括：
6.接收路侧部署的毫米波雷达感知并传输的车辆信息；
7.接收路侧单元rsu传输的位置信息；所述位置信息为：车载卫星定位设备获得并由车辆发送到所述rsu；
8.基于所述车辆信息和所述位置信息经融合定位算法确定所述车辆的定位结果，并经所述rsu将所述定位结果广播给该车辆。
9.其中，所述接收路侧部署的毫米波雷达感知并传输的车辆信息，包括：
10.接收路侧部署的毫米波雷达通过有线的方式传输的车辆信息；其中，
11.所述车辆信息为所述毫米波雷达感知到的探测范围内经过的车辆的信息数据；所述车辆信息至少包括：车辆的位置信息及速度信息。
12.其中，
13.两个相邻的所述路侧部署的毫米波雷达均感知到同一车辆时，接收到的毫米波雷达传输的车辆信息为两路；
14.两个相邻的所述路侧部署的毫米波雷达未同时感知到同一车辆时，接收到的毫米波雷达传输的车辆信息为一路。
15.其中，所述基于所述车辆信息和所述位置信息经融合定位算法确定所述车辆的定位结果，包括：
16.将所述车辆信息中的位置信息与rsu传输的位置信息进行匹配；
17.经匹配确定所述两路或三路位置信息均为同一车辆的位置信息；
18.基于卡尔曼滤波模型处理该车辆的每一路位置信息，并进行归一化，得到所述车辆的定位结果。
19.其中，所述基于卡尔曼滤波模型处理该车辆的每一路位置信息，并进行归一化，包括：
20.对该车辆的每一路位置信息采用卡尔曼滤波模型得到每一路位置信息的最佳位置估计值；
21.将每一路位置信息的最佳位置估计值采用误差协方差矩阵的倒数作为系数进行加和，得到所述车辆的定位结果；其中，
22.所述误差协方差矩阵为：采用卡尔曼滤波更新后的误差协方差矩阵。
23.本发明实施例还提供了一种融合定位方法，该方法应用于路侧单元rsu，包括：
24.上报车辆发送的车载卫星定位设备获得的位置信息到路侧计算单元，用于所述路侧计算单元基于路侧部署的毫米波雷达传输的车辆信息和所述位置信息经融合定位算法确定所述车辆的定位结果；
25.接收所述路侧计算单元发送的所述定位结果，并将所述定位结果广播给该车辆。
26.本发明实施例还提供了一种融合定位装置，该装置应用于路侧计算单元，包括：
27.第一接收模块，用于接收路侧部署的毫米波雷达感知并传输的车辆信息；接收路侧单元rsu传输的位置信息；所述位置信息为：车载卫星定位设备获得并由车辆发送到所述rsu；
28.处理模块，用于基于所述车辆信息和所述位置信息经融合定位算法确定所述车辆的定位结果；
29.第一发送模块，用于经所述rsu将所述定位结果广播给该车辆。
30.本发明实施例还提供了一种融合定位装置，该装置应用于路侧单元rsu，包括：
31.第二发送模块，用于上报车辆发送的车载卫星定位设备获得的位置信息到路侧计算单元，用于所述路侧计算单元基于路侧部署的毫米波雷达传输的车辆信息和所述位置信息经融合定位算法确定所述车辆的定位结果；
32.还用于将所述定位结果广播给该车辆；
33.第二接收模块，用于接收所述路侧计算单元发送的所述定位结果。
34.本发明实施例还提供了一种融合定位系统，该系统包括：若干个路侧部署的毫米波雷达、若干个路侧单元rsu、路口部署的一个或多个路侧计算单元；其中，
35.所述毫米波雷达，用于将感知到的车辆信息传输给所述路侧计算单元；
36.所述路侧计算单元，用于接收路侧部署的毫米波雷达传输的车辆信息；接收路侧单元rsu传输的位置信息；所述位置信息为：车载卫星定位设备获得并由车辆发送到所述rsu；基于所述车辆信息和所述位置信息经融合定位算法确定所述车辆的定位结果，并经所述rsu将所述定位结果广播给该车辆；
37.所述rsu，用于上报车辆发送的车载卫星定位设备获得的位置信息到路侧计算单元；接收所述路侧计算单元发送的所述定位结果，并将所述定位结果广播给该车辆。
38.本发明实施例还提供了一种融合定位装置，该装置包括：处理器和用于存储能够在处理器上运行的计算机程序的存储器，
39.其中，所述处理器用于运行所述计算机程序时，执行上述方法的步骤。
40.本发明实施例还提供了一种计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时实现上述方法的步骤。
41.本发明实施例提供的融合定位方法、装置、系统和计算机可读存储介质，接收路侧部署的毫米波雷达感知并传输的车辆信息；接收路侧单元rsu传输的位置信息；所述位置信息为：车载卫星定位设备获得并由车辆发送到所述rsu；基于所述车辆信息和所述位置信息经融合定位算法确定所述车辆的定位结果，并经所述rsu将所述定位结果广播给该车辆。本发明实施例将路侧毫米波雷达的感知信息与卫星定位系统获取的位置信息进行融合处理，提高定位的准确度；同时对于不同的数据源(位置信息)，根据卡尔曼滤波的误差协方差矩阵来确定每一路位置信息在本次定位结果的贡献度，提高了定位结果的可信度。
42.此外，本发明实施例不需在车辆上额外加装传感器设备，可节省定位成本和资源，使得没有条件安装传感器设备的车辆可以获得准确的定位信息。
附图说明
43.图1为本发明实施例所述融合定位方法流程示意图一；
44.图2为本发明实施例所述融合定位方法流程示意图二；
45.图3为本发明实施例所述融合定位装置结构示意图一；
46.图4为本发明实施例所述融合定位装置结构示意图二；
47.图5为本发明场景实施例所述定位相关数据传输示意图；
48.图6为本发明场景实施例所述定位方法流程示意图。
具体实施方式
49.下面结合附图和实施例对本发明进行描述。可以理解的是，此处所描述的具体实施例仅仅用于解释相关发明，而非对该发明的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与有关发明相关的部分。需要说明的是，在不冲突的情况下，本技术中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本技术。
50.本发明实施例提供了一种融合定位方法，如图1所示，该方法应用于路侧计算单元，包括：
51.步骤101：接收路侧部署的毫米波雷达感知并传输的车辆信息；
52.步骤102：接收路侧单元rsu传输的位置信息；所述位置信息为：车载卫星定位设备获得并由车辆发送到所述rsu；
53.步骤103：基于所述车辆信息和所述位置信息经融合定位算法确定所述车辆的定位结果，并经所述rsu将所述定位结果广播给该车辆。
54.这里，所述步骤101和步骤102的执行不分先后顺序。
55.本发明实施例中，所述接收路侧部署的毫米波雷达感知并传输的车辆信息，包括：
56.接收路侧部署的毫米波雷达通过有线的方式传输的车辆信息；其中，
57.所述车辆信息为所述毫米波雷达感知到的探测范围内经过的车辆的信息数据；所述车辆信息至少包括：车辆的位置信息及速度信息。
58.本发明实施例中，
59.两个相邻的所述路侧部署的毫米波雷达均感知到同一车辆时，接收到的毫米波雷达传输的车辆信息为两路；
60.两个相邻的所述路侧部署的毫米波雷达未同时感知到同一车辆时，接收到的毫米
波雷达传输的车辆信息为一路。
61.本发明实施例中，所述基于所述车辆信息和所述位置信息经融合定位算法确定所述车辆的定位结果，包括：
62.将所述车辆信息中的位置信息与rsu传输的位置信息进行匹配；
63.经匹配确定所述两路或三路位置信息均为同一车辆的位置信息；
64.基于卡尔曼滤波模型处理该车辆的每一路位置信息，并进行归一化，得到所述车辆的定位结果。
65.本发明实施例中，所述基于卡尔曼滤波模型处理该车辆的每一路位置信息，并进行归一化，包括：
66.对该车辆的每一路位置信息采用卡尔曼滤波模型得到每一路位置信息的最佳位置估计值；
67.将每一路位置信息的最佳位置估计值采用误差协方差矩阵的倒数作为系数进行加和，得到所述车辆的定位结果；其中，
68.所述误差协方差矩阵为：采用卡尔曼滤波更新后的误差协方差矩阵。
69.本发明实施例还提供了一种融合定位方法，如图2所示，该方法应用于路侧单元rsu，包括：
70.步骤201：上报车辆发送的车载卫星定位设备获得的位置信息到路侧计算单元，用于所述路侧计算单元基于路侧部署的毫米波雷达传输的车辆信息和所述位置信息经融合定位算法确定所述车辆的定位结果；
71.步骤202：接收所述路侧计算单元发送的所述定位结果，并将所述定位结果广播给该车辆。
72.为了实现上述方法实施例，本发明实施例还提供了一种融合定位装置，如图3所示，该装置应用于路侧计算单元，包括：
73.第一接收模块301，用于接收路侧部署的毫米波雷达感知并传输的车辆信息；接收路侧单元rsu传输的位置信息；所述位置信息为：车载卫星定位设备获得并由车辆发送到所述rsu；
74.处理模块302，用于基于所述车辆信息和所述位置信息经融合定位算法确定所述车辆的定位结果；
75.第一发送模块303，用于经所述rsu将所述定位结果广播给该车辆。
76.本发明实施例中，所述第一接收模块301接收路侧部署的毫米波雷达感知并传输的车辆信息，包括：
77.接收路侧部署的毫米波雷达通过有线的方式传输的车辆信息；其中，
78.所述车辆信息为所述毫米波雷达感知到的探测范围内经过的车辆的信息数据；所述车辆信息至少包括：车辆的位置信息及速度信息。
79.本发明实施例中，
80.两个相邻的所述路侧部署的毫米波雷达均感知到同一车辆时，第一接收模块301接收到的毫米波雷达传输的车辆信息为两路；
81.两个相邻的所述路侧部署的毫米波雷达未同时感知到同一车辆时，第一接收模块301接收到的毫米波雷达传输的车辆信息为一路。
82.本发明实施例中，所述处理模块302基于所述车辆信息和所述位置信息经融合定位算法确定所述车辆的定位结果，包括：
83.将所述车辆信息中的位置信息与rsu传输的位置信息进行匹配；
84.经匹配确定所述两路或三路位置信息均为同一车辆的位置信息；
85.基于卡尔曼滤波模型处理该车辆的每一路位置信息，并进行归一化，得到所述车辆的定位结果。
86.本发明实施例中，所述处理模块302基于卡尔曼滤波模型处理该车辆的每一路位置信息，并进行归一化，包括：
87.对该车辆的每一路位置信息采用卡尔曼滤波模型得到每一路位置信息的最佳位置估计值；
88.将每一路位置信息的最佳位置估计值采用误差协方差矩阵的倒数作为系数进行加和，得到所述车辆的定位结果；其中，
89.所述误差协方差矩阵为：采用卡尔曼滤波更新后的误差协方差矩阵。
90.本发明实施例还提供了一种融合定位装置，如图4所示，该装置应用于路侧单元rsu，包括：
91.第二发送模块401，用于上报车辆发送的车载卫星定位设备获得的位置信息到路侧计算单元，用于所述路侧计算单元基于路侧部署的毫米波雷达传输的车辆信息和所述位置信息经融合定位算法确定所述车辆的定位结果；
92.还用于将所述定位结果广播给该车辆；
93.第二接收模块402，用于接收所述路侧计算单元发送的所述定位结果。
94.本发明实施例还提供了一种融合定位系统，该系统包括：若干个路侧部署的毫米波雷达501、若干个路侧单元rsu 502、路口部署的一个或多个路侧计算单元503；其中，
95.所述毫米波雷达501，用于将感知到的车辆信息传输给所述路侧计算单元；
96.所述路侧计算单元503，用于接收路侧部署的毫米波雷达传输的车辆信息；接收路侧单元rsu传输的位置信息；所述位置信息为：车载卫星定位设备获得并由车辆发送到所述rsu；基于所述车辆信息和所述位置信息经融合定位算法确定所述车辆的定位结果，并经所述rsu将所述定位结果广播给该车辆；
97.所述rsu 502，用于上报车辆发送的车载卫星定位设备获得的位置信息到路侧计算单元；接收所述路侧计算单元发送的所述定位结果，并将所述定位结果广播给该车辆。
98.本发明实施例还提供了一种融合定位装置，该装置包括：处理器和用于存储能够在处理器上运行的计算机程序的存储器，
99.其中，所述处理器用于运行所述计算机程序时，执行：
100.接收路侧部署的毫米波雷达感知并传输的车辆信息；
101.接收路侧单元rsu传输的位置信息；所述位置信息为：车载卫星定位设备获得并由车辆发送到所述rsu；
102.基于所述车辆信息和所述位置信息经融合定位算法确定所述车辆的定位结果，并经所述rsu将所述定位结果广播给该车辆。
103.所述接收路侧部署的毫米波雷达感知并传输的车辆信息时，所述处理器还用于运行所述计算机程序时，执行：
104.接收路侧部署的毫米波雷达通过有线的方式传输的车辆信息；其中，
105.所述车辆信息为所述毫米波雷达感知到的探测范围内经过的车辆的信息数据；所述车辆信息至少包括：车辆的位置信息及速度信息。
106.其中，
107.两个相邻的所述路侧部署的毫米波雷达均感知到同一车辆时，接收到的毫米波雷达传输的车辆信息为两路；
108.两个相邻的所述路侧部署的毫米波雷达未同时感知到同一车辆时，接收到的毫米波雷达传输的车辆信息为一路。
109.所述基于所述车辆信息和所述位置信息经融合定位算法确定所述车辆的定位结果时，所述处理器还用于运行所述计算机程序时，执行：
110.将所述车辆信息中的位置信息与rsu传输的位置信息进行匹配；
111.经匹配确定所述两路或三路位置信息均为同一车辆的位置信息；
112.基于卡尔曼滤波模型处理该车辆的每一路位置信息，并进行归一化，得到所述车辆的定位结果。
113.所述基于卡尔曼滤波模型处理该车辆的每一路位置信息，并进行归一化时，所述处理器还用于运行所述计算机程序时，执行：
114.对该车辆的每一路位置信息采用卡尔曼滤波模型得到每一路位置信息的最佳位置估计值；
115.将每一路位置信息的最佳位置估计值采用误差协方差矩阵的倒数作为系数进行加和，得到所述车辆的定位结果；其中，
116.所述误差协方差矩阵为：采用卡尔曼滤波更新后的误差协方差矩阵。
117.本发明实施例还提供了一种融合定位装置，该装置包括：处理器和用于存储能够在处理器上运行的计算机程序的存储器，
118.其中，所述处理器用于运行所述计算机程序时，执行：
119.上报车辆发送的车载卫星定位设备获得的位置信息到路侧计算单元，用于所述路侧计算单元基于路侧部署的毫米波雷达传输的车辆信息和所述位置信息经融合定位算法确定所述车辆的定位结果；
120.接收所述路侧计算单元发送的所述定位结果，并将所述定位结果广播给该车辆。
121.需要说明的是：上述实施例提供的装置在进行融合定位时，仅以上述各程序模块的划分进行举例说明，实际应用中，可以根据需要而将上述处理分配由不同的程序模块完成，即将设备的内部结构划分成不同的程序模块，以完成以上描述的全部或者部分处理。另外，上述实施例提供的装置与相应方法实施例属于同一构思，其具体实现过程详见方法实施例，这里不再赘述。
122.在示例性实施例中，本发明实施例还提供了一种计算机可读存储介质，所述计算机可读存储介质可以是fram、rom、prom、eprom、eeprom、flash memory、磁表面存储器、光盘、或cd-rom等存储器；也可以是包括上述存储器之一或任意组合的各种设备，如移动电话、计算机、平板设备、个人数字助理等。
123.本发明实施例还提供了一种计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时，执行：
124.接收路侧部署的毫米波雷达感知并传输的车辆信息；
125.接收路侧单元rsu传输的位置信息；所述位置信息为：车载卫星定位设备获得并由车辆发送到所述rsu；
126.基于所述车辆信息和所述位置信息经融合定位算法确定所述车辆的定位结果，并经所述rsu将所述定位结果广播给该车辆。
127.所述接收路侧部署的毫米波雷达感知并传输的车辆信息时，所述计算机程序被处理器运行时，还执行：
128.接收路侧部署的毫米波雷达通过有线的方式传输的车辆信息；其中，
129.所述车辆信息为所述毫米波雷达感知到的探测范围内经过的车辆的信息数据；所述车辆信息至少包括：车辆的位置信息及速度信息。
130.其中，
131.两个相邻的所述路侧部署的毫米波雷达均感知到同一车辆时，接收到的毫米波雷达传输的车辆信息为两路；
132.两个相邻的所述路侧部署的毫米波雷达未同时感知到同一车辆时，接收到的毫米波雷达传输的车辆信息为一路。
133.所述基于所述车辆信息和所述位置信息经融合定位算法确定所述车辆的定位结果时，所述计算机程序被处理器运行时，还执行：
134.将所述车辆信息中的位置信息与rsu传输的位置信息进行匹配；
135.经匹配确定所述两路或三路位置信息均为同一车辆的位置信息；
136.基于卡尔曼滤波模型处理该车辆的每一路位置信息，并进行归一化，得到所述车辆的定位结果。
137.所述基于卡尔曼滤波模型处理该车辆的每一路位置信息，并进行归一化时，所述计算机程序被处理器运行时，还执行：
138.对该车辆的每一路位置信息采用卡尔曼滤波模型得到每一路位置信息的最佳位置估计值；
139.将每一路位置信息的最佳位置估计值采用误差协方差矩阵的倒数作为系数进行加和，得到所述车辆的定位结果；其中，
140.所述误差协方差矩阵为：采用卡尔曼滤波更新后的误差协方差矩阵。
141.本发明实施例还提供了一种计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时，执行：
142.上报车辆发送的车载卫星定位设备获得的位置信息到路侧计算单元，用于所述路侧计算单元基于路侧部署的毫米波雷达传输的车辆信息和所述位置信息经融合定位算法确定所述车辆的定位结果；
143.接收所述路侧计算单元发送的所述定位结果，并将所述定位结果广播给该车辆。
144.下面结合场景实施例对本发明进行描述。
145.当前多个省市区开展车联网项目示范验证项目，在车联网示范区的建设中，通常都会在路侧部署毫米波传感器，实时感知道路上行驶的车辆及周围环境。本实施例提供一种路侧毫米波雷达与卫星融合定位系统，解决车辆因成本受限感知能力不足的问题。
146.本实施例的路侧毫米波雷达与卫星融合定位系统结构如下：
147.在路侧部署毫米波雷达，两雷达间距约在200m-300m之间，且为保证无缝覆盖，两雷达覆盖区域需有一定重叠。这里，如果道路较宽，可在道路两侧部署毫米波雷达，如果较窄，则在道路一侧部署即可。同时，在路侧部署一定的路侧单元(rsu)用于采集车辆上报的数据。在路口布设路侧计算单元，根据计算单元的算力能力差异部署，算力强的路侧计算单元可连接多个路侧毫米波雷达和多个rsu，算力弱的路侧计算单元则挂载较少的路侧毫米波雷达和rsu。
148.如图5所示，车辆行驶在部署了该路侧毫米波雷达与卫星融合定位系统的道路上时，毫米波雷达感知到车辆的位置和速度信息后，通过有线回传给路侧计算单元。同时车辆自身装载的卫星定位设备获取车辆位置信息，由车辆将自身的位置信息通过加密后封装在一定的信息格式内，通过sl直连链路发送给所述rsu。rsu将车辆上报的位置通过有线回传的方式，回传给路侧计算单元。路侧计算单元在获取到路侧毫米波雷达感知的信息和车辆上报的卫星定位位置信息后，经过一定融合定位算法的处理，解算出车辆精确的位置，将该位置信息通过有线方式发送给rsu。最后，rsu通过sl直连链路广播给车辆。此时，一个因成本受限，没有加装传感器的车辆获得了路侧毫米波雷达与卫星融合定位的精确的定位结果。
149.具体地，路侧计算单元进行车辆融合定位的过程如下：
150.步骤1：路侧毫米波雷达将感知到的数据上传给路侧计算单元；
151.其中，路侧毫米波雷达感知在探测范围内经过的车辆信息数据，经处理后获得车辆的位置及速度信息。路侧毫米波雷达通过有线回传，将处理后的结构化数据上报给路侧计算单元。
152.步骤2：车辆将自身获取的gnss定位信息(位置信息)，通过sl直连链路发送给覆盖范围内的rsu，rsu将所述定位信息上报给路侧计算单元；
153.步骤3：路侧计算单元将接收到的毫米波雷达定位信息与gnss定位信息进行匹配；
154.其中，路侧计算单元通过位置比对及毫米波雷达的跟踪结果，将毫米波雷达的感知信息和车辆的卫星定位信息匹配(匹配方法为已有技术，此处不再详述)。路侧的毫米波雷达为保证连续覆盖，通常会部署的比较密集，感知区域会存在重叠，因此存在一种情况，即相邻的两个路侧毫米波雷达同时感知到同一个车辆，此时，路侧计算单元中关于该车辆的融合定位信息会输入三路数据，包括车辆自身的gnss定位信息，以及相邻的两个毫米波雷达探测的信息。路侧计算单元将三路或两路定位信息融合处理后，将最终确定的最优车辆位置下发给rsu，rsu将定位等信息发送给车辆，车辆获取最终的位置解算信息。
155.车辆在行驶过程中，车辆上装载的卫星导航接收机会不断的获取卫星提供的定位和速度观测值数据。但是在某些遮挡比较严重的位置，或者卫星条件不好的区域，卫星定位会失锁、定位输出漂移、甚至无法工作。因此，利用路侧部署的毫米波雷达和卫星定位导航融合，可以在某一类定位无法正常工作的时候，另一类定位作为补充。
156.这里，车辆在路上行驶，路侧部署的毫米波雷达会探测感知到车辆的定位信息和速度信息。毫米波雷达在将原始数据进行时间及空间的坐标系转换后形成的结构化数据上传至路侧计算单元。此时若车辆经过的区域正好被两个毫米波雷达覆盖，则两个毫米波雷达都会上报其探测信息。同时车辆将自身通过卫星定位获得的位置信息通过直连链路发送给周围的rsu，rsu将车辆的位置信息上报给路侧计算单元。由于相同区域的毫米波雷达和
rsu都会挂载在同一个路侧计算单元上，所以此时不管是毫米波雷达的探测信息还是车辆自身上报的位置信息都汇聚在同一个路侧计算单元上。
157.路侧计算单元在接收到毫米波雷达定位信息以及车辆上报的gnss定位信息后，会进行匹配是否为同一车辆的数据。匹配过程可基于位置距离，将相邻最近的gnss和毫米波定位信息识别为同一车辆的定位数据，匹配方法可采用已有相关技术，此处不再详述。如果在某些时刻，车辆的gnss信息无法工作或者输出的数据漂移较大，无法进行毫米波雷达定位信息(位置信息)与gnss定位信息匹配，则可以剔除掉当前时刻的gnss定位信息，这样同一车辆在路侧计算单元上将存储匹配后的唯一一路毫米波定位信息，或者一路卫星定位信息与一路毫米波定位信息，或者一路卫星定位信息与两路毫米波定位信息。
158.步骤4：对于每一路的定位信息采用卡尔曼滤波模型，输出该路定位信息的最佳位置估计值，具体步骤如下：
159.车辆的位置包括x，y两个坐标，车辆速度包括v在x、y两个方向上的速度。假设车辆t-1时刻状态为：
160.x
t-1
＝[x
t-1
,y
t-1
,v
t-1
]
t
；
[0161]
采用卡尔曼滤波进行定位融合，则运动状态预测模型和观测模型分别为：
[0162]
其中f是运动状态转移矩阵，c
t
是协方差为q的预测模型噪声；
[0163]zt
为观测量，其中为观测转移函数，u
t
是协方差为r的观测噪声。
[0164]
对于每一路的定位信息，卡尔曼滤波器的预测和更新过程，具体为：
[0165]
预测误差协方差矩阵
[0166]
更新最优状态量
[0167]
卡尔曼系数为
[0168]
更新后的误差协方差矩阵
[0169]
这里，关于卡尔曼滤波算法为已有相关技术，此处不再详述。
[0170]
步骤5：将每一路的定位信息的误差协方差矩阵进行归一化，确定当前时刻最佳定位结果；
[0171]
这里，将gnss和毫米波定位信息进行融合时，由于误差协方差矩阵代表了估计值与实际值之间误差概率，当估计值越不稳定，其协方差越大。因此在定位信息融合过程中，采用了协方差矩阵的倒数来表示不同数据源在最终的位置估计值中的一个权重值。针对某一个车辆，当前有效的定位信息为一路或者两路或者三路的情况，将多路数据的误差协方差矩阵进行归一化处理，最终的位置、速度估计值采用协方差矩阵的倒数作为系数的加和形式，具体见以下公式：
[0172]
x
t
＝∑p
ti
*x
ti
；其中，i＝1-3，表示定位信息的路数。
[0173]
步骤6：路侧计算单元将定位结果发送给rsu，rsu通过直连链路将该结果发送给对应车辆。
[0174]
可见，本发明实施例的路侧毫米波雷达的感知信息对卫星定位系统进行融合和补充，在卫星信号良好，gnss可以正常定位的情况下，路侧毫米波雷达可以作为卫星定位系统的融合信息，提高定位结果的精度。当卫星受到干扰或无卫星信号环境下，路侧毫米波雷达的感知信息作为卫星定位的补充，利用毫米波雷达定位信息作为定位系统的主要参考，使定位信息不间断输出，不影响车联网应用的实现。
[0175]
此外，由于路侧毫米波雷达通常布设间距在200米左右，相邻的毫米波雷达感知区域会有所重叠，在重叠区域，两个毫米波雷达的信息可以互相验证，同时作为定位系统的输入源来提高最终的定位输出结果的准确度，同时对于不同的数据源，根据卡尔曼滤波的误差协方差矩阵来确定每一路定位信息在本次定位结果的贡献度，提高了定位信息的可信度。该路侧毫米波雷达与卫星融合定位系统可节约车侧部署大量传感器的成本，还在卫星无法工作或定位漂移的时候，保证定位正常工作，此外双定位信息源融合，可提高定位结果的准确度。
[0176]
本发明实施例中车辆上不需要额外加装传感器设备，比如摄像头或价格昂贵的激光雷达，可节省定位成本和资源，使得没有条件安装传感器设备的车辆可以获得准确的定位信息。
[0177]
以上所述，仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。

技术特征：
1.一种融合定位方法，其特征在于，该方法应用于路侧计算单元，包括：接收路侧部署的毫米波雷达感知并传输的车辆信息；接收路侧单元rsu传输的位置信息；所述位置信息为：车载卫星定位设备获得并由车辆发送到所述rsu；基于所述车辆信息和所述位置信息经融合定位算法确定所述车辆的定位结果，并经所述rsu将所述定位结果广播给该车辆。2.根据权利要求1所述的方法，其特征在于，所述接收路侧部署的毫米波雷达感知并传输的车辆信息，包括：接收路侧部署的毫米波雷达通过有线的方式传输的车辆信息；其中，所述车辆信息为所述毫米波雷达感知到的探测范围内经过的车辆的信息数据；所述车辆信息至少包括：车辆的位置信息及速度信息。3.根据权利要求1所述的方法，其特征在于，两个相邻的所述路侧部署的毫米波雷达均感知到同一车辆时，接收到的毫米波雷达传输的车辆信息为两路；两个相邻的所述路侧部署的毫米波雷达未同时感知到同一车辆时，接收到的毫米波雷达传输的车辆信息为一路。4.根据权利要求1或3所述的方法，其特征在于，所述基于所述车辆信息和所述位置信息经融合定位算法确定所述车辆的定位结果，包括：将所述车辆信息中的位置信息与rsu传输的位置信息进行匹配；经匹配确定所述两路或三路位置信息均为同一车辆的位置信息；基于卡尔曼滤波模型处理该车辆的每一路位置信息，并进行归一化，得到所述车辆的定位结果。5.根据权利要求4所述的方法，其特征在于，所述基于卡尔曼滤波模型处理该车辆的每一路位置信息，并进行归一化，包括：对该车辆的每一路位置信息采用卡尔曼滤波模型得到每一路位置信息的最佳位置估计值；将每一路位置信息的最佳位置估计值采用误差协方差矩阵的倒数作为系数进行加和，得到所述车辆的定位结果；其中，所述误差协方差矩阵为：采用卡尔曼滤波更新后的误差协方差矩阵。6.一种融合定位方法，其特征在于，该方法应用于路侧单元rsu，包括：上报车辆发送的车载卫星定位设备获得的位置信息到路侧计算单元，用于所述路侧计算单元基于路侧部署的毫米波雷达传输的车辆信息和所述位置信息经融合定位算法确定所述车辆的定位结果；接收所述路侧计算单元发送的所述定位结果，并将所述定位结果广播给该车辆。7.一种融合定位装置，其特征在于，该装置应用于路侧计算单元，包括：第一接收模块，用于接收路侧部署的毫米波雷达感知并传输的车辆信息；接收路侧单元rsu传输的位置信息；所述位置信息为：车载卫星定位设备获得并由车辆发送到所述rsu；处理模块，用于基于所述车辆信息和所述位置信息经融合定位算法确定所述车辆的定位结果；
第一发送模块，用于经所述rsu将所述定位结果广播给该车辆。8.一种融合定位装置，其特征在于，该装置应用于路侧单元rsu，包括：第二发送模块，用于上报车辆发送的车载卫星定位设备获得的位置信息到路侧计算单元，用于所述路侧计算单元基于路侧部署的毫米波雷达传输的车辆信息和所述位置信息经融合定位算法确定所述车辆的定位结果；还用于将所述定位结果广播给该车辆；第二接收模块，用于接收所述路侧计算单元发送的所述定位结果。9.一种融合定位系统，其特征在于，该系统包括：若干个路侧部署的毫米波雷达、若干个路侧单元rsu、路口部署的一个或多个路侧计算单元；其中，所述毫米波雷达，用于将感知到的车辆信息传输给所述路侧计算单元；所述路侧计算单元，用于接收路侧部署的毫米波雷达传输的车辆信息；接收路侧单元rsu传输的位置信息；所述位置信息为：车载卫星定位设备获得并由车辆发送到所述rsu；基于所述车辆信息和所述位置信息经融合定位算法确定所述车辆的定位结果，并经所述rsu将所述定位结果广播给该车辆；所述rsu，用于上报车辆发送的车载卫星定位设备获得的位置信息到路侧计算单元；接收所述路侧计算单元发送的所述定位结果，并将所述定位结果广播给该车辆。10.一种融合定位装置，其特征在于，该装置包括：处理器和用于存储能够在处理器上运行的计算机程序的存储器，其中，所述处理器用于运行所述计算机程序时，执行权利要求1-5中任一项所述方法的步骤、或执行权利要求6所述方法的步骤。11.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该计算机程序被处理器执行时实现权利要求1-5中任一项所述方法的步骤、或实现权利要求6所述方法的步骤。

技术总结
本发明实施例提供了一种融合定位方法、装置、系统和计算机可读存储介质，所述方法包括：接收路侧部署的毫米波雷达感知并传输的车辆信息；接收路侧单元RSU传输的位置信息；所述位置信息为：车载卫星定位设备获得并由车辆发送到所述RSU；基于所述车辆信息和所述位置信息经融合定位算法确定所述车辆的定位结果，并经所述RSU将所述定位结果广播给该车辆。所述RSU将所述定位结果广播给该车辆。所述RSU将所述定位结果广播给该车辆。


技术研发人员：沈旭 孙天齐
受保护的技术使用者：中国移动通信集团有限公司
技术研发日：2022.01.18
技术公布日：2023/8/1