一种足式机器人隧道三维测图方法及装置

技术领域

本申请涉及测量机器人技术领域，特别涉及一种足式机器人隧道三维测图方法及装置。

背景技术

在隧道建设和管理过程中，采用激光雷达传感器和移动激光扫描技术来获取环境三维空间坐标信息来构建三维点云地图。然而，在隧道建设和管理过程中，因施工动态干扰、衬砌结构粗糙、测量噪声严重等复杂的环境问题会增加三维点云地图构建带来了挑战。

基于激光雷达的SLAM（Simultaneous Localization and Mapping，同时定位与地图构建）方法是通过将每帧点云配准到全局坐标系来构建三维点云地图，现有的基于原始点云和基于特征点的匹配方法主要是应用在结构丰富、特征明显的场景。然而，在长距离和噪声污染严重的隧道环境中，会容易出现地图退化、测量精度低等问题。

因而现有技术还有待改进和提高。

发明内容

本申请要解决的技术问题在于，针对现有技术的不足，提供一种足式机器人隧道三维测图方法及装置。

为了解决上述技术问题，本申请第一方面提供了一种足式机器人隧道三维测图方法，其中，所述的足式机器人隧道三维测图方法具体包括：

通过激光雷达采集隧道的一帧环境点云数据，并获取每个采集时刻的IMU信息和所述足式机器人搭载的各电机的旋转角度；

将按照时间先后顺序位于第一位的采集时刻作为前序采集时刻，将前一帧环境点云数据对应的目标运动姿态作为所述前序采集时刻的前序运动状态，根据所述IMU信息、旋转角度以及前序运动状态，确定所述足式机器人的预测运动状态，以得到前序采集时刻对应的预测运动状态；将位于第二位的采集时刻作为当前采集时刻，将前序采集时刻对应的预测运动姿态作为所述当前采集时刻的前序运动状态，根据所述IMU信息、旋转角度以及前序运动状态，确定所述足式机器人的预测运动状态，以得到当前采集时刻对应的预测运动状态；足式机器人的预测运动状态基于所述预测运动状态将所述前序采集时刻的局部点云数据校正至所述当前采集时刻，以更新当前采集时刻对应的局部点云数据；将当前采集时刻作为前序采集时刻，将当前采集时刻的下一采集时刻作为当前采集时刻，并重复根据所述IMU信息、旋转角度以及前序运动状态，确定所述足式机器人的预测运动状态，以得到当前采集时刻对应的预测运动状态的步骤，直至结束采集时刻，以得到目标点云数据；

基于所述目标点云数据进行状态估计以得到所述足式机器人的目标运动状态，并基于所述目标运动状态和所述目标点云数据构建隧道三维测图。

所述的足式机器人隧道三维测图方法，其中，所述根据所述IMU信息、旋转角度以及前序运动状态，确定所述足式机器人的预测运动状态具体包括：

根据所述旋转角度确定所述足式机器人的每只腿在腿部坐标系下的空间位置；

根据确定的所有空间位置，计算所述足式机器人的推进速度；

将所述推进速度和所述IMU信息输入预设运动学模型，通过所述预设运动学模型来确定运动状态变化量；

基于所述运动状态变化量和所述前序运动状态，确定所述足式机器人的预测运动状态。

所述的足式机器人隧道三维测图方法，其中，在得到目标点云数据之后，所述方法还包括：

计算所述目标点云数据中的各目标点云的配准残差，并根据所述配准残差确定各目标点云的点云类型；

根据各目标点云的点云类型对各目标点云进行残差补偿，采用补偿后的配准残差对目标点云数据进行配准，并将配准后的目标点云数据作为目标点云数据。

所述的足式机器人隧道三维测图方法，其中，所述计算所述目标点云数据中的各目标点云的配准残差具体包括：

对于目标点云数据中的每个目标点云，在子地图中搜索所述目标点云的预设数量的相邻点，基于预设数量的相邻点构建邻域区域；

获取所述邻域区域的平面法向量和质心，并根据所述平面法向量和质心计算所述目标点云对应的配准残差。

所述的足式机器人隧道三维测图方法，其中，所述根据各目标点云的点云类型对各目标点云进行残差补偿，采用补偿后的配准残差对目标点云数据进行配准具体包括：

