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

说明书

本申请提供一种组合定位方法和系统，所述方法包括：接收IMU数据，在第一卡尔曼滤波器中对IMU数据进行解算，得到无人驾驶车辆的系统状态变量；接收并缓存量测数据，将所述量测数据作为当前量测数据；执行量测更新：在第二卡尔曼滤波器中进行量测更新，得到更新后的系统状态变量；若已缓存有所述量测数据的量测时间之后的量测数据，则将已缓存的所述量测时间之后的量测数据作为当前量测数据，执行所述量测更新。可以解决不同传感器之间的解算时延不一致造成的量测信息乱序的问题，能够提高定位的实时性和准确性。

【技术领域】

本申请涉及自动控制领域，尤其涉及一种组合定位方法和系统。

【背景技术】

自动驾驶车辆在运行的过程中需要根据自动驾驶车辆当前所在位置，以及周围环境做出实时的控制，这就要求自动驾驶车辆的定位系统系统能够实时地给出当前位置信息。

自动驾驶车辆用于定位的传感器通常包括SINS捷联惯导系统(例如惯性传感器IMU)、GNSS全球导航卫星系统、LiDAR激光雷达等。其中GNSS、LiDAR传感器从采集到测量信息到解算出位置信息，通常存在10-20ms，以及80-100ms的延时，延时的不一致会造成融合滤波中接收到的量测信息存在乱序的情况。

对于实时性要求比较低的情况，通常不需要考虑定位的实时性。多数文献研究组合定位时，通常是SINS与GNSS、轮速计等的组合，同时也没有考虑不同系统之间的延时不一致造成的量测信息乱序等问题。

而自动驾驶车辆对实时性要求很高，因此如何将SINS、GNSS、LiDAR等传感器的数据进行融合，获取实时的定位信息，对于自动驾驶起着重要作用。

【发明内容】

本申请的多个方面提供一种实时定位方法和系统，用以解决量测信息存在乱序的问题，提高定位的实时性和准确性。

本申请的一方面，提供一种实时定位方法，包括：

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

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

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

如上所述的方面和任一可能的实现方式，进一步提供一种实现方式，所述接收IMU数据，在第一卡尔曼滤波器中对IMU数据进行解算，得到无人驾驶车辆的系统状态变量包括：

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

如上所述的方面和任一可能的实现方式，进一步提供一种实现方式，所述执行量测更新包括：

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

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

如上所述的方面和任一可能的实现方式，进一步提供一种实现方式，所述若已缓存有所述量测数据的量测时间之后的量测数据，则将已缓存的所述量测时间之后的量测数据作为当前量测数据，执行所述量测更新包括：

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