CN109933056B - 一种基于slam的机器人导航方法以及机器人 - Google Patents

Abstract

本发明揭示了一种基于SLAM的机器人导航方法，包括：位姿确定步骤、子地图构建步骤、子地图拼接步骤以及全局地图优化步骤。本发明所揭示基于SLAM的机器人导航方法，通过构建子地图并进行拼接的方式帮助机器人获取周围环境的完整地图信息，使用该方法对机器人进行导航，不仅能够保证传感器数据本身的准确性，而且还设置有多重的优化策略，从而能够最大限度地发挥激光SLAM导航方法的优势。本发明还揭示了一种机器人，该机器人采用本发明所述的方法进行导航，能够准确地获知自身位姿信息以及周围的环境特征信息，从而能够确保机器人行进过程中的稳定性。

Description

一种基于SLAM的机器人导航方法以及机器人

技术领域

本发明涉及激光导航领域，更具体地说，涉及一种基于SLAM的机器人导航方法以及一种机器人。

背景技术

SLAM(Simultaneous Localization and Mapping，即时定位与地图构建)导航方法是机器人领域一种广泛采用的即时定位导航方法，该方法通过采集机器人的传感器数据，智能感知机器人周围的环境，并根据采集到的传感器数据实时调整机器人的行进路线。目前的SLAM导航方法主要有视觉和激光两种实现方式，其中采用激光技术实现的SLAM导航方法具有成本和可靠性的优势，在多数机器人产品上已有较为成熟的应用。

然而，现有的激光SLAM导航方法也存在缺陷，一方面，机器人在运行过程中如果被搬离原来位置，需要重新扫描周围的环境特征信息，并将扫描到的环境特征信息与原来已经构建的完整地图中的环境特征信息进行匹配，才能重新确定自身位置，由于该环境特征信息的匹配过程是在完整地图中进行的，因而需要搜索和匹配的数据量大，完成重新定位的过程长达数分钟，从而影响机器人的工作效率以及用户体验。另一方面，现有的激光SLAM导航方法需要获取里程计数据、惯导数据以及激光数据以获知机器人的位姿状态，其中激光数据相对准确，在导航过程中通常采用激光数据对里程计数据和惯导数据进行优化校准，并持续将扫描到的环境特征信息归入已构建的地图之中直接建立完整地图，由于该优化校准的过程是持续进行的，而里程计和惯性导航***所获得的数据会随着使用时间延长而发生数据漂移，导致用于进行优化校准的里程计数据和惯导数据会越来越不准确，从而直接影响SLAM导航方法的准确性。

基于上述原因，现有的激光SLAM导航方法需要改进，以更好的指引机器人行进和工作。

发明内容

本发明为解决上述现有技术中存在的技术问题，提供了一种基于SLAM的机器人导航方法，该方法通过建立子地图并对子地图进行拼接的方式完成了全局地图的构建，不仅方便机器人快速确定自己的位置，而且将传感器漂移造成的误差通过求解子地图之间的旋转平移参数加以优化和消除，从而为机器人导航提供了准确的地图信息。本发明同时还揭示了一种机器人，该机器人采用本发明所述的激光SLAM方法进行导航，能够快速、准确地确定自己的位置。

为达到上述目的，本发明采用的技术方案如下：

一种基于SLAM的机器人导航方法，包括：位姿确定步骤，根据第一数据获取机器人的初始位姿和运动轨迹，第一数据包括里程计数据和惯导数据；子地图构建步骤，以第一数据为初始值通过第二数据对第一数据进行优化校准，并根据第二数据确定相应子地图中的环境特征信息，在达到预设条件时判断已完成相应子地图构建过程，并按照位姿确定步骤和子地图构建步骤开启一个新的子地图构建过程，第二数据为激光数据；子地图拼接步骤，提取子地图中的环境特征信息并根据环境特征信息确定机器人在沿着运动轨迹依次相邻的子地图中的位姿差异信息，根据位姿差异信息进行旋转平移变换对子地图进行拼接以获得相应的全局地图；全局地图优化步骤，根据环境特征信息补齐具有邻接关系的子地图并确定具有邻接关系的子地图之间的耦合约束条件，根据耦合约束条件对全局地图进行优化处理。

进一步地，预设条件为：在未超过预设时间的情况下机器人在相应子地图行进的过程中所确定的环境特征信息超过阈值；或者，在已超过预设时间的情况下机器人在相应子地图行进的过程中所确定的环境特征信息仍未超过阈值。

进一步地，子地图构建步骤包括：对子地图划分栅格，并根据第一数据在运动轨迹上进行采样，在采样点处扫描获得各个栅格中的第二数据并建立第一数据与第二数据的映射关系，其中，将包含有环境特征信息的栅格确定为特征栅格。

进一步地，子地图构建步骤包括：以一个完整子地图构建过程为周期确定相应特征栅格的出现概率以判断环境特征信息的真实性，其中出现概率＝(该周期内扫描到相应特征栅格的采样点的个数/该周期内采样点的总个数)*100％。

进一步地，子地图拼接步骤包括：对机器人在依次相邻的子地图中所共同扫描到的特征栅格进行匹配并确定其中用以表征相同环境特征信息的第一特征点对；根据第一特征点对的坐标信息求解旋转分量和平移分量以获得位姿差异信息；根据位姿差异信息将依次相邻的子地图拼接在一起。

进一步地，全局地图优化步骤包括：在全局地图中逐一选取目标子地图并确定与目标子地图具有邻接关系的边缘子地图；根据相应的目标子地图和边缘子地图确定目标子地图的边界区域，边界区域为在预设范围内横跨目标子地图和边缘子地图的区域；对机器人在相应边界区域所扫描到的特征栅格进行匹配并确定其中用以表征相同环境特征信息的第二特征点对；根据第二特征点对的坐标信息求解耦合约束条件以对全局地图进行优化处理。