对于每个目标点云，基于所述目标点云的点云类型确定补偿方式；

根据各相邻点云的配准残差和所述目标点云的配准残差，按照所述补偿方式对所述目标点云进行配准残差进行补偿，并采用补偿后的配准残差对目标点云数据进行配准。

所述的足式机器人隧道三维测图方法，其中，在得到目标点云数据之后，所述方法还包括：

获取各目标点云的配准残差特征和测量距离特征，以得到各目标点云的异常特征；

以异常一致性指标为约束，采用二分搜索法求解所述目标点云数据对应的目标旋转角度，其中，所述异常一致性指标用于反映基于异常特征的初始异常点云序列与基于旋转后的异常特征确定的旋转异常点云序列的差异程度；

基于通过目标旋转角度旋转后的旋转异常特征，确定所述目标点云对应的目标旋转异常点云序列；

基于所述初始异常点云序列和所述目标旋转异常点云序列，确定所述目标点云数据对应的异常点云，确定的所有异常点云从所述目标点云中剔除。

所述的足式机器人隧道三维测图方法，其中，所述基于所述目标点云数据进行状态估计以得到所述足式机器人的目标运动状态具体为：

基于所述目标点云数据，通过误差状态卡尔曼滤波器确定所述足式机器人的目标运动状态。

本申请第二方面提供了一种足式机器人隧道三维测图装置，其中，所述的足式机器人隧道三维测图装置具体包括：

采集模块，用于通过激光雷达采集隧道的一帧环境点云数据，并获取每个采集时刻的IMU信息和所述足式机器人搭载的各电机的旋转角度；

执行模块，用于将按照时间先后顺序位于第一位的采集时刻作为当前采集时刻，将前一帧环境点云数据对应的目标运动姿态作为前序运动状态；根据所述IMU信息、旋转角度以及前序运动状态，确定所述足式机器人的预测运动状态，基于所述预测运动状态将所述当前采集时刻的局部点云数据校正所述环境点云数据的结束采集时刻；将当前采集时刻的预测运动状态作为前序运动状态，将当前采集时刻的采集时刻作为当前采集时刻，以及重复根据所述IMU信息、旋转角度以及前序运动状态，确定所述足式机器人的预测运动状态的步骤，直至结束采集时刻，以得到目标点云数据；

构建模块，用于基于所述目标点云数据进行状态估计以得到所述足式机器人的目标运动状态，并基于所述目标运动状态和所述目标点云数据构建隧道三维测图。

本申请第三方面提供了一种计算机可读存储介质，所述计算机可读存储介质存储有一个或者多个程序，所述一个或者多个程序可被一个或者多个处理器执行，以实现如上任一所述的足式机器人隧道三维测图方法中的步骤。

本申请第四方面提供了一种终端设备，其包括：处理器和存储器；

所述存储器上存储有可被所述处理器执行的计算机可读程序；

所述处理器执行所述计算机可读程序时实现如上任一所述的足式机器人隧道三维测图方法中的步骤。

有益效果：与现有技术相比，本申请提供了一种足式机器人隧道三维测图方法及装置，所述方法包括通过激光雷达采集隧道的一帧环境点云数据，并获取每个采集时刻的IMU信息和所述足式机器人搭载的各电机的旋转角度；根据所述IMU信息、旋转角度以及前序运动状态，确定所述足式机器人的预测运动状态；基于所述预测运动状态将各采集时刻采集到的局部点云数据校正所述环境点云数据的结束采集时刻，以得到目标点云数据；基于所述目标点云数据进行状态估计以得到所述足式机器人的目标运动状态，并基于所述目标运动状态和所述目标点云数据构建隧道三维测图。本申请通过结合电机的旋转角度和IMU信息来确定足式机器人的运动姿态，然后基于该运动姿态对环境点云数据进行运动畸变，以将每个点云都投影到点云帧的结束采集时刻，这样可以通过电机的旋转角度来提供稳定的运动约束，弥补了IMU传感器长时间工作的累积误差，从而可以消除点云的运动偏移量，提高了隧道三维测图的准确性。

附图说明

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

