CN108571967B - 一种定位方法和装置 - Google Patents

一种定位方法和装置 Download PDF

Info

Publication number
CN108571967B
CN108571967B CN201710147835.3A CN201710147835A CN108571967B CN 108571967 B CN108571967 B CN 108571967B CN 201710147835 A CN201710147835 A CN 201710147835A CN 108571967 B CN108571967 B CN 108571967B
Authority
CN
China
Prior art keywords
point cloud
cloud data
matching degree
initial
historical
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
CN201710147835.3A
Other languages
English (en)
Other versions
CN108571967A (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.)
Shenzhen Launch Digital Technology Co Ltd
Original Assignee
Shenzhen Launch Digital 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 Shenzhen Launch Digital Technology Co Ltd filed Critical Shenzhen Launch Digital Technology Co Ltd
Priority to CN201710147835.3A priority Critical patent/CN108571967B/zh
Publication of CN108571967A publication Critical patent/CN108571967A/zh
Application granted granted Critical
Publication of CN108571967B publication Critical patent/CN108571967B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

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/20—Instruments for performing navigational calculations
    • G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Length Measuring Devices By Optical Means (AREA)

Abstract

本发明适用于电子技术领域，提供了一种定位方法和装置，旨在解决现有技术中使用微处理器的移动机器人定位效率低且定位精度不高的问题。所述方法包括：计算初始点云数据与预设时间段内的历史点云数据之间的点云匹配度；按照预设数量的步长变化量修改初始点云数据，并计算其与历史点云数据之间的点云匹配度；将最大点云匹配度对应的点云数据，作为第一点云数据；在第一点云数据的基础上，按照同样的方法得到第二点云数据；若第一点云数据的点云匹配度大于或者等于第二点云数据的点云匹配度，则根据第一点云数据输出定位信息。本发明的技术方案使得搭载微处理器的机器人能够快速准确的实现定位，从而提高定位效率和定位精度。

Description

