CN111373336B - 状态感知方法以及相关设备 - Google Patents

Abstract

一种状态感知方法以及相关设备，该方法包括：采集当前帧点云数据(S101)；从历史点云库中抽取N个历史帧点云数据(S102)，该N个历史帧的采集时间均在该当前帧的采集时间之前，其中，N为大于或等于1的正整数；将该当前帧点云数据和该N个历史帧点云数据分别进行点云配准，获得该当前帧点云数据的N个状态估计值(S103)；将该N个状态估计值进行叠加，得到该当前帧点云数据的状态估计结果(S104)。该方法及设备能够实现在不需要回环运动的情况下也能过滤无规则噪音，得到较准确的状态感知结果，提高SLAM系统定位建图精度。

Description

状态感知方法以及相关设备

技术领域

本申请涉及定位技术领域，尤其涉及状态感知方法以及相关设备。

背景技术

现如今，随着传感器技术、AR/VR技术、智能移动机器人以及无人驾驶技术的发展，行业对定位技术的需求越来越多，其中，同时定位与建图(Simultaneous Localizationand Mapping，SLAM)技术就人们重点研究的一个方向。SLAM需要解决的问题可以描述为，自动驾驶汽车或其它自主移动装置在未知环境中从一个未知位置开始移动，在移动过程中根据位置估计和地图进行自身定位，同时在自身定位的基础上建造增量式地图，实现自动驾驶汽车或其它自主移动装置的自主定位和导航。

对于SLAM技术，误差积累是一大技术难题。SLAM通常使用卡尔曼滤波和回环检测的方法来减少误差累积。其中，卡尔曼滤波是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法，卡尔曼滤波中对系统状态是用概率表示，其所针对的误差是正态分布或其它线性概率分布的。回环检测又称闭环检测(Loop closuredetection)，当自动驾驶汽车或其它自主移动装置做回环运动时，即起始点和终止点基本一致，那么就可以对先前估计的路径进行贝叶斯概率图优化，使整个路径估计更加准确。

然而，在实际应用中，自动驾驶汽车或其它自主移动装置在不平整路面上会遭遇坑洼、杂物等，导致定位中出现无规则噪音，这些噪音高频、高能、杂乱无章、不属于正态分布，会极大影响SLAM对系统状态的估计。而卡尔曼滤波的使用要求误差是正态分布或其它线性概率分布，难以过滤无规则噪音。而对于回环检测，应用范围也有限，如果自动驾驶汽车或其它自主移动装置不进行回环运动，就没法使用回环检测来减少误差。由于没法消除误差，导致误差在迭代过程中不断地积累放大，严重影响定位建图的效果。

发明内容

本发明实施例提供了一种状态感知方法以及相关设备，实现在不需要回环运动的情况下也能够过滤无规则噪音，提高SLAM系统定位建图精度。

第一方面，本发明实施例提供了一种状态感知方法，该方法包括：

采集当前帧点云数据；从历史点云库中抽取N个历史帧点云数据，所述N个历史帧的采集时间均在所述当前帧的采集时间之前，其中，N为大于或等于1的正整数；将所述当前帧点云数据和所述N个历史帧点云数据分别进行点云配准，获得所述当前帧点云数据的N个状态估计值；将所述N个状态估计值进行叠加，得到所述当前帧点云数据的状态估计结果。

当车辆行驶在不平整的路面(如遇到石头，障碍物，坑洼等)环境时，造成车辆在短时间内的强烈颠簸，体现在点云数据中即是遭遇到无规则噪音(简称噪音)，噪音一般是高频、短时间的冲击信号。本发明实施例中可应用安置于车辆的SLAM系统，SLAM系统感知车辆当前状态的方法是：采集到当前帧点云数据后，将当前点云数据与在之前采集的若干帧点云数据进行点云配准，从而得到若干个状态估计值，所述状态估计值即为对应于不同的历史帧点云数据而得到的位置和/或姿态。点云配准所得的每个状态估计值都具有一个相应的可信度，如果受噪音影响大，则可信度低，如果受噪音影响小，则可信度高。那么选取若干个状态估计值进行叠加，获得当前状态估计结果，该状态估计结果的可信度就会比较高，也就是结果就比较准确。例如，抽取的若干帧历史帧点云数据中，可信度高的历史帧数量较多，可信度低的历史帧数量较小，那么，经过叠加后所得的结果可信度就会比较高。又例如，对于，不同的历史帧点云数据，可信度越大则权重值越大，那么经过叠加后所得的结果可信度就会比较高，也就是说能够极大减少甚至消除噪音的影响，获得车辆当前较为准确的位置和/或姿态结果，那么，可以理解的，SLAM系统将能够基于这些结果进行较为准确的建图和定位。

基于第一方面，在具体的实施方式中，所述状态估计值包括期望值和权重值；其中，所述权重值由所述当前帧点云数据与所述历史帧点云数据的重合程度值确定；所述权重值用线性分布来表示；将所述N个态估计值进行叠加，得到所述当前帧点云数据的状态估计结果，包括：将所述N个状态估计值的所述期望值基于对应的所述权重值进行加权叠加，得到所述当前帧点云数据的状态估计结果。

本发明实施例中，点云配准过程中，两帧图像基于是否存在噪音，以及噪音的强烈程度，而具有不同的重合程度。如果两帧图像是完全准确、没有噪音的，那么两帧图像的重叠部分经平移、旋转等常规处理后，是完全重合的；如果当前帧点云数据和/或历史帧点云数据有噪音，则两帧图像的重叠部分不能完全重合。两帧图像的重叠部分的重合程度，所以，两帧图像的重合程度将表征状态估计值的期望值的可信度，重合程度越高，可信度越高，重合程度越低，可信度越低。具体实现中，可将重合程度量化为方差或标准差或协方差等线性分布形式的权重值。也就是说，这些方差或标准差或协方差的大小确定了相应的状态估计值的权重值。无规则噪音越大，则所得状态估计值的权重值就会越小；无规则噪音越小，则所得状态估计值的权重值就会越大。这样，多个状态估计值进行加权叠加后，受噪音影响的图像帧对最终结果的影响小，也就是说，实施本发明实施例能够实时将噪音过滤，获得较为准确的状态估计结果。

具体应用场景中，假设汽车遇到一个坎出现颠簸，那么过坎时采集的图像将是模糊的，基于这些图像进行点云配准所得结果误差会很大。过坎前采集的图像是清晰的，基于这些图像进行点云配准所得结果误差会很小。如果当前帧为过坎后图像，那么，取出过坎后采集图像和之前的若干帧历史图像分别进行点云配准，模糊图像所得结果的权重值较小，清晰图像的权重值较大，那么，对配准结果进行加权叠加后，所得的状态估计结果就比较准确。