图1为本申请实施例提供的足式机器人隧道三维测图方法的流程图。

图2为本申请实施例提供的足式机器人隧道三维测图方法的一个具体实现方式的流程示意图。

图3为本申请实施例提供的足式机器人隧道三维测图装置的结构原理图。

图4为本申请实施例提供的终端设备的结构原理图。

具体实施方式

本申请实施例提供一种足式机器人隧道三维测图方法及装置，为使本申请的目的、技术方案及效果更加清楚、明确，以下参照附图并举实施例对本申请进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本申请，并不用于限定本申请。

本技术领域技术人员可以理解，除非特意声明，这里使用的单数形式“一”、“一个”、“所述”和“该”也可包括复数形式。应该进一步理解的是，本申请的说明书中使用的措辞“包括”是指存在所述特征、整数、步骤、操作、元件和/或组件，但是并不排除存在或添加一个或多个其他特征、整数、步骤、操作、元件、组件和/或它们的组。应该理解，当我们称元件被“连接”或“耦接”到另一元件时，它可以直接连接或耦接到其他元件，或者也可以存在中间元件。此外，这里使用的“连接”或“耦接”可以包括无线连接或无线耦接。这里使用的措辞“和/或”包括一个或更多个相关联的列出项的全部或任一单元和全部组合。

本技术领域技术人员可以理解，除非另外定义，这里使用的所有术语(包括技术术语和科学术语)，具有与本申请所属领域中的普通技术人员的一般理解相同的意义。还应该理解的是，诸如通用字典中定义的那些术语，应该被理解为具有与现有技术的上下文中的意义一致的意义，并且除非像这里一样被特定定义，否则不会用理想化或过于正式的含义来解释。

应理解，本实施例中各步骤的序号和大小并不意味着执行顺序的先后，各过程的执行顺序以其功能和内在逻辑确定，而不应对本申请实施例的实施过程构成任何限定。

经过研究发现，在隧道建设和管理过程中，隧道因其直径和长度均较大，而导致结构变形、坍塌和落石等安全风险不易排查。特别是，复杂的地质条件极大的增加了隧道环境精准测量的困难、成本和风险。现有隧道环境测量方法主要依靠人工监测，监测人员存在较大的工作强度和工作风险，监测数据的精准性受人工的主观性影响。

为此，机器人技术作为无人化环境监测的手段被应用在隧道建设和管理过程中，可以极大降低测量任务的时间和成本。然而，由于高尘密闭空间和崎岖障碍地形，无人机和无人车辆无法穿越全地形，相比而言，高运动性能的足式机器人具有躲避障碍和跨越地形优势，被广泛应用于隧道测量。

在应用足式机器进行隧道测量时，普遍是采用基于激光雷达的SLAM（Simultaneous Localization and Mapping，同时定位与地图构建）方法，通过将每帧点云配准到全局坐标系来构建三维点云地图。然而，在长距离和噪声污染（例如，施工车辆、工人和机器的动态干扰，以及渗水、灰尘和混凝土表面反射的绝对测量噪声等）严重的隧道环境中，采集到点云数据会带噪声的点云。带噪声的点云会影响当前点云帧与全局坐标系的位姿估计，长时间测量的累积误差会影响三维点云地图的准确性，进而影响测量精度低等问题。

为了解决上述问题，在本申请实施例中，通过激光雷达采集隧道的一帧环境点云数据，并获取每个采集时刻的IMU信息和所述足式机器人搭载的各电机的旋转角度；根据所述IMU信息、旋转角度以及前序运动状态，确定所述足式机器人的预测运动状态；基于所述预测运动状态将各采集时刻采集到的局部点云数据校正所述环境点云数据的结束采集时刻，以得到目标点云数据；基于所述目标点云数据进行状态估计以得到所述足式机器人的目标运动状态，并基于所述目标运动状态和所述目标点云数据构建隧道三维测图。本申请通过结合电机的旋转角度和IMU信息来确定足式机器人的运动姿态，然后基于该运动姿态对环境点云数据进行运动畸变，以将每个点云都投影到点云帧的结束采集时刻，这样可以通过电机的旋转角度来提供稳定的运动约束，弥补了IMU传感器长时间工作的累积误差，从而可以消除点云的运动偏移量，提高了隧道三维测图的准确性。