一种定位方法和装置
技术领域
本发明涉及电子技术领域，尤其涉及一种定位方法和装置。
背景技术
目前，移动机器人被广泛应用于服务行业，其应用场景更多的趋向于商场、超市、医院等室内环境。随着应用环境的复杂度增加，移动机器人在运动过程中对定位精度的要求也越来越高。
同时，随着微处理器的不断发展，越来越多的机器人提供商开始采用微处理器作为移动机器人的核心处理单元。
但是，由于微处理器的处理能力和资源有限，现有定位精度高的定位方法往往依赖于较强的***处理能力，并且需要占用较多的***资源，使得使用微处理器的移动机器人在进行定位时受到限制，导致定位精度不高和定位效率低。
发明内容
本发明实施例提供一种定位方法和装置，旨在解决现有技术中使用微处理器的移动机器人定位效率低且定位精度不高的问题。
第一方面，本发明实施例提供了一种定位方法，该定位方法包括：
获取初始点云数据，其中，所述初始点云数据包括预设个数的采样点的采样信息，所述采样信息包括所述采样点的位置坐标和方向角；
计算所述初始点云数据与预设时间段内的历史点云数据之间的点云匹配度；
按照预设数量的步长变化量修改所述初始点云数据，得到所述预设数量的初始参考点云数据，并计算每个所述初始参考点云数据与所述历史点云数据之间的点云匹配度，其中，所述步长变化量包括位置坐标变化量和方向角变化量；
根据所述初始点云数据的点云匹配度和每个所述初始参考点云数据的点云匹配度，确定最大点云匹配度对应的点云数据，作为第一点云数据；
按照所述预设数量的步长变化量修改所述第一点云数据，得到所述预设数量第一参考点云数据，并计算每个所述第一参考点云数据与所述历史点云数据之间的点云匹配度；
根据所述第一点云数据的点云匹配度和每个所述第一参考点云数据的点云匹配度，确定最大点云匹配度对应的点云数据，作为第二点云数据；
若所述第一点云数据的点云匹配度大于或者等于所述第二点云数据的点云匹配度，则根据所述第一点云数据输出定位信息，其中，所述定位信息包括机器人的位置和姿态。
另一方面，本发明实施例提供了一种定位装置，该定位装置包括:
获取模块，用于获取初始点云数据，其中，所述初始点云数据包括预设个数的采样点的采样信息，所述采样信息包括所述采样点的位置坐标和方向角；
计算模块，用于计算所述初始点云数据与预设时间段内的历史点云数据之间的点云匹配度；
第一修改模块，用于按照预设数量的步长变化量修改所述初始点云数据，得到所述预设数量的初始参考点云数据，并计算每个所述初始参考点云数据与所述历史点云数据之间的点云匹配度，其中，所述步长变化量包括位置坐标变化量和方向角变化量；
第一选取模块，用于根据所述初始点云数据的点云匹配度和每个所述初始参考点云数据的点云匹配度，确定最大点云匹配度对应的点云数据，作为第一点云数据；
第二修改模块，用于按照所述预设数量的步长变化量修改所述第一点云数据，得到所述预设数量的第一参考点云数据，并计算每个所述第一参考点云数据与所述历史点云数据之间的点云匹配度；
第二选取模块，用于根据所述第一点云数据的点云匹配度和每个所述第一参考点云数据的点云匹配度，确定最大点云匹配度对应的点云数据，作为第二点云数据；
第一输出模块，用于若所述第一点云数据的点云匹配度大于或者等于所述第二点云数据的点云匹配度，则根据所述第一点云数据输出定位信息，其中，所述定位信息包括机器人的位置和姿态。
本发明实施获取包括采样点的位置坐标和方向角的初始点云数据，计算初始点云数据与预设时间段内的历史点云数据之间的点云匹配度，并以该初始点云数据为基础，按照预设数量的步长变化量修改该初始点云数据，得到预设数量的初始参考点云数据，并计算每个初始参考点云数据与历史点云数据之间的点云匹配度，从初始点云数据和初始参考点云数据中，选取最大点云匹配度对应的点云数据，作为第一点云数据，再以第一点云数据为基础，按照与得到第一点云数据相同的方法得到第二点云数据，若第一点云数据的点云匹配度大于或者等于第二点云数据的点云匹配度，则根据第一点云数据输出机器人的位置和姿态，通过将当前点云数据与历史点云数据在不同方位的刚性匹配，以及梯度下降的查找方式匹配最优的方位作为定位优化方向，实现在定位优化方向的快速收敛，使得搭载微处理器的机器人能够快速准确的实现定位，从而提高定位效率和定位精度。
附图说明
图1是本发明实施例一提供的一种定位方法的流程图；
图2是本发明实施例二提供的一种定位方法的流程图；
图3是本发明实施例三提供的一种定位装置的结构示意图；
图4是本发明实施例四提供的一种定位装置的结构示意图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
以下结合具体附图对本发明的实现进行详细的描述。
实施例一：
图1是本发明实施例一提供的一种定位方法的流程图，本发明实施例的执行主体为机器人，其具体可以是机器人的微处理器设备，图1示例的定位方法具体可以包括步骤S101至步骤S107，详述如下：
S101、获取初始点云数据，其中，该初始点云数据包括预设个数的采样点的采样信息，该采样信息包括预设个数的采样点的位置坐标和方向角。
点云(Point Cloud)也称为激光点云，是利用激光在同一空间参考系下获取物体表面每个采样点的空间坐标，得到的是一系列表达目标空间分布和目标表面特性的海量点的集合，这个点集合即为点云。点云数据可以包括空间分辨率、点位精度，表面法向量等。
点云数据可以根据各种扫描范围的激光传感器或者各种可以转换为点云输出的传感器采集到的信息获取。
在本发明实施例中，机器人可以包括机器人本体、激光传感器和微处理器等，其中，机器人本体可以包括车轮、车体、里程计、电机、电机驱动器，以及机器人本体运动控制器等，激光传感器用于采集在激光传感器的扫描范围内，机器人本体与当前环境中的参照物体之间的相对距离，微处理器用于处理激光传感器采集的数据，完成对机器人的定位并输出定位信息。
具体地，微处理器根据采集到的预设个数的采样点的采样信息，获取带有位置坐标和方向角信息的初始点云数据。
采样点的预设个数具体可以根据实际应用的需要进行设置，此处不做限制。
S102、计算初始点云数据与预设时间段内的历史点云数据之间的点云匹配度。
具体地，微处理器将步骤S101得到的初始点云数据，与预设时间段内的历史点云数据进行匹配，并计算点云匹配度，该点云匹配度用于标识初始点云数据中的采样点与历史点云数据中对应的采样点之间的采样信息的偏差情况。
预设时间段内的历史点云数据是指在当前时刻之前的预设时间段内的所有点云数据。需要说明的是，预设时间段的具体时长可以根据实际应用的需要进行设置，此处不做限制。
S103、按照预设数量的步长变化量修改初始点云数据，得到预设数量的初始参考点云数据，并计算每个初始参考点云数据与历史点云数据之间的点云匹配度，其中，该步长变化量包括位置坐标变化量和方向角变化量。
步长变化量包括位置坐标变化量和方向角变化量，每个步长变化量在坐标或者方向上有固定变化量。例如，假设在x坐标、y坐标和方向角均预设了5个固定变化量，则一共有125(5*5*5＝125)种步长变化量，即预设数量为125。
具体地，以步骤S101获取的初始点云数据为基础，按照预设数量的步长变化量修改该初始点云数据，得到预设数量的初始参考点云数据，并计算每种步长变化量下的初始参考点云数据与历史点云数据之间的点云匹配度，该点云匹配度的计算方法与步骤S102中计算点云匹配度的计算方法相同。
S104、根据初始点云数据的点云匹配度和每个初始参考点云数据的点云匹配度，确定最大点云匹配度对应的点云数据，作为第一点云数据。
具体地，从步骤S102计算得到的初始点云数据的点云匹配度，以及步骤S103计算得到的在每一种步长变化量下的初始参考点云数据的点云匹配度中，选出最大点云匹配度，将最大点云匹配度对应的点云数据作为第一点云数据。
S105、按照预设数量的步长变化量修改第一点云数据，得到预设数量的第一参考点云数据，并计算每个第一参考点云数据与历史点云数据之间的点云匹配度。
具体地，采用与步骤S103相同的方法，以步骤S104得到的第一点云数据为基础，按照预设数量的步长变化量修改第一点云数据，得到预设数量的第一参考点云数据，并计算每种步长变化量下的第一参考点云数据与历史点云数据之间的点云匹配度，该点云匹配度的计算方法与步骤S102中计算点云匹配度的计算方法相同。
S106、根据第一点云数据的点云匹配度和每个第一参考点云数据的点云匹配度，确定最大点云匹配度对应的点云数据，作为第二点云数据。
具体地，采用与步骤S104相同的方法，从步骤S104计算得到的第一点云数据的点云匹配度，以及步骤S105计算得到的在每一种步长变化量下的第一参考点云数据的点云匹配度中，选出最大点云匹配度，将最大点云匹配度对应的点云数据作为第二点云数据。
S107、若第一点云数据的点云匹配度大于或者等于第二点云数据的点云匹配度，则根据第一点云数据输出定位信息，其中，该定位信息包括机器人的位置和姿态。
具体地，判断第一点云数据的点云匹配度和第二点云数据的点云匹配度之间的大小，如果第一点云数据的点云匹配度大于或者等于第二点云数据的点云匹配度，则认为第一点云数据为机器人当前的最佳定位数据，因此，根据该第一点云数据中采样点的位置坐标和方向角确定机器人当前的位置和姿态，并输出包含机器人的位置和姿态的定位信息。
可以理解的是，在本发明实施例中，若第一点云数据的点云匹配度大于或者等于第二点云数据的点云匹配度，则根据第一点云数据输出定位信息，在其他实施例中，也可以是若第一点云数据的点云匹配度大于第二点云数据的点云匹配度，则根据第一点云数据输出定位信息。
本实施例中，获取包括采样点的位置坐标和方向角的初始点云数据，计算初始点云数据与预设时间段内的历史点云数据之间的点云匹配度，并以该初始点云数据为基础，按照预设数量的步长变化量修改该初始点云数据，得到预设数量的初始参考点云数据，并计算每个初始参考点云数据与历史点云数据之间的点云匹配度，从初始点云数据和初始参考点云数据中，选取最大点云匹配度对应的点云数据，作为第一点云数据，再以第一点云数据为基础，按照与得到第一点云数据相同的方法得到第二点云数据，若第一点云数据的点云匹配度大于或者等于第二点云数据的点云匹配度，则根据第一点云数据输出机器人的位置和姿态，通过将当前点云数据与历史点云数据在不同方位的刚性匹配，以及梯度下降的查找方式匹配最优的方位作为定位优化方向，实现在定位优化方向的快速收敛，使得搭载微处理器的机器人能够快速准确的实现定位，从而提高定位效率和定位精度。
实施例二：
图2是本发明实施例二提供的一种定位方法的流程图，本发明实施例的执行主体为机器人，其具体可以是机器人的微处理器设备，图2示例的定位方法具体可以包括步骤S201至步骤S216，详述如下：
S201、采集预设个数的每个采样点的里程计信息和激光传感器信息。
里程计是用于测量行程的传感装置，安装在机器人上的里程计通过记录电机转动的圈数计算机器人的行走里程。
具体地，微处理器通过里程计采集每个采样点的里程计信息，并通过激光传感器采集每个采样点与当前环境中对应的参照点之间的相对距离等激光传感器信息。
S202、根据里程计信息计算每个采样点的位置坐标和方向角。
具体地，微处理器根据里程计信息中的行程数据计算每个采样点的位置坐标和方向角。
S203、将每个采样点的位置坐标和方向角与激光传感器信息关联，生成初始点云数据，其中，该初始点云数据包括预设个数的采样点的采样信息，该采样信息包括采样点的位置坐标和方向角。
具体地，微处理器将每个采样点的位置坐标和方向角与激光传感器信息关联，即将每个采样点的位置坐标和方向角添加到该采样点的激光传感器信息中，让激光传感器信息中包含采样点的位置坐标和方向角，并根据关联后的激光传感器信息生成初始点云数据，该初始点云数据包括预设个数的采样点的采样信息，采样信息包括采样点的位置坐标和方向角。
采样点的预设个数具体可以根据实际应用的需要进行设置，此处不做限制。
S204、按照点云匹配条件，将初始点云数据与预设时间段内的历史点云数据进行匹配，得到满足该点云匹配条件的历史点云数据，其中，该点云匹配条件包括初始点云数据中的每个采样点与历史点云数据中的对应采样点之间的距离的平方和最小。
预设时间段内的历史点云数据是指在当前时刻之前的预设时间段内的所有点云数据。需要说明的是，预设时间段的具体时长可以根据实际应用的需要进行设置，此处不做限制。
具体地，初始点云数据中的采样点会在每个历史点云数据中有一个对应的采样点，在初始点云数据和每个历史点云数据中，计算对应采样点之间的距离的平方和，假设在预设时间段内有j个历史点云数据，则针对每个历史点云数据都能够计算得到一个距离的平方和，从计算得到的j个距离的平方和中选取最小值，该最小值对应的历史点云数据即为满足点云匹配条件的历史点云数据。
S205、计算初始点云数据与满足点云匹配条件的历史点云数据之间的角度偏差和距离偏差，其中，距离偏差包括初始点云数据中的每个采样点与满足点云匹配条件的历史点云数据中的对应采样点之间的距离误差。
具体地，根据步骤S204确定的历史点云数据，计算该历史点云数据与步骤S203生成的初始点云数据之间的角度偏差和距离偏差。
角度偏差可以是初始点云数据和历史点云数据之间的中心角度偏差，或者是每个对应采样点之间的角度偏差的综合，角度偏差的具体计算方式可以根据实际应用的需要进行选择，此处不做限制。
距离偏差包括在步骤S203生成的初始点云数据和步骤S204确定的历史点云数据中对应采样点之间的距离误差。
S206、按照公式(1)计算初始点云数据与满足点云匹配条件的历史点云数据之间的点云匹配度：
Figure BDA0001244474900000091
其中，S为点云匹配度，α为预设的可信度参数，θ为角度偏差，β为预设的匹配度参数，Mi为距离误差小于预设的误差阈值的采样点中第i个采样点对应的距离误差，N为距离误差小于预设的误差阈值的采样点的个数。
具体地，预设的可信度参数α、预设的匹配度参数β，以及预设的误差阈值可以根据实际应用需要进行设置，此处不做限制。
角度偏差θ为步骤S205计算得到的初始点云数据与满足点云匹配条件的历史点云数据之间的角度偏差。
根据步骤S205计算得到的在初始点云数据和满足点云匹配条件的历史点云数据中对应采样点之间的距离误差，确定距离误差小于误差阈值的采样点的个数N，Mi为在这N个采样点中第i个采样点对应的距离误差。
S207、按照预设数量的步长变化量修改初始点云数据，得到预设数量的初始参考点云数据，并计算每个初始参考点云数据与历史点云数据之间的点云匹配度，其中，该步长变化量包括位置坐标变化量和方向角变化量。
步长变化量包括位置坐标变化量和方向角变化量，每个步长变化量在坐标或者方向上有固定变化量，例如，假设在x坐标、y坐标和方向角均预设了5个固定变化量，则一共有125(5*5*5＝125)种步长变化量，即预设数量为125。
具体地，以步骤S203生成的初始点云数据为基础，按照预设数量的步长变化量修改该初始点云数据，得到预设数量的初始参考点云数据，并计算每种步长变化量下的初始参考点云数据与历史点云数据之间的点云匹配度，该点云匹配度的计算方法与步骤S204至步骤S206计算点云匹配度的计算方法相同。
S208、根据初始点云数据的点云匹配度和每个初始参考点云数据的点云匹配度，确定最大点云匹配度对应的点云数据，作为第一点云数据。
具体地，从步骤S206计算得到的初始点云数据的点云匹配度，以及步骤S207计算得到的在每一种步长变化量下的初始参考点云数据的点云匹配度中，选出最大点云匹配度，将最大点云匹配度对应的点云数据作为第一点云数据。
S209、按照预设数量的步长变化量修改第一点云数据，得到预设数量的第一参考点云数据，并计算每个第一参考点云数据与历史点云数据之间的点云匹配度。
具体地，采用与步骤S207相同的方法，以第一点云数据为基础，按照预设数量的步长变化量修改第一点云数据，得到预设数量的第一参考点云数据，并计算每种步长变化量下的第一参考点云数据与历史点云数据之间的点云匹配度，该点云匹配度的计算方法与步骤S204至步骤S206计算点云匹配度的计算方法相同。
S210、根据第一点云数据的点云匹配度和每个第一参考点云数据的点云匹配度，确定最大点云匹配度对应的点云数据，作为第二点云数据。
具体地，采用与步骤S208相同的方法，从第一点云数据的点云匹配度，和步骤S209计算得到的在每一种步长变化量下的第一参考点云数据的点云匹配度中，选出最大点云匹配度，将最大点云匹配度对应的点云数据作为第二点云数据。
S211、判断第一点云数据的点云匹配度是否大于或者等于第二点云数据的点云匹配度。
具体地，若第一点云数据的点云匹配度大于或者等于第二点云数据的点云匹配度，则执行步骤S212，否则执行步骤S213。
S212、根据第一点云数据输出定位信息，其中，该定位信息包括机器人的位置和姿态。
具体地，如果第一点云数据的点云匹配度大于或者等于第二点云数据的点云匹配度，则认为第一点云数据为机器人当前的最佳定位数据，因此，根据该第一点云数据中采样点的位置坐标和方向角确定机器人当前的位置和姿态，并输出包含机器人的位置和姿态的定位信息。
可以理解的是，在本发明实施例中，若第一点云数据的点云匹配度大于或者等于第二点云数据的点云匹配度，则根据第一点云数据输出定位信息，在其他实施例中，也可以是若第一点云数据的点云匹配度大于第二点云数据的点云匹配度，则根据第一点云数据输出定位信息。
当微处理器根据第一点云数据输出定位信息后，流程结束。
S213、获取当前的优化次数。
具体地，若第一点云数据的点云匹配度小于第二点云数据的点云匹配度，则微处理器获取当前的优化次数，该优化次数可以通过统计步骤S211的执行次数得到。
S214、判断当前的优化次数是否达到预设的次数阈值。
具体地，判断当前的优化次数是否达到预设的次数阈值，若当前的优化次数未达到预设的次数阈值，则执行步骤S215，否则执行步骤S216。
预设的次数阈值可以设置为10次，但并不限于此，具体的次数阈值可以根据实际应用的需要进行设置，此处不做限制。
S215、将第二点云数据识别为第一点云数据，返回步骤S209继续执行。
具体地，若当前的优化次数未达到预设的次数阈值，则将步骤S210得到的第二点云数据重新识别为第一点云数据，并返回步骤S209，继续寻找定位优化方向。
S216、根据第二点云数据输出定位信息。
具体地，若当前的优化次数达到预设的次数阈值，则不再继续寻找定位优化方向，直接认为第二点云数据即为机器人当前的最佳定位数据，根据该第二点云数据中采样点的位置坐标和方向角确定机器人当前的位置和姿态，并输出包含机器人的位置和姿态的定位信息。
当微处理器根据第二点云数据输出定位信息后，流程结束。
本实施例中，首先通过采集里程计信息计算采样点的位置坐标和方向角，并与激光传感器信息关联，生成包含位置坐标和方向角的初始点云数据，然后按照点云匹配条件将初始点云数据与预设时间段内的历史点云数据进行匹配，得到满足该点云匹配条件的历史点云数据，并计算初始点云数据与该历史点云数据之间的角度偏差和距离偏差，根据该角度偏差和距离偏差按照公式(1)计算初始点云数据与满足点云匹配条件的历史点云数据之间的点云匹配度，之后以该初始点云数据为基础，按照预设数量的步长变化量修改该初始点云数据，得到预设数量的初始参考点云数据，并计算每个初始参考点云数据与历史点云数据之间的点云匹配度，从初始点云数据和初始参考点云数据中，选取最大点云匹配度对应的点云数据，作为第一点云数据，再以第一点云数据为基础，按照与得到第一点云数据相同的方法得到第二点云数据，若第一点云数据的点云匹配度大于或者等于第二点云数据的点云匹配度，则根据第一点云数据输出机器人的位置和姿态，否则继续以第二点云数据为基础寻找定位优化方向，直到达到最大优化次数为止。通过将当前点云数据与历史点云数据在不同方位的刚性匹配，以及梯度下降的方式查找匹配最优的方位作为定位优化方向，实现在定位优化方向的快速收敛，使得搭载微处理器的移动机器人能够快速准确的实现定位，从而提高定位效率和定位精度。
实施例三：
图3是本发明实施例三提供的一种定位装置的结构示意图，为了便于说明，仅示出了与本发明实施例相关的部分。图3示例的一种定位装置可以是前述实施例一提供的定位方法的执行主体。图3示例的一种定位装置包括：获取模块31、计算模块32、第一修改模块33、第一选取模块34、第二修改模块35、第二选取模块36和第一输出模块37，各功能模块详细说明如下：
获取模块31，用于获取初始点云数据，其中，该初始点云数据包括预设个数的采样点的采样信息，该采样信息包括预设个数的采样点的位置坐标和方向角；
计算模块32，用于计算获取模块31获取到的初始点云数据与预设时间段内的历史点云数据之间的点云匹配度；
第一修改模块33，用于按照预设数量的步长变化量修改获取模块31获取到的初始点云数据，得到预设数量的初始参考点云数据，并计算每个初始参考点云数据与历史点云数据之间的点云匹配度，其中，步长变化量包括位置坐标变化量和方向角变化量；
第一选取模块34，用于根据计算模块32得到的初始点云数据的点云匹配度和第一修改模块33得到的每个初始参考点云数据的点云匹配度，确定最大点云匹配度对应的点云数据，作为第一点云数据；
第二修改模块35，用于按照预设数量的步长变化量修改第一选取模块34得到的第一点云数据，得到预设数量的第一参考点云数据，并计算每个第一参考点云数据与历史点云数据之间的点云匹配度；
第二选取模块36，用于根据第一选取模块34得到的第一点云数据的点云匹配度和第二修改模块35得到的每个第一参考点云数据的点云匹配度，确定最大点云匹配度对应的点云数据，作为第二点云数据；
第一输出模块37，用于若第一选取模块34得到的第一点云数据的点云匹配度大于或者等于第二选取模块36得到的第二点云数据的点云匹配度，则根据第一点云数据输出定位信息，其中，该定位信息包括机器人的位置和姿态。
本实施例提供的一种定位装置中各模块实现各自功能的过程，具体可参考前述图1所示实施例的描述，此处不再赘述。
从上述图3示例的一种定位装置可知，本实施例中，获取包括采样点的位置坐标和方向角的初始点云数据，计算初始点云数据与预设时间段内的历史点云数据之间的点云匹配度，并以该初始点云数据为基础，按照预设数量的步长变化量修改该初始点云数据，得到预设数量的初始参考点云数据，并计算每个初始参考点云数据与历史点云数据之间的点云匹配度，从初始点云数据和初始参考点云数据中，选取最大点云匹配度对应的点云数据，作为第一点云数据，再以第一点云数据为基础，按照与得到第一点云数据相同的方法得到第二点云数据，若第一点云数据的点云匹配度大于或者等于第二点云数据的点云匹配度，则根据第一点云数据输出机器人的位置和姿态，通过将当前点云数据与历史点云数据在不同方位的刚性匹配，以及梯度下降的查找方式匹配最优的方位作为定位优化方向，实现在定位优化方向的快速收敛，使得搭载微处理器的机器人能够快速准确的实现定位，从而提高定位效率和定位精度。
实施例四：
图4是本发明实施例四提供的一种定位装置的结构示意图，为了便于说明，仅示出了与本发明实施例相关的部分。图4示例的一种定位装置可以是前述实施例二提供的定位方法的执行主体。图4示例的一种定位装置包括：获取模块41、计算模块42、第一修改模块43、第一选取模块44、第二修改模块45、第二选取模块46和第一输出模块47，各功能模块详细说明如下：
获取模块41，用于获取初始点云数据，其中，该初始点云数据包括预设个数的采样点的采样信息，该采样信息包括预设个数的采样点的位置坐标和方向角；
计算模块42，用于计算获取模块41获取到的初始点云数据与预设时间段内的历史点云数据之间的点云匹配度；
第一修改模块43，用于按照预设数量的步长变化量修改获取模块41获取到的初始点云数据，得到预设数量的初始参考点云数据，并计算每个初始参考点云数据与历史点云数据之间的点云匹配度，其中，步长变化量包括位置坐标变化量和方向角变化量；
第一选取模块44，用于根据计算模块42得到的初始点云数据的点云匹配度和第一修改模块43得到的每个初始参考点云数据的点云匹配度，确定最大点云匹配度对应的点云数据，作为第一点云数据；
第二修改模块45，用于按照预设数量的步长变化量修改第一点云数据，得到预设数量的第一参考点云数据，并计算每个第一参考点云数据与历史点云数据之间的点云匹配度；
第二选取模块46，用于根据第一点云数据的点云匹配度和第二修改模块45得到的每个第一参考点云数据的点云匹配度，确定最大点云匹配度对应的点云数据，作为第二点云数据；
第一输出模块47，用于若第一点云数据的点云匹配度大于或者等于第二选取模块46得到的第二点云数据的点云匹配度，则根据第一点云数据输出定位信息，其中，该定位信息包括机器人的位置和姿态。
进一步地，获取模块41包括：
采集子模块411，用于采集每个采样点的里程计信息和激光传感器信息；
信息计算子模块412，用于根据采集子模块411得到的里程计信息计算每个采样点的位置坐标和方向角；
生成子模块413，用于将信息计算子模块412得到的位置坐标和方向角与采集子模块411采集的激光传感器信息关联，生成初始点云数据。
进一步地，计算模块42包括：
数据匹配子模块421，用于按照点云匹配条件，将生成子模块413生成的初始点云数据与预设时间段内的历史点云数据进行匹配，得到满足点云匹配条件的历史点云数据，其中，点云匹配条件包括初始点云数据中的每个采样点与历史点云数据中的对应采样点之间的距离的平方和最小；
偏差计算子模块422，用于计算初始点云数据与数据匹配子模块421得到的满足点云匹配条件的历史点云数据之间的角度偏差和距离偏差，其中，该距离偏差包括初始点云数据中的每个采样点与满足点云匹配条件的历史点云数据中的对应采样点之间的距离误差；
匹配度计算子模块423，用于按照公式(2)计算初始点云数据与满足点云匹配条件的历史点云数据之间的点云匹配度：
Figure BDA0001244474900000151
其中，S为点云匹配度，α为预设的可信度参数，θ为偏差计算子模块422计算的角度偏差，β为预设的匹配度参数，Mi为偏差计算子模块422得到的距离误差小于预设的误差阈值的采样点中第i个采样点对应的距离误差，N为偏差计算子模块422得到的距离误差小于预设的误差阈值的采样点的个数。
进一步地，该定位装置还包括：
优化次数获取模块48，用于若第一点云数据的点云匹配度小于第二选取模块46得到的第二点云数据的点云匹配度，则获取当前的优化次数；
重复处理模块49，用于若优化次数获取模块48获取的优化次数未达到预设的次数阈值，则将第二点云数据识别为第一点云数据，返回第二修改模块45执行所述按照预设数量的步长变化量修改第一点云数据，得到预设数量第一参考点云数据，并计算每个第一参考点云数据与历史点云数据之间的点云匹配度。
进一步地，该定位装置还包括：
第二输出模块50，用于若优化次数获取模块48获取的优化次数达到预设的次数阈值，则根据第二选取模块46得到的第二点云数据输出定位信息。
本实施例提供的一种定位装置中各模块实现各自功能的过程，具体可参考前述图2所示实施例的描述，此处不再赘述。
从上述图4示例的一种定位装置可知，本实施例中，首先通过采集里程计信息计算采样点的位置坐标和方向角，并与激光传感器信息关联，生成包含位置坐标和方向角的初始点云数据，然后按照点云匹配条件将初始点云数据与预设时间段内的历史点云数据进行匹配，得到满足该点云匹配条件的历史点云数据，并计算初始点云数据与该历史点云数据之间的角度偏差和距离偏差，根据该角度偏差和距离偏差按照公式(2)计算初始点云数据与满足点云匹配条件的历史点云数据之间的点云匹配度，之后以该初始点云数据为基础，按照预设数量的步长变化量修改该初始点云数据，得到预设数量的初始参考点云数据，并计算每个初始参考点云数据与历史点云数据之间的点云匹配度，从初始点云数据和初始参考点云数据中，选取最大点云匹配度对应的点云数据，作为第一点云数据，再以第一点云数据为基础，按照与得到第一点云数据相同的方法得到第二点云数据，若第一点云数据的点云匹配度大于或者等于第二点云数据的点云匹配度，则根据第一点云数据输出机器人的位置和姿态，否则继续以第二点云数据为基础寻找定位优化方向，直到达到最大优化次数为止。通过将当前点云数据与历史点云数据在不同方位的刚性匹配，以及梯度下降的方式查找匹配最优的方位作为定位优化方向，实现在定位优化方向的快速收敛，使得搭载微处理器的移动机器人能够快速准确的实现定位，从而提高定位效率和定位精度。
需要说明的是，本说明书中的各个实施例均采用递进的方式描述，每一个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同或者相似的部分互相参见即可。对于装置类实施例而言，由于其与方法实施例基本相似，所以描述的比较简单，相关之处参见方法实施例的部分说明即可。
值得注意的是，上述装置实施例中，所包括的各个模块只是按照功能逻辑进行划分的，但并不局限于上述的划分，只要能够实现相应的功能即可；另外，各功能模块的具体名称也只是为了便于相互区分，并不用于限制本发明的保护范围。
本领域普通技术人员可以理解，实现上述各实施例方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成，相应的程序可以存储于一计算机可读取存储介质中，所述的存储介质，如ROM/RAM、磁盘或光盘等。
以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种定位方法，其特征在于，所述定位方法包括：
获取初始点云数据，其中，所述初始点云数据包括预设个数的采样点的采样信息，所述采样信息包括所述采样点的位置坐标和方向角；
计算所述初始点云数据与预设时间段内的历史点云数据之间的点云匹配度；
按照预设数量的步长变化量修改所述初始点云数据，得到所述预设数量的初始参考点云数据，并计算每个所述初始参考点云数据与所述历史点云数据之间的点云匹配度，其中，所述步长变化量包括位置坐标变化量和方向角变化量；
根据所述初始点云数据的点云匹配度和每个所述初始参考点云数据的点云匹配度，确定最大点云匹配度对应的点云数据，作为第一点云数据；
按照所述预设数量的步长变化量修改所述第一点云数据，得到所述预设数量的第一参考点云数据，并计算每个所述第一参考点云数据与所述历史点云数据之间的点云匹配度；
根据所述第一点云数据的点云匹配度和每个所述第一参考点云数据的点云匹配度，确定最大点云匹配度对应的点云数据，作为第二点云数据；
若所述第一点云数据的点云匹配度大于或者等于所述第二点云数据的点云匹配度，则根据所述第一点云数据输出定位信息，其中，所述定位信息包括机器人的位置和姿态。
2.根据权利要求1所述的定位方法，其特征在于，所述获取初始点云数据包括：
采集每个所述采样点的里程计信息和激光传感器信息；
根据所述里程计信息计算每个所述采样点的所述位置坐标和所述方向角；
将所述位置坐标和所述方向角与所述激光传感器信息关联，生成所述初始点云数据。
3.根据权利要求1所述的定位方法，其特征在于，所述计算所述初始点云数据与预设时间段内的历史点云数据之间的点云匹配度包括：
按照点云匹配条件，将所述初始点云数据与所述预设时间段内的历史点云数据进行匹配，得到满足所述点云匹配条件的历史点云数据，其中，所述点云匹配条件包括所述初始点云数据中的每个所述采样点与所述历史点云数据中的对应采样点之间的距离的平方和最小；
计算所述初始点云数据与所述满足所述点云匹配条件的历史点云数据之间的角度偏差和距离偏差，其中，所述距离偏差包括所述初始点云数据中的每个所述采样点与所述满足所述点云匹配条件的历史点云数据中的对应采样点之间的距离误差；所述角度偏差是初始点云数据和历史点云数据之间的中心角度偏差，或者是每个对应采样点之间的角度偏差的综合；
按照如下公式计算所述初始点云数据与所述满足所述点云匹配条件的历史点云数据之间的点云匹配度：
Figure FDA0002372705780000021
其中，S为所述点云匹配度，α为预设的可信度参数，θ为所述角度偏差，β为预设的匹配度参数，Mi为所述距离误差小于预设的误差阈值的所述采样点中第i个采样点对应的距离误差，N为所述距离误差小于预设的误差阈值的所述采样点的个数。
4.根据权利要求1至3任一项所述的定位方法，其特征在于，所述若所述第一点云数据的点云匹配度大于或者等于所述第二点云数据的点云匹配度，则根据所述第一点云数据输出定位信息之后，所述定位方法还包括：
若所述第一点云数据的点云匹配度小于所述第二点云数据的点云匹配度，则获取当前的优化次数；
若所述优化次数未达到预设的次数阈值，则将所述第二点云数据识别为所述第一点云数据，执行所述按照所述预设数量的步长变化量修改所述第一点云数据，得到所述预设数量第一参考点云数据，并计算每个所述第一参考点云数据与所述历史点云数据之间的点云匹配度。
5.根据权利要求4所述的定位方法，其特征在于，所述若所述第一点云数据的点云匹配度小于所述第二点云数据的点云匹配度，则获取当前的优化次数之后，所述定位方法还包括：
若所述优化次数达到所述次数阈值，则根据所述第二点云数据输出所述定位信息。
6.一种定位装置，其特征在于，所述定位装置包括：
获取模块，用于获取初始点云数据，其中，所述初始点云数据包括预设个数的采样点的采样信息，所述采样信息包括所述采样点的位置坐标和方向角；
计算模块，用于计算所述初始点云数据与预设时间段内的历史点云数据之间的点云匹配度；
第一修改模块，用于按照预设数量的步长变化量修改所述初始点云数据，得到所述预设数量的初始参考点云数据，并计算每个所述初始参考点云数据与所述历史点云数据之间的点云匹配度，其中，所述步长变化量包括位置坐标变化量和方向角变化量；
第一选取模块，用于根据所述初始点云数据的点云匹配度和每个所述初始参考点云数据的点云匹配度，确定最大点云匹配度对应的点云数据，作为第一点云数据；
第二修改模块，用于按照所述预设数量的步长变化量修改所述第一点云数据，得到所述预设数量的第一参考点云数据，并计算每个所述第一参考点云数据与所述历史点云数据之间的点云匹配度；
第二选取模块，用于根据所述第一点云数据的点云匹配度和每个所述第一参考点云数据的点云匹配度，确定最大点云匹配度对应的点云数据，作为第二点云数据；
第一输出模块，用于若所述第一点云数据的点云匹配度大于或者等于所述第二点云数据的点云匹配度，则根据所述第一点云数据输出定位信息，其中，所述定位信息包括机器人的位置和姿态。
7.根据权利要求6所述的定位装置，其特征在于，所述获取模块包括：
采集子模块，用于采集每个所述采样点的里程计信息和激光传感器信息；
信息计算子模块，用于根据所述里程计信息计算每个所述采样点的所述位置坐标和所述方向角；
生成子模块，用于将所述位置坐标和所述方向角与所述激光传感器信息关联，生成所述初始点云数据。
8.根据权利要求6所述的定位装置，其特征在于，所述计算模块包括：
数据匹配子模块，用于按照点云匹配条件，将所述初始点云数据与所述预设时间段内的历史点云数据进行匹配，得到满足所述点云匹配条件的历史点云数据，其中，所述点云匹配条件包括所述初始点云数据中的每个所述采样点与所述历史点云数据中的对应采样点之间的距离的平方和最小；
偏差计算子模块，用于计算所述初始点云数据与所述满足所述点云匹配条件的历史点云数据之间的角度偏差和距离偏差，其中，所述距离偏差包括所述初始点云数据中的每个所述采样点与所述满足所述点云匹配条件的历史点云数据中的对应采样点之间的距离误差；所述角度偏差是初始点云数据和历史点云数据之间的中心角度偏差，或者是每个对应采样点之间的角度偏差的综合；
匹配度计算子模块，用于按照如下公式计算所述初始点云数据与所述满足所述点云匹配条件的历史点云数据之间的点云匹配度：
Figure FDA0002372705780000051
其中，S为所述点云匹配度，α为预设的可信度参数，θ为所述角度偏差，β为预设的匹配度参数，Mi为所述距离误差小于预设的误差阈值的所述采样点中第i个采样点对应的距离误差，N为所述距离误差小于预设的误差阈值的所述采样点的个数。
9.根据权利要求6至8任一项所述的定位装置，其特征在于，所述定位装置还包括：
优化次数获取模块，用于若所述第一点云数据的点云匹配度小于所述第二点云数据的点云匹配度，则获取当前的优化次数；
重复处理模块，用于若所述优化次数未达到预设的次数阈值，则将所述第二点云数据识别为所述第一点云数据，执行所述按照所述预设数量的步长变化量修改所述第一点云数据，得到所述预设数量第一参考点云数据，并计算每个所述第一参考点云数据与所述历史点云数据之间的点云匹配度。
10.根据权利要求9所述的定位装置，其特征在于，所述定位装置还包括：
第二输出模块，用于若所述优化次数达到所述次数阈值，则根据所述第二点云数据输出所述定位信息。
CN201710147835.3A 2017-03-13 2017-03-13 一种定位方法和装置 Active CN108571967B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201710147835.3A CN108571967B (zh) 2017-03-13 2017-03-13 一种定位方法和装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710147835.3A CN108571967B (zh) 2017-03-13 2017-03-13 一种定位方法和装置

