CN113311452B - 一种基于多传感器的定位方法及其*** - Google Patents

Abstract

本发明实施例提供一种基于多传感器的机器人定位方法及其***，该方法包括：在机器人使用SLAM构建环境地图后，给定机器人初始位姿；在机器人移动过程中，通过不同的传感器和/或传感器的组合获取机器人的定位信息和/或里程信息，并记录对应的时间戳；定位信息和/或里程信息通过多个独立线程输出；多个独立线程，根据自身输出的定位信息的时间戳，分别将对应的定位信息加入位姿图，基于各传感和/或传感器组合的不同权重，优化位姿图，输出机器人的位姿。本申请以位姿图的方式融合多种传感器定位结果，充分利用多种传感器的互补特性，有效解决了不同传感器数据不同频的问题，从而获得更稳定、更高精度的定位结果。

Description

一种基于多传感器的定位方法及其***

技术领域

本发明涉及机器人技术领域，尤其涉及一种基于多传感器的定位方法及其***。

背景技术

随着传感器、计算机信息、人工智能等技术的飞速发展，无人车技术的研究越来越受到国内外专家学者的重视。定位技术对于运动的机器人来说无疑是十分重要的基础，许多传感器都可以达到辅助定位的目的，例如，GPS, 惯性测量单元(Inertial MeasurementUnit，IMU)，编码器，激光雷达，超声波，摄像头，WIFI，蓝牙，外部追踪等等。无人车通过传感器获取外部环境信息和自身状态信息，依据这些信息实现自主运动并完成一定的作业任务。而自主定位是无人车智能导航和环境探索研究的基础，由于单一传感器难以获取***所需的全部信息，多传感器的信息融合就成为无人车实现自主定位的关键。

目前的技术来说，单独一种或者两种传感器的定位精度和稳定性很难满足要求，采用视觉或者激光里程计的方法较为成熟，但是在室外环境，高速运动和弱光照环境下其稳定性以及精确度有致命的缺点，并且二者在较大场景下都具有较高的累计误差；利用IMU来获取车辆的瞬时位移增量来推算出车辆的轨迹，然后来辅助GPS定位，具有较大的累积误差，且同一地点，不同的天气情况产生的误差相差很大，而且来自太空卫星的定位信号容易受到遮挡，比如隧道、高架桥、大树、高楼等等都会遮挡住卫星定位***的信号，一旦卫星定位***的信号被遮挡很容易产生很大的误差甚至无法定位。

为了提高稳定性，传感器的混合使用在机器人领域也是极为常见。由于不同传感器信息的频率不同，数据处理时间不同，导致融合信息的滞后不同。在移动机器人中，任何一个传感器都可能存在误差，单独的依赖某个传感器的读数都无法获得移动机器人精确的位姿信息，如何综合多个传感器的读数以获得更加贴近真实值的一个新的估计值，减小移动机器人的定位误差，提高移动机器人的定位精度与移动机器人导航的准确性，成为亟待解决的问题。

发明内容

本发明解决的技术问题是不同传感器信息的频率不同，数据处理时间不同，导致融合信息的滞后不同。在移动机器人中，任何一个传感器都可能存在误差，单独的依赖某个传感器的读数都无法获得移动机器人精确的位姿信息。

为解决上述技术问题，本发明第一方面公开了一种基于多传感器的机器人定位方法，包括以下步骤：

在机器人使用SLAM构建环境地图后，给定机器人初始位姿。

在机器人移动过程中，通过不同的传感器和/或传感器的组合获取所述机器人的定位信息和/或里程信息，并记录对应的时间戳。所述定位信息和/或里程通过多个独立线程输出。

所述多个独立线程，根据自身输出的所述定位信息的时间戳，分别将对应的定位信息加入所述机器人的位姿图，以增加所述机器人的绝对和/或相对约束。

基于各传感和/或传感器组合的不同权重，优化所述位姿图，输出所述机器人的位姿。

可选地，所述机器人上设置有激光雷达和IMU。