下面结合附图，通过对实施例的描述，对申请内容作进一步说明。

本实施例提供了一种足式机器人隧道三维测图方法，如图1所示，所述方法包括：

S10、通过激光雷达采集隧道的一帧环境点云数据，并获取每个采集时刻的IMU信息和所述足式机器人搭载的各电机的旋转角度。

具体地，足式机器人搭建有激光雷达、电机以及惯性传感器，激光雷达用于采集隧道中的环境点云数据，电机搭载于足式机器人的腿上，用于驱动足式机器人运动，惯性传感器用于采集IMU信息，IMU信息包括姿态信息和位置信息。

需要说明的是，激光雷达的采集频率小于惯性传感器的采集频率，以使得在激光雷达采集一帧环境点云数据的采集周期内，惯性传感器会采集到多个IMU信息。例如，激光雷达在0-9时刻采集到一帧环境点云数据，惯性传感器在0-9时刻采集到10个IMU信息。因此，当前采集时刻指的是在环境点云数据的采集周期内的一个采集时刻，在该采集时刻，激光雷达会采集到局部点云数据，惯性传感器会采集到一个IMU信息，足式机器人搭载的各电机处于一个旋转状态，从而可以获取到每个采集时刻的IMU信息、所述足式机器人搭载的各电机的旋转角度及局部点云数据。

S20、将按照时间先后顺序位于第一位的采集时刻作为前序采集时刻，将前一帧环境点云数据对应的目标运动姿态作为所述前序采集时刻的前序运动状态，根据所述IMU信息、旋转角度以及前序运动状态，确定所述足式机器人的预测运动状态，以得到前序采集时刻对应的预测运动状态；将位于第二位的采集时刻作为当前采集时刻，将前序采集时刻对应的预测运动姿态作为所述当前采集时刻的前序运动状态，根据所述IMU信息、旋转角度以及前序运动状态，确定所述足式机器人的预测运动状态，以得到当前采集时刻对应的预测运动状态；足式机器人的预测运动状态基于所述预测运动状态将所述前序采集时刻的局部点云数据校正至所述当前采集时刻，以更新当前采集时刻对应的局部点云数据；将当前采集时刻作为前序采集时刻，将当前采集时刻的下一采集时刻作为当前采集时刻，并重复根据所述IMU信息、旋转角度以及前序运动状态，确定所述足式机器人的预测运动状态，以得到当前采集时刻对应的预测运动状态的步骤，直至结束采集时刻，以得到目标点云数据。

具体地，每帧环境点云数据的采集周期内包括若干采集时刻，并且每个采集时刻均具有时间戳，从而可以按照时间先后顺序将各采集时刻进行排序以得到采集时刻序列。位于第一位的采集时刻对应先验知识为前一帧环境点云数据对应的目标运动姿态。也就是说，在对每帧环境点云数据而言，该环境点云数据的前一帧环境点云数据为其先验知识。此外，需要说明的是，对于第一帧环境点云数据中位于第一位的采集时刻而言，先验知识可以为0，也可以为其他预设的先验知识，这里不具体限制。

预测运动状态为基于IMU信息预测得到的运动状态，用于将当前采集时刻的局部点云数据变换至环境点云数据的当前采集时刻的下一采集时刻，其中，预测运动状态可以包括全局坐标系下的位置、姿态、速度和重量重力矢量，IMU坐标系下的姿态和位置，以及IMU偏置，相应的，预测运动状态可以表示为

在一个实现方式中，所述根据所述IMU信息、旋转角度以及前序运动状态，确定所述足式机器人的预测运动状态具体包括：

步骤S21、根据所述旋转角度确定所述足式机器人的每只腿在腿部坐标系下的空间位置；

步骤S22、根据确定的所有空间位置，计算所述足式机器人的推进速度；

步骤S23、将所述推进速度和所述IMU信息输入预设运动学模型，通过所述预设运动学模型来确定运动状态变化量；

步骤S24、基于所述运动状态变化量和所述前序运动状态，确定所述足式机器人的预测运动状态。