进一步地，子地图构建步骤包括：获取惯导数据中机器人在竖直方向的加速度值；在加速度值超过阈值时，对第二数据进行修正，或者舍弃相应的第一数据和第二数据。

进一步地，全局地图优化步骤还包括：对全局地图进行闭环检测以获取机器人的最优轨迹；根据最优轨迹重新拟合第二数据以消除子地图之间的累积误差。

进一步地，位姿确定步骤包括：采用卡尔曼滤波算法对里程计数据和惯导数据进行融合获得第一数据，并以四元数法表征机器人的姿态信息。

本发明同时还揭示了一种机器人：

一种采用SLAM方法进行导航的机器人，包括MCU模块、建图模块，以及用于获取第一数据的里程计模块和惯导模块、用于获取第二数据的激光模块，其中：MCU模块用于获取第一数据和第二数据，并将第一数据和第二数据发送至建图模块；建图模块用于根据第一数据和第二数据执行位姿确定步骤、子地图构建步骤、子地图拼接步骤以及全局地图优化步骤以获取周围环境的地图信息；MCU模块还用于根据地图信息控制机器人行进。

本发明技术方案的有益效果如下：

本发明所揭示基于SLAM的机器人导航方法，通过构建子地图并进行拼接的方式帮助机器人获取周围环境的完整地图信息，使用该方法对机器人进行导航，不仅能够保证传感器数据本身的准确性，而且还设置有多重的优化策略，从而能够最大限度地发挥激光SLAM导航方法的优势。本发明还揭示了一种机器人，该机器人采用本发明所述的方法进行导航，能够准确地获知自身位姿信息以及周围的环境特征信息，从而能够确保机器人行进过程中的稳定性。

本发明揭示了一种基于SLAM的机器人导航方法，包括：位姿确定步骤，根据第一数据获取机器人的初始位姿和运动轨迹，所述第一数据包括里程计数据和惯导数据；子地图构建步骤，以第一数据为初始值通过第二数据对所述第一数据进行优化校准，并根据第二数据确定相应子地图中的环境特征信息，在达到预设条件时判断已完成相应子地图构建过程，并按照所述位姿确定步骤和子地图构建步骤开启一个新的子地图构建过程，所述第二数据为激光数据；子地图拼接步骤，提取子地图中的环境特征信息并根据所述环境特征信息确定机器人在沿着所述运动轨迹依次相邻的子地图中的位姿差异信息，根据所述位姿差异信息进行旋转平移变换对所述子地图进行拼接以获得相应的全局地图；全局地图优化步骤，根据所述环境特征信息补齐具有邻接关系的子地图并确定所述具有邻接关系的子地图之间的耦合约束条件，根据所述耦合约束条件对所述全局地图进行优化处理。该实施例中，机器人沿着运动轨迹依次构建子地图，在达到预设条件后对相应子地图中的第一数据和第二数据进行冻结并开启一个新的子地图构建过程，相较于传统的采用EKF(扩展卡尔曼滤波)算法基于完整地图对里程计数据、惯导数据和激光数据进行优化的策略，本发明所述的方法能够大大减少运算量，并避免前、后数据之间的相互影响，从而在源头上保证了数据的质量。该实施例中，通过确定机器人在沿着所述运动轨迹依次相邻的子地图中的位姿差异信息，能够将传感器漂移所导致的误差通过计算旋转平移参数在子地图拼接的过程中加以消除，从而避免了里程计数据和惯导数据的误差累积，使最终获得的全局地图能够更加准确地反映周围环境信息。同时，在该实施例中，全局地图构建完成之后，由于相邻接的子地图之间具备一定的耦合约束条件，利用该耦合约束条件能够在全局范围内对地图信息再次进行优化，从而进一步地提高地图信息的质量。

在本发明其中一实施例中，所述预设条件为：在未超过预设时间的情况下机器人在相应子地图行进的过程中所确定的环境特征信息超过阈值；或者，在已超过预设时间的情况下机器人在相应子地图行进的过程中所确定的环境特征信息仍未超过阈值。该实施例中，在短时间内可以认为传感器漂移的影响足够小，因而需要在合适的时机完结相应的子地图构建过程，并开启一个新的子地图构建过程，从而避免传感器数据的误差累积。对环境特征信息的数据量和时间设置阈值作为子地图构建过程是否完成的标志，一方面能够确保所获得的激光数据足以逆推出机器人的位姿对第一数据进行校准优化，另一方面也能够避免子地图构建过程持续时间过长而导致传感器数据漂移。在本发明其中一实施例中，所述环境特征信息包括由激光雷达所扫描到的障碍物以及墙壁等。

在本发明其中一实施例中，所述子地图构建步骤包括：对子地图划分栅格，并根据第一数据在运动轨迹上进行采样，在采样点处扫描获得各个栅格中的第二数据并建立第一数据与第二数据的映射关系，其中，将包含有环境特征信息的栅格确定为特征栅格。该实施例中，对子地图划分栅格，并通过采样点扫描的方式建立第一数据与第二数据的映射关系，从而实现相应子地图中第一数据和第二数据的冻结。同时，当机器人在运动过程中被搬离至新的位置时，能够根据环境特征信息快速筛选确定特征栅格，判断自身是否位于已经构建的其中一个子地图，如果确定自己在一个已经构建的子地图中，则可根据相应子地图指引自身行进，从而避免重复建图；如果发现没有与之相对应的子地图，则确定自己位于一个新的区域，并按照相应的步骤重新构建地图。

