CN113739819B - 校验方法、装置、电子设备、存储介质及芯片 - Google Patents

Abstract

本发明提供一种校验方法、装置、电子设备、存储介质及芯片，所述校验方法包括：根据相邻两帧的里程计数据，计算得到表示机器人位姿变化的第一位姿变化信息，并根据相邻两帧的点云数据，计算得到表示所述机器人位姿变化的第二位姿变化信息，并使用所述第二位姿变化信息对里程计进行校验，以便于机器人进行后续的定位工作，本发明能够降低里程计出错的机率。

Description

校验方法、装置、电子设备、存储介质及芯片

技术领域

本发明涉及机器人技术领域，尤其涉及一种校验方法、装置、电子设备、存储介质及芯片。

背景技术

机器人的定位和规划控制等都依赖于正常稳定的里程计，但是在实际实用中，里程计可能会出现各种各样的错误，比如：在光滑地面打滑或者被某些障碍物卡住时，里程计根据轮子的转动角度认为机器人已经前进，但实际上机器人的位置原地不动；在被人为拖动时，里程计认为机器人没有动，但实际位置已经发生改变；在IMU(Inertial MeasurementUnit，惯性测量单元)或编码器硬件发生故障时，里程计计算出的位置和角度也会产生错误。

因此，在里程计提供的信息错误时，会导致机器人错误估计自己所处的位置，影响后续任务的执行，甚至可能会对机器人或环境产生破坏。

发明内容

有鉴于此，本发明旨在至少在一定程度上解决相关技术中的问题之一。本发明实施例提供一种校验方法、装置、电子设备、存储介质及芯片。

第一方面，本发明实施例提供一种校验方法，包括：

通过雷达以及所述里程计周期性地同步检测机器人的位姿，其中，对应于同一时段的点云数据和里程计数据构成与该时段对应的检测帧；

针对每个时段，根据当前检测帧的里程计数据和上一检测帧的里程计数据，计算所述机器人从所述上一检测帧到所述当前检测帧的位姿变化，得到第一位姿变化信息，并根据所述当前检测帧的点云数据和所述上一检测帧的点云数据，计算所述机器人从所述上一检测帧到所述当前检测帧的位姿变化，得到第二位姿变化信息；

将所述第二位姿变化信息与所述第一位姿变化信息的差值与预设阈值进行比较，并根据比较结果对所述里程计进行校验。

第二方面，本发明实施例还提供一种计校准装置，包括：

数据获取模块，用于通过雷达以及所述里程计周期性地同步检测机器人的位姿，其中，对应于同一时段的点云数据和里程计数据构成与该时段对应的检测帧；

位姿变化计算模块，用于针对每个时段，根据当前检测帧的里程计数据和上一检测帧的里程计数据，计算所述机器人从所述上一检测帧到所述当前检测帧的位姿变化，得到第一位姿变化信息，并根据所述当前检测帧的点云数据和所述上一检测帧的点云数据，计算所述机器人从所述上一检测帧到所述当前检测帧的位姿变化，得到第二位姿变化信息；

里程计校验模块，用于将所述第二位姿变化信息与所述第一位姿变化信息的差值与预设阈值进行比较，并根据比较结果对所述里程计进行校验。

第三方面，本发明实施例还提供一种电子设备，包括：

一个或多个处理器、存储器；和一个或多个程序，其中所述一个或多个程序被存储在所述存储器中，并且被一个或多个所述处理器执行，所述程序包括用于如上述任一种所述的校验方法的步骤。

第四方面，本发明实施例还提供一种非暂态计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时实现如上述任一种所述校验方法的步骤。

第五方面，本发明实施例还提供一种芯片，包括至少一个处理器和接口；所述接口用于为所述至少一个处理器提供程序指令或者数据；

所述至少一个处理器用于执行所述程序性指令，以实现第一方面或第一方面可能的实现方式中任一方法。

本发明实施例提供的校验方法、校验装置、电子设备、存储介质及芯片，通过使用对两帧之间的点云数据计算得到的第二位姿变化信息对里程计进行校验，从而降低里程计出错的机率。

附图说明

为了更清楚地说明本发明或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作一简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是本发明实施例提供的校验方法的流程示意图；

图2是本发明实施例提供的计算点云数据位姿变化的流程示意图；

图3是本发明实施例提供的PL-ICP算法的流程示意图；

图4是图3的进行位姿的迭代优化步骤的流程示意图；

图5是本发明实施例提供的单帧PL-ICP结果的示意图；

图6是本发明实施例提供的点到线的匹配关系的示意图；

图7是本发明实施例提供的在不分析动态环境下对里程计进行校验步骤的流程示意图；

图8是本发明实施例提供的判断里程计出错的流程示意图；

图9是本发明实施例提供的判断动态环境的流程示意图；

图10是本发明实施例提供的在动态环境下对累积处理的流程示意图；

图11是本发明实施例提供的在动态环境下对里程计进行校验步骤的流程示意图；

图12是本发明实施例提供的动态环境判定结果和累积误差值的示意图；

图13是本发明一实施例提供的计校验方法的流程示意图；

图14是本发明实施例提供的校验装置的结构示意图；

图15是本发明实施例提供的电子设备的结构示意图；

图16是本发明实施例提供的芯片的结构示意图。

具体实施方式

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

本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的实施例能够以除了在这里图示或描述的内容以外的顺序实施。

以下对本发明涉及的技术术语进行描述：

