一种组合定位方法和系统技术方案

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

A Combined Location Method and System

The application provides a combined positioning method and system. The method includes: receiving IMU data, calculating IMU data in the first Kalman filter, and obtaining system state variables of driverless vehicles; receiving and caching measurement data, and taking the measurement data as current measurement data; performing measurement updating: updating measurement in the second Kalman filter, and obtaining To the updated system state variable; if the measurement data after the measurement time of the measurement data has been cached, the measurement data after the cached measurement time will be used as the current measurement data to perform the measurement update. It can solve the problem of measurement information disorder caused by the inconsistency of computing time delays between different sensors, and improve the real-time and accuracy of positioning.

【技术实现步骤摘要】
一种组合定位方法和系统
本申请涉及自动控制领域，尤其涉及一种组合定位方法和系统。
技术介绍
自动驾驶车辆在运行的过程中需要根据自动驾驶车辆当前所在位置，以及周围环境做出实时的控制，这就要求自动驾驶车辆的定位系统系统能够实时地给出当前位置信息。自动驾驶车辆用于定位的传感器通常包括SINS捷联惯导系统(例如惯性传感器IMU)、GNSS全球导航卫星系统、LiDAR激光雷达等。其中GNSS、LiDAR传感器从采集到测量信息到解算出位置信息，通常存在10-20ms，以及80-100ms的延时，延时的不一致会造成融合滤波中接收到的量测信息存在乱序的情况。对于实时性要求比较低的情况，通常不需要考虑定位的实时性。多数文献研究组合定位时，通常是SINS与GNSS、轮速计等的组合，同时也没有考虑不同系统之间的延时不一致造成的量测信息乱序等问题。而自动驾驶车辆对实时性要求很高，因此如何将SINS、GNSS、LiDAR等传感器的数据进行融合，获取实时的定位信息，对于自动驾驶起着重要作用。
技术实现思路
本申请的多个方面提供一种实时定位方法和系统，用以解决量测信息存在乱序的问题，提高定位的实时性和准确性。本申请的一方面，提供一种实时定位方法，包括：接收IMU数据，在第一卡尔曼滤波器中对IMU数据进行解算，得到无人驾驶车辆的系统状态变量；接收并缓存量测数据，将所述量测数据作为当前量测数据；执行量测更新：获取当前量测数据的量测时间处的系统状态变量，在第二卡尔曼滤波器中进行解算，得到更新后的系统状态变量；若已缓存有所述量测数据的量测时间之后的量测数据，则将已缓存的所述量测时间之后的量测数据作为当前量测数据，执行所述量测更新。如上所述的方面和任一可能的实现方式，进一步提供一种实现方式，所述接收IMU数据，在第一卡尔曼滤波器中对IMU数据进行解算，得到无人驾驶车辆的系统状态变量包括：接收IMU数据，在第一卡尔曼滤波器中对IMU数据进行解算，得到无人驾驶车辆的系统状态变量以及误差状态变量；将IMU数据和解算得到的无人驾驶车辆的系统状态变量保存到缓存中。如上所述的方面和任一可能的实现方式，进一步提供一种实现方式，所述执行量测更新包括：进行时间更新，将第一卡尔曼滤波器中的误差状态变量赋给第二卡尔曼滤波器，从第一卡尔曼滤波器的缓存中获取所述当前量测数据的量测时间处的系统状态变量；进行量测更新，包括：根据当前量测数据、第一卡尔曼滤波器解算得到的系统状态变量以及误差状态变量，计算第二卡尔曼滤波器的误差状态变量对所述系统状态变量进行校正。如上所述的方面和任一可能的实现方式，进一步提供一种实现方式，所述若已缓存有所述量测数据的量测时间之后的量测数据，则将已缓存的所述量测时间之后的量测数据作为当前量测数据，执行所述量测更新包括：对第二卡尔曼滤波器进行时间更新直至所述量测数据的量测时间之后的量测数据的量测时间，进行量测更新。如上所述的方面和任一可能的实现方式，进一步提供一种实现方式，所述对第二卡尔曼滤波器进行时间更新直至所述量测数据的量测时间之后的量测数据的量测时间，进行量测更新还包括：若有多个量测数据，则依次进行第二卡尔曼滤波器的时间更新和量测更新，直到最新IMU数据的时间。如上所述的方面和任一可能的实现方式，进一步提供一种实现方式，所述若已缓存有所述量测数据的量测时间之后的量测数据，则将已缓存的所述量测时间之后的量测数据作为当前量测数据，执行所述量测更新之后还包括：对第二卡尔曼滤波器进行时间更新直到最新IMU数据的时间，将第二卡尔曼滤波器中的误差状态变量赋给第一卡尔曼滤波器。如上所述的方面和任一可能的实现方式，进一步提供一种实现方式，所述量测数据为GNSS或LiDAR量测数据。根据本专利技术的另一方面，提供一种组合定位系统，包括：第一卡尔曼滤波器解算模块，用于接收IMU数据，在第一卡尔曼滤波器中对IMU数据进行解算，得到无人驾驶车辆的系统状态变量；第二卡尔曼滤波器解算模块，用于接收并缓存量测数据，将所述量测数据作为当前量测数据；执行量测更新：获取当前量测数据的量测时间处的系统状态变量，在第二卡尔曼滤波器中进行解算，得到更新后的系统状态变量；若已缓存有所述量测数据的量测时间之后的量测数据，则将已缓存的所述量测时间之后的量测数据作为当前量测数据，执行所述量测更新。如上所述的方面和任一可能的实现方式，进一步提供一种实现方式，所述第一卡尔曼滤波器解算模块具体用于：接收IMU数据，在第一卡尔曼滤波器中对IMU数据进行解算，得到无人驾驶车辆的系统状态变量以及误差状态变量；将IMU数据和解算得到的无人驾驶车辆的系统状态变量保存到缓存中。如上所述的方面和任一可能的实现方式，进一步提供一种实现方式，所述第二卡尔曼滤波器解算模块包括：时间更新子模块，用于进行时间更新，将第一卡尔曼滤波器中的误差状态变量赋给第二卡尔曼滤波器，从第一卡尔曼滤波器的缓存中获取所述当前量测数据的量测时间处的系统状态变量；量测更新子模块，用于进行量测更新，根据当前量测数据、第一卡尔曼滤波器解算得到的系统状态变量以及误差状态变量，计算第二卡尔曼滤波器的误差状态变量对所述系统状态变量进行校正。如上所述的方面和任一可能的实现方式，进一步提供一种实现方式，所述若已缓存有所述量测数据的量测时间之后的量测数据，则将已缓存的所述量测时间之后的量测数据作为当前量测数据，执行所述量测更新包括：时间更新子模块对第二卡尔曼滤波器进行时间更新直至所述量测数据的量测时间之后的量测数据的量测时间，量测更新子模块进行量测更新。如上所述的方面和任一可能的实现方式，进一步提供一种实现方式，所述对第二卡尔曼滤波器进行时间更新直至所述量测数据的量测时间之后的量测数据的量测时间，进行量测更新还包括：若有多个量测数据，则依次进行第二卡尔曼滤波器的时间更新和量测更新，直到最新IMU数据的时间。如上所述的方面和任一可能的实现方式，进一步提供一种实现方式，所述时间更新子模块对第二卡尔曼滤波器进行时间更新直至所述量测数据的量测时间之后的量测数据的量测时间，量测更新子模块进行量测更新之后，时间更新子模块对第二卡尔曼滤波器进行时间更新直到最新IMU数据的时间，将第二卡尔曼滤波器中的误差状态变量赋给第一卡尔曼滤波器。如上所述的方面和任一可能的实现方式，进一步提供一种实现方式，所述量测数据为GNSS或LiDAR量测数据。本专利技术的另一方面，提供一种计算机设备，包括存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述处理器执行所述程序时实现如以上所述的方法。本专利技术的另一方面，提供一种计算机可读存储介质，其上存储有计算机程序，所述程序被处理器执行时实现如以上所述的方法。由所述技术方案可知，本申请实施例，可以解决量测信息存在乱序的问题，提高定位的实时性和准确性。【附图说明】为了更清楚地说明本申请实施例中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作一简单地介绍，显而易见地，下面描述中的附图是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。图1为本申请一实施例提供的组合定位方法的流程示意图；图2为本申请一实施例提供的组合定位系统的结构示意图；图3示出本文档来自技高网...