在本发明其中一实施例中，所述子地图构建步骤包括：以一个完整子地图构建过程为周期确定相应特征栅格的出现概率以判断所述环境特征信息的真实性，其中所述出现概率＝(该周期内扫描到相应特征栅格的采样点的个数/该周期内采样点的总个数)*100％。将全部子地图栅格化后，机器人沿着运动轨迹在一个子地图中行进时，能够扫描到一些环境特征信息，但其中有一些环境特征信息有可能并不是真实存在的障碍物、墙壁等的信息，而是由光影变换等因素引起的激光数据的噪点，为此需要判断环境特征信息的真实性，以确保激光数据的准确度。如果环境特征信息是由真实物体所产生的，则机器人在该子地图中行进时在大多数采样点处通常均能观测到相应的环境特征信息；但若所述环境特征信息属于激光数据的噪点，则在不同采样点处获得的结果会有所不同，即在某些采样点处能够观测到相应的环境特征信息，而某一些则不能。因此，在该实施例中，给出了特征栅格的出现概率的公式，以确定相应环境特征信息的真实性，从而保证激光数据的准确。

在本发明其中一实施例中，所述子地图拼接步骤包括：对机器人在依次相邻的子地图中所共同扫描到的特征栅格进行匹配并确定其中用以表征相同环境特征信息的第一特征点对；根据所述第一特征点对的坐标信息求解旋转分量和平移分量以获得所述位姿差异信息；根据所述位姿差异信息将所述依次相邻的子地图拼接在一起。该实施例中，由于所选取用于拼接的子地图是沿着运动轨迹依次相邻的，机器人在两张子地图中运动时会观测到大量表征相同环境特征信息的特征点，即机器人在其中一张子地图中观测到的障碍物，在运行至紧邻的下一张子地图中时也能够观测到该障碍物，但两张子图中所获得的障碍物的坐标可能会存在微小差异。将机器人在不同子地图中所观察到的表征相同环境特征信息的特征点组成第一特征点对，根据第一特征点对的坐标信息可以求解出引起误差的旋转平移矩阵。由于特征点之间的误差和子地图之间的误差所产生的原因相同，二者之间存在对应关系，因此根据旋转平移矩阵分离出旋转向量和平移向量即可获得机器人相应的位姿差异信息，根据所述位姿差异信息调整依次相邻的子地图之间距离和角度，就能够消除传感器的累积误差并实现子地图之间的平滑过渡，从而将各个子地图完整拼接为全局地图。

在本发明所述方法的其中一实施例中，所述全局地图优化步骤包括：在全局子地图中逐一选取目标子地图并确定与所述目标子地图具有邻接关系的边缘子地图；根据相应的目标子地图和边缘子地图确定所述目标子地图的边界区域，所述边界区域为在预设范围内横跨目标子地图和边缘子地图的区域；对机器人在相应边界区域所扫描到的特征栅格进行匹配并确定其中用以表征相同环境特征信息的第二特征点对；根据所述第二特征点对的坐标信息求解所述耦合约束条件以对所述全局地图进行优化处理。该实施例中，从全局地图出发，又对地图信息进行了优化，以确保地图信息的准确性。全局地图拼接完成后，存在邻接关系的子地图之间存在约束条件，即在子地图之间的交界区域由机器人所观察到的环境特征信息存在很高的一致性，即使两张子地图在时序上不相邻而仅在位置上相邻，机器人在二者的边界区域也能够观测到大量表征相同环境特征信息的特征点，将这些特征点组成第二特征点对，并根据第二特征点对的坐标信息求解相应的耦合约束条件，能够获得目标子地图与边缘子地图之间位置偏差，将具有邻接关系的子地图之间的距离和角度再作调整，从而能够进一步提升地图信息的质量。

在本发明所述方法的其中一实施例中，所述子地图构建步骤包括：获取惯导数据中机器人在竖直方向的加速度值；在所述加速度值超过阈值时，对所述第二数据进行修正，或者舍弃相应的第一数据和第二数据。该实施例中，惯导数据通过加速度计和陀螺仪获知。其中借助加速度计能够获知机器人在竖直方向的加速度值，当机器人在竖直方向的加速度值超过阈值时，说明路况比较颠簸，机器人由于爬坡、越障等原因在竖直方向的抖动比较严重，这将导致此时产生的激光数据存在很大的偏差，因而在这种情况下不适合通过第二数据对第一数据进行优化校准。为了避免这种情况的发生，在所述加速度值超过阈值时，可以选择丢弃此时获得的激光数据，因为机器人在短时间内能够获得大量激光数据，丢弃其中一帧或几帧的数据不会对计算机器人位姿的准确性造成影响；也可以选择根据惯导数据逆推机器人在该时刻下由于颠簸而引起的倾角，根据相应的角度信息对激光数据进行矫正后再用于构建子地图。

在本发明所述方法的其中一实施例中，所述全局地图优化步骤还包括：对所述全局地图进行闭环检测以获取机器人的最优轨迹；根据所述最优轨迹重新拟合所述第二数据以消除子地图之间的累积误差。该实施例中，通过闭环检测的方法进一步消除子地图之间的累积误差，使地图信息更加真实、可靠。

在本发明所述方法的其中一实施例中，所述位姿确定步骤包括：采用卡尔曼滤波算法对里程计数据和惯导数据进行融合获得所述第一数据，并以四元数法表征机器人的姿态信息。通过卡尔曼滤波算法对里程计数据和惯导数据进行融合能够消除大量传感器导致的噪声影响，采用四元数法表征机器人的位姿信息，能够避免在运算过程中“万向锁”问题的产生。