在具体实现中，还可以针对噪音的类型，大小等进一步调整叠加过程中每个估计状态值的权重值，噪音越大则对应的权重值调整得越小，进一步消除噪音，提高状态估计结果的精度。可以理解的，在得到状态估计结果后，由于状态估计结果中已经极大减少甚至消除了噪音，结果可信度高，将可基于所述状态估计结果进一步实现精确的建图与定位。当前帧点云数据保存至历史点云库作为历史帧点云数据后，下一次采集到点云数据时，重复上述过程进行迭代，将能达到实时性地消除噪音的效果。

需要说明的是，本发明实施例中，状态估计值的数学形式可以是概率分布估计、方差、协方差、协方差矩阵、非概率分布估计等，那么将所述状态估计值进行叠加后所得的结果数学形式不变。例如，状态估计值的数学形式为正态分布(包括期望值，和作为权重值的方差)，两个正态分布可以叠加，叠加后得到的结果仍然为正态分布。

基于第一方面，在可能的实施方式中，所述从历史点云库中抽取N个历史帧点云数据之前，包括：检测到无规则噪音；所述从历史点云库中抽取N个历史帧点云数据，包括：在检测到所述无规则噪音的情况下，触发从所述从历史点云库中抽取N个历史帧点云数据。

本发明可能实施例中，系统实时检测当前采集的当前数据帧是否存在无规则噪音，假如检测到当前数据帧存在无规则噪音，则可抽取在所述当前数据帧之前的若干历史帧点云数据。假如没有检测到当前数据帧存在无规则噪音，则可抽取在所述当前数据帧的前一帧历史帧点云数据。

本发明又一可能的实施例中，系统实时检测所述历史点云库中是否存在具有噪音的历史帧，假如检测到历史点云库中，存在不受噪音影响的历史帧(正常数据)和受噪音影响的历史帧，则可抽取在所述当前数据帧之前的若干历史帧点云数据。假如没有检测到历史点云库存在无规则噪音，则可抽取在所述当前数据帧的前一帧历史帧点云数据。

基于第一方面，在可能的实施方式中，所述从历史点云库中抽取N个历史帧点云数据，包括：根据所述无规则噪音确定抽样步长；基于所述抽样步长从历史点云库中抽取N个历史帧点云数据。

本发明可能实施例中，系统在检测到无规则噪声的情况下，根据所述无规则噪音确定抽样步长，基于所述抽样步长从历史点云库中抽取在所述点云数据之前的若干历史帧点云数据。例如，系统检测到在历史点云库中的第k+1帧到第n帧存在无规则噪音，确定抽样步长为n帧，忽略掉存在噪音的历史帧，抽取在所述噪音之前的历史帧(如图示中第1帧至第k帧)作为所述点云数据集合。在特殊应用场景中，甚至可以基于抽样步长，只抽取某一个不受无规则噪音影响的历史帧，以部分达到本发明实施例的过滤掉不同频率的噪音的效果。举例来说，具体应用场景中，假设汽车遇到了一个坎出现了颠簸，那么过坎期间采集的图像将是模糊的，如果抽取出过坎前和过坎后的两帧图像，因为这两帧图像都是清晰的，那么进行后述步骤的点云配准的结果也将是准确的。这样，过坎时的无规则噪音导致的误差，就不会扩散到过坎后的测量中。

基于第一方面，在可能的实施方式中，所述从历史点云库中抽取N个历史帧点云数据，包括：检测到所述当前帧点云数据的采集次序符合预设次序；从历史点云库中抽取在所述当前帧点云数据之前的点云数据集合，包括：在检测到所述当前帧点云数据的采集次序符合预设次序的情况下，触发从历史点云库中抽取N个历史帧点云数据。

本发明可能的实施例中，如果系统检测到当前帧点云数据的次序符合预设次序，则抽取在所述当前帧点云数据之前的若干历史帧点云数据。如果系统检测到当前帧点云数据的次序不符合预设次序，则可抽取在所述当前帧点云数据的前一帧作为所述点云数据集合。例如，如果当前帧的采集次序为第5帧，第10帧，第15帧…等等，则抽取当前帧点云数据的前4帧历史帧点云数据作为所述点云数据集合，而如果当前帧不符合上述周期性次序，则可抽取在所述当前数据帧的前一帧历史帧点云数据作为所述点云数据集合。

第二方面，本发明实施例提供了一种用于状态感知的设备，该设备包括采集模块、抽取模块、配准模块和叠加模块，其中：

采集模块，用于采集当前帧点云数据；

抽取模块，用于从历史点云库中抽取N个历史帧点云数据，所述N个历史帧的采集时间均在所述当前帧的采集时间之前，其中，N为大于或等于1的正整数；

配准模块，用于将所述当前帧点云数据和所述N个历史帧点云数据分别进行点云配准，获得所述当前帧点云数据的多个状态估计值；

叠加模块，用于将所述N个状态估计值进行叠加，得到所述当前帧点云数据的状态估计结果。

具体实施例中，采集模块、抽取模块、配准模块和叠加模块可用于实现第一方面所述方法。

第三方面，本发明实施例提供了一种用于状态感知的设备，该设备包括：处理器，以及耦合至所述处理器的感应器和存储器，其中，所述感应器用于采集当前帧点云数据；所述存储器用于存储历史点云库和程序代码；所述处理器用于调用所述程序代码执行第一方面所述的方法。

第四方面，所述本发明实施例提供了一种计算机可读存储介质，用于存储第一方面所述方法的实现代码。

第五方面，本发明实施例提供了一种计算机软件产品，当其在计算机中运行时，可用于实现第一方面所述的方法。

可以看出，本发明实施例中，SLAM系统所在对象在正常行驶过程中，通过当前帧点云数据和若干个历史帧点云数据进行点云配准，获得若干个关于当前位置或姿态的状态估计值，将这些状态估计值进行叠加，在叠加过程中通过算法消除无规则噪音。在自动驾驶中，通常感应器采样频率很高。在传统卡尔曼滤波中，如果采样频率高则容易受到无规则噪音的影响，导致结果出现较大误差。而实施本发明实施例，即使SLAM系统所在对象不做回环运动，也能够消除所遭遇到的无规则噪音，大幅提高状态估计结果的精度，并且这种精度提高是实时性的(而传统回环运动精度提高不是实时的)，从而能够进一步提高后续应用中建图与定位的精度，有利于自动驾驶产品的实用化，提升用户体验。