里程计：是机器人相对定位的有效传感器，为机器人提供了实时的位姿信息。对于大部分的机器人平台，根据安装在驱动电机上的光电编码器来检测车轮在一定时间内转过的弧度，进而推算机器人相对位姿变化。

IMU(Inertial Measurement Unit,惯性测量单元)：里程计可以根据它来计算机器人的姿态即角度信息

激光传感器：利用激光技术进行测量的传感器，装在机器人上可以获得周围环境的点云信息。

点云匹配：对于两片相似的点云，目的是找到第一片点云P和第二片点云Q之间的旋转矩阵R和平移向量T。

ICP(Iterative Closest Point)：是一种用于解决点云匹配问题的算法，通过最小二乘法进行迭代求解点集到点集的配准关系。

PL-ICP(Point to Line ICP)：相较于一般的ICP改进了误差方程，一般的ICP优化的是点到点的距离，PL-ICP优化的是点到最近两个点连线的距离。

本发明提供一种校验方法、校验装置、电子设备、存储介质及芯片，用以解决现有技术中里程计提供的信息错误时，导致机器人错误估计自己所处的位置，影响后续任务的执行，甚至可能会对机器人或环境产生破坏等问题。通过对两帧之间的点云数据计算得到的第二位姿变化信息对里程计进行校验，从而降低里程计出错的机率。

下面结合图1-图15描述本发明的校验方法、校验装置、电子设备、存储介质及芯片。

图1是本发明实施例提供的校验方法的流程示意图，如图1所示。一种计校验方法，包括：

步骤101，通过雷达以及里程计周期性地同步检测机器人的位姿，其中，对应于同一时段的点云数据和里程计数据构成与该时段对应的检测帧。

步骤102，针对每个时段，根据当前检测帧的里程计数据和上一检测帧的里程计数据，计算所述机器人从所述上一检测帧到所述当前检测帧的位姿变化，得到第一位姿变化信息，并根据所述当前检测帧的点云数据和所述上一检测帧的点云数据，计算所述机器人从所述上一检测帧到所述当前检测帧的位姿变化，得到第二位姿变化信息。

启动配置有激光设备和里程计设备的机器人，并不断给该机器人发送速度，使其运动，那么就可以收到由激光设备发送连续多帧的点云数据和由里程计设备发送的里程计数据。

比如，当第一帧数据到来时，记录该帧的点云数据(或称“点云”)P1和里程计数据01；当第二帧数据到来时，记录该帧的点云数据P2和里程计数据02。

其中，步骤102是采用由雷达设备和里程计设备发送的数据帧中的任意相邻两帧(即上一帧和当前帧)的数据进行计算。

其中，位姿是指机器人的位置和姿态，位置表示机器人相对世界坐标的位置(平移)，一般用坐标(x，y)表示，姿态表示机器人的偏航角即机器人实际前进方向与期望前进方向之间的偏差角度，可选用Φ表示。由此，位姿对应的即为三维空间可用(x，y，Φ)表示。

示例性地，上一帧的上一检测帧的里程计数据表示为(x1，y1，Φ1)，当前帧的当前检测帧的里程计数据表示为(x2，y2，Φ2)，那么对应的第一位姿变化信息ΔTO_1＝(x2-x1，y2-y1，Φ2-Φ1)，表示机器人从里程计的角度认为自己从上一帧到当前帧改变的位姿。

示例性地，上一帧的上一检测帧的点云数据表示为(x3，y3，Φ3)，当前帧的当前检测帧的点云数据表示为(x4，y4，Φ4)，那么对应的第二位姿变化信息ΔTL_1＝(x4-x3，y4-y3，Φ4-Φ3)，表示机器人从激光的角度认为自己从上一帧到当前帧改变的位姿。

步骤103，将所述第二位姿变化信息与所述第一位姿变化信息的差值与预设阈值进行比较，并根据比较结果对所述里程计进行校验。

示例性的，所述第二位姿变化信息与所述第一位姿变化信息的差值(ΔTL_1-ΔTO_1)与预设阈值进行比较。所述预设阈值可根据实际经验总结得到，在此不做限定。如果差值太大，可能这两帧之间的里程计的结果有误，后续需要将激光匹配结果(即第二位姿变化信息)来替代里程计结果(即第一位姿变化信息)。

示例性的，本发明对里程计进行校验的处理包括但不限于以下三种：第一种，将上一检测帧的里程计数据和第二位姿变化信息替换当前检测帧的里程计数据。第二种，将所述第二位姿变化信息与所述第一位姿变化信息的差值的累积误差值置零。第三种，将所述第二位姿变化信息与所述第一位姿变化信息的差值进行累积，得到累积误差值。

以下将结合附图对上述步骤102～103进行具体描述。

图2是本发明实施例提供的计算点云数据位姿变化的流程示意图，如图2所示。上述步骤102中，所述根据所述当前检测帧的点云数据和所述上一检测帧的点云数据，计算所述机器人从所述上一检测帧到所述当前检测帧的位姿变化，得到第二位姿变化信息的步骤包括：

步骤201，将所述当前检测帧的点云数据与所述上一检测帧的点云数据进行匹配，得到两帧之间的位姿变化关系的匹配结果。

示例性地，可使用PL-ICP算法对所述当前检测帧的点云数据与所述上一检测帧的点云数据进行匹配处理，得到所述两帧之间的位姿变化关系的匹配结果。

步骤202，对所述匹配结果进行协方差计算。

