一种感知特征失效度评估及自适应融合方法和系统

本发明属于移动机器人多传感器融合、状态估计、可靠性感知和智能感知评估，具体涉及一种感知特征失效度评估及自适应融合方法和系统。

背景技术：

1、移动机器人、无人车、巡检机器人、仓储搬运机器人等移动平台通常依赖激光雷达（lidar）、超宽带（uwb）定位、惯性测量单元（imu）、视觉相机、毫米波雷达和轮式里程计等多种传感器实现定位、建图、轨迹跟踪和环境感知。多传感器融合技术能够利用不同传感器之间的互补性提升系统鲁棒性，是移动机器人在复杂环境中稳定运行的重要基础。

2、现有多传感器融合方法通常将传感器观测作为整体输入，通过扩展卡尔曼滤波、因子图优化、滑动窗口优化或神经网络模型对多源数据进行融合。对于传感器退化或失效问题，现有方法多采用残差阈值、协方差调节、鲁棒核函数或传感器级权重调整等方式进行处理。

3、然而，上述现有技术存在以下不足：

4、1、在真实环境中，传感器失效并不总是表现为传感器整体不可用，而往往表现为特征层面的局部失效。例如，在雨雾环境下激光雷达仍能输出点云，但点云中可能存在由雨滴或雾滴引起的近场散射点；在室内遮挡环境下超宽带测距仍连续输出，但可能叠加非视距传播或多径反射引起的正偏差；在颠簸路面上惯性测量单元仍能输出加速度和角速度，但其中可能包含高频机械冲击分量。若融合系统仅以传感器整体为单位进行降权，容易将仍然可靠的特征一并丢弃，或者将已经失效的局部特征继续作为有效约束。

5、2、纯数据驱动的可靠性评估方法虽然能够学习观测数据与异常状态之间的统计关系，但其结果通常缺乏物理可解释性，难以说明异常来源。

6、3、纯物理模型方法虽然具有清晰机理，但难以覆盖真实环境中的复杂噪声、标定偏差和动态变化。

7、因此，如何将物理模型的可解释性与数据驱动方法的适应性结合起来，并在融合前对各类感知特征进行失效度量化，是当前移动机器人多传感器融合技术中亟需解决的问题。

技术实现思路

1、为解决上述现有技术所存在的问题，本发明提供了一种感知特征失效度评估及自适应融合方法和系统，其目的在于，解决现有多传感器融合方法难以对传感器内部不同感知特征的局部失效进行精细量化的问题；解决纯数据驱动方法可解释性不足、纯物理模型方法环境适应性不足的问题等。

2、为实现上述技术目的，本发明采取的技术方案为：

3、一种感知特征失效度评估及自适应融合方法，包含以下步骤：

4、步骤1：获取移动机器人在当前时刻的多源传感器数据，所述多源传感器数据包括激光雷达点云数据、超宽带测距数据、惯性测量数据、视觉数据、毫米波雷达数据和轮式里程计数据中的一种或多种；

5、步骤2：基于当前时刻及历史若干时刻的多源传感器数据，根据各类感知特征的有效更新频率确定对应时间窗口和窗口样本数，并针对至少一类感知特征分别提取实际特征值；

6、步骤3：针对每一类感知特征，基于上一时刻或当前预测状态、传感器标定参数及环境先验，利用与该类感知特征对应的物理模型生成预测特征；

7、步骤4：针对每一类感知特征，根据实际特征值与预测特征之间的偏差，计算物理一致性残差；

8、步骤5：针对每一类感知特征，根据滑动窗口内的历史物理一致性残差的均值和标准差，计算数据驱动异常评分；

9、步骤6：针对每一类感知特征，根据所述物理一致性残差或所述数据驱动异常评分在滑动窗口内的连续超阈情况，计算异常持续性指标；

10、步骤7：针对每一类感知特征，融合所述物理一致性残差、所述数据驱动异常评分和所述异常持续性指标，得到对应的特征失效度；