Publications (2)

Publication Number Publication Date
CN108571967A CN108571967A (zh) 2018-09-25
CN108571967B true CN108571967B (zh) 2020-06-26

Family

ID=63577477

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710147835.3A Active CN108571967B (zh) 2017-03-13 2017-03-13 一种定位方法和装置

Country Status (1)

Country Link
CN (1) CN108571967B (zh)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109540142B (zh) 2018-11-27 2021-04-06 达闼科技（北京）有限公司 一种机器人定位导航的方法、装置、计算设备
CN110954108A (zh) * 2019-12-04 2020-04-03 宁波羽声海洋科技有限公司 一种基于海流的水下匹配导航定位方法、装置及电子设备
CN111340860B (zh) * 2020-02-24 2023-09-19 北京百度网讯科技有限公司 点云数据的配准、更新方法、装置、设备和存储介质
CN111815706B (zh) * 2020-06-23 2023-10-27 熵智科技(深圳)有限公司 面向单品类拆垛的视觉识别方法、装置、设备及介质

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9043069B1 (en) * 2012-11-07 2015-05-26 Google Inc. Methods and systems for scan matching approaches for vehicle heading estimation
CN105806345A (zh) * 2016-05-17 2016-07-27 杭州申昊科技股份有限公司 一种用于变电站巡检机器人激光导航的初始化定位方法
CN106023210A (zh) * 2016-05-24 2016-10-12 百度在线网络技术（北京）有限公司 无人车、无人车定位方法、装置和***
CN106092104A (zh) * 2016-08-26 2016-11-09 深圳微服机器人科技有限公司 一种室内机器人的重定位方法及装置
CN106323273A (zh) * 2016-08-26 2017-01-11 深圳微服机器人科技有限公司 一种机器人重定位方法及装置

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9043069B1 (en) * 2012-11-07 2015-05-26 Google Inc. Methods and systems for scan matching approaches for vehicle heading estimation
CN105806345A (zh) * 2016-05-17 2016-07-27 杭州申昊科技股份有限公司 一种用于变电站巡检机器人激光导航的初始化定位方法
CN106023210A (zh) * 2016-05-24 2016-10-12 百度在线网络技术（北京）有限公司 无人车、无人车定位方法、装置和***
CN106092104A (zh) * 2016-08-26 2016-11-09 深圳微服机器人科技有限公司 一种室内机器人的重定位方法及装置
CN106323273A (zh) * 2016-08-26 2017-01-11 深圳微服机器人科技有限公司 一种机器人重定位方法及装置