步骤203，若计算出的协方差的值小于协方差预设阈值，则计算所述机器人从所述上一检测帧到所述当前检测帧的位姿变化，以得到所述第二位姿变化信息，否则，不计算当前时段对应的第二位姿变化信息并进入针对下一时段的处理过程。

以下对上述步骤201进行具体描述。

上述步骤201得到匹配结果的步骤如图3所示，图3是本发明实施例提供的PL-ICP算法的流程示意图。上述步骤201中，使用PL-ICP算法将所述当前检测帧的点云数据与所述上一检测帧的点云数据进行匹配，得到两帧之间的位姿变化关系的匹配结果的步骤包括：

步骤301，将需要比较的两帧点云数据(即上一检测帧的点云数据和当前检测帧的点云数据)提取出来。

步骤302，对所述两帧点云数据的合法性进行验证。

比如，包括对所述两帧点云数据中的有效数据的比例进行验证。

步骤303，将超出预设区域的点云数据设为无效数据，即valid＝0，并输出每帧点云数据中有效数据的比例。

步骤304，通过PL-ICP算法获取每个点云数据附近大小值的相对位置，从而寻找出所述两帧点云数据的配对关系。

步骤305，计算每个点云数据在笛卡尔坐标系中的位置。

步骤306，基于计算出每个点云数据的位置，并对其进行聚类处理。

可选地，将聚类处理后的一些孤立点云(表示其角度不合理)删除。

步骤307，对所述两帧点云数据进行位姿的迭代优化处理。

步骤308，输出所述两帧点云数据的匹配结果。

以下对上述步骤307进行具体描述。

示例性的，该步骤中所述两帧点云数据进行位姿的迭代优化处理流程如图4所示，图4是图3的进行位姿的迭代优化步骤的流程示意图。上述步骤307中，对所述两帧点云数据进行位姿的迭代优化处理的步骤包括：

步骤401，将所述两帧点云数据转换到参考坐标系下。

可选地，所述参考坐标系是上一帧的点云数据的坐标系，由于上述步骤304已经寻找出所述两帧点云数据的配对关系，所以通过预设公式可计算出所述两帧点云数据的位姿变换关系。根据计算出的所述两帧点云数据的位姿变换关系，将当前帧的点云数据变换到第一帧点云数据的坐标系。最好的情况下，转换后的两帧点云数据重合。但实际上，一般不会计算一次就能得到最好的解，所以需要继续寻找配对关系，即循环执行下述步骤402～403，以便循环一定次数继续进行位姿变换。

步骤402，查找所述两帧点云数据中配对关系。

步骤403，计算查找到的对应点个数。

当需要继续进行下一组(即之后的相邻两帧点云数据)估计位姿，则循环执行上述步骤401～403。

需要说明的是，上述图3和图4中涉及的PL-ICP算法流程，由于激光点云存在跳动的情况，比如激光打到一面墙上，其点云并不是呈现理想化的直线，所以本发明没有使用传统PL-ICP中点到最近两个点的连线的距离作为误差项，而是将连线的两个点的距离还需满足预设条件作为误差项，这样得到的直线更加贴合实际情况，如图5～图6所示。

图5是本发明实施例提供的单帧PL-ICP结果的示意图。图5示出了上一帧的点云数据pre_scan(用蓝色表示)、当前帧的点云数据curr_scan(用红色表示)以及由上一帧的点云数据经过计算出的匹配结果转换而成的结果match_result(即转换后的上一帧的点云数据，用黄色表示)。如果转换后的上一帧的点云数据(黄色)与上一帧的点云数据(红色)的重合度越高，则表示匹配得越好。

图6是本发明实施例提供的点到线的匹配关系的示意图。将当前帧的每个点云数据curr_scan(用红色表示)连接到两个匹配到转换后的上一帧的点云数据match_result(用黄色表示)，即表示当前帧的点云数据(红色)匹配到这两个黄色点云(即转换后的上一帧的点云数据)连成的线上。连线外侧的离散点是表示上一帧的点云数据pre_scan(用蓝色表示)。

综上所述，通过PL-ICP算法输出的信息包括：(1)匹配结果，表示相邻两帧间的位姿变换关系；(2)残差信息，表示经过匹配处理之后，相邻两帧点云不符合的程度；(3)协方差信息，表示周围环境相关，杂乱和相似度高的环境，其协方差信息的值也会高，则认为PL-ICP算法得到的结果出错的可能性高。

因此，如上述步骤203所述，选择所述协方差的值小于协方差预设阈值的匹配结果作为所述第二位姿变化信息，因为满足该条件下的匹配结果才是可靠的，然后根据误差的大小(即所述第二位姿变换信息与所述第一位姿变换信息的差值与预设阈值进行比较的结果)选择如下三种对里程计的校验操作之一：替换、置零或累积。

图7是本发明实施例提供的在不分析动态环境下对里程计进行校验步骤的流程示意图，如图7所示。上述步骤103中，所述将所述第二位姿变化信息与所述第一位姿变化信息的差值与预设阈值进行比较，并根据比较结果对所述里程计进行校验的步骤包括：

步骤701，判断所述第二位姿变化信息与所述第一位姿变化信息的差值是否大于第一预设阈值，如果是，则执行步骤702。

步骤702，用所述第二位姿变化信息替换所述第一位姿变化信息，以对所述机器人进行定位，否则执行步骤703。

