CN114910067A - 定位信息处理方法、装置以及计算机可读存储介质 - Google Patents

定位信息处理方法、装置以及计算机可读存储介质 Download PDF

Info

Publication number
CN114910067A
CN114910067A CN202210316889.9A CN202210316889A CN114910067A CN 114910067 A CN114910067 A CN 114910067A CN 202210316889 A CN202210316889 A CN 202210316889A CN 114910067 A CN114910067 A CN 114910067A
Authority
CN
China
Prior art keywords
inertial navigation
state information
attitude
error
positioning
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.)
Pending
Application number
CN202210316889.9A
Other languages
English (en)
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.)
DeepRoute AI Ltd
Original Assignee
DeepRoute AI 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 DeepRoute AI Ltd filed Critical DeepRoute AI Ltd
Priority to CN202210316889.9A priority Critical patent/CN114910067A/zh
Publication of CN114910067A publication Critical patent/CN114910067A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G01C21/1652—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with ranging devices, e.g. LIDAR or RADAR
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/183—Compensation of inertial measurements, e.g. for temperature effects
    • G01C21/188—Compensation of inertial measurements, e.g. for temperature effects for accumulated errors, e.g. by coupling inertial systems with absolute positioning systems
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Manufacturing & Machinery (AREA)
  • Navigation (AREA)

Abstract

本申请公开了定位信息处理方法、装置以及计算机可读存储介质。该方法包括：获取第一定位系统的第一状态信息，以及获取第二定位系统的第二状态信息；其中，第一定位系统的定位精度高于第二定位系统的定位精度；状态信息包括位置、速度和姿态；确定第一状态信息和第二状态信息之间的误差；基于误差，确定第二定位系统的实际定位精度。通过上述方式，能够实现对第二定位系统的定位精度的评价，便于后续对第二定位系统的修正。

Description