所述IMU采集高频运动信息，通过积分输出机器人第一定位信息、传播积分误差，并通过第一独立线程输出所述第一定位信息及对应的时间戳。

所述激光雷达接收IMU积分信息，解算帧间运动，去除点云运动畸变后构建最大后验概率问题，优化求解输出机器人第二定位信息，并通过第二独立线程输出所述第二定位信息及对应的时间戳。

可选地，所述多个独立线程，根据自身输出的所述定位信息的时间戳，分别将对应的定位信息加入所述机器人的位姿图，以增加所述机器人的位姿的绝对和/或相对约束具体包括：

所述第一独立线程根据输出的第一定位信息的时间戳，将所述第一定位信息加入所述机器人的位姿图，以便在所述机器人的位姿图中增加IMU约束；

所述第二独立线程根据输出的第二定位信息的时间戳，将所述第二定位信息加入所述机器人的位姿图，以便在所述机器人的位姿图中增加激光里程计约束。

可选地，所述机器人上还设置有GPS；其中：

所述GPS每隔预设的第三时间间隔或者每移动预设的第三距离，采集所述机器人的第三定位信息，并通过第三独立线程输出所述第三定位信息及对应的时间戳；

所述第三独立线程根据输出的第三定位信息的时间戳，在所述机器人的位姿图中查找候选姿态；

验证所述GPS采集的第三定位信息与所述候选姿态的一致性；

当所述第三定位信息通过所述一致性验证后，将所述第三定位信息加入所述机器人的位姿图，以便在所述机器人的位姿图中增加GPS约束。

可选地，还包括：通过第四独立线程，在所述机器人的位姿图中加入先验地图约束；具体包括：

当所述机器人每移动预设的第四距离，获取当前环境点云图；

采用点云注册方法对齐所述当前环境点云和先验地图，以获得约束位姿；所述先验地图为之前所述机器人通过SLAM构建的环境地图；

根据所述约束位姿对应的时间戳，在所述机器人的位姿图中查找候选姿态；

验证所述约束位姿与所述候选姿态的一致性；

当所述约束位姿通过所述一致性验证后，将所述约束位姿加入所述机器人的位姿图，以便在所述机器人的位姿图中增加先验地图约束。

可选地，各独立线程在将各自输出的定位信息加入所述机器人的位姿图之前还包括：

判断所述机器人的位姿图中的姿态数是否大于预设的姿态阈值；

当判定所述姿态数大于所述姿态阈值时，将所述位姿图中距离当前时刻最久的时间点所对应的位姿进行删除。

本发明第二方面公开了一种基于多传感器的机器人定位***，包括：

设定模块，用于在机器人通过SLAM构建环境地图后；给定机器人初始位姿；

定位模块，用于在机器人移动过程中，通过不同的传感器和/或传感器的组合获取所述机器人的多个定位信息，并记录对应的时间戳；所述多个定位信息通过多个独立线程输出；

融合模块，用于通过所述多个独立线程，根据自身输出的所述定位信息的时间戳，分别将对应的定位信息加入所述机器人的位姿图，以增加所述位姿的绝对和/或相对的约束；

优化模块，用于基于各传感和/或传感器组合的不同权重，优化所述位姿图，输出所述机器人的位姿。

可选地，所述定位模块包括：

IMU预积分子模块，用于采集高频运动信息，通过积分输出机器人第一定位信息、传播积分误差，并通过第一独立线程输出所述第一定位信息及对应的时间戳；

激光雷达里程计子模块，用于接收IMU积分信息，解算帧间运动，去除点云运动畸变后构建最大后验概率问题，优化求解输出机器人第二定位信息，并通过第二独立线程输出所述第二定位信息及对应的时间戳；

所述融合模块包括：

局部建图融合子模块，用于根据第二独立线程输出的第二定位信息构建位姿图，将所述第一定位信息、第三定位信息、第四定位信息以及第五定位信息，根据时间戳寻找位姿图中对应位姿，并增加相应约束，优化输出定位信息。