附图说明

图1是本发明实施例提供的一种SLAM系统的系统架构示意图；

图2是本发明实施例提供的一种SLAM系统功能实现的逻辑框图；

图3是本发明实施例提供的一种状态感知方法的流程示意图；

图4是本发明实施例提供的一种应用状态感知方法场景的逻辑框图；

图5是本发明实施例提供的又一种状态感知方法的流程示意图；

图6是本发明实施例提供的一种抽取历史帧的场景示意图；

图7a是本发明实施例提供的又一种抽取历史帧的场景示意图；

图7b是本发明实施例提供的又一种抽取历史帧的场景示意图；

图7c是本发明实施例提供的又一种抽取历史帧的场景示意图；

图8是本发明实施例提供的又一种抽取历史帧的场景示意图；

图9是本发明实施例提供的又一种抽取历史帧的场景示意图；

图10是本发明实施例提供的一种状态估计值叠加的场景示意图；

图11a是一种对比试验中传统方案所得结果示意图；

图11b是一种对比试验中本发明实施例方案所得结果示意图；

图12a是又一种对比试验中传统方案所得结果示意图；

图12b是又一种对比试验中本发明实施例方案所得结果示意图。

图13是本发明实施例提供的一种SLAM终端的结构示意图；

图14是本发明实施例提供的一种设备的结构示意图。

具体实施方式

下面结合相关附图说明本发明实施例的技术方案。

首先介绍本发明实施例的系统框架。本发明实施例提供了一种SLAM系统，该SLAM系统可应用于无人驾驶汽车、移动机器人等自动驾驶对象中。参见图1，图一示出了在无人驾驶汽车上应用SLAM系统的场景。在硬件实现上，该SLAM系统可包括感应器、存储器和处理器。感应器用于进行数据采集，存储器用于进行数据存储(如存储历史点云库的数据)，处理器用于进行数据处理。参见图2，在功能实现上，SLAM系统可包括数据采集、扩展视觉里程计、扩展后端优化以及建图等几个流程模块，其中，本发明实施例主要是对视觉里程计、后端优化这两个模块进行了扩展优化。在感应器完成数据采集后，处理器在执行扩展视觉里程计、扩展后端优化以及建图。具体说明如下：

(1)数据采集。

本发明实施例中，感应器用于进行数据采集，所述感应器例如可以是激光雷达、3D摄像头、GPS、速度计、加速计、里程计等等，所述数据为与行驶状态相关的数据。举例来说，感应器为3D摄像头或激光雷达，在汽车运行中，3D摄像头或激光雷达实时监测周围环境，得到一帧帧的立体图像(即空间点云数据)。举例来说，感应器还可为GPS、速度计、加速计、里程计等，那么可根据所述感应器获得汽车的运行状态信息(如速度，加速度，路程等)。

(2)扩展的视觉里程计。

现有技术中，视觉里程计(visual odometry，VO)是通过分析处理前后两帧图像，对前后两帧图像进行点云配准，来估计两个时刻所在自动驾驶对象的相对运动，确定自动驾驶对象的位置和/或姿态。本发明实施例中，对视觉里程计的功能进行扩展，扩展的视觉里程计中，实时获取的当前帧的点云数据(下文简称为当前帧点云数据)与在之前获取到的若干个历史帧的点云数据(下文简称为历史帧点云数据)相关，这里的相关是指，帧中的部分图像内容重合，可以根据图像的重合部位估计自动驾驶对象在两帧中的移动情况。所以，需要将当前帧点云数据与若干个历史帧点云数据分别进行点云配准，从而得到若干个状态估计值，所述状态估计值即为对应于不同的历史帧点云数据而得到的位置和/或姿态。其中，点云配准又称为点云匹配，是指两个或两个以上的点云数据根据公共特征或标记进行最佳拟合的技术。

(3)扩展的后端优化。现有技术中，在后端优化中通常采用卡尔曼滤波的方法进行误差消除。本发明实施例对后端优化进行拓展，在前一步视觉里程计中得到多个状态估计值后，在后端优化中，对所述多个状态估计值进行叠加，从而得到当前帧点云数据所对应的总的状态估计值。

具体实施例中，在将当前帧点云数据与前面的若干历史帧点云数据分别进行配准后，得到若干个独立的状态估计值，每个状态估计值都将对应于一个结果可信度。自动驾驶对象在不平整的路面(如遇到石头，障碍物，坑洼等)环境中，造成车辆在短时间内的强烈颠簸，体现在点云数据中即是遭遇到无规则噪音(简称噪音)，无规则噪音一般是高频、短时间的冲击信号。基于是否存在噪音，以及噪音的强烈程度，这些状态估计值将具有不同的可信度，如果两帧图像是完全准确、没有噪音的，那么两帧图像的重叠部分经平移、旋转等常规处理后，是完全重合的；如果当前帧点云数据和/或历史帧点云数据有噪音，则两帧图像的重叠部分不能完全重合。两帧图像的重叠部分的重合程度，所以，两帧图像的重合程度将表征状态估计值的可信度，重合程度越高，可信度越高，重合程度越低，可信度越低。具体实现中，可将重合程度量化为方差或标准差或协方差等线性分布形式。也就是说，这些方差或标准差或协方差的大小确定了相应的状态估计值的权重值。无规则噪音越大，则所得状态估计值的权重值就会越小；无规则噪音越小，则所得状态估计值的权重值就会越大。这样，多个状态估计值进行叠加后，就能够将无规则噪音过滤。

具体实施例中，所述状态估计值包括期望值和权重值；其中，所述权重值由所述当前帧点云数据与所述历史帧点云数据的重合程度值确定；所述权重值用线性分布来表示；将所述N个态估计值进行叠加，得到所述当前帧点云数据的状态估计结果，包括：将所述N个状态估计值的所述期望值基于对应的所述权重值进行加权叠加，得到所述当前帧点云数据的状态估计结果。

举例来说，当前汽车刚刚驶过一个坎，出现较大的颠簸，导致了当前帧的前五帧是受无规则噪音影响的，那么当前帧与前五帧分别进行点云配准后，所得的状态估计值可信度低，相应权重值低。如果当前帧是过坎后的点云数据、当前帧的前面第十帧是过坎前的点云数据，那么根据这两帧进行点云配准得到的状态估计值可信度高，相应权重值高。这样把多个状态估计值的结果加权叠加起来，就可以达到消除无规则噪音的效果。

(4)建图。