具体地，在步骤S21中，足式机器人的每条腿的至少搭建1台电机，例如，2台，3台，4台等。旋转角度包括足式机器人各腿上搭载的各电机的旋转角度。在本申请实施例中，以每条腿搭载3台电机为例进行说明，分别搭载于髋关节、大腿和小腿，那么在获取旋转角度时会获取到每条腿搭载的髋关节电机、大腿电机和小腿电机获取的旋转角度，即获取到12个旋转角度。

在获取到每条腿搭载的髋关节电机、大腿电机和小腿电机获取的旋转角度后，可以获取每条腿的髋部连杆长度、大腿连杆长度和小腿连杆长度，然后根据每条腿对应的髋部连杆长度、大腿连杆长度、小腿连杆长度以及髋关节电机、大腿电机和小腿电机获取的旋转角度，可以计算每条腿在腿部坐标系下的空间位置，其中，空间位置的计算公式可以为：

；

；

；

其中，

进一步，在步骤S22中，在获取到空间位置后，可以根据空间位置确定足式机器人在前序采集时刻和当前采集时刻的运动速度，其中，运动速度的计算公式可以为：

；

其中，

进一步，在获取到足式机器人在腿部坐标系下的运动速度后，由于从腿部坐标系到IMU坐标系属于刚体变换，即坐标系不会发生形变主要依靠旋转变换和平移变换相互转换，通过旋转矩阵

；

其中，

进一步，在步骤S23中，预设运动学模型为基于IMU信息和推进速度构建的，由于推进速度可以根据旋转角度确定，因此所述预设运动学模型为基于IMU信息和旋转角度构建的，通过利用腿部旋转角度来确定足部运动速度，可以为运动学模型提供稳定的速度信息，进而可以避免弥补IMU传感器长时间工作的累积误差。

在一个实现方式中，预设运动学模型可以表示为：

；

其中，

进一步，在步骤S24中，前序运动状态为预先获知的先验知识，其可以是预先设置的，也可以是根据前序运动状态对应的采集时刻的前一采集时刻的预测运动状态确定的，当前采集时刻的预测运动状态可以为前序运动状态与运动状态变化量的和，也可以是前序运动状态与运动状态变量加权得到的。

在本申请实施例中，前序运动状态为当前采集时刻的前一采集时刻的预测运动状态，当前采集时刻的预测运动状态可为前序运动状态与运动状态变化量的和。基于预设运动学模型可以预测当前采集时刻的预测运动状态及其协方差矩阵，其中，预测运动状态及其协方差矩阵可以分别表示为：

；

；

其中，

进一步，在获取到预测运动状态后，可以基于当前采集时刻的预测运动状态和当前采集时刻的前序运动状态确定变换关系，然后将前序采集时刻对应的局部点云数据中的每个点云均校正至当前采集时刻，以消除点云的运动偏移量。其中，基于前序运动状态和当前采集时刻的预测运动状态确定变换关系的确定过程可以采用现有方式，这里就不具体说明。

此外，在前序采集时刻对应的局部点云数据中的每个点云均校正至当前采集时刻后，再变换至全局坐标系内以得到变换后的局部点云数据，其中，变换后的局部点云数据可以表示为：

；

其中，

此外，需要说明的是，对于每帧环境点云数据中位于第一位的采集时刻，没有局部点云数据需要校正至该采集时刻，从而需要采用该采集时刻的运动状态作为其下一采集时刻的先验知识。因此，对于位于第一位的采集时刻时，可以将该采集时刻作为位于第二位的采集时刻的前序采集时刻，然后仅需要根据位于第一位的采集时刻获取到的IMU信息、旋转角度信息以及前一环境点云数据的目标运动状态来预测第一位的采集时刻的预测运动状态，然后将得到的预测运动状态作为位于第二位的采集时刻的先验知识。

