本发明公开了一种全局定位与局部定位互校准的机器人及其控制方法，所述方法包括对全局定位信息与局部定位信息之间的偏差进行分析，当所述偏差大于预设的范围时，对所述全局定位算法和/或局部定位算法进行检查，当所述检查的结果为全局定位算法及局部定位算法的其中一者正常运行而另一者异常运行时，使用其中正常运行的算法输出的定位信息对异常运行的算法进行校正等步骤。本发明通过全局定位算法和局部定位算法之间的互校准，当全局定位算法或局部定位算法发生故障时，发生故障的定位算法可以通过正常的定位算法获得良好的重启动条件而重新对机器人进行精确的定位，使得机器人定位和导航的精度大幅提高。本发明广泛应用于机器人控制技术领域。
1.一种全局定位与局部定位互校准的机器人，包括传感模块、控制模块和定位模块；所述控制模块接收传感模块检测到的数据并执行全局定位算法和局部定位算法，所述全局定位算法输出全局定位信息，所述局部定位算法输出局部定位信息，所述定位模块用于根据所述全局定位信息和局部定位信息进行定位；其特征在于，所述控制模块执行以下步骤：计算所述全局定位信息与局部定位信息之间的偏差并对所述偏差进行分析；
当所述偏差大于预设的范围时，对所述全局定位算法和/或局部定位算法进行检查；
当所述检查的结果为全局定位算法及局部定位算法的其中一者正常运行而另一者异常运行时，使用其中正常运行的算法输出的定位信息对异常运行的算法进行校正；
所述使用其中正常运行的算法输出的定位信息对异常运行的算法进行校正这一步骤，具体包括：当所述全局定位算法正常运行且局部定位算法异常运行时，使用所述全局定位信息对局部定位算法进行初始化，重新运行所述局部定位算法；
当所述局部定位算法正常运行且全局定位算法异常运行时，使用所述局部定位信息对全局定位算法进行初始化，重新运行所述全局定位算法；
所述局部定位算法为蒙特卡洛定位算法，所述全局定位算法为Cartographer算法、视觉SLAM算法、tag算法或GPS算法。
2.根据权利要求1所述的一种全局定位与局部定位互校准的机器人，其特征在于，所述全局定位信息和局部定位信息中均包括位移信息和角度信息，所述计算所述全局定位信息与局部定位信息之间的偏差并对所述偏差进行分析这一步骤，具体包括：根据所述全局定位信息中的位移信息与局部定位信息中的位移信息，计算全局定位信息与局部定位信息之间的位移偏差；
根据所述全局定位信息中的角度信息与局部定位信息中的角度信息，计算全局定位信息与局部定位信息之间的角度偏差；
当所述位移偏差大于预设的位移偏差阈值或所述角度偏差大于预设的角度偏差阈值时，判定所述偏差大于预设的范围。
3.根据权利要求1所述的一种全局定位与局部定位互校准的机器人，其特征在于，所述对所述全局定位算法和/或局部定位算法进行检查这一步骤，具体包括：根据所述全局定位信息确认在地图中的全局定位位置；所述地图包括可行区域和不可行区域；
当所述全局定位位置位于地图上的可行区域时，判定所述全局定位算法正常运行，反之，判定所述全局定位算法异常运行。
4.根据权利要求3所述的一种全局定位与局部定位互校准的机器人，其特征在于，所述局部定位算法还输出匹配分值，所述匹配分值与局部定位算法的参数设置有关，所述对所述全局定位算法和/或局部定位算法进行检查这一步骤，具体还包括：将所述匹配分值与预设的匹配分值阈值进行比较；
当所述匹配分值大于匹配分值阈值时，判定所述局部定位算法正常运行，反之，判定所述局部定位算法异常运行。
5.一种全局定位与局部定位互校准的机器人控制方法，所述机器人执行全局定位算法和局部定位算法，所述全局定位算法输出全局定位信息，所述局部定位算法输出局部定位信息，其特征在于，包括以下步骤：计算所述全局定位信息与局部定位信息之间的偏差并对所述偏差进行分析；
当所述偏差大于预设的范围时，对所述全局定位算法和/或局部定位算法进行检查；
当所述检查的结果为全局定位算法及局部定位算法的其中一者正常运行而另一者异常运行时，使用其中正常运行的算法输出的定位信息对异常运行的算法进行校正；
所述使用其中正常运行的算法输出的定位信息对异常运行的算法进行校正，具体包括：当所述全局定位算法正常运行且局部定位算法异常运行时，使用所述全局定位信息对局部定位算法进行初始化，重新运行所述局部定位算法；
当所述局部定位算法正常运行且全局定位算法异常运行时，使用所述局部定位信息对全局定位算法进行初始化，重新运行所述全局定位算法；
所述局部定位算法为蒙特卡洛定位算法，所述全局定位算法为Cartographer算法、视觉SLAM算法、tag算法或GPS算法。
6.根据权利要求5所述的一种全局定位与局部定位互校准的机器人控制方法，其特征在于，所述全局定位信息和局部定位信息中均包括位移信息和角度信息，所述计算所述全局定位信息与局部定位信息之间的偏差并对所述偏差进行分析这一步骤，具体包括：根据所述全局定位信息中的位移信息与局部定位信息中的位移信息，计算全局定位信息与局部定位信息之间的位移偏差；
根据所述全局定位信息中的角度信息与局部定位信息中的角度信息，计算全局定位信息与局部定位信息之间的角度偏差；
当所述位移偏差大于预设的位移偏差阈值或所述角度偏差大于预设的角度偏差阈值时，判定所述偏差大于预设的范围。
7.根据权利要求5所述的一种全局定位与局部定位互校准的机器人控制方法，其特征在于，所述对所述全局定位算法和/或局部定位算法进行检查这一步骤，具体包括：根据所述全局定位信息确认在地图中的全局定位位置；所述地图包括可行区域和不可行区域；
当所述全局定位位置位于地图上的可行区域时，判定所述全局定位算法正常运行，反之，判定所述全局定位算法异常运行。
8.根据权利要求7所述的一种全局定位与局部定位互校准的机器人控制方法，其特征在于，所述局部定位算法还输出匹配分值，所述匹配分值与局部定位算法的参数设置有关，所述对所述全局定位算法和/或局部定位算法进行检查这一步骤，具体还包括：将所述匹配分值与预设的匹配分值阈值进行比较；
当所述匹配分值大于匹配分值阈值时，判定所述局部定位算法正常运行，反之，判定所述局部定位算法异常运行。
一种全局定位与局部定位互校准的机器人及其控制方法
技术领域
[0001] 本发明涉及机器人控制技术领域，尤其是一种全局定位与局部定位互校准的机器人及其控制方法。
背景技术
[0002] 在机器人控制领域，机器人通过传感器获取激光数据、IMU数据和里程计数据等原始数据，然后应用Cartographer算法以及蒙特卡洛定位算法(Adaptive Monte Carlo Localization，AMCL)等算法进行处理，对机器人进行定位。
[0003] Cartographer算法是Google公司开发的一个开源SLAM库，它。Cartographer算法通过局部子图与全局地图的匹配得到较为准确的全局定位信息，通过闭环检测来消除构图过程中产生的累积误差提供准确的定位，即Cartographer算法是一种基于图优化的算法，它不需要提供机器人初始位姿，同时无论机器人在何处，只需局部子图和全局子图实现匹配即可得到较为准确全局位置。Cartographer算法是一种全局定位算法，它的缺点在于在相似的环境中容易出现误匹配导致位置丢失。
[0004] 蒙特卡洛定位算法的主要原理是通过粒子滤波来输出用于定位和导航的稳定的局部定位信息。蒙特卡洛定位算法是一种局部定位算法，它的缺点在于启动时必须给定一个较为准确初始位姿，当机器人位置丢失较为严重的时候无法使用蒙特卡洛定位算法进行定位。
[0005] 现有的机器人控制算法的不足主要在于：无论是全局定位算法还是局部定位算法，普遍存在容易丢失当前位置信息的缺点，这对机器人的定位和导航造成了不利影响。
发明内容
[0006] 为了解决上述技术问题，本发明的目在于提供一种全局定位与局部定位互校准的机器人及其控制方法。
[0007] 一方面，本发明实施例包括一种全局定位与局部定位互校准的机器人，包括传感模块、控制模块和定位模块；所述控制模块接收传感模块检测到的数据并执行全局定位算法和局部定位算法，所述全局定位算法输出全局定位信息，所述局部定位算法输出局部定位信息，所述定位模块用于根据所述全局定位信息和局部定位信息进行定位；所述控制模块执行以下步骤：
[0008] 计算所述全局定位信息与局部定位信息之间的偏差并对所述偏差进行分析；
[0009] 当所述偏差大于预设的范围时，对所述全局定位算法和/或局部定位算法进行检查；
[0010] 当所述检查的结果为全局定位算法及局部定位算法的其中一者正常运行而另一者异常运行时，使用其中正常运行的算法输出的定位信息对异常运行的算法进行校正。
[0011] 进一步地，所述全局定位信息和局部定位信息中均包括位移信息和角度信息，所述计算所述全局定位信息与局部定位信息之间的偏差并对所述偏差进行分析这一步骤，具体包括：
[0012] 根据所述全局定位信息中的位移信息与局部定位信息中的位移信息，计算全局定位信息与局部定位信息之间的位移偏差；
[0013] 根据所述全局定位信息中的角度信息与局部定位信息中的角度信息，计算全局定位信息与局部定位信息之间的角度偏差；
[0014] 当所述位移偏差大于预设的位移偏差阈值或所述角度偏差大于预设的角度偏差阈值时，判定所述偏差大于预设的范围。
[0015] 进一步地，所述对所述全局定位算法和/或局部定位算法进行检查这一步骤，具体包括：
[0016] 根据所述全局定位信息确认在地图中的全局定位位置；所述地图包括可行区域和不可行区域；
[0017] 当所述全局定位位置位于地图上的可行区域时，判定所述全局定位算法正常运行，反之，判定所述全局定位算法异常运行。
[0018] 进一步地，所述局部定位算法还输出匹配分值，所述匹配分值与局部定位算法的参数设置有关，所述对所述全局定位算法和/或局部定位算法进行检查这一步骤，具体还包括：
[0019] 将所述匹配分值与预设的匹配分值阈值进行比较；
[0020] 当所述匹配分值大于匹配分值阈值时，判定所述局部定位算法正常运行，反之，判定所述局部定位算法异常运行。
[0021] 进一步地，所述使用其中正常运行的算法输出的定位信息对异常运行的算法进行校正这一步骤，具体包括：
[0022] 当所述全局定位算法正常运行且局部定位算法异常运行时，使用所述全局定位信息对局部定位算法进行初始化，重新运行所述局部定位算法；
[0023] 当所述局部定位算法正常运行且全局定位算法异常运行时，使用所述局部定位信息对全局定位算法进行初始化，重新运行所述全局定位算法。
[0024] 另一方面，本发明实施例还包括一种全局定位与局部定位互校准的机器人控制方法，所述机器人执行全局定位算法和局部定位算法，所述全局定位算法输出全局定位信息，所述局部定位算法输出局部定位信息，所述方法包括以下步骤：
[0025] 计算所述全局定位信息与局部定位信息之间的偏差并对所述偏差进行分析；
[0026] 当所述偏差大于预设的范围时，对所述全局定位算法和/或局部定位算法进行检查；
[0027] 当所述检查的结果为全局定位算法及局部定位算法的其中一者正常运行而另一者异常运行时，使用其中正常运行的算法输出的定位信息对异常运行的算法进行校正。
[0028] 进一步地，所述全局定位信息和局部定位信息中均包括位移信息和角度信息，所述计算所述全局定位信息与局部定位信息之间的偏差并对所述偏差进行分析这一步骤，具体包括：
[0029] 根据所述全局定位信息中的位移信息与局部定位信息中的位移信息，计算全局定位信息与局部定位信息之间的位移偏差；
[0030] 根据所述全局定位信息中的角度信息与局部定位信息中的角度信息，计算全局定位信息与局部定位信息之间的角度偏差；
[0031] 当所述位移偏差大于预设的位移偏差阈值或所述角度偏差大于预设的角度偏差阈值时，判定所述偏差大于预设的范围。
[0032] 进一步地，所述对所述全局定位算法和/或局部定位算法进行检查这一步骤，具体包括：
[0033] 根据所述全局定位信息确认在地图中的全局定位位置；所述地图包括可行区域和不可行区域；
[0034] 当所述全局定位位置位于地图上的可行区域时，判定所述全局定位算法正常运行，反之，判定所述全局定位算法异常运行。
[0035] 进一步地，所述局部定位算法还输出匹配分值，所述匹配分值与局部定位算法的参数设置有关，所述对所述全局定位算法和/或局部定位算法进行检查这一步骤，具体还包括：
[0036] 将所述匹配分值与预设的匹配分值阈值进行比较；
[0037] 当所述匹配分值大于匹配分值阈值时，判定所述局部定位算法正常运行，反之，判定所述局部定位算法异常运行。
[0038] 进一步地，所述使用其中正常运行的算法输出的定位信息对异常运行的算法进行校正这一步骤，具体包括：
[0039] 当所述全局定位算法正常运行且局部定位算法异常运行时，使用所述全局定位信息对局部定位算法进行初始化，重新运行所述局部定位算法；
[0040] 当所述局部定位算法正常运行且全局定位算法异常运行时，使用所述局部定位信息对全局定位算法进行初始化，重新运行所述全局定位算法。
[0041] 本发明的有益效果是：通过全局定位算法和局部定位算法之间的互校准，当全局定位算法或局部定位算法发生位置丢失等故障时，发生故障的定位算法可以通过正常的定位算法获得良好的重启动条件而重新对机器人进行精确的定位，使得机器人定位和导航的精度大幅提高。
附图说明
[0042] 图1为本发明全局定位与局部定位互校准的机器人控制方法实施例的流程图；
[0043] 图2为本发明全局定位与局部定位互校准的机器人控制方法实施例的原理图。
具体实施方式
[0044] 实施例1
[0045] 本实施例中的机器人包括传感模块、控制模块和定位模块。其中，传感模块包括多个传感器，机器人可以通过多个传感器获取激光数据、IMU数据和里程计数据等原始数据。控制模块包括存储器和处理器，存储器中存储了多个计算机程序供处理器运行，使得处理器可以执行全局定位算法和局部定位算法属于激光数据、IMU数据和里程计数据等原始数据进行处理，分别输出全局定位信息和局部定位信息。全局定位信息中主要包含用于从全局角度描述机器人位移和角度等位置的位移信息和角度信息，局部定位信息中也主要包含用于从局部角度描述机器人位移和角度等位置的位移信息和角度信息。定位模块具有定位和导航的功能，即根据全局定位信息和局部定位信息确定机器人的位置并对机器人进行导航。
[0046] 本实施例中，所述局部定位算法是指蒙特卡洛定位算法，所述全局定位算法是指Cartographer算法。由于在实际生产过程中，机器人主要是依赖局部定位算法进行定位的，而本实施例中全局定位算法的作用主要是校准局部定位算法，因此也可以使用视觉SLAM算法、tag算法或GPS算法等精度稍低的算法代替Cartographer算法作为全局定位算法。
[0047] 本实施例中，参照图1，机器人的控制模块执行以下步骤以实现全局定位与局部定位的互校准：
[0048] S1.计算所述全局定位信息与局部定位信息之间的偏差并对所述偏差进行分析；
[0049] S2.当所述偏差大于预设的范围时，对所述全局定位算法和/或局部定位算法进行检查；
[0050] S3.当所述检查的结果为全局定位算法及局部定位算法的其中一者正常运行而另一者异常运行时，使用其中正常运行的算法输出的定位信息对异常运行的算法进行校正。
[0051] 机器人的控制模块所执行的局部定位算法和全局定位算法是从不同的角度对机器人进行定位，即在同一时刻，局部定位算法输出的局部定位信息和全局定位算法输出的全局定位信息均描述当时机器人所处的位置。由于不同的算法在原理和精度上存在差异，局部定位信息和全局定位信息之间可能会存在偏差，但局部定位算法和全局定位算法正常运行时，偏差将处于可接受的程度。
[0052] 本实施例的步骤S1中，通过将全局定位信息与局部定位信息进行对比，计算出它们之间的偏差，并对偏差的大小进行分析。
[0053] 局部定位算法和全局定位算法可能会出现四种情况：局部定位算法和全局定位算法均正常运行，局部定位算法正常运行、全局定位算法异常运行，局部定位算法异常运行、全局定位算法正常运行，以及局部定位算法和全局定位算法均异常运行。当局部定位算法和全局定位算法均正常运行时，全局定位信息与局部定位信息之间的偏差处于可接受的程度，即偏差小于预设的范围。当局部定位算法和全局定位算法均异常运行时，有可能出现全局定位信息与局部定位信息之间的偏差小于预设的范围，但由于局部定位算法和全局定位算法同时异常运行的概率很低，因此本实施例中不考虑这种情况。当局部定位算法正常运行、全局定位算法异常运行或局部定位算法异常运行、全局定位算法正常运行时，全局定位信息与局部定位信息之间的偏差超出可接受的程度，即偏差大于预设的范围。
[0054] 步骤S2中，当判定所述偏差大于预设的范围时，表明局部定位算法和全局定位算法之间任一者的运行出现了异常，执行对所述全局定位算法和/或局部定位算法进行检查的步骤，以具体地确定是局部定位算法或全局定位算法出现了异常。步骤S2中，也有可能检查到局部定位算法和全局定位算法同时出现了异常。
[0055] 步骤S3中，当所述检查的结果为全局定位算法及局部定位算法的其中一者正常运行而另一者异常运行时，使用其中正常运行的算法输出的定位信息对异常运行的算法进行校正。也就是当局部定位算法正常运行而全局定位算法异常运行时，使用局部定位算法输出的局部定位信息对全局定位算法进行校正；当全局定位算法正常运行而局部定位算法异常运行时，使用全局定位算法输出的全局定位信息对局部定位算法进行校正。
[0056] 步骤S1-S3的原理如图2所示。当使用蒙特卡洛定位算法和Cartographer算法时，步骤S1-S3的具体过程为：当通过步骤S1和S2检测到蒙特卡洛定位算法丢失当前位置时，通过步骤S3使用Cartographer算法输出的较为准确的全局定位信息对机器人位姿进行校正，再通过蒙特卡洛定位算法粒子滤波的收敛得到准确而稳定的局部定位信息用于定位和导航；当通过步骤S1和S2检测到Cartographer算法丢失当前位置时，通过步骤S3使用蒙特卡洛定位算法输出的较为准确的局部定位信息，使得Cartographer算法可以根据局部定位信息进行初始化，重新执行Cartographer算法并进行全局匹配。
[0057] 蒙特卡洛定位算法输出的局部定位信息中包含有机器人的位置[x，y，z]和姿态角[α，β，γ]等位姿信息。对Cartographer算法进行初始化时，结束当前执行的Cartographer算法，清除其历史数据，通过Cartographer算法的初始化接口，接收局部定位信息中的位姿信息[x，y，z，α，β，γ]作为算法的初始位姿，重新运行Cartographer算法，完成蒙特卡洛定位算法对Cartographer算法的校准过程。蒙特卡洛定位算法也有类似的初始化接口，Cartographer算法对蒙特卡洛定位算法的校准过程与蒙特卡洛定位算法对Cartographer算法的校准过程的相似。
[0058] 通过执行步骤S1-S3，机器人可以实现全局定位算法和局部定位算法之间的互校准，当全局定位算法或局部定位算法发生位置丢失等故障时，发生故障的定位算法可以通过正常的定位算法获得良好的重启动条件而重新对机器人进行精确的定位，使得机器人定位和导航的精度大幅提高。
[0059] 进一步作为优选的实施方式，所述全局定位信息和局部定位信息中均包括位移信息和角度信息，所述步骤S1，即计算所述全局定位信息与局部定位信息之间的偏差并对所述偏差进行分析这一步骤，具体包括：
[0060] S101.根据所述全局定位信息中的位移信息与局部定位信息中的位移信息，计算全局定位信息与局部定位信息之间的位移偏差；
[0061] S102.根据所述全局定位信息中的角度信息与局部定位信息中的角度信息，计算全局定位信息与局部定位信息之间的角度偏差；
[0062] S103.当所述位移偏差大于预设的位移偏差阈值或所述角度偏差大于预设的角度偏差阈值时，判定所述偏差大于预设的范围。
[0063] 本实施例中，步骤S1的判断条件具体为：只要位移偏差或角度偏差中的任一个大于相应的阈值时，就判定所述偏差大于预设的范围，执行步骤S2。
[0064] 进一步作为优选的实施方式，所述步骤S2，即对所述全局定位算法和/或局部定位算法进行检查这一步骤，具体包括：
[0065] S201.根据所述全局定位信息确认在地图中的全局定位位置；所述地图包括可行区域和不可行区域；
[0066] S202.当所述全局定位位置位于地图上的可行区域时，判定所述全局定位算法正常运行，反之，判定所述全局定位算法异常运行。
[0067] 步骤S201和S202可以用于检查Cartographer算法等全局定位算法是否正常运行，其具体步骤为：机器人的定位模块中存储了至少一个地图，定位模块可以根据Cartographer算法输出的全局定位信息来确定机器人在地图中的位置，实现对机器人的定位和导航。地图被至少划分为可行区域和不可行区域两部分，其中可行区域表示机器人可到达的区域，不可行区域表示机器人不可到达的区域。由于机器人实际上是不能到达现实当中的不可行区域的，因此，如果Cartographer算法输出的全局定位位置位于地图上的不可行区域，则表明Cartographer算法输出的全局定位位置与机器人的实际位置不符，可以判定全局定位算法异常运行。
[0068] 进一步作为优选的实施方式，蒙特卡洛定位算法等局部定位算法还输出匹配分值，所述匹配分值与蒙特卡洛定位算法的参数设置有关，所述步骤S2具体还包括：
[0069] S203.将所述匹配分值与预设的匹配分值阈值进行比较；
[0070] S204.当所述匹配分值大于匹配分值阈值时，判定所述局部定位算法正常运行，反之，判定所述局部定位算法异常运行。
[0071] 步骤S201和S202可以用于检查蒙特卡洛定位算法等局部定位算法是否正常运行。蒙特卡洛定位算法的输出值中还包括匹配分值，匹配分值与蒙特卡洛定位算法的参数设置有关，匹配分值越高，表明机器人的局部定位位置越准确。匹配分值阈值可以根据机器人的运行环境设定具体的数值，可以对机器人使用蒙特卡洛定位算法进行局部定位进行多次试验，将每次试验中机器人局部定位位置较准确时蒙特卡洛定位算法输出的匹配分值作为样本，求出这些匹配分值的均值，然后将匹配分值阈值设定为该分值的1/2。匹配分值阈值可以作为步骤S204中判断局部定位算法是否正常运行的标准。
[0072] 进一步作为优选的实施方式，所述步骤S3，即使用其中正常运行的算法输出的定位信息对异常运行的算法进行校正这一步骤，具体包括：
[0073] S301.当所述全局定位算法正常运行且局部定位算法异常运行时，使用所述全局定位信息对局部定位算法进行初始化，重新运行所述局部定位算法；
[0074] S302.当所述局部定位算法正常运行且全局定位算法异常运行时，使用所述局部定位信息对全局定位算法进行初始化，重新运行所述全局定位算法。
[0075] 本实施例中使用蒙特卡洛定位算法和Cartographer算法，其中步骤S301是指：当Cartographer算法正常运行且蒙特卡洛定位算法异常运行时，Cartographer算法输出的较为准确的全局定位信息对机器人位姿进行校正，再通过蒙特卡洛定位算法粒子滤波的收敛得到准确而稳定的局部定位信息，实现Cartographer算法对蒙特卡洛定位算法的校正；步骤S302是指：当蒙特卡洛定位算法正常运行且Cartographer算法异常运行时，使用蒙特卡洛定位算法输出的较为准确的局部定位信息，使得Cartographer算法可以根据局部定位信息进行初始化，重新执行Cartographer算法并进行全局匹配。
[0076] 本发明还包括一种全局定位与局部定位互校准的机器人控制方法，该方法与本发明产品实施例中的机器人原理是相同的，运行该方法的机器人可以实现产品实施例中机器人的功能，具备该产品实施例相应的功能和有益效果。
[0077] 以上是对本发明的较佳实施进行了具体说明，但对本发明创造并不限于所述实施例，熟悉本领域的技术人员在不违背本发明精神的前提下还可做作出种种的等同变形或替换，这些等同的变形或替换均包含在本申请权利要求所限定的范围内。