在确定当前帧点云数据的最终的状态估计值后，可将所述状态估计值保存到历史点云库中，在后述的运动过程中，不断地进行迭代计算，重复本发明实施例的上述过程，就能够实现感知当前的运行状态信息，实时进行建图处理，基于图像匹配算法，依次把前后两帧图像连接起来，最后得到全局的立体图像，得到SLAM所建的地图，所述地图可以是几何地图、栅格地图、拓扑地图、2D地图、3D地图等等。本发明实施例对建图过程不做具体限定。可以理解的，在后述应用中，SLAM系统将实时获得的当前帧与上文描述的所建地图进行匹配，就可定位自动驾驶对象在地图中的位置。

基于所描述的SLAM系统，下面详细描述本发明实施例提供的一种状态感知方法，参见图3，该方法包括但不限于以下步骤：

步骤S101、采集当前帧点云数据。

本发明实施例中，当前帧点云数据为当前所采集的点云数据。具体的，可通过感应器采集所述点云数据；所述感应器包括光学传感器(如3D摄像头)、激光传感器(如激光雷达)、速度计、加速计、里程计的至少一种。例如，自动驾驶汽车通过3D摄像头或激光雷达，周期性的对周围环境进行拍摄，得到一帧一帧的点云数据。所述点云数据是指在一个三维坐标系统中的一系列扫描点的集合，所述扫描点通常以一组向量的形式存在，这些向量通常以三维坐标的形式表示所监测(扫描)对象的几何位置信息。所述点云数据还可以表示扫描点的RGB颜色，灰度值，深度，分割结果等等信息。

步骤S102、从历史点云库中抽取在所述当前帧点云数据之前的点云数据集合。

具体的，SLAM系统中设置一个历史点云库，所述历史点云库用于保存所采集到的若干个点云数据，这样的点样数据又可称为历史帧点云数据。参见图4，图4是本发明实施例提供的状态感知方法的流程框图，如图4所示，在实时通过感应器获取当前帧点云数据后，一方面，可将所述当前帧点云数据保存至所述历史点云库；另一方面，从所述历史点云库中抽取在所述当前帧点云数据之前的点云数据集合，所述点云数据集合包括一个或多个历史帧点云数据。

需要说明的是，在可能的实施例中，所述历史点云库动态维护所保存的点云数据，也就是说，历史点云库中动态保存最近获得的若干个点云数据，而将超过期限的点云数据删除。所述点云数据集合可以是所述历史点云库中的部分或全部的点云数据。例如，历史点云库中只保存最近所采集的20帧图像，而将20帧之前的图像删除，所抽取的点云数据集合为所述历史点云库中的10帧图像。

S103、将所述当前帧点云数据和各个所述历史帧点云数据分别进行点云配准，获得所述当前帧点云数据的多种状态估计值。

其中，所述状态估计值用于描述SLAM系统所在对象的位置或姿态，例如所述状态估计值为自动驾驶汽车的当前位置的估计值。具体实现中，所述状态估计值的数学形式可以是概率分布估计(如正态分布)、方差、协方差、协方差矩阵、非概率分布估计等等线性分布。

具体实施例中，将所述点云数据和各个所述历史帧点云数据分别进行点云配准，所得的结果包括：基于点云匹配所获得SLAM系统所在对象当前的的多个状态估计值，以及，根据所述点云数据与各个所述历史帧点云数据的重合程度值确定各个所述状态估计值的权重值。所述权重值的数学形式可以是概率分布估计(如正态分布)、方差、协方差、协方差矩阵、非概率分布估计等等线性分布。

其中，点云配准是对图像经过平移、旋转等操作，使得两帧图像上的相同扫描点能够一一对准，判断SLAM系统所在对象在两帧图像期间的状态变化情况，比如当前帧比历史帧前进了多少米、转向了多少角度等，从而算出当前位置或姿态等状态估计值。如图3所示，所抽取的点云数据集合包括历史帧数据1、历史帧数据2、历史帧数据3，将当前帧点云数据分别与历史帧数据1、历史帧数据2、历史帧数据3进行点云配准，分别获得状态估计值1与权重值1，状态估计值2与权重值2，状态估计值3与权重值3。

其中，由于扫描角度、环境噪音等原因，总有一些特征(特征指图像中的点、线、面等)会配准不上。那么，需要找到一个最佳配准，使得两帧图像尽可能多的特征能够配准上。根据所能够配准上的特征的数目、未配准上的特征的数目，可以算出状态估计值对应的可信度。匹配上的特征的数目越少状态估计值的可信度越低，匹配上的特征的数目越多状态估计值的可信度越高。具体实现中，可信度可以用权重值来表征，所述权重值的数学形式可以方差或标准差或协方差，可以是一个概率分布，例如正态分布。可以理解的，本发明实施例中，配准结果是能够感知噪音的，如果噪音大，那么点云配准时就会有更多的特征配准不上，对应的计算出的权重值也较低。

S104、将所述多个状态估计值进行叠加，得到所述当前帧点云数据的状态估计结果。

具体的，在获得多种状态估计值后，将所述多个状态估计值进行叠加到一起，得到一个总的关于SLAM所在对象的状态估计值。由于状态估计值的数学形式可以是概率分布估计、方差、协方差、协方差矩阵、非概率分布估计等，那么将所述状态估计值进行叠加后所得的结果数学形式不变。例如，状态估计值的数学形式为正态分布，两个正态分布可以叠加，叠加后得到的结果仍然为正态分布。

具体实施例中，点云配准的结果除了状态估计值，还包括权重值。那么，需要将各个状态估计值基于对应的所述权重值进行加权叠加，从而得到所述当前帧点云数据的状态估计结果。

本发明具体实施例中，点云配准过程可以感知无规则噪音，基于不同的历史帧点云数据算出来的权重值与无规则噪音负相关，那么经过叠加后，所得的状态估计结果能够相应减少无规则噪音的影响。在具体实现中，还可以针对噪音的类型，大小等调整叠加过程中每个估计状态值的权重值，噪音越大则对应的权重值调整得越小，进一步消除噪音，可以保证在处理当前帧点云数据时，所得的状态估计结果是相对准确的。

可以理解的，在得到状态估计结果后，由于状态估计结果中已经极大减少甚至消除了噪音，结果可信度高，将可基于所述状态估计结果进一步实现精确的建图与定位。当前帧点云数据保存至历史点云库作为历史帧点云数据后，下一次采集到点云数据时，重复上述过程进行迭代，将能达到实时性地消除噪音的效果。