IMU预积分融合子模块，用于根据所述局部建图融合模块输出的定位信息解算机器人运动增量，校正IMU预积分误差。

可选地，所述定位模块还包括：GPS定位子模块和/或编码器积分子模块，

其中：

所述GPS定位子模块，用于每隔预设的第三时间间隔或者每移动预设的第三距离，采集所述机器人的第三定位信息，并通过第三独立线程输出所述第三定位信息及对应的时间戳；

所述编码器积分子模块，用于积分位姿图中相邻位姿间的相对位移，并采集所述机器人的第五定位信息，并通过第五独立线程输出所述第五定位信息及对应的时间戳；

所述激光雷达里程计子模块，还用于每隔预设的第四距离，采集当前的环境点云图；

所述融合模块还包括：GPS融合子模块、编码器子模块、先验地图融合子模块之中的至少一种，以及验证子模块；其中：

所述验证子模块，用于根据验证所述定位模块输出的定位信息对应的时间戳，在所述机器人的位姿图中查找候选姿态，并验证所述定位信息与所述候选姿态的一致性；

所述GPS融合子模块，用于通过所述第三独立线程，在所述GPS采集的第三定位信息通过了所述验证子模块的验证后，根据输出的第三定位信息的时间戳，将所述第三定位信息加入所述机器人的位姿图，以便在所述机器人的位姿图中增加GPS约束；

编码器融合子模块，用于通过所述第五独立线程，在所述编码器采集的第五定位信息通过了所述验证子模块的验证后，根据输出的第五定位信息的时间戳，将所述第五定位信息加入所述机器人的位姿图，以便在所述机器人的位姿图中增加编码器约束；

所述先验地图融合子模块，用于通过第四独立线程，在所述机器人的位姿图中加入先验地图约束；具体包括：

点云对齐单元，用于当所述机器人每移动预设的第四距离，便采用点云注册方法将当前获取的所述机器人的当前环境点云图与先验地图进行对齐，以获得约束位姿；所述先验地图为之前所述机器人通过SLAM构建的环境地图；

先验地图融合单元，用于在所述约束位姿通过了所述验证子模块的验证后，根据所述约束位姿的时间戳，将所述约束位姿加入所述机器人的位姿图，以便在所述机器人的位姿图中增加先验地图约束。

可选地，还包括：

滑窗模块，用于判断所述机器人的位姿图中的姿态数是否大于预设的姿态阈值；当判定所述姿态数大于所述姿态阈值时，将所述位姿图中距离当前时刻最久的时间点所对应的位姿进行删除。

与现有技术相比，本发明实施例的技术方案以位姿图的方式融合多种传感器定位结果，充分利用多种传感器的优势互补特性，有效解决了不同传感器定位数据不同频问题，在保证各传感器信息充分利用的同时，有效应对单一传感器短时失效的问题，减小机器人的定位误差，从而获得更稳定、更高精度的定位结果。同时使用本申请的多传感器融合定位方案，能够有效应对各种复杂环境和剧烈运动，能够在环境变化大的场景保证准确定位。

附图说明

图1是本发明实施例一种基于多传感器的机器人定位方法的示意图；

图2是本发明实施例另一种基于多传感器的机器人定位方法的示意图；

图3是本发明实施例另一种基于多传感器的机器人定位方法的示意图；

图4是本发明实施例另一种基于多传感器的机器人定位方法的示意图；

图5是本发明实施例一种基于多传感器的机器人定位***的结构示意图。

具体实施方式

下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。

需要说明的是，在本申请实施例中使用的术语是仅仅出于描述特定实施例的目的，而非旨在限制本申请。在本申请实施例和所附权利要求书中所使用的单数形式的“一种”、“所述”和“该”也旨在包括多数形式，除非上下文清楚地表示其他含义。还应当理解，本文中使用的术语“和/或”是指并包含一个或多个相关联的列出项目的任何或所有可能组合。

实施例一

图1是本发明实施例一种基于多传感器的定位方法的原理示意图。

S110：在机器人使用SLAM构建环境地图后，给定机器人初始位姿；