对于除位于第一位的采集时刻后，均有需要校正至该采集时刻的局部点云数据，因此，在获取到除位于第一位的采集时刻外的各采集时刻的预测运动状态后，进而执行校正操作以将该采集时刻的前序采集时刻对应的局部点云数据校正至该采集时刻。具体地，对于相邻的两个采集时刻，将前一采集时刻作为前序采集时刻，将后一采集时刻作为当前采集时刻，然后基于前序采集时刻预测当前采集时刻的预测运动状态，再根据前序采集时刻对应的预测运动状态和当前采集时刻的预测运动状态构建变换关系，以将前一采集时刻对应的局部点云数据校正至后一采集时刻，其中，前一采集时刻对应的局部点云数据包括至校正至该采集时刻的点云数据。由此，依次累积变换，可以将各采集时刻采集到的局部点云数据均变换至结束采集时刻。

在一个实现方式中，在得到目标点云数据之后，如图2所示，所述方法还包括残差补偿过程，残差补偿过程识别隧道环境中粗糙结构面并补偿点云残差，从而提高粗糙结构点的配准精度。所述残差补偿过程具体包括：

H10、计算所述目标点云数据中的各目标点云的配准残差，并根据所述配准残差确定各目标点云的点云类型；

H20、根据各目标点云的点云类型对各目标点云进行残差补偿，采用补偿后的配准残差对目标点云数据进行配准，并将配准后的目标点云数据作为目标点云数据。

具体地，在步骤H10中，配准残差用于对目标点云的点云位置进行配准，点云类型用于反映目标点云所处的结构位置，其中，点云类型可以包括平面结构、劣角结构和曲面结构。

配准残差可以根据目标点云的相邻点确定的，也可以是根据目标点云和相邻点拟合形成的拟合平面的平面法向量和质心确定的等。在本申请实施例的一个具体实现方式中，所述计算所述目标点云数据中的各目标点云的配准残差具体包括：

对于目标点云数据中的每个目标点云，在子地图中搜索所述目标点云的预设数量的相邻点，基于预设数量的相邻点构建邻域区域；

获取所述邻域区域的平面法向量和质心，并根据所述平面法向量和质心计算所述目标点云对应的配准残差。

具体地，子地图为在当前采集时刻之前的所有已配准完成的环境点云数据构建的三维测量图，第一预设数量可以为预先设置的，例如，5，6，7等。例如，第一预设数量为5，可以通过最近邻域搜索方法在子地图中搜索到目标点云对应的5个第一相邻点。在获取到预设数量的相邻点后，将预设数量的相邻点构成的来得到邻域区域，并对邻域区域进行面法向量和质心提取，以得到邻域区域的平面法向量和质心，其中，邻域区域可以为利用最小二乘法对预设数量的相邻点进行区域拟合得到的。

进一步，在获取到邻域区域的平面法向量和质心后，可以根据邻域区域的平面法向量和质心计算目标点云对应的配准残差，其中，所述目标点云对应的配准残差可以为：

；

其中，

进一步，在计算得到每个目标点云的配准残差后，可以通过K均值聚类算法确定每个目标点云的点云类型，其具体过程可以为：

首先，根据点云类型的类型数量初始化类型数量个初始质心，并通过最小化残差值间的欧式距离将配准残差进行划分，以得到类型数量各聚类簇，最小化残差值间的欧式距离可以表示为：

；

其中，

其次，迭代更新各聚类簇的质心以得到最终聚类簇，每个最终聚类簇对应一个结构类型，该最终聚类簇中的每个目标点云的点云类型均为该最终聚类簇对应的结构类型，其中，在各最终聚类簇中，最小质心（

进一步，在步骤H20中，在确定各目标点云的点云类型后，可以根据各目标点云的点云类型确定更新方式，并采用更新方式对配准残差进行更新，根据更新后的配准残差各目标点云进行残差补充，也可以根据点云类型来确定需要补偿的目标点云，然后采用配准残差需要补偿的目标点云进行残差补偿等。

在一个实现方式中，所述根据各目标点云的点云类型对各目标点云进行残差补偿，采用补偿后的配准残差对目标点云数据进行配准具体包括：

对于每个目标点云，基于所述目标点云的点云类型确定补偿方式；

根据各相邻点云的配准残差和所述目标点云的配准残差，按照所述补偿方式对所述目标点云进行配准残差进行补偿，并采用补偿后的配准残差对目标点云数据进行配准。

具体地，补偿方式为根据点云类型确定的，点云类型和补偿方式的对应关系可以为：

；

其中，