本发明还揭示了一种基于SLAM方法进行导航的机器人，包括MCU模块、建图模块，以及用于获取第一数据的里程计模块和惯导模块、用于获取第二数据的激光模块，其中：所述MCU模块用于获取所述第一数据和第二数据，并将所述第一数据和第二数据发送至所述建图模块；所述建图模块用于根据所述第一数据和第二数据执行所述位姿确定步骤、子地图构建步骤、子地图拼接步骤以及全局地图优化步骤以获取周围环境的地图信息；所述MCU模块还用于根据所述地图信息控制机器人行进。该实施例中的机器人，由于采用了本发明所述的方法基于激光SLAM策略进行导航，不仅通过多种方式保证了第一数据和第二数据在源头上的准确性，而且通过子地图构建和子地图拼接的方式获得周围环境的地图信息，从而能够更加精准、高效地进行定位和导航。

附图说明

图1是本发明所述方法其中一实施例的步骤框图；

图2是本发明所述方法其中一实施例中子地图构建过程中的示意图；

图3是本发明所述方法其中一实施例中子地图拼接的示意图；

图4是本发明所述方法其中一实施例中子地图拼接的又一示意图；

图5是本发明所述方法其中一实施例中全局地图优化的示意图；

图6a和图6b是本发明所述方法其中一实施例中对激光数据进行筛选的示意图；

图7是本发明所述机器人其中一实施例的模块架构图。

具体实施方式

以下通过附图和具体实施例对本发明所提供的技术方案做更加详细的描述：

如图1所示的，是本发明其中一实施例的步骤框图。该实施例揭示了一种基于SLAM的机器人导航方法，包括：

步骤101，位姿确定步骤，根据第一数据获取机器人的初始位姿和运动轨迹，第一数据包括里程计数据和惯导数据；

步骤102，子地图构建步骤，以第一数据为初始值通过第二数据对第一数据进行优化校准，并根据第二数据确定相应子地图中的环境特征信息，在达到预设条件时判断已完成相应子地图构建过程，并按照位姿确定步骤和子地图构建步骤开启一个新的子地图构建过程，第二数据为激光数据；

步骤103，子地图拼接步骤，提取子地图中的环境特征信息并根据环境特征信息确定机器人在沿着运动轨迹依次相邻的子地图中的位姿差异信息，根据位姿差异信息进行旋转平移变换对子地图进行拼接以获得相应的全局地图；

步骤104，全局地图优化步骤，根据环境特征信息补齐具有邻接关系的子地图并确定具有邻接关系的子地图之间的耦合约束条件，根据耦合约束条件对全局地图进行优化处理。

该实施例中，机器人沿着运动轨迹依次构建子地图，在达到预设条件后对相应子地图中的第一数据和第二数据进行冻结并开启一个新的子地图构建过程，相较于传统的采用EKF(扩展卡尔曼滤波)算法基于完整地图对里程计数据、惯导数据和激光数据进行优化的策略，本发明所述的方法能够大大减少运算量，并避免前、后数据之间的相互影响，从而在源头上保证了数据的质量。该实施例中，通过确定机器人在沿着所述运动轨迹依次相邻的子地图中的位姿差异信息，能够将传感器漂移所导致的误差通过计算旋转平移参数在子地图拼接的过程中加以消除，从而避免了里程计数据和惯导数据的误差累积，使最终获得的全局地图能够更加准确地反映周围环境信息。同时，在该实施例中，全局地图构建完成之后，由于相邻接的子地图之间具备一定的耦合约束条件，利用该耦合约束条件能够在全局范围内对地图信息再次进行优化，从而进一步地提高地图信息的质量。

如图2所示的，是本发明所述方法其中一实施例中子地图构建过程的示意图，该实施例中，通过获取机器人201的第一数据(里程计数据和惯导数据)能够获知机器人201的初始位姿和运动轨迹202。通过第二数据(激光数据)可以对第一数据进行优化校准，以使机器人201的位姿信息更加准确，同时还能够能够确定相应子地图中的环境特征信息204以确定机器人201周围的障碍物分布情况。随着机器人201的运动，在达到一定的预设条件时，机器人201会对第一数据和第二数据进行冻结，完成其中一个子地图的构建过程，并开启一个新的子地图构建过程，沿着运动轨迹依次对子地图进行构建。对于在机器人201运动轨迹之外的子地图，其中的第一数据为空而仅包含第二数据，根据第二数据确定的环境特征信息可以将具有邻接关系的子地图补齐。对全部的子地图203进行拼接即可获得一张完整的全局地图205，为机器人201提供可靠的地图信息。该实施例中，扫地机的初始位姿首先通过IMU和里程计数据进行估计，随后通过图优化方法对位姿进一步校正。其中，IMU称为惯导测量器件，由加速度计和陀螺仪(有些IMU还包含磁场计)组成，加速度计利用地球重力场用来测量载体的姿态角，当载体偏转时，在各方向轴上受的重力不同，容易受到其它力矩的影响，但具有较好的静态特性，即低频段的动态响应好；陀螺仪在短时间间隔内读取载体坐标系下机器人在各个方向轴上的角速度，存在温度漂移和零点漂移，由于角速度为积分量，因此随着时间的累积，角速度才会有较大的姿态误差，但具有较好的高频段动态性能。里程计以机器人两个主动轮的编码器读数为输入，通过差分运动模型可计算出扫地机在二维空间中的初始位姿，而由于里程计存在打滑及数据漂移等情况，该数据在从长时间来看，存在较大误差。因此，本发明首先利用IMU自身的动态响应速度及累计误差源的不同，利用互补滤波算法对加速度计与陀螺仪的信息进行融合，随后通过卡尔曼滤波对滤波后的IMU数据与里程计数据进行进一步融合，从而达到较好的位姿估计效果。

