一种基于位置自适应权重与时间衰减的多传感器融合系统的制作方法

本发明涉及多传感器融合，尤其是涉及一种基于位置自适应权重与时间衰减的多传感器融合系统。

背景技术：

1、在智能驾驶多传感器后融合系统中，当一帧新传感器量测与已有航迹完成关联后，需要将该量测与航迹的历史信息融合，更新航迹的运动状态（位置、速度）与存在可信度。运动状态融合的质量决定了目标位置与速度的精度，存在可信度融合的质量决定了航迹是否应被发布给下游决策功能。

2、现有主流后融合系统在进行运动状态融合与存在可信度融合时，多采用以下方案：

3、方案一：固定权重加权融合。 对所有传感器、所有空间位置采用同一组固定权重做加权平均，不区分传感器类型、不区分目标所处的空间位置区域。

4、方案二：单一卡尔曼滤波融合。 直接用单一卡尔曼滤波器融合所有量测，不区分传感器对与空间位置，不做时间错位补偿。

5、现有技术方案中，未建模同一传感器在不同空间位置的可信度差异： 相机横向位置在近距可信、远距不可信；雷达纵向速度在中远距可信、横向位置不可信。固定权重无法适配，导致融合后位置/速度精度下降。原因是固定权重未按目标空间位置做差异化配置。

6、未处理多传感器量测的时间错位：不同传感器量测时刻不同，直接加权会把旧状态与新状态混合，引入系统性漂移。原因是未对历史量测做时间外推补偿或时间衰减。

7、如何解决现有技术中空间位置可信度差异未利用、时间错位漂移、存在可信度评估因子单一缺乏闭环、嵌入式落地难以及运动状态与存在可信度割裂的问题，成为我们亟待解决的问题。

技术实现思路

1、鉴于以上问题，本发明提供了一种基于位置自适应权重与时间衰减的多传感器融合系统，不仅通过位置自适应权重与时间衰减因子的两级平滑融合，结合存在概率两阶段闭环演化，实现高精度运动状态估计与高鲁棒航迹存在性判定，而且避免了固定权重导致的精度损失，近距目标横向位置精度与远距目标纵向速度精度均有提升。

2、为了实现上述目的及其他相关目的，本发明提供的技术方案如下：

3、一种基于位置自适应权重与时间衰减的多传感器融合系统，所述系统包括：

4、位置自适应权重计算子模块，用于对每种传感器类型，按目标纵向/横向位置，以若干标定点经分段线性插值计算各维度的权重；

5、时间时间衰减与外推补偿子模块，与所述位置自适应权重计算子模块连接，用于根据参考量测相对融合时刻的时间差计算时间衰减因子，并以恒速模型把历史量测状态外推到融合时刻；

6、多维加权平均子模块，与所述时间衰减与外推补偿子模块连接，用于对所有参考传感器量测与当前量测，以“位置自适应权重与时间衰减因子之积”为权重，对位置与速度做加权平均；

7、卡尔曼滤波平滑子模块，与所述多维加权平均子模块连接，用于对加权平均得到的融合前状态，以恒速状态转移矩阵f做卡尔曼预测与更新；

8、第一阶段存在概率评估子模块，与所述卡尔曼滤波平滑子模块连接，用于在航迹状态更新前，依据新量测的传感器类型、是否主传感器、量测年龄、新量测时刻与上一融合时刻的时差，计算初始存在概率；

9、第二阶段存在概率确认子模块，与所述第一阶段存在概率评估子模块连接，用于在航迹状态更新后，以参考传感器确认数与被跟踪次数二次确认。

10、进一步地，所述系统还包括：

11、存在概率阀值门控子模块，与所述第二阶段存在概率确认子模块连接，用于仅发布存在概率高于阈值的有效航迹，过滤不稳定目标。

12、进一步地，所述系统还包括：

13、航迹状态更新模块，与所述存在概率阀值门控子模块连接，用于将新量测纳入参考传感器集合，按各传感器类型的不可见周期淘汰过期参考量测，更新航迹时间戳、被跟踪次数和年龄。