可以看出，本发明实施例中，SLAM系统所在对象在正常行驶过程中，通过当前帧点云数据和若干个历史帧点云数据进行点云配准，获得若干个关于当前位置或姿态的状态估计值，将这些状态估计值进行叠加，在叠加过程中通过算法消除无规则噪音。在自动驾驶中，通常感应器采样频率很高。在传统卡尔曼滤波中，如果采样频率高则容易受到无规则噪音的影响，导致结果出现较大误差。而实施本发明实施例，即使SLAM系统所在对象不做回环运动，也能够消除所遭遇到的无规则噪音，大幅提高状态估计结果的精度，并且这种精度提高是实时性的(而传统回环运动精度提高不是实时的)，从而能够进一步提高后续应用中建图与定位的精度，有利于自动驾驶产品的实用化，提升用户体验。

基于所描述的SLAM系统，下面详细描述本发明实施例提供的又一种状态感知方法，参见图5，该方法包括但不限于以下步骤：

步骤S201、采集当前帧点云数据。具体的，可通过感应器采集所述点云数据；所述感应器包括光学传感器(如3D摄像头)、激光传感器(如激光雷达)、速度计、加速计、里程计的至少一种。

步骤S202、检测当前是否符合预设触发策略，如果符合，则触发执行步骤S203。

在具体的实施例中，为了提高工作效率，实现快速消除噪音的目的，不需要每次采集到当前帧点云数据都执行本发明实施例的技术方案，而是选择性地在符合预设的触发策略下才执行本发明实施例的技术方案。

在一可能的实施例中，所述触发策略为：在检测到当前帧点云数据存在无规则噪音的情况下，触发执行后续步骤S203。例如，采集到当前帧点云数据时，先将当前帧点云数据与前一帧点云数据进行点云配准，通过两帧的重合程度确定是否存在噪音，如果重合程度较低，则说明存在无规则噪音，从而触发执行后续步骤S203。

在又一可能的实施例中，所述触发策略为：在检测到历史点云库中存在无规则噪音的情况下，触发执行后续步骤S203。例如，上文提到，历史点云库中维护有最近一段时间的历史帧点云数据，并且这些历史帧点云数据对应于权重值，权重值较小说明可信度低，也就是该历史帧点云数据受到噪音影响。所以，在每次采集当前帧历史数据时，首先检测历史点云库中是否存在受噪音影响的历史帧点云数据，如果存在，则说明检测到无规则噪音，触发执行后续步骤S203。

在又一可能的实施例中，所述触发策略为：在通过传感器检测到环境易引起无规则噪音的情况下，触发执行后续步骤S203。例如，通过传感器检测汽车运动状态，当传感器检测到车辆在不平整路面发生较明显颠簸时，即可判定所在环境易引起不规则噪音，触发执行后续步骤S203。

在又一可能的实施例中，所述触发策略为：检测到当前帧点云数据的次序符合预设次序的情况下，触发执行后续步骤S203。例如，每个点云数据均对应一个次序号，当前帧点云数据的次序满足周期性次序时，如当前帧为第5帧，第10帧，第15帧…的情况下，即触发执行后续步骤S203。

需要说明的是，当检测当前不符合预设触发策略，说明此时不存在噪音或者噪音较弱，这样情况下，可不执行本发明技术方案，当前帧点云数据直接与前一帧进行点云配准以及完成后端图优化、建图定位等工作。

步骤S203、从历史点云库中抽取在所述点云数据之前的点云数据集合。

在SLAM系统中设置一个历史点云库，所述历史点云库用于保存近期所采集到的点云数据，动态维护所保存的点云数据。

参见图6，在可能的实施例中，当需要抽取点云数据集合时，固定在所述历史点云库中抽取所述当前帧之前的k帧历史帧点云数据作为所述点云数据集合，其中k＞1，例如k＝10，即固定抽取所述当前帧之前的10帧历史帧点云数据作为所述点云数据集合。

在一可能的实施例中，系统实时检测当前采集的当前数据帧是否存在无规则噪音，参见图7a，假如检测到当前数据帧存在无规则噪音，则抽取在所述当前数据帧之前的k帧历史帧点云数据作为所述点云数据集合。参见图7c，假如没有检测到当前数据帧存在无规则噪音，则抽取在所述当前数据帧的前一帧历史帧点云数据作为所述点云数据集合。

在又一可能的实施例中，系统实时检测所述历史点云库中是否存在具有噪音的历史帧，参见图7b，假如检测到历史点云库中，存在不受噪音影响的历史帧(正常数据)和受噪音影响的历史帧(图中第k+1帧)，则抽取在所述当前数据帧之前的k帧历史帧点云数据作为所述点云数据集合。假如没有检测到历史点云库存在无规则噪音，则抽取在所述当前数据帧的前一帧历史帧点云数据作为所述点云数据集合(同样如图7c所示)。

在又一可能的实施例中，系统在检测到无规则噪声的情况下，根据所述无规则噪音确定抽样步长，基于所述抽样步长从历史点云库中抽取在所述点云数据之前的点云数据集合。参见图8，系统检测到在历史点云库中的第k+1帧到第n帧存在无规则噪音，确定抽样步长为n帧，忽略掉存在噪音的历史帧，抽取在所述噪音之前的历史帧(如图示中第1帧至第k帧)作为所述点云数据集合。在特殊应用场景中，甚至可以基于抽样步长，只抽取某一个不受无规则噪音影响的历史帧，以部分达到本发明实施例的过滤掉不同频率的噪音的效果。举例来说，具体应用场景中，假设汽车遇到了一个坎出现了颠簸，那么过坎期间采集的图像将是模糊的，如果抽取出过坎前和过坎后的两帧图像，因为这两帧图像都是清晰的，那么进行后述步骤的点云配准的结果也将是准确的。这样，过坎时的无规则噪音导致的误差，就不会扩散到过坎后的测量中。

在又一可能的实施例中，如果系统检测到当前帧点云数据的次序符合预设次序，则抽取在所述当前帧点云数据之前的k帧作为点云数据集合。如果系统检测到当前帧点云数据的次序不符合预设次序，则抽取在所述当前帧点云数据的前一帧作为所述点云数据集合。参见图9，如果当前帧的次序为第5帧，第10帧，第15帧…等等，则抽取当前帧点云数据的前4帧历史帧点云数据作为所述点云数据集合，而如果当前帧不符合上述周期性次序，则抽取在所述当前数据帧的前一帧历史帧点云数据作为所述点云数据集合。