S120：在机器人移动过程中，通过不同的传感器和/或传感器的组合获取机器人的多个定位信息和/或里程信息，并记录对应的时间戳。多个定位信息和/或里程信息通过多个独立线程输出。

S130：多个独立线程，根据自身输出的定位信息的时间戳，分别将对应的定位信息加入机器人的位姿图，以增加机器人的位姿的绝对和/或相对约束。

S140：基于各传感和/或传感器组合的不同权重，优化位姿图，输出机器人的位姿。

在步骤S110的执行中，给定机器人初始位姿,通过给定初始位姿，来测量相对于机器人初始位姿的距离和方向来确定当前机器人的位姿。设置机器人初始位置，此过程可以手动或者通过其他自动初始化方法完成。

在步骤S120的执行中，在机器人移动过程中，通过不同的传感器和/或传感器的组合获取机器人的多个定位信息，多个定位信息通过多个独立线程输出。定位信息对应的时间戳，即采集的时间信息。例如，机器人上设置有多个用于定位的传感器，比如：激光雷达根据采集的环境点云信息，通过数据处理后，会输出得到机器人的移动距离信息；IMU根据采集的三轴加速度信息和角度信息，获得机器人移动后当前的位姿；GPS根据移动前后采集的经纬度信息，也可以获得机器人移动的位置或距离。这些移动距离和角度方面的信息都属于机器人的定位信息。且不同的传感器采用不同的线程来进行处理，从而使得，即使传感器的采集频率不一样，处理时间不一样，也不会影响到整***姿图的优化进度，每个独立线程一旦获得对应传感器输出的定位信息后，便可及时将该定位信息融入到机器人的位姿图中，有效解决了不同传感器定位数据不同频问题，大大提高了最终的定位效率和精度。

比如，机器人通过IMU和激光雷达来进行定位信息和/或里程记信息的采集；IMU采集高频运动信息，通过积分输出机器人第一定位信息、传播积分误差，并通过第一独立线程输出所述第一定位信息及对应的时间戳。激光雷达接收IMU积分信息，解算帧间运动，去除点云运动畸变后构建最大后验概率问题，优化求解输出机器人第二定位信息，并通过第二独立线程输出所述第二定位信息及对应的时间戳。

在步骤S130的执行中，多个独立线程，根据自身输出的定位信息的时间戳，分别将对应的定位信息加入机器人的位姿图，以增加机器人的位姿的绝对和/或相对约束。例如通过第一独立线程根据输出的第一定位信息的时间戳，将所述第一定位信息加入所述位姿图，以便在所述位姿图中增加IMU约束。通过第二独立线程根据输出的第二定位信息的时间戳，将所述第二定位信息加入所述位姿图，以便在所述位姿图中增加激光里程计约束。

在步骤S140的执行中，基于各传感和/或传感器组合的不同权重，优化位姿图，输出机器人的位姿。首先根据不同传感器的采集精度设定不同的融合权重，通过采用图优化方法，根据权重融合多传感器的定位信息以得到更加精确平滑的机器人位姿。具体的，比如IMU采集的定位信息中，姿态角度信息一般比较准确，位置/距离信息精度略低；GPS采集的定位信息中位置/距离信息更为精准。那么便可在融合计算时，位置计算时，对GPS采集的位置数据设置较高权重；姿态计算时，对IMU传感器采集的角度数据设置较高权重，从而充分利用多传感器的互补特性，提高定位精度。

本发明实施例，在位姿图中融合多种传感器定位结果，充分利用多种传感器的互补特性，有效解决了不同传感器定位数据不同频问题，从而获得更稳定、更高精度的定位结果。

实施例二

较佳的，如图2所示，在上述实施例一的基础上，机器人上还设置有GPS。其中：GPS每隔预设的时间间隔或者每移动预设的距离，采集机器人的GPS定位信息，并通过线程3输出GPS定位信息及对应的时间戳。线程3根据输出的GPS定位信息的时间戳，在机器人的位姿图中查找候选姿态。验证GPS采集的定位信息与候选姿态的一致性。当定位信息通过一致性验证后，将定位信息加入机器人的位姿图，以便在机器人的位姿图中增加GPS约束，待下一个激光里程计定位加入位姿图后进行优化。