该实施例中首先将里程计数据和惯导数据融合为一个整体作为第一数据，具体方法为：

利用加速度计、陀螺仪组成的IMU***获取扫地机的姿态信息，对各传感器的输出信号进行表示，建立传感器输出信号的数学表达式。其中，陀螺仪的误差主要由随机漂移和噪声构成，设y_wt为陀螺仪的输出信号，w_t为真实角速度值，v_wt为测量噪声，b为漂移量，其数学模型可表示为：

y_wt＝w_t+v_wt+b (1)

同理，加速度计的输出信号为y_at，载体运动的加速度为a_t，地球重力加速度为g_t，其中

为旋转矩阵，v_at表示高斯白噪声，加速度计的数学模型可表示为

y_at＝a_t-g_t+v_at (2)

利用滑动均值滤波可除去(1)式中的测量噪声和高斯白噪声以及干扰信息，提高***的精度：其中滑动均值滤波公式为：

式(3)中，y_{_i}为各传感器输出的原始数据，w为滑动均值滤波器的输出结果，n为滑动均值滤波器阶数。

由公式(3)可推得，每次参与滤波的传感器数据式中为当前传感器最新采样的n个数据，滑动均值滤波器只涉及线性运算，可以保证滤波的实时性。

为了防止运算过程中出现“万向锁”的问题，且为了保证计算效率，采用四元数表示机器人的姿态，建立载体坐标系与参考坐标系之间的空间角位置关系，利用变换矩阵得到姿态角的四元数表达式，分别进行加速度计和陀螺仪的角加速度和角速度解算。

四元数的表示形式为：

机器人的姿态由俯仰角θ、横滚角φ、偏航角ψ描述，选取东北天坐标系(g系)为参考坐标系；载体坐标系(b系)以载体作为参考，坐标原点为载体重心。

参考坐标系被认为是固定不变的，载体坐标系相对于参考坐标系的方位变化，相对变化情况即为待求的载体姿态，常用姿态角表示，两坐标系之间的关系可以用旋转矩阵

表示为：

上式由四元数表达法可表示为

因此，可推导得到姿态角为：

基于四元数法进行姿态解算，结合加速度计和陀螺仪所测数据，加速度计主要是测量载体坐标系下旋转加速度，陀螺仪主要测量载体坐标系下的旋转角速度。以加速度计在无加速状况下时的输出结果计算IMU惯性测量***的初始欧拉角，联立(4)(5)(6)可求出初始四元数，对脱落与偶的角速度进行等时间间隔定时采样得到IMU惯性测量***角增量，结合四元数微分方程，根据上一时刻姿态四元数求解当前时刻更新的姿态四元数，带入(7)完成姿态解算。

通过IMU惯导测量器件，我们可以得到机器人的姿态。与此同时，机器人的里程计不断记录机器人的位移，根据差分驱动方程，可计算出机器人的初始位姿

在本发明其中一实施例中，所述预设条件为：在未超过预设时间的情况下机器人在相应子地图行进的过程中所确定的环境特征信息超过阈值；或者，在已超过预设时间的情况下机器人在相应子地图行进的过程中所确定的环境特征信息仍未超过阈值。该实施例中，在短时间内可以认为传感器漂移的影响足够小，因而需要在合适的时机完结相应的子地图构建过程，并开启一个新的子地图构建过程，从而避免传感器数据的误差累积。对环境特征信息的数据量和时间设置阈值作为子地图构建过程是否完成的标志，一方面能够确保所获得的激光数据的数据量足以逆推出机器人的位姿对第一数据进行校准优化，另一方面也能够避免子地图构建过程持续时间过长而导致传感器数据漂移。在本发明其中一实施例中，所述环境特征信息包括由激光雷达所扫描到的障碍物以及墙壁等。

在本发明所述方法的其中一实施例中，所述子地图构建步骤包括：对子地图划分栅格，并根据第一数据在运动轨迹上进行采样，在采样点处扫描获得各个栅格中的第二数据并建立第一数据与第二数据的映射关系，其中，将包含有环境特征信息的栅格确定为特征栅格。该实施例中，对子地图划分栅格，并通过采样点扫描的方式建立第一数据与第二数据的映射关系，从而实现相应子地图中第一数据和第二数据的冻结。同时，当机器人在运动过程中被搬离至新的位置时，能够根据环境特征信息快速筛选确定特征栅格，判断自身是否位于已经构建的其中一个子地图，如果确定自己在一个已经构建的子地图中，则可根据相应子地图指引自身行进，从而避免重复建图；如果发现没有与之相对应的子地图，则确定自己位于一个新的区域，并按照相应的步骤重新构建地图。采样点位姿与采样点坐标系之间存在如下关系：

其中，可以定义每个栅格的单位长度为5cm，激光传感器向周边环境不断发射红外激光，如果某一个栅格中存在障碍物或者是存在边界，激光传感器所接收到的相对于扫地机的方位信息为

其所占据的栅格信息为(i,j)。在短时间内，位姿信息的漂移较小，可以使用该位姿信息进行地图构建，当子地图构建完成后，在一定时间和区域内，将不再对子地图进行修改，以避免前后数据的相互影响。