需要说明的是，第二预设数量可以根据实际需求设置，例如，5,6,7等。第二相邻点云可以采用最近邻域搜索获取得到的，也可以是基于点云之间的欧式距离确定的等。

在本申请实施例的该实现方式中，通过对目标点云数据中的目标点云进行分类，然后保持平面结构的目标点云的配准残差不变，通过第二相邻点的配准残差来对劣角结构和曲面结构的目标点云进行残差补偿，可以提高劣角结构和曲面结构的配准残差的精确度。

在一些实现方式中，如图2所示，在对配准残差进行残差补偿后，还可以对目标点云进行异常点剔除，以减少三维点云地图的累积误差。具体地，所述异常点云剔除基于旋转引导策略，引入异常一致性指标作为约束来完成异常点云的检测和剔除，这样可以避免当孤立森林的分割线倾向于点云集中的地方时，会出现因点云的深度和异常程度而导致的误检测问题，进而可以提高异常点云剔除的准确性。

具体地，本实现方式通过将目标点云的异常特征进行平面旋转，并通过旋转前的异常特征和旋转后的异常特征构建旋转孤立森林，然后利用旋转前和旋转后的异常程度来确定初始异常点云序列和旋转异常点云序列，根据初始异常点云序列和旋转异常点云序列构建异常一致性指标，然后以异常一致性指标为约束确定目标点云数据对应的目标旋转角度，最后再基于异常特征和经过目标旋转角度的旋转异常特征确定目标点云数据中的异常点云并剔除。相应的，所述异常点剔除过程具体可以包括：

获取各目标点云的配准残差特征和测量距离特征，以得到各目标点云的异常特征；

以异常一致性指标为约束，采用二分搜索法求解所述目标点云数据对应的目标旋转角度，其中，所述异常一致性指标用于反映基于异常特征的初始异常点云序列与基于旋转后的异常特征确定的旋转异常点云序列的差异程度；

基于通过目标旋转角度旋转后的旋转异常特征，确定所述目标点云对应的目标旋转异常点云序列；

基于所述初始异常点云序列和所述目标旋转异常点云序列，确定所述目标点云数据对应的异常点云，确定的所有异常点云从所述目标点云中剔除。

具体地，配准残差特征为经过残差补充的配准残差，测量距离特征为目标点云的欧式测量距离，将配准残差特征和测量距离特征作为目标点云的两个异常特征，并利用标准化对配准残差特征和测量距离特征进行处理来得到异常特征，然后将异常特征进行旋转得到旋转后的异常特征，其中，异常特征和旋转后的异常特征可以表示为：

；

；

其中，

通过旋转处理后的依次特征训练孤立森林，目标点云

；

其中，

；

其中，

基于异常特征确定目标点云的旋转前异常程度，基于旋转后的异常特征确定目标点云的旋转后异常程度，然后根据旋转前异常程度构建初始异常点队列

。

异常一致性指标为正时，表明角度为

基于此，旋转角度

首先，基于水平和垂直切割方式，取

其次，初始化参数

然后，更新旋转角度，如

；

当

在获取到目标旋转角度后，将异常特征旋转目标旋转角度后确定目标旋转异常点云序列，然后基于所述初始异常点云序列和所述目标旋转异常点云序列，确定所述目标点云数据对应的异常点云检测，其中，异常点云可表示为：

。

S30、基于所述目标点云数据进行状态估计以得到所述足式机器人的目标运动状态，并基于所述目标运动状态和所述目标点云数据构建隧道三维测图。

具体地，目标点云数据为校正后的一帧环境点云数据，基于该目标点云数据可以确定足式机器人的目标运动状态，其中，足式机器人的目标运动状态可直接采用最后一个采集时刻的预测运动状态，也可以采用在任一采集时刻的预测运动状态。在本申请实施例中，为了提高目标运动状态的准确性，在获取到目标点云数据后，采用基于误差状态卡尔曼滤波器来确定目标点云数据对应的目标运动状态。基于此，所述基于所述目标点云数据进行状态估计以得到所述足式机器人的目标运动状态具体为：

基于所述目标点云数据，通过误差状态卡尔曼滤波器确定所述足式机器人的目标运动状态。

具体地，迭代观测点云数据，并利用最大后验估计方法计算误差状态