示例性地，如果步骤701中的所述第二位姿变化信息与所述第一位姿变化信息的差值是大于第一预设阈值K1，则认为该帧中的里程计误差异常，则用所述第二位姿变化信息替换所述第一位姿变化信息以便进行后续的定位工作。

步骤703，继续判断所述第二位姿变换信息与所述第一位姿变换信息的差值是否小于第二预设阈值，如果是，则执行步骤704。

步骤704，将所述第二位姿变换信息与所述第一位姿变换信息的差值的累积误差值置零，并使用所述第一位姿变化信息对所述机器人进行定位，否则执行步骤705。

示例性地，如果步骤703中的述第二位姿变化信息与所述第一位姿变化信息的差值小于第二预设阈值K2，则将步骤704中的所述第二位姿变化信息与所述第一位姿变化信息的差值置零，认为所述差值是可接受的误差范围，所以将其置零。

步骤705，将所述第二位姿变换信息与所述第一位姿变换信息的差值计入所述累积误差值，并使用所述第一位姿变化信息对所述机器人进行定位。

示例性地，如果所述第二位姿变化信息与所述第一位姿变化信息的差值介于所述第一预设阈值K1和所述第二预设阈值K2之间(即所述第二位姿变化信息与所述第一位姿变化信息的差值大于或等于所述第二预设阈值并小于或等于所述第一预设阈值)，则继续正常使用里程计的计算结果(即第一位姿变化信息)，即使用所述第一位姿变化信息对所述机器人进行定位，但是会记录该次的差值，后续会对所述差值进行累积，所述累积误差值表示位移的累积误差值。

其中，上述步骤103中的所述预设阈值包括步骤701中的所述第一预设阈值K1和步骤703中的所述第二预设阈值K2。所述第一预设阈值K1和所述第二预设阈值K2根据实际需求而设定，在此不做限定。

图8是本发明实施例提供的判断里程计出错的流程示意图，如图8所示。上述步骤103中，所述将所述第二位姿变化信息与所述第一位姿变化信息的差值与预设阈值进行比较，并根据比较结果对所述里程计进行校验的步骤还包括：

步骤801，判断所述机器人是否继续移动了预设距离，如果是则执行步骤802。

步骤802，判断所述累积误差值是否大于累积误差预设阈值，如果是，则执行步骤803，否则返回步骤801。

如果所述累积误差值小于或等于累积误差预设阈值时，则机器人可正常行驶，程序不会上报里程计出问题。

步骤803，提示用户里程计出错，并控制所述机器人停止运动。

示例性的，当所述累积误差值大于累积误差预设阈值时，则认为里程计出现问题，立即停止机器人的运动，并向使用者发出告警信息。

需要说明的时，图8示出的上报里程计错误是在正常环境下进行的，如果遇到特殊环境，比如大人流环境，由于动态点云数据太多，可能会影响程序上报里程计出错的结果；再比如长走廊等高度相似环境，由于相似度太高，PL-ICP算法的计算结果会认为机器人没有移动。因此这些特殊环境下的PL-ICP算法的计算结果可能会影响程序上报里程计出错的结果，造成误报，所以还需对机器人是否处于动态环境进行检测，如图9所示。

图9是本发明实施例提供的判断动态环境的流程示意图。上述步骤103中，所述将所述第二位姿变化信息与所述第一位姿变化信息的差值与预设阈值进行比较，并根据比较结果对所述里程计进行校验的步骤还包括：

步骤901，持续记录预设时间周期内的点云数据，所述点云数据包括动态点云和静态点云。

示例性地，比如所述预设时间周期是15秒。如果累积点云数据的时间未达到15秒，则继续获取点云数据，并将获取到的点云数据记录在数据队列中。

步骤902，针对每一预设时间周期，判断该周期内的每一帧点云数据中的动态点云的总数是否超过动态点云预设阈值，如果是，则执行步骤903，否则返回步骤901。

示例性地，可通过temporal filter(时间滤波器)方法在里程计坐标系下区分动态点云和静态点云。通过temporal filter方法如何区分动态点云和静态点云，在此不做赘述。

示例性地，所述动态点云预设阈值是40％，如果某一帧中的点云数据中的动态点云数据占比大于40％，则认为该帧为动态帧。比如：某一帧中，点云数据的个数为100，如果动态点云数据的个数为50，则50％大于40％，则认为该帧是动态帧。

步骤903，将该帧确定为动态帧。

步骤904，判断该周期内的动态帧的总数是否超过动态帧预设阈值，如果是，则执行步骤905。

示例性地，所述动态帧预设阈值是85％，如果所述累积的数据帧中的动态帧的总数超过85％，则判定所述机器人当前处于动态环境。比如：累积的数据帧中，数据帧的个数为100，如果动态帧的个数为90，则90％大于85％，则认为是动态环境。

步骤905，判定所述机器人当前处于动态环境。

需要说明的是，上述步骤905中，所述判定机器人是否处于动态环境，用于修正上述步骤801因累积误差值大于累积误差预设阈值时因机器人处于动态环境可能会造成误报里程计出问题。

以下是对机器人若处于动态环境下，则不需要上报里程计错误以免造成误报的情况进行描述。

图10是本发明实施例提供的在动态环境下对累积处理的流程示意图，如图10所示。上述步骤103中，所述将所述第二姿变换值与所述第一位姿变换值的差值与预设阈值进行比较，并根据比较结果对里程计进行校验以实现对机器人的定位的步骤还包括：

