[发明专利]多源传感器信息融合方法及装置

权利要求书

1.一种多源传感器信息融合方法，其特征在于，包括：

根据状态量在第一时刻的估计值，计算得到所述状态量在第二时刻的预测值，第二时刻为第一时刻的后一时刻；

根据误差协方差矩阵在第一时刻的估计值，计算得到所述误差协方差矩阵在第二时刻的预测值；

根据预先确定的测量值与测量误差的对应关系，对多源传感器在第二时刻融合后的测量值进行修正，得到在第二时刻修正后的测量值；

根据所述多源传感器的量测协方差矩阵和所述误差协方差矩阵在第二时刻的预测值，计算得到在第二时刻的卡尔曼增益矩阵；

根据在第二时刻的卡尔曼增益矩阵和所述误差协方差矩阵在第二时刻的预测值，计算得到所述误差协方差矩阵在第二时刻的估计值；

根据在第二时刻的卡尔曼增益矩阵、在第二时刻修正后的测量值以及所述状态量在第一时刻的估计值，计算得到所述状态量在第二时刻的估计值。

2.根据权利要求1所述的多源传感器信息融合方法，其特征在于，在所述得到在第二时刻修正后的测量值的步骤之后，还包括：

将所述状态量在第二时刻的预测值减去所述在第二时刻修正后的测量值，得到所述状态量在第二时刻的非高斯噪声值；

当所述状态量在第二时刻的非高斯噪声值大于预设阈值时，对所述多源传感器在第一时刻的量测协方差矩阵进行增大修正，得到在第二时刻的量测协方差矩阵；

根据所述多源传感器的量测协方差矩阵和所述误差协方差矩阵在第二时刻的预测值，计算得到在第二时刻的卡尔曼增益矩阵，具体为：

根据所述多源传感器的在第二时刻的量测协方差矩阵和所述误差协方差矩阵在第二时刻的预测值，计算得到在第二时刻的卡尔曼增益矩阵。

3.根据权利要求1或2所述的多源传感器信息融合方法，其特征在于，所述多源传感器，包括：

摄像头、毫米波雷达和激光雷达中的至少两个。

4.根据权利要求3所述的多源传感器信息融合方法，其特征在于，所述状态量，包括：

障碍物相对于本车的位置、速度和/或加速度。

5.根据权利要求1所述的多源传感器信息融合方法，其特征在于，所述预先确定的测量值与测量误差的对应关系，具体为：

不同的测量值范围分别对应一个测量误差。

6.一种多源传感器信息融合装置，其特征在于，包括：

状态量预测单元，用于根据状态量在第一时刻的估计值，计算得到所述状态量在第二时刻的预测值，第二时刻为第一时刻的后一时刻；

误差协方差矩阵预测单元，用于根据误差协方差矩阵在第一时刻的估计值，计算得到所述误差协方差矩阵在第二时刻的预测值；

测量值修正单元，用于根据预先确定的测量值与测量误差的对应关系，对多源传感器在第二时刻融合后的测量值进行修正，得到在第二时刻修正后的测量值；

增益矩阵计算单元，用于根据所述多源传感器的量测协方差矩阵和所述误差协方差矩阵在第二时刻的预测值，计算得到在第二时刻的卡尔曼增益矩阵；

误差协方差矩阵估计单元，用于根据在第二时刻的卡尔曼增益矩阵和所述误差协方差矩阵在第二时刻的预测值，计算得到所述误差协方差矩阵在第二时刻的估计值；

状态量估计单元，用于根据在第二时刻的卡尔曼增益矩阵、在第二时刻修正后的测量值以及所述状态量在第一时刻的估计值，计算得到所述状态量在第二时刻的估计值。

7.根据权利要求6所述的多源传感器信息融合装置，其特征在于，还包括：

非高斯噪声计算单元，用于在执行所述测量值修正单元之后，将所述状态量在第二时刻的预测值减去所述在第二时刻修正后的测量值，得到所述状态量在第二时刻的非高斯噪声值；

量测协方差矩阵修正单元，用于当所述状态量在第二时刻的非高斯噪声值大于预设阈值时，对所述多源传感器在第一时刻的量测协方差矩阵进行增大修正，得到在第二时刻的量测协方差矩阵；

所述增益矩阵计算单元，具体用于根据所述多源传感器的在第二时刻的量测协方差矩阵和所述误差协方差矩阵在第二时刻的预测值，计算得到在第二时刻的卡尔曼增益矩阵。