；

根据运动学模型的前序运动状态

；

其中，

观测模型的表达式在最后采集时刻的预测运动状态

；

其中，

；

；

其中，

；

迭代求解误差状态以补偿运动状态，作为下一次迭代的前序运动状态输入，可以得到：

；

；

当

；

；

由于三维点云地图是激光雷达扫描和位姿的集合，因此在时间段

。

综上所述，本实施例提供了一种足式机器人隧道三维测图方法，所述方法包括通过激光雷达采集隧道的一帧环境点云数据，并获取当前采集时刻的IMU信息、旋转角度及局部点云数据；根据IMU信息和旋转角度确定预测运动状态；基于预测运动状态将局部点云数据校正结束采集时刻，以此类推直至得到目标点云数据；基于所述目标点云数据进行状态估计以得到目标运动状态，并基于目标运动状态和目标点云数据构建隧道三维测图。本申请通过结合电机的旋转角度和IMU信息来确定足式机器人的运动姿态，然后基于该运动姿态对环境点云数据进行运动畸变，以将每个点云都投影到点云帧的结束采集时刻，从而消除点云的运动偏移量，提高了隧道三维测图的准确性。

基于上述足式机器人隧道三维测图方法，本实施例提供了一种足式机器人隧道三维测图装置，如图3所示，所述的足式机器人隧道三维测图装置具体包括：

采集模块100，用于通过激光雷达采集隧道的一帧环境点云数据，并获取每个采集时刻的IMU信息和所述足式机器人搭载的各电机的旋转角度；

执行模块200，用于将按照时间先后顺序位于第一位的采集时刻作为当前采集时刻，将前一帧环境点云数据对应的目标运动姿态作为前序运动状态；根据所述IMU信息、旋转角度以及前序运动状态，确定所述足式机器人的预测运动状态，基于所述预测运动状态将所述当前采集时刻的局部点云数据校正所述环境点云数据的结束采集时刻；将当前采集时刻的预测运动状态作为前序运动状态，将当前采集时刻的采集时刻作为当前采集时刻，以及重复根据所述IMU信息、旋转角度以及前序运动状态，确定所述足式机器人的预测运动状态的步骤，直至结束采集时刻，以得到目标点云数据；

构建模块300，用于基于所述目标点云数据进行状态估计以得到所述足式机器人的目标运动状态，并基于所述目标运动状态和所述目标点云数据构建隧道三维测图。

基于上述足式机器人隧道三维测图方法，本实施例提供了一种计算机可读存储介质，所述计算机可读存储介质存储有一个或者多个程序，所述一个或者多个程序可被一个或者多个处理器执行，以实现如上述实施例所述的足式机器人隧道三维测图方法中的步骤。

基于上述足式机器人隧道三维测图方法，本申请还提供了一种终端设备，如图4所示，其包括至少一个处理器（processor）20；显示屏21；以及存储器（memory）22，还可以包括通信接口（Communications Interface）23和总线24。其中，处理器20、显示屏21、存储器22和通信接口23可以通过总线24完成相互间的通信。显示屏21设置为显示初始设置模式中预设的用户引导界面。通信接口23可以传输信息。处理器20可以调用存储器22中的逻辑指令，以执行上述实施例中的方法。

此外，上述的存储器22中的逻辑指令可以通过软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。

存储器22作为一种计算机可读存储介质，可设置为存储软件程序、计算机可执行程序，如本公开实施例中的方法对应的程序指令或模块。处理器20通过运行存储在存储器22中的软件程序、指令或模块，从而执行功能应用以及数据处理，即实现上述实施例中的方法。

存储器22可包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需的应用程序；存储数据区可存储根据终端设备的使用所创建的数据等。此外，存储器22可以包括高速随机存取存储器，还可以包括非易失性存储器。例如，U盘、移动硬盘、只读存储器(Read-Only Memory，ROM)、随机存取存储器(Random Access Memory，RAM)、磁碟或者光盘等多种可以存储程序代码的介质，也可以是暂态存储介质。

此外，上述存储介质以及终端设备中的多条指令处理器加载并执行的具体过程在上述方法中已经详细说明，在这里就不再一一陈述。

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