步骤1001，根据上述步骤905中，若判定所述机器人当前处于动态环境，则将所述累积误差值置零。

步骤1002，在第一预设时段内停止对所述第二位姿变换信息与所述第一位姿变换信息的差值的累积，以及在所述第一预设时段之后重新启动对所述第二位姿变换信息与所述第一位姿变换信息的差值的累积。

上述步骤1001和步骤1002的目的是避免在动态环境中产生误报里程计错误的情形。

图11是本发明实施例提供的在动态环境下对里程计进行校验步骤的流程示意图，如图11所示。上述步骤103中，所述将所述第二位姿变化信息与所述第一位姿变化信息的差值与预设阈值进行比较，并根据比较结果对所述里程计进行校验的步骤还包括：

步骤1101，在上述步骤1002的所述第一预设时段之后。

步骤1102，判断所述累积误差值是否大于所述累积误差预设阈值，如果是，则执行步骤1103和步骤1104。

步骤1103，判断在当前时间之前的第三预设时段内的单帧的动态点云数据的总数是否超过所述动态点云预设阈值。

如果在当前时间之前的第三预设时段内的单帧的动态点云数据的总数不超过所述动态点云预设阈值，则执行步骤1105，否则执行步骤1106。

需要说明的是，在该步骤中，即使检测到所述累积误差值大于所述累积误差预设阈值，但程序先不上报里程计错误，需要查询在当前时间之前的第三预设时段内(比如前15秒内)有没有单帧的动态点特别多，即当前时间之前的第三预设时段内(比如前15秒内)的单帧的动态点云数据的总数超过所述动态点云预设阈值，如果不超过，才认为结果是可靠的，才会发出提示里程计出错。也就是说在需要查询当前时间之前的第三预设时段内的单帧的动态点云数据不超过动态点云预设阈值才会上报里程计错误。

步骤1104，判断在随后的第二预设时段内机器人是否处于动态环境的判定结果。

如果在随后的第二预设时段内获得当前不处于动态环境的判定结果，则执行步骤1105，否则执行步骤1106。

需要说明的是，该步骤中，即使检测到所述累积误差值大于所述累积误差预设阈值，但程序不会像上述步骤801那样直接上报，而是先不上报里程计错误，需要再等待第二预设时段(比如15秒)内检测机器人是否处于动态环境的判定结果，如果在随后的所述第二预设时段内获得当前不处于动态环境的判定结果，才认为结果是可靠的，才会发出提示里程计出错。也就是说在需要滞后第二预设时段(比如15秒)才会上报里程计错误。

步骤1105，提示用户里程计出错并控制所述机器人停止运动。

步骤1106，不会提示用户里程计出错。

之所以要查询上述步骤1103所述的当前时间之前的第三预设时段内的单帧的动态点云数据情况，才确定是否上报里程计错误，是因为从temporal filter的原理上讲，会把几乎所有的点判断为动态点，所以里程计出错则一定会有某些帧的动态点特别多。

以下通过图12说明上述步骤1103和步骤1104的情况。

图12是本发明实施例提供的动态环境判定结果和累积误差值的示意图，如图12所示。图12中的红色线示出了每帧的动态点数量的比重，黑色线示出了前15秒，动态帧(即动态点数超过40％的帧)占的比重。图12中的蓝色线示出了累积误差值。

如图12示出的在250秒附近，表示里程计出错，可能是人为拖动正在运行中的机器人导致的。图12中的红色的线突然上去了，是因为里程计认为自己没动，但是图12中示出的所有点都在动。而黑色线示出的累积的动态帧比重不高(一般达到0.8才算高，图12示出的是0.4左右)，说明此次里程计出错不是由动态环境引起的。所以经过查询当前时间之前的15秒内的单帧的动态点云数据的总数是否超过所述动态点云预设阈值，如果不超过，才认为上报里程计错误的结果是可靠的，所以才会发出提示用户里程计出错。因此，此次上报里程计错误是一次成功的告警，不会造成误报。

综上所述，本发明所述的用于机器人的校验方法，通过结合对动态环境的检测，使得机器人在大人流处运行时，程序不会误报里程计错误，并且也使得机器人在人为拖动正在运行中的机器人或长走廊等高度相似环境中运行时，程序也不会误报里程计错误。

比如，将机器人开到光滑地面或其他易打滑处，机器人的车轮开始打滑，一定时间后程序才会上报里程计错误。人为拖动正在运行中的机器人，也是一定时间后程序才会上报里程计错误。换用有错误的轮式编码器或者IMU等里程计数据源，也是一定时间后程序才会上报里程计错误。

图13是本发明一实施例提供的计校验方法的流程示意图，如图13所示。所述校验方法，包括以下步骤：

步骤1301，启动里程计的校验程序。

步骤1302，获取上一检测帧的里程计数据和当前检测帧的里程计数据。

步骤1303，获取上一检测帧的点云数据和当前检测帧的点云数据。

步骤1304，将步骤1303的上一检测帧的点云数据和当前检测帧的点云数据进行匹配，得到匹配结果，并将所述匹配结果进行协方差计算，判断计算出的协方差的值是否小于协方差预设阈值。如果是，则执行步骤1306，否则返回步骤1303。

步骤1305，计算从所述上一帧到所述当前帧的位姿变换，得到第一位姿变化信息ΔTO_1。

其中，所述第一位姿变化信息ΔTO_1是机器人从里程计的角度认为的自己从上一帧到当前帧改变的位姿。

