CN113096181B - 设备位姿的确定方法、装置、存储介质及电子装置 - Google Patents
设备位姿的确定方法、装置、存储介质及电子装置 Download PDFInfo
- Publication number
- CN113096181B CN113096181B CN202010019371.XA CN202010019371A CN113096181B CN 113096181 B CN113096181 B CN 113096181B CN 202010019371 A CN202010019371 A CN 202010019371A CN 113096181 B CN113096181 B CN 113096181B
- Authority
- CN
- China
- Prior art keywords
- matching
- point cloud
- laser point
- target
- determining
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 43
- 238000003860 storage Methods 0.000 title claims abstract description 11
- 238000009826 distribution Methods 0.000 claims abstract description 10
- 230000009466 transformation Effects 0.000 claims abstract description 9
- 238000004590 computer program Methods 0.000 claims description 13
- 239000000758 substrate Substances 0.000 claims 1
- 238000004364 calculation method Methods 0.000 description 9
- 239000002245 particle Substances 0.000 description 9
- 238000000605 extraction Methods 0.000 description 5
- 230000008901 benefit Effects 0.000 description 4
- 238000004422 calculation algorithm Methods 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 3
- 230000005540 biological transmission Effects 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 230000007613 environmental effect Effects 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000003287 optical effect Effects 0.000 description 2
- 238000005070 sampling Methods 0.000 description 2
- 238000012512 characterization method Methods 0.000 description 1
- 230000003247 decreasing effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000001914 filtration Methods 0.000 description 1
- 230000006870 function Effects 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000009191 jumping Effects 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 230000008569 process Effects 0.000 description 1
- 238000004904 shortening Methods 0.000 description 1
- 238000013179 statistical model Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/70—Determining position or orientation of objects or cameras
- G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/10—Image acquisition modality
- G06T2207/10028—Range image; Depth image; 3D point clouds
Abstract
本发明提供了一种设备位姿的确定方法、装置、存储介质及电子装置，其中，该方法包括：从预先建立的地图中确定出初始关键帧；从所述地图中确定出包含目标位姿的候选关键帧，其中，所述目标位姿与所述初始关键帧中包含的初始位姿相差预定位姿域值；从所述候选关键帧中确定出目标关键帧，并对目标激光点云与第一激光点云进行匹配，以得到所述目标关键帧中包含的位姿与所述目标设备当前的位姿之间的相对位姿；以所述相对位姿为初始值，采用正态分布变换NDT对所述目标激光点云与所述第一激光点云进行匹配，进而得到所述目标设备当前的位姿。
Description
技术领域
本发明涉及通信领域，具体而言，涉及一种设备位姿的确定方法、装置、存储介质及电子装置。
背景技术
自主移动设备在移动作业前需要进行初始定位(即计算在地图中的位姿)，通常要求定位精度分米级，全天候作业，且要求尽量缩短定位时长、降低定位设备成本。
目前自主移动设备主要利用全球定位系统(Global Positioning System，简称为GPS)设备、差分GPS、相机、激光雷达等传感器实现定位。其中，GPS设备成本最低，但定位误差会在几米到几十米范围内，定位精度差，一般不采用。差分GPS在空旷地带精度可达厘米级，但在受到高大建筑物、树木等遮挡信号影响时定位误差同样会在几米到几十米范围内，所以需要首先将自主移动设备移动至空旷地带，其使用范围受限。相机图像受光线影响大，无法在夜晚工作。激光雷达具有测距精准、全天候作业等优点，应用前景广阔，目前基于激光雷达初始定位算法主要有正态分布变换(Normal Distributions Transform，简称为NDT)、迭代最近点(Iterative Closest Point，简称为ICP)等，该算法思路是匹配当前激光点云与地图点云，可以实现分米或厘米级定位精度，但单纯采用激光点云匹配存在耗时过长的问题。所以，通常需要通过人工指定或差分GPS进行粗定位(一般在真实位置周围几米范围内)，再将当前点云和局部地图点云进行逐帧遍历匹配。但人工指定方式增大了人力时间成本；差分GPS同样需要将车辆移动至空旷地带。
由此可知，在相关技术中存在着设备定位精度差，或者定位时间长，定位条件受限的问题。
针对相关技术中存在的上述问题，目前尚未提出有效的解决方案。
发明内容
本发明实施例提供了一种设备位姿的确定方法、装置、存储介质及电子装置，以至少解决相关技术中存在的设备定位精度差，或者定位时间长，定位条件受限的问题。
根据本发明的一个实施例，提供了一种设备位姿的确定方法，包括：从预先建立的地图中确定出初始关键帧，其中，所述初始关键帧包含有第一GPS位置，所述第一GPS位置与第一位置的距离小于预设距离阈值，所述第一位置为通过安装在目标设备上的GPS设备确定的所述目标设备的当前位置；从所述地图中确定出包含目标位姿的候选关键帧，其中，所述目标位姿与所述初始关键帧中包含的初始位姿相差预定位姿域值；从所述候选关键帧中确定出目标关键帧，并对目标激光点云与第一激光点云进行匹配，以得到所述目标关键帧中包含的位姿与所述目标设备当前的位姿之间的相对位姿，其中，所述目标激光点云包含在所述目标关键帧中，所述第一激光点云为利用位于所述目标设备上的激光雷达获取的所述目标设备当前所处的位置的激光点云；以所述相对位姿为初始值，采用正态分布变换NDT对所述目标激光点云与所述第一激光点云进行匹配，进而得到所述目标设备当前的位姿。
根据本发明的另一个实施例，提供了一种设备位姿的确定装置，包括：第一确定模块，用于从预先建立的地图中确定出初始关键帧，其中，所述初始关键帧包含有第一GPS位置，所述第一GPS位置与第一位置的距离小于预设距离阈值，所述第一位置为通过安装在目标设备上的GPS设备确定的所述目标设备的当前位置；第二确定模块，用于从所述地图中确定出包含目标位姿的候选关键帧，其中，所述目标位姿与所述初始关键帧中包含的初始位姿相差预定位姿域值；第一匹配模块，用于从所述候选关键帧中确定出目标关键帧，并对目标激光点云与第一激光点云进行匹配，以得到所述目标关键帧中包含的位姿与所述目标设备当前的位姿之间的相对位姿，其中，所述目标激光点云包含在所述目标关键帧中，所述第一激光点云为利用位于所述目标设备上的激光雷达获取的所述目标设备当前所处的位置的激光点云；第二匹配模块，用于以所述相对位姿为初始值，采用正态分布变换NDT对所述目标激光点云与所述第一激光点云进行匹配，进而得到所述目标设备当前的位姿。
根据本发明的又一个实施例，还提供了一种计算机可读存储介质，所述计算机可读存储介质中存储有计算机程序，其中，所述计算机程序被设置为运行时执行上述方法实施例中的步骤。
根据本发明的又一个实施例，还提供了一种电子装置，包括存储器和处理器，所述存储器中存储有计算机程序，所述处理器被设置为运行所述计算机程序以执行上述方法实施例中的步骤。
通过本发明，采用GPS粗定位以及NDT精定位实现了设备的精准定位，其定位操作不受环境影响，其中，仅利用激光雷达和GPS设备即可实现定位操作，降低了设备定位的成本，此外，该方法仅进行一次NDT计算，既保留了高定位精度优点，同时减小计算开销，有效解决相关技术中存在的设备定位精度差，或者定位时间长，定位条件受限的问题，达到提高设备定位精度，降低定位时间，且避免了定位条件受限的效果。
附图说明
此处所说明的附图用来提供对本发明的进一步理解，构成本申请的一部分，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：
图1是根据本发明实施例的设备位姿的确定方法的流程图；
图2是根据本发明实施例的定位方法整体流程图；
图3是根据本发明实施例的设备位姿的确定装置的结构框图。
具体实施方式
下文中将参考附图并结合实施例来详细说明本发明。需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。
需要说明的是，本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。
首先对相关技术中的设备定位方法进行说明：
(1)大多数相关专利介绍的方法主要基于图像，而基于图像进行定位时会存在无法全天候定位的缺点。
(2)在相关技术中还存在着基于激光雷达的定位方法，例如，在相关技术的一种方案中，会对激光模块扫描到的所述车辆周围环境的3D点云数据进行分析处理，得到环境角点特征参数数据，利用所述环境角点特征参数数据进行三维地图重建，得到三维地图，将所述三维地图与所述局部地图进行匹配，得到所述车辆的实际位置信息和航向角信息。但是该方法主要用于车辆已完成初始定位后运动过程中，不适合初始定位。在相关技术的另一种方案中，会使用车辆实时获取的局部点云与预先保存的地图点云进行定位，得到点云定位结果，所述地图点云的初始分辨率高于局部点云的初始分辨率。该方法从地图点云的采样范围中随机选择N个粒子作为匹配粒子，将每个匹配粒子与局部点云的粒子进行ICP匹配，得到每个匹配粒子的匹配估计系数，N根据地图点云在大地平面上的投影面积与当前地图点云的分辨率计算得到；根据匹配估计系数对所述匹配粒子通过粒子滤波得到稳定的有效匹配粒子；根据有效匹配粒子对地图点云进行更新并缩小采样范围；提高地图点云和局部点云分辨率，如果满足迭代退出条件则退出迭代匹配，确定点云定位结果，否则执行下一次迭代匹配.该方法的不足是纯粹靠激光雷达ICP计算，从全局地图开始搜索到局部范围，计算量大、耗时长。
由上述内容可知，相关技术中的方案实际上也无法解决，设备定位精度差，或者定位时间长，定位条件受限的问题。
下面对本发明实施例进行说明：
在本实施例中提供了一种设备位姿的确定方法，图1是根据本发明实施例的设备位姿的确定方法的流程图，如图1所示，该流程包括如下步骤：
步骤S102，从预先建立的地图中确定出初始关键帧，其中，所述初始关键帧包含有第一GPS位置，所述第一GPS位置与第一位置的距离小于预设距离阈值，所述第一位置为通过安装在目标设备上的GPS设备确定的所述目标设备的当前位置；
步骤S104，从所述地图中确定出包含目标位姿的候选关键帧，其中，所述目标位姿与所述初始关键帧中包含的初始位姿相差预定位姿域值；
步骤S106，从所述候选关键帧中确定出目标关键帧，并对目标激光点云与第一激光点云进行匹配，以得到所述目标关键帧中包含的位姿与所述目标设备当前的位姿之间的相对位姿，其中，所述目标激光点云包含在所述目标关键帧中，所述第一激光点云为利用位于所述目标设备上的激光雷达获取的所述目标设备当前所处的位置的激光点云；
步骤S108，以所述相对位姿为初始值，采用正态分布变换NDT对所述目标激光点云与所述第一激光点云进行匹配，进而得到所述目标设备当前的位姿。
其中，执行上述操作的可以是硬件处理器或具备处理能力的其他设备，该处理器可以位于上述设备内，或者独立于上述设备设置。上述的设备可以是具备室内和/或室外自主移动能力的设备，例如，机器人。上述的预先建立的地图中包括有多个关键帧，且各关键帧均包含有位姿，GPS位置和激光点云。在上述实施例中，在从候选关键帧中确定出目标关键帧时，可也利用智能启发方式(或称为启发式搜索Heuristically Search)来从候选关键帧中确定出目标关键帧，其中，上述的启发式搜索是指利用问题拥有的启发信息来引导搜索，达到减少搜索范围、降低问题复杂度的目的，这种利用启发信息的搜索过程称为启发式搜索。NDT是一种配准算法，它应用于三维点的统计模型，使用标准最优化技术来确定两个点云间的最优的匹配。在上述实施例中，在以所述相对位姿为初始值，采用正态分布变换NDT对所述目标激光点云与所述第一激光点云进行匹配之后会进一步得到一个相对位姿，将进一步得到的该相对位姿与目标位姿叠加之后可以得到目标设备的当前位姿。
在上述实施例中，可以利用上述设备中携带的GPS设备获取该设备当前GPS位置(对应于上述的第一位置)，该位置可以用GPScur表征；还可以利用设备上的激光雷达获取该设备当前周围环境的激光点云(对应于上述的第一激光点云)。其次，在先前建立的地图中查找距离GPScur最近的关键帧作为最近关键帧KFnearest(对应于上述的初始关键帧)。地图由一系列关键帧{KFi}组成，其中，第i关键帧KFi由位姿Ti、GPS位置GPSi、激光雷达点云Ci三部分构成。最后，将KFnearest的位姿作为粗定位结果，该位姿用c表示。其中，GPS设备的定位误差通常在几米到几十米范围内。用符号Emax表示最大误差，则当前自主移动装置(对应于上述设备)的真实位置T*在Tnearest(对应于上述的初始位姿)附近的Emax距离范围内，且距离Tnearest越远，可能性越小。其中，上述操作也可称为是基于GPS的粗定位，具体可参见附图2中“粗定位”阶段中的操作。
此外，在确定目标关键帧时，可以首先截取地图中处于距离Tnearest的Emax误差范围内的所有关键帧作为候选关键帧组Gcandidate＝{KFi/distance(Ti,Tnearest)≤Emax}，其次，采用智能启发方式从候选关键帧组Gcandidate中选择匹配关键帧，并进行匹配计算，直至选出最优关键帧(对应于上述的目标关键帧)。其中，上述操作也可称为是智能启发式搜索+关键特征提取匹配细定位，具体可参见附图2中“细定位”阶段中的操作。
在上述实施例中，采用GPS粗定位以及NDT精定位(具体可参见附图2中的“细定位”阶段中的操作)实现了设备的精准定位，其定位操作不受环境影响，其中，仅利用激光雷达和GPS设备即可实现定位操作，降低了设备定位的成本，此外，该方法仅进行一次NDT计算，既保留了高定位精度优点，同时减小计算开销，有效解决相关技术中存在的设备定位精度差，或者定位时间长，定位条件受限的问题，达到提高设备定位精度，降低定位时间，且避免了定位条件受限的效果。
在一个可选的实施例中，从所述候选关键帧中确定出目标关键帧包括：在所述地图中沿着远离所述初始位姿的预定方向上，按照预定间隔从所述候选关键帧中选择多个匹配关键帧；对所述第一激光点云与各个所述匹配关键帧中包含的激光点云分别进行匹配，以得到各个所述匹配关键帧的匹配分数；根据所述匹配分数，从所述匹配关键帧中确定出目标关键帧。可选地，在从所述匹配关键帧中确定出目标关键帧时，可以将匹配分数最高的匹配关键帧确定为所述目标关键帧。在本实施例中，可以沿着远离Tnearest方向上间隔选择若干候选关键帧作为匹配关键帧。例如：在正方向(该正方向可以表示构建地图时的移动方向，或者该正方向可以是指定的方向)上选择KFnearest、KFnearest+1、KFnearest+2、KFnearest+5关键帧，其中，1,2,5指代构建地图采点的步进值；在正方向上选择KFnearest、KFnearest-1、KFnearest-2、KFnearest-5帧。该方式既可以保持匹配得分趋势，又能减少所需匹配计算的关键帧数量，上述的选取关键帧的方式仅是一种可选的方式，在实际应用中还可以基于其他的间隔大小选取其他的关键帧，例如，选取KFnearest、KFnearest+2、KFnearest+4、KFnearest+6等。
在一个可选的实施例中，对所述第一激光点云与各个所述匹配关键帧中包含的激光点云分别进行匹配，以得到各个所述匹配关键帧的匹配分数包括：对于各个所述匹配关键帧，确定所述第一激光点云在所述第一激光点云所对应的第一地面上的第一投影以及确定所述匹配关键帧中包含的第二激光点云在所述第二激光点云所对应的第二地面上的第二投影；基于所述第一投影确定第一位置特征值，以及，基于所述第二投影确定第二位置特征值；利用迭代最近点ICP(Iterative Closest Point)对所述第一位置特征值和所述第二位置特征值进行匹配，以得到匹配分数。可选地，确定所述第一激光点云在所述第一激光点云所对应的第一地面上的第一投影包括：对所述第一地面进行栅格化；将所述第一激光点云投影到栅格化后的第一地面上，以得到所述第一投影；基于所述第一投影确定第一位置特征值包括：清空所述第一投影所位于的栅格中点数少于预定点数阈值的栅格内的数据，并计算其余未清空数据的栅格内所有点的平均位置，以得到所述第一位置特征值。可选地，确定所述第二激光点云在所述第二激光点云所对应的第二地面上的第二投影包括：对所述第二地面进行栅格化；将所述第二激光点云投影到栅格化后的第二地面上，以得到所述第二投影；基于所述第二投影确定第二位置特征值包括：清空所述第二投影所位于的栅格中点数少于预定点数阈值的栅格内的数据，并计算其余未清空数据的栅格内所有点的平均位置，以得到所述第二位置特征值。在上述实施例中，可以通过对第一激光点云和各匹配关键帧分别进行特征点提取，分别计算第一激光点云的特征点与各匹配关键帧的特征点的匹配得分。其中，关于特征点提取方法如下：将地面进行栅格化；将激光点云投影到地面，如果栅格内点数过少，则清空该栅格内数据，否则，计算该栅格内所有点的平均位置。栅格内所有点的平均位置代表了该栅格位置处的特征。通过将3D点云投影到二维的方式，不仅提取出了点云的高度特征，同时大幅度削减了用于匹配的点云数量。其中，对于第一激光点云特征点的提取和对于各匹配关键帧中激光点云特征点的提取均可以采用上述的提取方法。之后，可以采用ICP等方法计算各匹配关键帧和当前激光点云间匹配分数。其中，ICP为一种点集对点集配准方法。
在一个可选的实施例中，在对所述第一激光点云与各个所述匹配关键帧中包含的激光点云分别进行匹配，以得到各个所述匹配关键帧的匹配分数之后，所述方法还包括：在确定沿着远离所述初始位姿的预定方向上得到匹配分数持续增大的情况下，将匹配分数最高的匹配关键帧更新为所述初始位姿；重复执行选择多个匹配关键帧以及得到各匹配分数的步骤，直到沿着远离所述初始位姿的预定方向上得到匹配分数不再持续增大为止。在本实施例中，可以从前述的匹配得分规律中启发出下一步计算任务。如果沿着远离Tnearest方向的得分不满足持续减小的要求，则将最高得分关键帧更新为Tnearest，并跳转到选择匹配关键帧的操作步骤；如果得分持续减小，则停止沿该方向搜索。从而得到整个地图上匹配分数最高的匹配关键帧。将沿着远离Tnearest方向上的各匹配关键帧中，选出最高得分关键帧并将选出的该关键帧和第一激光点云间进行匹配，并将匹配得到的位姿(即，二者的相对位姿，对应于前述的相对位姿)作为细定位结果/>该细定位结果的误差通常为栅格边长。在执行完上述操作之后，可以按照步骤S108将/>作为初始值，采用NDT方法匹配当前激光点云(即，第一激光点云)和/>点云，计算得出精确定位结果，即，得到目标设备的位姿。
通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到根据上述实施例的方法可借助软件加必需的通用硬件平台的方式来实现，当然也可以通过硬件，但很多情况下前者是更佳的实施方式。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质(如ROM/RAM、磁碟、光盘)中，包括若干指令用以使得一台终端设备(可以是手机，计算机，服务器，或者网络设备等)执行本发明各个实施例所述的方法。
在本实施例中还提供了一种设备位姿的确定装置，该装置用于实现上述实施例及优选实施方式，已经进行过说明的不再赘述。如以下所使用的，术语“模块”可以实现预定功能的软件和/或硬件的组合。尽管以下实施例所描述的装置较佳地以软件来实现，但是硬件，或者软件和硬件的组合的实现也是可能并被构想的。
图3是根据本发明实施例的设备位姿的确定装置的结构框图，如图3所示，该装置包括：
第一确定模块32，用于从预先建立的地图中确定出初始关键帧，其中，所述初始关键帧包含有第一GPS位置，所述第一GPS位置与第一位置的距离小于预设距离阈值，所述第一位置为通过安装在目标设备上的GPS设备确定的所述目标设备的当前位置；第二确定模块34，用于从所述地图中确定出包含目标位姿的候选关键帧，其中，所述目标位姿与所述初始关键帧中包含的初始位姿相差预定位姿域值；第一匹配模块36，用于从所述候选关键帧中确定出目标关键帧，并对目标激光点云与第一激光点云进行匹配，以得到所述目标关键帧中包含的位姿与所述目标设备当前的位姿之间的相对位姿，其中，所述目标激光点云包含在所述目标关键帧中，所述第一激光点云为利用位于所述目标设备上的激光雷达获取的所述目标设备当前所处的位置的激光点云；第二匹配模块38，用于以所述相对位姿为初始值，采用正态分布变换NDT对所述目标激光点云与所述第一激光点云进行匹配，进而得到所述目标设备当前的位姿。
在一个可选的实施例中，所述第一匹配模块36用于通过如下方式确定出所述目标关键帧：在所述地图中沿着远离所述初始位姿的预定方向上，按照预定间隔从所述候选关键帧中选择多个匹配关键帧；对所述第一激光点云与各个所述匹配关键帧中包含的激光点云分别进行匹配，以得到各个所述匹配关键帧的匹配分数；根据所述匹配分数，从所述匹配关键帧中确定出目标关键帧。
在一个可选的实施例中，所述第一匹配模块36用于通过如下方式对所述第一激光点云与各个所述匹配关键帧中包含的激光点云分别进行匹配，以得到各个所述匹配关键帧的匹配分数：对于各个所述匹配关键帧，确定所述第一激光点云在所述第一激光点云所对应的第一地面上的第一投影以及确定所述匹配关键帧中包含的第二激光点云在所述第二激光点云所对应的第二地面上的第二投影；基于所述第一投影确定第一位置特征值，以及，基于所述第二投影确定第二位置特征值；利用迭代最近点ICP对所述第一位置特征值和所述第二位置特征值进行匹配，以得到匹配分数。
在一个可选的实施例中，所述第一匹配模块36用于通过如下方式确定所述第一激光点云在所述第一激光点云所对应的第一地面上的第一投影：对所述第一地面进行栅格化；将所述第一激光点云投影到栅格化后的第一地面上，以得到所述第一投影；以及，通过如下方式基于所述第一投影确定第一位置特征值：清空所述第一投影所位于的栅格中点数少于预定点数阈值的栅格内的数据，并计算其余未清空数据的栅格内所有点的平均位置，以得到所述第一位置特征值。
在一个可选的实施例中，所述第一匹配模块36用于通过如下方式确定所述第二激光点云在所述第二激光点云所对应的第二地面上的第二投影：对所述第二地面进行栅格化；将所述第二激光点云投影到栅格化后的第二地面上，以得到所述第二投影；以及，通过如下方式基于所述第二投影确定第二位置特征值：清空所述第二投影所位于的栅格中点数少于预定点数阈值的栅格内的数据，并计算其余未清空数据的栅格内所有点的平均位置，以得到所述第二位置特征值。
在一个可选的实施例中，所述装置还用于在对所述第一激光点云与各个所述匹配关键帧中包含的激光点云分别进行匹配，以得到各个所述匹配关键帧的匹配分数之后，在确定沿着远离所述初始位姿的预定方向上得到匹配分数持续增大的情况下，将匹配分数最高的匹配关键帧更新为所述初始位姿；重复执行选择多个匹配关键帧以及得到各匹配分数的步骤，直到沿着远离所述初始位姿的预定方向上得到匹配分数不再持续增大为止。
需要说明的是，上述各个模块是可以通过软件或硬件来实现的，对于后者，可以通过以下方式实现，但不限于此：上述模块均位于同一处理器中；或者，上述各个模块以任意组合的形式分别位于不同的处理器中。
本发明的实施例还提供了一种计算机可读存储介质，该计算机可读存储介质中存储有计算机程序，其中，该计算机程序被设置为运行时执行上述任一项方法实施例中的步骤。
可选地，在本实施例中，上述计算机可读存储介质可以包括但不限于：U盘、只读存储器(Read-Only Memory，简称为ROM)、随机存取存储器(Random Access Memory，简称为RAM)、移动硬盘、磁碟或者光盘等各种可以存储计算机程序的介质。
本发明的实施例还提供了一种电子装置，包括存储器和处理器，该存储器中存储有计算机程序，该处理器被设置为运行计算机程序以执行上述任一项方法实施例中的步骤。
可选地，上述电子装置还可以包括传输设备以及输入输出设备，其中，该传输设备和上述处理器连接，该输入输出设备和上述处理器连接。
可选地，本实施例中的具体示例可以参考上述实施例及可选实施方式中所描述的示例，本实施例在此不再赘述。
由前述的实施例可知，在本发明中首先利用低成本GPS信息实现粗定位，作为先验信息，减小了对地图搜索范围；其次采用智能启发式选择待匹配地图激光点云关键帧与当前激光点云进行特征提取和匹配的方法，可以在粗定位误差范围内快速收敛定位；最后充分利用NDT算法对初始值不敏感、定位准确的特点，实现精细定位。进而实现了仅采用一个激光雷达和一个低成本GPS设备即可实现设备的定位；此外，采用智能启发方式从候选关键帧中选择出待匹配关键帧，而非遍历所有候选关键帧，大幅度减少了计算量，通过对激光点云进行特征提取和匹配进行细定位，而非直接进行点云匹配，进一步降低了计算开销。并且，在该方法中仅进行一次NDT计算，既保留了高定位精度优点，同时减小计算开销。
显然，本领域的技术人员应该明白，上述的本发明的各模块或各步骤可以用通用的计算装置来实现，它们可以集中在单个的计算装置上，或者分布在多个计算装置所组成的网络上，可选地，它们可以用计算装置可执行的程序代码来实现，从而，可以将它们存储在存储装置中由计算装置来执行，并且在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤，或者将它们分别制作成各个集成电路模块，或者将它们中的多个模块或步骤制作成单个集成电路模块来实现。这样，本发明不限制于任何特定的硬件和软件结合。
以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。
Claims (12)
1.一种设备位姿的确定方法，其特征在于，包括：
从预先建立的地图中确定出初始关键帧，其中，所述初始关键帧包含有第一GPS位置，所述第一GPS位置与第一位置的距离小于预设距离阈值，所述第一位置为通过安装在目标设备上的GPS设备确定的所述目标设备的当前位置；
从所述地图中确定出包含目标位姿的候选关键帧，其中，所述目标位姿与所述初始关键帧中包含的初始位姿相差预定位姿阈值；
从所述候选关键帧中确定出目标关键帧，并对目标激光点云与第一激光点云进行匹配，以得到所述目标关键帧中包含的位姿与所述目标设备当前的位姿之间的相对位姿，其中，所述目标激光点云包含在所述目标关键帧中，所述第一激光点云为利用位于所述目标设备上的激光雷达获取的所述目标设备当前所处的位置的激光点云；
以所述相对位姿为初始值，采用正态分布变换NDT对所述目标激光点云与所述第一激光点云进行匹配，进而得到所述目标设备当前的位姿；
其中，从所述候选关键帧中确定出目标关键帧包括：在所述地图中沿着远离所述初始位姿的预定方向上，按照预定间隔从所述候选关键帧中选择多个匹配关键帧；对所述第一激光点云与各个所述匹配关键帧中包含的激光点云分别进行匹配，以得到各个所述匹配关键帧的匹配分数；根据所述匹配分数，从所述匹配关键帧中确定出目标关键帧。
2.根据权利要求1所述的方法，其特征在于，对所述第一激光点云与各个所述匹配关键帧中包含的激光点云分别进行匹配，以得到各个所述匹配关键帧的匹配分数包括：
对于各个所述匹配关键帧，确定所述第一激光点云在所述第一激光点云所对应的第一地面上的第一投影，以及确定所述匹配关键帧中包含的第二激光点云在所述第二激光点云所对应的第二地面上的第二投影；
基于所述第一投影确定第一位置特征值，以及，基于所述第二投影确定第二位置特征值；
利用迭代最近点ICP对所述第一位置特征值和所述第二位置特征值进行匹配，以得到匹配分数。
3.根据权利要求2所述的方法，其特征在于，
确定所述第一激光点云在所述第一激光点云所对应的第一地面上的第一投影包括：对所述第一地面进行栅格化；将所述第一激光点云投影到栅格化后的第一地面上，以得到所述第一投影；
基于所述第一投影确定第一位置特征值包括：清空所述第一投影所位于的栅格中点数少于预定点数阈值的栅格内的数据，并计算其余未清空数据的栅格内所有点的平均位置，以得到所述第一位置特征值。
4.根据权利要求2或3所述的方法，其特征在于，
确定所述第二激光点云在所述第二激光点云所对应的第二地面上的第二投影包括：对所述第二地面进行栅格化；将所述第二激光点云投影到栅格化后的第二地面上，以得到所述第二投影；
基于所述第二投影确定第二位置特征值包括：清空所述第二投影所位于的栅格中点数少于预定点数阈值的栅格内的数据，并计算其余未清空数据的栅格内所有点的平均位置，以得到所述第二位置特征值。
5.根据权利要求1所述的方法，其特征在于，在对所述第一激光点云与各个所述匹配关键帧中包含的激光点云分别进行匹配，以得到各个所述匹配关键帧的匹配分数之后，所述方法还包括：
在确定沿着远离所述初始位姿的预定方向上得到匹配分数持续增大的情况下，将匹配分数最高的匹配关键帧更新为所述初始位姿；
重复执行选择多个匹配关键帧以及得到各匹配分数的步骤，直到沿着远离所述初始位姿的预定方向上得到匹配分数不再持续增大为止。
6.一种设备位姿的确定装置，其特征在于，包括：
第一确定模块，用于从预先建立的地图中确定出初始关键帧，其中，所述初始关键帧包含有第一GPS位置，所述第一GPS位置与第一位置的距离小于预设距离阈值，所述第一位置为通过安装在目标设备上的GPS设备确定的所述目标设备的当前位置；
第二确定模块，用于从所述地图中确定出包含目标位姿的候选关键帧，其中，所述目标位姿与所述初始关键帧中包含的初始位姿相差预定位姿阈值；
第一匹配模块，用于从所述候选关键帧中确定出目标关键帧，并对目标激光点云与第一激光点云进行匹配，以得到所述目标关键帧中包含的位姿与所述目标设备当前的位姿之间的相对位姿，其中，所述目标激光点云包含在所述目标关键帧中，所述第一激光点云为利用位于所述目标设备上的激光雷达获取的所述目标设备当前所处的位置的激光点云；
第二匹配模块，用于以所述相对位姿为初始值，采用正态分布变换NDT对所述目标激光点云与所述第一激光点云进行匹配，进而得到所述目标设备当前的位姿；
其中，所述第一匹配模块用于通过如下方式确定出所述目标关键帧：在所述地图中沿着远离所述初始位姿的预定方向上，按照预定间隔从所述候选关键帧中选择多个匹配关键帧；对所述第一激光点云与各个所述匹配关键帧中包含的激光点云分别进行匹配，以得到各个所述匹配关键帧的匹配分数；根据所述匹配分数，从所述匹配关键帧中确定出目标关键帧。
7.根据权利要求6所述的装置，其特征在于，所述第一匹配模块用于通过如下方式对所述第一激光点云与各个所述匹配关键帧中包含的激光点云分别进行匹配，以得到各个所述匹配关键帧的匹配分数：
对于各个所述匹配关键帧，确定所述第一激光点云在所述第一激光点云所对应的第一地面上的第一投影，以及确定所述匹配关键帧中包含的第二激光点云在所述第二激光点云所对应的第二地面上的第二投影；
基于所述第一投影确定第一位置特征值，以及，基于所述第二投影确定第二位置特征值；
利用迭代最近点ICP对所述第一位置特征值和所述第二位置特征值进行匹配，以得到匹配分数。
8.根据权利要求7所述的装置，其特征在于，所述第一匹配模块用于通过如下方式确定所述第一激光点云在所述第一激光点云所对应的第一地面上的第一投影：对所述第一地面进行栅格化；将所述第一激光点云投影到栅格化后的第一地面上，以得到所述第一投影；以及，通过如下方式实现基于所述第一投影确定第一位置特征值：清空所述第一投影所位于的栅格中点数少于预定点数阈值的栅格内的数据，并计算其余未清空数据的栅格内所有点的平均位置，以得到所述第一位置特征值。
9.根据权利要求7或8所述的装置，其特征在于，所述第一匹配模块用于通过如下方式确定所述第二激光点云在所述第二激光点云所对应的第二地面上的第二投影：对所述第二地面进行栅格化；将所述第二激光点云投影到栅格化后的第二地面上，以得到所述第二投影；以及，通过如下方式实现基于所述第二投影确定第二位置特征值：清空所述第二投影所位于的栅格中点数少于预定点数阈值的栅格内的数据，并计算其余未清空数据的栅格内所有点的平均位置，以得到所述第二位置特征值。
10.根据权利要求6所述的装置，其特征在于，所述装置还用于：
在对所述第一激光点云与各个所述匹配关键帧中包含的激光点云分别进行匹配，以得到各个所述匹配关键帧的匹配分数之后，在确定沿着远离所述初始位姿的预定方向上得到匹配分数持续增大的情况下，将匹配分数最高的匹配关键帧更新为所述初始位姿；
重复执行选择多个匹配关键帧以及得到各匹配分数的步骤，直到沿着远离所述初始位姿的预定方向上得到匹配分数不再持续增大为止。
11.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质中存储有计算机程序，其中，所述计算机程序被设置为运行时执行所述权利要求1至5任一项中所述的方法。
12.一种电子装置，包括存储器和处理器，其特征在于，所述存储器中存储有计算机程序，所述处理器被设置为运行所述计算机程序以执行所述权利要求1至5任一项中所述的方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010019371.XA CN113096181B (zh) | 2020-01-08 | 2020-01-08 | 设备位姿的确定方法、装置、存储介质及电子装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010019371.XA CN113096181B (zh) | 2020-01-08 | 2020-01-08 | 设备位姿的确定方法、装置、存储介质及电子装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113096181A CN113096181A (zh) | 2021-07-09 |
| CN113096181B true CN113096181B (zh) | 2024-04-09 |
Family
ID=76663414
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010019371.XA Active CN113096181B (zh) | 2020-01-08 | 2020-01-08 | 设备位姿的确定方法、装置、存储介质及电子装置 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113096181B (zh) |
Families Citing this family (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115375870B (zh) * | 2022-10-25 | 2023-02-10 | 杭州华橙软件技术有限公司 | 回环检测优化方法、电子设备及计算机可读存储装置 |
| CN116012624B (zh) * | 2023-01-12 | 2024-03-26 | 阿波罗智联(北京)科技有限公司 | 定位方法、装置、电子设备、介质以及自动驾驶设备 |
| CN116559928B (zh) * | 2023-07-11 | 2023-09-22 | 新石器慧通（北京）科技有限公司 | 激光雷达的位姿信息确定方法、装置、设备及存储介质 |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108181636A (zh) * | 2018-01-12 | 2018-06-19 | 中国矿业大学 | 石化工厂巡检机器人环境建模与地图构建装置和方法 |
| CN109001711A (zh) * | 2018-06-05 | 2018-12-14 | 北京智行者科技有限公司 | 多线激光雷达标定方法 |
| CN109489660A (zh) * | 2018-10-09 | 2019-03-19 | 上海岚豹智能科技有限公司 | 机器人定位方法及设备 |
| CN109813310A (zh) * | 2019-03-11 | 2019-05-28 | 中南大学 | 地下作业设备定位方法、装置、系统及存储介质 |
| CN109839112A (zh) * | 2019-03-11 | 2019-06-04 | 中南大学 | 地下作业设备定位方法、装置、系统及存储介质 |
| CN110515055A (zh) * | 2019-10-24 | 2019-11-29 | 奥特酷智能科技(南京)有限公司 | 利用半径搜索优化激光雷达定位的方法 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CA3026914C (en) * | 2018-07-02 | 2021-11-30 | Beijing Didi Infinity Science And Development Co., Ltd. | Vehicle navigation system using pose estimation based on point cloud |
-
2020
- 2020-01-08 CN CN202010019371.XA patent/CN113096181B/zh active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108181636A (zh) * | 2018-01-12 | 2018-06-19 | 中国矿业大学 | 石化工厂巡检机器人环境建模与地图构建装置和方法 |
| CN109001711A (zh) * | 2018-06-05 | 2018-12-14 | 北京智行者科技有限公司 | 多线激光雷达标定方法 |
| CN109489660A (zh) * | 2018-10-09 | 2019-03-19 | 上海岚豹智能科技有限公司 | 机器人定位方法及设备 |
| CN109813310A (zh) * | 2019-03-11 | 2019-05-28 | 中南大学 | 地下作业设备定位方法、装置、系统及存储介质 |
| CN109839112A (zh) * | 2019-03-11 | 2019-06-04 | 中南大学 | 地下作业设备定位方法、装置、系统及存储介质 |
| CN110515055A (zh) * | 2019-10-24 | 2019-11-29 | 奥特酷智能科技(南京)有限公司 | 利用半径搜索优化激光雷达定位的方法 |
Non-Patent Citations (1)
| Title |
|---|
| 基于NDT匹配和改进回环检测的SLAM研究;林俊钦;韩宝玲;罗庆生;赵嘉珩;葛卓;;光学技术(第02期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113096181A (zh) | 2021-07-09 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108319655B (zh) | 用于生成栅格地图的方法和装置 | |
| CN108871353B (zh) | 路网地图生成方法、系统、设备及存储介质 | |
| CN108763287B (zh) | 大规模可通行区域驾驶地图的构建方法及其无人驾驶应用方法 | |
| CN113096181B (zh) | 设备位姿的确定方法、装置、存储介质及电子装置 | |
| US10521694B2 (en) | 3D building extraction apparatus, method and system | |
| CN111598916A (zh) | 一种基于rgb-d信息的室内占据栅格地图的制备方法 | |
| CN108279670B (zh) | 用于调整点云数据采集轨迹的方法、设备以及计算机可读介质 | |
| CN111582054B (zh) | 点云数据处理方法及装置、障碍物检测方法及装置 | |
| CN111968229A (zh) | 高精度地图制图方法及装置 | |
| CN113989450B (zh) | 图像处理方法、装置、电子设备和介质 | |
| CN110598541B (zh) | 一种提取道路边缘信息的方法及设备 | |
| CN113761999B (zh) | 一种目标检测方法、装置、电子设备和存储介质 | |
| CN113593017A (zh) | 露天矿地表三维模型构建方法、装置、设备及存储介质 | |
| KR101549155B1 (ko) | 라이다 자료를 활용한 구조물의 직선경계 추출방법 | |
| CN113298910A (zh) | 生成交通标志线地图的方法、设备和存储介质 | |
| CN115867939A (zh) | 用于空中到地面配准的系统和方法 | |
| CN114066773B (zh) | 一种基于点云特征与蒙特卡洛扩展法的动态物体去除 | |
| CN113721254A (zh) | 一种基于道路指纹空间关联矩阵的车辆定位方法 | |
| CN117053779A (zh) | 一种基于冗余关键帧去除的紧耦合激光slam方法及装置 | |
| CN107808160B (zh) | 三维建筑物提取方法和装置 | |
| CN115790621A (zh) | 高精地图更新方法、装置及电子设备 | |
| JP2023528530A (ja) | 訓練装置、制御方法、及びプログラム | |
| CN107392209B (zh) | 一种提取线段的装置及方法 | |
| Yu et al. | Advanced approach for automatic reconstruction of 3d buildings from aerial images | |
| Subash | Automatic road extraction from satellite images using extended Kalman filtering and efficient particle filtering |
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 |