【技术保护点】
1.一种组合定位方法，其特征在于，包括：接收IMU数据，在第一卡尔曼滤波器中对IMU数据进行解算，得到无人驾驶车辆的系统状态变量；接收并缓存量测数据，将所述量测数据作为当前量测数据；执行量测更新：获取当前量测数据的量测时间处的系统状态变量，在第二卡尔曼滤波器中进行解算，得到更新后的系统状态变量；若已缓存有所述量测数据的量测时间之后的量测数据，则将已缓存的所述量测时间之后的量测数据作为当前量测数据，执行所述量测更新。

【技术特征摘要】
1.一种组合定位方法，其特征在于，包括：接收IMU数据，在第一卡尔曼滤波器中对IMU数据进行解算，得到无人驾驶车辆的系统状态变量；接收并缓存量测数据，将所述量测数据作为当前量测数据；执行量测更新：获取当前量测数据的量测时间处的系统状态变量，在第二卡尔曼滤波器中进行解算，得到更新后的系统状态变量；若已缓存有所述量测数据的量测时间之后的量测数据，则将已缓存的所述量测时间之后的量测数据作为当前量测数据，执行所述量测更新。2.根据权利要求1所述的方法，其特征在于，所述接收IMU数据，在第一卡尔曼滤波器中对IMU数据进行解算，得到无人驾驶车辆的系统状态变量包括：接收IMU数据，在第一卡尔曼滤波器中对IMU数据进行解算，得到无人驾驶车辆的系统状态变量以及误差状态变量；将IMU数据和解算得到的无人驾驶车辆的系统状态变量保存到缓存中。3.根据权利要求2所述的方法，其特征在于，所述执行量测更新包括：进行时间更新，将第一卡尔曼滤波器中的误差状态变量赋给第二卡尔曼滤波器，从第一卡尔曼滤波器的缓存中获取所述当前量测数据的量测时间处的系统状态变量；进行量测更新，包括：根据当前量测数据、第一卡尔曼滤波器解算得到的系统状态变量以及误差状态变量，计算第二卡尔曼滤波器的误差状态变量对所述系统状态变量进行校正。4.根据权利要求3所述的方法，其特征在于，所述若已缓存有所述量测数据的量测时间之后的量测数据，则将已缓存的所述量测时间之后的量测数据作为当前量测数据，执行所述量测更新包括：对第二卡尔曼滤波器进行时间更新直至所述量测数据的量测时间之后的量测数据的量测时间，进行量测更新。5.根据权利要求4所述的方法，其特征在于，所述对第二卡尔曼滤波器进行时间更新直至所述量测数据的量测时间之后的量测数据的量测时间，进行量测更新还包括：若有多个量测数据，则依次进行第二卡尔曼滤波器的时间更新和量测更新，直到最新IMU数据的时间。6.根据权利要求4所述的方法，其特征在于，所述若已缓存有所述量测数据的量测时间之后的量测数据，则将已缓存的所述量测时间之后的量测数据作为当前量测数据，执行所述量测更新之后还包括：对第二卡尔曼滤波器进行时间更新直到最新IMU数据的时间，将第二卡尔曼滤波器中的误差状态变量赋给第一卡尔曼滤波器。7.根据权利要求1所述的方法，其特征在于，所述量测数据为GNSS或LiDAR量测数据。8.一种组合定位系统，其特征在于，包括：第一卡尔曼滤波器解算模块，用于接收IMU数据，在第一卡尔曼滤波器中对IMU数据进行解算，得到无人驾驶车辆的系统状态变量；第二卡尔曼滤波器解算模块，用于接收并缓存量测数据，将所...

【专利技术属性】
技术研发人员：杨晓龙，蔡仁澜，宋适宇，董芳芳，
申请(专利权)人：百度在线网络技术北京有限公司，
类型：发明
国别省市：北京,11