11、步骤8：根据不同感知特征的特征失效度确定对应的特征可信度，并根据所采用的融合框架，利用所述特征可信度调整观测噪声矩阵、信息矩阵、特征匹配权重或因子权重中的至少一种；将调整后的融合参数用于滤波量测更新、特征关联或加权优化，执行自适应融合并输出移动机器人的位置、姿态、速度中的一种或多种状态估计结果。

12、作为本发明进一步改进的技术方案，步骤2中所述感知特征包括以下特征中的一种或多种：

13、激光雷达点云数据中的边缘特征、平面特征、近场离散点特征和点云匹配残差特征；

14、超宽带测距数据中的测距特征、基站几何一致性特征和接收信号强度特征；

15、惯性测量数据中的加速度特征、角速度特征和高频冲击能量特征；

16、视觉数据中的重投影误差特征、特征跟踪丢失率和光照一致性特征；

17、毫米波雷达数据中的多普勒一致性特征、目标运动残差特征和虚警点比例；

18、轮式里程计数据中的轮速残差和打滑率；

19、其中，每一类感知特征表示从一种传感器数据中提取的、能够单独进行物理预测、异常评分和失效度计算的特征量。

20、作为本发明进一步改进的技术方案，步骤3中预测特征表示为：

21、；

22、其中，为第类感知特征在时刻的物理模型预测特征；为感知特征类别索引；为当前有效观测时刻；上标表示物理模型相关量；为第类感知特征对应的物理观测模型；为根据时刻的融合状态以及运动模型、控制量或惯性测量量外推得到的时刻的先验状态；下标表示基于时刻及其之前的信息对时刻进行预测；为第类感知特征对应的传感器内参、外参或物理模型参数；为时刻的环境先验或地图先验，包括局部地图几何基元、超宽带基站坐标、相机内参、道路约束和目标运动约束中的一种或多种。

23、作为本发明进一步改进的技术方案，步骤4中所述物理一致性残差表示为：

24、；

25、其中，为第类感知特征在时刻的物理一致性残差；为实际特征值；为对应的预测特征；表示二范数；为第类感知特征的残差归一化尺度，根据该类感知特征在正常工况下的历史残差统计量确定；为防止分母为零的预设极小正数；表示将计算结果限制在闭区间内。

26、作为本发明进一步改进的技术方案，步骤5中的数据驱动异常评分表示为：

27、；

28、其中，为第类感知特征在时刻的数据驱动异常评分；上标表示数据驱动相关量；为不包含当前物理一致性残差的历史滑动窗口内，第类感知特征的物理一致性残差均值；为所述历史滑动窗口内物理一致性残差的标准差，上标表示该标准差针对物理一致性残差计算；表示绝对值；

29、在初始阶段，当历史有效样本数少于三个时，将数据驱动异常评分设置为预设初始值；当历史有效样本数达到三个但滑动窗口尚未填满时，使用当前已有的全部历史有效样本计算所述均值和标准差。

30、作为本发明进一步改进的技术方案，步骤6中的异常持续性指标表示为：

31、；

32、；

33、；

34、其中，为第类感知特征在时刻的异常指示量；为示性函数，括号内条件成立时取1，否则取0；表示逻辑“或”；和分别为第类感知特征的数据驱动异常评分阈值和物理一致性残差阈值；为截至时刻连续出现异常的有效样本数，当前异常指示量为0时，归零；为上一有效观测时刻的连续异常样本数；为第类感知特征在时刻的滑动窗口样本数；为异常持续性指标。

35、作为本发明进一步改进的技术方案，步骤7中的特征失效度表示为：

36、；

37、并满足：；

38、其中，为第类感知特征在时刻的特征失效度；分别为第类感知特征的物理一致性残差权重、数据驱动异常评分权重和异常持续性指标权重；不同感知特征对应的所述权重允许不同，所述权重根据正常样本和异常本的标定结果预先确定，并在一次状态估计过程中保持不变。