定位信息处理方法、装置以及计算机可读存储介质
技术领域
本申请涉及定位处理技术领域，特别涉及定位信息处理方法、装置以及计算机可读存储介质。
背景技术
定位技术为自动驾驶车辆提供准确的位置、速度和姿态的估计，是自动驾驶技术的重要部分。
不同的定位系统由于硬件因素，则具有不同的定位精度，因此，如何确定定位系统的精度问题成为一难题。
发明内容
本申请主要解决的技术问题是提供定位信息处理方法、装置以及计算机可读存储介质，能够实现对第二定位系统的定位精度的评价，便于后续对第二定位系统的修正。
为解决上述技术问题，本申请采用的一个技术方案是：提供一种定位信息处理方法，该方法包括：获取第一定位系统的第一状态信息，以及获取第二定位系统的第二状态信息；其中，第一定位系统的定位精度高于第二定位系统的定位精度；状态信息包括位置、速度和姿态；确定第一状态信息和第二状态信息之间的误差；基于误差，确定第二定位系统的实际定位精度。
其中，第一定位系统包括：第一惯导、基础定位模块、轮速采集模块；获取第一定位系统的第一状态信息，包括：获取第一惯导检测的第一惯导数据，以及获取基础定位模块采集的第一位置，以及获取轮速采集模块采集的第一轮速；对第一惯导数据、第一位置和第一轮速使用正向/反向卡尔曼滤波算法进行融合，得到第一惯导的第一状态信息。
其中，第二定位系统包括：第二惯导、基础定位模块、轮速采集模块和激光雷达定位模块；获取第二定位系统的第二状态信息，包括：获取第二惯导检测的第二惯导数据，以及获取基础定位模块采集的第一观测位置，以及获取轮速采集模块采集的观测轮速，以及获取激光雷达定位模块采集的第二观测位置和观测姿态；对第二惯导数据、第一观测位置、观测轮速和/或第二观测位置、观测姿态，使用卡尔曼滤波算法进行融合，得到第二惯导的第二状态信息。
其中，对第二惯导数据、第一观测位置、观测轮速和/或第二观测位置、观测姿态数据，使用卡尔曼滤波算法进行融合，得到第二惯导的第二状态信息，包括：利用第二惯导上一时刻的估计位置、估计速度、估计姿态和第二惯导数据进行预测，以得到第二惯导在当前时刻的预测位置、预测速度和预测姿态；根据当前时刻的第一观测位置、观测姿态、观测轮速和/或第二观测位置，以及第二惯导当前时刻的预测位置、预测速度和预测姿态进行卡尔曼滤波处理，得到第二惯导的第二状态信息。
其中，利用第二惯导上一时刻的估计位置、估计速度、估计姿态和第二惯导数据进行预测，以得到第二惯导在当前时刻的预测位置、预测速度和预测姿态，包括：利用第二惯导数据的角速度数据和第二惯导上一时刻的估计姿态，来获得第二惯导在当前时刻的预测姿态；利用第二惯导的预测姿态和重力加速度，对第二惯导数据的加速度进行处理，得到目标加速度；利用第二惯导在上一时刻的估计速度和目标加速度，得到第二惯导当前时刻的预测速度；利用第二惯导在上一时刻的估计位置数据和第二惯导的估计速度，得到第二惯导当前时刻的预测位置。
其中，根据当前时刻的第一观测位置、观测姿态、观测轮速和/或第二观测位置，以及第二惯导当前时刻的预测位置、预测速度和预测姿态进行卡尔曼滤波处理，得到第二惯导的第二状态信息，包括：
利用第一观测位置和预测位置，和/或利用第二观测位置和预测位置，确定第二惯导对应的位置误差观测值；
利用观测轮速和预测速度，确定第二惯导对应的速度误差观测值；
利用观测姿态和预测姿态，确定第二惯导对应的姿态误差观测值；根据位置误差观测值、速度误差观测值和姿态误差观测值，利用卡尔曼滤波算法，计算得到当前时刻的估计位置误差、估计速度误差、估计姿态误差；利用第二惯导当前时刻的预测位置、预测速度和预测姿态数据、估计位置误差、估计速度误差、估计姿态误差，来计算第二惯导当前时刻的估计位置、估计速度和估计姿态。
其中，确定第一状态信息和第二状态信息之间的误差，包括：获取第一定位系统和第二定位系统的变换矩阵；利用变换矩阵将第一状态信息投影至第二状态信息的坐标下，得到第三状态信息；确定第三状态信息和第二状态信息之间的误差。
其中，确定第三状态信息和第二状态信息之间的误差，包括：确定第二状态信息对应的采集时刻；若采集时刻介于相邻两个第一状态信息的采集时刻之间；利用相邻两个第一状态信息对应的第三状态信息进行插值计算，得到对应采集时刻的第三状态信息；确定采集时刻的第三状态信息和第二状态信息之间的误差。
为解决上述技术问题，本申请采用的另一个技术方案是：提供一种定位信息处理装置，该定位信息处理装置包括处理器以及与处理器耦接的存储器，存储器用于存储计算机程序，处理器用于执行计算机程序，以实现如上述技术方案提供的方法。
为解决上述技术问题，本申请采用的另一个技术方案是：提供一种计算机可读存储介质，该计算机可读存储介质用于存储计算机程序，计算机程序在被处理器执行时，用于实现如上述技术方案提供的方法。
本申请实施例的有益效果是：区别于现有技术，本申请提供的定位信息处理方法，该方法包括：获取第一定位系统的第一状态信息，以及获取第二定位系统的第二状态信息；其中，第一定位系统的定位精度高于第二定位系统的定位精度；状态信息包括位置、速度和姿态；确定第一状态信息和第二状态信息之间的误差；基于误差，确定第二定位系统的实际定位精度。通过上述方式，利用高精度的第一定位系统作为基准，对低精度的第二定位系统进行定位精度的确定，能够实现对第二定位系统的定位精度的评价，便于后续对第二定位系统的修正。
附图说明
为了更清楚地说明本申请实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。其中：
图1是本申请提供的定位信息处理方法第一实施例的流程示意图；
图2是本申请提供的定位信息处理方法第二实施例的流程示意图；
图3是本申请提供的步骤24一实施例的流程示意图；
图4是本申请提供的步骤242一实施例的流程示意图；
图5是本申请提供的定位信息处理方法第三实施例的流程示意图；
图6是本申请提供的步骤64一实施例的流程示意图；
图7是本申请提供的定位信息处理方法一应用场景示意图；
图8是本申请提供的定位信息处理方法一应用场景示意图；
图9是本申请提供的定位信息处理装置一实施例的结构示意图；
图10是本申请提供的计算机可读存储介质一实施例的结构示意图。
具体实施方式
下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述。可以理解的是，此处所描述的具体实施例仅用于解释本申请，而非对本申请的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与本申请相关的部分而非全部结构。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。
在本文中提及“实施例”意味着，结合实施例描述的特定特征、结构或特性可以包含在本申请的至少一个实施例中。在说明书中的各个位置出现该短语并不一定均是指相同的实施例，也不是与其它实施例互斥的独立的或备选的实施例。本领域技术人员显式地和隐式地理解的是，本文所描述的实施例可以与其它实施例相结合。
参阅图1，图1是本申请提供的定位信息处理方法第一实施例的流程示意图。该方法包括：
步骤11：获取第一定位系统的第一状态信息，以及获取第二定位系统的第二状态信息；其中，第一定位系统的定位精度高于第二定位系统的定位精度；状态信息包括位置、速度和姿态。
在一些实施例中，第一状态信息和第二状态可以是第一定位系统和第二定位系统在同一环境下获得。如，在同一路线A中，第一定位系统采集到第一状态信息，以及第二定位系统采集到第二状态信息。
在一些实施例中，第一定位系统可以和第二定位系统同时设置于同一装置中，如设置于同一车辆上，则第一定位系统可以和第二定位系统可分别采集到第一状态信息和第二状态。若第一定位系统和第二定位系统的采集频率和起始采集时间相同，则同一时刻可以同时采集到第一状态信息和第二状态。
在一些实施例中，以定位系统应用于车辆为例，第一定位系统可以包括第一惯导、基础定位模块、轮速采集模块。第二定位系统可以包括第二惯导、基础定位模块、轮速采集模块和激光雷达定位模块。惯导由三轴加速度计与三轴陀螺仪组成，其中，惯导可以是IMU(Inertial Measurement Unit，惯性测量单元)。
其中，第一定位系统和第二定位系统可以共用基础定位模块、轮速采集模块。其中，基础定位模块可以是GNSS(Global Navigation Satellite System，全球导航卫星系统)，轮速采集模块可以采集车辆的车轮速度。
其中，第一惯导的定位精度高于第二惯导的定位精度，因此，状态信息可以表示为惯导的位置、速度和姿态。
步骤12：确定第一状态信息和第二状态信息之间的误差。
因状态信息包括位置、速度和姿态，则第一状态信息包括每一时刻的第一位置、第一速度和第一姿态。第二状态信息包括每一时刻的第二位置、第二速度和第二姿态。
则分别确定第一位置和第二位置之间的位置误差，确定第一速度和第二速度之间的速度误差，确定第一姿态和第二姿态之间的姿态误差。
步骤13：基于误差，确定第二定位系统的实际定位精度。
根据上述的位置误差、速度误差和姿态误差，确定出第二定位系统的实际定位精度。在确定出第二定位系统的实际定位精度后，因定位系统的状态信息是利用相应的软件算法确定出的，则可以基于上述误差，对第二定位系统的软件算法中的相关参数进行修正，进而提升第二定位系统的定位精度。
在本实施例中，通过获取第一定位系统的第一状态信息，以及获取第二定位系统的第二状态信息；其中，第一定位系统的定位精度高于第二定位系统的定位精度；状态信息包括位置、速度和姿态；确定第一状态信息和第二状态信息之间的误差；基于误差，确定第二定位系统的实际定位精度的方式，利用高精度的第一定位系统作为基准，对低精度的第二定位系统进行定位精度的确定，能够实现对第二定位系统的定位精度的评价，便于后续对第二定位系统的修正。
在一些实施例中，第一定位系统可以包括第一惯导、基础定位模块、轮速采集模块。第二定位系统可以包括第二惯导、基础定位模块、轮速采集模块和激光雷达定位模块。第一惯导的定位精度高于第二惯导的定位精度，为确定第二惯导的定位精度，本申请提出以下技术方案。
参阅图2，图2是本申请提供的定位信息处理方法第二实施例的流程示意图。该方法包括：
步骤21：获取第一惯导检测的第一惯导数据，以及获取基础定位模块采集的第一位置，以及获取轮速采集模块采集的第一轮速。
其中，第一惯导数据可以包括加速度零偏和角速度零偏。在第一惯导设置于车辆上时，则还获取轮速采集模块采集的第一轮速。
步骤22：对第一惯导数据、第一位置和第一轮速使用正向/反向卡尔曼滤波算法进行融合，得到第一惯导的第一状态信息。
在一些实施例中，使用双向滤波算法来处理第一惯导数据、第一位置和第一轮速，输出第一惯导的位置、速度和姿态的估计值。其中，双向滤波算法包含一次正向卡尔曼滤波和一次反向卡尔曼滤波。
将两次滤波估计的位置、速度、姿态、加速度零偏和角速度零偏进行融合，得到第一惯导的第一状态信息。此时得到的第一状态信息相比于单独的正向卡尔曼滤波具有更高精度。
在一应用场景中，可以利用第一惯导配合自动驾驶车辆的雷达传感器采集的激光点云，进行激光雷达地图的构建。因第一惯导为高精度传感器，输出的惯导数据(角速度、加速度)的精度较高，则双向滤波后位置/速度/姿态的精度较高，则构建的激光雷达地图也具有较高的精度。
步骤23：获取第二惯导检测的第二惯导数据，以及获取基础定位模块采集的第一观测位置，以及获取轮速采集模块采集的观测轮速，以及获取激光雷达定位模块采集的第二观测位置和观测姿态。
其中，第二惯导数据可以包括加速度零偏、角速度零偏。在第二惯导设置于车辆上时，则还获取轮速采集模块采集的第二轮速。
步骤24：对第二惯导数据、第一观测位置、观测轮速和/或第二观测位置、观测姿态，使用卡尔曼滤波算法进行融合，得到第二惯导的第二状态信息。
在一些实施例中，在同一时刻存在三个惯导系统变量值，一个为该时刻的预测值，一个为该时刻的观测值，一个为该时刻的估计值。其中，通过第二惯导采集的第二惯导数据可以求出该时刻的观测值，通过上一时刻的估计值可以求出该时刻的预测值，基于该时刻的预测值，以及该时刻的观测值求出该时刻的估计值，然后基于估计值可以求出第二惯导的状态。
如，利用GNSS/轮速/激光雷达观测位置/速度/姿态、通过卡尔曼滤波(多传感器融合定位算法)来估计出第二惯导的位置/速度/姿态。
在一些实施例中，将第二惯导采集的第二惯导数据看作是线性模型，则上一时刻与当前时刻成线性关系，则可根据上一时刻的第二惯导数据预测得到当前时刻的预测惯导数据。
在一些实施例中，参阅图3，步骤24可以是以下流程：
步骤241：利用第二惯导上一时刻的估计位置、估计速度、估计姿态和第二惯导数据进行预测，以得到第二惯导在当前时刻的预测位置、预测速度和预测姿态。
在一些实施例中，采用以下方式确定预测姿态。如，可以利用第二惯导数据的角速度数据和第二惯导上一时刻的估计姿态，来获得第二惯导在当前时刻的预测姿态。
在一些实施例中，利用轮速采集模块上一时刻的观测轮速进行转换，转换为第二惯导上一时刻的观测速度。利用第二惯导上一时刻的观测速度和预测速度，确定第二惯导在上一时刻的估计速度。
在一些实施例中，在轮速采集模块采集到轮速时(此时为观测值)，因轮速采集模块与第二惯导存在位置上的偏差，则对轮速进行转换，转换为第二惯导对应的观测速度。其中，轮速采集模块和第二惯导之间的转换关系可以提前标定。
在一些实施例中，采用以下方式确定预测速度。如，可以利用第二惯导的预测姿态和重力加速度，对第二惯导数据的加速度进行处理，得到目标加速度。即，通过利用第二惯导的预测姿态和重力加速度来消除重力加速度对第二惯导数据中的加速度的影响，得到目标加速度。
然后利用第二惯导在上一时刻的估计速度和目标加速度，得到第二惯导当前时刻的预测速度。
在一些实施例中，采用以下方式确定预测位置。如，可以利用第二惯导在上一时刻的估计位置和第二惯导的估计速度，得到第二惯导当前时刻的预测位置。
在一些实施例中，基础定位模块和激光雷达定位模块可以同时使用，也可以单独使用。
如，在基础定位模块采集到第二位置时(此时为观测值)，因基础定位模块与第二惯导存在位置上的偏差，则对第二位置进行转换，转换为第二惯导对应的观测位置。其中，基础定位模块和第二惯导之间的转换关系可以提前标定。
又如，在激光雷达定位模块采集到第三位置时(此时为观测值)，因激光雷达定位模块与第二惯导存在位置上的偏差，则对第三位置进行转换，转换为第二惯导对应的观测位置。其中，激光雷达定位模块和第二惯导之间的转换关系可以提前标定。
进一步，可以利用第二惯导对应的观测位置确定出估计位置数据。具体地，在可以利用上一时刻的估计位置确定出当前时刻的预测位置，然后利用当前时刻的观测位置和预测位置确定出估计位置数据。
步骤242：根据当前时刻的第一观测位置、观测姿态、观测轮速和/或第二观测位置，以及第二惯导当前时刻的预测位置、预测速度和预测姿态进行卡尔曼滤波处理，得到第二惯导的第二状态信息。
在一些实施例中，参阅图4，步骤242可以是以下流程：
步骤2421：利用第一观测位置和预测位置，和/或利用第二观测位置和预测位置，确定第二惯导对应的位置误差观测值。
步骤2422：利用观测轮速和预测速度，确定第二惯导对应的速度误差观测值。
步骤2423：利用观测姿态和预测姿态，确定第二惯导对应的姿态误差观测值。
步骤2424：根据位置误差观测值、速度误差观测值和姿态误差观测值，利用卡尔曼滤波算法，计算得到当前时刻的估计位置误差、估计速度误差、估计姿态误差。
在一些实施例中，可以获取第二惯导的状态的输出矩阵。
可以理解，第二惯导的输出矩阵可以成为第二惯导的惯导数据与系统变量之间的系数。
然后利用输出矩阵和位置误差观测值、速度误差观测值和姿态误差观测值计算得到当前时刻的估计位置误差、估计速度误差、估计姿态误差。
可以理解，每个时刻的估计位置误差、估计速度误差、估计姿态误差均不相同，每个时刻的估计位置误差、估计速度误差、估计姿态误差均需要利用输出矩阵和位置误差观测值、速度误差观测值和姿态误差观测值进行计算得到。
步骤2425：利用第二惯导当前时刻的预测位置、预测速度、预测姿态数据、估计位置误差、估计速度误差和估计姿态误差，来计算第二惯导当前时刻的估计位置、估计速度和估计姿态。
如，利用当前时刻的预测位置和估计位置误差，来计算第二惯导当前时刻的估计位置。
如，利用第二惯导当前时刻的预测速度和估计速度误差，来计算第二惯导当前时刻的估计速度。
如，利用第二惯导当前时刻的预测姿态数据和估计姿态误差，来计算第二惯导当前时刻的估计姿态。
在一些实施例中，可以采用多传感器融合定位算法进行计算，得到第二惯导当前时刻的估计位置、估计速度和估计姿态，即第二惯导的第二状态信息。
步骤25：确定第一状态信息和第二状态信息之间的误差。
步骤26：基于误差，确定第二定位系统的实际定位精度。
基于误差，确定第二定位系统的实际定位精度，可以根据第二定位系统的实际定位精度，进而确定第二惯导的实际定位精度，若实际定位精度较低，则可以选择不使用该第二惯导。
在本实施例中，利用高精度的第一定位系统作为基准，对低精度的第二定位系统进行定位精度的确定，能够实现对第二定位系统的定位精度的评价，便于后续对第二定位系统的修正。
参阅图5，图5是本申请提供的定位信息处理方法第三实施例的流程示意图。该方法包括：
步骤51：获取第一定位系统的第一状态信息，以及获取第二定位系统的第二状态信息；其中，第一定位系统的定位精度高于第二定位系统的定位精度；状态信息包括位置、速度和姿态。
步骤51与上述任一实施例具有相同或相似的技术方案，这里不在赘述。
步骤52：获取第一定位系统和第二定位系统的变换矩阵。
其中，因第一定位系统可以包括第一惯导、基础定位模块、轮速采集模块。第二定位系统可以包括第二惯导、基础定位模块、轮速采集模块。基础定位模块、轮速采集模块为共用模块，则只需要对第一惯导和第二惯导确定变换矩阵。
具体地，在安装第一惯导时，确定第一惯导的第一坐标。以及在安装第二惯导时，确定第二惯导的第二坐标。
然后根据第一坐标和第二坐标确定变换矩阵。如，利用第一坐标和第二坐标确定齐次变换矩阵。
步骤53：利用变换矩阵将第一状态信息投影至第二状态信息的坐标下，得到第三状态信息。
步骤54：确定第三状态信息和第二状态信息之间的误差。
在一些实施例中，参阅图6，步骤54可以是以下流程：
步骤541：确定第二状态信息对应的采集时刻。
步骤542：若采集时刻介于相邻两个第一状态信息的采集时刻之间；利用相邻两个第一状态信息对应的第三状态信息进行插值计算，得到对应采集时刻的第三状态信息。
可以理解，因第三状态由第一状态转换而来，因此，采集时刻是相同的。
在一些实施例中，因第一惯导和第二惯导的硬件差异，则可能存在采集时刻的不同。如，第一惯导的采集时刻为0、2、4。第二惯导的采集时刻为1、3、5，则两者之间的状态信息并不是在同一时刻采集的。即，对应的位置、姿态、速度也不相同。为了减少误差，则将相邻两个第一状态信息对应的第三状态信息进行插值计算，得到对应采集时刻的第三状态信息。
即，基于第三状态信息，确定出第一惯导在第二状态信息对应的采集时刻的状态信息。
结合图7进行说明：
如图7所示，第一惯导分别在T0时刻、T1时刻、T2时刻采集到第一状态信息。第二惯导分别在t0时刻、t1时刻、t2时刻采集到第二状态信息。
其中，T0<t0<T1，T1<t1<T2。即，第一惯导和第二惯导之间的状态信息并不是在同一时刻采集的。因此，将相邻两个第一状态信息对应的第三状态信息进行插值计算，得到对应采集时刻的第三状态信息。即，计算出第一惯导对应在t0时刻、t1时刻、t2时刻的状态信息。
如，第一惯导的位置插值表示为：pt＝(1-t)p0+tp1。其中，pt表示第一惯导在t时刻对应第二惯导坐标系的位置信息，p0表示第一惯导在0时刻对应第二惯导坐标系的位置信息，p1表示第一惯导在1时刻对应第二惯导坐标系的位置信息。
第一惯导的速度插值表示为：vt＝(1-t)v0+tv1。其中，vt表示第一惯导在t时刻对应第二惯导坐标系的速度信息，v0表示第一惯导在0时刻对应第二惯导坐标系的速度信息，v1表示第一惯导在1时刻对应第二惯导坐标系的速度信息。
对于姿态，由于欧拉角非连续，因此本申请采用四元数的姿态表述方式。首先将方向余弦矩阵转换为四元数，然后采用正规化线性插值计算姿态，具体公式为：
qt＝(1-t)q0+tq1/||(1-t)q0+tq1||。其中，qt表示第一惯导在t时刻对应第二惯导坐标系的姿态信息，q0表示第一惯导在0时刻对应第二惯导坐标系的姿态信息，q1表示第一惯导在1时刻对应第二惯导坐标系的姿态信息。
步骤543：确定采集时刻的第三状态信息和第二状态信息之间的误差。
步骤55：基于误差，确定第二定位系统的实际定位精度。
在本实施例中，利用高精度的第一定位系统作为基准，对低精度的第二定位系统进行定位精度的确定，能够实现对第二定位系统的定位精度的评价，便于后续对第二定位系统的修正。且，通过对第一惯导和第二惯导进行位姿变换，能够使第一惯导和第二惯导在同一维度上，进行能够提升确定第二定位系统的实际定位精度的准确性。
在一应用场景中，参阅图8，使用双向滤波算法来处理第一惯导、GNSS和轮速数据，输出第一惯导的位置、速度和姿态的估计值。双向滤波算法包含一次正向卡尔曼滤波和一次反向卡尔曼滤波，并将两次滤波估计的位置、速度、姿态、加速度计零偏和角速度零偏进行融合，获得相比于单独的正向卡尔曼滤波更高精度的估计结果。
使用激光雷达的点云数据和第一惯导的位置、速度和姿态的参考值，可以计算点云数据中的每个激光点的具体位置，从而建立可靠的激光点云地图。
多传感器融合定位算法使用第二惯导、GNSS、轮速、激光雷达和点云地图数据，估计出第二惯导的位置、速度和姿态。其中，融合算法采用误差状态卡尔曼滤波，误差状态为位置误差、速度误差、姿态误差、加速度零偏、角速度零偏和轮速比例因子。根据第二惯导的位置、速度、姿态和第二惯导输出的加速度、角速度预测下一步的误差状态，再根据观测的GNSS位置、轮速、激光雷达和激光点云地图匹配出第二惯导的位置和姿态，来进行测量更新。
具体的，参阅下述方程以进行理解。公式如下所示：
方程1：
Figure BDA0003569213490000131
其中，
Figure BDA0003569213490000132
表示当前时刻系统变量的预测值，
Figure BDA0003569213490000133
表示上一时刻的系统变量的更新值，Φk/k-1表示状态转移矩阵。即方程1可以表示当前时刻的状态预测。
方程2：
Figure BDA0003569213490000134
Pk/k-1表示当前时刻系统变量的预测值与当前时刻系统变量的观测值之间的误差，Pk-1表示上一时刻的系统变量的估计值与上一时刻的系统变量的观测值之间的误差，Γk-1为观测噪声矩阵，Qk-1为传感器测量误差，具体表示为当测量过程中出现扰动时带来的误差，即可表征第二惯导的性能指标。
方程3：
Figure BDA0003569213490000135
Kk表示当前时刻的增益值，R为设定值，用于表示对传感器采集的观测值的置信度。
方程4：
Figure BDA0003569213490000136
Figure BDA0003569213490000137
表示当前时刻的系统变量的估计值，Zk表示观测误差。
方程5：Pk＝(I-KkHk)Pk/k-1。Pk表示当前时刻的系统变量的估计值与当前时刻的系统变量的观测值之间的误差，I为单位矩阵。
然后，第一惯导的位置、速度和姿态通过位姿变换投影到第二惯导的坐标系下。如，假设第一惯导的位置、速度、姿态和角速度分别为
Figure BDA0003569213490000138
Figure BDA0003569213490000141
和
Figure BDA0003569213490000142
变换后的第一惯导的位置、速度和姿态分别为
Figure BDA0003569213490000143
Figure BDA0003569213490000144
和
Figure BDA0003569213490000145
第二惯导相对于第一惯导的位置和姿态分别为
Figure BDA0003569213490000146
和
Figure BDA0003569213490000147
因此，第一惯导和第二惯导之间的位置变换为：
Figure BDA0003569213490000148
第一惯导和第二惯导之间的速度变换为：
Figure BDA0003569213490000149
第一惯导和第二惯导之间的姿态变换为：
Figure BDA00035692134900001410
其中，为了与多传感器融合定位算法估计的位姿时间对齐，需要对变换后的第一惯导的位置、速度、姿态数据进行时间线性插值。
假设在第一惯导的位姿在0时刻和1时刻有估计值，对应的第二惯导的位姿在t(0<t≤1)时刻有估计值，则第一惯导的位姿的插值方法为：
第一惯导的位置插值表示为：pt＝(1-t)p0+tp1。
第一惯导的速度插值表示为：vt＝(1-t)v0+tv1。
对于姿态，由于欧拉角非连续，因此本申请采用四元数的姿态表述方式。首先将方向余弦矩阵转换为四元数，然后采用正规化线性插值计算姿态，具体公式为：
qt＝(1-t)q0+tq1/||(1-t)q0+tq1||。
在自动驾驶定位领域，利用高精度的定位系统确定出激光点云地图，进而利用此时的激光点云地图自动驾驶车辆的低精度的定位系统按照上述方式进行精度确定，能够提高确定低精度的定位系统的实际定位精度的准确性。
参阅图9，图9是本申请提供的定位信息处理装置一实施例的结构示意图。该定位信息处理装置90包括处理器91以及与处理器91耦接的存储器92，存储器92用于存储计算机程序，处理器91用于执行计算机程序，以实现以下方法：
获取第一定位系统的第一状态信息，以及获取第二定位系统的第二状态信息；其中，第一定位系统的定位精度高于第二定位系统的定位精度；状态信息包括位置、速度和姿态；确定第一状态信息和第二状态信息之间的误差；基于误差，确定第二定位系统的实际定位精度。
可以理解，处理器91用于执行计算机程序，以实现上述任一实施例的技术方案，这里不做赘述。
在一些实施例中，定位信息处理装置90可以是自动驾驶车辆中的车载控制系统。或，定位信息处理装置90与自动驾驶车辆中的车载控制系统连接。
参阅图10，图10是本申请提供的计算机可读存储介质一实施例的结构示意图。该计算机可读存储介质100用于存储计算机程序101，计算机程序101在被处理器执行时，用于实现以下方法：
获取第一定位系统的第一状态信息，以及获取第二定位系统的第二状态信息；其中，第一定位系统的定位精度高于第二定位系统的定位精度；状态信息包括位置、速度和姿态；确定第一状态信息和第二状态信息之间的误差；基于误差，确定第二定位系统的实际定位精度。
可以理解，计算机程序101在被处理器执行时，还用于实现上述任一实施例的技术方案，这里不做赘述。
综上所述，本申请通过上述任一实施例的技术方案，使用双向滤波算法，来融合高精度惯导、GNSS和轮速信息，输出可信的高精度惯导的位姿信息，作为真值，实现良好的自动化处理；根据后处理输出的位姿，来建立可靠的激光点云地图；将低精度惯导、GNSS、轮速、激光点云的数据通过卡尔曼滤波进行多传感器融合位姿估计，输出算法估计的低精度惯导的位姿信息；将高精度惯导的位姿投影到低精度惯导处；将空间变换后的高精度惯导的位姿与低精度惯导的参考位姿进行对比，计算低精度惯导的位姿估计误差，即定位算法的估计精度，进而能够对定位算法调整参数。
在本申请所提供的几个实施方式中，应该理解到，所揭露的方法以及设备，可以通过其它的方式实现。例如，以上所描述的设备实施方式仅仅是示意性的，例如，所述电路或单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。
所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施方式方案的目的。
另外，在本申请各个实施方式中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。
以上所述仅为本申请的实施方式，并非因此限制本申请的专利范围，凡是根据本申请说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本申请的专利保护范围内。