在本发明所述方法其中一实施例中，所述子地图构建步骤包括：以一个完整子地图构建过程为周期确定相应特征栅格的出现概率以判断所述环境特征信息的真实性，其中所述出现概率＝(该周期内扫描到相应特征栅格的采样点的个数/该周期内采样点的总个数)*100％。将全部子地图栅格化后，机器人沿着运动轨迹在一个子地图中行进时，能够扫描到一些环境特征信息，但其中有一些环境特征信息有可能并不是真实存在的障碍物、墙壁等的信息，而是由光影变换等因素引起的激光数据的噪点，为此需要判断环境特征信息的真实性，以确保激光数据的准确度。如果环境特征信息是由真实物体所产生的，则机器人在该子地图中行进时在大多数采样点处通常均能观测到相应的环境特征信息；但若所述环境特征信息属于激光数据的噪点，则在不同采样点处获得的结果会有所不同，即在某些采样点处能够观测到相应的环境特征信息，而某一些则不能。因此，在该实施例中，给出了特征栅格的出现概率的公式，以确定相应环境特征信息的真实性，从而保证激光数据的准确。例如，机器人在其中一张子地图中行进，该子地图中设置的采样点的个数n＝100，若在其中的m(m＝86)个采样点的位置处，机器人均能够观测到某一特征栅格，则该特征栅格的出现概率＝(m/n)*100％＝86％，若阈值为80％，由于该特征栅格的出现概率超过了阈值，可以确定该特征栅格是真实存在的，而非由噪点引起。

在本发明所述方法的其中一实施例中，所述子地图拼接步骤包括：对机器人在依次相邻的子地图中所共同扫描到的特征栅格进行匹配并确定其中用以表征相同环境特征信息的第一特征点对；根据所述第一特征点对的坐标信息求解旋转分量和平移分量以获得所述位姿差异信息；根据所述位姿差异信息将所述依次相邻的子地图拼接在一起。如图3和图4所示的，涉及本发明所述方法的子地图拼接步骤。在图3中，子地图301和子地图302为沿着运动轨迹303依次相邻的两张子地图，由于传感器误差，两张子地图没有平滑地拼接在一起，子地图302相对于子地图301间隔了一定距离，并偏转了一定的角度，因而需要计算获得相应的旋转分量和平移分量，确定两张地图的位姿差异信息，并根据位姿差异信息对子地图302进行旋转平移变换，从而将子地图302和子地图301平滑地拼接在一起，以消除传感器漂移带来的误差。图3中的弧形箭头代表需要对子地图302进行旋转的方向，直线箭头代表需要对子地图302进行平移的方向。该实施例中，子地图构建过程和子地图拼接过程可同时进行，机器人在构建新子地图时，将原有的子地图进行拼接，以提升地图的构建效率。

该实施例中，由于所选取用于拼接的子地图是沿着运动轨迹依次相邻的，机器人在两张子地图中运动时会观测到大量表征相同环境特征信息的特征点，即机器人在其中一张子地图中观测到的障碍物，在运行至紧邻的下一张子地图中时也能够观测到该障碍物，但两张子图中所获得的障碍物的坐标可能会存在微小差异。将机器人在不同子地图中所观察到的表征相同环境特征信息的特征点组成第一特征点对，根据第一特征点对的坐标信息可以求解出引起误差的旋转平移矩阵。由于特征点之间的误差和子地图之间的误差所产生的原因相同，二者之间存在对应关系，因此根据旋转平移矩阵分离出旋转向量和平移向量即可获得机器人相应的位姿差异信息，根据所述位姿差异信息调整依次相邻的子地图之间距离和角度，就能够消除传感器的累积误差并实现子地图之间的平滑过渡，从而将各个子地图完整拼接为全局地图。如图4所示的，涉及本发明所述方法的子地图拼接步骤。该实施例中，子地图401和子地图402是沿着运动轨迹依次相邻的两张子地图，两张子地图之间存在间隙和夹角，需要通过旋转平移变换后实现平滑过渡拼接。全局地图406中的子地图均被划分为若干个栅格403，其中栅格403中带有“×”形标记的为机器人在子地图401中所扫描到的特征点404，带有“○”形标记的为机器人在子地图402中所扫描到的特征点405，其中有一些特征点是机器人在子地图401和子地图402中都能扫描到的，即图中“×”形标记和“○”形标记相交叠进行表示的两个特征点，这些特征点所表征的环境特征信息是相同的，即机器人在相应的子地图中均扫描到该位置处存在障碍物，但从子地图401和子地图402中所观察到的表征相同环境特征信息的特征点的坐标存在差异，将机器人在子地图401和子地图402中所扫描到的表征相同环境信息的特征点组成第一特征点对407，根据第一特征点对的坐标信息可以求解处相应的旋转分量和平移分量，从而最终将子地图401和子地图402拼接在一起，并消除传感器的漂移误差。以一个第一特征点对为例，其中包含有从子地图401中所观察到的特征点，其归一化坐标可记为x1＝[u1，v1,1]^T，以及从子地图402中所观察到的特征点，其归一化坐标可记为x2＝[u2,v2,1]^T，则有下列关系：

其中E为旋转平移矩阵，

只要求解出E，即获得了两张子地图的位姿差异信息，从而完成两张子地图之间的拼接。其中，要求解旋转平移矩阵E，只要获取8个第一特征点对407的坐标信息即可，而激光雷达每一帧可扫描数百个激光点数据，有足够的第一特征点对可供选取，并从中进行筛选以确保数据的准确性，求解获得旋转平移矩阵E后，通过SVD(奇异值分解)方法可获得旋转分量和平移分量，从而对子地图402进行旋转平移，平滑地将子地图401和子地图402拼接在一起。

