CN114001730A - 融合定位方法、装置、计算机设备和存储介质 - Google Patents

融合定位方法、装置、计算机设备和存储介质 Download PDF

Info

Publication number
CN114001730A
CN114001730A CN202111122685.3A CN202111122685A CN114001730A CN 114001730 A CN114001730 A CN 114001730A CN 202111122685 A CN202111122685 A CN 202111122685A CN 114001730 A CN114001730 A CN 114001730A
Authority
CN
China
Prior art keywords
course
observable
information
predicted
observability
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.)
Granted
Application number
CN202111122685.3A
Other languages
English (en)
Other versions
CN114001730B (zh
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 CN202111122685.3A priority Critical patent/CN114001730B/zh
Publication of CN114001730A publication Critical patent/CN114001730A/zh
Application granted granted Critical
Publication of CN114001730B publication Critical patent/CN114001730B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12Navigation; 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/16Navigation; 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/165Navigation; 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
    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12Navigation; 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/14Navigation; 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 recording the course traversed by the object
    • GPHYSICS
    • G01MEASURING; TESTING
    • G01SRADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42Determining position
    • GPHYSICS
    • G01MEASURING; TESTING
    • G01SRADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42Determining position
    • G01S19/45Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial

Landscapes

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

Abstract

本申请涉及一种融合定位方法、装置、计算机设备和存储介质。所述方法包括:获取与当前时刻对应的观测导航定位信息、速度变化信息以及实际行驶参数;当根据速度变化信息确定航向弱可观时,获取航向增量,航向弱可观是指航向可观性不满足预设航向可观性指标,航向增量用于表征航向变化;根据航向增量、观测导航定位信息、速度变化信息以及实际行驶参数,得到与预测时刻对应的弱可观预测定位结果;获取与预测时刻对应的观测导航定位信息;根据与预测时刻对应的观测导航定位信息和弱可观预测定位结果,得到与预测时刻对应的目标定位结果。采用本方法能够在利用速度变化信息确定航向弱可观后,引入航向增量来进行预测实现减小估计误差。

Description

融合定位方法、装置、计算机设备和存储介质
技术领域
本申请涉及自动驾驶技术领域,特别是涉及一种融合定位方法、装置、计算机设备和存储介质。
背景技术
随着自动驾驶技术的发展,出现了定位算法,定位算法能够为自动驾驶车辆提供位置、速度、姿态等信息,是自动驾驶技术的重要一环。
传统技术中,常用的定位算法为GNSS/INS(Global Navigation SatelliteSystem/Inertial Navigation System,卫星/惯性导航系统)组合定位算法,GNSS/INS组合定位算法是指通过全球导航卫星系统和惯性导航系统来进行组合定位,估计位置、速度以及姿态。
然而,传统方法,对于位置、速度、姿态的估计存在可观性的问题,在卫星/惯性导航系统可观性较好时,能良好地估计出状态量,而在卫星/惯性导航系统可观性较差时,这些状态的估计量往往不太准确,存在估计误差大的问题。
发明内容
基于此,有必要针对上述技术问题,提供一种能够减小估计误差的融合定位方法、装置、计算机设备和存储介质。
一种融合定位方法,所述方法包括:
获取与当前时刻对应的观测导航定位信息、速度变化信息以及实际行驶参数;
当根据速度变化信息确定航向弱可观时,获取航向增量,航向弱可观是指航向可观性不满足预设航向可观性指标,航向增量用于表征航向变化;
根据航向增量、观测导航定位信息、速度变化信息以及实际行驶参数,得到与预测时刻对应的弱可观预测定位结果;
获取与预测时刻对应的观测导航定位信息;
根据与预测时刻对应的观测导航定位信息和弱可观预测定位结果,得到与预测时刻对应的目标定位结果。
在一个实施例中,融合定位方法还包括:
当缓存中存在与航向强可观对应的速度变化零偏时,根据速度变化零偏对速度变化信息进行修正。
在一个实施例中,融合定位方法还包括:
根据速度变化信息中角速度信息以及加速度信息,得到航向可观性评估值;
比对航向可观性评估值和预设航向可观性指标;
当航向可观性评估值满足预设航向可观性指标时,确定航向可观性满足预设航向可观性指标,航向可观性为航向强可观;
当航向可观性评估值不满足预设航向可观性指标时,确定航向可观性不满足预设航向可观性指标,航向可观性为航向弱可观。
在一个实施例中,实际行驶参数包括实际行驶姿态以及实际行驶速度;
根据航向增量、观测导航定位信息、速度变化信息以及实际行驶参数,得到与预测时刻对应的弱可观预测定位结果包括:
根据航向增量、实际行驶姿态以及速度变化信息中角速度信息,得到与预测时刻对应的预测姿态信息;
根据预测姿态信息、实际行驶速度以及速度变化信息中加速度计测量的比力,得到与预测时刻对应的预测速度信息;
根据预测速度信息、观测导航定位信息以及实际行驶速度,得到预测位置信息;
归集预测姿态信息、预测速度信息以及预测位置信息,得到与预测时刻对应的弱可观预测定位结果。
在一个实施例中,根据与预测时刻对应的观测导航定位信息和弱可观预测定位结果,得到与预测时刻对应的目标定位结果包括:
根据与预测时刻对应的观测导航定位信息和弱可观预测定位结果中预测位置信息,得到弱可观位置误差;
根据弱可观位置误差利用误差状态卡尔曼滤波,得到弱可观预测定位误差;
根据弱可观预测定位结果和弱可观预测定位误差,得到与预测时刻对应的目标定位结果。
在一个实施例中,获取与当前时刻对应的观测导航定位信息、速度变化信息以及实际行驶参数之后,还包括:
当根据速度变化信息确定航向强可观时,根据观测导航定位信息、速度变化信息以及实际行驶参数,得到与预测时刻对应的强可观预测定位结果,航向强可观是指航向可观性满足预设航向可观性指标;
获取与预测时刻对应的观测导航定位信息;
根据与预测时刻对应的观测导航定位信息和强可观预测定位结果,得到与预测时刻对应的目标定位结果。
在一个实施例中,根据与预测时刻对应的观测导航定位信息和强可观预测定位结果,得到与预测时刻对应的目标定位结果包括:
根据与预测时刻对应的观测导航定位信息和强可观预测定位结果中预测位置信息,得到强可观位置误差;
根据强可观位置误差,利用误差状态卡尔曼滤波,得到强可观预测定位误差以及与航向强可观对应的速度变化零偏;
根据强可观预测定位结果和强可观预测定位误差,得到与预测时刻对应的目标定位结果,并缓存与航向强可观对应的速度变化零偏。
一种融合定位装置,所述装置包括:
第一获取模块,用于获取与当前时刻对应的观测导航定位信息、速度变化信息以及实际行驶参数;
处理模块,用于当根据速度变化信息确定航向弱可观时,获取航向增量,航向弱可观是指航向可观性不满足预设航向可观性指标,航向增量用于表征航向变化;
预测模块,用于根据航向增量、观测导航定位信息、速度变化信息以及实际行驶参数,得到与预测时刻对应的弱可观预测定位结果;
第二获取模块,用于获取与预测时刻对应的观测导航定位信息;
修正模块,用于根据与预测时刻对应的观测导航定位信息和弱可观预测定位结果,得到与预测时刻对应的目标定位结果。
一种计算机设备,包括存储器和处理器,所述存储器存储有计算机程序,所述处理器执行所述计算机程序时实现以下步骤:
获取与当前时刻对应的观测导航定位信息、速度变化信息以及实际行驶参数;
当根据速度变化信息确定航向弱可观时,获取航向增量,航向弱可观是指航向可观性不满足预设航向可观性指标,航向增量用于表征航向变化;
根据航向增量、观测导航定位信息、速度变化信息以及实际行驶参数,得到与预测时刻对应的弱可观预测定位结果;
获取与预测时刻对应的观测导航定位信息;
根据与预测时刻对应的观测导航定位信息和弱可观预测定位结果,得到与预测时刻对应的目标定位结果。
一种计算机可读存储介质,其上存储有计算机程序,所述计算机程序被处理器执行时实现以下步骤:
获取与当前时刻对应的观测导航定位信息、速度变化信息以及实际行驶参数;
当根据速度变化信息确定航向弱可观时,获取航向增量,航向弱可观是指航向可观性不满足预设航向可观性指标,航向增量用于表征航向变化;
根据航向增量、观测导航定位信息、速度变化信息以及实际行驶参数,得到与预测时刻对应的弱可观预测定位结果;
获取与预测时刻对应的观测导航定位信息;
根据与预测时刻对应的观测导航定位信息和弱可观预测定位结果,得到与预测时刻对应的目标定位结果。
上述融合定位方法、装置、计算机设备和存储介质,通过获取与当前时刻对应的观测导航定位信息、速度变化信息以及实际行驶参数,能够利用速度变化信息对航向可观性进行判断,并在航向弱可观,即航向可观性不满足预设航向可观性指标时,获取用于描述航向变化的航向增量,利用航向增量、观测导航定位信息、速度变化信息以及实际行驶参数,实现对预测时刻的定位结果的准确预测,得到弱可观预测定位结果,通过获取与预测时刻对应的观测导航定位信息,能够结合与预测时刻对应的观测导航定位信息和弱可观预测定位结果,得到准确的、与预测时刻对应的目标定位结果。整个过程,能够在利用速度变化信息确定航向弱可观后,引入航向增量来进行预测实现减小估计误差。
附图说明
图1为一个实施例中融合定位方法的流程示意图;
图2为另一个实施例中融合定位方法的流程示意图;
图3为又一个实施例中融合定位方法的流程示意图;
图4为一个实施例中融合定位装置的结构框图;
图5为一个实施例中计算机设备的内部结构图。
具体实施方式
为了使本申请的目的、技术方案及优点更加清楚明白,以下结合附图及实施例,对本申请进行进一步详细说明。应当理解,此处描述的具体实施例仅仅用以解释本申请,并不用于限定本申请。
在一个实施例中,如图1所示,提供了一种融合定位方法,本实施例以该方法应用于终端进行举例说明,可以理解的是,该方法也可以应用于服务器,还可以应用于包括终端和服务器的系统,并通过终端和服务器的交互实现。其中,终端可以但不限于是各种个人计算机、笔记本电脑、智能手机、平板电脑、便携式可穿戴设备和和智能驾驶车辆计算平台,服务器可以用独立的服务器或者是多个服务器组成的服务器集群来实现。本实施例中,该方法包括以下步骤:
步骤102,获取与当前时刻对应的观测导航定位信息、速度变化信息以及实际行驶参数。
其中,当前时刻是指自动驾驶车辆行驶过程中除了行驶终止时刻外的任意时刻。比如,当前时刻具体可以是指行驶初始时刻。观测导航定位信息是指观测到的、用于对车辆进行定位的定位信息。比如,观测导航定位信息具体可以是指根据全球导航卫星系统等定位系统所观测得到的经纬度位置信息。速度变化信息用于表征车辆的速度变化情况。比如速度变化信息具体可以是指惯性测量单元的加速度计以及陀螺仪所输出的信息。实际行驶参数用于表征车辆实际行驶过程中的状态。比如,实际行驶参数具体可以是指实际行驶姿态以及实际行驶速度。
具体的,全球导航卫星系统接收机可以获取到观测导航定位信息并输出,惯性测量单元可以获取到速度变化信息并输出,车辆上设置的传感器可以获取到实际行驶参数并输出,车辆在行驶时,终端会获取与当前时刻对应的观测导航定位信息、速度变化信息以及实际行驶参数,对预测时刻的姿态、速度以及位置进行估计。其中,预测时刻是指与当前时刻对应的下一时刻。
步骤104,当根据速度变化信息确定航向弱可观时,获取航向增量,航向弱可观是指航向可观性不满足预设航向可观性指标,航向增量用于表征航向变化。
其中,航向可观性是指航向可观测性。在车辆定位时,车辆的状态变量(如位置、速度、姿态)不是由直接测量得到的,而是通过组合定位的观测方法得到的,航向可观测性用于评价通过组合定位的观测方法所得到的状态变量能否真实反映车辆的状态,即可以通过航向可观测性确定车辆的状态变量是否准确。预设航向可观性指标是指预设航向可观性指标是指对航向可观性进行评价的指标。比如,预设航向可观性指标具体可以是指对航向可观性进行评价的航向可观性阈值,该航向可观性阈值可按照需要自行设置。航向增量是指用于表征航向变化情况的航向增加值。比如,航向增量具体可以是指由雷达里程计输出的标准航向变化情况的航向增加值。
具体的,在得到观测导航定位信息、速度变化信息以及实际行驶参数后,终端会根据速度变化信息进行航向可观性判断,当确定航向弱可观,即航向可观性不满足预设航向可观性指标时,终端会获取航向增量,以便结合航向增量进行预测。其中,在根据速度变化信息进行航向可观性判断时,终端需要确认缓存中是否存在与航向强可观对应的速度变化零偏,当存在与航向强可观对应的速度变化零偏时,需要先利用速度变化零偏对速度变化信息进行修正,再根据修正后的速度变化信息进行航向可观性判断。其中,与航向强可观对应的速度变化零偏是指在航向可观性满足预设航向可观性指标时估计得到的零偏,包括陀螺零偏和加速度零偏。
步骤106,根据航向增量、观测导航定位信息、速度变化信息以及实际行驶参数,得到与预测时刻对应的弱可观预测定位结果。
其中,弱可观预测定位结果是指在航向弱可观,即航向可观性不满足预设航向可观性指标时预测得到的定位结果,包括姿态、速度以及位置。
具体的,在进行预测时,终端会先根据航向增量、实际行驶参数中的实际行驶姿态以及速度变化信息中角速度信息,得到预测姿态信息,再根据预测姿态信息、实际行驶参数中实际行驶速度以及速度变化信息中加速度计测量的比力,得到预测速度信息,最后根据预测速度信息、观测导航定位信息以及实际行驶速度,得到预测位置信息,归集预测姿态信息、预测速度信息以及预测位置信息,即可得到弱可观预测定位结果。
步骤108,获取与预测时刻对应的观测导航定位信息。
具体的,在得到弱可观预测定位结果后,终端会获取与预测时刻对应的观测导航定位信息。与预测时刻对应的观测导航定位信息是指在预测时刻所观测到的导航定位结果。
步骤110,根据与预测时刻对应的观测导航定位信息和弱可观预测定位结果,得到与预测时刻对应的目标定位结果。
具体的,终端会根据与预测时刻对应的观测导航定位信息以及弱可观预测定位结果,得到弱可观预测定位误差,利用弱可观预测定位误差对弱可观预测定位结果进行修正,得到与预测时刻对应的目标定位结果。
进一步的,在得到与预测时刻对应的目标定位结果后,若预测时刻不为行驶终止时刻,终端会将预测时刻作为当前时刻,继续对与预测时刻对应的下一时刻进行预测,以得到与预测时刻对应的下一时刻的目标定位结果,直到更新后的预测时刻为行驶终止时刻,得到自动驾驶车辆在行驶过程中每个信息采集时刻的目标定位结果。其中,信息采集时刻是指自动驾驶车辆在行驶过程中对观测导航定位信息、速度变化信息以及实际行驶参数等信息进行采集的时刻。
上述融合定位方法,通过获取与当前时刻对应的观测导航定位信息、速度变化信息以及实际行驶参数,能够利用速度变化信息对航向可观性进行判断,并在航向弱可观,即航向可观性不满足预设航向可观性指标时,获取用于描述航向变化的航向增量,利用航向增量、观测导航定位信息、速度变化信息以及实际行驶参数,实现对预测时刻的定位结果的准确预测,得到弱可观预测定位结果,通过获取与预测时刻对应的观测导航定位信息,能够结合与预测时刻对应的观测导航定位信息和弱可观预测定位结果,得到准确的、与预测时刻对应的目标定位结果。整个过程,能够在利用速度变化信息确定航向弱可观后,引入航向增量来进行预测实现减小估计误差。
在一个实施例中,融合定位方法还包括:
当缓存中存在与航向强可观对应的速度变化零偏时,根据速度变化零偏对速度变化信息进行修正。
具体的,在得到速度变化信息后,终端会先确认缓存中是否存在与航向强可观对应的速度变化零偏,当缓存中存在与航向强可观对应的速度变化零偏时,表示可根据该与航向强可观对应的速度变化零偏对速度变化信息进行修正,以得到更准确的速度变化信息,终端会根据与航向强可观对应的速度变化零偏中加速度零偏对加速度信息进行修正,并根据速度变化零偏中陀螺零偏对角速度信息进行修正,得到修正后的角速度信息以及加速度信息,再根据修正后角速度信息以及加速度信息进行航向可观性判断。
具体的,当缓存中不存在与航向强可观对应的速度变化零偏时,即之前的航向可观性均为航向弱可观,由前面的描述可以知道,航向弱可观时观测所得到的状态变量是不能准确反映车辆的真实情况的,因此此时不能根据估计得到的速度变化零偏对速度变化信息进行修正,即若从车辆行驶的行驶初始时刻开始,航向可观性均为航向弱可观,则缓存中不会存在与航向强可观对应的速度变化零偏。终端会直接根据未修正的、原始的速度变化信息进行航向可观性判断。
需要说明的是,本实施例中,所缓存的与航向强可观对应的速度变化零偏是实时更新的,在每次得到新的与航向强可观对应的速度变化零偏时,终端都会根据新的与航向强可观对应的速度变化零偏对已缓存的与航向强可观对应的速度变化零偏进行更新。通过这种方式,能够保证速度变化零偏的准确,以确保得到准确的修正后的速度变化信息。
本实施例中,通过确认缓存中是否存在与航向强可观对应的速度变化零偏,能够实现对速度变化信息的修正,从而得到更准确的速度变化信息,以便根据速度变化信息实现对航向可观性的准确判断。
在一个实施例中,融合定位方法还包括:
根据速度变化信息中角速度信息以及加速度信息,得到航向可观性评估值;
比对航向可观性评估值和预设航向可观性指标;
当航向可观性评估值满足预设航向可观性指标时,确定航向可观性满足预设航向可观性指标,航向可观性为航向强可观;
当航向可观性评估值不满足预设航向可观性指标时,确定航向可观性不满足预设航向可观性指标,航向可观性为航向弱可观。
其中,加速度信息是通过惯性测量单元中的加速度计得到的,加速度计可以输出三个轴向的加速度,即X轴加速度、Y轴加速度以及Z轴加速度。角速度信息是通过惯性测量单元中的陀螺仪得到的,陀螺仪可以输出三个轴向的角速度,即X轴角速度、Y轴角速度以及Z轴角速度。航向可观性评估值用于对航向可观性进行评价。比如,航向可观性评估值具体可以根据预先设置的航向可观性评价公式得到,其中的航向可观性评价公式可按照需要自行设置。
举例说明,航向可观性评价公式具体可以为:
Figure BDA0003277561580000091
其中a和b为人为设置的系数,wz为Z轴角速度,fx为X轴加速度,fy为Y轴加速度,O为航向可观性评估值。
具体的,终端会根据速度变化信息中角速度信息以及加速度信息,计算得到航向可观性评估值,再比对航向可观性评估值和预设航向可观性指标,通过确认航向可观性评估值是否满足预设航向可观性指标,来确定航向可观性是否满足预设航向可观性指标。当航向可观性评估值满足预设航向可观性指标时,表示航向可观性为航向强可观,航向可观性满足预设航向可观性指标。当航向可观性评估值不满足预设航向可观性指标时,表示航向可观性为航向弱可观,航向可观性不满足预设航向可观性指标。
举例说明,当航向可观性评估值为航向可观性评估值且预设航向可观性指标为航向可观性阈值时,航向可观性评估值满足预设航向可观性指标是指航向可观性评估值大于或者等于航向可观性阈值,航向可观性评估值不满足预设航向可观性指标是指航向可观性评估值小于航向可观性阈值。
本实施例中,通过先根据角速度信息以及加速度信息,得到航向可观性评估值,再比对航向可观性评估值和预设航向可观性指标,以确定航向可观性是否满足预设航向可观性指标,能够利用角速度信息以及加速度信息实现对航向可观性的确定。
在一个实施例中,实际行驶参数包括实际行驶姿态以及实际行驶速度;
根据航向增量、观测导航定位信息、速度变化信息以及实际行驶参数,得到与预测时刻对应的弱可观预测定位结果包括:
根据航向增量、实际行驶姿态以及速度变化信息中角速度信息,得到与预测时刻对应的预测姿态信息;
根据预测姿态信息、实际行驶速度以及速度变化信息中加速度计测量的比力,得到与预测时刻对应的预测速度信息;
根据预测速度信息、观测导航定位信息以及实际行驶速度,得到预测位置信息;
归集预测姿态信息、预测速度信息以及预测位置信息,得到与预测时刻对应的弱可观预测定位结果。
具体的,实际行驶参数包括实际行驶姿态以及实际行驶速度,在确定航向可观性不满足预设航向可观性指标,即航向弱可观后,终端会先根据航向增量、实际行驶姿态以及速度变化信息中角速度信息,得到与预测时刻对应的预测姿态信息,再利用预测姿态信息、实际行驶速度以及速度变化信息中加速度计测量的比力,得到与预测时刻对应的预测速度信息,再根据预测速度信息、观测导航定位信息以及实际行驶速度,得到预测位置信息,最后通过归集预测姿态信息、预测速度信息以及预测位置信息,得到与预测时刻对应的弱可观预测定位结果。
下面对得到与预测时刻对应的弱可观预测定位结果中预测姿态信息进行举例说明。
具体的,可以利用矩阵链乘得到预测姿态信息:
Figure BDA0003277561580000111
其中,
Figure BDA0003277561580000112
由地球自转角速度
Figure BDA0003277561580000113
计算得到(如公式(2)所示),
Figure BDA0003277561580000114
由角速度
Figure BDA0003277561580000115
和航向增量δheading计算得到(如公式(3)所示),
Figure BDA0003277561580000116
Figure BDA0003277561580000117
分别为tk-1时刻(即当前时刻)和tk时刻(即预测时刻)的姿态矩阵,Δt为当前时刻与预测时刻之间的时间间隔。
Figure BDA0003277561580000118
Figure BDA0003277561580000119
下面对得到与预测时刻对应的弱可观预测定位结果中预测速度信息进行举例说明。
在导航系下的速度微分方程可以表示为:
Figure BDA00032775615800001110
其中,
Figure BDA00032775615800001111
为预测姿态信息,
Figure BDA00032775615800001112
为加速度计测量的比力,gn为重力矢量,
Figure BDA00032775615800001113
代表了载体运动导致的向心加速度,
Figure BDA00032775615800001114
代表了由于地球自转和载体运动导致的科里奥利力。其中:
Figure BDA00032775615800001115
Figure BDA00032775615800001116
其中,
Figure BDA00032775615800001117
分别为n系(即导航坐标系)下的东向速度与北向速度(可通过当前时刻的实际行驶速度得到),e代表地球扁平率,L为纬度,h为海拔高度,RM和RN则分别表示地球子午圈和卯酉圈曲率半径。
则预测速度信息v(k):
Figure BDA00032775615800001211
其中,v(k-1)为当前时刻的实际行驶速度,Δt为当前时刻与预测时刻之间的时间间隔。
下面对得到与预测时刻对应的弱可观预测定位结果中预测位置信息进行举例说明。
惯性导航系统的位置微分方程为:
Figure BDA0003277561580000121
Figure BDA0003277561580000122
Figure BDA0003277561580000123
其中,
Figure BDA0003277561580000124
为n系(即导航坐标系)下的北向速度,
Figure BDA0003277561580000125
为n系下的东向速度,
Figure BDA0003277561580000126
为n系下的天向速度(可通过当前时刻的实际行驶速度得到),λ为经度,L为纬度(可通过当前时刻的观测导航定位信息得到)。
使用矩阵乘法可以表示为:
Figure BDA0003277561580000127
采用梯形积分法计算位置更新递推方程:
Figure BDA0003277561580000128
其中,
Figure BDA0003277561580000129
为预测位置信息,
Figure BDA00032775615800001210
为当前时刻的观测导航定位信息,Δt为当前时刻与预测时刻之间的时间间隔。
本实施例中,通过在航向可观性不满足预设航向可观性指标时,利用航向增量、实际行驶姿态以及角速度信息来进行姿态预测,能够实现对姿态的准确预测,从而可以利用预测姿态信息、实际行驶速度以及加速度计测量的比例,实现对速度的准确预测,并根据预测速度信息、观测导航定位信息以及实际行驶速度,实现对位置的准确预测,以得到弱可观预测定位结果。
在一个实施例中,根据与预测时刻对应的观测导航定位信息和弱可观预测定位结果,得到与预测时刻对应的目标定位结果包括:
根据与预测时刻对应的观测导航定位信息和弱可观预测定位结果中预测位置信息,得到弱可观位置误差;
根据弱可观位置误差利用误差状态卡尔曼滤波,得到弱可观预测定位误差;
根据弱可观预测定位结果和弱可观预测定位误差,得到与预测时刻对应的目标定位结果。
其中,弱可观位置误差是指与预测时刻对应的观测导航定位信息与弱可观预测定位结果中预测位置信息之间的误差,即测量值与估计值之间的误差。弱可观预测定位误差包括与弱可观对应的姿态误差、速度误差以及导航定位误差。
具体的,终端会获取与预测时刻对应的观测导航定位信息,根据与预测时刻对应的观测导航定位信息和弱可观预测定位结果中预测位置信息,计算位置误差,得到弱可观位置误差,再利用误差状态卡尔曼滤波,根据弱可观位置误差以及弱可观预测定位结果,得到弱可观预测定位误差,最后利用弱可观预测定位误差对弱可观预测定位结果进行修正,得到与预测时刻对应的目标定位结果。此处需要说明的是,虽然在航向可观性不满足预设航向可观性指标时,终端也会通过误差估计得到速度变化零偏,但是并不会保留该速度变化零偏,终端只会在得到与航向强可观对应的速度变化零偏时对其进行缓存。
其中,利用误差状态卡尔曼滤波,根据弱可观位置误差以及弱可观预测定位结果,得到弱可观预测定位误差是指根据弱可观位置误差以及弱可观预测定位结果,得到误差状态卡尔曼滤波对应的状态量,基于状态量进行误差预测,以得到弱可观预测定位误差。
本实施例中,通过获取与预测时刻对应的观测导航定位信息,根据该观测导航定位信息和弱可观预测定位结果中预测位置信息,得到弱可观位置误差,能够根据弱可观位置误差利用误差状态卡尔曼滤波,得到弱可观预测定位误差,从而可以根据弱可观预测定位误差对弱可观预测定位结果进行修正,得到与预测时刻对应的目标定位结果。
在一个实施例中,获取与当前时刻对应的观测导航定位信息、速度变化信息以及实际行驶参数之后,还包括:
当根据速度变化信息确定航向强可观时,根据观测导航定位信息、速度变化信息以及实际行驶参数,得到与预测时刻对应的强可观预测定位结果,航向强可观是指航向可观性满足预设航向可观性指标;
获取与预测时刻对应的观测导航定位信息;
根据与预测时刻对应的观测导航定位信息和强可观预测定位结果,得到与预测时刻对应的目标定位结果。
具体的,当根据速度变化信息确定航向强可观,即航向可观性不满足预设航向可观性指标时,表示在进行预测定位时不需要使用航向增量,终端可以直接根据观测导航定位信息、速度变化信息以及实际行驶参数,得到与预测时刻对应的强可观预测定位结果。在得到与预测时刻对应的强可观预测定位结果后,终端会获取与预测时刻对应的观测导航定位信息,根据与预测时刻对应的观测导航定位信息以及强可观预测定位结果,利用误差状态卡尔曼滤波,得到强可观预测定位误差,根据强可观预测定位误差和强可观预测定位结果,得到与预测时刻对应的目标定位结果。
具体的,终端根据观测导航定位信息、速度变化信息以及实际行驶参数,得到与预测时刻对应的强可观预测定位结果的方式可以为:先根据实际行驶参数中实际行驶姿态以及速度变化信息中角速度,得到与预测时刻对应的预测姿态信息,再利用预测姿态信息、实际行驶参数中实际行驶速度以及速度变化信息中加速度计测量的比力,得到与预测时刻对应的预测速度信息,再根据预测速度信息、观测导航定位信息以及实际行驶速度,得到预测位置信息,最后通过归集预测姿态信息、预测速度信息以及预测位置信息,得到与预测时刻对应的强可观预测定位结果。
下面对得到与预测时刻对应的强可观预测定位结果中预测姿态信息进行举例说明。
具体的,与公式(1)相同,可以利用矩阵链乘得到预测姿态信息:
Figure BDA0003277561580000151
其中,
Figure BDA0003277561580000152
由地球自转角速度
Figure BDA0003277561580000153
计算得到(如公式(2)所示),
Figure BDA0003277561580000154
由角速度
Figure BDA0003277561580000155
计算得到(如公式(13)所示),
Figure BDA0003277561580000156
Figure BDA0003277561580000157
分别为tk-1时刻(即当前时刻)和tk时刻(即预测时刻)的姿态矩阵,Δt为当前时刻与预测时刻之间的时间间隔。
Figure BDA0003277561580000158
Figure BDA0003277561580000159
即预测姿态信息,就是由k-1时刻的姿态
Figure BDA00032775615800001510
惯性测量单元中陀螺输出的姿态
Figure BDA00032775615800001511
地球自转加速度造成的姿态矩阵
Figure BDA00032775615800001512
预测k时刻的姿态
Figure BDA00032775615800001513
需要说明的是,得到与预测时刻对应的强可观预测定位结果中预测速度信息的方式与得到与预测时刻对应的弱可观预测定位结果中预测速度信息的方式基本相同,本实施例在此处不再赘述。得到与预测时刻对应的强可观预测定位结果中预测位置信息的方式与得到与预测时刻对应的弱可观预测定位结果中预测位置信息的方式基本相同,本实施例在此处也不再赘述。
本实施例中,在确定航向强可观后,通过根据观测导航定位信息、速度变化信息以及实际行驶参数进行预测,能够得到与预测时刻对应的强可观预测定位结果,通过获取与预测时刻对应的观测导航定位信息,能够根据与预测时刻对应的观测导航定位信息和强可观预测定位结果,实现对与预测时刻对应的目标定位结果的确定。
在一个实施例中,根据与预测时刻对应的观测导航定位信息和强可观预测定位结果,得到与预测时刻对应的目标定位结果包括:
根据与预测时刻对应的观测导航定位信息和强可观预测定位结果中预测位置信息,得到强可观位置误差;
根据强可观位置误差,利用误差状态卡尔曼滤波,得到强可观预测定位误差以及与航向强可观对应的速度变化零偏;
根据强可观预测定位结果和强可观预测定位误差,得到与预测时刻对应的目标定位结果,并缓存与航向强可观对应的速度变化零偏。
其中,强可观位置误差是指与预测时刻对应的观测导航定位信息与强可观预测定位结果中预测位置信息之间的误差,即测量值与估计值之间的误差。强可观预测定位误差包括与强可观对应的姿态误差、速度误差以及导航定位误差。与航向强可观对应的速度变化零偏是指与航向强可观对应的陀螺零偏和加速度零偏。
具体的,终端会根据与预测时刻对应的观测导航定位信息和强可观预测定位结果中预测位置信息,计算位置误差,得到弱可观位置误差,再利用误差状态卡尔曼滤波,根据强可观位置误差以及强可观预测定位结果,得到强可观预测定位误差以及与航向强可观对应的速度变化零偏,最后利用强可观预测定位误差对强可观预测定位结果进行修正,得到与预测时刻对应的目标定位结果。
此处需要说明的是,终端会存储与航向强可观对应的速度变化零偏,若此时缓存中已存在与航向强可观对应的速度变化零偏,终端会根据本次得到的速度变化零偏对已缓存的速度变化零偏进行更新,若此时缓存中不存在与航向强可观对应的速度变化零偏,终端直接存储本次得到的速度变化零偏即可。
其中,利用误差状态卡尔曼滤波,根据强可观位置误差以及强可观预测定位结果,得到强可观预测定位误差是指根据强可观位置误差以及强可观预测定位结果,得到误差状态卡尔曼滤波对应的状态量,基于状态量进行误差预测,以得到强可观预测定位误差以及与航向强可观对应的速度变化零偏。
本实施例中,通过根据与预测时刻对应的观测导航定位信息和强可观预测定位结果中预测位置信息,得到强可观位置误差,能够根据强可观位置误差,利用误差状态卡尔曼滤波,得到强可观预测定位误差以及与航向强可观对应的速度变化零偏,从而可以根据强可观预测定位误差对强可观预测定位结果进行修正,得到与预测时刻对应的目标定位结果,并实现对与航向强可观对应的速度变化零偏的缓存。
在一个实施例中,如图2所示,通过一个流程示意图来说明本申请的融合定位方法。
终端获取与当前时刻对应的观测导航定位信息(GNSS输出)、速度变化信息(IMU中加速度计和陀螺输出)以及实际行驶参数,判断缓存中是否存在与航向强可观对应的速度变化零偏,当存在时根据速度变化零偏对速度变化信息进行修正(即图2中的IMU误差补偿),当不存在时不对速度变化信息进行修正,根据速度变化信息中角速度信息以及加速度信息,得到航向可观性评估值,比对航向可观性评估值和预设航向可观性指标。
当航向可观性评估值满足预设航向可观性指标时,确定航向可观性满足预设航向可观性指标,即航向弱可观,获取航向增量(即Lidar里程计输出的航向增加量),根据航向增量、实际行驶姿态以及速度变化信息中角速度信息,得到与预测时刻对应的预测姿态信息,根据预测姿态信息、实际行驶速度以及速度变化信息中加速度计测量的比力,得到与预测时刻对应的预测速度信息,根据预测速度信息、观测导航定位信息以及实际行驶速度,得到预测位置信息,归集预测姿态信息、预测速度信息以及预测位置信息,得到与预测时刻对应的弱可观预测定位结果(即姿态更新、速度更新、位置更新),获取与预测时刻对应的观测导航定位信息(即观测位置),根据与预测时刻对应的观测导航定位信息和弱可观预测定位结果中预测位置信息,得到弱可观位置误差(即位置误差),根据弱可观位置误差利用误差状态卡尔曼滤波,得到弱可观预测定位误差(即姿态/速度/位置误差),根据弱可观预测定位结果和弱可观预测定位误差,得到与预测时刻对应的目标定位结果(即姿态/速度/位置输出)。
当航向可观性评估值不满足预设航向可观性指标时,确定航向可观性不满足预设航向可观性指标,即航向强可观,根据观测导航定位信息、速度变化信息以及实际行驶参数,得到与预测时刻对应的强可观预测定位结果(即姿态更新、速度更新、位置更新),获取与预测时刻对应的观测导航定位信息(即观测位置),根据与预测时刻对应的观测导航定位信息和强可观预测定位结果中预测位置信息,得到强可观位置误差(即位置误差),根据强可观位置误差,利用误差状态卡尔曼滤波,得到强可观预测定位误差(即姿态/速度/位置误差)以及与航向强可观对应的速度变化零偏(即陀螺零偏和加速度零偏),根据强可观预测定位结果和强可观预测定位误差,得到与预测时刻对应的目标定位结果,并缓存与航向强可观对应的速度变化零偏(即姿态/速度/位置输出)。
在一个实施例中,如图3所示,通过一个流程示意图来说明本申请的融合定位方法,该融合定位方法具体包括以下步骤:
步骤302,获取与当前时刻对应的观测导航定位信息、速度变化信息以及实际行驶参数;
步骤304,判断缓存中是否存在与航向强可观对应的速度变化零偏,当存在时根据速度变化零偏对速度变化信息进行修正,当不存在时不对速度变化信息进行修正;
步骤306,根据速度变化信息中角速度信息以及加速度信息,得到航向可观性评估值;
步骤308,比对航向可观性评估值和预设航向可观性指标;
步骤310,当航向可观性评估值满足预设航向可观性指标时,确定航向可观性满足预设航向可观性指标,航向可观性为航向强可观,跳转至步骤330,当航向可观性评估值不满足预设航向可观性指标时,确定航向可观性不满足预设航向可观性指标,航向可观性为航向弱可观,跳转至步骤312;
步骤312,获取航向增量,航向增量用于表征航向变化;
步骤314,根据航向增量、实际行驶姿态以及速度变化信息中角速度信息,得到与预测时刻对应的预测姿态信息;
步骤316,根据预测姿态信息、实际行驶速度以及速度变化信息中加速度计测量的比力,得到与预测时刻对应的预测速度信息;
步骤318,根据预测速度信息、观测导航定位信息以及实际行驶速度,得到预测位置信息;
步骤320,归集预测姿态信息、预测速度信息以及预测位置信息,得到与预测时刻对应的弱可观预测定位结果;
步骤322,获取与预测时刻对应的观测导航定位信息;
步骤324,根据与预测时刻对应的观测导航定位信息和弱可观预测定位结果中预测位置信息,得到弱可观位置误差;
步骤326,根据弱可观位置误差利用误差状态卡尔曼滤波,得到弱可观预测定位误差;
步骤328,根据弱可观预测定位结果和弱可观预测定位误差,得到与预测时刻对应的目标定位结果;
步骤330,根据观测导航定位信息、速度变化信息以及实际行驶参数,得到与预测时刻对应的强可观预测定位结果;
步骤332,获取与预测时刻对应的观测导航定位信息;
步骤334,根据与预测时刻对应的观测导航定位信息和强可观预测定位结果中预测位置信息,得到强可观位置误差;
步骤336,根据强可观位置误差,利用误差状态卡尔曼滤波,得到强可观预测定位误差以及与航向强可观对应的速度变化零偏;
步骤338,根据强可观预测定位结果和强可观预测定位误差,得到与预测时刻对应的目标定位结果,并缓存与航向强可观对应的速度变化零偏。
应该理解的是,虽然上述实施例涉及的各流程图中的各个步骤按照箭头的指示依次显示,但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明,这些步骤的执行并没有严格的顺序限制,这些步骤可以以其它的顺序执行。而且,上述实施例涉及的各流程图中的至少一部分步骤可以包括多个步骤或者多个阶段,这些步骤或者阶段并不必然是在同一时刻执行完成,而是可以在不同的时刻执行,这些步骤或者阶段的执行顺序也不必然是依次进行,而是可以与其它步骤或者其它步骤中的步骤或者阶段的至少一部分轮流或者交替地执行。
在一个实施例中,如图4所示,提供了一种融合定位装置,包括:第一获取模块402、处理模块404、预测模块406、第二获取模块408和修正模块410,其中:
第一获取模块402,用于获取与当前时刻对应的观测导航定位信息、速度变化信息以及实际行驶参数;
处理模块404,用于当根据速度变化信息确定航向弱可观时,获取航向增量,航向弱可观是指航向可观性不满足预设航向可观性指标,航向增量用于表征航向变化;
预测模块406,用于根据航向增量、观测导航定位信息、速度变化信息以及实际行驶参数,得到与预测时刻对应的弱可观预测定位结果;
第二获取模块408,用于获取与预测时刻对应的观测导航定位信息;
修正模块410,用于根据与预测时刻对应的观测导航定位信息和弱可观预测定位结果,得到与预测时刻对应的目标定位结果。
上述融合定位装置,通过获取与当前时刻对应的观测导航定位信息、速度变化信息以及实际行驶参数,能够利用速度变化信息对航向可观性进行判断,并在航向弱可观,即航向可观性不满足预设航向可观性指标时,获取用于描述航向变化的航向增量,利用航向增量、观测导航定位信息、速度变化信息以及实际行驶参数,实现对预测时刻的定位结果的准确预测,得到弱可观预测定位结果,通过获取与预测时刻对应的观测导航定位信息,能够结合与预测时刻对应的观测导航定位信息和弱可观预测定位结果,得到准确的、与预测时刻对应的目标定位结果。整个过程,能够在利用速度变化信息确定航向弱可观后,引入航向增量来进行预测实现减小估计误差。
在一个实施例中,修正模块还用于当缓存中存在与航向强可观对应的速度变化零偏时,根据速度变化零偏对速度变化信息进行修正。
在一个实施例中,处理模块还用于根据速度变化信息中角速度信息以及加速度信息,得到航向可观性评估值,比对航向可观性评估值和预设航向可观性指标,当航向可观性评估值满足预设航向可观性指标时,确定航向可观性满足预设航向可观性指标,航向可观性为航向强可观,当航向可观性评估值不满足预设航向可观性指标时,确定航向可观性不满足预设航向可观性指标,航向可观性为航向弱可观。
在一个实施例中,实际行驶参数包括实际行驶姿态以及实际行驶速度;预测模块还用于根据航向增量、实际行驶姿态以及速度变化信息中角速度信息,得到与预测时刻对应的预测姿态信息,根据预测姿态信息、实际行驶速度以及速度变化信息中加速度计测量的比力,得到与预测时刻对应的预测速度信息,根据预测速度信息、观测导航定位信息以及实际行驶速度,得到预测位置信息,归集预测姿态信息、预测速度信息以及预测位置信息,得到与预测时刻对应的弱可观预测定位结果。
在一个实施例中,预测模块还用于根据与预测时刻对应的观测导航定位信息和弱可观预测定位结果中预测位置信息,得到弱可观位置误差,根据弱可观位置误差利用误差状态卡尔曼滤波,得到弱可观预测定位误差,根据弱可观预测定位结果和弱可观预测定位误差,得到与预测时刻对应的目标定位结果。
在一个实施例中,预测模块还用于当根据速度变化信息确定航向强可观时,根据观测导航定位信息、速度变化信息以及实际行驶参数,得到与预测时刻对应的强可观预测定位结果,航向强可观是指航向可观性满足预设航向可观性指标,获取与预测时刻对应的观测导航定位信息,根据与预测时刻对应的观测导航定位信息和强可观预测定位结果,得到与预测时刻对应的目标定位结果。
在一个实施例中,预测模块还用于根据与预测时刻对应的观测导航定位信息和强可观预测定位结果中预测位置信息,得到强可观位置误差,根据强可观位置误差,利用误差状态卡尔曼滤波,得到强可观预测定位误差以及与航向强可观对应的速度变化零偏,根据强可观预测定位结果和强可观预测定位误差,得到与预测时刻对应的目标定位结果,并缓存与航向强可观对应的速度变化零偏。
关于融合定位装置的具体实施例可以参见上文中对于融合定位方法的实施例,在此不再赘述。上述融合定位装置中的各个模块可全部或部分通过软件、硬件及其组合来实现。上述各模块可以硬件形式内嵌于或独立于计算机设备中的处理器中,也可以以软件形式存储于计算机设备中的存储器中,以便于处理器调用执行以上各个模块对应的操作。
在一个实施例中,提供了一种计算机设备,该计算机设备可以是终端,其内部结构图可以如图5所示。该计算机设备包括通过系统总线连接的处理器、存储器、通信接口、显示屏和输入装置。其中,该计算机设备的处理器用于提供计算和控制能力。该计算机设备的存储器包括非易失性存储介质、内存储器。该非易失性存储介质存储有操作系统和计算机程序。该内存储器为非易失性存储介质中的操作系统和计算机程序的运行提供环境。该计算机设备的通信接口用于与外部的终端进行有线或无线方式的通信,无线方式可通过WIFI、运营商网络、NFC(近场通信)或其他技术实现。该计算机程序被处理器执行时以实现一种融合定位方法。该计算机设备的显示屏可以是液晶显示屏或者电子墨水显示屏,该计算机设备的输入装置可以是显示屏上覆盖的触摸层,也可以是计算机设备外壳上设置的按键、轨迹球或触控板,还可以是外接的键盘、触控板或鼠标等。
本领域技术人员可以理解,图5中示出的结构,仅仅是与本申请方案相关的部分结构的框图,并不构成对本申请方案所应用于其上的计算机设备的限定,具体的计算机设备可以包括比图中所示更多或更少的部件,或者组合某些部件,或者具有不同的部件布置。
在一个实施例中,提供了一种计算机设备,包括存储器和处理器,存储器中存储有计算机程序,该处理器执行计算机程序时实现以下步骤:
获取与当前时刻对应的观测导航定位信息、速度变化信息以及实际行驶参数;
当根据速度变化信息确定航向弱可观时,获取航向增量,航向弱可观是指航向可观性不满足预设航向可观性指标,航向增量用于表征航向变化;
根据航向增量、观测导航定位信息、速度变化信息以及实际行驶参数,得到与预测时刻对应的弱可观预测定位结果;
获取与预测时刻对应的观测导航定位信息;
根据与预测时刻对应的观测导航定位信息和弱可观预测定位结果,得到与预测时刻对应的目标定位结果。
在一个实施例中,处理器执行计算机程序时还实现以下步骤:当缓存中存在与航向强可观对应的速度变化零偏时,根据速度变化零偏对速度变化信息进行修正。
在一个实施例中,处理器执行计算机程序时还实现以下步骤:根据速度变化信息中角速度信息以及加速度信息,得到航向可观性评估值,比对航向可观性评估值和预设航向可观性指标,当航向可观性评估值满足预设航向可观性指标时,确定航向可观性满足预设航向可观性指标,航向可观性为航向强可观,当航向可观性评估值不满足预设航向可观性指标时,确定航向可观性不满足预设航向可观性指标,航向可观性为航向弱可观。
在一个实施例中,处理器执行计算机程序时还实现以下步骤:根据航向增量、实际行驶姿态以及速度变化信息中角速度信息,得到与预测时刻对应的预测姿态信息,根据预测姿态信息、实际行驶速度以及速度变化信息中加速度计测量的比力,得到与预测时刻对应的预测速度信息,根据预测速度信息、观测导航定位信息以及实际行驶速度,得到预测位置信息,归集预测姿态信息、预测速度信息以及预测位置信息,得到与预测时刻对应的弱可观预测定位结果。
在一个实施例中,处理器执行计算机程序时还实现以下步骤:根据与预测时刻对应的观测导航定位信息和弱可观预测定位结果中预测位置信息,得到弱可观位置误差,根据弱可观位置误差利用误差状态卡尔曼滤波,得到弱可观预测定位误差,根据弱可观预测定位结果和弱可观预测定位误差,得到与预测时刻对应的目标定位结果。
在一个实施例中,处理器执行计算机程序时还实现以下步骤:当根据速度变化信息确定航向强可观时,根据观测导航定位信息、速度变化信息以及实际行驶参数,得到与预测时刻对应的强可观预测定位结果,航向强可观是指航向可观性满足预设航向可观性指标,获取与预测时刻对应的观测导航定位信息,根据与预测时刻对应的观测导航定位信息和强可观预测定位结果,得到与预测时刻对应的目标定位结果。
在一个实施例中,处理器执行计算机程序时还实现以下步骤:根据与预测时刻对应的观测导航定位信息和强可观预测定位结果中预测位置信息,得到强可观位置误差,根据强可观位置误差,利用误差状态卡尔曼滤波,得到强可观预测定位误差以及与航向强可观对应的速度变化零偏,根据强可观预测定位结果和强可观预测定位误差,得到与预测时刻对应的目标定位结果,并缓存与航向强可观对应的速度变化零偏。
在一个实施例中,提供了一种计算机可读存储介质,其上存储有计算机程序,计算机程序被处理器执行时实现以下步骤:
获取与当前时刻对应的观测导航定位信息、速度变化信息以及实际行驶参数;
当根据速度变化信息确定航向弱可观时,获取航向增量,航向弱可观是指航向可观性不满足预设航向可观性指标,航向增量用于表征航向变化;
根据航向增量、观测导航定位信息、速度变化信息以及实际行驶参数,得到与预测时刻对应的弱可观预测定位结果;
获取与预测时刻对应的观测导航定位信息;
根据与预测时刻对应的观测导航定位信息和弱可观预测定位结果,得到与预测时刻对应的目标定位结果。
在一个实施例中,计算机程序被处理器执行时还实现以下步骤:当缓存中存在与航向强可观对应的速度变化零偏时,根据速度变化零偏对速度变化信息进行修正。
在一个实施例中,计算机程序被处理器执行时还实现以下步骤:根据速度变化信息中角速度信息以及加速度信息,得到航向可观性评估值,比对航向可观性评估值和预设航向可观性指标,当航向可观性评估值满足预设航向可观性指标时,确定航向可观性满足预设航向可观性指标,航向可观性为航向强可观,当航向可观性评估值不满足预设航向可观性指标时,确定航向可观性不满足预设航向可观性指标,航向可观性为航向弱可观。
在一个实施例中,计算机程序被处理器执行时还实现以下步骤:根据航向增量、实际行驶姿态以及速度变化信息中角速度信息,得到与预测时刻对应的预测姿态信息,根据预测姿态信息、实际行驶速度以及速度变化信息中加速度计测量的比力,得到与预测时刻对应的预测速度信息,根据预测速度信息、观测导航定位信息以及实际行驶速度,得到预测位置信息,归集预测姿态信息、预测速度信息以及预测位置信息,得到与预测时刻对应的弱可观预测定位结果。
在一个实施例中,计算机程序被处理器执行时还实现以下步骤:根据与预测时刻对应的观测导航定位信息和弱可观预测定位结果中预测位置信息,得到弱可观位置误差,根据弱可观位置误差利用误差状态卡尔曼滤波,得到弱可观预测定位误差,根据弱可观预测定位结果和弱可观预测定位误差,得到与预测时刻对应的目标定位结果。
在一个实施例中,计算机程序被处理器执行时还实现以下步骤:当根据速度变化信息确定航向强可观时,根据观测导航定位信息、速度变化信息以及实际行驶参数,得到与预测时刻对应的强可观预测定位结果,航向强可观是指航向可观性满足预设航向可观性指标,获取与预测时刻对应的观测导航定位信息,根据与预测时刻对应的观测导航定位信息和强可观预测定位结果,得到与预测时刻对应的目标定位结果。
在一个实施例中,计算机程序被处理器执行时还实现以下步骤:根据与预测时刻对应的观测导航定位信息和强可观预测定位结果中预测位置信息,得到强可观位置误差,根据强可观位置误差,利用误差状态卡尔曼滤波,得到强可观预测定位误差以及与航向强可观对应的速度变化零偏,根据强可观预测定位结果和强可观预测定位误差,得到与预测时刻对应的目标定位结果,并缓存与航向强可观对应的速度变化零偏。
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程,是可以通过计算机程序来指令相关的硬件来完成,所述的计算机程序可存储于一非易失性计算机可读取存储介质中,该计算机程序在执行时,可包括如上述各方法的实施例的流程。其中,本申请所提供的各实施例中所使用的对存储器、存储、数据库或其它介质的任何引用,均可包括非易失性和易失性存储器中的至少一种。非易失性存储器可包括只读存储器(Read-Only Memory,ROM)、磁带、软盘、闪存或光存储器等。易失性存储器可包括随机存取存储器(Random Access Memory,RAM)或外部高速缓冲存储器。作为说明而非局限,RAM可以是多种形式,比如静态随机存取存储器(Static Random Access Memory,SRAM)或动态随机存取存储器(Dynamic Random Access Memory,DRAM)等。
以上实施例的各技术特征可以进行任意的组合,为使描述简洁,未对上述实施例中的各个技术特征所有可能的组合都进行描述,然而,只要这些技术特征的组合不存在矛盾,都应当认为是本说明书记载的范围。
以上所述实施例仅表达了本申请的几种实施方式,其描述较为具体和详细,但并不能因此而理解为对发明专利范围的限制。应当指出的是,对于本领域的普通技术人员来说,在不脱离本申请构思的前提下,还可以做出若干变形和改进,这些都属于本申请的保护范围。因此,本申请专利的保护范围应以所附权利要求为准。

Claims (10)

1.一种融合定位方法,其特征在于,所述方法包括:
获取与当前时刻对应的观测导航定位信息、速度变化信息以及实际行驶参数;
当根据所述速度变化信息确定航向弱可观时,获取航向增量,所述航向弱可观是指航向可观性不满足预设航向可观性指标,所述航向增量用于表征航向变化;
根据所述航向增量、所述观测导航定位信息、所述速度变化信息以及实际行驶参数,得到与预测时刻对应的弱可观预测定位结果;
获取与所述预测时刻对应的观测导航定位信息;
根据与所述预测时刻对应的观测导航定位信息和所述弱可观预测定位结果,得到与所述预测时刻对应的目标定位结果。
2.根据权利要求1所述的方法,其特征在于,还包括:
当缓存中存在与航向强可观对应的速度变化零偏时,根据所述速度变化零偏对所述速度变化信息进行修正。
3.根据权利要求1所述的方法,其特征在于,还包括:
根据所述速度变化信息中角速度信息以及加速度信息,得到航向可观性评估值;
比对所述航向可观性评估值和所述预设航向可观性指标;
当所述航向可观性评估值满足所述预设航向可观性指标时,确定航向可观性满足所述预设航向可观性指标,所述航向可观性为航向强可观;
当所述航向可观性评估值不满足所述预设航向可观性指标时,确定航向可观性不满足所述预设航向可观性指标,所述航向可观性为航向弱可观。
4.根据权利要求1所述的方法,其特征在于,所述实际行驶参数包括实际行驶姿态以及实际行驶速度;
所述根据所述航向增量,所述观测导航定位信息、所述速度变化信息以及所述实际行驶参数,得到与预测时刻对应的弱可观预测定位结果包括:
根据所述航向增量、所述实际行驶姿态以及所述速度变化信息中角速度信息,得到与预测时刻对应的预测姿态信息;
根据所述预测姿态信息、所述实际行驶速度以及所述速度变化信息中加速度计测量的比力,得到与预测时刻对应的预测速度信息;
根据所述预测速度信息、所述观测导航定位信息以及所述实际行驶速度,得到预测位置信息;
归集所述预测姿态信息、所述预测速度信息以及所述预测位置信息,得到与预测时刻对应的弱可观预测定位结果。
5.根据权利要求1所述的方法,其特征在于,所述根据与所述预测时刻对应的观测导航定位信息和所述弱可观预测定位结果,得到与所述预测时刻对应的目标定位结果包括:
根据与所述预测时刻对应的观测导航定位信息和所述弱可观预测定位结果中预测位置信息,得到弱可观位置误差;
根据所述弱可观位置误差利用误差状态卡尔曼滤波,得到弱可观预测定位误差;
根据所述弱可观预测定位结果和所述弱可观预测定位误差,得到与所述预测时刻对应的目标定位结果。
6.根据权利要求1所述的方法,其特征在于,所述获取与当前时刻对应的观测导航定位信息、速度变化信息以及实际行驶参数之后,还包括:
当根据所述速度变化信息确定航向强可观时,根据所述观测导航定位信息、所述速度变化信息以及所述实际行驶参数,得到与预测时刻对应的强可观预测定位结果,所述航向强可观是指航向可观性满足所述预设航向可观性指标;
获取与所述预测时刻对应的观测导航定位信息;
根据与所述预测时刻对应的观测导航定位信息和所述强可观预测定位结果,得到与所述预测时刻对应的目标定位结果。
7.根据权利要求6所述的方法,其特征在于,所述根据与所述预测时刻对应的观测导航定位信息和所述强可观预测定位结果,得到与所述预测时刻对应的目标定位结果包括:
根据与所述预测时刻对应的观测导航定位信息和所述强可观预测定位结果中预测位置信息,得到强可观位置误差;
根据所述强可观位置误差,利用误差状态卡尔曼滤波,得到强可观预测定位误差以及与航向强可观对应的速度变化零偏;
根据所述强可观预测定位结果和所述强可观预测定位误差,得到与所述预测时刻对应的目标定位结果,并缓存与所述航向强可观对应的速度变化零偏。
8.一种融合定位装置,其特征在于,所述装置包括:
第一获取模块,用于获取与当前时刻对应的观测导航定位信息、速度变化信息以及实际行驶参数;
处理模块,用于当根据所述速度变化信息确定航向弱可观时,获取航向增量,所述航向弱可观是指航向可观性不满足预设航向可观性指标,所述航向增量用于表征航向变化;
预测模块,用于根据所述航向增量、所述观测导航定位信息、所述速度变化信息以及实际行驶参数,得到与预测时刻对应的弱可观预测定位结果;
第二获取模块,用于获取与所述预测时刻对应的观测导航定位信息;
修正模块,用于根据与所述预测时刻对应的观测导航定位信息和所述弱可观预测定位结果,得到与所述预测时刻对应的目标定位结果。
9.一种计算机设备,包括存储器和处理器,所述存储器存储有计算机程序,其特征在于,所述处理器执行所述计算机程序时实现权利要求1至7中任一项所述的方法的步骤。
10.一种计算机可读存储介质,其上存储有计算机程序,其特征在于,所述计算机程序被处理器执行时实现权利要求1至7中任一项所述的方法的步骤。
CN202111122685.3A 2021-09-24 2021-09-24 融合定位方法、装置、计算机设备和存储介质 Active CN114001730B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111122685.3A CN114001730B (zh) 2021-09-24 2021-09-24 融合定位方法、装置、计算机设备和存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111122685.3A CN114001730B (zh) 2021-09-24 2021-09-24 融合定位方法、装置、计算机设备和存储介质

Publications (2)

Publication Number Publication Date
CN114001730A true CN114001730A (zh) 2022-02-01
CN114001730B CN114001730B (zh) 2024-03-08

Family

ID=79921883

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111122685.3A Active CN114001730B (zh) 2021-09-24 2021-09-24 融合定位方法、装置、计算机设备和存储介质

Country Status (1)

Country Link
CN (1) CN114001730B (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114897942A (zh) * 2022-07-15 2022-08-12 深圳元戎启行科技有限公司 点云地图的生成方法、设备及相关存储介质

Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2004239643A (ja) * 2003-02-03 2004-08-26 Furuno Electric Co Ltd ハイブリッド航法装置
US20110238307A1 (en) * 2010-03-26 2011-09-29 Mark Lockwood Psiaki Vehicle navigation using non-gps leo signals and on-board sensors
CN103217158A (zh) * 2012-12-31 2013-07-24 贾继超 一种提高车载sins/od组合导航精度的方法
CN104515527A (zh) * 2013-09-27 2015-04-15 上海置微信息科技有限公司 一种无gps信号环境下的抗粗差组合导航方法
CN106950586A (zh) * 2017-01-22 2017-07-14 无锡卡尔曼导航技术有限公司 用于农机作业的gnss/ins/车辆组合导航方法
US20170350704A1 (en) * 2016-06-06 2017-12-07 Honeywell International Inc. Heading inconsistency mitigation for inertial navigation using low-performance inertial measurement units with relative aiding
CN107655476A (zh) * 2017-08-21 2018-02-02 南京航空航天大学 基于多信息融合补偿的行人高精度足部导航算法
CN110631574A (zh) * 2018-06-22 2019-12-31 北京自动化控制设备研究所 一种惯性/里程计/rtk多信息融合方法
CN111780756A (zh) * 2020-07-20 2020-10-16 北京百度网讯科技有限公司 车辆航位推算方法、装置、设备以及存储介质
CN113063429A (zh) * 2021-03-18 2021-07-02 苏州华米导航科技有限公司 一种自适应车载组合导航定位方法

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2004239643A (ja) * 2003-02-03 2004-08-26 Furuno Electric Co Ltd ハイブリッド航法装置
US20110238307A1 (en) * 2010-03-26 2011-09-29 Mark Lockwood Psiaki Vehicle navigation using non-gps leo signals and on-board sensors
CN103217158A (zh) * 2012-12-31 2013-07-24 贾继超 一种提高车载sins/od组合导航精度的方法
CN104515527A (zh) * 2013-09-27 2015-04-15 上海置微信息科技有限公司 一种无gps信号环境下的抗粗差组合导航方法
US20170350704A1 (en) * 2016-06-06 2017-12-07 Honeywell International Inc. Heading inconsistency mitigation for inertial navigation using low-performance inertial measurement units with relative aiding
CN106950586A (zh) * 2017-01-22 2017-07-14 无锡卡尔曼导航技术有限公司 用于农机作业的gnss/ins/车辆组合导航方法
CN107655476A (zh) * 2017-08-21 2018-02-02 南京航空航天大学 基于多信息融合补偿的行人高精度足部导航算法
CN110631574A (zh) * 2018-06-22 2019-12-31 北京自动化控制设备研究所 一种惯性/里程计/rtk多信息融合方法
CN111780756A (zh) * 2020-07-20 2020-10-16 北京百度网讯科技有限公司 车辆航位推算方法、装置、设备以及存储介质
CN113063429A (zh) * 2021-03-18 2021-07-02 苏州华米导航科技有限公司 一种自适应车载组合导航定位方法

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
TALMON ALEXANDRI; ROEE DIAMANT: "A Reverse Bearings Only Target Motion Analysis (BO-TMA) for improving AUV navigation accuracy", 2016 13TH WORKSHOP ON POSITIONING, NAVIGATION AND COMMUNICATIONS *
刘伟;徐海刚;张京娟;: "高精度、微小型惯性/GPS组合导航系统研究", 武汉大学学报(信息科学版), vol. 35, no. 02 *
王万征, 李洪, 郭玉胜: "一种车载组合导航系统航向修正方法研究", 导航定位与授时, vol. 2, no. 6 *
祝燕华;刘建业;钱伟行;赖际舟;: "低成本捷联惯导系统的静基座快速精对准方法", 上海交通大学学报, vol. 42, no. 05, 31 May 2008 (2008-05-31) *

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114897942A (zh) * 2022-07-15 2022-08-12 深圳元戎启行科技有限公司 点云地图的生成方法、设备及相关存储介质
CN114897942B (zh) * 2022-07-15 2022-10-28 深圳元戎启行科技有限公司 点云地图的生成方法、设备及相关存储介质

Also Published As

Publication number Publication date
CN114001730B (zh) 2024-03-08

Similar Documents

Publication Publication Date Title
CN110873888B (zh) 定位方法、定位装置、定位设备和计算机存储介质
CN111721289B (zh) 自动驾驶中车辆定位方法、装置、设备、存储介质及车辆
CN112577521B (zh) 一种组合导航误差校准方法及电子设备
CN113566850B (zh) 惯性测量单元的安装角度标定方法、装置和计算机设备
CN111982106A (zh) 导航方法、装置、存储介质及电子装置
CN108731664B (zh) 机器人状态估计方法、装置、计算机设备和存储介质
CN111562603B (zh) 基于航位推算的导航定位方法、设备及存储介质
CN112798021B (zh) 基于激光多普勒测速仪的惯导系统行进间初始对准方法
CN113252048B (zh) 一种导航定位方法、导航定位系统及计算机可读存储介质
CN114019954B (zh) 航向安装角标定方法、装置、计算机设备和存储介质
CN110319850B (zh) 一种获取陀螺仪的零点偏移的方法及装置
CN113063425B (zh) 车辆定位方法、装置、电子设备及存储介质
CN114545472B (zh) 一种gnss/ins组合系统的导航方法和装置
CN114689047B (zh) 基于深度学习的组合导航方法、装置、系统及存储介质
US20220057517A1 (en) Method for constructing point cloud map, computer device, and storage medium
CN114061570A (zh) 车辆定位方法、装置、计算机设备和存储介质
CN114061611A (zh) 目标对象定位方法、装置、存储介质和计算机程序产品
CN114001730B (zh) 融合定位方法、装置、计算机设备和存储介质
CN113009816B (zh) 时间同步误差的确定方法及装置、存储介质及电子装置
Forno et al. Techniques for improving localization applications running on low-cost IoT devices
CN111197994A (zh) 位置数据修正方法、装置、计算机设备和存储介质
CN113566849B (zh) 惯性测量单元的安装角度标定方法、装置和计算机设备
CN115060275A (zh) 一种多惯导设备导航信息择优方法
CN114088104B (zh) 一种自动驾驶场景下的地图生成方法
CN116152950B (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
GR01 Patent grant
GR01 Patent grant