Claims (10)

1.一种定位信息处理方法，其特征在于，所述方法包括：
获取第一定位系统的第一状态信息，以及获取第二定位系统的第二状态信息；其中，所述第一定位系统的定位精度高于所述第二定位系统的定位精度；所述状态信息包括位置、速度和姿态；
确定所述第一状态信息和所述第二状态信息之间的误差；
基于所述误差，确定所述第二定位系统的实际定位精度。
2.根据权利要求1所述的方法，其特征在于，所述第一定位系统包括：第一惯导、基础定位模块、轮速采集模块；
所述获取第一定位系统的第一状态信息，包括：
获取所述第一惯导检测的第一惯导数据，以及获取所述基础定位模块采集的第一位置，以及获取所述轮速采集模块采集的第一轮速；
对所述第一惯导数据、所述第一位置和所述第一轮速使用正向/反向卡尔曼滤波算法进行融合，得到所述第一惯导的所述第一状态信息。
3.根据权利要求1所述的方法，其特征在于，所述第二定位系统包括：第二惯导、基础定位模块、轮速采集模块和激光雷达定位模块；
所述获取第二定位系统的第二状态信息，包括：
获取所述第二惯导检测的第二惯导数据，以及获取所述基础定位模块采集的第一观测位置，以及获取所述轮速采集模块采集的观测轮速，以及获取所述激光雷达定位模块采集的第二观测位置和观测姿态；
对所述第二惯导数据、所述第一观测位置、所述观测轮速和/或所述第二观测位置、所述观测姿态，使用卡尔曼滤波算法进行融合，得到所述第二惯导的所述第二状态信息。
4.根据权利要求3所述的方法，其特征在于，所述对所述第二惯导数据、所述第一观测位置、所述观测轮速和/或所述第二观测位置、所述观测姿态数据，使用卡尔曼滤波算法进行融合，得到所述第二惯导的所述第二状态信息，包括：
利用所述第二惯导上一时刻的估计位置、估计速度、估计姿态和第二惯导数据进行预测，以得到所述第二惯导在当前时刻的预测位置、预测速度和预测姿态；
根据当前时刻的所述第一观测位置、所述观测姿态、所述观测轮速和/或所述第二观测位置，以及所述第二惯导当前时刻的预测位置、预测速度和预测姿态进行卡尔曼滤波处理，得到所述第二惯导的所述第二状态信息。
5.根据权利要求4所述的方法，其特征在于，所述利用所述第二惯导上一时刻的估计位置、估计速度、估计姿态和第二惯导数据进行预测，以得到所述第二惯导在当前时刻的预测位置、预测速度和预测姿态，包括：
利用所述第二惯导数据的角速度数据和第二惯导上一时刻的估计姿态，来获得所述第二惯导在当前时刻的预测姿态；
利用所述第二惯导的预测姿态和重力加速度，对所述第二惯导数据的加速度进行处理，得到目标加速度；
利用所述第二惯导在上一时刻的估计速度和所述目标加速度，得到所述第二惯导当前时刻的所述预测速度；
利用所述第二惯导在上一时刻的估计位置和第二惯导的估计速度，得到所述第二惯导当前时刻的所述预测位置。
6.根据权利要求4所述的方法，其特征在于，所述根据当前时刻的所述第一观测位置、所述观测姿态、所述观测轮速和/或所述第二观测位置，以及所述第二惯导当前时刻的预测位置、预测速度和预测姿态进行卡尔曼滤波处理，得到所述第二惯导的所述第二状态信息，包括：
利用所述第一观测位置和所述预测位置，和/或利用所述第二观测位置和所述预测位置，确定所述第二惯导对应的位置误差观测值；
利用所述观测轮速和所述预测速度，确定所述第二惯导对应的速度误差观测值；
利用所述观测姿态和所述预测姿态，确定所述第二惯导对应的姿态误差观测值；
根据所述位置误差观测值、所述速度误差观测值和所述姿态误差观测值，利用卡尔曼滤波算法，计算得到当前时刻的估计位置误差、估计速度误差、估计姿态误差；
利用第二惯导当前时刻的所述预测位置、所述预测速度、所述预测姿态数据、所述估计位置误差、所述估计速度误差和所述估计姿态误差，来计算第二惯导当前时刻的估计位置、估计速度和估计姿态。
7.根据权利要求1所述的方法，其特征在于，所述确定所述第一状态信息和所述第二状态信息之间的误差，包括：
获取所述第一定位系统和所述第二定位系统的变换矩阵；
利用所述变换矩阵将所述第一状态信息投影至所述第二状态信息的坐标下，得到第三状态信息；
确定所述第三状态信息和所述第二状态信息之间的误差。
8.根据权利要求7所述的方法，其特征在于，所述确定所述第三状态信息和所述第二状态信息之间的误差，包括：
确定所述第二状态信息对应的采集时刻；
若所述采集时刻介于相邻两个所述第一状态信息的采集时刻之间；
利用相邻两个所述第一状态信息对应的第三状态信息进行插值计算，得到对应所述采集时刻的第三状态信息；
确定所述采集时刻的第三状态信息和所述第二状态信息之间的误差。
9.一种定位信息处理装置，其特征在于，所述定位信息处理装置包括处理器以及与所述处理器耦接的存储器，所述存储器用于存储计算机程序，所述处理器用于执行所述计算机程序，以实现如权利要求1-8任一项所述的方法。
10.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质用于存储计算机程序，所述计算机程序在被处理器执行时，用于实现如权利要求1-8任一项所述的方法。
CN202210316889.9A 2022-03-28 2022-03-28 定位信息处理方法、装置以及计算机可读存储介质 Pending CN114910067A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210316889.9A CN114910067A (zh) 2022-03-28 2022-03-28 定位信息处理方法、装置以及计算机可读存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210316889.9A CN114910067A (zh) 2022-03-28 2022-03-28 定位信息处理方法、装置以及计算机可读存储介质

