CN109545072A - 地图构建的位姿计算方法、装置、存储介质和系统 - Google Patents
地图构建的位姿计算方法、装置、存储介质和系统 Download PDFInfo
- Publication number
- CN109545072A CN109545072A CN201811371714.8A CN201811371714A CN109545072A CN 109545072 A CN109545072 A CN 109545072A CN 201811371714 A CN201811371714 A CN 201811371714A CN 109545072 A CN109545072 A CN 109545072A
- Authority
- CN
- China
- Prior art keywords
- pose
- point cloud
- target
- rough
- target point
- 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.)
- Granted
Links
- 238000004364 calculation method Methods 0.000 title claims abstract description 112
- 238000001914 filtration Methods 0.000 claims abstract description 42
- 238000010276 construction Methods 0.000 claims description 24
- 238000004590 computer program Methods 0.000 claims description 22
- 239000011159 matrix material Substances 0.000 claims description 21
- 238000005457 optimization Methods 0.000 claims description 20
- 230000009466 transformation Effects 0.000 claims description 15
- 238000000354 decomposition reaction Methods 0.000 claims description 7
- 238000012897 Levenberg–Marquardt algorithm Methods 0.000 claims description 5
- 238000000034 method Methods 0.000 description 13
- 230000006870 function Effects 0.000 description 9
- 230000008569 process Effects 0.000 description 8
- 230000006872 improvement Effects 0.000 description 5
- 230000009286 beneficial effect Effects 0.000 description 4
- 238000010586 diagram Methods 0.000 description 4
- 238000004422 calculation algorithm Methods 0.000 description 1
- 230000001413 cellular effect Effects 0.000 description 1
- 238000013500 data storage Methods 0.000 description 1
- 238000007726 management method Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 239000007787 solid Substances 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G09—EDUCATION; CRYPTOGRAPHY; DISPLAY; ADVERTISING; SEALS
- G09B—EDUCATIONAL OR DEMONSTRATION APPLIANCES; APPLIANCES FOR TEACHING, OR COMMUNICATING WITH, THE BLIND, DEAF OR MUTE; MODELS; PLANETARIA; GLOBES; MAPS; DIAGRAMS
- G09B29/00—Maps; Plans; Charts; Diagrams, e.g. route diagram
- G09B29/003—Maps
- G09B29/005—Map projections or methods associated specifically therewith
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Mathematical Physics (AREA)
- Business, Economics & Management (AREA)
- Educational Administration (AREA)
- Educational Technology (AREA)
- General Physics & Mathematics (AREA)
- Image Processing (AREA)
- Image Analysis (AREA)
Abstract
本发明公开了一种地图构建的位姿计算方法，涉及地图信息采集领域，包括步骤：载入目标点云图；其中，所述目标点云图为机器人在目标位姿获取的点云图；基于预设的分辨率对所述目标点云图进行滤波，以在相应的精度范围内确定所述目标位姿的坐标，得到粗定位姿；判断所述粗定位姿是否达到预设要求；若所述粗定位姿的精度未达到预设要求，调整所述分辨率，并根据所述粗定位姿再次进行所述滤波，以更新所述粗定位姿；若所述粗定位姿的精度达到预设要求，基于所述粗定位姿进行梯度寻优，以确定所述目标位姿。本发明实施例还提供了地图构建的位姿计算装置、存储介质和系统，能有效提高对点云图的处理效率和位姿计算精度。
Description
技术领域
本发明涉及地图信息采集领域，尤其涉及一种地图构建的位姿计算方法、装置、存储介质和系统。
背景技术
栅格地图法作为机器人学中地图的主要表示方法之一，通过将环境划分成一系列的栅格，其中每一栅格给定一个可能值，表示该栅格被障碍物占据的概率，从而构建当前环境的栅格地图。为了根据机器人获取的点云图对栅格地图进行更新，从而得到更加全面而准确的栅格地图，需要对点云图的位姿进行计算。
在现有技术中，通常是根据直接对点云图进行稀疏矩阵求解，从而得到该点云图的位姿。而在实施本发明的过程中，发明人发现，由于直接对点云图进行稀疏矩阵求解所需进行的运算量过大，导致对点云图的处理效率和位姿计算精度受到影响。
发明内容
本发明实施例的目的是提供一种地图构建的位姿计算方法、装置、存储介质和系统，能有效提高对点云图的处理效率和位姿计算精度。
为实现上述目的，本发明实施例提供了一种地图构建的位姿计算方法，包括步骤：
载入目标点云图；其中，所述目标点云图为机器人在目标位姿获取的点云图；
基于预设的分辨率对所述目标点云图进行滤波，以在相应的精度范围内确定所述目标位姿的坐标，得到粗定位姿；
判断所述粗定位姿是否达到预设要求；
若所述粗定位姿的精度未达到预设要求，调整所述分辨率，并根据所述粗定位姿再次进行所述滤波，以更新所述粗定位姿；
若所述粗定位姿的精度达到预设要求，基于所述粗定位姿进行梯度寻优，以确定所述目标位姿。
作为上述方案的改进，所述若所述粗定位姿的精度达到预设要求，基于所述粗定位姿进行梯度寻优，以确定所述目标位姿，包括：
根据所述粗定位姿，构建所述目标点云图与栅格子图的坐标变换公式；
采用Levenberg-Marquardt算法进行对所述坐标变换公式进行梯度寻优，以得到所述目标位姿。
作为上述方案的改进，所述坐标变换公式为：
其中，为所述粗定位姿，为所述目标点云图中有效反射数据的坐标，g(x)为点x在所述栅格子图上的值。
作为上述方案的改进，所述采用Levenberg-Marquardt算法进行对所述坐标变换公式进行梯度寻优，以得到所述目标位姿，包括：
求解梯度更新公式，以得到所述坐标变换公式的梯度更新方向；所述梯度更新公式为：
其中，Fk＝F(pk)，Jk＝F′(pk)，
根据更新步长mk和更新公式，更新所述粗定位姿；其中，所述更新公式为：
所述更新步长mk满足更新步长条件：
以最新的粗定位姿作为所述目标位姿。
作为上述方案的改进，所述更新步长条件还包括：mk≥mk-1。
本发明实施例还提供了一种地图构建的位姿计算装置，包括：
载入模块，用于载入目标点云图；其中，所述目标点云图为机器人在目标位姿获取的点云图；
滤波模块，基于预设的分辨率对所述目标点云图进行滤波，以在相应的精度范围内确定所述目标位姿的坐标，得到粗定位姿；
判断模块，用于判断所述粗定位姿是否达到预设要求；
循环模块，用于若所述粗定位姿的精度未达到预设要求，调整所述分辨率，并根据所述粗定位姿再次进行所述滤波，以更新所述粗定位姿；
寻优模块，用于若所述粗定位姿的精度达到预设要求，基于所述粗定位姿进行梯度寻优，以确定所述目标位姿。
本发明实施例还提供了一种地图构建的位姿计算装置，包括处理器、存储器以及存储在所述存储器中且被配置为由所述处理器执行的计算机程序，所述处理器执行所述计算机程序时实现如上任意一项所述的位姿计算方法。
本发明实施例还提供了一种计算机可读存储介质，所述计算机可读存储介质包括存储的计算机程序，其中，在所述计算机程序运行时控制所述计算机可读存储介质所在设备执行如上任意一项所述的位姿计算方法。
本发明实施例还提供了一种地图构建的位姿计算系统，包括位姿计算装置和服务器；
所述位姿计算装置，用于通过如上任一项所述的位姿计算方法，计算机器人在获取目标点云图时的目标位姿，并将所述目标点云图和所述目标位姿上传至所述服务器；
所述服务器，用于根据所述目标位姿，将所述目标点云图融合到栅格地图中，从而更新所述栅格地图。
作为上述方案的改进，所述服务器对所述栅格地图的更新还包括：
根据已接收到的所述目标点云图和所述目标位姿构建位姿图；
基于所述位姿图构建稀疏矩阵，并采用Cholesky分解进行求解以优化所述已接收到的所述目标点云图和所述目标位姿；其中，在所述稀疏矩阵为对称矩阵时，还需对所述稀疏矩阵的行列顺序进行重排，再进行所述Cholesky分解。
与现有技术相比，本发明公开的地图构建的位姿计算方法、装置、存储介质和系统，基于目标点云图进行滤波，从而得到粗定位姿，并且在所述粗定位姿未达到预设要求的情况下，在已得到的粗定位姿的基础上再次进行滤波，对最终得到的粗定位姿进行梯度寻优，以确定目标位姿的技术方案。由于通过多级分辨率确定所述粗定位姿，减少了确定所述粗定位姿过程中所需的运算量，提高了处理效率，并且对所述粗定位姿进行梯度寻优以确定所述目标位姿，更提高位姿计算精度的同时，也减少了提高位姿计算精度所产生的运算量，解决了现有技术中对点云图的位姿计算效率不高，且精度较低的问题，有效地提高了位姿计算的处理效率和位姿计算的精度。
附图说明
图1是本发明实施例1的一种地图构建的位姿计算方法的流程示意图。
图2是如图1所示的位姿计算方法的步骤S150的流程示意图。
图3是如图2所示的位姿计算方法的步骤S152的流程示意图。
图4是本发明实施例2的一种地图构建的位姿计算装置的结构示意图。
图5是本发明实施例3的一种地图构建的位姿计算装置的结构示意图。
图6是本发明实施例4的一种地图构建的位姿计算系统的结构示意图。
图7是如图6所示的位姿计算系统的服务器对栅格地图进行更新的流程示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本发明实施例1提供了一种地图构建的位姿计算方法，该位姿计算方法可以运行在具有激光接收功能的地图信息采集设备上，例如具有激光接收功能的机器人等。参见图1，是实施例1提供的一种地图构建的位姿计算方法的流程示意图，该内存管理方法包括步骤S110至步骤S150。
S110、载入目标点云图；其中，所述目标点云图为机器人在目标位姿获取的点云图。
本方法中载入的所述目标点云图，可以是由所述机器人根据接收到的反射回来的激光信号，确定所述激光信号的反射位置的对应格点进行构建，从而得到所述目标点云图。可以理解地，也可以是其他可以接收激光信号的设备接收激光信号，并由具有地图运算能力的设备构建所述目标点云图，均不影响本发明可取得的有益效果。
S120、基于预设的分辨率对所述目标点云图进行滤波，以在相应的精度范围内确定所述目标位姿的坐标，得到粗定位姿。
优选地，所述滤波可以是最大值滤波。在实际应用中，可以根据具体情况选取相应的滤波方式，不影响本发明可取得的有益效果。
作为举例，假设所述预设的分辨率为27×27，即以27×27的格点作为所述滤波的窗口大小，对所述目标点云图进行滤波，从而将所述粗定位姿确定在27×27格点构成的方格G1内。可以理解地，所述预设的分辨率不限于上述举例，在实际应用中，可以根据具体情况进行调整，不影响本发明可取得的有益效果。
S130、判断所述粗定位姿是否达到预设要求。
具体地，结合步骤S120的举例，所述预设要求可以是所述粗定位姿指向唯一格点。而在实际应用中，所述预设要求也可以是所述粗定位姿指向3×3格点构成方格，或是其他形状或大小的区域，均不影响本发明可取得的有益效果。
S140、若所述粗定位姿的精度未达到预设要求，调整所述分辨率，并根据所述粗定位姿再次进行所述滤波，以更新所述粗定位姿。
具体地，结合步骤S120和步骤S130的举例，第一次执行所述滤波时，所述粗定位姿的精度达到27×27，而未达到所述预设要求，也即唯一格点，因此需要对所述分辨率进行调整。例如，可以将所述分辨率调整到1×1；优选地，可以是在再次执行所述滤波时，将所述分辨率调整为上一次执行的分辨率的特定倍数，例如第一次执行所述滤波时，所述分辨率为27×27，第二次执行时调整为9×9，第三次执行时调整为3×3，以此类推。
上述举例的流程中，第二次执行所述滤波时，在所述方格G1的基础上，将所述粗定位姿的精度确定到由9×9的格点构成的方格G2中；第三次执行所述滤波时，在所述方格G2的基础上，将所述粗定位姿的精度确定到3×3的格点构成的方格G3中；在第四次执行所述滤波时，在所述方格G3的基础上，将所述粗定位姿的精度确定到1×1的格点构成的方格G4中。更优选地，还可以在得到所述方格G3之后，使用原始地图，将所述粗定位姿的精度确定到所述方格G3中的唯一格点。
S150、若所述粗定位姿的精度达到预设要求，基于所述粗定位姿进行梯度寻优，以确定所述目标位姿。
对所述目标点云图建立二维欧式坐标系，以激光信号的接收点作为坐标原点，x轴指向扫描0°的方向，则所述目标点云图具备一个全局位姿栅格子图由一些格点组成，所述栅格子图也具备位姿，通常为所述栅格子图中第一帧点云图对应的位姿，对所述栅格子图建立二维欧式坐标系，通常为第一帧点云图的坐标系。那么，所述目标点云图相对于所述栅格子图的位姿即为两个坐标系之间的变换关系。如果我们能够得出所述目标点云图相对于所述栅格子图的位姿，那么就能够通过所述栅格子图的全局位姿给出所述目标点云图的全局位姿。
优选地，参见图2，步骤S150可以通过如步骤S151至步骤S152所示的流程执行。
S151、根据所述粗定位姿，构建所述目标点云图与栅格子图的坐标变换公式。
具体地，所述坐标变换公式可以是：
其中，为所述粗定位姿，为所述目标点云图中有效反射数据的坐标，g(x)为点x在所述栅格子图上的值。
S152、采用Levenberg-Marquardt算法进行对所述坐标变换公式进行梯度寻优，以得到所述目标位姿。
具体地，参见图3，步骤S152可以通过如步骤S152a至步骤S152c所示的流程执行。
S152a、求解梯度更新公式，以得到所述坐标变换公式的梯度更新方向；所述梯度更新公式为：
其中，Fk＝F(pk)，Jk＝F′(pk)。更优选地，还包括
S152b、根据更新步长mk和更新公式，更新所述粗定位姿；其中，所述更新公式为：
所述更新步长mk满足更新步长条件：
更优选地，所述更新步长条件还可以进一步包括mk≥mk-1。从而缩小步骤S152b所需要搜索的区间，在不影响算法收敛速度的同时，节省了大量的运算。
S152c、以最新的粗定位姿作为所述目标位姿。
本发明实施例1公开的地图构建的位姿计算方法，基于目标点云图进行滤波，从而得到粗定位姿，并且在所述粗定位姿未达到预设要求的情况下，在已得到的粗定位姿的基础上再次进行滤波，对最终得到的粗定位姿进行梯度寻优，以确定目标位姿的技术方案。由于通过多级分辨率确定所述粗定位姿，减少了确定所述粗定位姿过程中所需的运算量，提高了处理效率，并且对所述粗定位姿进行梯度寻优以确定所述目标位姿，更提高位姿计算精度的同时，也减少了提高位姿计算精度所产生的运算量，解决了现有技术中对点云图的位姿计算效率不高，且精度较低的问题，有效地提高了位姿计算的处理效率和位姿计算的精度。
本发明实施例2提供了一种地图构建的位姿计算装置20。参见图4，本发明实施例2提供的位姿计算装置20包括载入模块21、滤波模块22、判断模块23、循环模块24和寻优模块25。
所述载入模块21，用于载入目标点云图；其中，所述目标点云图为机器人在目标位姿获取的点云图；所述滤波模块22，基于预设的分辨率对所述目标点云图进行滤波，以在相应的精度范围内确定所述目标位姿的坐标，得到粗定位姿；所述判断模块23，用于判断所述粗定位姿是否达到预设要求；所述循环模块24，用于若所述粗定位姿的精度未达到预设要求，调整所述分辨率，并根据所述粗定位姿再次进行所述滤波，以更新所述粗定位姿；所述寻优模块25，用于若所述粗定位姿的精度达到预设要求，基于所述粗定位姿进行梯度寻优，以确定所述目标位姿。
本发明实施例2公开的地图构建的位姿计算装置，基于目标点云图进行滤波，从而得到粗定位姿，并且在所述粗定位姿未达到预设要求的情况下，在已得到的粗定位姿的基础上再次进行滤波，对最终得到的粗定位姿进行梯度寻优，以确定目标位姿的技术方案。由于通过多级分辨率确定所述粗定位姿，减少了确定所述粗定位姿过程中所需的运算量，提高了处理效率，并且对所述粗定位姿进行梯度寻优以确定所述目标位姿，更提高位姿计算精度的同时，也减少了提高位姿计算精度所产生的运算量，解决了现有技术中对点云图的位姿计算效率不高，且精度较低的问题，有效地提高了位姿计算的处理效率和位姿计算的精度。
本发明实施例3提供了另一种地图构建的位姿计算装置30。参见图5，本发明实施例3提供的位姿计算装置30包括：处理器31、存储器32以及存储在所述存储器中并可在所述处理器上运行的计算机程序，例如位姿计算程序。所述处理器31执行所述计算机程序时实现上述各个测试方法实施例中的步骤，例如图1所示的步骤S120。或者，所述处理器执行所述计算机程序时实现上述各装置实施例中各模块的功能，例如上述实施例中所述的位姿计算装置。
示例性的，所述计算机程序可以被分割成一个或多个模块，所述一个或者多个模块被存储在所述存储器32中，并由所述处理器31执行，以完成本发明。所述一个或多个模块可以是能够完成特定功能的一系列计算机程序指令段，该指令段用于描述所述计算机程序在所述位姿计算装置30中的执行过程。例如，所述计算机程序可以被分割成载入模块、滤波模块、判断模块、循环模块和寻优模块，各模块具体功能如下：所述载入模块，用于载入目标点云图；其中，所述目标点云图为机器人在目标位姿获取的点云图；所述滤波模块，基于预设的分辨率对所述目标点云图进行滤波，以在相应的精度范围内确定所述目标位姿的坐标，得到粗定位姿；所述判断模块，用于判断所述粗定位姿是否达到预设要求；所述循环模块，用于若所述粗定位姿的精度未达到预设要求，调整所述分辨率，并根据所述粗定位姿再次进行所述滤波，以更新所述粗定位姿；所述寻优模块，用于若所述粗定位姿的精度达到预设要求，基于所述粗定位姿进行梯度寻优，以确定所述目标位姿。
所述位姿计算装置30可以是桌上型计算机、笔记本、掌上电脑及云端服务器等计算设备。所述位姿计算装置30可包括，但不仅限于，处理器31、存储器32。本领域技术人员可以理解，所述示意图仅仅是图像增强设备的示例，并不构成对位姿计算装置30的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件，例如所述位姿计算装置30还可以包括输入输出设备、网络接入设备、总线等。
所称处理器31可以是中央处理单元(Central Processing Unit，CPU)，还可以是其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现成可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等，所述处理器31是所述位姿计算装置30的控制中心，利用各种接口和线路连接整个位姿计算装置30的各个部分。
所述存储器32可用于存储所述计算机程序和/或模块，所述处理器31通过运行或执行存储在所述存储器32内的计算机程序和/或模块，以及调用存储在存储器32内的数据，实现所述位姿计算装置30的各种功能。所述存储器32可主要包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需的应用程序(比如声音播放功能、图像播放功能等)等；存储数据区可存储根据手机的使用所创建的数据(比如音频数据、电话本等)等。此外，存储器32可以包括高速随机存取存储器，还可以包括非易失性存储器，例如硬盘、内存、插接式硬盘，智能存储卡(Smart Media Card,SMC)，安全数字(SecureDigital,SD)卡，闪存卡(Flash Card)、至少一个磁盘存储器件、闪存器件、或其他易失性固态存储器件。
其中，所述位姿计算装置30集成的模块如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明实现上述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤。其中，所述计算机程序包括计算机程序代码，所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。所述计算机可读介质可以包括：能够携带所述计算机程序代码的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、电载波信号、电信信号以及软件分发介质等。
本发明实施例3公开的地图构建的位姿计算装置和存储介质，基于目标点云图进行滤波，从而得到粗定位姿，并且在所述粗定位姿未达到预设要求的情况下，在已得到的粗定位姿的基础上再次进行滤波，对最终得到的粗定位姿进行梯度寻优，以确定目标位姿的技术方案。由于通过多级分辨率确定所述粗定位姿，减少了确定所述粗定位姿过程中所需的运算量，提高了处理效率，并且对所述粗定位姿进行梯度寻优以确定所述目标位姿，更提高位姿计算精度的同时，也减少了提高位姿计算精度所产生的运算量，解决了现有技术中对点云图的位姿计算效率不高，且精度较低的问题，有效地提高了位姿计算的处理效率和位姿计算的精度。
本发明实施例4还提供了一种地图构建的位姿计算系统40。参见图6，本发明实施例4提供的位姿计算系统40包括位姿计算装置41和服务器42。
所述位姿计算装置41，用于通过如上任一项实施例所述的位姿计算方法，计算机器人在获取目标点云图时的目标位姿，并将所述目标点云图和所述目标位姿上传至所述服务器。
所述服务器42，用于根据所述目标位姿，将所述目标点云图融合到栅格地图中，从而更新所述栅格地图。
具体地，参见图7，所述服务器42对所述栅格地图的更新还可以包括步骤S410至步骤S420。
S410、根据已接收到的所述目标点云图和所述目标位姿构建位姿图。
S420、基于所述位姿图构建稀疏矩阵，并采用Cholesky分解进行求解以优化所述已接收到的所述目标点云图和所述目标位姿；其中，在所述稀疏矩阵为对称矩阵时，还需对所述稀疏矩阵的行列顺序进行重排，再进行所述Cholesky分解。
具体地，所述稀疏矩阵可以是hessian矩阵。为了使分解后的矩阵稀疏度更高，在所述稀疏矩阵为对称矩阵时，还需要对所述稀疏矩阵的行列顺序进行重排，并采用Cholesky分解对重排后的矩阵进行求解。
本发明公开的地图构建的位姿计算系统，基于目标点云图进行滤波，从而得到粗定位姿，并且在所述粗定位姿未达到预设要求的情况下，在已得到的粗定位姿的基础上再次进行滤波，对最终得到的粗定位姿进行梯度寻优，以确定目标位姿的技术方案。由于通过多级分辨率确定所述粗定位姿，减少了确定所述粗定位姿过程中所需的运算量，提高了处理效率，并且对所述粗定位姿进行梯度寻优以确定所述目标位姿，更提高位姿计算精度的同时，也减少了提高位姿计算精度所产生的运算量，解决了现有技术中对点云图的位姿计算效率不高，且精度较低的问题，有效地提高了位姿计算的处理效率和位姿计算的精度。
以上所述是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也视为本发明的保护范围。
Claims (10)
1.一种地图构建的位姿计算方法，其特征在于，包括步骤：
载入目标点云图；其中，所述目标点云图为机器人在目标位姿获取的点云图；
基于预设的分辨率对所述目标点云图进行滤波，以在相应的精度范围内确定所述目标位姿的坐标，得到粗定位姿；
判断所述粗定位姿是否达到预设要求；
若所述粗定位姿的精度未达到预设要求，调整所述分辨率，并根据所述粗定位姿再次进行所述滤波，以更新所述粗定位姿；
若所述粗定位姿的精度达到预设要求，基于所述粗定位姿进行梯度寻优，以确定所述目标位姿。
2.如权利要求1所述的位姿计算方法，其特征在于，所述若所述粗定位姿的精度达到预设要求，基于所述粗定位姿进行梯度寻优，以确定所述目标位姿，包括：
根据所述粗定位姿，构建所述目标点云图与栅格子图的坐标变换公式；
采用Levenberg-Marquardt算法进行对所述坐标变换公式进行梯度寻优，以得到所述目标位姿。
3.如权利要求2所述的位姿计算方法，其特征在于，所述坐标变换公式为：
其中，为所述粗定位姿，为所述目标点云图中有效反射数据的坐标，g(x)为点x在所述栅格子图上的值。
4.如权利要求3所述的位姿计算方法，其特征在于，所述采用Levenberg-Marquardt算法进行对所述坐标变换公式进行梯度寻优，以得到所述目标位姿，包括：
求解梯度更新公式，以得到所述坐标变换公式的梯度更新方向；所述梯度更新公式为：
其中，Fk＝F(pk)，Jk＝F′(pk)，
根据更新步长mk和更新公式，更新所述粗定位姿；其中，所述更新公式为：
所述更新步长mk满足更新步长条件：
以最新的粗定位姿作为所述目标位姿。
5.如权利要求4所述的位姿计算方法，其特征在于，所述更新步长条件还包括：mk≥mk-1。
6.一种地图构建的位姿计算装置，其特征在于，包括：
载入模块，用于载入目标点云图；其中，所述目标点云图为机器人在目标位姿获取的点云图；
滤波模块，基于预设的分辨率对所述目标点云图进行滤波，以在相应的精度范围内确定所述目标位姿的坐标，得到粗定位姿；
判断模块，用于判断所述粗定位姿是否达到预设要求；
循环模块，用于若所述粗定位姿的精度未达到预设要求，调整所述分辨率，并根据所述粗定位姿再次进行所述滤波，以更新所述粗定位姿；
寻优模块，用于若所述粗定位姿的精度达到预设要求，基于所述粗定位姿进行梯度寻优，以确定所述目标位姿。
7.一种地图构建的位姿计算装置，包括处理器、存储器以及存储在所述存储器中且被配置为由所述处理器执行的计算机程序，所述处理器执行所述计算机程序时实现如权利要求1至5中任意一项所述的位姿计算方法。
8.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质包括存储的计算机程序，其中，在所述计算机程序运行时控制所述计算机可读存储介质所在设备执行如权利要求1至5中任意一项所述的位姿计算方法。
9.一种地图构建的位姿计算系统，包括位姿计算装置和服务器；
所述位姿计算装置，用于通过如权利要求1至5任一项所述的位姿计算方法，机器人在获取目标点云图时的目标位姿，并将所述目标点云图和所述目标位姿上传至所述服务器；
所述服务器，用于根据所述目标位姿，将所述目标点云图融合到栅格地图中，从而更新所述栅格地图。
10.如权利要求9所述的位姿计算系统，其特征在于，所述服务器对所述栅格地图的更新还包括：
根据已接收到的所述目标点云图和所述目标位姿构建位姿图；
基于所述位姿图构建稀疏矩阵，并采用Cholesky分解进行求解以优化所述已接收到的所述目标点云图和所述目标位姿；其中，在所述稀疏矩阵为对称矩阵时，还需对所述稀疏矩阵的行列顺序进行重排，再进行所述Cholesky分解。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811371714.8A CN109545072B (zh) | 2018-11-14 | 2018-11-14 | 地图构建的位姿计算方法、装置、存储介质和系统 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811371714.8A CN109545072B (zh) | 2018-11-14 | 2018-11-14 | 地图构建的位姿计算方法、装置、存储介质和系统 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN109545072A true CN109545072A (zh) | 2019-03-29 |
| CN109545072B CN109545072B (zh) | 2021-03-16 |
Family
ID=65847937
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201811371714.8A Active CN109545072B (zh) | 2018-11-14 | 2018-11-14 | 地图构建的位姿计算方法、装置、存储介质和系统 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109545072B (zh) |
Cited By (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111383286A (zh) * | 2020-02-11 | 2020-07-07 | 北京迈格威科技有限公司 | 定位方法、装置、电子设备及可读存储介质 |
| CN111381245A (zh) * | 2020-02-29 | 2020-07-07 | 天津大学 | 激光slam自适应分辨率栅格地图构建方法 |
| CN111552757A (zh) * | 2020-04-30 | 2020-08-18 | 上海商汤临港智能科技有限公司 | 生成电子地图的方法、装置、设备及存储介质 |
| CN111581312A (zh) * | 2020-04-24 | 2020-08-25 | 贵州力创科技发展有限公司 | 一种基于多种位置信息的精度定位方法和装置 |
| CN112123343A (zh) * | 2020-11-25 | 2020-12-25 | 炬星科技（深圳）有限公司 | 点云匹配方法、设备及存储介质 |
| CN112307139A (zh) * | 2019-07-30 | 2021-02-02 | 阿里巴巴集团控股有限公司 | 一种数据处理方法、装置、服务器及存储介质 |
| WO2021056190A1 (en) * | 2019-09-24 | 2021-04-01 | Beijing Didi Infinity Technology And Development Co., Ltd. | Semantic-assisted multi-resolution point cloud registration |
| CN113313765A (zh) * | 2021-05-28 | 2021-08-27 | 上海高仙自动化科技发展有限公司 | 定位方法、装置、电子设备和存储介质 |
| CN114549633A (zh) * | 2021-09-16 | 2022-05-27 | 北京小米移动软件有限公司 | 位姿检测方法、装置、电子设备和存储介质 |
| CN115239899A (zh) * | 2022-06-29 | 2022-10-25 | 北京百度网讯科技有限公司 | 位姿图生成方法、高精地图生成方法和装置 |
| CN115290098A (zh) * | 2022-09-30 | 2022-11-04 | 成都朴为科技有限公司 | 一种基于变步长的机器人定位方法和系统 |
Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101000507A (zh) * | 2006-09-29 | 2007-07-18 | 浙江大学 | 移动机器人在未知环境中同时定位与地图构建的方法 |
| CN104166989A (zh) * | 2014-07-04 | 2014-11-26 | 电子科技大学中山学院 | 一种用于二维激光雷达点云匹配的快速icp方法 |
| CN104537709A (zh) * | 2014-12-15 | 2015-04-22 | 西北工业大学 | 一种基于位姿变化的实时三维重建关键帧确定方法 |
| CN104764457A (zh) * | 2015-04-21 | 2015-07-08 | 北京理工大学 | 一种用于无人车的城市环境构图方法 |
| CN107991683A (zh) * | 2017-11-08 | 2018-05-04 | 华中科技大学 | 一种基于激光雷达的机器人自主定位方法 |
| CN108225341A (zh) * | 2016-12-14 | 2018-06-29 | 乐视汽车（北京）有限公司 | 车辆定位方法 |
| CN108225345A (zh) * | 2016-12-22 | 2018-06-29 | 乐视汽车（北京）有限公司 | 可移动设备的位姿确定方法、环境建模方法及装置 |
| US20180302554A1 (en) * | 2013-03-15 | 2018-10-18 | Fotonation Cayman Limited | Systems and Methods for Synthesizing High Resolution Images Using Image Deconvolution Based on Motion and Depth Information |
| CN108759833A (zh) * | 2018-04-25 | 2018-11-06 | 中国科学院合肥物质科学研究院 | 一种基于先验地图的智能车辆定位方法 |
-
2018
- 2018-11-14 CN CN201811371714.8A patent/CN109545072B/zh active Active
Patent Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101000507A (zh) * | 2006-09-29 | 2007-07-18 | 浙江大学 | 移动机器人在未知环境中同时定位与地图构建的方法 |
| US20180302554A1 (en) * | 2013-03-15 | 2018-10-18 | Fotonation Cayman Limited | Systems and Methods for Synthesizing High Resolution Images Using Image Deconvolution Based on Motion and Depth Information |
| CN104166989A (zh) * | 2014-07-04 | 2014-11-26 | 电子科技大学中山学院 | 一种用于二维激光雷达点云匹配的快速icp方法 |
| CN104537709A (zh) * | 2014-12-15 | 2015-04-22 | 西北工业大学 | 一种基于位姿变化的实时三维重建关键帧确定方法 |
| CN104764457A (zh) * | 2015-04-21 | 2015-07-08 | 北京理工大学 | 一种用于无人车的城市环境构图方法 |
| CN108225341A (zh) * | 2016-12-14 | 2018-06-29 | 乐视汽车（北京）有限公司 | 车辆定位方法 |
| CN108225345A (zh) * | 2016-12-22 | 2018-06-29 | 乐视汽车（北京）有限公司 | 可移动设备的位姿确定方法、环境建模方法及装置 |
| CN107991683A (zh) * | 2017-11-08 | 2018-05-04 | 华中科技大学 | 一种基于激光雷达的机器人自主定位方法 |
| CN108759833A (zh) * | 2018-04-25 | 2018-11-06 | 中国科学院合肥物质科学研究院 | 一种基于先验地图的智能车辆定位方法 |
Non-Patent Citations (2)
| Title |
|---|
| 万幼川，徐景中，赖旭东，张圣望: "基于多分辨率方向预测的LIDAR点云滤波方法", 《武汉大学学报》 * |
| 黄锦诚: "基于三维相机的三维地图创建与优化的关键技术研究", 《中国优秀硕士学位论文全文数据库信息科技辑》 * |
Cited By (16)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112307139A (zh) * | 2019-07-30 | 2021-02-02 | 阿里巴巴集团控股有限公司 | 一种数据处理方法、装置、服务器及存储介质 |
| WO2021056190A1 (en) * | 2019-09-24 | 2021-04-01 | Beijing Didi Infinity Technology And Development Co., Ltd. | Semantic-assisted multi-resolution point cloud registration |
| CN111383286A (zh) * | 2020-02-11 | 2020-07-07 | 北京迈格威科技有限公司 | 定位方法、装置、电子设备及可读存储介质 |
| CN111383286B (zh) * | 2020-02-11 | 2023-10-27 | 北京迈格威科技有限公司 | 定位方法、装置、电子设备及可读存储介质 |
| CN111381245B (zh) * | 2020-02-29 | 2023-06-06 | 天津大学 | 激光slam自适应分辨率栅格地图构建方法 |
| CN111381245A (zh) * | 2020-02-29 | 2020-07-07 | 天津大学 | 激光slam自适应分辨率栅格地图构建方法 |
| CN111581312A (zh) * | 2020-04-24 | 2020-08-25 | 贵州力创科技发展有限公司 | 一种基于多种位置信息的精度定位方法和装置 |
| CN111552757A (zh) * | 2020-04-30 | 2020-08-18 | 上海商汤临港智能科技有限公司 | 生成电子地图的方法、装置、设备及存储介质 |
| CN112123343A (zh) * | 2020-11-25 | 2020-12-25 | 炬星科技（深圳）有限公司 | 点云匹配方法、设备及存储介质 |
| CN112123343B (zh) * | 2020-11-25 | 2021-02-05 | 炬星科技（深圳）有限公司 | 点云匹配方法、设备及存储介质 |
| CN113313765A (zh) * | 2021-05-28 | 2021-08-27 | 上海高仙自动化科技发展有限公司 | 定位方法、装置、电子设备和存储介质 |
| CN113313765B (zh) * | 2021-05-28 | 2023-12-01 | 上海高仙自动化科技发展有限公司 | 定位方法、装置、电子设备和存储介质 |
| CN114549633A (zh) * | 2021-09-16 | 2022-05-27 | 北京小米移动软件有限公司 | 位姿检测方法、装置、电子设备和存储介质 |
| CN115239899A (zh) * | 2022-06-29 | 2022-10-25 | 北京百度网讯科技有限公司 | 位姿图生成方法、高精地图生成方法和装置 |
| CN115239899B (zh) * | 2022-06-29 | 2023-09-05 | 北京百度网讯科技有限公司 | 位姿图生成方法、高精地图生成方法和装置 |
| CN115290098A (zh) * | 2022-09-30 | 2022-11-04 | 成都朴为科技有限公司 | 一种基于变步长的机器人定位方法和系统 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN109545072B (zh) | 2021-03-16 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109545072B (zh) | 地图构建的位姿计算方法、装置、存储介质和系统 | |
| CN110307838B (zh) | 机器人重定位方法、装置、计算机可读存储介质及机器人 | |
| CN109543489B (zh) | 基于二维码的定位方法、装置和存储介质 | |
| CN109558471B (zh) | 栅格地图的更新方法、装置、存储介质和系统 | |
| CN113510703B (zh) | 机器人位姿的确定方法、装置、机器人及存储介质 | |
| CN113869293B (zh) | 车道线识别方法、装置、电子设备和计算机可读介质 | |
| CN111915657B (zh) | 一种点云配准方法、装置、电子设备及存储介质 | |
| CN112435193B (zh) | 一种点云数据去噪的方法、装置、存储介质和电子设备 | |
| CN118230231B (zh) | 一种无人车的位姿构建方法、装置、电子设备及存储介质 | |
| CN114779209B (zh) | 一种激光雷达点云体素化方法及装置 | |
| Han et al. | DiLO: Direct light detection and ranging odometry based on spherical range images for autonomous driving | |
| CN109388784A (zh) | 最小熵核密度估计器生成方法、装置和计算机可读存储介质 | |
| CN117058022A (zh) | 深度图像去噪方法、装置、计算机设备及存储介质 | |
| Dan et al. | Multifeature energy optimization framework and parameter adjustment-based nonrigid point set registration | |
| CN111898408A (zh) | 一种快速人脸识别方法及装置 | |
| CN115457202B (zh) | 一种三维模型更新的方法、装置及存储介质 | |
| US20230401670A1 (en) | Multi-scale autoencoder generation method, electronic device and readable storage medium | |
| CN116596750A (zh) | 一种点云处理方法、装置、电子设备及存储介质 | |
| CN111382834B (zh) | 一种置信度比较方法及装置 | |
| CN113868939A (zh) | 一种风功率的概率密度评估方法、装置、设备及介质 | |
| CN108917768B (zh) | 无人机定位导航方法和系统 | |
| CN111461136B (zh) | 数据处理方法、装置、系统、计算机可读存储介质 | |
| CN114359413B (zh) | 用于三维扫描的旋转平台位置参数计算方法及系统 | |
| CN118537605B (zh) | 一种三维点云匹配退化的检测方法、装置及程序产品 | |
| CN115994955B (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 |