可以理解的，本发明实施例中，从历史点云库中抽取在所述点云数据之前的点云数据集合的方式还可以有其他实现形式，上述示例仅仅用于解释本发明实施例而非限制。

S204、将所述当前帧点云数据和点云数据集合中各个所述历史帧点云数据分别进行点云配准，获得所述当前帧点云数据的多种状态估计值，以及每个状态估计值对应的权重值。具体可参考图3实施例步骤S103的描述，这里不再赘述。

S205、将所述多个状态估计值进行加权叠加，得到所述当前帧点云数据的状态估计结果。

具体实施例中，所述状态估计值包括期望值和权重值；其中，所述权重值由所述当前帧点云数据与所述历史帧点云数据的重合程度值确定；所述权重值用线性分布来表示；将所述N个态估计值进行叠加，得到所述当前帧点云数据的状态估计结果，包括：将所述N个状态估计值的所述期望值基于对应的所述权重值进行加权叠加，得到所述当前帧点云数据的状态估计结果。状态估计值的数学形式可以是概率分布估计(如正态分布)、方差、协方差、协方差矩阵、非概率分布估计等等。

举例来说，参见图10，假设汽车运行经过A、B、C三个位置点，在各个点采集到的点云数据分别为A帧、B帧、C帧。其中，所得到A帧的状态估计值为数学期望a、权重值用方差σ²的正态分布来表征。B帧与A帧点云配准后，算出B帧相对于A帧，其位置点前进距离的数学期望是Δx₁、方差为σ₁ ²的正态分布，则B帧的状态估计值为数学期望b＝(a+Δx₁)、权重值用方差(σ²+σ₁ ²)的正态分布表征。对于C帧，应用本发明实施例的技术方案，C帧与A帧、B帧分别进行点云配准，算出C帧相对于B帧位置点前进距离的数学期望是Δx₂、方差为σ₂ ²的正态分布，则C帧的一个状态估计值为数学期望c₁＝(a+Δx₁+Δx₂)、权重值用方差σc₁ ²＝(σ²+σ₁ ²+σ₂ ²)的正态分布表征；算出C帧相对于A帧位置点前进距离的数学期望是Δx₃、方差为σ₃ ²的正态分布，则C帧的另一个状态估计值为数学期望c₂＝(a+Δx₃)、权重值用方差σc₂ ²＝(σ²+σ₃ ²)的正态分布表征。然后，将所得到的两个对C点的状态估计值c₁、c₂及对应的权重值进行加权叠加，叠加过程可使用均方误差的公式进行计算，从而得到C点的总的状态估计值为数学期望c＝c₁+sqrt(σc₁ ²/(σc₁ ²+σc₂ ²))*(c₂-c₁)、权重值用方差σc²＝(1-sqrt(σc₁ ²/(σc₁ ²+σc₂ ²)))*σc₁ ²表征。

可以理解的，在得到状态估计结果后，下一次采集到点云数据时，参考上述位置点C的计算过程进行迭代处理，在汽车行驶过程中的每个点，都能较准确的评估出汽车的位置，达到实时性地消除噪音的效果。

可以看出，本发明实施例中，SLAM系统所在对象在正常行驶过程中，通过预设策略来触发当前帧点云数据和若干个历史帧点云数据进行点云配准，获得若干个关于当前位置或姿态的状态估计值和权重值，将这些状态估计值进行加权叠加，在叠加过程中通过算法消除无规则噪音。实施本发明实施例，即使SLAM系统所在对象不做回环运动，也能够消除所遭遇到的无规则噪音，大幅提高状态估计结果的精度，并且这种精度提高是实时性的(而传统回环运动精度提高不是实时的)，从而能够进一步提高后续应用中建图与定位的精度，有利于自动驾驶产品的实用化，提升用户体验。

为了更好地理解本发明实施例的技术效果，下面简述现有技术方案与本发明技术方案的测试对比结果。

在一种对比试验中，参见图11a与图11b，图11a示出了使用传统SLAM方案对在不平整路面上行驶的被测汽车的行驶轨迹进行建图所得到的结果，将该结果在二维坐标图呈现，图示中灰色轨迹即为所得到的汽车的运行轨迹。可以看到，由于受到无规则噪音的严重影响该运行轨迹交错杂乱，线条不连续，建图结果误差较大。图11b示出了在同等测试条件下，使用本发明实施例提供的SLAM方案对在不平整路面上行驶的被测汽车的行驶轨迹进行建图所得到的结果。可以看到二维坐标图呈现运行轨迹线条光滑平整，线条连续，也就是说，本发明实施例能够极大的消除噪音对后续帧的影响，建图结果误差较小。

在又一种对比试验中，参见图12a与图12b，图12a示出了使用传统SLAM方案对在不平整路面上进行回环行驶的被测汽车的行驶轨迹进行建图所得到的结果，在该方案中，采用人为手动调参的方式去掉出现噪音的卡尔曼滤波图像帧，所得到的行驶轨迹虽然线条连续，但是起始点和终止点在图示结果中的误差大约为250米，也就是说，该建图定位的结果误差较大，结果可信度低，不利于实际产品应用。图12b示出了在同等测试条件下，使用本发明实施例提供的SLAM方案对在不平整路面上进行回环行驶的被测汽车的行驶轨迹进行建图所得到的结果，可以看到，二维坐标图呈现运行轨迹线条光滑平整，线条连续，并且起始点和终止点在图示结果中的误差大约为70米.也就是相比起传统方案，本发明实施例能够大幅减少建图定位的误差，大幅提高了汽车位置评估的精度，结果可信度高。另外，需要说明的是，由于无规则噪音意味着系统可能遭遇各种频率或强度的噪音，这就意味着人工调参的方式只适合特定场景的特点噪音，而无法使用一组普适的参数可以过滤掉所有噪音，场景改变后系统的效果会变差。而应用本发明实施例可以自动针对各种无规则噪音进行过滤，适用于各种应用场景，实际产品实用性更强。

上文描述了本发明实施例所涉及的方法流程，下面描述本发明实施例所涉及的相关设备。

参见图13，图13是本发明实施例提供的状态感知的SLAM终端300的一种实现方式的结构框图。如图13所示，SLAM终端300可包括：芯片310、存储器315(一个或多个计算机可读存储介质)、外围系统317。这些部件可在一个或多个通信总线314上通信。