39、作为本发明进一步改进的技术方案，步骤8中的特征可信度表示为：

40、；

41、根据所述特征可信度调整观测噪声矩阵、信息矩阵或者特征匹配权重或因子权重，分别表示为：

42、；

43、；

44、；

45、其中，为第类感知特征在时刻的特征可信度；和分别为第类感知特征的基准观测噪声协方差矩阵和调整后的观测噪声协方差矩阵；和分别为基准信息矩阵和调整后的信息矩阵；和分别为基准特征匹配权重或基准因子权重以及调整后的特征匹配权重或因子权重；为预设可信度下限；表示取括号内各数值中的最大值；

46、当采用滤波融合框架时使用进行量测更新；当采用因子图或滑动窗口优化框架时使用或进行残差加权；当采用特征匹配框架时使用进行特征关联或匹配筛选。

47、作为本发明进一步改进的技术方案，当所述多源传感器数据包括激光雷达点云数据时，激光雷达感知特征的物理一致性残差表示为：

48、；

49、并满足：；

50、其中，为时刻的激光雷达物理一致性残差；下标表示激光雷达感知特征；为预设近场范围内满足孤立性条件或反射强度异常条件的点数与近场点总数的比值；为当前点云与局部地图中的平面或边缘几何基元之间的归一化平均距离；为邻域几何连续性不满足预设条件的点数与参与连续性判断的点总数的比值；为对应的非负权重，且三者之和为1；

51、当所述多源传感器数据包括超宽带测距数据时，超宽带测距感知特征的物理一致性残差表示为：

52、；

53、并满足：；

54、其中，为时刻针对第个超宽带基站的物理一致性残差；下标uwb表示超宽带测距感知特征；为超宽带基站索引；为实测距离；为根据机器人先验位置和第个基站坐标计算的预测几何距离；为实测接收信号强度；为根据预测几何距离和预设路径损耗模型得到的预测接收信号强度；为正常工况下的距离残差尺度，为正常工况下的接收信号强度残差尺度；为根据首径功率、总接收功率、信道脉冲响应或多基站几何一致性中的一种或多种确定的非视距或多径指示量；为对应的非负权重，且三者之和为1；

55、当所述多源传感器数据包括惯性测量数据时，计算高频冲击能量：

56、；

57、并计算惯性测量感知特征的物理一致性残差：

58、；

59、其中，为时刻对应统计时间窗内的高频冲击能量；为惯性测量数据的统计时间窗长度；为积分变量；为对三轴加速度数据进行带通滤波后得到的加速度向量；下标表示带通滤波；为根据正常行驶数据的高频冲击能量统计量确定的正常冲击能量尺度；为时刻的惯性测量的物理一致性残差；

60、当所述多源传感器数据包括视觉数据时，计算平均重投影误差及特征点丢失率；

61、平均重投影误差表示为：

62、；

63、特征点丢失率表示为：

64、；

65、则视觉感知特征的物理一致性残差表示为：

66、；

67、并满足：；

68、其中，为当前时刻；为当前时刻参与重投影误差计算的视觉特征点索引；为当前时刻参与重投影误差计算的有效视觉特征点数量，且；为第个视觉特征点在当前图像中的二维像素观测向量；为根据预测位姿、相机内外参数及对应场景三维点投影得到的二维像素预测向量；符号表示预测量；表示向量的二范数；为当前时刻的平均重投影误差；为当前时间窗口内跟踪丢失的视觉特征点数量；为所述时间窗口起始时参与跟踪的视觉特征点数量；为取最大值函数，用于避免分母为零；为特征点丢失率；为视觉感知特征的物理一致性残差；上标表示物理一致性量，下标表示视觉感知特征；为正常工况下视觉重投影误差的尺度参数；为防止分母为零的正数；为平均重投影误差项的非负权重，为特征点丢失率项的非负权重；表示将输入限制在区间内；

