CN111881955B - 多源传感器信息融合方法及装置 - Google Patents

多源传感器信息融合方法及装置 Download PDF

Info

Publication number
CN111881955B
CN111881955B CN202010679789.3A CN202010679789A CN111881955B CN 111881955 B CN111881955 B CN 111881955B CN 202010679789 A CN202010679789 A CN 202010679789A CN 111881955 B CN111881955 B CN 111881955B
Authority
CN
China
Prior art keywords
moment
value
covariance matrix
measurement
state quantity
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202010679789.3A
Other languages
English (en)
Other versions
CN111881955A (zh
Inventor
张斯怡
万国强
郭旭东
李一冰
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing Jingwei Hirain Tech Co Ltd
Original Assignee
Beijing Jingwei Hirain Tech Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Beijing Jingwei Hirain Tech Co Ltd filed Critical Beijing Jingwei Hirain Tech Co Ltd
Priority to CN202010679789.3A priority Critical patent/CN111881955B/zh
Publication of CN111881955A publication Critical patent/CN111881955A/zh
Application granted granted Critical
Publication of CN111881955B publication Critical patent/CN111881955B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/25—Fusion techniques
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/86—Combinations of radar systems with non-radar systems, e.g. sonar, direction finder
    • G01S13/865—Combination of radar systems with lidar systems
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/86—Combinations of radar systems with non-radar systems, e.g. sonar, direction finder
    • G01S13/867—Combination of radar systems with cameras
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/25—Fusion techniques
    • G06F18/251—Fusion techniques of input or preprocessed data

Landscapes

  • Engineering & Computer Science (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Data Mining & Analysis (AREA)
  • Theoretical Computer Science (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Evolutionary Computation (AREA)
  • General Engineering & Computer Science (AREA)
  • Evolutionary Biology (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Artificial Intelligence (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Electromagnetism (AREA)
  • Traffic Control Systems (AREA)
  • Radar Systems Or Details Thereof (AREA)

Abstract

本发明的实施例提供了一种多源传感器信息融合方法及装置，方法包括：利用预先确定的测量值与测量误差的对应关系，对多源传感器融合后的测量值进行修正，在使用卡尔曼滤波处理时，通过融合状态量的预测值和修正后的测量值，得到状态量的估计值。相对于传统的只考虑高斯噪声的方法，本发明还考虑了测量误差，从而能更好地降低噪声，进而提高多源传感器检测精度。还可以将状态量的预测值减去修正后的测量值，得到状态量的非高斯噪声值，并在非高斯噪声值大于预设阈值时，对多源传感器在量测协方差矩阵进行增大修正，以使在卡尔曼滤波计算状态量和误差协方差矩阵的估计值时，利用修正后的量测协方差矩阵进行计算，进一步提高多源传感器检测精度。

Description

多源传感器信息融合方法及装置
技术领域
本发明涉及数据处理领域，更具体地说，涉及多源传感器信息融合方法及装置。
背景技术
多源传感器融合是指将来自多个信息源的数据进行综合处理，从而得出比单一传感器更为准确的结论。例如车辆中使用摄像头、毫米波雷达和激光雷达等的信号，来实现的目标的识别；每种传感器都因识别的原理不同，会有不同的性能限制，而进行多源传感器信息融合，可以取长补短，提高目标的识别准确性。发明人发现现有的多源传感器信息融合方案仅考虑了高斯噪声，而忽视了其它种类噪声的影响，导致最终融合后的检测不准确。
发明内容
有鉴于此，本发明提出一种多源传感器信息融合方法及装置，欲实现提高检测精度的目的。
为了实现上述目的，现提出的方案如下：
第一方面，提供一种多源传感器信息融合方法，包括：
根据状态量在第一时刻的估计值，计算得到所述状态量在第二时刻的预测值，第二时刻为第一时刻的后一时刻；
根据误差协方差矩阵在第一时刻的估计值，计算得到所述误差协方差矩阵在第二时刻的预测值；
根据预先确定的测量值与测量误差的对应关系，对多源传感器在第二时刻融合后的测量值进行修正，得到在第二时刻修正后的测量值；
根据所述多源传感器的量测协方差矩阵和所述误差协方差矩阵在第二时刻的预测值，计算得到在第二时刻的卡尔曼增益矩阵；
根据在第二时刻的卡尔曼增益矩阵和所述误差协方差矩阵在第二时刻的预测值，计算得到所述误差协方差矩阵在第二时刻的估计值；
根据在第二时刻的卡尔曼增益矩阵、在第二时刻修正后的测量值以及所述状态量在第一时刻的估计值，计算得到所述状态量在第二时刻的估计值。
可选的，在所述得到在第二时刻修正后的测量值的步骤之后，还包括：
将所述状态量在第二时刻的预测值减去所述在第二时刻修正后的测量值，得到所述状态量在第二时刻的非高斯噪声值；
当所述状态量在第二时刻的非高斯噪声值大于预设阈值时，对所述多源传感器在第一时刻的量测协方差矩阵进行增大修正，得到在第二时刻的量测协方差矩阵；
根据所述多源传感器的量测协方差矩阵和所述误差协方差矩阵在第二时刻的预测值，计算得到在第二时刻的卡尔曼增益矩阵，具体为：
根据所述多源传感器的在第二时刻的量测协方差矩阵和所述误差协方差矩阵在第二时刻的预测值，计算得到在第二时刻的卡尔曼增益矩阵。
可选的，所述多源传感器，包括：摄像头、毫米波雷达和激光雷达中的至少两个。
可选的，所述状态量，包括：障碍物相对于本车的位置、速度和/或加速度。
可选的，所述预先确定的测量值与测量误差的对应关系，具体为：不同的测量值范围分别对应一个测量误差。
第二方面，提供一种多源传感器信息融合装置，包括：
状态量预测单元，用于根据状态量在第一时刻的估计值，计算得到所述状态量在第二时刻的预测值，第二时刻为第一时刻的后一时刻；
误差协方差矩阵预测单元，用于根据误差协方差矩阵在第一时刻的估计值，计算得到所述误差协方差矩阵在第二时刻的预测值；
测量值修正单元，用于根据预先确定的测量值与测量误差的对应关系，对多源传感器在第二时刻融合后的测量值进行修正，得到在第二时刻修正后的测量值；
增益矩阵计算单元，用于根据所述多源传感器的量测协方差矩阵和所述误差协方差矩阵在第二时刻的预测值，计算得到在第二时刻的卡尔曼增益矩阵；
误差协方差矩阵估计单元，用于根据在第二时刻的卡尔曼增益矩阵和所述误差协方差矩阵在第二时刻的预测值，计算得到所述误差协方差矩阵在第二时刻的估计值；
状态量估计单元，用于根据在第二时刻的卡尔曼增益矩阵、在第二时刻修正后的测量值以及所述状态量在第一时刻的估计值，计算得到所述状态量在第二时刻的估计值。
可选的，所述多源传感器信息融合装置，还包括：
非高斯噪声计算单元，用于在执行所述测量值修正单元之后，将所述状态量在第二时刻的预测值减去所述在第二时刻修正后的测量值，得到所述状态量在第二时刻的非高斯噪声值；
量测协方差矩阵修正单元，用于当所述状态量在第二时刻的非高斯噪声值大于预设阈值时，对所述多源传感器在第一时刻的量测协方差矩阵进行增大修正，得到在第二时刻的量测协方差矩阵；
所述增益矩阵计算单元，具体用于根据所述多源传感器的在第二时刻的量测协方差矩阵和所述误差协方差矩阵在第二时刻的预测值，计算得到在第二时刻的卡尔曼增益矩阵。
可选的，所述多源传感器包括：摄像头、毫米波雷达和激光雷达中的至少两个。
可选的，所述状态量包括：障碍物相对于本车的位置、速度和/或加速度。
可选的，所述预先确定的测量值与测量误差的对应关系，具体为：不同的测量值范围分别对应一个测量误差。
与现有技术相比，本发明的技术方案具有以下优点：
上述技术方案提供的一种多源传感器信息融合方法及装置，方法包括：利用预先确定的测量值与测量误差的对应关系，对多源传感器融合后的测量值进行修正，在使用卡尔曼滤波处理时，通过融合状态量的预测值和修正后的测量值，得到状态量的估计值。相对于传统的只考虑高斯噪声的方法，本发明还考虑了测量误差，从而能更好地降低噪声，进而提高了多源传感器检测精度。
进一步的，将状态量的预测值减去修正后的测量值，得到状态量的非高斯噪声值，并在非高斯噪声值大于预设阈值时，对多源传感器在量测协方差矩阵进行增大修正，以使在卡尔曼滤波计算状态量和误差协方差矩阵的估计值时，利用修正后的量测协方差矩阵进行计算。由于又考虑非高斯噪声，因此，进一步降低了噪声，进一步提高了多源传感器检测精度。
当然，实施本发明的任一产品并不一定需要同时达到以上所述的所有优点。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据提供的附图获得其他的附图。
图1为本发明实施例提供的一种多源传感器信息融合方法的流程图；
图2为本发明实施例提供的另一种多源传感器信息融合方法的流程图；
图3为本发明实施例提供的一种根据测量距离和角度划分不同区域的示意图；
图4为本发明实施例提供的一种多源传感器信息融合装置的示意图；
图5为本发明实施例提供的一种控制器的示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
参见图1，为本发明实施例提供的一种多源传感器信息融合方法，该方法可以包括以下步骤：
S11：根据状态量在第一时刻的估计值，计算得到状态量在第二时刻的预测值。
状态量为多源传感器要检测的参数。当多源传感器为用于目标检测的摄像头、毫米波雷达和激光雷达中的至少两个时，状态量为障碍物信息，具体可以包括障碍物相对于本车的位置、速度和/或加速度等，障碍物信息还可以包括障碍物的宽度和类型等。
由状态量在第一时刻的估计值，计算状态量在第二时刻的预测值时，根据下述公式计算：
Figure BDA0002585356400000051
其中，
Figure BDA0002585356400000052
为状态量在第二时刻的预测值，/>
Figure BDA0002585356400000053
为状态量在第一时刻的估计值，t为第一时刻，t+1为第二时刻，f为运动方程。
第二时刻为第一时刻的后一时刻，即第一时刻是第二时刻的前一时刻。状态量在初始时刻的预测值使用测量值，之后，都是利用上述公式根据状态量在前一时刻的估计值，计算得到当前时刻的预测值。f为假定状态量为某个变化趋势而确定的方程。
S12：根据误差协方差矩阵在第一时刻的估计值，计算得到误差协方差矩阵在第二时刻的预测值。
由误差协方差矩阵在第一时刻的估计值，计算误差协方差矩阵在第二时刻的预测值时，根据下述公式计算：
Pt+1|t＝F·Pt|tF·T+Q
其中，Pt+1|t为误差协方差矩阵在第二时刻的预测值，Pt|t为误差协方差矩阵在第一时刻的估计值，F为运动方程f的雅克比矩阵，FT为F的转置，Q为预测协方差矩阵。预测协方差矩阵是由于计算状态量的预测值时，假定状态量为某个变化趋势但是实际并不是而造成的，可以根据现有的理论计算方法或者根据实际测试数据统计得到该预测协方差矩阵。预测协方差矩阵、误差协方差矩阵以及状态量均为同阶矩阵。误差协方差矩阵在初始时刻的预测值，预先设定为对角线上元素均为1的对角阵。
S13：根据预先确定的测量值与测量误差的对应关系，对多源传感器在第二时刻融合后的测量值进行修正，得到在第二时刻修正后的测量值。
预先确定的测量值与测量误差的对应关系，可以是一个线性关系，具体可以测试得到多个不同测量值对应的测量误差，进而通过最小二乘法或其他拟算法，得到测量值和测量误差的对应关系。
预先确定的测量值与测量误差的对应关系，还可以是不同的测量值范围分别对应一个测量误差。具体的，按照划分不同的测量值范围，统计不同测量值范围多源传感器融合后测量值的误差均值，并建立测量值范围与误差均值的对应关系，该误差均值即为测量误差。
S14：根据多源传感器的量测协方差矩阵和误差协方差矩阵在第二时刻的预测值，计算得到在第二时刻的卡尔曼增益矩阵。
计算第二时刻的卡尔曼增益矩阵的公式如下：
Kt+1＝Pt+1|t·HT·(H·Pt+1|t·HT+R)-1
其中，Kt+1为第二时刻的卡尔曼矩阵增益，H为量测转换矩阵，HT为H的转置，R为多源传感器的量测协方差矩阵。传感器的检测本身就会有误差，一般认为这种误差服从均值为0，测量各个状态量时的误差组成矩阵就是量测协方差矩阵。用于将状态量转为测量值的矩阵即为量测转换矩阵H。
S15：根据在第二时刻的卡尔曼增益矩阵和误差协方差矩阵在第二时刻的预测值，计算得到误差协方差矩阵在第二时刻的估计值。
计算误差协方差矩阵在第二时刻的估计值的公式如下：
Figure BDA0002585356400000061
其中，
Figure BDA0002585356400000062
表示第二时刻修正后的测量值。
S16：根据在第二时刻的卡尔曼增益矩阵、在第二时刻修正后的测量值以及状态量在第一时刻的估计值，计算得到状态量在第二时刻的估计值。
计算状态量在第二时刻的估计值的公式为Pt+1|t+1＝Pt+1|t-Kt+1·H·Pt+1|t。
本实施例提供的上述多源传感器信息融合方法，利用预先确定的测量值与测量误差的对应关系，对多源传感器融合后的测量值进行修正，在使用卡尔曼滤波处理时，通过融合状态量的预测值和修正后的测量值，得到状态量的估计值。相对于传统的只考虑高斯噪声的方法，还考虑了测量误差，从而能更好地降低噪声，进而提高了多源传感器检测精度。
参见图2，为本发明实施例提供的另一种多源传感器信息融合方法，该方法可以包括以下步骤：
S21：根据状态量在第一时刻的估计值，计算得到状态量在第二时刻的预测值。
S22：根据误差协方差矩阵在第一时刻的估计值，计算得到误差协方差矩阵在第二时刻的预测值。
S23：根据预先确定的测量值与测量误差的对应关系，对多源传感器在第二时刻融合后的测量值进行修正，得到在第二时刻修正后的测量值。
步骤S21～S23分别与步骤S11～S13一致，本实施例不再赘述。
S24：将状态量在第二时刻的预测值减去在第二时刻修正后的测量值，得到状态量在第二时刻的非高斯噪声值。
S25：当状态量在第二时刻的非高斯噪声值大于预设阈值时，对多源传感器在第一时刻的量测协方差矩阵进行增大修正，得到在第二时刻的量测协方差矩阵。
当状态量在第二时刻的非高斯噪声值不大于预设阈值时，不对多源传感器在第一时刻的量测协方差矩阵进行修正，即保持原来量测协方差矩阵不变，作为第二时刻的量测协方差矩阵。多源传感器在初始时刻的量测协方差矩阵即为R。本实施例提供的多源传感器信息融合方法相对于图1公开的方法的主要区别就是，考虑了非高斯噪声值对量测协方差矩阵的影响。增大量测协方差矩阵即调整该矩阵对角线上的元素。预先通过标定方式得到各个元素的调整值。
S26：根据多源传感器的在第二时刻的量测协方差矩阵和所述误差协方差矩阵在第二时刻的预测值，计算得到在第二时刻的卡尔曼增益矩阵。
执行步骤S26时，计算第二时刻的卡尔曼增益矩阵的公式如下：
Kt+1＝Pt+1|t·HT·(H·Pt+1|t·HT+Rt+1)-1
相比于执行步骤S16利用的公式，区别在于将R修改为了Rt+1，Rt+1表示多源传感器在第二时刻的量测协方差矩阵。
S27：根据在第二时刻的卡尔曼增益矩阵和误差协方差矩阵在第二时刻的预测值，计算得到误差协方差矩阵在第二时刻的估计值。
S28：根据在第二时刻的卡尔曼增益矩阵、在第二时刻修正后的测量值以及状态量在第一时刻的估计值，计算得到状态量在第二时刻的估计值。
本实施例提供的多源传感器信息融合方法，将状态量的预测值减去修正后的测量值，得到状态量的非高斯噪声值，并在非高斯噪声值大于预设阈值时，对多源传感器在量测协方差矩阵进行增大修正，以使在卡尔曼滤波计算状态量和误差协方差矩阵的估计值，利用修正后的量测协方差矩阵进行计算。相比于图1公开的方法，由于又考虑非高斯噪声，因此，进一步降低了噪声，进一步提高了多源传感器检测精度。
下面举例说明本发明提供的多源传感器信息融合方法，多源传感器包括摄像头、毫米波雷达和激光雷达中的至少两个，状态量为障碍物相对于本车的距离和速度，传感器只能观测到障碍物相对于本车的距离。
测量值为Z＝[d]，状态量为：
Figure BDA0002585356400000081
其中，Z为测量值，X为状态量，d为障碍物相对于本车的距离，v为障碍物相对于本车的速度。则t时刻的状态量的估计值为：/>
Figure BDA0002585356400000082
初始时刻，状态量的估计值取值测量值，即在t时刻为初始时刻时，状态量的估计值为：
Figure BDA0002585356400000083
状态量和测量值之间的关系为测量值为Z＝H·X，H＝[1 0]。
假设物体做匀速直线运动，则根据状态量在t时刻的估计值，预测的t+1时刻的预测值为
Figure BDA0002585356400000084
其中，dt+1＝dt+vt×△t，vt+1＝vt，△t为预测的步长，即t时刻和t+1时刻的时间间隔。通过运动方程f来描述/>
Figure BDA0002585356400000085
和/>
Figure BDA0002585356400000086
之间的关系，记为/>
Figure BDA0002585356400000087
通过矩阵形式表示则/>
Figure BDA0002585356400000088
其中，/>
Figure BDA0002585356400000089
即运动方程f的雅克比矩阵。
Q为假设物体做匀速直线运动所导致的预测协方差矩阵，若这种假设导致的位置的误差为derror，速度的误差为verror，位置的真实值为d′t+1，速度的真实值为v′t+1，则d′t+1＝dt+vt×Δt+derror，v′t+1＝vt+verror，其中，derror服从均值为0，方差为σd的正态分布；verror服从均值为0，方差为σv的正态分布。则
Figure BDA00025853564000000810
可以根据实际测试时障碍物相对于本车的速度和位置的真实值和测量值，得到该预测协方差矩阵。
传感器仅检测距离，则R就是检测距离的方差，是一个1×1的矩阵，例如可以取1。若传感器检测距离和速度两个值，则R就是2×2的矩阵，各个元素的含义为
Figure BDA00025853564000000811
但一般会取成对角阵，即认为各个检测值之间的误差协方差为0，即/>
Figure BDA0002585356400000091
例如可以取/>
Figure BDA0002585356400000092
如图3所示，离线统计传感器的测量数据，按不同距离和角度划分为不同区域，计算出该区域内的传感器测量的误差均值。然后在线测量时，根据目标的测量值的距离与角度所处区域，进行相应的误差均值补偿。例如，根据离线统计出，当目标在正前方正负10度，0-50m范围内时，传感器测量出的目标纵向距离与实际的目标纵向距离的误差均值为0.2m。则当在线测量时，检测到的目标在这个区域内时，算法会将测量到的纵向距离减去0.2m后再进行后续的计算。比如测量到正前方20m处有个障碍物，则算法会告诉后面的模块，在正前方19.8m处检测到障碍物。
对于前述的各方法实施例，为了简单描述，故将其都表述为一系列的动作组合，但是本领域技术人员应该知悉，本发明并不受所描述的动作顺序的限制，因为依据本发明，某些步骤可以采用其他顺序或者同时进行。
下述为本发明装置实施例，可以用于执行本发明方法实施例。对于本发明装置实施例中未披露的细节，请参照本发明方法实施例。
参见图4，为本发明的实施例提供的一种多源传感器信息融合装置，该装置包括状态量预测单元、误差协方差矩阵预测单元、测量值修正单元、增益矩阵计算单元、误差协方差矩阵估计单元和状态量估计单元。
状态量预测单元，用于根据状态量在第一时刻的估计值，计算得到状态量在第二时刻的预测值，第二时刻为第一时刻的后一时刻。
误差协方差矩阵预测单元，用于根据误差协方差矩阵在第一时刻的估计值，计算得到误差协方差矩阵在第二时刻的预测值。
测量值修正单元，用于根据预先确定的测量值与测量误差的对应关系，对多源传感器在第二时刻融合后的测量值进行修正，得到在第二时刻修正后的测量值。
增益矩阵计算单元，用于根据多源传感器的量测协方差矩阵和误差协方差矩阵在第二时刻的预测值，计算得到在第二时刻的卡尔曼增益矩阵。
误差协方差矩阵估计单元，用于根据在第二时刻的卡尔曼增益矩阵和误差协方差矩阵在第二时刻的预测值，计算得到误差协方差矩阵在第二时刻的估计值。
状态量估计单元，用于根据在第二时刻的卡尔曼增益矩阵、在第二时刻修正后的测量值以及状态量在第一时刻的估计值，计算得到状态量在第二时刻的估计值。
本发明的实施例提供了另一种多源传感器信息融合装置，该装置包括状态量预测单元、误差协方差矩阵预测单元、测量值修正单元、非高斯噪声计算单元、量测协方差矩阵修正单元、增益矩阵计算单元、误差协方差矩阵估计单元和状态量估计单元。
状态量预测单元，用于根据状态量在第一时刻的估计值，计算得到状态量在第二时刻的预测值，第二时刻为第一时刻的后一时刻。
误差协方差矩阵预测单元，用于根据误差协方差矩阵在第一时刻的估计值，计算得到误差协方差矩阵在第二时刻的预测值。
测量值修正单元，用于根据预先确定的测量值与测量误差的对应关系，对多源传感器在第二时刻融合后的测量值进行修正，得到在第二时刻修正后的测量值。
非高斯噪声计算单元，用于将状态量在第二时刻的预测值减去在第二时刻修正后的测量值，得到状态量在第二时刻的非高斯噪声值。
量测协方差矩阵修正单元，用于当状态量在第二时刻的非高斯噪声值大于预设阈值时，对多源传感器在第一时刻的量测协方差矩阵进行增大修正，得到在第二时刻的量测协方差矩阵。
增益矩阵计算单元，具体用于根据多源传感器的在第二时刻的量测协方差矩阵和误差协方差矩阵在第二时刻的预测值，计算得到在第二时刻的卡尔曼增益矩阵。
误差协方差矩阵估计单元，用于根据在第二时刻的卡尔曼增益矩阵和误差协方差矩阵在第二时刻的预测值，计算得到误差协方差矩阵在第二时刻的估计值。
状态量估计单元，用于根据在第二时刻的卡尔曼增益矩阵、在第二时刻修正后的测量值以及状态量在第一时刻的估计值，计算得到状态量在第二时刻的估计值。
可选的，多源传感器包括：摄像头、毫米波雷达和激光雷达中的至少两个。
可选的，状态量包括：障碍物相对于本车的位置、速度和/或加速度。
可选的，预先确定的测量值与测量误差的对应关系，具体为不同的测量值范围分别对应一个测量误差。
本发明实施例提供的一种控制器如图5所示。该控制器的硬件结构可以包括：至少一个处理器51，至少一个通信接口52，至少一个存储器53和至少一个通信总线54；且处理器51、通信接口52、存储器53通过通信总线54完成相互间的通信。
处理器51在一些实施例中可以是一个CPU(Central Processing Unit，中央处理器)，或者是ASIC(Application Specific Integrated Circuit，特定集成电路)，或者是被配置成实施本发明实施例的一个或多个集成电路等。
通信接口52可以包括标准的有线接口、无线接口。通常用于在控制器与其他电子设备或系统之间建立通信连接。
存储器53包括至少一种类型的可读存储介质。可读存储介质可以为如闪存、硬盘、多媒体卡、卡型存储器等NVM(non-volatile memory，非易失性存储器)。可读存储介质还可以是高速RAM(random access memory，随机存取存储器)存储器。
其中，存储器53存储有计算机程序，处理器51可调用存储器53存储的计算机程序，所述计算机程序用于：
根据状态量在第一时刻的估计值，计算得到状态量在第二时刻的预测值，第二时刻为第一时刻的后一时刻；
根据误差协方差矩阵在第一时刻的估计值，计算得到误差协方差矩阵在第二时刻的预测值；
根据预先确定的测量值与测量误差的对应关系，对多源传感器在第二时刻融合后的测量值进行修正，得到在第二时刻修正后的测量值；
根据多源传感器的量测协方差矩阵和误差协方差矩阵在第二时刻的预测值，计算得到在第二时刻的卡尔曼增益矩阵；
根据在第二时刻的卡尔曼增益矩阵和误差协方差矩阵在第二时刻的预测值，计算得到误差协方差矩阵在第二时刻的估计值；
根据在第二时刻的卡尔曼增益矩阵、在第二时刻修正后的测量值以及状态量在第一时刻的估计值，计算得到状态量在第二时刻的估计值。
所述程序的细化功能和扩展功能可参照上文描述。
图5仅示出了具有组件51～54的控制器，但是应理解的是，并不要求实施所有示出的组件，可以替代的实施更多或者更少的组件。
本发明实施例还提供一种可读存储介质，该可读存储介质可存储有适于处理器执行的程序，所述程序用于：
根据状态量在第一时刻的估计值，计算得到状态量在第二时刻的预测值，第二时刻为第一时刻的后一时刻；
根据误差协方差矩阵在第一时刻的估计值，计算得到误差协方差矩阵在第二时刻的预测值；
根据预先确定的测量值与测量误差的对应关系，对多源传感器在第二时刻融合后的测量值进行修正，得到在第二时刻修正后的测量值；
根据多源传感器的量测协方差矩阵和误差协方差矩阵在第二时刻的预测值，计算得到在第二时刻的卡尔曼增益矩阵；
根据在第二时刻的卡尔曼增益矩阵和误差协方差矩阵在第二时刻的预测值，计算得到误差协方差矩阵在第二时刻的估计值；
根据在第二时刻的卡尔曼增益矩阵、在第二时刻修正后的测量值以及状态量在第一时刻的估计值，计算得到状态量在第二时刻的估计值。
所述程序的细化功能和扩展功能可参照上文描述。
以上所描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。本领域普通技术人员在不付出创造性劳动的情况下，即可以理解并实施。
在本文中，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。
本说明书中各个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可，且本说明书中各实施例中记载的特征可以相互替换或者组合。
对本发明所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。

Claims (10)

1.一种多源传感器信息融合方法，其特征在于，包括：
根据状态量在第一时刻的估计值，计算得到所述状态量在第二时刻的预测值，第二时刻为第一时刻的后一时刻；
根据误差协方差矩阵在第一时刻的估计值，计算得到所述误差协方差矩阵在第二时刻的预测值；
根据预先确定的测量值与测量误差的对应关系，对多源传感器在第二时刻融合后的测量值进行修正，得到在第二时刻修正后的测量值；
根据所述多源传感器的量测协方差矩阵和所述误差协方差矩阵在第二时刻的预测值，计算得到在第二时刻的卡尔曼增益矩阵；
根据在第二时刻的卡尔曼增益矩阵和所述误差协方差矩阵在第二时刻的预测值，计算得到所述误差协方差矩阵在第二时刻的估计值；
根据在第二时刻的卡尔曼增益矩阵、在第二时刻修正后的测量值以及所述状态量在第一时刻的估计值，计算得到所述状态量在第二时刻的估计值。
2.根据权利要求1所述的多源传感器信息融合方法，其特征在于，在所述得到在第二时刻修正后的测量值的步骤之后，还包括：
将所述状态量在第二时刻的预测值减去所述在第二时刻修正后的测量值，得到所述状态量在第二时刻的非高斯噪声值；
当所述状态量在第二时刻的非高斯噪声值大于预设阈值时，对所述多源传感器在第一时刻的量测协方差矩阵进行增大修正，得到在第二时刻的量测协方差矩阵；
根据所述多源传感器的量测协方差矩阵和所述误差协方差矩阵在第二时刻的预测值，计算得到在第二时刻的卡尔曼增益矩阵，具体为：
根据所述多源传感器的在第二时刻的量测协方差矩阵和所述误差协方差矩阵在第二时刻的预测值，计算得到在第二时刻的卡尔曼增益矩阵。
3.根据权利要求1或2所述的多源传感器信息融合方法，其特征在于，所述多源传感器，包括：
摄像头、毫米波雷达和激光雷达中的至少两个。
4.根据权利要求3所述的多源传感器信息融合方法，其特征在于，所述状态量，包括：
障碍物相对于本车的位置、速度和/或加速度。
5.根据权利要求1所述的多源传感器信息融合方法，其特征在于，所述预先确定的测量值与测量误差的对应关系，具体为：
不同的测量值范围分别对应一个测量误差。
6.一种多源传感器信息融合装置，其特征在于，包括：
状态量预测单元，用于根据状态量在第一时刻的估计值，计算得到所述状态量在第二时刻的预测值，第二时刻为第一时刻的后一时刻；
误差协方差矩阵预测单元，用于根据误差协方差矩阵在第一时刻的估计值，计算得到所述误差协方差矩阵在第二时刻的预测值；
测量值修正单元，用于根据预先确定的测量值与测量误差的对应关系，对多源传感器在第二时刻融合后的测量值进行修正，得到在第二时刻修正后的测量值；
增益矩阵计算单元，用于根据所述多源传感器的量测协方差矩阵和所述误差协方差矩阵在第二时刻的预测值，计算得到在第二时刻的卡尔曼增益矩阵；
误差协方差矩阵估计单元，用于根据在第二时刻的卡尔曼增益矩阵和所述误差协方差矩阵在第二时刻的预测值，计算得到所述误差协方差矩阵在第二时刻的估计值；
状态量估计单元，用于根据在第二时刻的卡尔曼增益矩阵、在第二时刻修正后的测量值以及所述状态量在第一时刻的估计值，计算得到所述状态量在第二时刻的估计值。
7.根据权利要求6所述的多源传感器信息融合装置，其特征在于，还包括：
非高斯噪声计算单元，用于在执行所述测量值修正单元之后，将所述状态量在第二时刻的预测值减去所述在第二时刻修正后的测量值，得到所述状态量在第二时刻的非高斯噪声值；
量测协方差矩阵修正单元，用于当所述状态量在第二时刻的非高斯噪声值大于预设阈值时，对所述多源传感器在第一时刻的量测协方差矩阵进行增大修正，得到在第二时刻的量测协方差矩阵；
所述增益矩阵计算单元，具体用于根据所述多源传感器的在第二时刻的量测协方差矩阵和所述误差协方差矩阵在第二时刻的预测值，计算得到在第二时刻的卡尔曼增益矩阵。
8.根据权利要求6或7所述多源传感器信息融合装置，其特征在于，所述多源传感器包括：
摄像头、毫米波雷达和激光雷达中的至少两个。
9.根据权利要求8所述的多源传感器信息融合装置，其特征在于，所述状态量包括：
障碍物相对于本车的位置、速度和/或加速度。
10.根据权利要求6所述的多源传感器信息融合方法，其特征在于，所述预先确定的测量值与测量误差的对应关系，具体为：
不同的测量值范围分别对应一个测量误差。
CN202010679789.3A 2020-07-15 2020-07-15 多源传感器信息融合方法及装置 Active CN111881955B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010679789.3A CN111881955B (zh) 2020-07-15 2020-07-15 多源传感器信息融合方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010679789.3A CN111881955B (zh) 2020-07-15 2020-07-15 多源传感器信息融合方法及装置

Publications (2)

Publication Number Publication Date
CN111881955A CN111881955A (zh) 2020-11-03
CN111881955B true CN111881955B (zh) 2023-07-04

Family

ID=73151251

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010679789.3A Active CN111881955B (zh) 2020-07-15 2020-07-15 多源传感器信息融合方法及装置

Country Status (1)

Country Link
CN (1) CN111881955B (zh)

Families Citing this family (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112465193A (zh) * 2020-11-09 2021-03-09 苏州挚途科技有限公司 多传感器数据融合的参数优化方法及装置
CN112578286B (zh) * 2020-11-23 2022-11-22 经纬恒润(天津)研究开发有限公司 电池soc估计方法及装置
CN112590806B (zh) * 2020-11-30 2022-08-30 上海欧菲智能车联科技有限公司 基于卡尔曼滤波的车辆信息处理方法、装置、设备和介质
CN112887262B (zh) * 2020-12-28 2021-11-23 北京航空航天大学 一种基于多源信息融合的汽车信息安全防护方法及装置
CN112990256A (zh) * 2020-12-29 2021-06-18 北京电子工程总体研究所 多传感器噪声计算方法及信息融合方法、存储介质及设备
CN113361562B (zh) * 2021-04-20 2024-03-15 武汉理工大学 一种用于动力电池反应控制模块的多传感器融合方法及装置
CN113486795A (zh) * 2021-07-06 2021-10-08 广州小鹏自动驾驶科技有限公司 视觉识别性能测试方法、装置、系统及设备
CN113888606A (zh) * 2021-08-31 2022-01-04 际络科技(上海)有限公司 数据处理方法、装置及存储介质
CN113739768A (zh) * 2021-09-06 2021-12-03 北京智源人工智能研究院 基于卡尔曼滤波的视觉目标处理方法、装置、设备及介质
CN116541663B (zh) * 2023-06-21 2023-09-19 四川信息职业技术学院 一种基于卡尔曼滤波提高双套站传感器数据质量的方法

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2000304854A (ja) * 1999-04-26 2000-11-02 Mitsubishi Electric Corp 目標追尾方法及び目標追尾装置
CN109472418A (zh) * 2018-11-16 2019-03-15 西安电子科技大学 基于卡尔曼滤波的机动目标状态预测优化方法
JP2019056642A (ja) * 2017-09-21 2019-04-11 三菱電機株式会社 追尾処理装置及び追尾処理方法
CN109902568A (zh) * 2019-01-22 2019-06-18 长江大学 一种自适应卡尔曼滤波算法
WO2019114757A1 (zh) * 2017-12-15 2019-06-20 蔚来汽车有限公司 多传感器目标信息融合的优化方法及装置、计算机设备和记录介质
WO2019114807A1 (zh) * 2017-12-15 2019-06-20 蔚来汽车有限公司 多传感器目标信息融合
CN110289989A (zh) * 2019-05-27 2019-09-27 东南大学 一种基于容积卡尔曼滤波算法的分布式状态估计方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP6677533B2 (ja) * 2016-03-01 2020-04-08 クラリオン株式会社 車載装置、及び、推定方法

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2000304854A (ja) * 1999-04-26 2000-11-02 Mitsubishi Electric Corp 目標追尾方法及び目標追尾装置
JP2019056642A (ja) * 2017-09-21 2019-04-11 三菱電機株式会社 追尾処理装置及び追尾処理方法
WO2019114757A1 (zh) * 2017-12-15 2019-06-20 蔚来汽车有限公司 多传感器目标信息融合的优化方法及装置、计算机设备和记录介质
WO2019114807A1 (zh) * 2017-12-15 2019-06-20 蔚来汽车有限公司 多传感器目标信息融合
CN109472418A (zh) * 2018-11-16 2019-03-15 西安电子科技大学 基于卡尔曼滤波的机动目标状态预测优化方法
CN109902568A (zh) * 2019-01-22 2019-06-18 长江大学 一种自适应卡尔曼滤波算法
CN110289989A (zh) * 2019-05-27 2019-09-27 东南大学 一种基于容积卡尔曼滤波算法的分布式状态估计方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
基于多传感器的卡尔曼滤波跟踪算法;项拙;;中国新通信(16);全文 *
空中战机的雷达目标信号跟踪精度优化研究;程佳林;张贞凯;;计算机仿真(07);全文 *

Also Published As

Publication number Publication date
CN111881955A (zh) 2020-11-03

Similar Documents

Publication Publication Date Title
CN111881955B (zh) 多源传感器信息融合方法及装置
US10088553B2 (en) Method of automatic sensor pose estimation
US7991550B2 (en) Method and apparatus for on-vehicle calibration and orientation of object-tracking systems
US9148657B2 (en) Calibration device, range-finding system including the calibration device and stereo camera, and vehicle mounting the range-finding system
US20070182623A1 (en) Method and apparatus for on-vehicle calibration and orientation of object-tracking systems
US11899100B2 (en) Method of determination of alignment angles of radar sensors for a road vehicle radar auto-alignment controller
US9338438B2 (en) Calibrating range-finding system using parallax from two different viewpoints and vehicle mounting the range-finding system
US11415690B2 (en) Method and system comparing odometer velocity to radar based velocity
EP3654064A1 (en) Apparatus and method for characterizing an object based on measurement samples from one or more location sensors
CN113701745B (zh) 一种外参变化检测方法、装置、电子设备及检测系统
US10605897B2 (en) Vehicle lane alignment correction improvements
JP2022520003A (ja) マルチ入力‐マルチ出力レーダセンサのキャリブレーション装置および方法
CN111856418A (zh) 车载雷达相位校准方法、装置、电子设备及存储介质
CN110637209B (zh) 估计机动车的姿势的方法、设备和具有指令的计算机可读存储介质
IL199747A (en) A method for estimating sensor measurement values
US20220365193A1 (en) Method for estimating correction angles in a radar sensor for motor vehicles
CN116614621B (zh) 相机内感知算法的测试方法、设备和存储介质
CN110865360B (zh) 一种数据融合方法及装置
CN113805147B (zh) 车载雷达水平测量角度自校正方法、装置、介质及设备
CN116148821A (zh) 激光雷达的外参修正方法和装置、电子设备和存储介质
CN116819561A (zh) 一种点云数据匹配方法、系统、电子设备及存储介质
CN116182905A (zh) 激光雷达和组合惯导的时空外参标定方法、装置及系统
JP2019219180A (ja) 車両用物体検知装置
GB2604175A (en) A method for determining a mounting position error of an environment sensor device of an assistance system of a motor vehicle as well as an assistance syste
JP7482440B2 (ja) 情報処理装置、情報処理方法及びプログラム

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
CB02 Change of applicant information
CB02 Change of applicant information

Address after: 4 / F, building 1, No.14 Jiuxianqiao Road, Chaoyang District, Beijing 100020

Applicant after: Beijing Jingwei Hirain Technologies Co.,Inc.

Address before: 8 / F, block B, No. 11, Anxiang Beili, Chaoyang District, Beijing 100101

Applicant before: Beijing Jingwei HiRain Technologies Co.,Ltd.

GR01 Patent grant
GR01 Patent grant