[发明专利]初始化定位方法、装置、交通工具及存储介质

说明书

本发明涉及一种初始化定位方法，包括如下步骤：获得第一位姿信息；以所述第一位姿信息为中心进行采样，获得多个第二位姿信息；将所述第一位姿信息及所述第二位姿信息与地图进行配准，得到配准后的第三位姿信息；对所述第三位姿信息进行聚类得到第四位姿信息；对所述第四位姿信息进行判断得到第五位姿信息。该方法可在室内外环境或定位设备信号缺失情况下，如地下车库无信号情况下，实现并应用于自动驾驶车辆从静止启动时，进行可靠的初始化定位，以得到的第五位姿信息作为初始化位姿信息。

技术领域

本发明属于交通工具定位技术领域，涉及初始化定位方法、初始化定信装置、交通工具及存储介质。

背景技术

初始化定位是自动驾驶车辆的关键技术，高精定位模块是L4级自动驾驶车辆的一个不可缺少重要模块，一般通过EKF融合多种传感器数据获得自动驾驶车辆精确的位姿，进而为后续的定位、感知、规划等模块提供参考数据。

自动驾驶车辆在静止启动时，由于车辆处于静止或存在GPS信号缺失情况，无法通过常规的EKF获得可靠的初始的位姿，如果初始的位姿不够精确，可能会导致后续EKF(Extended Kalman Filter的缩写，扩展卡尔曼滤波)融合不收敛，以致定位跑飞，对自动驾驶造成极大的安全隐患。因此需要一种自动驾驶车辆初始化定位方法，在在静止启动时来进行鲁棒的初始化定位。

发明内容

本发明的目的旨在提供一种初始化定位方法，解决交通工具在静止启动阶段时进行初始化定位不精确的技术问题，通过该方法可以获得可靠的初始化位姿信息。

本发明是通过如下的技术方案来实现的。

本发明的第一个技术方案是提供一种初始化定位方法，包括如下步骤：

获得第一位姿信息；

以所述第一位姿信息为中心进行采样，获得多个第二位姿信息；

将所述第一位姿信息及所述第二位姿信息与地图进行配准，得到配准后的第三位姿信息；

对所述第三位姿信息进行聚类得到第四位姿信息；

对所述第四位姿信息进行判断得到第五位姿信息。

本技术方案的初始化定位方法可在室内外环境或定位设备信号缺失情况下，如地下车库无信号情况下，实现并应用于自动驾驶车辆从静止启动时，进行可靠的初始化定位，以得到的第五位姿信息作为初始化位姿信息。

在该技术方案一个实施例中，所述对所述第三位姿信息进行聚类得到第四位姿信息，包括如下步骤：

使用DBSCAN聚类算法对所述第三位姿信息进行三维空间聚类得到第四位姿信息，所述第四位姿信息包括优胜类位姿信息和次优类位姿信息。

进一步的，所述对所述第四位姿信息进行判断得到第五位姿信息，包括如下步骤：

步骤A1：计算所述优胜类位姿信息的比例，得到第一比例值；计算次优类位姿信息的比例，得到第二比例值；用所述第一比例值减去所述第二比例值，得到比例差值；判断所述比例差值是否大于第一阈值，如果大于，则执行步骤A2；

步骤A2：，对所述优胜类位姿信息的进行方差计算，得到方差值；判断所述方差值是否小于第二阈值，如果小于，则执行步骤A3；

步骤A3：对所述优胜类位姿信息的进行平均计算，得到平均值；判断所述平均值是否小于第三阈值，如果小于，则执行步骤A4；

步骤A4：判断第一比例值是否大于第四阈值，如果大于，则执行步骤A5；

步骤A5：从所述优胜类位姿信息中选择一个与所述平均值的距离最近的位姿信息作为第五位姿信息。

在该技术方案一个实施例中，所述获得第一位姿信息，包括以下步骤：