在本发明所述方法的其中一实施例中，所述全局地图优化步骤包括：在全局子地图中逐一选取目标子地图并确定与所述目标子地图具有邻接关系的边缘子地图；根据相应的目标子地图和边缘子地图确定所述目标子地图的边界区域，所述边界区域为在预设范围内横跨目标子地图和边缘子地图的区域；对机器人在相应边界区域所扫描到的特征栅格进行匹配并确定其中用以表征相同环境特征信息的第二特征点对；根据所述第二特征点对的坐标信息求解所述耦合约束条件以对所述全局地图进行优化处理。该实施例中，从全局地图出发，又对地图信息进行了优化，以确保地图信息的准确性。全局地图拼接完成后，存在邻接关系的子地图之间存在耦合约束条件，即在子地图之间的交界区域由机器人所观察到的环境特征信息存在很高的一致性，即使两张子地图在时序上不相邻而仅在位置上相邻，机器人在二者的边界区域也能够观测到大量表征相同环境特征信息的特征点，将这些特征点组成第二特征点对，并根据第二特征点对的坐标信息求解相应的耦合约束条件，能够获得目标子地图与边缘子地图之间位置偏差，将具有邻接关系的子地图之间的距离和角度再作调整，从而能够进一步提升地图信息的质量。如图5所示的，涉及本发明所述方法其中一实施例的全局地图拼接步骤，该实施例中，与目标子地图501具有邻接关系的边缘子地图502共有4块，以其中一块位于目标子地图501左侧的边缘子地图502为例，其与目标子地图501存在一定的边界区域503，在该边界区域503内被机器人所扫描到的激光数据具有高度的一致性，其中既包括机器人所扫描到的在目标子地图501中部分数据，也包括机器人所扫描到在边缘子地图502中的部分数据，图5中“×”形标记代表机器人在边界区域503中属于目标子地图501部分的特征点504，“○”形标记代表机器人在边界区域503中属于边缘子地图502部分的特征点505，而由“×”形标记和“○”标记相交叠表示的部分，属于在边缘区域503中目标子地图501和边缘子地图502均能观测到的两个特征点，它们表征相同的环境特征信息，但坐标信息存在差异，该类特征点组成相应的第二特征点对506。根据第二特征点对506的坐标信息同样可以求解出相应的旋转分量和平移分量，以确定相应的耦合约束条件，根据相应的耦合约束条件对目标子地图501周边的边缘子地图502进行旋转平移变换，能够进一步消除子地图之间的拼接误差，使全局地图507更加平滑，从而提高地图信息的可靠性。

在本发明所述方法的其中一实施例中，所述子地图构建步骤包括：获取惯导数据中机器人在竖直方向的加速度值；在所述加速度值超过阈值时，对所述第二数据进行修正，或者舍弃相应的第一数据和第二数据。该实施例中，惯导数据通过加速度计和陀螺仪获知。其中借助加速度计能够获知机器人在竖直方向的加速度值，当机器人在竖直方向的加速度值超过阈值时，说明路况比较颠簸，机器人由于爬坡、越障等原因在竖直方向的抖动比较严重，这将导致此时产生的激光数据存在很大的偏差，因而在这种情况下不适合通过第二数据对第一数据进行优化校准。为了避免这种情况的发生，在所述加速度值超过阈值时，可以选择丢弃此时获得的激光数据，因为机器人在短时间内能够获得大量激光数据，丢弃其中一帧或几帧的数据不会对计算机器人位姿的准确性造成影响；也可以选择根据惯导数据逆推机器人在该时刻下由于颠簸而引起的倾角，根据相应的角度信息对激光数据进行矫正后再用于构建子地图。如图6a和图6b所示的，涉及本发明所述方法其中一实施例的子地图构建步骤。正常情况下，如图6a所示的，机器人601如果平稳行进，其通过激光雷达602获得的激光数据则较为准确，能够准确获知障碍物603的位置信息；但如果机器人601行进的路线较为颠簸，如图6b所示的，若此时机器人601在越障时被抬起θ角度，此时通过激光雷达602测得的障碍物603的位置信息将发生较大的误差，这一时刻的激光数据不适宜对第一数据进行优化校准。为避免此情况的发生，该实施例中通过获取惯导数据中机器人在竖直方向的加速度值a；在所述加速度值a超过阈值时，对所述第二数据进行修正，或者舍弃相应的第一数据和第二数据。由于激光雷达603扫描的速度非常快，在构图过程中丢弃某一时刻的第一数据和第二数据，不会对地图的构建造成太大影响。

在本发明所述方法的其中一实施例中，所述全局地图优化步骤还包括：对所述全局地图进行闭环检测以获取机器人的最优轨迹；根据所述最优轨迹重新拟合所述第二数据以消除子地图之间的累积误差。该实施例中，通过闭环检测的方法进一步消除子地图之间的累积误差，使地图信息更加真实、可靠。闭环检测所依据的目标函数为：

其中，

为在全局地图中需要被优化的位姿。∑_ij为两个位姿之间的信息矩阵，其存在一定的约束关系。由于上式为二次式，其复杂度会随着数据量的增长进而二次增长，因而，我们将其转换为：

其中

上面式子中机器人在激光SLAM导航过程中最优轨迹的求解可以表示求解机器人位姿使下面误差平方函数最小，即：

经过闭环检测后的优化结果为机器人的最优轨迹，通过将最优轨迹中的位姿对激光数据进行重新拟合，则可获取子地图之间累积误差最小的全局地图。

在本发明所述方法的其中一实施例中，所述位姿确定步骤包括：采用卡尔曼滤波算法对里程计数据和惯导数据进行融合获得所述第一数据，并以四元数法表征机器人的姿态信息。通过卡尔曼滤波算法对里程计数据和惯导数据进行融合能够消除大量传感器导致的噪声影响，采用四元数法表征机器人的位姿信息，能够避免在运算过程中“万向锁”问题的产生。

