CN113503876B - 多传感器融合的激光雷达定位方法、***以及终端 - Google Patents
多传感器融合的激光雷达定位方法、***以及终端 Download PDFInfo
- Publication number
- CN113503876B CN113503876B CN202110777452.0A CN202110777452A CN113503876B CN 113503876 B CN113503876 B CN 113503876B CN 202110777452 A CN202110777452 A CN 202110777452A CN 113503876 B CN113503876 B CN 113503876B
- Authority
- CN
- China
- Prior art keywords
- pose
- positioning
- robot
- repositioning
- current moment
- 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 113
- 230000004927 fusion Effects 0.000 title claims abstract description 75
- 230000008569 process Effects 0.000 claims description 67
- 238000005457 optimization Methods 0.000 claims description 21
- 230000033001 locomotion Effects 0.000 claims description 16
- 230000015654 memory Effects 0.000 claims description 15
- 230000008859 change Effects 0.000 claims description 12
- 230000009466 transformation Effects 0.000 claims description 10
- 238000004590 computer program Methods 0.000 claims description 5
- 238000010276 construction Methods 0.000 claims 2
- 230000004807 localization Effects 0.000 claims 1
- 230000006870 function Effects 0.000 description 12
- 238000012545 processing Methods 0.000 description 12
- 230000006399 behavior Effects 0.000 description 6
- 238000010586 diagram Methods 0.000 description 5
- 238000005516 engineering process Methods 0.000 description 5
- 238000003860 storage Methods 0.000 description 5
- 239000011159 matrix material Substances 0.000 description 4
- 230000001133 acceleration Effects 0.000 description 3
- 238000003491 array Methods 0.000 description 3
- 238000013507 mapping Methods 0.000 description 3
- 238000013473 artificial intelligence Methods 0.000 description 2
- 238000004422 calculation algorithm Methods 0.000 description 2
- 238000004364 calculation method Methods 0.000 description 2
- 238000011161 development Methods 0.000 description 2
- 238000006073 displacement reaction Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000003287 optical effect Effects 0.000 description 2
- 238000013519 translation Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 239000002131 composite material Substances 0.000 description 1
- 230000001186 cumulative effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 230000009467 reduction Effects 0.000 description 1
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/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/18—Stabilised platforms, e.g. by gyroscope
-
- 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
- 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
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Electromagnetism (AREA)
- Computer Networks & Wireless Communication (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Traffic Control Systems (AREA)
Abstract
本发明的多传感器融合的激光雷达定位方法、***以及终端，利用激光雷达传感器、里程计和陀螺仪采集的数据构建二维地图，并利用当前激光雷达的扫描点与已建好的二维地图进行扫描匹配可以获得高精度位姿，且不会存在累计误差；并且当机器人受到动态物体干扰而导致错误定位时，开启重定位线程和/或融合定位线程，有效解决了机器人长时间不知道自己的位置，失去了自主行为能力的问题，增加了机器人的灵活性，还可以有效解决定位错误的情况，并且局部重定位相对于全局重定位在耗时上大大的减少，增加了机器人工作的稳定性和定位的精准性。
Description
技术领域
本发明涉及机器人领域，特别是涉及一种多传感器融合的激光雷达定位方法、***以及终端。
背景技术
在科技日新月异的今天，人工智能成为未来重要的发展方向，而自主机器人正是人工智能发展的一个重要体现。自主机器人往往需要像人一样具有自主移动、判断和行为能力，对于未知的环境中，机器人的定位技术是进行自主行为的重要前提，目前激光雷达定位已经成为商业化应用的一种技术。而当前激光雷达定位的一些技术存在定位精度不高，长时间运行下，容易出现累积误差，且难以消除的问题。另外，还存在机器人定位因受到动态物体的影响而造成错误定位时，使用全局重定位对地图进行搜索消耗大量的时间，而导致的机器人长时间不知道自己的位置，失去了自主行为能力的问题，这种情况会大大的降低机器人的工作效率和灵活性。
发明内容
鉴于以上所述现有技术的缺点，本发明的目的在于提供一种多传感器融合的激光雷达定位方法、***以及终端，用于解决现有技术中采用的激光雷达定位方法中存在的定位精度不高、定位累计误差、重定位过程中机器人丢失位姿以及重定位耗费太多时间等问题。
为实现上述目的及其他相关目的，本发明提供一种多传感器融合的激光雷达定位方法，应用于移动机器人，其上设有激光雷达传感器、里程计和陀螺仪，所述方法包括：基于所述激光雷达传感器采集的当前环境下各障碍物的位置信息以及由所述里程计与陀螺仪实时采集的机器人的位姿信息构建当前环境下的二维地图，并实时更新所述二维地图；当接收到对应障碍物扫描完毕的控制指令时，停止更新所述二维地图，并将最新更新的二维地图作为最终二维地图进行保存；其中，所述最终二维地图包括：各障碍物在所述地图坐标系下的坐标位置信息；加载保存的所述最终二维地图，将所述激光雷达传感器采集的障碍物的位置信息与所述二维地图中的各障碍物在所述地图坐标系下的坐标位置信息进行扫描匹配，以获得所述机器人当前时刻的最优位姿；判断在定位过程中是否出现定位不准的情况；若未出现定位不准的情况，则将所述最优位姿作为所述机器人的最终位姿；若出现定位不准的情况，进行重定位流程和/或融合定位流程，以获得所述机器人的最终位姿。
于本发明的一实施例中，以由所述里程计与陀螺仪采集的机器人的初始位姿信息构建地图坐标系；将所述激光雷达传感器采集的当前环境下各障碍物的位置信息转换为同一机器人坐标系下，再转换为世界坐标系，以获得所述激光雷达传感器采集的当前环境下各障碍物的运动畸变去除位置信息；利用所述里程计与陀螺仪实时采集的机器人的位姿信息，获得机器人当前时刻的预测位姿；将所述预测位姿作为当前时刻激光雷达扫描匹配的中心位姿且以设定的第一搜索范围搜索一或多个候选位姿，并计算在各候选位姿下激光雷达扫描的障碍物的位置信息与所述二维地图上各障碍物在所述地图坐标系下的坐标位置信息进行扫描匹配的匹配得分；基于各候选位姿的匹配得分选择当前时刻的初始位姿，再将所述初始位姿进行非线性优化，获得当前时刻的最优位姿以及所述最优位姿的匹配得分：将所述激光雷达所采集的各障碍物的运动畸变去除位置信息以所述最优位姿***所述地图坐标系，更新获得当前环境的二维地图。
于本发明的一实施例中，所述加载保存的所述最终二维地图，所述加载保存的所述最终二维地图，将所述激光雷达传感器采集的障碍物的位置信息与所述二维地图中的各障碍物在所述地图坐标系下的坐标位置信息进行扫描匹配，以获得所述机器人当前时刻的最优位姿包括：当所述机器人重新启动时，加载保存的所述最终二维地图；基于由所述里程计以及陀螺仪实时采集的机器人的位姿信息和/或上一时刻的最优位姿，计算获得机器人的角速度以及线速度，以获得机器人的位姿变化信息；根据所述机器人的位姿变化信息，获得当前时刻的预测位姿；将所述预测位姿作为当前时刻激光雷达扫描匹配的中心位姿且以设定的第一搜索范围搜索一或多个候选位姿，并计算在各候选位姿下激光雷达扫描的障碍物的位置信息与所述二维地图上各障碍物在所述地图坐标系下的坐标位置信息进行扫描匹配的匹配得分；基于各候选位姿的匹配得分选择当前时刻的初始位姿，再将所述初始位姿进行非线性优化，获得当前时刻的最优位姿以及所述最优位姿的匹配得分。
于本发明的一实施例中，所述判断在定位过程中是否出现定位不准的情况的方式包括：基所述判断在定位过程中是否出现定位不准的情况的方式包括：基于定位不准条件，判断在定位过程中是否出现定位不准的情况；若符合定位不准条件，则判断为在在定位过程中出现定位不准的情况；若不符合定位不准条件，则判断为在定位过程中未出现定位不准的情况；其中，所述定位不准条件包括：所述当前时刻的最优位姿的匹配得分小于设定的第一阈值、所述当前时刻的最优位姿和上一时刻的最优位姿的偏移大于设定的第二阈值、所述当前时刻的最优位姿与所述当前时刻的预测位姿的差值大于设定的第三阈值、所述当前时刻的预测位姿与上一时刻的最优位姿的差值小于设定的第四阈值以及激光雷达扫描点的距离小于设定阈值的个数占比低于40％情况中的一种或多种。
于本发明的一实施例中，所所述若出现定位不准的情况，进行重定位流程和/或融合定位流程，以获得所述机器人的最终位姿的方式包括：若出现定位不准的情况，判断是否符合重定位条件和/或融合定位条件，以获得由符合重定位条件而进行的重定位流程和/或由符合融合定位条件而进行的融合定位流程得到的当前时刻的最优位姿，以获得当前时刻的最终位姿；其中，所述重定位条件包括：所述当前时刻的最优位姿的匹配得分小于设定的重定位得分阈值；所述融合定位条件包括：所述当前时刻的最优位姿的匹配得分小于设定的融合定位得分阈值、所述当前时刻的最优位姿和上一时刻的最优位姿的偏移大于设定的所述第二阈值、所述当前时刻的最优位姿与所述当前时刻的预测位姿的差值大于设定的所述第三阈值、所述当前时刻的预测位姿与上一时刻的最优位姿的差值小于设定的所述第四阈值、重定位流程失败情况以及激光雷达扫描点的距离小于设定阈值的个数占比低于40％情况中的一种或多种。
于本发明的一实施例中，所述重定位流程包括：
将所述预测位姿作为当前时刻激光雷达扫描匹配的中心位姿且以设定的大于第一搜索范围的第二搜索范围搜索一或多个候选位姿，并计算在各候选位姿下激光雷达扫描的障碍物的位置信息与所述二维地图上各障碍物在所述地图坐标系下的坐标位置信息进行扫描匹配的匹配得分；基于各候选位姿的匹配得分选择当前时刻的初始位姿，再将所述初始位姿进行非线性优化，获得当前时刻的局部重定位位姿以及所述局部重定位位姿的匹配得分；若所述局部重定位位姿的匹配得分大于局部重定位的得分阈值，则根据由所述里程计以及陀螺仪采集的机器人的当前时刻的位姿信息以及重定位前的位姿信息计算获得位姿变换信息，再结合所述当前时刻的重定位位姿获得经过局部重定位的当前时刻的最优位姿；若所述局部重定位位姿的匹配得分小于局部重定位的得分阈值，则局部重定位失败，将第二搜索范围替换为大于所述第二搜索范围且小于全局搜索范围的一或多个搜索范围和/或将局部重定位的得分阈值替换为小于所述局部重定位的得分阈值的一或多个得分阈值，以进行一或多次局部重定位。
于本发明的一实施例中，所述重定位流程还包括：若局部重定位次数大于局部重定位次数阈值，则将所述二维地图的中心作为当前时刻激光雷达扫描的中心位姿且以设定的全局搜索范围搜索一或多个候选位姿，并计算在各候选位姿下激光雷达扫描的障碍物的位置信息与所述二维地图上各障碍物在所述地图坐标系下的坐标位置信息进行扫描匹配的匹配得分；
基于各候选位姿的匹配得分选择当前时刻的初始位姿，再将所述初始位姿进行非线性优化，获得当前时刻的全局重定位位姿以及所述全局重定位位姿的匹配得分；
若所述全局重定位位姿的匹配得分大于全局重定位的得分阈值，则根据由所述里程计以及陀螺仪采集的机器人的当前时刻的位姿信息以及重定位前的位姿信息计算获得重定位期间的位姿变换信息，再结合所述当前时刻的全局重定位位姿获得经过全局重定位的当前时刻的最优位姿；若所述全局重定位位姿的匹配得分小于全局重定位的得分阈值，则全局重定位失败，即重定位流程失败。
于本发明的一实施例中，所述融合定位流程包括：将所述当前时刻的预测位姿作为机器人的最优位姿。
为实现上述目的及其他相关目的，本发明提供一种多传感器融合的激光雷达定位***，应用于移动机器人，其上设有激光雷达传感器、里程计和陀螺仪，所述***包括：建图模块，用于基于所述激光雷达传感器采集的当前环境下各障碍物的位置信息以及由所述里程计与陀螺仪实时采集的机器人的位姿信息构建当前环境下的二维地图，并实时更新所述二维地图；地图停止更新模块，连接所述建图模块，用于当接收到对应障碍物扫描完毕的控制指令时，停止更新所述二维地图，并将最新更新的二维地图作为最终二维地图进行保存；其中，所述最终二维地图包括：各障碍物在所述地图坐标系下的坐标位置信息；扫描匹配模块，连接所述地图停止更新模块，用于加载保存的所述最终二维地图，将所述激光雷达传感器采集的障碍物的位置信息与所述二维地图中的各障碍物在所述地图坐标系下的坐标位置信息进行扫描匹配，以获得所述机器人当前时刻的最优位姿；定位判断模块，连接所述扫描匹配模块，用于判断在定位过程中是否出现定位不准的情况；定位准确模块，连接所述定位判断模块，用于若未出现定位不准的情况，则将所述最优位姿作为所述机器人的最终位姿；定位不准确解决模块，连接所述定位判断模块，用于若出现定位不准的情况，进行重定位流程和/或融合定位流程，以获得所述机器人的最终位姿。
为实现上述目的及其他相关目的，本发明提供一种多传感器融合的激光雷达定位终端，包括：存储器，用于存储计算机程序；处理器，用于执行所述的多传感器融合的激光雷达定位方法。
如上所述，本发明的一种多传感器融合的激光雷达定位方法、***以及终端，具有以下有益效果：本发明利用激光雷达传感器、里程计和陀螺仪采集的数据构建二维地图，并利用当前激光雷达的扫描点与已建好的二维地图进行扫描匹配可以获得高精度位姿，且不会存在累计误差；并且当机器人受到动态物体干扰而导致错误定位时，开启重定位线程或融合定位线程，有效解决了机器人长时间不知道自己的位置，失去了自主行为能力的问题，增加了机器人的灵活性，还可以有效解决定位错误的情况，并且局部重定位相对于全局重定位在耗时上大大的减少，增加了机器人工作的稳定性和定位的精准性。
附图说明
图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：当接收到对应障碍物扫描完毕的控制指令时，停止更新所述二维地图，并将最新更新的二维地图作为最终二维地图进行保存。
详细来说，所述最终二维地图包括：各障碍物在所述地图坐标系下的坐标位置信息。
可选的，建图完成之后，打开纯定位模式，停止更新地图，不再将激光雷达扫描的点***到二维地图中，并保存地图。
可选的，但当接收到对应所有障碍物扫描完毕的控制指令时，停止更新所述二维地图，并将最新更新的二维地图作为最终二维地图进行保存。其中保存的地图形式为栅格地图，所述栅格地图包括：地图边界信息、栅格数量以及每个栅格的占据概率。
使用不再更新的地图进行后续步骤，相比于现有技术中的在定位过程中不停更新地图的情况来说，定位过程中不会出现累计误差，定位精度高。
步骤S13：加载保存的所述最终二维地图，将所述激光雷达传感器采集的障碍物的位置信息与所述二维地图中的各障碍物在所述地图坐标系下的坐标位置信息进行扫描匹配，以获得所述机器人当前时刻的最优位姿。
详细来说，机器人重新启动之后，首先加载最终二维地图，设置机器人为纯定位模式，此时激光雷达扫描到的障碍物点不再更新到地图中，也不再对激光雷达帧数累加计数；将所述激光雷达传感器采集的障碍物的位置信息与所述二维地图中的各障碍物在所述地图坐标系下的坐标位置信息进行扫描匹配，以获得所述机器人当前时刻的最优位姿。
可选的，步骤S13包括：当所述机器人重新启动时，加载保存的所述最终二维地图；具体的，机器人重新启动之后，首先加载最终二维地图，设置机器人为纯定位模式，此时激光雷达扫描到的障碍物点不再更新到地图中，也不再对激光雷达帧数累加计数；基于由所述里程计以及陀螺仪实时采集的机器人的位姿信息和/或上一时刻的位姿计算获得机器人的角速度角速度以及线速度，以获得机器人的位姿变化信息；根据所述机器人的位姿变化信息，获得当前时刻的预测位姿；将所述预测位姿作为当前时刻激光雷达扫描的中心位姿且以设定的第一搜索范围搜索一或多个候选位姿，并计算在各候选位姿下激光雷达扫描的障碍物的位置信息与所述二维地图上各障碍物在所述地图坐标系下的坐标位置信息进行扫描匹配的匹配得分；基于各候选位姿的匹配得分选择评分最高的候选位姿作为当前时刻的初始位姿，再将所述初始位姿进行非线性优化，获得当前时刻的最优位姿以及所述最优位姿的匹配得分；优选的，选择匹配得分最高的候选位姿作为初始位姿。
可选的，所述基于由所述里程计以及陀螺仪实时采集的机器人的位姿信息和/或上一时刻的位姿，计算获得机器人的角速度以及线速度，以获得机器人的位姿变化信息包括：使用上一时刻激光扫描匹配的机器人的最优位姿前一时刻的里程计以及陀螺仪采集的机器人的位姿信息以及当前最新时刻的里程计以及陀螺仪采集的机器人的位姿信息，获取两个位姿之间的时间差，计算出机器人的线速度和角速度。再根据机器人的线速度和角速度以及利用当前机器人时刻距离上一次激光扫描匹配的时间差，计算出机器人发生的位姿变化信息，例如旋转和位移。
可选的，所述根据所述机器人的位姿变化信息，获得当前时刻的预测位姿的方式包括：将上一时刻激光扫描匹配的机器人的最优位姿加上机器人发生的旋转和位移，即为当前时刻的预测位姿。
可选的，所述将所述预测位姿作为当前时刻激光雷达扫描的中心位姿且以设定的第一搜索范围搜索一或多个候选位姿，并计算在各候选位姿下激光雷达扫描的障碍物的位置信息与所述二维地图上各障碍物在所述地图坐标系下的坐标位置信息进行扫描匹配的匹配得分的方式包括：
将预测位姿作为当前激光扫描匹配的中心位姿，以设定的第一搜索范围创建搜索窗，开始具有在激光雷达扫描的各激光点云以及其分别所对应的位姿信息所构成的数据库中搜索一或多个候选位姿；需要说明的是，在搜索之前不仅可以设置搜索窗的大小，还可以设置搜索角度；例如建立一个2m x2m的搜索窗，搜索角度为180度。
并计算在每一个候选位姿下，对应的激光点云分别打在栅格地图上，占据概率值大于预设阈值的激光点个数，然后除以总的激光点个数，即为当前候选位姿的匹配得分；其中，所述预设阈值可根据需求而设定，优选的，为0.5。
可选的，所述基于各候选位姿的匹配得分选择当前时刻的初始位姿，再将所述初始位姿进行非线性优化，获得当前时刻的最优位姿以及所述最优位姿的匹配得分包括：选择匹配得分最高的候选位姿为初始位姿；利用获取到的初始位姿其对应的激光点云，构建最小二乘等式进行非线性优化，添加点云的代价函数、平移的代价函数以及旋转的代价函数以及各自的权重进行优化求解，最终得到的位姿是机器人的最优位姿，以及所述最优位姿所对应的匹配得分。
步骤S14：判断在定位过程中是否出现定位不准的情况。
可选的，所述判断在定位过程中是否出现定位不准的情况的方式包括：基于定位不准条件，判断在定位过程中是否出现定位不准的情况；若符合定位不准条件，则判断为在在定位过程中出现定位不准的情况；若不符合定位不准条件，则判断为在定位过程中未出现定位不准的情况；
其中，所述定位不准条件包括：所述当前时刻的最优位姿的匹配得分小于设定的第一阈值、所述当前时刻的最优位姿和上一时刻的最优位姿的偏移大于设定的第二阈值、所述当前时刻的最优位姿与所述当前时刻的预测位姿的差值大于设定的第三阈值、所述当前时刻的预测位姿与上一时刻的最优位姿的差值小于设定的第四阈值以及激光雷达扫描点的距离小于设定阈值的个数占比低于40％情况中的一种或多种。
步骤S15：若未出现定位不准的情况，则将所述最优位姿作为所述机器人的最终位姿。
可选的，若不符合定位不准条件，则判断为在定位过程中未出现定位不准的情况则将由步骤13获得的所述最优位姿作为所述机器人的最终位姿。
步骤S16：若出现定位不准的情况，进行重定位流程和/或融合定位流程，以获得所述机器人的最终位姿。
可选的，若出现定位不准的情况，判断是否符合重定位条件和/或融合定位条件，以获得由符合重定位条件而进行的重定位流程和/或由符合融合定位条件而进行的融合定位流程得到的当前时刻的最优位姿，以获得当前时刻的最终位姿；
具体的，若出现定位不准的情况时，判断是否符合重定位条件和/或融合定位条件，若符合重定位条件，则进行重定位流程；若符合融合定位条件，则进行融合定位流程；再根据由重定位流程和/或融合定位流程获得的最优位姿，再获得当前时刻的最终位姿。
其中，所述重定位条件包括：所述当前时刻的最优位姿的匹配得分小于设定的重定位得分阈值；所述融合定位条件包括：所述当前时刻的最优位姿的匹配得分小于设定的融合定位得分阈值、所述当前时刻的最优位姿和上一时刻的最优位姿的偏移大于设定的所述第二阈值、所述当前时刻的最优位姿与所述当前时刻的预测位姿的差值大于设定的所述第三阈值、所述当前时刻的预测位姿与上一时刻的最优位姿的差值小于设定的所述第四阈值、重定位流程失败以及激光雷达扫描点的距离小于设定阈值的个数占比低于40％情况中的一种或多种。
需要注意的是，所述重定位得分阈值、融合定位得分阈值、第二阈值、第三阈值以及第四阈值可根据具体需求而设定，在本申请中对此不作限定。
可选的，所述重定位得分阈值可以大于所述融合定位得分阈值，也可以等于或小于所述融合定位得分阈值，在此不作限定。举例来说，若重定位得分阈值为0.2，所述融合定位得分阈值为0.3，则当在步骤S13获得的最优位姿的匹配得分小于0.3且大于0.2时，则进行融合定位流程；若小于0.2则进行重定位流程以及融合定位流程。
可选的，所述重定位流程包括：将所述预测位姿作为当前时刻激光雷达扫描的中心位姿且以设定的大于第一搜索范围的第二搜索范围搜索一或多个候选位姿，并计算在各候选位姿下激光雷达扫描的障碍物的位置信息与所述二维地图上各障碍物在所述地图坐标系下的坐标位置信息进行扫描匹配的匹配得分；具体的，将预测位姿作为当前激光扫描匹配的中心位姿，以设定的大于第一搜索范围的第二搜索范围创建搜索窗，开始具有在激光雷达扫描的各激光点云以及其分别所对应的位姿信息所构成的数据库中搜索一或多个候选位姿；并计算在每一个候选位姿下，对应的激光点云分别打在栅格地图上，占据概率值大于预设阈值的激光点个数，然后除以总的激光点个数，即为当前候选位姿的匹配得分；例如建立一个3m x3m的搜索窗，搜索角度为360度。
基于各候选位姿的匹配得分选择当前时刻的初始位姿，再将所述初始位姿进行非线性优化，获得当前时刻的局部重定位位姿以及所述局部重定位位姿的匹配得分；选择匹配得分最高的候选位姿为初始位姿；利用获取到的初始位姿，构建最小二乘等式进行非线性优化，添加点云的代价函数、平移的代价函数以及旋转的代价函数以及各自的权重进行优化求解，最终得到的位姿是机器人的最优位姿以及所述最优位姿的匹配得分。
若所述局部重定位位姿的匹配得分大于局部重定位的得分阈值，则根据由所述里程计以及陀螺仪采集的机器人的当前时刻的位姿信息以及重定位前的位姿信息计算获得位姿变换信息，再结合所述当前时刻的重定位位姿获得经过局部重定位的当前时刻的最优位姿。若所述局部重定位位姿的匹配得分小于局部重定位的得分阈值，则局部重定位失败，将第二搜索范围替换为大于所述第二搜索范围且小于全局搜索范围的一或多个搜索范围和/或将局部将重定位的得分阈值替换为小于所述局部重定位的得分阈值的一或多个得分阈值，以进行一或多次如以上步骤的局部重定位。
可选的，如果重定位失败，则将重定位失败次数加一，当局部重定位连续失败超过3次，降低重定位最小得分阈值为0.50。当局部重定位连续失败超过5次，则扩大搜索窗为5mx 5m, 搜索角度不变，得分阈值不变。当局部重定位连续失败超过10次，则开启全局重定位线程。
可选的，与局部重定位步骤相似的是，所述重定位流程还包括：若局部重定位次数大于局部重定位次数阈值，则将所述二维地图中心位置作为当前时刻激光雷达扫描的中心位姿且以设定的全局搜索范围搜索一或多个候选位姿，并计算在各候选位姿下激光雷达扫描的障碍物的位置信息与所述二维地图上各障碍物在所述地图坐标系下的坐标位置信息进行扫描匹配的匹配得分；基于各候选位姿的匹配得分选择当前时刻的初始位姿，再将所述初始位姿进行非线性优化，获得当前时刻的全局重定位位姿以及所述全局重定位位姿的匹配得分；若所述全局重定位位姿的匹配得分大于全局重定位的得分阈值，则根据由所述里程计以及陀螺仪采集的机器人的当前时刻的位姿信息以及重定位前的位姿信息计算获得重定位期间的位姿变换信息，再结合所述当前时刻的全局重定位位姿获得经过全局重定位的当前时刻的最优位姿；若所述全局重定位位姿的匹配得分小于全局重定位的得分阈值，则全局重定位失败，即重定位流程失败。
可选的，在进行重定位流程时，可将一个任务分割成多个子任务，采用多线程技术，提高了重定位的速度，最高可以提高3倍以上。
可选的，所述融合定位流程包括：
在纯定位模式下，当完成激光扫描匹配之后，对激光扫描匹配的结果进行验证，当所述当前时刻的最优位姿的匹配得分小于设定的融合定位得分阈值融合定位得分阈值、所述当前时刻的最优位姿和上一时刻的最优位姿的偏移大于设定的所述第二阈值、所述当前时刻的最优位姿与所述当前时刻的预测位姿的差值大于设定的所述第三阈值，则认为激光扫描匹配的位姿不正确，使用预测位姿作为机器人的最优位姿。
当所述当前时刻的预测位姿与上一时刻的最优位姿的差值小于设定的所述第四阈值、则认为机器人的运动变化不大，为了避免动态物体导致激光扫描匹配出现定位错误的情况，此时使用预测位姿作为机器人的最优位姿。
当激光雷达扫描匹配得分非常低的情况，开启局部重定位或者全局重定位且失败时，使用预测位姿作为当前机器人的最终位姿。
当激光雷达扫描点的距离小于设定阈值的个数低于40％时，则认为机器人被围住，为了避免机器被围住导致的错误定位，此时使用预测位姿作为当前机器人的最优位姿。
可选的，所述的局部重定位和全局重定位都使用了分支定界算法加速和多线程加速进行了加速搜索。
可选的，所述再根据由重定位流程和/或融合定位流程获得的最优位姿，再获得当前时刻的最终位姿的方式包括以下几种情况：
(1)若只进行重定位流程且获得了最优位姿，则令该位姿为最终位姿；
(2)若只进行融合定位流程且获得了最优位姿，则令该位姿为最终位姿；
(3)若同时进行了重定位流程和融合定位流程获得的最优位姿，则可以根据设置两个位姿的权重，计算获得最终位姿。
(4)若同时进行了重定位流程和融合定位流程获得的最优位姿，直接选择重定位流程获得的最优为最终位姿。
(5)若同时进行了重定位流程和融合定位流程获得的最优位姿，直接选择融合定位流程获得的最优为最终位姿。
使用由符合重定位条件而进行的重定位流程和/或由符合融合定位条件而进行的融合定位流程得到的当前时刻的最优位姿的方案，可以有效避免长时间运行导致的建图错误而出现的定位不准和定位累计误差问题；融合轮式里程计和陀螺仪数据，在机器人局部重定位期间，或者激光雷达扫描匹配不可靠的情况下，使用预测位姿作为当前机器人的位姿，避免了机器人定位错误以及丢失位姿的情况；同时局部重定位相比较于全局重定位，在耗时上也是大大的减少。
可选的，当每一次里程计数据到来时，都会更新机器人的最终位姿。计算当前最新时刻的里程计位姿和上一时刻的里程计位姿的位姿变换矩阵，将位姿变换矩阵乘以上一时刻的机器人最终位姿，更新为当前最新时刻的机器人位姿。
可选的，机器人最终位姿的更新在里程计数据到来、激光雷达数据到来以及重定位成功都会进行更新的。
与上述实施例原理相似的是，本发明提供一种多传感器融合的激光雷达定位***。
以下结合附图提供具体实施例：
如图2展示本发明实施例中的一种多传感器融合的激光雷达定位***的结构示意图。
应用于移动机器人，其上设有激光雷达传感器、里程计和陀螺仪，所述***包括：
建图模块21，用于基于所述激光雷达传感器采集的当前环境下各障碍物的位置信息以及由所述里程计与陀螺仪实时采集的机器人的位姿信息构建当前环境下的二维地图，并实时更新所述二维地图；
地图停止更新模块22，连接所述建图模块21，用于当接收到对应障碍物扫描完毕的控制指令时，停止更新所述二维地图，并将最新更新的二维地图作为最终二维地图进行保存；其中，所述最终二维地图包括：各障碍物在所述地图坐标系下的坐标位置信息；
扫描匹配模块23，连接所述地图停止更新模块22，用于加载保存的所述最终二维地图，将所述激光雷达传感器采集的障碍物的位置信息与所述二维地图中的各障碍物在所述地图坐标系下的坐标位置信息进行扫描匹配，以获得所述机器人当前时刻的最优位姿；
定位判断模块24，连接所述扫描匹配模块23，用于判断在定位过程中是否出现定位不准的情况；
定位准确模块25，连接所述定位判断模块24，用于若未出现定位不准的情况，则将所述最优位姿作为所述机器人的最终位姿；
定位不准确解决模块26，连接所述定位判断模块24，用于若出现定位不准的情况，进行重定位流程和/或融合定位流程，以获得所述机器人的最终位姿。
需说明的是，应理解图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 (9)
1.一种多传感器融合的激光雷达定位方法，其特征在于，应用于移动机器人，其上设有激光雷达传感器、里程计和陀螺仪，所述方法包括：
基于所述激光雷达传感器采集的当前环境下各障碍物的位置信息以及由所述里程计与陀螺仪实时采集的机器人的位姿信息构建当前环境下的二维地图，并实时更新所述二维地图；
当接收到对应障碍物扫描完毕的控制指令时，停止更新所述二维地图，并将最新更新的二维地图作为最终二维地图进行保存；其中，所述最终二维地图包括：各障碍物在地图坐标系下的坐标位置信息；
加载保存的所述最终二维地图，将所述激光雷达传感器采集的障碍物的位置信息与所述二维地图中的各障碍物在所述地图坐标系下的坐标位置信息进行扫描匹配，以获得所述机器人当前时刻的最优位姿；
判断在定位过程中是否出现定位不准的情况；
若未出现定位不准的情况，则将所述最优位姿作为所述机器人的最终位姿；
若出现定位不准的情况，进行重定位流程和/或融合定位流程，以获得所述机器人的最终位姿；
其中，所述基于所述激光雷达传感器采集的当前环境下各障碍物的位置信息以及由所述里程计与陀螺仪实时采集的机器人的位姿信息构建当前环境下的二维地图，并实时更新所述二维地图包括：
以由所述里程计与陀螺仪采集的机器人的初始位姿信息构建地图坐标系；
将所述激光雷达传感器采集的当前环境下各障碍物的位置信息转换为同一机器人坐标系下，再转换为世界坐标系，以获得所述激光雷达传感器采集的当前环境下各障碍物的运动畸变去除位置信息；
利用所述里程计与陀螺仪实时采集的机器人的位姿信息，获得机器人当前时刻的预测位姿；
将所述预测位姿作为当前时刻激光雷达扫描匹配的中心位姿且以设定的第一搜索范围搜索一或多个候选位姿，并计算在各候选位姿下激光雷达扫描的障碍物的位置信息与所述二维地图上各障碍物在所述地图坐标系下的坐标位置信息进行扫描匹配的匹配得分；
基于各候选位姿的匹配得分选择当前时刻的初始位姿，再将所述初始位姿进行非线性优化，获得当前时刻的最优位姿以及所述最优位姿的匹配得分：
将所述激光雷达所采集的各障碍物的运动畸变去除位置信息以所述最优位姿***所述地图坐标系，更新获得当前环境的二维地图。
2.根据权利要求1中所述的多传感器融合的激光雷达定位方法，其特征在于，所述加载保存的所述最终二维地图，将所述激光雷达传感器采集的障碍物的位置信息与所述二维地图中的各障碍物在所述地图坐标系下的坐标位置信息进行扫描匹配，以获得所述机器人当前时刻的最优位姿包括：
当所述机器人重新启动时，加载保存的所述最终二维地图；
基于由所述里程计以及陀螺仪实时采集的机器人的位姿信息和/或上一时刻的最优位姿，计算获得机器人的角速度以及线速度，以获得机器人的位姿变化信息；
根据所述机器人的位姿变化信息，获得当前时刻的预测位姿；
将所述预测位姿作为当前时刻激光雷达扫描匹配的中心位姿且以设定的第一搜索范围搜索一或多个候选位姿，并计算在各候选位姿下激光雷达扫描的障碍物的位置信息与所述二维地图上各障碍物在所述地图坐标系下的坐标位置信息进行扫描匹配的匹配得分；
基于各候选位姿的匹配得分选择当前时刻的初始位姿，再将所述初始位姿进行非线性优化，获得当前时刻的最优位姿以及所述最优位姿的匹配得分。
3.根据权利要求2中所述的多传感器融合的激光雷达定位方法，其特征在于，所述判断在定位过程中是否出现定位不准的情况的方式包括：
基于定位不准条件，判断在定位过程中是否出现定位不准的情况；
若符合定位不准条件，则判断为在定位过程中出现定位不准的情况；
若不符合定位不准条件，则判断为在定位过程中未出现定位不准的情况；
其中，所述定位不准条件包括：所述当前时刻的最优位姿的匹配得分小于设定的第一阈值、所述当前时刻的最优位姿和上一时刻的最优位姿的偏移大于设定的第二阈值、所述当前时刻的最优位姿与所述当前时刻的预测位姿的差值大于设定的第三阈值、所述当前时刻的预测位姿与上一时刻的最优位姿的差值小于设定的第四阈值以及激光雷达扫描点的距离小于设定阈值的个数占比低于40％情况中的一种或多种。
4.根据权利要求3中所述的多传感器融合的激光雷达定位方法，其特征在于，所述若出现定位不准的情况，进行重定位流程和/或融合定位流程，以获得所述机器人的最终位姿的方式包括：
若出现定位不准的情况，判断是否符合重定位条件和/或融合定位条件，以获得由符合重定位条件而进行的重定位流程和/或由符合融合定位条件而进行的融合定位流程得到的当前时刻的最优位姿，以获得当前时刻的最终位姿；
其中，所述重定位条件包括：所述当前时刻的最优位姿的匹配得分小于设定的重定位得分阈值；所述融合定位条件包括：所述当前时刻的最优位姿的匹配得分小于设定的融合定位得分阈值、所述当前时刻的最优位姿和上一时刻的最优位姿的偏移大于设定的所述第二阈值、所述当前时刻的最优位姿与所述当前时刻的预测位姿的差值大于设定的所述第三阈值、所述当前时刻的预测位姿与上一时刻的最优位姿的差值小于设定的所述第四阈值、重定位流程失败以及激光雷达扫描点的距离小于设定阈值的个数占比低于40％情况中的一种或多种。
5.根据权利要求4中所述的多传感器融合的激光雷达定位方法，其特征在于，所述重定位流程包括：
将所述预测位姿作为当前时刻激光雷达扫描匹配的中心位姿且以设定的大于第一搜索范围的第二搜索范围搜索一或多个候选位姿，并计算在各候选位姿下激光雷达扫描的障碍物的位置信息与所述二维地图上各障碍物在所述地图坐标系下的坐标位置信息进行扫描匹配的匹配得分；
基于各候选位姿的匹配得分选择当前时刻的初始位姿，再将所述初始位姿进行非线性优化，获得当前时刻的局部重定位位姿以及所述局部重定位位姿的匹配得分；
若所述局部重定位位姿的匹配得分大于局部重定位的得分阈值，则根据由所述里程计以及陀螺仪采集的机器人的当前时刻的位姿信息以及重定位前的位姿信息计算获得位姿变换信息，再结合所述当前时刻的重定位位姿获得经过局部重定位的当前时刻的最优位姿；
若所述局部重定位位姿的匹配得分小于局部重定位的得分阈值，则局部重定位失败，将第二搜索范围替换为大于所述第二搜索范围且小于全局搜索范围的一或多个搜索范围和/或将局部重定位的得分阈值替换为小于所述局部重定位的得分阈值的一或多个得分阈值，以进行一或多次局部重定位。
6.根据权利要求5中所述的多传感器融合的激光雷达定位方法，其特征在于，所述重定位流程还包括：
若局部重定位次数大于局部重定位次数阈值，则将所述二维地图的中心作为当前时刻激光雷达扫描的中心位姿且以设定的全局搜索范围搜索一或多个候选位姿，并计算在各候选位姿下激光雷达扫描的障碍物的位置信息与所述二维地图上各障碍物在所述地图坐标系下的坐标位置信息进行扫描匹配的匹配得分；
基于各候选位姿的匹配得分选择当前时刻的初始位姿，再将所述初始位姿进行非线性优化，获得当前时刻的全局重定位位姿以及所述全局重定位位姿的匹配得分；
若所述全局重定位位姿的匹配得分大于全局重定位的得分阈值，则根据由所述里程计以及陀螺仪采集的机器人的当前时刻的位姿信息以及重定位前的位姿信息计算获得重定位期间的位姿变换信息，再结合所述当前时刻的全局重定位位姿获得经过全局重定位的当前时刻的最优位姿；
若所述全局重定位位姿的匹配得分小于全局重定位的得分阈值，则全局重定位失败，即重定位流程失败。
7.根据权利要求4中所述的多传感器融合的激光雷达定位方法，其特征在于，所述融合定位流程包括：
将所述当前时刻的预测位姿作为机器人的最优位姿。
8.一种多传感器融合的激光雷达定位***，其特征在于，应用于移动机器人，其上设有激光雷达传感器、里程计和陀螺仪，所述***包括：
建图模块，用于基于所述激光雷达传感器采集的当前环境下各障碍物的位置信息以及由所述里程计与陀螺仪实时采集的机器人的位姿信息构建当前环境下的二维地图，并实时更新所述二维地图；
地图停止更新模块，连接所述建图模块，用于当接收到对应障碍物扫描完毕的控制指令时，停止更新所述二维地图，并将最新更新的二维地图作为最终二维地图进行保存；其中，所述最终二维地图包括：各障碍物在地图坐标系下的坐标位置信息；
扫描匹配模块，连接所述地图停止更新模块，用于加载保存的所述最终二维地图，将所述激光雷达传感器采集的障碍物的位置信息与所述二维地图中的各障碍物在所述地图坐标系下的坐标位置信息进行扫描匹配，以获得所述机器人当前时刻的最优位姿；
定位判断模块，连接所述扫描匹配模块，用于判断在定位过程中是否出现定位不准的情况；
定位准确模块，连接所述定位判断模块，用于若未出现定位不准的情况，则将所述最优位姿作为所述机器人的最终位姿；
定位不准确解决模块，连接所述定位判断模块，用于若出现定位不准的情况，进行重定位流程和/或融合定位流程，以获得所述机器人的最终位姿；
其中，所述基于所述激光雷达传感器采集的当前环境下各障碍物的位置信息以及由所述里程计与陀螺仪实时采集的机器人的位姿信息构建当前环境下的二维地图，并实时更新所述二维地图包括：
以由所述里程计与陀螺仪采集的机器人的初始位姿信息构建地图坐标系；
将所述激光雷达传感器采集的当前环境下各障碍物的位置信息转换为同一机器人坐标系下，再转换为世界坐标系，以获得所述激光雷达传感器采集的当前环境下各障碍物的运动畸变去除位置信息；
利用所述里程计与陀螺仪实时采集的机器人的位姿信息，获得机器人当前时刻的预测位姿；
将所述预测位姿作为当前时刻激光雷达扫描匹配的中心位姿且以设定的第一搜索范围搜索一或多个候选位姿，并计算在各候选位姿下激光雷达扫描的障碍物的位置信息与所述二维地图上各障碍物在所述地图坐标系下的坐标位置信息进行扫描匹配的匹配得分；
基于各候选位姿的匹配得分选择当前时刻的初始位姿，再将所述初始位姿进行非线性优化，获得当前时刻的最优位姿以及所述最优位姿的匹配得分：
将所述激光雷达所采集的各障碍物的运动畸变去除位置信息以所述最优位姿***所述地图坐标系，更新获得当前环境的二维地图。
9.一种多传感器融合的激光雷达定位终端，其特征在于，包括：
存储器，用于存储计算机程序；
处理器，用于执行如权利要求1至7中任一项所述的多传感器融合的激光雷达定位方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110777452.0A CN113503876B (zh) | 2021-07-09 | 2021-07-09 | 多传感器融合的激光雷达定位方法、***以及终端 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110777452.0A CN113503876B (zh) | 2021-07-09 | 2021-07-09 | 多传感器融合的激光雷达定位方法、***以及终端 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113503876A CN113503876A (zh) | 2021-10-15 |
| CN113503876B true CN113503876B (zh) | 2023-11-21 |
Family
ID=78012460
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110777452.0A Active CN113503876B (zh) | 2021-07-09 | 2021-07-09 | 多传感器融合的激光雷达定位方法、***以及终端 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113503876B (zh) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2024001339A1 (zh) * | 2022-07-01 | 2024-01-04 | 华为云计算技术有限公司 | 确定位姿的方法、装置以及计算设备 |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105241461A (zh) * | 2015-11-16 | 2016-01-13 | 曾彦平 | 机器人的地图创建及定位方法和机器人*** |
| CN108931245A (zh) * | 2018-08-02 | 2018-12-04 | 上海思岚科技有限公司 | 移动机器人的局部自定位方法及设备 |
| CN110285806A (zh) * | 2019-07-05 | 2019-09-27 | 电子科技大学 | 基于多次位姿校正的移动机器人快速精确定位算法 |
| CN111536964A (zh) * | 2020-07-09 | 2020-08-14 | 浙江大华技术股份有限公司 | 机器人定位方法及装置、存储介质 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108717710B (zh) * | 2018-05-18 | 2022-04-22 | 京东方科技集团股份有限公司 | 室内环境下的定位方法、装置及*** |
-
2021
- 2021-07-09 CN CN202110777452.0A patent/CN113503876B/zh active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105241461A (zh) * | 2015-11-16 | 2016-01-13 | 曾彦平 | 机器人的地图创建及定位方法和机器人*** |
| CN108931245A (zh) * | 2018-08-02 | 2018-12-04 | 上海思岚科技有限公司 | 移动机器人的局部自定位方法及设备 |
| CN110285806A (zh) * | 2019-07-05 | 2019-09-27 | 电子科技大学 | 基于多次位姿校正的移动机器人快速精确定位算法 |
| CN111536964A (zh) * | 2020-07-09 | 2020-08-14 | 浙江大华技术股份有限公司 | 机器人定位方法及装置、存储介质 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113503876A (zh) | 2021-10-15 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP6987797B2 (ja) | リアルタイムオンラインエゴモーション推定を有するレーザスキャナ | |
| WO2021135645A1 (zh) | 一种地图更新方法及装置 | |
| Holz et al. | Sancta simplicitas-on the efficiency and achievable results of SLAM using ICP-based incremental registration | |
| Saarinen et al. | Normal distributions transform occupancy maps: Application to large-scale online 3D mapping | |
| CN111895989A (zh) | 一种机器人的定位方法、装置和电子设备 | |
| US9651388B1 (en) | System and method for improved simultaneous localization and mapping | |
| CN113514843A (zh) | 多子图激光雷达定位方法、***以及终端 | |
| CN110986956B (zh) | 一种基于改进的蒙特卡洛算法的自主学习全局定位方法 | |
| CN113587933B (zh) | 一种基于分支定界算法的室内移动机器人定位方法 | |
| JP2019028988A (ja) | ロボットクラスターにおけるフォールト・トレラントな同時位置推定および地図構築を実行するためのシステムおよび方法 | |
| JP6649743B2 (ja) | 一致性評価装置および一致性評価方法 | |
| CN111288971B (zh) | 一种视觉定位方法及装置 | |
| CN111680673A (zh) | 一种栅格地图中动态物体的检测方法、装置及设备 | |
| LU500407B1 (en) | Real-time positioning method for inspection robot | |
| CN112904358B (zh) | 基于几何信息的激光定位方法 | |
| CN113503876B (zh) | 多传感器融合的激光雷达定位方法、***以及终端 | |
| CN112327329A (zh) | 避障方法、目标设备和存储介质 | |
| CN116449392B (zh) | 一种地图构建方法、装置、计算机设备及存储介质 | |
| CN111045433B (zh) | 一种机器人的避障方法、机器人及计算机可读存储介质 | |
| Garrote et al. | Mobile robot localization with reinforcement learning map update decision aided by an absolute indoor positioning system | |
| CN113759928B (zh) | 用于复杂大尺度室内场景的移动机器人高精度定位方法 | |
| CN112731337B (zh) | 地图构建方法、装置和设备 | |
| JPH11194822A (ja) | 移動ロボットのグローバル地図構築方法 | |
| KR102097722B1 (ko) | 빅셀그리드맵을 이용한 이동체의 자세 추정 방법, 이를 구현하기 위한 프로그램이 저장된 기록매체 및 이를 구현하기 위해 매체에 저장된 컴퓨터프로그램 | |
| CN116465393A (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 |