步骤1306，将所述匹配结果作为所述第二位姿变化信息ΔTL_1。

其中，所述第二位姿变化信息ΔTL_1是机器人从激光的角度认为的自己从上一帧到当前帧改变的位姿。

步骤1307，判断所述第二位姿变化信息ΔTL_1与所述第一位姿变化信息ΔTO_1的差值是否大于第一预设阈值。如果是，则执行步骤1308，否则执行步骤1309。

步骤1308，用所述第二位姿变化信息替换所述第一位姿变化信息，以对所述机器人进行定位。

步骤1309，判断所述第二位姿变化信息与所述第一位姿变化信息的差值是否小于第二预设阈值。如果是，则执行步骤1310，否则执行步骤1311。

步骤1310，将所述第二位姿变换信息与所述第一位姿变换信息的差值的累积误差值置零，并使用所述第一位姿变化信息对所述机器人进行定位。

步骤1311，将所述第二位姿变换信息与所述第一位姿变换信息的差值计入所述累积误差值，并使用所述第一位姿变化信息对所述机器人进行定位。

步骤1312，判断所述累积误差值是否大于累积误差预设阈值。如果是，则执行步骤1313和步骤1314。

步骤1313，判断在当前时间之前的第三预设时段内的单帧的动态点云数据的总数是否超过动态点云预设阈值。

如果在当前时间之前的第三预设时段内的单帧的动态点云数据的总数是超过动态点云预设阈值，则执行步骤1316；否则执行步骤1315。

步骤1314，判断在随后的第二预设时段内获得机器人当前是否处于动态环境的判定结果。

如果在随后的第二预设时段内获得机器人当前是处于动态环境的判定结果，则执行步骤1316，否则执行步骤1315。

步骤1315，提示用户里程计出错并控制所述机器人停止运动。

步骤1316，不会提示用户里程计出错。

上述图13给出的示例目的在于在判断出所述累积误差值大于所述累积误差预设阈值时，还需要对动态点云的总数是否超过动态点云阈值的结果以及机器人当前是否处于动态环境的结果进行分析，并根据分析的结果才决定是否需要上报里程计出错的告警。

下面对本发明提供的用于机器人的里程计校验装置进行描述，下文描述的用于机器人的里程计校验装置与上文描述的用于机器人的校验方法可相互对应参照。

图14是本发明实施例提供的校验装置的结构示意图，如图14所示。一种计校验装置1400，包括数据获取模块1410、位姿变化计算模块1420以及里程计校验模块1430，其中，

数据获取模块1410，用于通过雷达以及里程计周期性地同步检测机器人的位姿，其中，对应于同一时段的点云数据和里程计数据构成与该时段对应的检测帧。

位姿变化计算模块1420，用于针对每个时段，根据当前检测帧的里程计数据和上一检测帧的里程计数据，计算所述机器人从所述上一检测帧到所述当前检测帧的位姿变化，得到第一位姿变化信息，并根据所述当前检测帧的点云数据和所述上一检测帧的点云数据，计算所述机器人从所述上一检测帧到所述当前检测帧的位姿变化，得到第二位姿变化信息。

里程计校验模块1430，用于将所述第二位姿变化信息与所述第一位姿变化信息的差值与预设阈值进行比较，并根据比较结果对所述里程计进行校验。

可选地，所述位姿变化计算模块1420，还用于：

将所述当前检测帧的点云数据与所述上一检测帧的点云数据进行匹配，得到两帧之间的位姿变化关系的匹配结果；

对所述匹配结果进行协方差计算；

若计算出的协方差的值小于协方差预设阈值，则计算所述机器人从所述上一检测帧到所述当前检测帧的位姿变化，以得到所述第二位姿变化信息，否则，不计算当前时段对应的第二位姿变化信息并进入针对下一时段的处理过程。

可选地，所述里程计校验模块1430，还用于：

判断所述第二位姿变化信息与所述第一位姿变化信息的差值是否大于第一预设阈值；

如果所述第二位姿变化信息与所述第一位姿变化信息的差值大于第一预设阈值，则用所述第二位姿变化信息替换所述第一位姿变化信息，以对所述机器人进行定位，否则继续判断所述第二位姿变换信息与所述第一位姿变换信息的差值是否小于第二预设阈值；

如果所述第二位姿变换信息与所述第一位姿变换信息的差值小于第二预设阈值，则将所述第二位姿变换信息与所述第一位姿变换信息的差值的累积误差值置零，并使用所述第一位姿变化信息对所述机器人进行定位，否则将所述第二位姿变换信息与所述第一位姿变换信息的差值计入所述累积误差值，并使用所述第一位姿变化信息对所述机器人进行定位；

其中，所述预设阈值包括所述第一预设阈值和所述第二预设阈值。

可选地，所述里程计校验模块1430，还用于：

当所述机器人继续移动预设距离后，若所述累积误差值大于累积误差预设阈值，则提示用户里程计出错，并控制所述机器人停止运动。

可选地，所述里程计校验模块1430，还用于：

持续记录预设时间周期内的点云数据，所述点云数据包括动态点云和静态点云；

针对每一预设时间周期，判断该周期内的每一帧点云数据中的动态点云的总数是否超过动态点云预设阈值，如果该周期内的每一帧点云数据中的动态点云的总数超过动态点云预设阈值，则将该帧确定为动态帧；

判断该周期内的动态帧的总数是否超过动态帧预设阈值；