本发明还揭示了一种基于SLAM方法进行导航的机器人，包括MCU模块701，建图模块702，以及用于获取第一数据的里程计模块703和惯导模块704、用于获取第二数据的激光模块705，其中：MCU模块用于获取第一数据和第二数据，并将第一数据和第二数据发送至建图模块702；建图模块702用于根据第一数据和第二数据执行位姿确定步骤、子地图构建步骤、子地图拼接步骤以及全局地图优化步骤以获取周围环境的地图信息；MCU模块701还用于根据地图信息控制机器人行进。该实施例中的机器人，由于采用了本发明所述的方法基于激光SLAM策略进行导航，不仅通过多种方式保证了第一数据和第二数据在源头上的准确性，而且通过子地图构建和子地图拼接的方式获得周围环境的地图信息，从而能够更加精准、高效地进行定位和导航。该实施例中的机器人，可选地为一款扫地机器人，其MCU模块701还用于连接并控制扫地机器人的行走轮707以及清扫模块708，以对房间进行清洁。该实施例中的建图模块702为一块上层算法板，其还可连接外网网关，与服务器进行数据交互，同时还能够根据激光SLAM算法和行走路径规划算法为扫地机器人进行导航。该扫地机器人还设置有碰撞检测模块706，与MCU模块701相连。

上述具体实施方式只是用于说明本发明的设计方法，并不能用来限定本发明的保护范围。对于在本发明技术方案的思想指导下的变形和转换，都应该归于本发明保护范围以内。

Claims (10)

1.一种基于SLAM的机器人导航方法，其特征在于，包括：

位姿确定步骤，根据第一数据获取机器人的初始位姿和运动轨迹，所述第一数据包括里程计数据和惯导数据；

子地图构建步骤，以第一数据为初始值通过第二数据对所述第一数据进行优化校准，并根据第二数据确定相应子地图中的环境特征信息，在达到预设条件时判断已完成相应子地图构建过程，并按照所述位姿确定步骤和子地图构建步骤开启一个新的子地图构建过程，所述第二数据为激光数据；

子地图拼接步骤，提取子地图中的环境特征信息并根据所述环境特征信息确定机器人在沿着所述运动轨迹依次相邻的子地图中的位姿差异信息，根据所述位姿差异信息进行旋转平移变换对所述子地图进行拼接以获得相应的全局地图；

全局地图优化步骤，根据所述环境特征信息补齐具有邻接关系的子地图并确定所述具有邻接关系的子地图之间的耦合约束条件，根据所述耦合约束条件对所述全局地图进行优化处理。

2.根据权利要求1所述的方法，其特征在于，所述预设条件为：

在未超过预设时间的情况下机器人在相应子地图行进的过程中所确定的环境特征信息超过阈值；或者，

在已超过预设时间的情况下机器人在相应子地图行进的过程中所确定的环境特征信息仍未超过阈值。

3.根据权利要求1所述的方法，其特征在于，所述子地图构建步骤包括：

对子地图划分栅格，并根据第一数据在运动轨迹上进行采样，在采样点处扫描获得各个栅格中的第二数据并建立第一数据与第二数据的映射关系，其中，将包含有环境特征信息的栅格确定为特征栅格。

4.根据权利要求3所述的方法，其特征在于，所述子地图构建步骤包括：

以一个完整子地图构建过程为周期确定相应特征栅格的出现概率以判断所述环境特征信息的真实性，其中所述出现概率＝(该周期内扫描到相应特征栅格的采样点的个数/该周期内采样点的总个数)*100％。

5.根据权利要求4所述的方法，其特征在于，所述子地图拼接步骤包括：对机器人在依次相邻的子地图中所共同扫描到的特征栅格进行匹配并确定其中用以表征相同环境特征信息的第一特征点对；

根据所述第一特征点对的坐标信息求解旋转分量和平移分量以获得所述位姿差异信息；

根据所述位姿差异信息将所述依次相邻的子地图拼接在一起。

6.根据权利要求4所述的方法，其特征在于，所述全局地图优化步骤包括：

在全局地图中逐一选取目标子地图并确定与所述目标子地图具有邻接关系的边缘子地图；

根据相应的目标子地图和边缘子地图确定所述目标子地图的边界区域，所述边界区域为在预设范围内横跨目标子地图和边缘子地图的区域；

对机器人在相应边界区域所扫描到的特征栅格进行匹配并确定其中用以表征相同环境特征信息的第二特征点对；

根据所述第二特征点对的坐标信息求解所述耦合约束条件以对所述全局地图进行优化处理。

7.根据权利要求1-6中任一项所述的方法，其特征在于，所述子地图构建步骤包括：

获取惯导数据中机器人在竖直方向的加速度值；

在所述加速度值超过阈值时，对所述第二数据进行修正，或者舍弃相应的第一数据和第二数据。

8.根据权利要求1-6中任一项所述的方法，其特征在于，所述全局地图优化步骤还包括：

对所述全局地图进行闭环检测以获取机器人的最优轨迹；

根据所述最优轨迹重新拟合所述第二数据以消除子地图之间的累积误差。

9.根据权利要求1-6中任一项所述的方法，其特征在于，所述位姿确定步骤包括：

采用卡尔曼滤波算法对里程计数据和惯导数据进行融合获得所述第一数据，并以四元数法表征机器人的姿态信息。

10.一种采用权利要求1-9中任一项所述方法进行导航的机器人，包括MCU模块、建图模块，以及用于获取第一数据的里程计模块和惯导模块、用于获取第二数据的激光模块，其特征在于：

所述MCU模块用于获取所述第一数据和第二数据，并将所述第一数据和第二数据发送至所述建图模块；

所述建图模块用于根据所述第一数据和第二数据执行所述位姿确定步骤、子地图构建步骤、子地图拼接步骤以及全局地图优化步骤以获取周围环境的地图信息；所述MCU模块还用于根据所述地图信息控制机器人行进。

Images