较佳的，线程1、线程2、线程3在将各自输出的定位信息加入机器人的位姿图之前还包括：判断机器人的位姿图中的姿态数是否大于预设的姿态阈值。当判定姿态数大于姿态阈值时，将位姿图中距离当前时刻最久的时间点所对应的位姿进行删除。如预设的姿态阈值为10个姿态，当判断第11个位姿要加入姿态图时，将位姿图中距离当前时刻最久的时间点所对应的位姿进行删除，以便可以将该第11个位姿到位姿图。

实施例三

较佳的，如图3所示，在上述实施例二的基础上，机器人定位方法还包括：通过线程4，在机器人的位姿图中加入先验地图约束。当机器人每移动预设的距离，获取当前环境点云图。

采用点云注册方法对齐当前环境点云和先验地图，以获得约束位姿。先验地图为之前机器人通过SLAM构建的环境地图。根据约束位姿对应的时间戳，在机器人的位姿图中查找候选姿态。验证约束位姿与候选姿态的一致性。当约束位姿通过一致性验证后，将约束位姿加入机器人的位姿图，以便在机器人的位姿图中增加先验地图约束。并更新位姿图，待下一个激光里程计定位加入位姿图后进行优化。

如图4所示，该机器人定位方法包括：

在机器人使用SLAM构建环境地图后，给定机器人初始位姿。在机器人移动过程中，通过激光雷达、IMU、GPS获取所述机器人的多个定位信息，并记录对应的时间戳。

其中，激光雷达的定位信息通过线程2输出，激光雷达每隔预设的时间间隔采集机器人当前的激光雷达定位信息，并通过线程2输出定位信息及对应的时间戳，线程2根据输出的定位信息的时间戳，将该定位信息加入机器人的位姿图，以便在机器人的位姿图中增加激光雷达约束。根据激光雷达采集的历史数据和当前数据以及机器人上一数据采集时刻的位置推算机器人当前位置，若位姿图中的姿态数小于设置阈值则直接将该位姿加入位姿图并优化，将位姿图中距离当前时刻最久的时间点所对应的位姿进行删除后，将当前位姿加入位姿图并优化，优化后进行位姿一致性检查，若检查失败则删除错误约束并更新、优化，整个过程记录每个机器人位姿的时间戳。

IMU的定位信息通过线程1独立输出，IMU每隔预设的时间间隔采集机器人当前的定位信息，并通过线程1输出定位信息及对应的时间戳，线程1根据输出的定位信息的时间戳，将定位信息加入机器人的位姿图，以便在机器人的位姿图中增加IMU约束。

GPS的定位信息通过线程3独立输出，GPS每隔预设的时间间隔或者每移动预设的距离，采集机器人的GPS定位信息，并通过线程3输出GPS定位信息及对应的时间戳，线程3根据输出的GPS定位信息的时间戳，在机器人的位姿图中查找候选姿态。且验证GPS采集的GPS定位信息与候选姿态的一致性。当GPS定位信息通过一致性验证后，将GPS定位信息加入机器人的位姿图，以便在机器人的位姿图中增加GPS约束。

本实施例中的定位方法，还包括：通过线程4，在机器人的位姿图中加入先验地图约束。即当机器人每移动预设的距离时，获取当前环境点云图，采用点云注册方法对齐当前环境点云和先验地图，以获得约束位姿。先验地图为之前机器人通过SLAM构建的环境地图，根据约束位姿对应的时间戳，在机器人的位姿图中查找候选姿态，并验证约束位姿与候选姿态的一致性，当约束位姿通过一致性验证后，将该约束位姿加入机器人的位姿图，以便在机器人的位姿图中增加先验地图约束。

