CN113514843A - 多子图激光雷达定位方法、系统以及终端 - Google Patents
多子图激光雷达定位方法、系统以及终端 Download PDFInfo
- Publication number
- CN113514843A CN113514843A CN202110777459.2A CN202110777459A CN113514843A CN 113514843 A CN113514843 A CN 113514843A CN 202110777459 A CN202110777459 A CN 202110777459A CN 113514843 A CN113514843 A CN 113514843A
- Authority
- CN
- China
- Prior art keywords
- pose
- current
- track
- subgraph
- sub
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 41
- 238000001514 detection method Methods 0.000 claims abstract description 28
- 238000005457 optimization Methods 0.000 claims description 44
- 230000015654 memory Effects 0.000 claims description 15
- 239000011159 matrix material Substances 0.000 claims description 13
- 230000008859 change Effects 0.000 claims description 10
- 230000008569 process Effects 0.000 claims description 10
- 238000012546 transfer Methods 0.000 claims description 9
- 238000004590 computer program Methods 0.000 claims description 6
- 230000004888 barrier function Effects 0.000 claims description 5
- 238000004364 calculation method Methods 0.000 claims description 5
- 238000013507 mapping Methods 0.000 claims description 5
- 230000001133 acceleration Effects 0.000 claims description 4
- 230000004927 fusion Effects 0.000 abstract description 5
- 230000007613 environmental effect Effects 0.000 abstract description 4
- 238000012545 processing Methods 0.000 description 7
- 230000006870 function Effects 0.000 description 6
- 238000005516 engineering process Methods 0.000 description 5
- 230000009466 transformation Effects 0.000 description 5
- 238000010586 diagram Methods 0.000 description 4
- 230000006399 behavior Effects 0.000 description 3
- 230000009471 action Effects 0.000 description 2
- 238000013473 artificial intelligence Methods 0.000 description 2
- 238000006073 displacement reaction Methods 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
- 230000007704 transition Effects 0.000 description 2
- 238000003491 array Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 235000019800 disodium phosphate Nutrition 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000008030 elimination Effects 0.000 description 1
- 238000003379 elimination reaction Methods 0.000 description 1
- 238000003780 insertion Methods 0.000 description 1
- 230000037431 insertion Effects 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
Images
Classifications
-
- 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/02—Systems using the reflection of electromagnetic waves other than radio waves
- G01S17/06—Systems determining position data of a target
- G01S17/42—Simultaneous measurement of distance and other co-ordinates
-
- 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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
- G01C21/1652—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with ranging devices, e.g. LIDAR or RADAR
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Electromagnetism (AREA)
- Computer Networks & Wireless Communication (AREA)
- Automation & Control Theory (AREA)
- Traffic Control Systems (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明的多子图激光雷达定位方法、系统以及终端，利用激光雷达传感器、里程计和陀螺仪采集的数据构建至少两个子图，并利用当前激光雷达的扫描点与已建好的子图进行扫描匹配，结合多子图可以获得高精度位姿，减少累计误差；利用当前激光雷达扫描的环境信息与已建立的子图进行闭环检测，从而建立约束，消除定位累积误差，解决了累积误差难以消除的问题；并且当机器人受到动态物体干扰而导致错误定位时，开启重定位线程或融合定位线程，有效解决了机器人长时间不知道自己的位置，失去了自主行为能力的问题，增加了机器人的灵活性，还可以有效解决定位错误的情况，增加了机器人工作的稳定性和定位的精准性。
Description
技术领域
本发明涉及机器人领域，特别是涉及一种多子图激光雷达定位方法、系统以及终端。
背景技术
近年来，科技水平日益提高，随之而来的人工智能技术蓬勃发展，其中移动机器人正是人工智能发展的一个重要体现，其目的是令机器人像人类一样具有感知、理解和行动的能力，而机器人想要实现自主行动的能力，定位技术是重要前提。其中激光雷达因测量准确、误差模型简单以及理论研究相对成熟，在定位应用上使用较多，激光雷达定位方法已经成为商业化应用的一种技术。目前激光雷达定位的一些技术中存在定位不准、定位错误以及容易出现定位累积误差，难以消除的问题。另外，还存在机器人位姿丢失后，重定位耗费太多的时间，从而导致机器人在工作过程中不能很好的实现路径规划和导航，进而促使机器人的工作效率降低的问题。
发明内容
鉴于以上所述现有技术的缺点，本发明的目的在于提供一种多子图激光雷达定位方法、系统以及终端，用于解决现有技术中采用的激光雷达定位方法中存在的定位精度不高、定位累计误差、重定位过程中机器人丢失位姿以及重定位耗费太多时间等问题。
为实现上述目的及其他相关目的，本发明提供一种多子图激光雷达定位方法，应用于移动机器人，其上设有激光雷达传感器、里程计和陀螺仪，所述方法包括：基于所述激光雷达传感器采集的当前环境下各障碍物的位置信息以及以由所述里程计与陀螺仪采集的机器人的初始位姿信息构建的地图坐标系，分别构建当前环境下的至少两个局部子图以及一全局子图，以获得由各子图组成的二维地图；其中，所述二维地图包括：各障碍物在所述地图坐标系下的坐标位置信息；将所述激光雷达传感器当前时刻采集的障碍物的位置信息与二维地图中的全局子图所对应的各障碍物在地图坐标系下的坐标位置信息分别进行扫描匹配，以获得所述机器人当前时刻的最优位姿；根据所述最优位姿获得所述机器人当前的最终位姿，并将所述激光雷达传感器当前时刻采集的障碍物的位置信息以所述最终位姿插入至各局部子图和全局子图中，以更新所述二维地图以及各局部子图以及全局子图的轨迹信息；接收到对应障碍物扫描完毕的控制指令时，停止更新所述二维地图，并将最新更新的二维地图作为最终二维地图进行保存；其中，所述最终二维地图包括：各障碍物在所述地图坐标系下的最终坐标位置信息以及各子图的最终轨迹信息；当所述机器人重新启动时，加载保存的所述最终二维地图，并将所述激光雷达传感器当前时刻采集的障碍物的位置信息与所述二维地图中的全局子图的各障碍物在所述地图坐标系下的最终坐标位置信息进行扫描匹配，以获得所述机器人当前时刻的最优位姿以及具有当前轨迹信息的局部子图以及全局子图；利用具有上一轨迹信息的局部子图对当前轨迹信息的中的轨迹节点以及具有当前轨迹信息的完成的旧局部子图和上一轨迹信息已保存的轨迹节点进行闭环检测，建立闭环约束；当具有当前轨迹信息的局部子图所对应的轨迹节点的个数大于优化阈值时，开始计算约束，进行优化求解，获取优化后的当前轨迹下的局部子图的全局位姿和轨迹节点的全局位姿，并更新当前时刻的最优位姿，以获得当前时刻的最终位姿。
于本发明的一实施例中，所述基于所述激光雷达传感器采集的当前环境下各障碍物的位置信息以及以由所述里程计与陀螺仪采集的机器人的初始位姿信息构建的地图坐标系，分别获得构建当前环境下的至少两个局部子图以及一全局子图，以获得由各子图组成的二维地图包括：以由所述里程计与陀螺仪采集的机器人的初始位姿信息构建地图坐标系；将所述激光雷达传感器采集的当前环境下各障碍物的位置信息转换为同一机器人坐标系下，再转换为世界坐标系，以获得所述激光雷达传感器采集的当前环境下各障碍物的运动畸变去除位置信息；基于激光雷达帧阈值，依次构建一或多个局部子图，以获得由各局部子图以及全局子图组成的二维地图。
于本发明的一实施例中，所述将所述激光雷达传感器当前时刻采集的障碍物的位置信息与二维地图中的全局子图所对应的各障碍物在地图坐标系下的坐标位置信息分别进行扫描匹配，以获得所述机器人当前时刻的最优位姿包括：基于由所述里程计以及陀螺仪实时采集的机器人的位姿信息和/或上一时刻的最终位姿，计算获得机器人的加速度以及线速度，以获得机器人的位姿变化信息；根据所述机器人的位姿变化信息，获得当前时刻的预测位姿；将所述预测位姿作为当前时刻激光雷达扫描的中心位姿且以设定的第一搜索范围搜索一或多个候选位姿，并计算在各候选位姿下激光雷达扫描的障碍物的位置信息与所述二维地图上局部子图中各障碍物在所述地图坐标系下的坐标位置信息进行扫描匹配的匹配得分；基于各候选位姿的匹配得分选择当前时刻的初始位姿，再将所述初始位姿进行非线性优化，获得当前时刻的最优位姿以及所述最优位姿的匹配得分。
于本发明的一实施例中，所述根据所述最优位姿获得所述机器人当前的最终位姿包括：基于定位判断条件，判断在定位过程中是否出现定位不准的情况；若未出现定位不准的情况，则将所述最优位姿作为所述机器人的最终位姿；若出现定位不准的情况，则将当前时刻的预测位姿作为当前时刻的最终位姿；将所述激光雷达传感器当前时刻采集的障碍物的位置信息以所述最终位姿插入至各子图中，以更新所述二维地图以及各子图的轨迹信息；其中，所述定位不准条件包括：所述当前时刻的最优位姿的匹配得分小于设定的第一阈值、所述当前时刻的最优位姿和上一时刻的最优位姿的差值大于设定的第二阈值、所述当前时刻的最优位姿与所述当前时刻的预测位姿的差值大于设定的第三阈值、所述当前时刻的预测位姿与上一时刻的最优位姿的差值小于设定的第四阈值情况中的一种或多种。
于本发明的一实施例中，所述当所述机器人重新启动时，加载保存的所述最终二维地图，并将所述激光雷达传感器当前时刻采集的障碍物的位置信息与所述二维地图中的全局子图的各障碍物在所述地图坐标系下的最终坐标位置信息进行扫描匹配，以获得所述机器人当前时刻的最优位姿以及具有当前轨迹信息的局部子图以及全局子图包括：当所述机器人重新启动时，加载保存的所述最终二维地图，打开纯定位模式，停止上一轨迹以创建新的轨迹；计算所述二维地图中的全局子图的中心位置，并将所述中心位置作为当前时刻激光雷达扫描的中心位姿全局搜索一或多个候选位姿，并计算在各候选位姿下激光雷达扫描的障碍物的位置信息与所述全局子图中各障碍物在所述地图坐标系下的坐标位置信息进行扫描匹配的匹配得分；基于各候选位姿的匹配得分选择当前时刻的初始位姿，再将所述初始位姿进行非线性优化，获得当前时刻的最优位姿以及所述最优位姿的匹配得分；根据该最优位姿更新所述最终二维地图中的各子图，并获得具有当前轨迹信息的局部子图以及全局子图。
于本发明的一实施例中，所述利用具有上一轨迹信息的局部子图对具有当前轨迹信息的子图中的轨迹节点以及具有当前轨迹信息的完成的旧局部子图和上一轨迹信息已保存的轨迹节点进行闭环检测，建立闭环约束包括：对具有当前轨迹信息的局部子图的数量进行检验，当所述局部子图数量大于两个时，将最先构建的子图删除掉；当前轨迹下的最新插入的两个局部子图与当前轨迹节点建立观测约束；利用具有上一轨迹信息的各局部子图对当前轨迹信息中的轨迹节点进行闭环检测，并建立闭环约束；完成状态下的旧局部子图和上一轨迹的所有已保存的轨迹节点进行闭环检测，并建立闭环约束。
于本发明的一实施例中，所述当具有当前轨迹信息的局部子图所对应的轨迹节点的个数大于优化阈值时，开始计算约束，进行优化求解，获取优化后的当前轨迹下的局部子图的全局位姿和轨迹节点的全局位姿，并更新当前时刻的最优位姿，以获得当前时刻的最终位姿包括：当具有当前轨迹信息的子图所对应的轨迹节点个数大于优化阈值时，添加优化变量，构建残差，开始计算约束；获取优化后的当前轨迹下的局部子图的全局位姿和轨迹节点的全局位姿，并进行更新；基于更新后的具有当前轨迹信息的局部子图所对应的全局位姿获得转移矩阵，并根据所述转移矩阵更新所述当前时刻的最优位姿，以获得当前时刻的最终位姿。
于本发明的一实施例中，所述轨迹信息包括：当前轨迹的索引和/或轨迹节点的位姿信息。
为实现上述目的及其他相关目的，本发明提供一种多子图激光雷达定位系统，应用于移动机器人，其上设有激光雷达传感器、里程计和陀螺仪，所述系统包括：建图模块，用于基于所述激光雷达传感器采集的当前环境下各障碍物的位置信息以及以由所述里程计与陀螺仪采集的机器人的初始位姿信息构建的地图坐标系，分别构建当前环境下的至少两个局部子图以及一全局子图，以获得由各子图组成的二维地图；其中，所述二维地图包括：各障碍物在所述地图坐标系下的坐标位置信息；最优位姿获取模块，连接所述建图模块，用于将所述激光雷达传感器当前时刻采集的障碍物的位置信息与二维地图中的全局子图所对应的各障碍物在地图坐标系下的坐标位置信息分别进行扫描匹配，以获得所述机器人当前时刻的最优位姿；子图更新模块，连接所述最优位姿获取模块，用于根据所述最优位姿获得所述机器人当前的最终位姿，并将所述激光雷达传感器当前时刻采集的障碍物的位置信息以所述最终位姿插入至各局部子图和全局子图中，以更新所述二维地图以及各局部子图以及全局子图的轨迹信息；最终地图获取模块，连接所述子图更新模块，用于接收到对应障碍物扫描完毕的控制指令时，停止更新所述二维地图，并将最新更新的二维地图作为最终二维地图进行保存；其中，所述最终二维地图包括：各障碍物在所述地图坐标系下的最终坐标位置信息以及各子图的最终轨迹信息；当前轨迹获取模块，连接所述最终地图获取模块，用于当所述机器人重新启动时，加载保存的所述最终二维地图，并将所述激光雷达传感器当前时刻采集的障碍物的位置信息与所述二维地图中的全局子图的各障碍物在所述地图坐标系下的最终坐标位置信息进行扫描匹配，以获得所述机器人当前时刻的最优位姿以及具有当前轨迹信息的局部子图以及全局子图；闭环检测模块，连接所述当前轨迹获取模块，用于利用具有上一轨迹信息的局部子图对当前轨迹信息的中的轨迹节点以及具有当前轨迹信息的完成的旧局部子图和上一轨迹信息已保存的轨迹节点进行闭环检测，建立闭环约束；优化模块，连接所述闭环检测模块，用于当具有当前轨迹信息的局部子图所对应的轨迹节点的个数大于优化阈值时，开始计算约束，进行优化求解，获取优化后的当前轨迹下的局部子图的全局位姿和轨迹节点的全局位姿，并更新当前时刻的最优位姿，以获得当前时刻的最终位姿。
为实现上述目的及其他相关目的，本发明提供一种多子图激光雷达定位终端，包括：存储器，用于存储计算机程序；处理器，用于执行所述的多子图激光雷达定位方法。
如上所述，本发明的一种多子图激光雷达定位方法、系统以及终端，具有以下有益效果：本发明利用激光雷达传感器、里程计和陀螺仪采集的数据构建至少两个子图，并利用当前激光雷达的扫描点与已建好的子图进行扫描匹配，结合多子图可以获得高精度位姿，减少累计误差；利用当前激光雷达扫描的环境信息与已建立的子图进行闭环检测，从而建立约束，消除定位累积误差，解决了累积误差难以消除的问题；并且当机器人受到动态物体干扰而导致错误定位时，开启重定位线程或融合定位线程，有效解决了机器人长时间不知道自己的位置，失去了自主行为能力的问题，增加了机器人的灵活性，还可以有效解决定位错误的情况，增加了机器人工作的稳定性和定位的精准性。
附图说明
图1显示为本发明一实施例中的多子图激光雷达定位方法的流程示意图。
图2显示为本发明一实施例中的多子图激光雷达定位系统的结构示意图。
图3显示为本发明一实施例中的多子图激光雷达定位终端的结构示意图。
具体实施方式
以下通过特定的具体实例说明本发明的实施方式，本领域技术人员可由本说明书所揭露的内容轻易地了解本发明的其他优点与功效。本发明还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本发明的精神下进行各种修饰或改变。需说明的是，在不冲突的情况下，以下实施例及实施例中的特征可以相互组合。
需要说明的是，在下述描述中，参考附图，附图描述了本发明的若干实施例。应当理解，还可使用其他实施例，并且可以在不背离本发明的精神和范围的情况下进行机械组成、结构、电气以及操作上的改变。下面的详细描述不应该被认为是限制性的，并且本发明的实施例的范围仅由公布的专利的权利要求书所限定。这里使用的术语仅是为了描述特定实施例，而并非旨在限制本发明。空间相关的术语，例如“上”、“下”、“左”、“右”、“下面”、“下方”、““下部”、“上方”、“上部”等，可在文中使用以便于说明图中所示的一个元件或特征与另一元件或特征的关系。
在通篇说明书中，当说某部分与另一部分“连接”时，这不仅包括“直接连接”的情形，也包括在其中间把其它元件置于其间而“间接连接”的情形。另外，当说某种部分“包括”某种构成要素时，只要没有特别相反的记载，则并非将其它构成要素，排除在外，而是意味着可以还包括其它构成要素。
其中提到的第一、第二及第三等术语是为了说明多样的部分、成份、区域、层及/或段而使用的，但并非限定于此。这些术语只用于把某部分、成份、区域、层或段区别于其它部分、成份、区域、层或段。因此，以下叙述的第一部分、成份、区域、层或段在不超出本发明范围的范围内，可以言及到第二部分、成份、区域、层或段。
再者，如同在本文中所使用的，单数形式“一”、“一个”和“该”旨在也包括复数形式，除非上下文中有相反的指示。应当进一步理解，术语“包含”、“包括”表明存在所述的特征、操作、元件、组件、项目、种类、和/或组，但不排除一个或多个其他特征、操作、元件、组件、项目、种类、和/或组的存在、出现或添加。此处使用的术语“或”和“和/或”被解释为包括性的，或意味着任一个或任何组合。因此，“A、B或C”或者“A、B和/或C”意味着“以下任一个：A；B；C；A和B；A和C；B和C；A、B和C”。仅当元件、功能或操作的组合在某些方式下内在地互相排斥时，才会出现该定义的例外。
本发明实施例中提供一种多子图激光雷达定位方法，本发明利用激光雷达传感器、里程计和陀螺仪采集的数据构建至少两个子图，并利用当前激光雷达的扫描点与已建好的子图进行扫描匹配，结合多子图的可以获得高精度位姿，减少累计误差；利用当前激光雷达扫描的环境信息与已建立的子图进行闭环检测，从而建立约束，消除定位累积误差，解决了累积误差难以消除的问题；并且当机器人受到动态物体干扰而导致错误定位时，开启重定位线程或融合定位线程，有效解决了机器人长时间不知道自己的位置，失去了自主行为能力的问题，增加了机器人的灵活性，还可以有效解决定位错误的情况，增加了机器人工作的稳定性和定位的精准性。
下面以附图为参考，针对本发明的实施例进行详细说明，以便本发明所述技术领域的技术人员能够容易地实施。本发明可以以多种不同形态体现，并不限于此处说明的实施例。
如图1所示，展示本发明实施例中的多子图激光雷达定位方法的流程示意图。
应用于移动机器人，所述方法包括：
步骤S11：基于所述激光雷达传感器采集的当前环境下各障碍物的位置信息以及以由所述里程计与陀螺仪采集的机器人的初始位姿信息构建的地图坐标系，分别构建当前环境下的至少两个局部子图以及一全局子图，以获得由各子图组成的二维地图。
详细来说，所述二维地图包括：各障碍物在所述地图坐标系下的坐标位置信息。
可选的，步骤S11包括：以由所述里程计与陀螺仪采集的机器人的初始位姿信息构建地图坐标系；将所述激光雷达传感器采集的当前环境下各障碍物的位置信息从激光雷达坐标系转换为同一机器人坐标系下，再转换为世界坐标系，以获得所述激光雷达传感器采集的当前环境下各障碍物的运动畸变去除位置信息；基于激光雷达帧阈值，依次构建一或多个局部子图，以获得由各局部子图以及全局子图组成的二维地图。
其中，在开始建图的时候，会创建一个全局子图，该全局子图从开始到建图结束都一直存在，是用来进行扫描匹配获取位姿的，其他的局部子图只进行插入障碍物操作并更新。其他的局部子图是当激光雷达到达帧到达一定阈值时，就不再插入障碍物信息了。
可选的，所述将所述激光雷达传感器采集的当前环境下各障碍物的位置信息从激光雷达坐标系转换为同一机器人坐标系下，再转换为世界坐标系，以获得所述激光雷达传感器采集的当前环境下各障碍物的运动畸变去除位置信息包括：将所述里程计与陀螺仪实时采集的机器人的最新时刻的位姿信息和上一时刻的位姿信息计算获得机器人的线速度和角速度；对于激光雷达的每一个激光点，获取其时间戳，将当前激光点的时间戳减去第一个激光点的时间戳，得到机器人运动的时间间隔。假设机器人在接收一个激光雷达数据时间段内属于匀速运动，计算出每个激光点的时间戳对应的机器人位姿，以当前机器人位姿建立坐标系，计算出当前激光点时间戳对应的机器人位姿与第一个激光点时间戳对应的机器人位姿的位姿变换矩阵，将位姿变换矩阵乘以当前的激光点坐标，最终将所有的激光点坐标转换到同一机器人坐标系下，从而完成激光雷达运动畸变的去除。
可选的，通过检验局部子图中激光雷达帧的数量来确定构建的局部子图的数量，如果当前子图插入的激光雷达帧数量大于设定的激光雷达阈值，设置局部子图为完成状态，以当前机器人的位姿为原点，创建新的局部子图。
可选的，所述局部子图的原点位姿是局部位姿，也就是没有经过优化之前的位姿，优化后的子图位姿称为全局位姿。
步骤S12：将所述激光雷达传感器当前时刻采集的障碍物的位置信息与二维地图中的全局子图所对应的各障碍物在地图坐标系下的坐标位置信息分别进行扫描匹配，以获得所述机器人当前时刻的最优位姿。
可选的，步骤S12包括：基于由所述里程计以及陀螺仪实时采集的机器人的位姿信息和/ 或上一时刻的最优位姿，计算获得机器人的加速度以及线速度，以获得机器人的位姿变化信息；根据所述机器人的位姿变化信息，获得当前时刻的预测位姿；将所述预测位姿作为当前时刻激光雷达扫描的中心位姿且以设定的第一搜索范围搜索一或多个候选位姿，并计算在各候选位姿下激光雷达扫描的障碍物的位置信息与所述二维地图上全局子图中各障碍物在所述地图坐标系下的坐标位置信息进行扫描匹配的匹配得分；基于各候选位姿的匹配得分选择当前时刻的初始位姿，再将所述初始位姿进行非线性优化，获得当前时刻的最优位姿以及所述最优位姿的匹配得分。
可选的，所述基于由所述里程计以及陀螺仪实时采集的机器人的位姿信息和/或上一时刻的最优位姿，计算获得机器人的加速度以及线速度，以获得机器人的位姿变化信息包括：使用上一时刻激光扫描匹配的机器人的最优位姿前一时刻的里程计以及陀螺仪采集的机器人的位姿信息以及当前最新时刻的里程计以及陀螺仪采集的机器人的位姿信息，获取两个位姿之间的时间差，计算出机器人的线速度和角速度。再根据机器人的线速度和角速度以及利用当前机器人时刻距离上一次激光扫描匹配的时间差，计算出机器人发生的位姿变化信息，例如旋转和位移。
可选的，所述根据所述机器人的位姿变化信息，获得当前时刻的预测位姿的方式包括：将上一时刻机器人的最终位姿加上机器人发生的旋转和位移，即为当前时刻的预测位姿。
可选的，所述将所述预测位姿作为当前时刻激光雷达扫描的中心位姿且以设定的第一搜索范围搜索一或多个候选位姿，并计算在各候选位姿下激光雷达扫描的障碍物的位置信息与所述二维地图上全局子图中各障碍物在所述地图坐标系下的坐标位置信息进行扫描匹配的匹配得分的方式包括：
将预测位姿作为当前激光扫描匹配的中心位姿，以设定的第一搜索范围创建搜索窗，开始具有在激光雷达扫描的各激光点云以及其分别所对应的位姿信息所构成的数据库中搜索一或多个候选位姿；需要说明的是，在搜索之前不仅可以设置搜索窗的大小，还可以设置搜索角度；并计算在每一个候选位姿下，对应的激光点云分别打在各栅格地图上，占据概率值大于预设阈值的激光点个数，然后除以总的激光点个数，即为当前候选位姿的匹配得分；其中，所述预设阈值可根据需求而设定。
可选的，所述基于各候选位姿的匹配得分选择当前时刻的初始位姿，再将所述初始位姿进行非线性优化，获得当前时刻的最优位姿以及所述最优位姿的匹配得分包括：选择匹配得分最高的候选位姿为初始位姿；利用获取到的初始位姿和激光点云，构建最小二乘等式进行非线性优化，添加点云的代价函数、平移的代价函数以及旋转的代价函数以及各自的权重进行优化求解，最终得到的位姿是机器人的最优位姿以及所述最优位姿的匹配得分。
步骤S13：根据所述最优位姿获得所述机器人当前的最终位姿，并将所述激光雷达传感器当前时刻采集的障碍物的位置信息以所述最终位姿插入至各局部子图和全局子图中，以更新所述二维地图以及各局部子图以及全局子图的轨迹信息。
可选的，所述轨迹信息包括：当前轨迹的索引和/或轨迹节点的位姿信息。
可选的，将激光雷达扫描到的障碍物点的运动畸变去除位置信息以所述最终位姿插入到各局部子图以及全局子图中，更新各障碍物点的坐标位置，同时更新机器人到障碍物点中间的空闲区域，将激光雷达帧数累加计数。
可选的，步骤S13包括：基于定位判断条件，判断在定位过程中是否出现定位不准的情况；若符合定位判断条件，则判断为在在定位过程中出现定位不准的情况；若不符合定位判断条件，则判断为在定位过程中未出现定位不准的情况；若未出现定位不准的情况，则将所述最优位姿作为所述机器人的最终位姿；若出现定位不准的情况，则将当前时刻的预测位姿作为当前时刻的最终位姿；将所述激光雷达传感器当前时刻采集的障碍物的位置信息以所述最终位姿插入至各子图中，以更新所述二维地图以及各子图的轨迹信息；其中，所述定位不准条件包括：所述当前时刻的最优位姿的匹配得分小于设定的第一阈值、所述当前时刻的最优位姿和上一时刻的最优位姿的差值大于设定的第二阈值、所述当前时刻的最优位姿与所述当前时刻的预测位姿的差值大于设定的第三阈值、所述当前时刻的预测位姿与上一时刻的最优位姿的差值小于设定的第四阈值情况中的一种或多种
需要注意的是，所述重定位阈值、融合定位阈值、第二阈值、第三阈值以及第四阈值可根据具体需求而设定，在本申请中对此不作限定。
步骤S14：接收到对应障碍物扫描完毕的控制指令时，停止更新所述二维地图，并将最新更新的二维地图作为最终二维地图进行保存。
详细来说，所述最终二维地图包括：各障碍物在所述地图坐标系下的最终坐标位置信息以及各子图的最终轨迹信息。
可选的，各子图建立完成之后，保存子图所对应的轨迹的索引、轨迹节点以及约束的数据。
可选的，建图完成之后，打开纯定位模式，即添加一条新的轨迹，停止更新地图，不再将激光雷达扫描的点插入到二维地图中，并保存地图。
可选的，但接收到对应所有障碍物扫描完毕的控制指令时，停止更新各子图，并将最新更新的二维地图作为最终二维地图进行保存。优选的，保存的地图形式为栅格地图，所述栅格地图包括：地图边界信息、栅格数量以及每个栅格的占据概率。需要说说明的是，所述地图的分别率根据需求而设定，举例来说，为0.05m。
使用不再更新的地图进行后续步骤，相比于现有技术中的再定位过程中不停更新地图的情况来说，定位过程中出现的错误少，精确度高。
步骤S15：当所述机器人重新启动时，加载保存的所述最终二维地图，并将所述激光雷达传感器当前时刻采集的障碍物的位置信息与所述二维地图中的各障碍物在所述地图坐标系下的最终坐标位置信息进行扫描匹配，以获得所述机器人当前时刻的最优位姿以及具有当前轨迹信息的局部子图以及全局子图。
可选的，步骤S15包括：当所述机器人重新启动时，加载保存的所述最终二维地图包括子图的局部位姿、全局位姿、子图的边界以及栅格占据概率等信息，停止上一轨迹以创建新的轨迹；计算所述全局子图的中心位置，并将所述中心位置作为当前时刻激光雷达扫描的中心位姿全局搜索一或多个候选位姿并计算在各候选位姿下激光雷达扫描的障碍物的位置信息与所述二维地图上各子图中各障碍物在所述地图坐标系下的坐标位置信息进行扫描匹配的匹配得分；基于各候选位姿的匹配得分选择当前时刻的初始位姿，再将所述初始位姿进行非线性优化，获得当前时刻的最优位姿以及所述最优位姿的匹配得分；当根据该最优位姿更新所述最终二维地图中的各子图，并获得具有当前轨迹信息的一或多个局部子图以及全局子图。
步骤S16：利用具有上一轨迹信息的局部子图对当前轨迹信息的中的轨迹节点以及具有当前轨迹信息的完成的旧局部子图和上一轨迹信息已保存的轨迹节点进行闭环检测，建立闭环约束。
可选的，步骤S16包括：对具有当前轨迹信息的子图的数量进行检验，当所述局部子图数量大于两个时，将最先构建的局部子图删除掉；当前轨迹下的最新插入的两个局部子图与当前轨迹节点建立观测约束；利用具有上一轨迹信息的各局部子图对当前轨迹信息中的轨迹节点进行闭环检测，并建立闭环约束；利用完成状态下的旧局部子图和上一轨迹的所有已保存的轨迹节点进行闭环检测，并建立闭环约束。
其中，所述轨迹节点由时间戳、局部位姿和激光点云构成的。
可选的，步骤S16包括：维护与优化相关的最新的两个局部子图，对当前轨迹上的局部子图数量进行检验，当创建的局部子图数量大于两个时，将最旧的那个局部子图删除掉，同时删除掉该局部子图上的轨迹节点以及与该子图建立的约束数据；在新的轨迹上当前的轨迹节点与最新插入的两个局部子图都建立观测约束；当前的轨迹节点与具有上一轨迹信息的每个子图基于设置的轨迹节点采样率进行闭环检测，当设置采样率为0.2时，每隔5个轨迹节点进行一次闭环检测，并建立约束；当最新插入的两个局部子图中的旧子图的状态为完成状态时，使用旧子图与上一轨迹保存的所有轨迹节点进行闭环检测，建立闭环约束；
步骤S17：当具有当前轨迹信息的局部子图所对应的轨迹节点的个数大于优化阈值时，开始计算约束，进行优化求解，获取优化后的当前轨迹下的局部子图的全局位姿和轨迹节点的全局位姿，并更新当前时刻的最优位姿，以获得当前时刻的最终位姿。
可选的，步骤S17包括：当具有当前轨迹信息的子图所对应的轨迹节点个数大于优化阈值时，添加优化变量，其中，优化变量包括局部子图的全局位姿和轨迹节点的全局位姿；并基于所述闭环约束优化所述具有当前轨迹信息的局部子图所对应的各位姿信息，以更新具有当前轨迹信息的局部子图所对应的各位姿信息；基于更新后的具有当前轨迹信息的子图所对应的各位姿信息获得转移矩阵，并根据所述转移矩阵更新所述当前时刻的最优位姿，以获得当前时刻的最终位姿。
可选的，所述当具有当前轨迹信息的子图所对应的位姿信息个数大于优化阈值时，添加优化变量，并基于所述闭环约束优化所述具有当前轨迹信息的子图所对应的各位姿信息，以更新具有当前轨迹信息的子图所对应的各位姿信息包括：当优化节点计数器累计的位姿信息个数数量大于设定阈值时，构建最小二乘等式，开始进行非线性优化。添加优化变量，主要包括世界坐标系下的子图位姿和轨迹节点位姿，使用约束构建残差，并计算约束；所述约束主要包括子图位姿与子图内的轨迹节点之间的位姿变换、闭环检测搜索到的轨迹节点位姿与搜索到此轨迹节点位姿的子图位姿之间的位姿变换以及轨迹节点与节点之间的位姿变换中的一种或多种。当约束计算完成后，只获取优化后的当前轨迹下的子图全局位姿和轨迹节点的全局位姿，将旧的子图和轨迹节点的全局位姿更新为最新获取的优化后的全局位姿。
可选的，所述基于更新后的具有当前轨迹信息的子图所对应的各位姿信息获得转移矩阵，并根据所述转移矩阵更新所述当前时刻的最优位姿，以获得当前时刻的最终位姿包括：计算出参与优化的最后一个旧子图的全局位姿到刚优化完成的子图的全局位姿的转移矩阵，将后续未参与优化的轨迹节点以此转移矩阵更新到最新的全局位姿，当前机器人位姿也以此转移矩阵进行更新。
与上述实施例原理相似的是，本发明提供一种多子图激光雷达定位系统。
以下结合附图提供具体实施例：
如图2展示本发明实施例中的一种多子图激光雷达定位系统的结构示意图。
应用于移动机器人，其上设有激光雷达传感器、里程计和陀螺仪，所述系统包括：
建图模块21，用于基于所述激光雷达传感器采集的当前环境下各障碍物的位置信息以及以由所述里程计与陀螺仪采集的机器人的初始位姿信息构建的地图坐标系，分别构建当前环境下的至少两个局部子图以及一全局子图，以获得由各子图组成的二维地图；其中，所述二维地图包括：各障碍物在所述地图坐标系下的坐标位置信息；
最优位姿获取模块22，连接所述建图模块21，用于将所述激光雷达传感器当前时刻采集的障碍物的位置信息与二维地图中的全局子图所对应的各障碍物在地图坐标系下的坐标位置信息分别进行扫描匹配，以获得所述机器人当前时刻的最优位姿；
子图更新模块23，连接所述最优位姿获取模块22，用于根据所述最优位姿获得所述机器人当前的最终位姿，并将所述激光雷达传感器当前时刻采集的障碍物的位置信息以所述最终位姿插入至各局部子图和全局子图中，以更新所述二维地图以及各局部子图以及全局子图的轨迹信息；
最终地图获取模块，连接所述子图更新模块，用于接收到对应障碍物扫描完毕的控制指令时，停止更新所述二维地图，并将最新更新的二维地图作为最终二维地图进行保存；其中，所述最终二维地图包括：各障碍物在所述地图坐标系下的最终坐标位置信息以及各子图的最终轨迹信息；
当前轨迹获取模块，连接所述最终地图获取模块，用于当所述机器人重新启动时，加载保存的所述最终二维地图，并将所述激光雷达传感器当前时刻采集的障碍物的位置信息与所述二维地图中的全局子图的各障碍物在所述地图坐标系下的最终坐标位置信息进行扫描匹配，以获得所述机器人当前时刻的最优位姿以及具有当前轨迹信息的局部子图以及全局子图；
闭环检测模块，连接所述当前轨迹获取模块，用于利用具有上一轨迹信息的局部子图对当前轨迹信息的中的轨迹节点以及具有当前轨迹信息的完成的旧局部子图和上一轨迹信息已保存的轨迹节点进行闭环检测，建立闭环约束；
优化模块，连接所述闭环检测模块，用于当具有当前轨迹信息的局部子图所对应的轨迹节点的个数大于优化阈值时，开始计算约束，进行优化求解，获取优化后的当前轨迹下的局部子图的全局位姿和轨迹节点的全局位姿，并更新当前时刻的最优位姿，以获得当前时刻的最终位姿。
需说明的是，应理解图2系统实施例中的各个模块的划分仅仅是一种逻辑功能的划分，实际实现时可以全部或部分集成到一个物理实体上，也可以物理上分开。且这些模块可以全部以软件通过处理元件调用的形式实现；也可以全部以硬件的形式实现；还可以部分模块通过处理元件调用软件的形式实现，部分模块通过硬件的形式实现；
例如各模块可以是被配置成实施以上方法的一个或多个集成电路，例如：一个或多个特定集成电路(Application Specific Integrated Circuit，简称ASIC)，或，一个或多个微处理器(digital signal processor，简称DSP)，或，一个或者多个现场可编程门阵列(Field Programmable Gate Array，简称FPGA)等。再如，当以上某个模块通过处理元件调度程序代码的形式实现时，该处理元件可以是通用处理器，例如中央处理器(CentralProcessing Unit，简称CPU)或其它可以调用程序代码的处理器。再如，这些模块可以集成在一起，以片上系统(system-on-a-chip，简称SOC)的形式实现。
因此，由于该多子图激光雷达定位系统的实现原理已在前述实施例中进行了叙述，因此此处不作重复赘述。
如图3展示本发明实施例中的多子图激光雷达定位终端30的结构示意图。
所述多子图激光雷达定位终端30包括：存储器31及处理器32所述存储器31用于存储计算机程序；所述处理器32运行计算机程序实现如图1所述的多子图激光雷达定位方法。
可选的，所述存储器31的数量均可以是一或多个，所述处理器32的数量均可以是一或多个，而图3中均以一个为例。
可选的，所述多子图激光雷达定位终端30中的处理器32会按照如图1所述的步骤，将一个或多个以应用程序的进程对应的指令加载到存储器31中，并由处理器32来运行存储在第一存储器31中的应用程序，从而实现如图1所述多子图激光雷达定位方法中的各种功能。
可选的，所述存储器31，可能包括但不限于高速随机存取存储器、非易失性存储器。例如一个或多个磁盘存储设备、闪存设备或其他非易失性固态存储设备；所述处理器32，可能包括但不限于中央处理器(Central Processing Unit，简称CPU)、网络处理器(Network Processor，简称NP)等；还可以是数字信号处理器(Digital SignalProcessing，简称DSP)、专用集成电路 (Application Specific Integrated Circuit，简称ASIC)、现场可编程门阵列(Field－Programmable Gate Array，简称FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。
可选的，所述处理器32可以是通用处理器，包括中央处理器(Central ProcessingUnit，简称CPU)、网络处理器(Network Processor，简称NP)等；还可以是数字信号处理器(Digital Signal Processing，简称DSP)、专用集成电路(Application SpecificIntegrated Circuit，简称ASIC)、现场可编程门阵列(Field－Programmable Gate Array，简称FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。
本发明还提供计算机可读存储介质，存储有计算机程序，所述计算机程序运行时实现如图1所示的多子图激光雷达定位方法。所述计算机可读存储介质可包括，但不限于，软盘、光盘、CD-ROM(只读光盘存储器)、磁光盘、ROM(只读存储器)、RAM(随机存取存储器)、EPROM(可擦除可编程只读存储器)、EEPROM(电可擦除可编程只读存储器)、磁卡或光卡、闪存、或适于存储机器可执行指令的其他类型的介质/机器可读介质。所述计算机可读存储介质可以是未接入计算机设备的产品，也可以是已接入计算机设备使用的部件。
综上所述，本发明多子图激光雷达定位方法、系统以及终端，利用激光雷达传感器、里程计和陀螺仪采集的数据构建至少两个子图，并利用当前激光雷达的扫描点与已建好的子图进行扫描匹配，结合多子图的可以获得高精度位姿，减少累计误差；利用当前激光雷达扫描的环境信息与已建立的子图进行闭环检测，从而建立约束，消除定位累积误差，解决了累积误差难以消除的问题；并且当机器人受到动态物体干扰而导致错误定位时，开启重定位线程或融合定位线程，有效解决了机器人长时间不知道自己的位置，失去了自主行为能力的问题，增加了机器人的灵活性，还可以有效解决定位错误的情况，增加了机器人工作的稳定性和定位的精准性。所以，本发明有效克服了现有技术中的种种缺点而具高度产业利用价值。
上述实施例仅示例性说明本发明的原理及其功效，而非用于限制本发明。任何熟悉此技术的人士皆可在不违背本发明的精神及范畴下，对上述实施例进行修饰或改变。因此，但凡所属技术领域中具有通常知识者在未脱离本发明所揭示的精神与技术思想下所完成的一切等效修饰或改变，仍应由本发明的权利要求所涵盖。
Claims (10)
1.一种多子图激光雷达定位方法，其特征在于，应用于移动机器人，其上设有激光雷达传感器、里程计和陀螺仪，所述方法包括：
基于所述激光雷达传感器采集的当前环境下各障碍物的位置信息以及以由所述里程计与陀螺仪采集的机器人的初始位姿信息构建的地图坐标系，分别构建当前环境下的至少两个局部子图以及一全局子图，以获得由各子图组成的二维地图；其中，所述二维地图包括：各障碍物在所述地图坐标系下的坐标位置信息；
将所述激光雷达传感器当前时刻采集的障碍物的位置信息与二维地图中的全局子图所对应的各障碍物在地图坐标系下的坐标位置信息分别进行扫描匹配，以获得所述机器人当前时刻的最优位姿；
根据所述最优位姿获得所述机器人当前的最终位姿，并将所述激光雷达传感器当前时刻采集的障碍物的位置信息以所述最终位姿插入至各局部子图和全局子图中，以更新所述二维地图以及各局部子图以及全局子图的轨迹信息；
接收到对应障碍物扫描完毕的控制指令时，停止更新所述二维地图，并将最新更新的二维地图作为最终二维地图进行保存；其中，所述最终二维地图包括：各障碍物在所述地图坐标系下的最终坐标位置信息以及各子图的最终轨迹信息；
当所述机器人重新启动时，加载保存的所述最终二维地图，并将所述激光雷达传感器当前时刻采集的障碍物的位置信息与所述二维地图中的全局子图的各障碍物在所述地图坐标系下的最终坐标位置信息进行扫描匹配，以获得所述机器人当前时刻的最优位姿以及具有当前轨迹信息的局部子图以及全局子图；
利用具有上一轨迹信息的局部子图对当前轨迹信息的中的轨迹节点以及具有当前轨迹信息的完成的旧局部子图和上一轨迹信息已保存的轨迹节点进行闭环检测，建立闭环约束；
当具有当前轨迹信息的局部子图所对应的轨迹节点的个数大于优化阈值时，开始计算约束，进行优化求解，获取优化后的当前轨迹下的局部子图的全局位姿和轨迹节点的全局位姿，并更新当前时刻的最优位姿，以获得当前时刻的最终位姿。
2.根据权利要求1中所述的多子图激光雷达定位方法，其特征在于，所述基于所述激光雷达传感器采集的当前环境下各障碍物的位置信息以及以由所述里程计与陀螺仪采集的机器人的初始位姿信息构建的地图坐标系，分别获得构建当前环境下的至少两个局部子图以及一全局子图，以获得由各子图组成的二维地图包括：
以由所述里程计与陀螺仪采集的机器人的初始位姿信息构建地图坐标系；
将所述激光雷达传感器采集的当前环境下各障碍物的位置信息转换为同一机器人坐标系下，再转换为世界坐标系，以获得所述激光雷达传感器采集的当前环境下各障碍物的运动畸变去除位置信息；
基于激光雷达帧阈值，依次构建一或多个局部子图，以获得由各局部子图以及全局子图组成的二维地图。
3.根据权利要求1或2中所述的多子图激光雷达定位方法，其特征在于，所述将所述激光雷达传感器当前时刻采集的障碍物的位置信息与二维地图中的全局子图所对应的各障碍物在地图坐标系下的坐标位置信息分别进行扫描匹配，以获得所述机器人当前时刻的最优位姿包括：
基于由所述里程计以及陀螺仪实时采集的机器人的位姿信息和/或上一时刻的最终位姿，计算获得机器人的加速度以及线速度，以获得机器人的位姿变化信息；
根据所述机器人的位姿变化信息，获得当前时刻的预测位姿；
将所述预测位姿作为当前时刻激光雷达扫描的中心位姿且以设定的第一搜索范围搜索一或多个候选位姿，并计算在各候选位姿下激光雷达扫描的障碍物的位置信息与所述二维地图上局部子图中各障碍物在所述地图坐标系下的坐标位置信息进行扫描匹配的匹配得分；
基于各候选位姿的匹配得分选择当前时刻的初始位姿，再将所述初始位姿进行非线性优化，获得当前时刻的最优位姿以及所述最优位姿的匹配得分。
4.根据权利要求3中所述的多子图激光雷达定位方法，其特征在于，所述根据所述最优位姿获得所述机器人当前的最终位姿包括：
基于定位判断条件，判断在定位过程中是否出现定位不准的情况；
若未出现定位不准的情况，则将所述最优位姿作为所述机器人的最终位姿；
若出现定位不准的情况，则将当前时刻的预测位姿作为当前时刻的最终位姿；
将所述激光雷达传感器当前时刻采集的障碍物的位置信息以所述最终位姿插入至各子图中，以更新所述二维地图以及各子图的轨迹信息；
其中，所述定位不准条件包括：所述当前时刻的最优位姿的匹配得分小于设定的第一阈值、所述当前时刻的最优位姿和上一时刻的最优位姿的差值大于设定的第二阈值、所述当前时刻的最优位姿与所述当前时刻的预测位姿的差值大于设定的第三阈值、所述当前时刻的预测位姿与上一时刻的最优位姿的差值小于设定的第四阈值情况中的一种或多种。
5.根据权利要求1中所述的多子图激光雷达定位方法，其特征在于，所述当所述机器人重新启动时，加载保存的所述最终二维地图，并将所述激光雷达传感器当前时刻采集的障碍物的位置信息与所述二维地图中的全局子图的各障碍物在所述地图坐标系下的最终坐标位置信息进行扫描匹配，以获得所述机器人当前时刻的最优位姿以及具有当前轨迹信息的局部子图以及全局子图包括：
当所述机器人重新启动时，加载保存的所述最终二维地图，打开纯定位模式，停止上一轨迹以创建新的轨迹；
计算所述二维地图中的全局子图的中心位置，并将所述中心位置作为当前时刻激光雷达扫描的中心位姿全局搜索一或多个候选位姿，并计算在各候选位姿下激光雷达扫描的障碍物的位置信息与所述全局子图中各障碍物在所述地图坐标系下的坐标位置信息进行扫描匹配的匹配得分；
基于各候选位姿的匹配得分选择当前时刻的初始位姿，再将所述初始位姿进行非线性优化，获得当前时刻的最优位姿以及所述最优位姿的匹配得分；
根据该最优位姿更新所述最终二维地图中的各子图，并获得具有当前轨迹信息的局部子图以及全局子图。
6.根据权利要求1或5中所述的多子图激光雷达定位方法，其特征在于，所述利用具有上一轨迹信息的局部子图对具有当前轨迹信息的子图中的轨迹节点以及具有当前轨迹信息的完成的旧局部子图和上一轨迹信息已保存的轨迹节点进行闭环检测，建立闭环约束包括：
对具有当前轨迹信息的局部子图的数量进行检验，当所述局部子图数量大于两个时，将最先构建的子图删除掉；
当前轨迹下的最新插入的两个局部子图与当前轨迹节点建立观测约束；
利用具有上一轨迹信息的各局部子图对当前轨迹信息中的轨迹节点进行闭环检测，并建立闭环约束；
完成状态下的旧局部子图和上一轨迹的所有已保存的轨迹节点进行闭环检测，并建立闭环约束。
7.根据权利要求6中所述的多子图激光雷达定位方法，其特征在于，所述当具有当前轨迹信息的局部子图所对应的轨迹节点的个数大于优化阈值时，开始计算约束，进行优化求解，获取优化后的当前轨迹下的局部子图的全局位姿和轨迹节点的全局位姿，并更新当前时刻的最优位姿，以获得当前时刻的最终位姿包括：
当具有当前轨迹信息的子图所对应的轨迹节点个数大于优化阈值时，添加优化变量，构建残差，开始计算约束；
获取优化后的当前轨迹下的局部子图的全局位姿和轨迹节点的全局位姿，并进行更新；
基于更新后的具有当前轨迹信息的局部子图所对应的全局位姿获得转移矩阵，并根据所述转移矩阵更新所述当前时刻的最优位姿，以获得当前时刻的最终位姿。
8.根据权利要求1中所述的多子图激光雷达定位方法，其特征在于，所述轨迹信息包括：当前轨迹的索引和/或轨迹节点的位姿信息。
9.一种多子图激光雷达定位系统，其特征在于，应用于移动机器人，其上设有激光雷达传感器、里程计和陀螺仪，所述系统包括：
建图模块，用于基于所述激光雷达传感器采集的当前环境下各障碍物的位置信息以及以由所述里程计与陀螺仪采集的机器人的初始位姿信息构建的地图坐标系，分别构建当前环境下的至少两个局部子图以及一全局子图，以获得由各子图组成的二维地图；其中，所述二维地图包括：各障碍物在所述地图坐标系下的坐标位置信息；
最优位姿获取模块，连接所述建图模块，用于将所述激光雷达传感器当前时刻采集的障碍物的位置信息与二维地图中的全局子图所对应的各障碍物在地图坐标系下的坐标位置信息分别进行扫描匹配，以获得所述机器人当前时刻的最优位姿；
子图更新模块，连接所述最优位姿获取模块，用于根据所述最优位姿获得所述机器人当前的最终位姿，并将所述激光雷达传感器当前时刻采集的障碍物的位置信息以所述最终位姿插入至各局部子图和全局子图中，以更新所述二维地图以及各局部子图以及全局子图的轨迹信息；
最终地图获取模块，连接所述子图更新模块，用于接收到对应障碍物扫描完毕的控制指令时，停止更新所述二维地图，并将最新更新的二维地图作为最终二维地图进行保存；其中，所述最终二维地图包括：各障碍物在所述地图坐标系下的最终坐标位置信息以及各子图的最终轨迹信息；
当前轨迹获取模块，连接所述最终地图获取模块，用于当所述机器人重新启动时，加载保存的所述最终二维地图，并将所述激光雷达传感器当前时刻采集的障碍物的位置信息与所述二维地图中的全局子图的各障碍物在所述地图坐标系下的最终坐标位置信息进行扫描匹配，以获得所述机器人当前时刻的最优位姿以及具有当前轨迹信息的局部子图以及全局子图；
闭环检测模块，连接所述当前轨迹获取模块，用于利用具有上一轨迹信息的局部子图对当前轨迹信息的中的轨迹节点以及具有当前轨迹信息的完成的旧局部子图和上一轨迹信息已保存的轨迹节点进行闭环检测，建立闭环约束；
优化模块，连接所述闭环检测模块，用于当具有当前轨迹信息的局部子图所对应的轨迹节点的个数大于优化阈值时，开始计算约束，进行优化求解，获取优化后的当前轨迹下的局部子图的全局位姿和轨迹节点的全局位姿，并更新当前时刻的最优位姿，以获得当前时刻的最终位姿。
10.一种多子图激光雷达定位终端，其特征在于，包括：
存储器，用于存储计算机程序；
处理器，用于执行如权利要求1至8中任一项所述的多子图激光雷达定位方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110777459.2A CN113514843A (zh) | 2021-07-09 | 2021-07-09 | 多子图激光雷达定位方法、系统以及终端 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110777459.2A CN113514843A (zh) | 2021-07-09 | 2021-07-09 | 多子图激光雷达定位方法、系统以及终端 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN113514843A true CN113514843A (zh) | 2021-10-19 |
Family
ID=78066874
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110777459.2A Pending CN113514843A (zh) | 2021-07-09 | 2021-07-09 | 多子图激光雷达定位方法、系统以及终端 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113514843A (zh) |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114279434A (zh) * | 2021-12-27 | 2022-04-05 | 驭势科技（北京）有限公司 | 一种建图方法、装置、电子设备和存储介质 |
| CN114440855A (zh) * | 2022-01-19 | 2022-05-06 | 深圳市神州云海智能科技有限公司 | 一种动态场景下定位和地图更新的方法及系统 |
| CN115218892A (zh) * | 2022-09-20 | 2022-10-21 | 成都睿芯行科技有限公司 | 一种基于多传感器融合的轻量级定位与建图方法 |
| CN115685223A (zh) * | 2022-12-15 | 2023-02-03 | 深圳市智绘科技有限公司 | 位置识别方法、装置、电子设备及可读存储介质 |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107741743A (zh) * | 2017-11-06 | 2018-02-27 | 深圳精智机器有限公司 | 改进的图优化slam方法 |
| CN108931245A (zh) * | 2018-08-02 | 2018-12-04 | 上海思岚科技有限公司 | 移动机器人的局部自定位方法及设备 |
| CN110989595A (zh) * | 2019-12-03 | 2020-04-10 | 武汉科技大学 | 一种基于子图归并与位姿优化的激光slam方法 |
| CN111536964A (zh) * | 2020-07-09 | 2020-08-14 | 浙江大华技术股份有限公司 | 机器人定位方法及装置、存储介质 |
| CN112068154A (zh) * | 2020-09-14 | 2020-12-11 | 中科院软件研究所南京软件技术研究院 | 一种激光建图定位方法、装置、存储介质及电子设备 |
-
2021
- 2021-07-09 CN CN202110777459.2A patent/CN113514843A/zh active Pending
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107741743A (zh) * | 2017-11-06 | 2018-02-27 | 深圳精智机器有限公司 | 改进的图优化slam方法 |
| CN108931245A (zh) * | 2018-08-02 | 2018-12-04 | 上海思岚科技有限公司 | 移动机器人的局部自定位方法及设备 |
| CN110989595A (zh) * | 2019-12-03 | 2020-04-10 | 武汉科技大学 | 一种基于子图归并与位姿优化的激光slam方法 |
| CN111536964A (zh) * | 2020-07-09 | 2020-08-14 | 浙江大华技术股份有限公司 | 机器人定位方法及装置、存储介质 |
| CN112068154A (zh) * | 2020-09-14 | 2020-12-11 | 中科院软件研究所南京软件技术研究院 | 一种激光建图定位方法、装置、存储介质及电子设备 |
Non-Patent Citations (2)
| Title |
|---|
| 纪嘉文;杨明欣;: "一种基于多传感融合的室内建图和定位算法", 成都信息工程大学学报, no. 04 * |
| 郑国贤;张磊;张华希;: "机器人室内环境自主探索与地图构建方法", 控制工程, no. 10 * |
Cited By (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114279434A (zh) * | 2021-12-27 | 2022-04-05 | 驭势科技（北京）有限公司 | 一种建图方法、装置、电子设备和存储介质 |
| CN114440855A (zh) * | 2022-01-19 | 2022-05-06 | 深圳市神州云海智能科技有限公司 | 一种动态场景下定位和地图更新的方法及系统 |
| CN114440855B (zh) * | 2022-01-19 | 2022-08-23 | 深圳市神州云海智能科技有限公司 | 一种动态场景下定位和地图更新的方法及系统 |
| CN115218892A (zh) * | 2022-09-20 | 2022-10-21 | 成都睿芯行科技有限公司 | 一种基于多传感器融合的轻量级定位与建图方法 |
| CN115685223A (zh) * | 2022-12-15 | 2023-02-03 | 深圳市智绘科技有限公司 | 位置识别方法、装置、电子设备及可读存储介质 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111536964B (zh) | 机器人定位方法及装置、存储介质 | |
| CN113514843A (zh) | 多子图激光雷达定位方法、系统以及终端 | |
| CN109509210B (zh) | 障碍物跟踪方法和装置 | |
| CN112179330B (zh) | 移动设备的位姿确定方法及装置 | |
| CN113587933B (zh) | 一种基于分支定界算法的室内移动机器人定位方法 | |
| JP6987797B2 (ja) | リアルタイムオンラインエゴモーション推定を有するレーザスキャナ | |
| CN111402339B (zh) | 一种实时定位方法、装置、系统及存储介质 | |
| CN110930495A (zh) | 基于多无人机协作的icp点云地图融合方法、系统、装置及存储介质 | |
| CN109163722B (zh) | 一种仿人机器人路径规划方法及装置 | |
| CN110986956B (zh) | 一种基于改进的蒙特卡洛算法的自主学习全局定位方法 | |
| KR20220025028A (ko) | 시각적 비콘 기반의 비콘 맵 구축 방법, 장치 | |
| US9651388B1 (en) | System and method for improved simultaneous localization and mapping | |
| CN113110455B (zh) | 一种未知初始状态的多机器人协同探索方法、装置及系统 | |
| CN113191550B (zh) | 地图匹配方法及装置 | |
| CN112700479B (zh) | 一种基于cnn点云目标检测的配准方法 | |
| CN111288971B (zh) | 一种视觉定位方法及装置 | |
| CN113516682B (zh) | 一种激光slam的回环检测方法 | |
| CN113503876A (zh) | 多传感器融合的激光雷达定位方法、系统以及终端 | |
| CN115326051A (zh) | 一种基于动态场景的定位方法、装置、机器人及介质 | |
| CN116380039A (zh) | 一种基于固态激光雷达和点云地图的移动机器人导航系统 | |
| CN115962773A (zh) | 一种移动机器人同步定位与地图构建方法、装置和设备 | |
| CN112132951B (zh) | 一种基于视觉的网格语义地图的构建方法 | |
| CA2894863A1 (en) | Indoor localization using crowdsourced data | |
| CN111951341A (zh) | 一种基于rgb-d slam的闭环检测改进方法 | |
| CN113485372B (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 |