14、进一步地，所述以若干标定点经分段线性插值计算各维度的权重包括：

15、所述标定点为(p0,ω0)、(p1,ω1)、(p2,ω2)和(p3,ω3)，目标纵向位置为x，若x

16、，，

17、，x≧p1，

18、其中，分段线性插值函数分三段定义，

19、，

20、，

21、，

22、其中，(pa,ωa)、(pb,ωb)为标定点。

23、进一步地，所述时间衰减因子的定义为，

24、，

25、∆tr=tf-fr，

26、所述外推补偿子模块的时间补偿外推函数的定位为，

27、，

28、其中，为横向补偿位置，为纵向补偿位置，xr为横向位置，yr为纵向位置，vx,r为目标横向速度，yy,r为目标纵向速度。

29、进一步地，所述对所有参考传感器量测与当前量测包括：

30、，

31、，

32、其中，ω0,x为分段插值，x0为初始目标纵向位置，为第r个分段插值，为横向补偿位置，λr为时间衰减因子，σx,0为初始速度，σx,r为r时刻的速度。

33、进一步地，所述恒速状态转移矩阵f为，

34、，

35、其中，h为观测矩阵，t为转置，p为预测误差协方差矩阵，r为观测噪音协方差矩阵。

36、进一步地，所述初始存在概率e(1)为，

37、，

38、其中，为新量测的传感器类型，δmain为是否主传感器，g(a0)为量测年龄，h(tf-tprov)为新量测时刻与上一融合时刻的时差。

39、进一步地，所述以参考传感器确认数与被跟踪次数二次确认为，

40、，

41、其中，nrot为参考传感器确认数，ntrk为被跟踪次数，与为权重系数。

42、进一步地，所述各维度的权重包括纵/横位置、纵/横向速度的权重。

43、本发明具有以下积极效果：

44、本发明通过在运动状态融合中，按传感器类型与目标空间位置用分段线性插值计算位置自适应权重，结合时间衰减因子与时间补偿外推，对所有参考传感器量测与当前量测做加权平均后再经卡尔曼滤波平滑；在存在可信度融合中，采用两阶段闭环——在航迹状态更新前以“传感器类型/主传感器/量测年龄/更新时差”评估初始存在概率，在航迹状态更新后以“多传感器同时确认/被跟踪次数”二次确认——并以存在概率阀值作为发布门控，旨在通过位置自适应权重与时间衰减因子的两级平滑融合，结合存在概率两阶段闭环演化，实现高精度运动状态估计与高鲁棒航迹存在性判定。

1.一种基于位置自适应权重与时间衰减的多传感器融合系统，其特征在于，所述系统包括：

2.根据权利要求1所述的基于位置自适应权重与时间衰减的多传感器融合系统，其特征在于，所述系统还包括：

3.根据权利要求1所述的基于位置自适应权重与时间衰减的多传感器融合系统，所述系统还包括：

4.根据权利要求1所述的基于位置自适应权重与时间衰减的多传感器融合系统，其特征在于，所述以若干标定点经分段线性插值计算各维度的权重包括：

5.根据权利要求1所述的基于位置自适应权重与时间衰减的多传感器融合系统，其特征在于：所述时间衰减因子的定义为，

6.根据权利要求1所述的基于位置自适应权重与时间衰减的多传感器融合系统，其特征在于，所述对所有参考传感器量测与当前量测包括：

7.根据权利要求1所述的基于位置自适应权重与时间衰减的多传感器融合系统，其特征在于：所述恒速状态转移矩阵f为，

8.根据权利要求1所述的基于位置自适应权重与时间衰减的多传感器融合系统，其特征在于：所述初始存在概率e(1)为，

9.根据权利要求1所述的基于位置自适应权重与时间衰减的多传感器融合系统，其特征在于，所述以参考传感器确认数与被跟踪次数二次确认为，

10.根据权利要求1所述的基于位置自适应权重与时间衰减的多传感器融合系统，其特征在于：所述各维度的权重包括纵/横位置、纵/横向速度的权重。