69、当所述多源传感器数据包括毫米波雷达数据时，计算归一化多普勒速度残差和虚警率；

70、归一化多普勒速度残差表示为：

71、；

72、虚警率表示为：

73、；

74、则毫米波雷达感知特征的物理一致性残差表示为：

75、

76、并满足：；

77、其中，为当前时刻参与计算的毫米波雷达有效目标索引；为当前时刻具有有效多普勒速度观测的目标数量，且；为第个目标的径向速度观测值；上标表示传感器实测量；为根据预测运动状态以及目标与毫米波雷达之间的视线方向计算得到的径向速度预测值；表示取绝对值；为正常工况下多普勒速度残差的尺度参数；为归一化多普勒速度残差；为当前时间窗口内经多帧关联、可达域约束或其他确定性规则判定为虚警的毫米波雷达目标数量；为所述时间窗口内毫米波雷达输出的目标总数；为虚警率；为毫米波雷达感知特征的物理一致性残差；下标表示毫米波雷达感知特征，下标表示多普勒速度相关量；为归一化多普勒速度残差项的非负权重，为虚警率项的非负权重；

78、当所述多源传感器数据包括轮式里程计数据时，计算轮速里程计线速度、轮速里程计横摆角速度和滑移指标；

79、轮速里程计线速度表示为：

80、；

81、轮速里程计横摆角速度表示为：

82、；

83、滑移指标表示为：

84、；

85、则轮式里程计感知特征的物理一致性残差表示为：

86、

87、并满足：；

88、其中，为当前时刻由轮速计算得到的车辆纵向线速度；为当前时刻由轮速计算得到的车辆横摆角速度；为车轮有效滚动半径；为左右驱动轮接地点之间的轮距；和分别为右驱动轮的角速度和左驱动轮的角速度；为根据上一时刻融合状态、控制输入和车辆运动学模型预测得到的当前时刻车辆纵向速度；为根据所述车辆运动学模型预测得到的当前时刻横摆角速度；下标表示车辆纵向方向，下标表示车辆竖直轴方向；为预设的正速度下限，用于避免车辆低速或静止时滑移指标的分母过小；为滑移指标；和分别为正常工况下线速度残差和横摆角速度残差的尺度参数；为轮式里程计感知特征的物理一致性残差；下标表示轮式里程计感知特征；为线速度残差项的非负权重，为横摆角速度残差项的非负权重，为滑移指标项的非负权重。

89、为实现上述技术目的，本发明采取的另一个技术方案为：

90、一种感知特征失效度评估及自适应融合系统，包括数据采集模块、特征提取模块、物理预测模块、物理残差计算模块、数据驱动异常评分模块、持续性评估模块、特征失效度计算模块和自适应融合模块；其中：

91、数据采集模块，用于获取移动机器人在当前时刻的多源传感器数据；

92、特征提取模块，用于基于当前时刻及历史若干时刻的多源传感器数据，根据各类感知特征的有效更新频率确定对应时间窗口和窗口样本数，并针对至少一类感知特征分别提取实际特征值；

93、物理预测模块用于针对每一类感知特征，基于上一时刻或当前预测状态、传感器标定参数及环境先验，利用与该类感知特征对应的物理模型生成预测特征；

94、物理残差计算模块，用于针对每一类感知特征，根据实际特征值与预测特征之间的偏差，计算物理一致性残差；

95、数据驱动异常评分模块，用于针对每一类感知特征，根据滑动窗口内的历史物理一致性残差的均值和标准差，计算数据驱动异常评分；

96、持续性评估模块，用于针对每一类感知特征，根据所述物理一致性残差或所述数据驱动异常评分在滑动窗口内的连续超阈情况，计算异常持续性指标；

