[发明专利]一种组合定位方法和系统

权利要求书

1.一种组合定位方法，其特征在于，包括：

接收IMU数据，在第一卡尔曼滤波器中对IMU数据进行解算，得到无人驾驶车辆的系统状态变量；

接收并缓存量测数据，将所述量测数据作为当前量测数据；执行量测更新：获取当前量测数据的量测时间处的所述系统状态变量，在第二卡尔曼滤波器中进行解算，得到更新后的系统状态变量；

若已缓存有所述量测数据的量测时间之后的量测数据，则将已缓存的所述量测时间之后的量测数据作为当前量测数据，执行所述量测更新，

所述若已缓存有所述量测数据的量测时间之后的量测数据，则将已缓存的所述量测时间之后的量测数据作为当前量测数据，执行所述量测更新包括：

对第二卡尔曼滤波器进行时间更新直至所述量测数据的量测时间之后的量测数据的量测时间，进行量测更新。

2.根据权利要求1所述的方法，其特征在于，所述接收IMU数据，在第一卡尔曼滤波器中对IMU数据进行解算，得到无人驾驶车辆的系统状态变量包括：

接收IMU数据，在第一卡尔曼滤波器中对IMU数据进行解算，得到无人驾驶车辆的系统状态变量以及误差状态变量；将IMU数据和解算得到的无人驾驶车辆的系统状态变量保存到缓存中。

3.根据权利要求2所述的方法，其特征在于，获取当前量测数据的量测时间处的所述系统状态变量，在第二卡尔曼滤波器中进行解算，得到更新后的系统状态变量，包括：

进行时间更新，将第一卡尔曼滤波器中的误差状态变量赋给第二卡尔曼滤波器，从第一卡尔曼滤波器的缓存中获取所述当前量测数据的量测时间处的系统状态变量；

进行量测更新，包括：根据当前量测数据、第一卡尔曼滤波器解算得到的系统状态变量以及误差状态变量，计算第二卡尔曼滤波器的误差状态变量对所述系统状态变量进行校正。

4.根据权利要求1所述的方法，其特征在于，所述对第二卡尔曼滤波器进行时间更新直至所述量测数据的量测时间之后的量测数据的量测时间，进行量测更新还包括：

若有多个量测数据，则依次进行第二卡尔曼滤波器的时间更新和量测更新，直到最新IMU数据的时间。

5.根据权利要求1所述的方法，其特征在于，所述若已缓存有所述量测数据的量测时间之后的量测数据，则将已缓存的所述量测时间之后的量测数据作为当前量测数据，执行所述量测更新之后还包括：

对第二卡尔曼滤波器进行时间更新直到最新IMU数据的时间，将第二卡尔曼滤波器中的误差状态变量赋给第一卡尔曼滤波器。

6.根据权利要求1所述的方法，其特征在于，所述量测数据为GNSS或LiDAR量测数据。

7.一种组合定位系统，其特征在于，包括：

第一卡尔曼滤波器解算模块，用于接收IMU数据，在第一卡尔曼滤波器中对IMU数据进行解算，得到无人驾驶车辆的系统状态变量；

第二卡尔曼滤波器解算模块，用于接收并缓存量测数据，将所述量测数据作为当前量测数据；执行量测更新：获取当前量测数据的量测时间处的系统状态变量，在第二卡尔曼滤波器中进行解算，得到更新后的系统状态变量；

若已缓存有所述量测数据的量测时间之后的量测数据，则将已缓存的所述量测时间之后的量测数据作为当前量测数据，执行所述量测更新，

所述若已缓存有所述量测数据的量测时间之后的量测数据，则将已缓存的所述量测时间之后的量测数据作为当前量测数据，执行所述量测更新包括：

时间更新子模块对第二卡尔曼滤波器进行时间更新直至所述量测数据的量测时间之后的量测数据的量测时间，量测更新子模块进行量测更新。

8.根据权利要求7所述的系统，其特征在于，所述第一卡尔曼滤波器解算模块具体用于：

接收IMU数据，在第一卡尔曼滤波器中对IMU数据进行解算，得到无人驾驶车辆的系统状态变量以及误差状态变量；将IMU数据和解算得到的无人驾驶车辆的系统状态变量保存到缓存中。