如果该周期内的动态帧的总数超过动态帧预设阈值，则判定所述机器人当前处于动态环境。

可选地，所述里程计校验模块1430，还用于：

若判定所述机器人当前处于动态环境，则将所述累积误差值置零，并在第一预设时段内停止对所述第二位姿变换信息与所述第一位姿变换信息的差值的累积，以及在所述第一预设时段之后重新启动对所述第二位姿变换信息与所述第一位姿变换信息的差值的累积。

可选地，所述里程计校验模块1430，还用于：

在所述累积误差值大于所述累积误差预设阈值时，在满足下列第一、第二辅助条件中的至少一个的情况下提示用户里程计出错并控制所述机器人停止运动：

第一辅助条件：在随后的第二预设时段内获得当前不处于动态环境的判定结果；以及

第二辅助条件：在当前时间之前的第三预设时段内的单帧的动态点云数据的总数不超过所述动态点云预设阈值。

综上所述，本发明能够实现使用2D激光校验里程计正确性的功能，并且考虑到了全场景应用中可能会遇到的一些问题，使本发明所述用于机器人的里程计校准方法适用性更广，鲁棒性更强。

图15是本发明实施例提供的一种电子设备的实体结构示意图，如图15所示，该电子设备可以包括：处理器(processor)1510、通信接口(Communications Interface)1520、存储器(memory)1530和通信总线1540，其中，处理器1510，通信接口1520，存储器1530通过通信总线1540完成相互间的通信。处理器1510可以调用存储器1530中的逻辑指令，以执行上述所述校验方法，所述方法包括：

通过雷达以及里程计周期性地同步检测机器人的位姿，其中，对应于同一时段的点云数据和里程计数据构成与该时段对应的检测帧；

针对每个时段，根据当前检测帧的里程计数据和上一检测帧的里程计数据，计算所述机器人从所述上一检测帧到所述当前检测帧的位姿变化，得到第一位姿变化信息，并根据所述当前检测帧的点云数据和所述上一检测帧的点云数据，计算所述机器人从所述上一检测帧到所述当前检测帧的位姿变化，得到第二位姿变化信息；

将所述第二位姿变化信息与所述第一位姿变化信息的差值与预设阈值进行比较，并根据比较结果对所述里程计进行校验。

此外，上述的存储器1530中的逻辑指令可以通过软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。

另一方面，本发明还提供一种计算机程序产品，所述计算机程序产品包括存储在非暂态计算机可读存储介质上的计算机程序，所述计算机程序包括程序指令，当所述程序指令被计算机执行时，计算机能够执行上述各方法所提供的上述所述校验方法。

又一方面，本发明还提供一种非暂态计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时实现以执行上述各提供的上述所述校验方法。

图16是本发明实施例提供的芯片的结构示意图，如图16所示。芯片1600包括一个或多个处理器1601以及接口电路1602。

可选地，芯片1600还可以包含总线1603。其中，处理器1601可能是一种集成电路芯片，具有信号的处理能力。

在实现过程中，上述方法的各步骤可以通过处理器1601中的硬件的集成逻辑电路或者软件形式的指令完成。上述的处理器1601可以是通用处理器、数字通信器(DSP)、专用集成电路(ASIC)、现场可编程门阵列(FPGA)或者其它可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件、MCU、MPU、CPU或者协处理器中的一个或多个。可以实现或者执行本申请实施例中的公开的各方法、步骤。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。接口电路1602可以用于数据、指令或者信息的发送或者接收，处理器1601可以利用接口电路1602接收的数据、指令或者其它信息，进行加工，可以将加工完成信息通过接口电路1602发送出去。

可选地，芯片还包括存储器，存储器可以包括只读存储器和随机存取存储器，并向处理器提供操作指令和数据。存储器的一部分还可以包括非易失性随机存取存储器(NVRAM)。

可选地，存储器存储了可执行软件模块或者数据结构，处理器可以通过调用存储器存储的操作指令(该操作指令可存储在操作系统中)，执行相应的操作。

可选地，芯片可以使用在本申请实施例涉及的目标检测装置中。可选的，接口电路1602可用于输出处理器1601的执行结果。

关于本申请的一个或多个实施例提供的校验方法可参考前述各个实施例，这里不再赘述。

需要说明的，处理器1601、接口电路1602各自对应的功能既可以通过硬件设计实现，也可以通过软件设计来实现，还可以通过软硬件结合的方式来实现，这里不作限制。

以上所描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。本领域普通技术人员在不付出创造性的劳动的情况下，即可以理解并实施。

通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到各实施方式可借助软件加必需的通用硬件平台的方式来实现，当然也可以通过硬件。基于这样的理解，上述技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品可以存储在计算机可读存储介质中，如ROM/RAM、磁碟、光盘等，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行各个实施例或者实施例的某些部分所述的方法。

最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (10)

1.一种校验方法，其特征在于，包括：

通过雷达以及里程计周期性地同步检测机器人的位姿，其中，对应于同一时段的点云数据和里程计数据构成与该时段对应的检测帧；

针对每个时段，根据当前检测帧的里程计数据和上一检测帧的里程计数据，计算所述机器人从所述上一检测帧到所述当前检测帧的位姿变化，得到第一位姿变化信息，并根据所述当前检测帧的点云数据和所述上一检测帧的点云数据，计算所述机器人从所述上一检测帧到所述当前检测帧的位姿变化，得到第二位姿变化信息；