4个独立线程根据自身输出的定位信息的时间戳，分别将对应的定位信息加入机器人的位姿图，以增加机器人的位姿图的约束。之后基于各传感和/或传感器组合的不同权重，优化位姿图，输出机器人的位姿。

实施例四

如图5所示，本发明实施例提供的一种基于多传感器的机器人定位***200，该定位***应用上述任一实施例的定位方法，包括:

设定模块210，用于在机器人通过SLAM构建环境地图后。给定机器人初始位姿。

定位模块220，用于在机器人移动过程中，通过不同的传感器和/或传感器的组合获取机器人的多个定位信息，并记录对应的时间戳。多个定位信息通过多个独立线程输出。

融合模块230，用于通过多个独立线程，根据自身输出的定位信息的时间戳，分别将对应的定位信息加入机器人的位姿图，以增加机器人的位姿绝对和/或相对约束。

优化模块240，用于基于各传感和/或传感器组合的不同权重，优化位姿图，输出机器人的位姿。

较佳的，定位模块220包括：

IMU预积分子模块，用于采集高频运动信息，通过积分输出机器人的高频运动信息、传播积分误差，并通过独立线程输出IMU定位信息及对应的时间戳，用于获取机器人瞬时的位移。

激光雷达里程计子模块，用于接收IMU积分信息，解算帧间运动，去除点云运动畸变后构建最大后验概率问题，优化求解输出机器人激光雷达定位信息，并通过独立线程输出激光雷达定位信息及对应的时间戳。

融合模块230包括：

局部建图融合子模块，根据线程2输出的定位信息构建位姿图，将高频运动信息、GPS信息、先验地图信息以及编码器信息，根据时间戳寻找位姿图中对应位姿，并增加相应约束，优化输出定位信息。

IMU预积分融合子模块，根据所述局部建图融合模块输出的定位信息解算机器人运动增量，校正IMU预积分误差。

较佳的，定位模块220还包括：GPS定位子模块和/或编码器积分子模块，其中：GPS定位子模块，用于每隔预设的时间间隔或者每移动预设的距离，采集机器人的GPS定位信息，并通过线程3输出GPS定位信息及对应的时间戳。

编码器积分子模块，用于每隔预设的时间间隔或者每移动预设的距离，采集机器人的编码器定位信息，并通过线程5独立输出编码器定位信息及对应的时间戳，用于获取机器人的速度和行走方向。

激光雷达里程计子模块，还用于每隔预设的距离，采集当前的环境点云图。

融合模块230还包括：GPS融合子模块、编码器子模块、先验地图融合子模块之中的至少一种，以及验证子模块。其中：

验证子模块，用于根据验证定位模块220输出的定位信息对应的时间戳，在机器人的位姿图中查找候选姿态，并验证定位信息与候选姿态的一致性。

GPS融合子模块，用于通过线程3，在GPS采集的定位信息通过了验证子模块的验证后，根据输出的定位信息的时间戳，将定位信息加入机器人的位姿图，以便在机器人的位姿图中增加GPS约束。

编码器融合子模块，用于通过线程5，在编码器采集的定位信息通过了验证子模块的验证后，根据输出的定位信息的时间戳，将定位信息加入机器人的位姿图，以便在机器人的位姿图中增加编码器约束。

先验地图融合子模块，用于通过线程4，在机器人的位姿图中加入先验地图约束，具体包括：

点云对齐单元，用于当机器人每移动预设的距离，便采用点云注册方法将当前获取的机器人的当前环境点云图与先验地图进行对齐，以获得约束位姿。先验地图为之前机器人通过SLAM构建的环境地图。

先验地图融合单元，用于在约束位姿通过了验证子模块的验证后，根据约束位姿的时间戳，将约束位姿加入机器人的位姿图，以便在机器人的位姿图中增加先验地图约束。

较佳的，一种基于多传感器的机器人定位***，还包括：

滑窗模块，用于判断机器人的位姿图中的姿态数是否大于预设的姿态阈值。当判定姿态数大于姿态阈值时，将位姿图中距离当前时刻最久的时间点所对应的位姿进行删除。