外围系统317主要用于实现SLAM终端300和用户/外部环境之间的交互功能，具体实现中，外围系统317可包括：触摸屏控制器318、3D摄像头控制器319、激光雷达控制器320以及传感器管理模块321中的若干个部件。其中，各个控制器可与各自对应的外围设备如触摸屏323、3D摄像头324、激光雷达325以及传感器326等耦合，其中，所述3D摄像头控制器319、激光雷达控制器320以及传感器326等都属于感应器。在一些实施例中，所述传感器可以是速度计、加速计、里程计GPS中的一个或多个。需要说明的，外围系统317还可以包括其他I/O外设。

芯片310可集成包括：一个或多个处理器311、时钟模块312以及可能的电源管理模块313。集成于芯片310中的时钟模块312主要用于为处理器311产生数据传输和时序控制所需要的时钟。集成于基带芯片310中的电源管理模块313主要用于为处理器311以及外围系统提供稳定的、高精确度的电压。

存储器315与处理器311耦合，用于存储数据(如历史点云库)、各种软件程序和/或多组程序指令。具体实现中，存储器315可包括高速随机存取的存储器，并且也可包括非易失性存储器，例如一个或多个磁盘存储设备、闪存设备或其他非易失性固态存储设备。存储器315还可以存储一个或多个应用程序。如图示中这些应用程序可包括：地图类应用程序、图像管理类应用程序等等。

应当理解，SLAM终端300仅为本发明实施例提供的一个例子，并且，SLAM终端300可具有比示出的部件更多或更少的部件，可以组合两个或更多个部件，或者可具有部件的不同配置实现。

本发明具体实施例中，3D摄像头324或激光雷达325或传感器326可用于采集当前帧点云数据；存储器312用于存储历史点云库；所述处理器311可用于调用存储器中的程序指令，执行以下程序步骤：

从历史点云库中抽取N个历史帧点云数据，所述N个历史帧的采集时间均在所述当前帧的采集时间之前，其中，N为大于或等于1的正整数；

将所述当前帧点云数据和所述N个历史帧点云数据分别进行点云配准，获得所述当前帧点云数据的N个状态估计值；

将所述N个状态估计值进行叠加，得到所述当前帧点云数据的状态估计结果。

具体实施例中，所述状态估计值包括期望值和权重值；其中，所述权重值由所述当前帧点云数据与所述历史帧点云数据的重合程度值确定；所述权重值用线性分布来表示；处理器311用于执行将所述多种状态估计值进行叠加，得到所述当前帧点云数据的状态估计结果，包括：处理器311用于将所述N个态估计值进行叠加，得到所述当前帧点云数据的状态估计结果，包括：将所述N个状态估计值的所述期望值基于对应的所述权重值进行加权叠加，得到所述当前帧点云数据的状态估计结果。

在具体实施例中，处理器311用于执行所述从历史点云库中抽取N个历史帧点云数据之前，所述方法还包括：检测到无规则噪音；所述从历史点云库中抽取N个历史帧点云数据，包括：处理器311用于执行在检测到所述无规则噪音的情况下，触发所述从历史点云库中抽取N个历史帧点云数据。

具体实施例中，处理器311用于执行所述从历史点云库中抽取N个历史帧点云数据之前，所述方法还包括：处理器311用于执行检测到所述当前帧点云数据的采集次序符合预设次序；处理器311用于执行所述从历史点云库中抽取N个历史帧点云数据，包括：处理器311用于执行在检测到所述当前帧点云数据的采集次序符合预设次序的情况下，触发从历史点云库中抽取N个历史帧点云数据。

具体实施例中，处理器311用于执行从历史点云库中抽取在所述当前帧点云数据之前的点云数据集合，包括：处理器311用于执行根据所述无规则噪音确定抽样步长；基于所述抽样步长从历史点云库中抽取在所述当前帧点云数据之前的点云数据集合。

其中，所述状态估计值的数学形式包括概率分布估计、方差、协方差、协方差矩阵、非概率分布估计的至少一种。

需要说明的，通过前述图3或图5方法实施例的详细描述，本领域技术人员可以清楚的知道SLAM终端300所包含的各个部件的实现方法，所以为了说明书的简洁，在此不再赘述。

基于相同的发明构思，本发明实施例提供了又一种用于状态感知的设备400，参见图14，所述设备400包括：采集模块401、抽取模块402、配准模块403、叠加模块404，具体描述如下：

采集模块401，用于采集当前帧点云数据；

抽取模块402，用于从历史点云库中抽取N个历史帧点云数据，所述N个历史帧的采集时间均在所述当前帧的采集时间之前，其中，N为大于或等于1的正整数；

配准模块403，用于将所述当前帧点云数据和所述N个历史帧点云数据分别进行点云配准，获得所述当前帧点云数据的多个状态估计值；

叠加模块404，用于将所述N个状态估计值进行叠加，得到所述当前帧点云数据的状态估计结果。

具体实施例中，所述状态估计值包括期望值和权重值；其中，所述权重值由所述当前帧点云数据与所述历史帧点云数据的重合程度值确定；所述权重值用线性分布来表示；

所述叠加模块404用于将所述多种状态估计值进行叠加，得到所述当前帧点云数据的状态估计结果，包括：所述叠加模块404用于将所述N个状态估计值的所述期望值基于对应的所述权重值进行加权叠加，得到所述当前帧点云数据的状态估计结果。

具体实施例中，所述设备还包括检测模块405，所述检测模块405用于检测无规则噪音；

所述抽取模块402用于从历史点云库中抽取N个历史帧点云数据，包括：

在所述检测模块405检测到所述无规则噪音的情况下，触发所述抽取模块402从所述从历史点云库中抽取N个历史帧点云数据。

具体实施例中，所述检测模块405用于检测到所述当前帧点云数据的采集次序是否符合预设次序；

所述抽取模块402用于从历史点云库中抽取N个历史帧点云数据，包括：在所述检测模块405检测到所述当前帧点云数据的采集次序符合预设次序的情况下，触发所述抽取模块402从所述从历史点云库中抽取N个历史帧点云数据。

具体实施例中，所述抽取模块405从所述从历史点云库中抽取N个历史帧点云数据，包括：所述抽取模块402用于根据所述无规则噪音确定抽样步长；基于所述抽样步长从所述从历史点云库中抽取N个历史帧点云数据。

其中，所述状态估计值的数学形式包括概率分布估计、方差、协方差、协方差矩阵、非概率分布估计的至少一种。

需要说明的，通过前述图3或图5方法实施例的详细描述，本领域技术人员可以清楚的知道设备400所包含的各个功能模块的实现方法，所以为了说明书的简洁，在此不再赘述。