将所述第二位姿变化信息与所述第一位姿变化信息的差值与预设阈值进行比较，并根据比较结果对所述里程计进行校验，其包括：

若所述第二位姿变化信息与所述第一位姿变化信息的差值大于第一预设阈值，则用所述第二位姿变化信息替换所述第一位姿变化信息，以对所述机器人进行定位；

若所述第二位姿变化信息与所述第一位姿变化信息的差值小于第二预设阈值，则将所述第二位姿变换信息与所述第一位姿变换信息的差值的累积误差值置零，并使用所述第一位姿变化信息对所述机器人进行定位；

若所述第二位姿变化信息与所述第一位姿变化信息的差值大于或等于所述第二预设阈值并小于或等于所述第一预设阈值，将所述第二位姿变换信息与所述第一位姿变换信息的差值计入所述累积误差值，并使用所述第一位姿变化信息对所述机器人进行定位。

2.根据权利要求1所述的校验方法，其特征在于，所述根据所述当前检测帧的点云数据和所述上一检测帧的点云数据，计算所述机器人从所述上一检测帧到所述当前检测帧的位姿变化，得到第二位姿变化信息的步骤包括：

将所述当前检测帧的点云数据与所述上一检测帧的点云数据进行匹配，得到两帧之间的位姿变化关系的匹配结果；

对所述匹配结果进行协方差计算；

若计算出的协方差的值小于协方差预设阈值，则计算所述机器人从所述上一检测帧到所述当前检测帧的位姿变化，以得到所述第二位姿变化信息。

3.根据权利要求1所述的校验方法，其特征在于，所述方法还包括：

若所述累积误差值大于累积误差预设阈值，则提示用户里程计出错，并控制所述机器人停止运动。

4.根据权利要求1所述的校验方法，其特征在于：所述方法还包括：

持续记录预设时间周期内的点云数据，所述点云数据包括动态点云和静态点云；

针对每一预设时间周期，判断该周期内的每一帧点云数据中的动态点云的总数是否超过动态点云预设阈值；

若该周期内的某一帧点云数据中的动态点云的总数超过动态点云预设阈值，则将该帧确定为动态帧；

判断该周期内的动态帧的总数是否超过动态帧预设阈值；

若该周期内的动态帧的总数超过动态帧预设阈值，则判定所述机器人当前处于动态环境。

5.根据权利要求4所述的校验方法，其特征在于，所述方法还包括：

若判定所述机器人当前处于动态环境，则将所述累积误差值置零，并在第一预设时段内停止对所述第二位姿变换信息与所述第一位姿变换信息的差值的累积，以及在所述第一预设时段之后重新启动对所述第二位姿变换信息与所述第一位姿变换信息的差值的累积。

6.根据权利要求5所述的校验方法，其特征在于，所述方法还包括：在所述累积误差值大于所述累积误差预设阈值时，在满足下列第一、第二辅助条件中的至少一个的情况下提示用户里程计出错并控制所述机器人停止运动：

第一辅助条件：在随后的第二预设时段内获得当前不处于动态环境的判定结果；以及

第二辅助条件：在当前时间之前的第三预设时段内的单帧的动态点云数据的总数不超过所述动态点云预设阈值。

7.一种校准装置，其特征在于，包括：

数据获取模块，用于通过雷达以及里程计周期性地同步检测机器人的位姿，其中，对应于同一时段的点云数据和里程计数据构成与该时段对应的检测帧；

位姿变化计算模块，用于针对每个时段，根据当前检测帧的里程计数据和上一检测帧的里程计数据，计算所述机器人从所述上一检测帧到所述当前检测帧的位姿变化，得到第一位姿变化信息，并根据所述当前检测帧的点云数据和所述上一检测帧的点云数据，计算所述机器人从所述上一检测帧到所述当前检测帧的位姿变化，得到第二位姿变化信息；

里程计校验模块，用于将所述第二位姿变化信息与所述第一位姿变化信息的差值与预设阈值进行比较，并根据比较结果对所述里程计进行校验，其包括：

若所述第二位姿变化信息与所述第一位姿变化信息的差值大于第一预设阈值，则用所述第二位姿变化信息替换所述第一位姿变化信息，以对所述机器人进行定位；

若所述第二位姿变化信息与所述第一位姿变化信息的差值小于第二预设阈值，则将所述第二位姿变换信息与所述第一位姿变换信息的差值的累积误差值置零，并使用所述第一位姿变化信息对所述机器人进行定位；

若所述第二位姿变化信息与所述第一位姿变化信息的差值大于或等于所述第二预设阈值并小于或等于所述第一预设阈值，将所述第二位姿变换信息与所述第一位姿变换信息的差值计入所述累积误差值，并使用所述第一位姿变化信息对所述机器人进行定位。

8.一种电子设备，其特征在于，包括：

一个或多个处理器、存储器；和一个或多个程序，其中所述一个或多个程序被存储在所

述存储器中，并且被一个或多个所述处理器执行，所述程序包括用于执行权利要求1至6任一项所述的校验方法的步骤。

9.一种包含计算机程序的非易失性计算机可读存储介质，当所述计算机程序被一个或多个处理器执行时，使得所述处理器实现权利要求1至6任一项所述的校验方法的步骤。

10.一种芯片，其特征在于，包括至少一个处理器的接口；

所述接口，用于为所述至少一个处理器提供程序指令或者数据；

所述至少一个处理器用于执行所述程序性指令，以实现如权利要求1-6中任一项所述的校验方法。