本发明实施例，本采取单一传感器很难获取线路全面、准确的环境信息，多传感器融合技术对于提高巡检机器人障碍物定位与识别能力有着十分重要的作用。

本发明实施例，提供的多传感器融合定位方法不受传感器数量限制，能够有效应对单一传感器失效问题，提高了定位鲁棒性。能够有效应对环境变化，即使在环境变化巨大的场景也可准确定位。结构清晰，简单，扩展性强，可以附加各种传感器而不改变整体架构。以位姿图的方式，融合多种传感器定位结果，有效解决了不同传感器定位数据不同频问题，充分利用多种传感器的互补特性，获得更稳定、更高精度的高频定位结果。同时使用多传感器构建激光里程计，能够有效应对各种复杂环境和剧烈运动，能够在环境变化大的场景保证准确定位。

虽然本发明披露如上，但本发明并非限定于此。任何本领域技术人员，在不脱离本发明的精神和范围内，均可作各种更动与修改，因此本发明的保护范围应当以权利要求所限定的范围为准。

Claims (5)

1.一种基于多传感器的机器人定位方法，其特征在于，包括以下步骤：

在机器人使用SLAM构建环境地图后，给定机器人初始位姿；

所述机器人上设置有激光雷达和IMU；所述IMU采集高频运动信息，通过积分输出机器人第一定位信息、传播积分误差，并通过第一独立线程输出所述第一定位信息及对应的时间戳；所述激光雷达接收IMU积分信息，解算帧间运动，去除点云运动畸变后构建最大后验概率问题，优化求解输出机器人第二定位信息，并通过第二独立线程输出所述第二定位信息及对应的时间戳;

通过第四独立线程，在所述位姿图中加入先验地图约束；具体包括：当所述机器人每移动预设的第四距离，获取当前环境点云图；采用点云注册方法对齐所述当前环境点云和先验地图，以获得约束位姿；所述先验地图为之前所述机器人通过SLAM构建的环境地图；根据所述约束位姿对应的时间戳，在所述机器人的位姿图中查找候选姿态；验证所述约束位姿与所述候选姿态的一致性；当所述约束位姿通过所述一致性验证后，将所述约束位姿加入所述位姿图，以便在所述位姿图中增加先验地图约束;

在机器人移动过程中，通过不同的传感器和/或传感器的组合获取所述机器人的定位信息和/或里程信息，并记录对应的时间戳；所述定位信息和/或里程信息通过多个独立线程输出；

所述多个独立线程，根据自身输出的所述信息的时间戳，判断所述机器人的位姿图中的姿态数是否大于预设的姿态阈值；当判定所述姿态数大于所述姿态阈值时，将所述位姿图中距离当前时刻最久的时间点所对应的位姿进行删除;分别将对应的信息加入位姿图，以增加所述机器人的位姿的绝对和/或相对约束；

基于各传感和/或传感器组合的不同权重，优化所述位姿图，输出所述机器人的位姿。

2.根据权利要求1所述的一种基于多传感器的机器人定位方法，其特征在于，所述多个独立线程，根据自身输出的所述定位信息的时间戳，分别将对应的定位信息加入所述位姿图，以增加所述机器人的位姿的绝对和/或相对约束具体包括：

所述第一独立线程根据输出的第一定位信息的时间戳，将所述第一定位信息加入所述位姿图，以便在所述位姿图中增加IMU约束；

所述第二独立线程根据输出的第二定位信息的时间戳，将所述第二定位信息加入所述位姿图，以便在所述位姿图中增加激光里程计约束。

3.根据权利要求1所述的一种基于多传感器的机器人定位方法，其特征在于，所述机器人上还设置有GPS；其中：

所述GPS每隔预设的第三时间间隔或者每移动预设的第三距离，采集所述机器人的第三定位信息，并通过第三独立线程输出所述第三定位信息及对应的时间戳；

所述第三独立线程根据输出的第三定位信息的时间戳，在所述机器人的位姿图中查找候选姿态；