Also Published As

Publication number Publication date
CN108571967A (zh) 2018-09-25

Similar Documents

Publication Publication Date Title
CN108571967B (zh) 一种定位方法和装置
CN111121754A (zh) 移动机器人定位导航方法、装置、移动机器人及存储介质
CN112179353B (zh) 自移动机器人的定位方法、装置、机器人及可读存储介质
CN1705861A (zh) 步行者导航设备和程序
CN110969649A (zh) 一种激光点云与地图的匹配评价方法、介质、终端和装置
CN110906924A (zh) 一种定位初始化方法和装置、定位方法和装置及移动装置
Li et al. Spatio-temporal trajectory simplification for inferring travel paths
KR20200010506A (ko) 정적 물체의 로케이션을 결정하기 위한 방법 및 장치
KR20210083198A (ko) 증강 현실 장치 및 위치 설정 방법
CN115290071A (zh) 相对定位融合方法、装置、设备及存储介质
CN112129282B (zh) 一种不同导航方式之间定位结果的转换方法、转换装置
CN113932790A (zh) 一种地图更新方法、装置、***、电子设备及存储介质
CN111474560B (zh) 一种障碍物定位方法、装置及设备
CN117274255A (zh) 数据检测方法、装置、电子设备及存储介质
CN112130166A (zh) 基于反光板网络的agv定位方法及装置
KR20090113746A (ko) 물체의 공간적 의미정보를 이용한 로봇의 자기위치 추정 방법
CN113203424B (zh) 多传感器的数据融合方法、装置及相关设备
JP2011511943A (ja) コンピュータを援用してセンサデータから物体の運動を計算する方法
CN111982115B (zh) 基于惯性导航***的特征点地图构建方法、装置以及介质
JP4210763B2 (ja) 無線ｌａｎ測位とｇｐｓ測位とを併用した移動体の連続的測位方法及び装置、並びに移動体の連続的測位プログラム
Fu et al. Using RFID and INS for indoor positioning
CN108955564B (zh) 激光数据重采样方法及***
CN110967027B (zh) 地图校正方法、装置和电子设备
CN112578369A (zh) 一种不确定性的估计方法、装置、电子设备及存储介质
CN114677284A (zh) 地图构建方法、装置、电子设备及存储介质

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
CP03 Change of name, title or address

Address after: B501, Building F2, TCL Science Park, No. 1001, Zhongshanyuan Road, Shuguang Community, Xili Street, Nanshan District, Shenzhen City, Guangdong Province, 518000

Patentee after: LAUNCH DIGITAL TECHNOLOGY Co.,Ltd.

Address before: 518000 Third Floor, Fengyun Building, Galaxy, No. 5 Xinxi Road, North District, Nanshan High-tech Park, Shenzhen City, Guangdong Province

Patentee before: LAUNCH DIGITAL TECHNOLOGY Co.,Ltd.

CP03 Change of name, title or address