在上述实施例中，可以全部或部分地通过软件、硬件、固件或者任意组合来实现。当使用软件实现时，可以全部或者部分地以计算机程序产品的形式实现。所述计算机程序产品包括一个或多个计算机指令，在计算机上加载和执行所述计算机程序指令时，全部或部分地产生按照本发明实施例所述的流程或功能。所述计算机可以是通用计算机、专用计算机、计算机网络或其他可编程装置。所述计算机指令可存储在计算机可读存储介质中，或者从一个计算机可读存储介质向另一个计算机可读存储介质传输，例如，所述计算机指令可以从一个网络站点、计算机、服务器或数据中心通过有线(例如同轴电缆、光纤、数字用户线)或无线(例如红外、微波等)方式向另一个网络站点、计算机、服务器或数据中心进行传输。所述计算机可读存储介质可以是计算机能够存取的任何可用介质，也可以是包含一个或多个可用介质集成的服务器、数据中心等数据存储设备。所述可用介质可以是磁性介质(例如软盘、硬盘、磁带等)、光介质(例如DVD等)、或者半导体介质(例如固态硬盘)等等。

在上述实施例中，对各个实施例的描述各有侧重，某个实施例中没有详述的部分，可以参见其他实施例的相关描述。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以所述权利要求的保护范围为准。

Claims (13)

1.一种状态感知方法，其特征在于，包括：

采集当前帧点云数据；

从历史点云库中抽取N个历史帧点云数据，所述N个历史帧的采集时间均在所述当前帧的采集时间之前，其中，N为大于或等于1的正整数；

将所述当前帧点云数据和所述N个历史帧点云数据分别进行点云配准，获得所述当前帧点云数据的N个状态估计值；

将所述N个状态估计值进行叠加，得到所述当前帧点云数据的状态估计结果。

2.根据权利要求1所述的方法，其特征在于，

所述状态估计值包括期望值和权重值；其中，所述权重值由所述当前帧点云数据与所述历史帧点云数据的重合程度值确定；所述权重值用线性分布来表示；

将所述N个态估计值进行叠加，得到所述当前帧点云数据的状态估计结果，包括：将所述N个状态估计值的所述期望值基于对应的所述权重值进行加权叠加，得到所述当前帧点云数据的状态估计结果。

3.根据权利要求1或2所述的方法，其特征在于，所述从历史点云库中抽取N个历史帧点云数据之前，所述方法还包括：检测到无规则噪音；

所述从历史点云库中抽取N个历史帧点云数据，包括：

在检测到所述无规则噪音的情况下，触发所述从历史点云库中抽取N个历史帧点云数据。

4.根据权利要求3所述的方法，其特征在于，所述从历史点云库中抽取N个历史帧点云数据，包括：

根据所述无规则噪音确定抽样步长；

基于所述抽样步长从历史点云库中抽取N个历史帧点云数据。

5.根据权利要求1或2所述的方法，其特征在于，所述从历史点云库中抽取N个历史帧点云数据之前，所述方法还包括：检测到所述当前帧点云数据的采集次序符合预设次序；

所述从历史点云库中抽取N个历史帧点云数据，包括：

在检测到所述当前帧点云数据的采集次序符合预设次序的情况下，触发从历史点云库中抽取N个历史帧点云数据。

6.一种用于状态感知的设备，其特征在于，包括：

采集模块，用于采集当前帧点云数据；

抽取模块，用于从历史点云库中抽取N个历史帧点云数据，所述N个历史帧的采集时间均在所述当前帧的采集时间之前，其中，N为大于或等于1的正整数；

配准模块，用于将所述当前帧点云数据和所述N个历史帧点云数据分别进行点云配准，获得所述当前帧点云数据的多个状态估计值；

叠加模块，用于将所述N个状态估计值进行叠加，得到所述当前帧点云数据的状态估计结果。

7.根据权利要求6所述的设备，其特征在于，

所述状态估计值包括期望值和权重值；其中，所述权重值由所述当前帧点云数据与所述历史帧点云数据的重合程度值确定；所述权重值用线性分布来表示；

所述叠加模块用于将所述N个态估计值进行叠加，得到所述当前帧点云数据的状态估计结果，包括：所述叠加模块用于将所述N个状态估计值的所述期望值基于对应的所述权重值进行加权叠加，得到所述当前帧点云数据的状态估计结果。

8.根据权利要求6或7所述的设备，其特征在于，所述设备还包括检测模块，所述检测模块用于检测无规则噪音；

所述抽取模块用于从历史点云库中抽取N个历史帧点云数据，包括：

在所述检测模块检测到所述无规则噪音的情况下，触发所述抽取模块从所述从历史点云库中抽取N个历史帧点云数据。

9.根据权利要求8所述的设备，其特征在于，所述抽取模块从所述从历史点云库中抽取N个历史帧点云数据，包括：

所述抽取模块用于根据所述无规则噪音确定抽样步长；基于所述抽样步长从所述从历史点云库中抽取N个历史帧点云数据。

10.根据权利要求6或7所述的设备，其特征在于，所述设备还包括检测模块，所述检测模块用于检测到所述当前帧点云数据的采集次序是否符合预设次序；

所述抽取模块用于从历史点云库中抽取N个历史帧点云数据，包括：

在所述检测模块检测到所述当前帧点云数据的采集次序符合预设次序的情况下，触发所述抽取模块从所述从历史点云库中抽取N个历史帧点云数据。

11.一种用于状态感知的设备，其特征在于，包括：处理器，以及耦合至所述处理器的感应器和存储器，其中，所述感应器用于采集当前帧点云数据；所述存储器用于存储历史点云库；所述处理器用于：

从历史点云库中抽取N个历史帧点云数据，所述N个历史帧的采集时间均在所述当前帧的采集时间之前，其中，N为大于或等于1的正整数；

将所述当前帧点云数据和所述N个历史帧点云数据分别进行点云配准，获得所述当前帧点云数据的N个状态估计值；

将所述N个状态估计值进行叠加，得到所述当前帧点云数据的状态估计结果。

12.根据权利要求11所述的设备，其特征在于，

所述状态估计值包括期望值和权重值；其中，所述权重值由所述当前帧点云数据与所述历史帧点云数据的重合程度值确定；所述权重值用线性分布来表示；

所述处理器用于将所述N个态估计值进行叠加，得到所述当前帧点云数据的状态估计结果，包括：将所述N个状态估计值的所述期望值基于对应的所述权重值进行加权叠加，得到所述当前帧点云数据的状态估计结果。

13.一种存储计算机指令的可读非易失性存储介质，其特征在于，包括计算机指令，所述计算机指令被执行以实现权利要求1至5任一项描述的方法。

Images