验证所述GPS采集的第三定位信息与所述候选姿态的一致性；

当所述第三定位信息通过所述一致性验证后，将所述第三定位信息加入所述位姿图，以便在位姿图中增加GPS约束。

4.一种基于多传感器的机器人定位***，其特征在于，包括：

设定模块，用于在机器人通过SLAM构建环境地图后；给定机器人初始位姿；

定位模块，用于在机器人移动过程中，通过不同的传感器和/或传感器的组合获取所述机器人的多个定位信息，并记录对应的时间戳；所述多个定位信息通过多个独立线程输出；所述定位模块包括：

GPS定位子模块，用于每隔预设的第三时间间隔或者每移动预设的第三距离，采集所述机器人的第三定位信息，并通过第三独立线程输出所述第三定位信息及对应的时间戳；

编码器积分子模块，用于积分位姿图中相邻位姿间的相对位移，并通过第五独立线程输出第五定位信息及对应的时间戳；

IMU预积分子模块，用于采集高频运动信息，通过积分输出机器人第一定位信息、传播积分误差，并通过第一独立线程输出所述第一定位信息和对应的时间戳；

激光雷达里程计子模块，用于接收IMU积分信息，解算帧间运动，去除点云运动畸变后构建最大后验概率问题，优化求解输出机器人第二定位信息，并通过第二独立线程输出所述第二定位信息及对应的时间戳；

融合模块，用于通过所述多个独立线程，根据自身输出的所述定位信息的时间戳，分别将对应的定位信息加入位姿图，以增加位姿的绝对和/或相对约束；所述融合模块包括：

局部建图融合子模块，根据第二独立线程输出的第二定位信息构建位姿图，将第一定位信息、第三定位信息以及第四定位信息、第五定位信息，根据时间戳寻找位姿图中对应位姿，并增加相应约束，优化输出定位信息;

IMU预积分融合子模块，根据所述局部建图融合模块输出的定位信息解算机器人运动增量，校正IMU预积分误差;

验证子模块，用于根据验证所述定位模块输出的定位信息对应的时间戳，在所述机器人的位姿图中查找候选姿态，并验证所述定位信息与所述候选姿态的一致性；

编码器融合子模块，用于通过第五独立线程，在所述编码器采集的第五定位信息通过了所述验证子模块的验证后，根据输出的第的时间戳，将所述第五定位信息加入所述机器人的位姿图，以便在所述机器人的位姿图中增加编码器约束；

滑窗模块，用于判断所述机器人的位姿图中的姿态数是否大于预设的姿态阈值；当判定所述姿态数大于所述姿态阈值时，将所述位姿图中距离当前时刻最久的时间点所对应的位姿进行删除;

优化模块，用于基于各传感和/或传感器组合的不同权重，优化所述位姿图，输出所述机器人的位姿。

5.根据权利要求4所述的一种基于多传感器的机器人定位***，其特征在于，包括：

所述激光雷达里程计子模块，还用于每隔预设的第四距离，采集当前的环境点云图；

所述融合模块还包括：GPS融合子模块、编码器子模块、先验地图融合子模块之中的至少一种，其中：

所述GPS融合子模块，用于通过所述第三独立线程，在所述GPS采集的第三定位信息通过了所述验证子模块的验证后，根据输出的第三定位信息的时间戳，将所述第三定位信息加入所述机器人的位姿图，以便在所述机器人的位姿图中增加GPS约束；

所述先验地图融合子模块，用于通过第四独立线程，在所述机器人的位姿图中加入先验地图约束；具体包括：

点云对齐单元，用于当所述机器人每移动预设的第四距离，便采用点云注册方法将当前获取的所述机器人的当前环境点云图与先验地图进行对齐，以获得约束位姿；所述先验地图为之前所述机器人通过SLAM构建的环境地图；

先验地图融合单元，用于在所述约束位姿通过了所述验证子模块的验证后，根据所述约束位姿的时间戳，将所述约束位姿加入所述机器人的位姿图，以便在所述机器人的位姿图中增加先验地图约束。

Images