97、特征失效度计算模块，用于针对每一类感知特征，融合所述物理一致性残差、所述数据驱动异常评分和所述异常持续性指标，得到对应的特征失效度；

98、自适应融合模块；用于根据不同感知特征的特征失效度确定对应的特征可信度，并根据所采用的融合框架，利用所述特征可信度调整观测噪声矩阵、信息矩阵、特征匹配权重或因子权重中的至少一种；将调整后的融合参数用于滤波量测更新、特征关联或加权优化，执行自适应融合并输出移动机器人的位置、姿态、速度中的一种或多种状态估计结果。

99、与现有技术相比，本发明的有益效果为：

100、第一，本发明以感知特征为失效度评估对象，能够区分同一传感器内部不同特征的可靠性，避免因局部失效而整体丢弃有效传感器信息。针对背景技术中指出的“现有方法以传感器整体为评估对象，无法识别局部失效”的问题，本发明将传感器数据分解为边缘特征、平面特征、测距特征、加速度特征等细粒度感知特征，分别计算各自的特征失效度。当一个传感器内部部分特征失效而其他特征仍然可靠时（如雨雾环境下激光雷达近场散射点异常但平面特征仍然可用），本发明能够对失效特征进行降权而对可靠特征保持正常约束，实现特征层面的精细化失效管理，避免了将整个传感器从融合系统中剔除的粗放处理方式。解决了现有多传感器融合方法难以对传感器内部不同感知特征的局部失效进行精细量化的问题。

101、第二，本发明将物理模型引入特征失效度评估过程，能够解释特征异常产生的物理原因，提高融合调权的可解释性。针对背景技术中指出的“纯数据驱动方法可解释性不足”的问题，本发明步骤3通过物理观测模型生成预测特征，步骤4计算实际特征与预测特征之间的物理一致性残差，使特征失效度的来源具有明确的物理含义。当某一感知特征的物理一致性残差升高时，可以追溯至对应的物理模型参数（传感器内参、外参）或环境先验（地图几何基元、基站坐标），从而判断异常是源于传感器标定偏差、环境变化还是传感器本身故障。这一机制使得融合系统的调权决策可追溯、可解释，有利于安全性验证和故障溯源。解决了解决纯数据驱动方法可解释性不足的问题。

102、第三，本发明利用滑动窗口数据驱动异常评分适应不同传感器、不同机器人平台和不同环境噪声条件，提高对复杂环境的适应性。针对背景技术中指出的“纯物理模型方法环境适应性不足”的问题，本发明步骤5基于滑动窗口内的历史物理一致性残差统计分布计算数据驱动异常评分。该评分不依赖预设的固定阈值或固定的噪声模型，而是根据近期历史数据的均值和标准差动态调整异常判据。当机器人进入新的环境条件（如从室内进入室外、从晴天进入雨雾天气）导致传感器噪声特性发生变化时，滑动窗口内的统计特征会随之更新，数据驱动异常评分自动适应新的噪声水平，使得失效度评估在不同环境和工况下保持有效性。解决了纯物理模型方法环境适应性不足的问题。

103、第四，本发明将特征失效度直接作用于观测噪声矩阵、信息矩阵、特征匹配权重或因子权重，使异常识别、异常解释、失效度量化和自适应融合形成闭环。本发明步骤8建立了从特征失效度到特征可信度再到融合参数的完整映射链路。特征失效度越高，对应的观测噪声协方差矩阵越大（滤波框架）、信息矩阵或因子权重越小（优化框架）、特征匹配权重越低（匹配框架），从而在融合层面对失效特征进行自适应抑制。同时，当异常消失后，特征失效度自动降低，融合参数自动恢复，实现了无需人工干预的自适应闭环调节。所述状态估计结果可用于定位、建图、导航、轨迹跟踪或运动控制。解决了现有融合方法缺少从异常识别、异常解释、失效度量化到融合调权的闭环机制的问题。