Publications (1)

Publication Number Publication Date
CN114910067A true CN114910067A (zh) 2022-08-16

Family

ID=82763488

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210316889.9A Pending CN114910067A (zh) 2022-03-28 2022-03-28 定位信息处理方法、装置以及计算机可读存储介质

Country Status (1)

Country Link
CN (1) CN114910067A (zh)

Similar Documents

Publication Publication Date Title
CN108731670B (zh) 基于量测模型优化的惯性/视觉里程计组合导航定位方法
JP2021177168A (ja) 車両のデッドレコニング方法、装置、デバイス、記憶媒体、及びプログラム
CN109059907B (zh) 轨迹数据处理方法、装置、计算机设备和存储介质
CN112835085B (zh) 确定车辆位置的方法和装置
WO2020189079A1 (ja) 自己位置推定装置、それを備えた自動運転システム、および、自己生成地図共有装置
CN110715659A (zh) 零速检测方法、行人惯性导航方法、装置及存储介质
CN115143954B (zh) 一种基于多源信息融合的无人车导航方法
CN114264301B (zh) 车载多传感器融合定位方法、装置、芯片及终端
CN112946681B (zh) 融合组合导航信息的激光雷达定位方法
CN112146655A (zh) 一种BeiDou/SINS紧组合导航系统弹性模型设计方法
CN113566850B (zh) 惯性测量单元的安装角度标定方法、装置和计算机设备
JP2014240266A (ja) センサドリフト量推定装置及びプログラム
CN116399351A (zh) 一种车辆位置估计方法
CN114323007A (zh) 一种载体运动状态估计方法及装置
CN112835086B (zh) 确定车辆位置的方法和装置
CN114897942B (zh) 点云地图的生成方法、设备及相关存储介质
CN111982126A (zh) 一种全源BeiDou/SINS弹性状态观测器模型设计方法
CN106886037A (zh) 适用于弱gnss信号条件的pos数据纠偏方法
CN114019954B (zh) 航向安装角标定方法、装置、计算机设备和存储介质
CN114001730B (zh) 融合定位方法、装置、计算机设备和存储介质
CN114088104B (zh) 一种自动驾驶场景下的地图生成方法
CN114910067A (zh) 定位信息处理方法、装置以及计算机可读存储介质
CN114895340A (zh) 双天线gnss/ins组合导航系统的定位方法和装置
Do et al. An Improvement of 3D DR/INS/GNSS Integrated System using Inequality Constrained EKF
CN113566849B (zh) 惯性测量单元的安装角度标定方法、装置和计算机设备

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