CN113899373B - 初始化定位方法、装置、交通工具及存储介质 - Google Patents

初始化定位方法、装置、交通工具及存储介质 Download PDF

Info

Publication number
CN113899373B
CN113899373B CN202111166373.2A CN202111166373A CN113899373B CN 113899373 B CN113899373 B CN 113899373B CN 202111166373 A CN202111166373 A CN 202111166373A CN 113899373 B CN113899373 B CN 113899373B
Authority
CN
China
Prior art keywords
pose information
value
proportion
pose
positioning
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202111166373.2A
Other languages
English (en)
Other versions
CN113899373A (zh
Inventor
蓝锦山
王志谦
徐迅
韩旭
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Guangzhou Weride Technology Co Ltd
Original Assignee
Guangzhou Weride Technology Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Guangzhou Weride Technology Co Ltd filed Critical Guangzhou Weride Technology Co Ltd
Priority to CN202111166373.2A priority Critical patent/CN113899373B/zh
Publication of CN113899373A publication Critical patent/CN113899373A/zh
Application granted granted Critical
Publication of CN113899373B publication Critical patent/CN113899373B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
    • G01S19/485—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an optical system or imaging system

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Automation & Control Theory (AREA)
  • Electromagnetism (AREA)
  • Navigation (AREA)

Abstract

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

Description

初始化定位方法、装置、交通工具及存储介质
技术领域
本发明属于交通工具定位技术领域，涉及初始化定位方法、初始化定信装置、交通工具及存储介质。
背景技术
初始化定位是自动驾驶车辆的关键技术，高精定位模块是L4级自动驾驶车辆的一个不可缺少重要模块，一般通过EKF融合多种传感器数据获得自动驾驶车辆精确的位姿，进而为后续的定位、感知、规划等模块提供参考数据。
自动驾驶车辆在静止启动时，由于车辆处于静止或存在GPS信号缺失情况，无法通过常规的EKF获得可靠的初始的位姿，如果初始的位姿不够精确，可能会导致后续EKF(Extended Kalman Filter的缩写，扩展卡尔曼滤波)融合不收敛，以致定位跑飞，对自动驾驶造成极大的安全隐患。因此需要一种自动驾驶车辆初始化定位方法，在在静止启动时来进行鲁棒的初始化定位。
发明内容
本发明的目的旨在提供一种初始化定位方法，解决交通工具在静止启动阶段时进行初始化定位不精确的技术问题，通过该方法可以获得可靠的初始化位姿信息。
本发明是通过如下的技术方案来实现的。
本发明的第一个技术方案是提供一种初始化定位方法，包括如下步骤：
获得第一位姿信息；
以所述第一位姿信息为中心进行采样，获得多个第二位姿信息；
将所述第一位姿信息及所述第二位姿信息与地图进行配准，得到配准后的第三位姿信息；
对所述第三位姿信息进行聚类得到第四位姿信息；
对所述第四位姿信息进行判断得到第五位姿信息。
本技术方案的初始化定位方法可在室内外环境或定位设备信号缺失情况下，如地下车库无信号情况下，实现并应用于自动驾驶车辆从静止启动时，进行可靠的初始化定位，以得到的第五位姿信息作为初始化位姿信息。
在该技术方案一个实施例中，所述对所述第三位姿信息进行聚类得到第四位姿信息，包括如下步骤：
使用DBSCAN聚类算法对所述第三位姿信息进行三维空间聚类得到第四位姿信息，所述第四位姿信息包括优胜类位姿信息和次优类位姿信息。
进一步的，所述对所述第四位姿信息进行判断得到第五位姿信息，包括如下步骤：
步骤A1：计算所述优胜类位姿信息的比例，得到第一比例值；计算次优类位姿信息的比例，得到第二比例值；用所述第一比例值减去所述第二比例值，得到比例差值；判断所述比例差值是否大于第一阈值，如果大于，则执行步骤A2；
步骤A2：对所述优胜类位姿信息的进行方差计算，得到方差值；判断所述方差值是否小于第二阈值，如果小于，则执行步骤A3；
步骤A3：对所述优胜类位姿信息的进行平均计算，得到平均值；判断所述平均值是否小于第三阈值，如果小于，则执行步骤A4；
步骤A4：判断第一比例值是否大于第四阈值，如果大于，则执行步骤A5；
步骤A5：从所述优胜类位姿信息中选择一个与所述平均值的距离最近的位姿信息作为第五位姿信息。
在该技术方案一个实施例中，所述获得第一位姿信息，包括以下步骤：
查找保存位姿信息的本地文件；
加载所述本地文件，以最后记录的位姿信息作为第一位姿信息。
进一步的，还包括：
在查找不到所述保存位姿信息的本地文件后，获取GNSS定位设备检测的位置信息；
利用所述位置信息合成位姿信息作为第一位姿信息。
在该技术方案一个实施例中，以所述第一位姿信息为中心进行采样的采样方法为高斯采样或均匀采样。
在该技术方案一个实施例中，将所述第一位姿信息及所述第二位姿信息与地图进行配准的方法为正态分布变换算法或迭代最近点算法。
本发明的另一技术方案是提供一种初始化定位装置，包括获取模块、采样模块、配准模块、聚类模块和判断模块，其中：
获取模块用于获得第一位姿信息；
采样模块用于以所述第一位姿信息为中心进行采样，获得多个第二位姿信息；
配准模块用于将所述第一位姿信息及所述第二位姿信息与地图进行配准，得到配准后的第三位姿信息；
聚类模块用于对所述第三位姿信息进行聚类得到第四位姿信息；
判断模块用于对所述第四位姿信息进行判断得到第五位姿信息。
本发明的第三技术方案是一种交通工具，其特征在于，所述交通工具包括：存储器、处理器及存储在所述存储器上并可在所述处理器上运行的初始化定位程序，所述初始化定位程序配置为实现如上文所述初始化定位方法的步骤。
本发明的第四个技术方案是提供一种计算机可读存储介质，计算机可读存储介质上存储有计算机程序，计算机程序被处理器执行时实现如上文所述的初始化定位方法的步骤。
应当理解的是，以上的一般描述和后文的细节描述仅是示例性和解释性的，并不能限制本申请。
附图说明
通过结合附图对本申请示例性实施方式进行更详细的描述，本申请的上述以及其它目的、特征和优势将变得更加明显，其中，在本申请示例性实施方式中，相同的参考标号通常代表相同部件。
图1是第一实施例中示出的初始化定位方法的流程示意图。
图2是第一实施例中示出的获取第一位姿信息的流程示意图。
图3是第一实施例中示出的获取第五位姿信息的流程示意图。
图4是第一实施例中示出的第二位姿信息对应粒子的分布示意图。
图5是第一实施例中示出的第二位姿信息对应粒子的分布示意图。
图6是第二实施例中示出的初始化定位装置的结构示意图。
图7是本发明各个实施例方案涉及的硬件运行环境的交通工具结构示意图。
本发明目的的实现、功能特点及优点将结合实施例，参照附图做进一步说明。
具体实施方式
下面将参照附图更详细地描述本申请的优选实施方式。虽然附图中显示了本申请的优选实施方式，然而应该理解，可以以各种形式实现本申请而不应被这里阐述的实施方式所限制。相反，提供这些实施方式是为了使本申请更加透彻和完整，并且能够将本申请的范围完整地传达给本领域的技术人员。
在自动驾驶车辆从静止启动时，需要通过常规的EKF获得可靠的初始化位姿信息，现有技术的初始化定位一般都需要车辆有GPS信号情况才能进行，在GPS信号不好或缺失的情况，初始化后的位姿信息不够精确或不能完成初始化定位，这会对自动驾驶造成极大的安全隐患。
针对现在技术存在的问题，本发明的技术方案是提供一种初始化定位方法、装置、交通工具及存储介质，解决自动驾驶车辆不能初始化定位或定位不精确的问题，能够适用于室内外环境，即使是在GPS信号缺失情况下，也可以实现自动驾驶车辆从静止启动时，进行可靠的初始化定位。
以下结合附图详细描述本发明的技术方案的各个实施例。
如图1中所示，本发明的第一实施例是一种初始化定位方法，包括如下步骤101-105：
101、获得第一位姿信息；
需要说明的是，在自动驾使车辆通电后，先是获取第一位姿信息，第一位姿信息作为初始化定位的基础，也可称之为先验位姿信息，第一位姿信息包含有位置坐标信息(x,y,z)和姿态角(roll，pitch，yaw)，在本实施例中，自动驾驶车辆出于计算量的考虑，第一位姿信息中包含位置坐标信息(x,y)和姿态角中的偏航角yaw，第一位姿信息可以记为P0＝(X0,Y0,YAW0)。
102、以第一位姿信息为中心进行采样，获得多个第二位姿信息；
需要说明的是，在本实施例中，仅对X,Y,YAW三个维度进行采样。以P0的三个维度值X0,Y0,YAW0为中心，分别进行采样，采样方法为高斯采样，从采样的粒子中,可以得到r个x维度的值有X1，…，Xr和得到s个y维度的值有Y1，…，Ys以及得到t个yaw维度的值有YAW1，…，YAWt。基于得到地这些值通过进行组合可得到k个第二位姿信息，记为P1，…，Pk,其中k＝r×s×t。至此，算上第一位姿信息P0就可以得到k+1个信姿信息。
本实施例中出于计算量考虑，仅仅对x、y、yaw三个维度进行采样，若算力足够，可对x、y、z、roll、pitch、yaw维度都进行采样，此外，采样方法还可选择均匀采样。由于高斯采样和均匀采样的原理属于本领域的基本知识，对它们的原理及过程在此不再赘述。
103、将第一位姿信息及第二位姿信息与地图进行配准，得到配准后的第三位姿信息；
需要说明的是，正态分布变换(Normal Distribution Transform,NDT)算法是一种常用的几何匹配算法，NDT对局部点的统计特性进行了表征，将点云分割为栅格，每一个栅格都可以用正态分布表示，并且计算每一个栅格中点的均值和方差，通过对正态分布的匹配算法来匹配点集。NDT对点集进行了抽象，并不需要逐点找对应点，但它本质上仍是基于最近点假设，仍然容易陷入局部极小值。
本实施例采用NDT匹配算法进行配准，对于第一位姿信息P0对应的粒子及第二位姿信息Pk对应的粒子，以当前帧点云和周围高精度地图进行点云配准，点云配准采用正态分布变换算法，并以P0与Pk作为NDT算法匹配的初值，通过NDT算法匹配可得到配准后的第三位姿信息Qi，其中i取0至k。为了加速匹配，可以采用多线程并行技术，让多个NDT算法匹配同时进行。
在本实施例中，还可选用其他点云匹配方法，例如迭代最近点(IterativeClosest Point，ICP)算法，采用迭代最近点算法，以P0与Pk作为ICP算法匹配的初值，通过ICP算法匹配同样可得到配准后的第三位姿信息Qi。由于正态分布变换算法和迭代最近点算法的算法原理属于本领域的基本知识，对它们的算法原理及过程在此不再赘述。
104、对第三位姿信息进行聚类得到第四位姿信息；
需要说明的是，NDT算法点云配准依赖环境的特征，由于初值不同，离真实位姿信息较远的初始Pi可能会收敛到一个错误的位置Qi，但由于粒子是在P0附近采样得到的，只要P0离当前真实位姿不远大部分初始位姿粒子Pi在NDT算法配准后能收敛到真实的位姿附近。P0是自动驾驶系统关闭时保存的车辆位姿位息，但系统停止后可能由于惯性或人为等原因，导致车辆移动了，此时真实位置位息和P0存在偏差。
对Q0,…,Qk的x、y、z进行三维空间聚类，聚类方法采用DBSCAN(Density-BasedSpatial Clustering of Applications with Noise)聚类算法，DBSCAN聚类算法是一个比较有代表性的基于密度的聚类算法。与划分和层次聚类方法不同，它将簇定义为密度相连的点的最大集合，能够把具有足够高密度的区域划分为簇，并可在噪声的空间数据库中发现任意形状的聚类。
第三位姿信息Qi通过聚类算法可以得到第四位姿信息，第四位姿信息包含有m个优胜类位姿信息Q1,…,Qm和n个次优类位姿信息Q1,…,Qn。
本实施例中采用DBSCAN聚类算法，在其它实施例中，还可以采用其他的聚类方法，如K-means聚类算法等。
105、对第四位姿信息进行判断得到第五位姿信息。
需要说明的是，对于获得的第四位位姿信息，通过判断是否符合要求，在经过判断确定符合后，从第四位位姿信息的优胜类位姿信息当中选择一项作为第五位姿信息，第五位姿信息作为初始化位姿信息输出。
需要说明的是，如果没有通过本步骤中的判断，认定初始化定位失败，通过重新获得第一位姿信息P0，并且重复以上步骤101至105再次进行初始化定位。
如图2中所示，在本实施例中，步骤101、获得第一位姿信息，包括如下步骤，
201、查找保存位姿信息的本地文件；
202、加载本地文件，以最后记录的位姿信息作为第一位姿信息。
需要说明的是，在自动驾驶运行过程中，通过车辆定位系统可得到实时精确的位姿信息，将当前精确的定位位姿信息会以一定的频率(例如1Hz)记录在本地Pose文件中，由于上次熄火时已经将停车时的位姿信息保存在本地Pose文件中，可加载该本地文件中记录的最后一项位姿信息作为初始的第一位姿信息P0。
如图2中所示，在本实施例中，步骤101、获得第一位姿信息，进一步的还包括如下步骤203-204：
203、在查找不到保存位姿信息的本地文件后，获取GNSS定位设备检测的位置信息；
204、利用位置信息合成位姿信息作为第一位姿信息。
需要说明的是，若本地查找没有Pose文件的请况下，如果当前信号较好，可通过GNSS定位设备获得位置坐标信息，以及能过车辆上的两个天线获得自动驾驶车辆的姿态角，即偏航角YAW，通过GNSS定位设备获得的位置坐标信息和两个天线获得的姿态角，即可以合成初始化定位前的第一位姿信息P0。
如图3中所示，在本实施例中，步骤105、对第四位姿信息进行判断得到第五位姿信息，包括有如下步骤301-305：
301、计算优胜类位姿信息的比例，得到第一比例值；计算次优类位姿信息的比例，得到第二比例值；用第一比例值减去第二比例值，得到比例差值；判断比例差值是否大于第一阈值，如果大于，则执行步骤302。
需要说明的是，在本实施例中，第一比例值为第二比例值为
如果比例差值/>大于预选设定的第一阈值，则认为优胜类位姿信息有效，否则优胜类位姿信息和次优类位姿信息之间相差不大，存在较大风险,无法获取第五位姿信息。
302、对优胜类位姿信息的进行方差计算，得到方差值；判断方差值是否小于第二阈值，如果小于，则执行步骤303。
对优胜类位姿信息Q1,…,Qm进行方差计算，得到方差值std。判断方差值std是否小于预先设定的第二阈值，如果方差值std较大并且超过第二阈值，说明优胜类位姿信息Q1,…,Qm的结果不集中，例如呈一字型紧密排列情况，存在较大风险,无法获取第五位姿信息。
303、对优胜类位姿信息的进行平均计算，得到平均值；判断平均值是否小于第三阈值，如果小于，则执行步骤304。
需要说明的是，判断优胜类位姿信息Q1,…,Qm的平均值是否离P0较近,即平均值是否小于预先设定的第三阈值；如果较远则超过第三阈值，会导致结果不可靠，存在较大风险,无法获取第五位姿信息。
304、判断第一比例值是否大于第四阈值，如果大于，则执行步骤305；
只有在第一比例值大于第四阈值时，才能认为有足够数量的第四位姿信息收敛到了真实的位姿信息。
305、从优胜类位姿信息中选择一个与平均值的距离最近的位姿信息作为第五位姿信息。
需要说明的是，距离最近是指在三维空间上的直线距离最近，即计算所有优胜类位姿信息与平均值之间的距离，取最近的一个优胜类位姿信息作为第五位姿信息，当存在多个距离最近的优胜类位姿信息时，从中随机一个距离最近的位姿信息作为第五位姿信息。
需要说明的是，通过步骤301-304的四重判断，就可以从优胜类位姿信息中挑选一个离平均值最近的一个位姿信息，作为定位后的最终精确的初始化位姿信息输出了，至此，初始化定位成功。
如图4中所示，本实施例的其中一个初始化定位过程中的第二位姿信息对应粒子的分布示意图可以看到，中心为第一位姿信息的粒子，几乎所有采样的粒子都收敛到中心位置，说明初始化定位成功，并且自动驾驶车辆当前的真实位姿信息和本地pose文件记录的先验位姿信息基本上一致，可判断从上次熄火到本次启动，车子没有发生移动。
如图5中所示，本实施例的一个初始化定位过程中的第二位姿信息对应粒子的分布示意图可以看出，大部分采样的粒子收敛到左上角位置，自动驾驶车辆当前的真实位姿信息，特别是左上角收敛的位置和本地pose文件记录的第一位姿信息存在偏差，可判断从上次熄火到本次启动，车子发生了移动。如果该位置偏差小于设定的阈值，则认定初始化成功。
本发明在自动驾驶车辆运行过程中以一定频率保存当前定位位姿到本地文件中，在车辆熄火后再次静止启动时，会利用保存在文件中的先验位姿作为初值p0，然后以p0为中心，对位置和姿态进行采样，得到N个初值粒子p1…pN；将当前激光雷达点云帧和高精地图做NDT匹配，并分别以P0…pN作为NDT匹配的初值，可得到N+1个匹配后的位姿q0…qN，分别对q0…qN的位置(x,y,z)做三维空间聚类，得到最优类别和次优类别，如果属于最优类别的粒子比例远大于属于次优类别的粒子比例，说明属于最优类别的位姿有效；然后对属于最优类别的位姿，分别计算它们位置(x,y,z)和yaw角的均值和方差；若位置和yaw角的方差都较小，且位置均值和初值p0的偏差在设定的距离阈值内，则认为得到了精确的位姿，定位初始化成功。本发明可以在室内外环境或GPS信号缺失情况下(如地下车库)进行可靠的定位初始化，提高了自动驾驶的可靠性。
如图6所示，本发明的第二实施例是一种初始化定位装置，初始化定位装置包括：获取模块601，采样模块602，配准模块603，聚类模块604和判断模块605；其中，
获取模块601，用于获得第一位姿信息；
采样模块602，用于以第一位姿信息为中心进行采样，获得多个第二位姿信息；
配准模块603，用于将第一位姿信息及第二位姿信息与地图进行配准，得到配准后的第三位姿信息；
聚类模块604，用于对第三位姿信息进行聚类得到第四位姿信息；
判断模块605，用于对第四位姿信息进行判断得到第五位姿信息。
在本实施例中，获取模块601执行获得第一位姿信息的功能时，具体用于，查找保存位姿信息的本地文件；加载本地文件，以最后记录的位姿信息作为第一位姿信息。
进一步的，还具体用于，在查找不到保存位姿信息的本地文件后，获取GNSS定位设备检测的位置信息；利用位置信息合成位姿信息作为第一位姿信息。
在本实施例，判断模块605执行对第四位姿信息进行判断得到第五位姿信息的功能时，具体用于：计算优胜类位姿信息的比例，得到第一比例值；计算次优类位姿信息的比例，得到第二比例值；用第一比例值减去第二比例值，得到比例差值；判断比例差值是否大于第一阈值，如果大于，则，
对优胜类位姿信息的进行方差计算，得到方差值；判断方差值是否小于第二阈值，如果小于，则，
对优胜类位姿信息的进行平均计算，得到平均值；判断平均值是否小于第三阈值，如果小于，则，
判断第一比例值是否大于第四阈值，如果大于，则
从优胜类位姿信息中选择一个与平均值的距离最近的位姿信息作为第五位姿信息。
需要说明的是，上述装置中的各模块可用于实现上述方法中的各个步骤，同时达到相应的技术效果，本实施例在此不再赘述。
本实施例的初始化定位装置应用到自动驾驶车辆中，在自动驾驶车辆运行过程中以一定频率保存当前定位位姿到本地文件中，在车辆熄火后再次静止启动时，会利用保存在文件中的先验位姿作为初值P0，然后以P0为中心，对位置和姿态进行采样，得到k个初值粒子P0、…、Pk；将当前激光雷达点云帧和高精地图做NDT算法匹配，并分别以P0、…、Pk作为NDT算法匹配的初值，可得到k+1个匹配后的位姿Q0、…、Qk，分别对Q0、…、Qk的位置(x,y,z)做三维空间聚类，得到最优类别和次优类别，如果属于最优类别的粒子比例远大于属于次优类别的粒子比例，说明属于最优类别的位姿有效；然后对属于最优类别的位姿，分别计算它们位置(x,y,z)和yaw角的均值和方差；若位置和yaw角的方差都较小，且位置均值和初值P0的偏差在设定的距离阈值内，则认为得到了精确的位姿，定位初始化成功。本发明可以在室内外环境或GPS信号缺失情况下(如地下车库)进行可靠的定位初始化，提高了自动驾驶的可靠性。
图7为本发明各个实施例方案涉及的硬件运行环境的交通工具的结构示意图。如图7所示，本发明的第三个实施例是一种交通工具，该交通工具可以包括：处理器1001，例如CPU，通信总线1002、用户接口1003，网络接口1004，存储器1005。其中，通信总线1002用于实现这些组件之间的连接通信。用户接口1003可以包括显示屏(Display)、输入单元比如键盘(Keyboard)，可选用户接口1003还可以包括标准的有线接口、无线接口。网络接口1004可选的可以包括标准的有线接口、无线接口(如WI-FI、4G、5G接口)。存储器1005可以是高速RAM存储器，也可以是稳定的存储器(non-volatile memory)，例如磁盘存储器。存储器1005可选的还可以是独立于前述处理器1001的存储装置。
本领域技术人员可以理解，图7中示出的结构并不构成对交通工具的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件布置。
如图7所示，作为一种计算机存储介质的存储器1005中可以包括操作系统、网络通信模块、用户接口模块以及初始化定位程序。
在图7所示的交通工具中，网络接口1004主要用于与外部网络进行数据通信；用户接口1003主要用于接收用户的输入指令；交通工具通过处理器1001调用存储器1005中存储的初始化定位程序，并执行以下操作：
获得第一位姿信息；
以第一位姿信息为中心进行采样，获得多个第二位姿信息；
将第一位姿信息及第二位姿信息与地图进行配准，得到配准后的第三位姿信息；
对第三位姿信息进行聚类得到第四位姿信息；
对第四位姿信息进行判断得到第五位姿信息。
本实施例的交通工具可在室内外环境或定位设备信号缺失情况下，如地下车库无信号情况下，实现并应用于自动驾驶交通工具从静止启动时，进行可靠的初始化定位，以得到的第五位姿信息作为初始化位姿信息。
在本实施例中，对第三位姿信息进行聚类得到第四位姿信息，包括如下步骤：
使用DBSCAN聚类算法对第三位姿信息进行三维空间聚类得到第四位姿信息，第四位姿信息包括优胜类位姿信息和次优类位姿信息。
在本实施例中，对第四位姿信息进行判断得到第五位姿信息，包括如下步骤：
步骤A1：计算优胜类位姿信息的比例，得到第一比例值；计算次优类位姿信息的比例，得到第二比例值；用第一比例值减去第二比例值，得到比例差值；判断比例差值是否大于第一阈值，如果大于，则执行步骤A2；
步骤A2：对优胜类位姿信息的进行方差计算，得到方差值；判断方差值是否小于第二阈值，如果小于，则执行步骤A3；
步骤A3：对优胜类位姿信息的进行平均计算，得到平均值；判断平均值是否小于第三阈值，如果小于，则执行步骤A4；
步骤A4：判断第一比例值是否大于第四阈值，如果大于，则执行步骤A5；
步骤A5：从优胜类位姿信息中选择一个与平均值的距离最近的位姿信息作为第五位姿信息。
在本实施例中，获得第一位姿信息，包括以下步骤：
查找保存位姿信息的本地文件；
加载本地文件，以最后记录的位姿信息作为第一位姿信息。
进一步的，获得第一位姿信息，还包括以下步骤：
在查找不到保存位姿信息的本地文件后，获取GNSS定位设备检测的位置信息；
利用位置信息合成位姿信息作为第一位姿信息。
在本实施例中，以第一位姿信息为中心进行采样的采样方法为高斯采样或均匀采样。
在本实施例中，将第一位姿信息及第二位姿信息与地图进行配准的方法为正态分布变换算法或迭代最近点算法。
需要说明的是，本实施例中的各模块可用于实现第一实施例的方法中的各个步骤，同时达到相应的技术效果，各步骤的详细说明请参考第一实施例，本实施例在此不再赘述。
以自动驾驶车辆作为本实施例的交通工具的一个具体示例，在自动驾驶车辆运行过程中以一定频率保存当前定位位姿到本地文件中，在车辆熄火后再次静止启动时，会利用保存在文件中的先验位姿作为初值P0，然后以P0为中心，对位置和姿态进行采样，得到k个初值粒子P0、…、Pk；将当前激光雷达点云帧和高精地图做NDT算法匹配，并分别以P0、…、Pk作为NDT算法匹配的初值，可得到k+1个匹配后的位姿Q0、…、Qk，分别对Q0、…、Qk的位置(x,y,z)做三维空间聚类，得到最优类别和次优类别，如果属于最优类别的粒子比例远大于属于次优类别的粒子比例，说明属于最优类别的位姿有效；然后对属于最优类别的位姿，分别计算它们位置(x,y,z)和yaw角的均值和方差；若位置和yaw角的方差都较小，且位置均值和初值P0的偏差在设定的距离阈值内，则认为得到了精确的位姿，定位初始化成功。本发明可以在室内外环境或GPS信号缺失情况下(如地下车库)进行可靠的定位初始化，提高了自动驾驶的可靠性。
此外，本发明第四个实施例还提出一种计算机可读存储介质，计算机可读存储介质上存储有初始化定位程序，初始化定位程序被处理器执行时实现如下操作：
获得第一位姿信息；
以第一位姿信息为中心进行采样，获得多个第二位姿信息；
将第一位姿信息及第二位姿信息与地图进行配准，得到配准后的第三位姿信息；
对第三位姿信息进行聚类得到第四位姿信息；
对第四位姿信息进行判断得到第五位姿信息。
本实施例的可读存储介质上存储的初始化定位程序被处理器执行时，可在室内外环境或定位设备信号缺失情况下，如地下车库无信号情况下，实现并应用于自动驾驶车辆从静止启动时，进行可靠的初始化定位，以得到的第五位姿信息作为初始化位姿信息。
在本实施例中，对第三位姿信息进行聚类得到第四位姿信息，包括如下步骤：
使用DBSCAN聚类算法对第三位姿信息进行三维空间聚类得到第四位姿信息，第四位姿信息包括优胜类位姿信息和次优类位姿信息。
进一步的，对第四位姿信息进行判断得到第五位姿信息，包括如下步骤：
步骤A1：计算优胜类位姿信息的比例，得到第一比例值；计算次优类位姿信息的比例，得到第二比例值；用第一比例值减去第二比例值，得到比例差值；判断比例差值是否大于第一阈值，如果大于，则执行步骤A2；
步骤A2：对优胜类位姿信息的进行方差计算，得到方差值；判断方差值是否小于第二阈值，如果小于，则执行步骤A3；
步骤A3：对优胜类位姿信息的进行平均计算，得到平均值；判断平均值是否小于第三阈值，如果小于，则执行步骤A4；
步骤A4：判断第一比例值是否大于第四阈值，如果大于，则执行步骤A5；
步骤A5：从优胜类位姿信息中选择一个与平均值的距离最近的位姿信息作为第五位姿信息。
在本实施例中，获得第一位姿信息，包括以下步骤：
查找保存位姿信息的本地文件；
加载本地文件，以最后记录的位姿信息作为第一位姿信息。
进一步的，还包括：
在查找不到保存位姿信息的本地文件后，获取GNSS定位设备检测的位置信息；
利用位置信息合成位姿信息作为第一位姿信息。
在本实施例中，以第一位姿信息为中心进行采样的采样方法为高斯采样或均匀采样。
在本实施例中，将第一位姿信息及第二位姿信息与地图进行配准的方法为正态分布变换算法或迭代最近点算法。
需要说明的是，本实施例中的各模块可用于实现第一实施例的方法中的各个步骤，同时达到相应的技术效果，各步骤的详细说明请参考第一实施例，本实施例在此不再赘述。
本实施例的计算机可读存储介质应用到自动驾驶车辆上，存储介质存储的初始化定位程序在被处理器执行时，自动驾驶车辆运行过程中以一定频率保存当前定位位姿到本地文件中，在车辆熄火后再次静止启动时，会利用保存在文件中的先验位姿作为初值P0，然后以P0为中心，对位置和姿态进行采样，得到k个初值粒子P0、…、Pk；将当前激光雷达点云帧和高精地图做NDT算法匹配，并分别以P0、…、Pk作为NDT算法匹配的初值，可得到k+1个匹配后的位姿Q0、…、Qk，分别对Q0、…、Qk的位置(x,y,z)做三维空间聚类，得到最优类别和次优类别，如果属于最优类别的粒子比例远大于属于次优类别的粒子比例，说明属于最优类别的位姿有效；然后对属于最优类别的位姿，分别计算它们位置(x,y,z)和yaw角的均值和方差；若位置和yaw角的方差都较小，且位置均值和初值P0的偏差在设定的距离阈值内，则认为得到了精确的位姿，定位初始化成功。本发明可以在室内外环境或GPS信号缺失情况下(如地下车库)进行可靠的定位初始化，提高了自动驾驶的可靠性。
需要说明的是，在本文中，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者系统不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者系统所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括该要素的过程、方法、物品或者系统中还存在另外的相同要素。
上述本发明实施例序号仅仅为了描述，不代表实施例的优劣。
通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到上述实施例方法可借助软件加必需的通用硬件平台的方式来实现，当然也可以通过硬件，但很多情况下前者是更佳的实施方式。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品存储在如上的一个存储介质(如ROM/RAM、磁碟、光盘)中，包括若干指令用以使得一台终端设备(可以是手机，计算机，服务器，控制器，或者网络设备等)执行本发明各个实施例的方法。
以上仅为本发明的优选实施例，并非因此限制本发明的专利范围，凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本发明的专利保护范围内。

Claims (9)

1.一种初始化定位方法，其特征在于，包括如下步骤：
获得第一位姿信息；
以所述第一位姿信息为中心进行采样，获得多个第二位姿信息；
将所述第一位姿信息及所述第二位姿信息与地图进行配准，得到配准后的第三位姿信息；
对所述第三位姿信息进行聚类得到第四位姿信息；
对所述第四位姿信息进行判断得到第五位姿信息；
其中，所述第四位姿信息包括优胜类位姿信息和次优类位姿信息；所述对所述第四位姿信息进行判断得到第五位姿信息，包括如下步骤：
步骤A1：计算所述优胜类位姿信息的比例，得到第一比例值；计算次优类位姿信息的比例，得到第二比例值；用所述第一比例值减去所述第二比例值，得到比例差值；判断所述比例差值是否大于第一阈值，如果大于，则执行步骤A2；
步骤A2：对所述优胜类位姿信息的进行方差计算，得到方差值；判断所述方差值是否小于第二阈值，如果小于，则执行步骤A3；
步骤A3：对所述优胜类位姿信息的进行平均计算，得到平均值；判断所述平均值是否小于第三阈值，如果小于，则执行步骤A4；
步骤A4：判断第一比例值是否大于第四阈值，如果大于，则执行步骤A5；
步骤A5：从所述优胜类位姿信息中选择一个与所述平均值的距离最近的位姿信息作为第五位姿信息。
2.根据权利要求1所述的初始化定位方法，其特征在于，所述对所述第三位姿信息进行聚类得到第四位姿信息，包括如下步骤：
使用DBSCAN聚类算法对所述第三位姿信息进行三维空间聚类得到第四位姿信息。
3.根据权利要求1所述的初始化定位方法，其特征在于，所述获得第一位姿信息，包括以下步骤：
查找保存位姿信息的本地文件；
加载所述本地文件，以最后记录的位姿信息作为第一位姿信息。
4.根据权利要求3所述的初始化定位方法，其特征在于，还包括：
在查找不到所述保存位姿信息的本地文件后，获取GNSS定位设备检测的位置信息；
利用所述位置信息合成位姿信息作为第一位姿信息。
5.根据权利要求1所述的初始化定位方法，其特征在于，以所述第一位姿信息为中心进行采样的采样方法为高斯采样或均匀采样。
6.根据权利要求1所述的初始化定位方法，其特征在于，将所述第一位姿信息及所述第二位姿信息与地图进行配准的方法为正态分布变换算法或迭代最近点算法。
7.一种初始化定位装置，其特征在于，包括获取模块、采样模块、配准模块、聚类模块和判断模块，其中：
获取模块用于获得第一位姿信息；
采样模块用于以所述第一位姿信息为中心进行采样，获得多个第二位姿信息；
配准模块用于将所述第一位姿信息及所述第二位姿信息与地图进行配准，得到配准后的第三位姿信息；
聚类模块用于对所述第三位姿信息进行聚类得到第四位姿信息；
判断模块用于对所述第四位姿信息进行判断得到第五位姿信息；
其中，所述第四位姿信息包括优胜类位姿信息和次优类位姿信息；
其中，所述判断模块还用于：
计算所述优胜类位姿信息的比例，得到第一比例值；计算次优类位姿信息的比例，得到第二比例值；用所述第一比例值减去所述第二比例值，得到比例差值；判断所述比例差值是否大于第一阈值，如果大于，对所述优胜类位姿信息的进行方差计算，得到方差值；判断所述方差值是否小于第二阈值，如果小于，对所述优胜类位姿信息的进行平均计算，得到平均值；判断所述平均值是否小于第三阈值，如果小于，判断第一比例值是否大于第四阈值，如果大于，则从所述优胜类位姿信息中选择一个与所述平均值的距离最近的位姿信息作为第五位姿信息。
8.一种交通工具，其特征在于，所述交通工具包括：存储器、处理器及存储在所述存储器上并可在所述处理器上运行的初始化定位程序，所述初始化定位程序配置为实现根据权利要求1至6中任一项所述初始化定位方法的步骤。
9.一种计算机可读存储介质，其特征在于，计算机可读存储介质上存储有计算机程序，计算机程序被处理器执行时实现如权利要求1至6任一项所述的初始化定位方法的步骤。
CN202111166373.2A 2021-09-30 2021-09-30 初始化定位方法、装置、交通工具及存储介质 Active CN113899373B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111166373.2A CN113899373B (zh) 2021-09-30 2021-09-30 初始化定位方法、装置、交通工具及存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111166373.2A CN113899373B (zh) 2021-09-30 2021-09-30 初始化定位方法、装置、交通工具及存储介质

Publications (2)

Publication Number Publication Date
CN113899373A CN113899373A (zh) 2022-01-07
CN113899373B true CN113899373B (zh) 2024-04-23

Family

ID=79190077

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111166373.2A Active CN113899373B (zh) 2021-09-30 2021-09-30 初始化定位方法、装置、交通工具及存储介质

Country Status (1)

Country Link
CN (1) CN113899373B (zh)

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108225341A (zh) * 2016-12-14 2018-06-29 乐视汽车（北京）有限公司 车辆定位方法
CN108332758A (zh) * 2018-01-26 2018-07-27 上海思岚科技有限公司 一种移动机器人的走廊识别方法及装置
CN110148170A (zh) * 2018-08-31 2019-08-20 北京初速度科技有限公司 一种应用于车辆定位的定位初始化方法及车载终端
CN112051590A (zh) * 2020-08-31 2020-12-08 广州文远知行科技有限公司 激光雷达与惯性测量单元的检测方法及相关装置

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20160363647A1 (en) * 2015-06-15 2016-12-15 GM Global Technology Operations LLC Vehicle positioning in intersection using visual cues, stationary objects, and gps
EP3425339A4 (en) * 2016-03-01 2019-03-20 Ricoh Company, Ltd. POSITION ESTIMATING DEVICE, POSITION ESTIMATING METHOD, AND PROGRAM

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108225341A (zh) * 2016-12-14 2018-06-29 乐视汽车（北京）有限公司 车辆定位方法
CN108332758A (zh) * 2018-01-26 2018-07-27 上海思岚科技有限公司 一种移动机器人的走廊识别方法及装置
CN110148170A (zh) * 2018-08-31 2019-08-20 北京初速度科技有限公司 一种应用于车辆定位的定位初始化方法及车载终端
CN112051590A (zh) * 2020-08-31 2020-12-08 广州文远知行科技有限公司 激光雷达与惯性测量单元的检测方法及相关装置

Also Published As

Publication number Publication date
CN113899373A (zh) 2022-01-07

Similar Documents

Publication Publication Date Title
US11852751B2 (en) Method, apparatus, computing device and computer-readable storage medium for positioning
CN111812658B (zh) 位置确定方法、装置、系统和计算机可读存储介质
CN112179330B (zh) 移动设备的位姿确定方法及装置
US11725944B2 (en) Method, apparatus, computing device and computer-readable storage medium for positioning
US9244152B1 (en) Determining device locations using movement, signal strength
KR20190082070A (ko) 지도 생성 및 운동 객체 위치 결정 방법 및 장치
CN112639502A (zh) 机器人位姿估计
CN114111774B (zh) 车辆的定位方法、系统、设备及计算机可读存储介质
US11373328B2 (en) Method, device and storage medium for positioning object
CN116449392B (zh) 一种地图构建方法、装置、计算机设备及存储介质
CN110672102B (zh) 视觉辅助机器人初始化定位方法、机器人及可读存储介质
CN112154429B (zh) 高精度地图定位方法、系统、平台及计算机可读存储介质
CN113899373B (zh) 初始化定位方法、装置、交通工具及存储介质
CN112835086B (zh) 确定车辆位置的方法和装置
CN110794434B (zh) 一种位姿的确定方法、装置、设备及存储介质
US20230304802A1 (en) Passive combined indoor positioning system and method based on intelligent terminal sensor
US20240160222A1 (en) Method and system for localizing a mobile robot
CN113673288A (zh) 空闲车位检测方法、装置、计算机设备及存储介质
CN113885064B (zh) 双系统单频的北斗惯性导航定位方法、装置及存储介质
CN111665533A (zh) 基于卫星定位有效性的定位方法/系统、介质及设备
CN115963858A (zh) 无人机飞行控制方法、装置、设备及存储介质
CN115952248A (zh) 终端设备的位姿处理方法、装置、设备、介质及产品
CN108240807B (zh) 估计空间占据的方法
CN113687336A (zh) 一种雷达标定方法、装置、电子设备和介质
CN113810846A (zh) 一种基于WiFi和IMU融合的室内定位方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant