CN116399351A - 一种车辆位置估计方法 - Google Patents

一种车辆位置估计方法 Download PDF

Info

Publication number
CN116399351A
CN116399351A CN202310440555.7A CN202310440555A CN116399351A CN 116399351 A CN116399351 A CN 116399351A CN 202310440555 A CN202310440555 A CN 202310440555A CN 116399351 A CN116399351 A CN 116399351A
Authority
CN
China
Prior art keywords
state
sensor
time
value
influence
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
CN202310440555.7A
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.)
Yanshan University
Original Assignee
Yanshan University
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 Yanshan University filed Critical Yanshan University
Priority to CN202310440555.7A priority Critical patent/CN116399351A/zh
Publication of CN116399351A publication Critical patent/CN116399351A/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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • 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
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO 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/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining 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/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO 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/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining 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/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining 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
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

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

一种车辆位置估计方法
技术领域
本发明涉及自动驾驶车辆定位领域，尤其涉及一种车辆位置估计方法。
背景技术
在新一代信息技术和人工智能技术不断发展的背景下，自动驾驶车辆已经成为当下智能交通***研究和应用的热点。其中，自动驾驶车辆定位***能够实时、准确地提供自车的位姿信息，是自动驾驶车辆自主决策、路径规划和底层协同控制的必要前提，对于保证交通环境中交通参与者的出行安全、提升自动驾驶车辆的安全性具有重要意义。
但是，自动驾驶车辆定位***中的导航定位传感器会因运行环境复杂多变而引起精度波动，当前的融合算法并未考虑各传感器定位精度变化，而定位精度变化会引起定位信息可用性的改变，如果不对异常传感器的导航定位信息进行修正或补偿，多源传感器融合定位结果将产生漂移。除此之外，目前主流采用基于固定权重滑动时窗的图优化方法融合多源传感器导航信息，以达到***实时性需求，但固定权重滑动时窗针对不同优化次数的因子节点享有相同权重，导致定位准确低。
发明内容
本发明的目的是提供一种车辆位置估计方法，以解决或至少部分地解决现有技术中，由于自动驾驶车辆运行环境复杂多变引起的导航定位传感器的精度波动而定位不准确的问题，提高车辆定位的精度与鲁棒性。本发明的技术方案如下。
第一方面，本发明提出一种车辆位置估计方法，所述方法包括下述步骤：
确定在车辆定位中使用的传感器集合，对传感器集合中的每个传感器，确定要估计的状态集合；
对状态集合中每个状态，计算每个状态估计产生的损失，并对损失进行修正；
对每个状态修正后的损失进行定位准确率计算，进而建立如下全局定位准确率函数：
Figure BDA0004193867410000021
Figure BDA0004193867410000022
式中：D(χ)为全局代价函数，C为传感器集合，c为传感器集合中元素，
Figure BDA0004193867410000023
为传感器c在k时刻状态估计损失修正系数，/>
Figure BDA0004193867410000024
为传感器c在k时刻状态估计损失，Xc为传感器c待估计状态值，χ为待估计状态集合，f(Xc)为传感器c进行状态估计损失的准确率；
获取使全局定位准确率函数取得最大值的***状态估计值，将其作为车辆最优定位值。
在上述技术方案中，对损失进行修正的一种实施方式，步骤包括：
估计的可信度与影响力结合进行度量或仅采用影响力进行度量，从而对每个状态估计误差进行修正；
其中，影响力为不同时刻***状态对当前状态估计的影响程度。
在这种实施方式中，提供了两种提高定位精度的技术手段，一方面是引入状态影响力，由于要估计的状态集合包括历史状态，每个历史状态会被多次估计，随着时间更新，某个历史状态距离最新状态的距离在逐渐变大，该历史状态对最新状态估计准确度的影响力是在逐步减小的，因此引入状态影响力；另一方面是不仅考虑状态影响力，还考虑传感器的可信度，通过可信度对传感器异常信息进行修正或补偿。
在上述技术方案中，传感器集合为：{全球导航卫星***(GNSS)，惯性导航***(INS)，车辆轮速里程计(ODO)}，将INS、ODO传感器时间同步为GNSS时间，采用世界坐标系确定为导航坐标系。通过多源传感器融合克服单一传感器的异常信息时定位结果漂移的问题，而采用多源传感器进行时空同步，以保证融合结果的准确性和融合时的稳定输入。
在上述技术方案中，影响力为设定值或为计算值，计算步骤包括：
对传感器集合中的传感器c，为其设定对应的滑动时窗，统计滑动时窗内的第i个状态估计频次，将其记作
Figure BDA0004193867410000031
通过下述函数计算状态估计影响力
Figure BDA0004193867410000032
Figure BDA0004193867410000033
式中，β0为设置的最低影响力；a为影响力变化速率。
在上述技术方案中，可信度与影响力结合为将可信度与影响力进行相乘，可信度为计算值，计算步骤包括：
当传感器为全球导航卫星***时，其k时刻的可信度
Figure BDA0004193867410000034
通过下式计算：
Figure BDA0004193867410000041
式中：G为观测矩阵，I为单位矩阵，wk为k时刻的GNSS定位伪距残差向量，wk-1为k-1时刻的GNSS定位伪距残差向量；
当传感器为惯性导航***、车辆轮速里程计时，其k时刻的状态估计可信度
Figure BDA0004193867410000042
通过下式计算：
Figure BDA0004193867410000043
式中：s为惯性导航***或车辆轮速里程计的预测值与量测值的残差，s0为设定的残差阈值，r为不同传感器的调节系数。
在上述技术方案中，全局定位准确率函数将量测值作为迭代初始值，通过最优迭代算法缩小预测值和量测值之间的误差而提高定位准确率，从而获得最优***状态估计值。
在上述技术方案中，全球导航卫星***在k时刻状态估计误差计算的量测值为通过GNSS的原始位置观测值通过噪声补偿获得，预测值为经过GNSS天线杆臂补偿的车辆导航坐标系位置信息。
在上述技术方案中，惯性导航***采用惯性预积分构建[k-1，k]时间段的自动驾驶车辆相对位置、姿态与速度，作为k时刻的位置、姿态与速度的量测值。
在上述技术方案中，轮式里程计采用轮式里程计预积分构建[k-1，k]时间段内的位置增量，作为k时刻状态估计误差计算的量测值。
第二方面，本发明提出一种车辆，所述车辆上配置有采用上述任一种方法实现的定位***。
第三方面，本发明提出一种计算机可读存储介质，存储有能够被处理器加载并执行上述任一种方法的计算机程序。
附图说明
为了更清楚地说明本申请实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1、一种具体实施方式中的方法流程示意图；
图2、一种具体实施方式中的影响力函数图像示意图；
图3、一种具体实施方式中的滑动时窗示意图；
图4、一种具体实施方式中GNSS可信度函数图像示意图
图5、一种具体实施方式中INS/ODO可信度函数图像示意图；。
具体实施方式
由于自动驾驶车辆运行环境的复杂性和未知性引发的各导航传感器性能变化，将导致各传感器异常输出甚至失效，如全球导航卫星***(GNSS)会由于障碍物遮挡、周跳偏差和多径效应等原因导致定位精度下降甚至拒止情况出现。基于此，本发明提出一种车辆定位方法，提供了两种提高定位精度的技术手段，一方面是引入状态影响力，由于要估计的状态集合包括历史状态，每个历史状态会被多次估计，随着时间更新，某个历史状态距离最新状态的距离在逐渐变大，该历史状态对最新状态估计准确度的影响力是在逐步减小的，因此引入状态影响力；另一方面是不仅考虑状态估计的影响力，还考虑传感器的可靠性，通过可靠性对传感器异常信息进行修正或补偿，以增强自动驾驶车辆定位***的环境适应能力，提高***鲁棒性。具体实施方式如下。
图1为方法实施流程示意图。流程示意图的操作可以不按顺序实现。相反，操作可以以反转顺序或同时实现。此外，可以向流程示意图添加一个或多个其他操作。可以从流程示意图中移除一个或多个操作。
S10、确定在车辆定位中使用的传感器集合，对传感器集合中的每个传感器，确定要估计的状态集合。
在该步骤的一种实施方式中，确定的传感器集合为：{全球导航卫星***(GNSS)，惯性导航***(INS)，车辆轮速里程计(ODO)}。但还可以只采用其中的一种或两种传感器，对同一种传感器也不做数量限定。
当为两类或两类以上传感器时，会存在传感器采集过程中的时间不同步的问题，以传感器集合{全球导航卫星***(GNSS)，惯性导航***(INS)，车辆轮速里程计(ODO)}为例，全球导航卫星***的采样时间为高精度GNSS时间，由导航卫星与原子钟共同维护，INS和ODO的采样时间均为本地时间。而GNSS接收机获取由导航卫星或地面基站发出的导航信号，获取载体当前时刻的地心地固坐标系下的位置信息(本实施例在WGS-84坐标系下的经纬高信息表示载***置)。惯性导航***接收INS传感器坐标系相对于地心惯性坐标系的加速度与角速度信息，通过惯性解算后可得到载体相对于前一时刻INS坐标系下的位置、速度与姿态信息。ODO通过接收车轮的编码器脉冲数据，计算载体相对前一时刻ODO坐标系下的位置增量。各传感器之间的空间信息的坐标系不统一，不具有可比性，不方便最后对车辆定位估计。
通过对各传感器的时间信息进行同步处理，统一所有待融合传感器的时间***，以保证融合结果的准确性。此外，如果将多源传感器计算的导航信息统一到导航坐标系中，有利于定位信息比较计算。
以传感器集合{全球导航卫星***(GNSS)，惯性导航***(INS)，车辆轮速里程计(ODO)}为例，对三类传感器进行硬件时间同步，将所有传感器的时间***均同步为GNSS时间，通过结合GNSS接收机1PPS触发信号与其他传感器的采集时刻接口信号，可求得各传感器在采样完成时刻的GNSS时间，同步INS与ODO的采样时间戳。将世界坐标系作为导航坐标系，以GNSS初始时刻定位结果通过GNSS天线杆臂补偿后作为导航坐标系原点0，x轴为指向东的纬线切线，y轴为指向北的经线切线，z轴垂直于XOY平面，形成右手坐标系。将GNSS、INS、ODO的导航定位结果投影到导航坐标系(n系)中，形成统一坐标系下的各传感器对载体的位置约束信息。
INS在导航坐标系下的k时刻状态量
Figure BDA0004193867410000071
更新可表示为：
Figure BDA0004193867410000072
其中，n为导航坐标系，b为载体坐标系，
Figure BDA0004193867410000081
为k-1时刻的载体导航坐标系n下的状态量，/>
Figure BDA0004193867410000082
为单位时间内的载体导航状态增量，gn为导航坐标系n下的重力矢量，Δt为单位时间增量，/>
Figure BDA0004193867410000083
为k-1时刻的载体坐标系b相对于导航坐标系n的旋转矩阵。
GNSS在导航坐标系下的状态量更新可表示为：
Figure BDA0004193867410000084
其中，n为导航坐标系，e为地心地固坐标系，
Figure BDA0004193867410000085
为k时刻GNSS在导航坐标系n下的定位结果，/>
Figure BDA0004193867410000086
为地心地固坐标系e相对于导航坐标系n的变换矩阵，/>
Figure BDA0004193867410000087
为k时刻GNSS在地心地固坐标系e下的定位结果，/>
Figure BDA0004193867410000088
为***初始化后的GNSS在地心地固坐标系e下的0时刻的定位结果。
ODO在导航坐标系下的状态量更新可表示为：
Figure BDA0004193867410000089
其中，n为导航坐标系，o为轮式里程计坐标系，b为载体坐标系，
Figure BDA00041938674100000810
为k时刻ODO在导航坐标系n下的位置更新结果，/>
Figure BDA00041938674100000811
为k-1时刻载体在导航坐标系下的位置信息，/>
Figure BDA00041938674100000812
为k-1时刻载体坐标系n相对于导航坐标系b的旋转矩阵，/>
Figure BDA00041938674100000813
为轮式里程计坐标系o相对于载体坐标系b的旋转矩阵，/>
Figure BDA00041938674100000814
为k-1时刻到k时刻ODO的载体速度量测值。
对每个传感器，确定要估计的状态集合。状态是某一个时刻传感器的定位信息输出，其可以是一个数值，也可以是一组数值，因传感器而异。状态集合中的数据信息，根据需求设定，所属时刻除了当前时刻，还可以有过去时刻，在这种情况下，是对一段时间的传感器状态进行估计。各个传感器状态输出可以是相同时刻，也可以不是相同时刻，一个传感器两个状态之间的输出间隔可以和其它传感器相同，也可以不同。因此，采用多个传感器时，每个传感器确定的状态集合大小可以相同，也可以不同。
S20、对状态集合中每个状态，计算每个状态估计产生的损失，并对损失进行修正。
在一种实施方式中，通过计算每个状态估计的误差平方和作为每个状态估计产生的损失。
将
Figure BDA0004193867410000091
为传感器c在k时刻状态的量测值，Xc为传感器c待估计状态值，则传感器c在k时刻状态估计误差函数记作/>
Figure BDA0004193867410000092
则传感器c在k时刻状态估计的损失可表示为：
Figure BDA0004193867410000093
∑c，k为传感器c的k时刻协方差矩阵。
下面示例性的给出上述传感器集合中的传感器状态误差估计函数。
(2.1.1)惯性导航***误差函数
先将INS各状态量统一为导航坐标系，然后构建为包含载***置等信息的预测状态与量测状态的差值，以通过最优迭代算法缩小两者差距。
INS的加速度计和陀螺仪分别获取载体在惯性空间下的比力与角速度信息，其量测模型为：
Figure BDA0004193867410000094
其中，ba(t)和bg(t)分别为t时刻的加速度计和陀螺仪的零偏，
Figure BDA0004193867410000101
和/>
Figure BDA0004193867410000102
分别为t时刻的比力和角速度的原始观测量，ft和ωt分别为t时刻经过零偏补偿后的比力和角速度的观测量。
为适应INS的高频特性，保证多源传感器融合定位算法实时性，采用惯性预积分构建一段时间内的自动驾驶车辆相对位置、姿态与速度约束。此外，预积分作为INS输出状态的量测值，可避免状态约束与初始状态量的关联性，减少优化过程中由运动状态更新引起的***重复积分，以提高算法实时性。INS预积分模型如下所示：
Figure BDA0004193867410000103
其中，
Figure BDA0004193867410000104
分别为载体坐标系b下的自动驾驶车辆在k-1时刻到k时刻的位置、速度、姿态预积分，/>
Figure BDA0004193867410000105
为载体坐标系b下的t时刻相对于k-1时刻的姿态四元数。
基于INS预积分的误差表达式为：
Figure BDA0004193867410000106
其中，X表示***状态量，/>
Figure BDA0004193867410000107
表示在载体坐标系b下k-1时刻到k时刻的各传感器量测信息，
Figure BDA0004193867410000108
分别表示载体坐标系b下k-1时刻到k时刻的位置残差、速度残差、姿态残差和加速度计零偏残差与陀螺仪零偏残差，Δt为单位时间增量，gn为导航坐标系n下的重力矢量，/>
Figure BDA0004193867410000111
为k-1时刻导航坐标系n相对于载体坐标系b的旋转四元数，/>
Figure BDA0004193867410000112
为导航坐标系下n的k时刻位置，/>
Figure BDA0004193867410000113
为导航坐标系下n的k-1时刻位置，/>
Figure BDA0004193867410000114
为导航坐标系下n的k时刻速度，/>
Figure BDA0004193867410000115
为导航坐标系下n的k-1时刻速度，/>
Figure BDA0004193867410000116
为导航坐标系下n的k时刻姿态四元数，
Figure BDA0004193867410000117
为导航坐标系下n的k-1时刻姿态四元数，[·]xyz表示返回四元数的虚部部分，/>
Figure BDA0004193867410000118
表示四元数的乘法，/>
Figure BDA0004193867410000119
为k时刻的加速度计零偏，/>
Figure BDA00041938674100001110
为k-1时刻加速度计零偏，/>
Figure BDA00041938674100001111
为k时刻的陀螺仪零偏，/>
Figure BDA00041938674100001112
为k-1时刻陀螺仪零偏。
(2.1.2)轮式里程计误差函数
轮式里程计可以提供载体的速度信息，其导航坐标系下的载体速度模型为：
Figure BDA00041938674100001113
其中，
Figure BDA00041938674100001114
为导航坐标系n下t时刻的载体速度，εv为轮式里程计的线速度量测噪声，so为轮式里程计的标度因数，/>
Figure BDA00041938674100001115
为t时刻轮式里程计坐标系o相对于导航坐标系n的旋转矩阵，/>
Figure BDA00041938674100001116
为轮式里程计的速度量测值。
由于轮速计的高频输出特性使其无法利用所有里程计量测信息估计每帧里程计状态，可采用轮式里程计预积分替代原有状态推导算法。根据载体运动学原理，轮式里程计预积分仅构成相对时间内的位置增量约束，而与初始位置状态无关，其预积分表达式为：
Figure BDA00041938674100001117
其中，
Figure BDA0004193867410000121
为t时刻载体坐标系b相对于导航坐标系n的旋转矩阵，/>
Figure BDA0004193867410000122
为轮式里程计坐标系o相对于载体坐标系b的旋转矩阵。
根据轮式里程计预积分形式，为ODO构建包含载***置信息的预测状态与量测状态的误差函数，其表达式为：
Figure BDA0004193867410000123
其中，X为***状态量，
Figure BDA0004193867410000124
表示在载体坐标系b下k-1时刻到k时刻的各传感器量测信息，/>
Figure BDA0004193867410000125
为k时刻导航坐标系n下载***置信息，/>
Figure BDA0004193867410000126
为k-1时刻导航坐标系n下载***置信息，/>
Figure BDA0004193867410000127
为k-1时刻导航坐标系n相对于载体坐标系b的旋转矩阵。
(2.1.3)全球导航卫星***误差函数
GNSS通过解算可以得到载体的位置量测值，其量测方程为：
Figure BDA0004193867410000128
其中，
Figure BDA0004193867410000129
为k时刻通过噪声补偿后的GNSS位置量测值；pk为k时刻GNSS原始位置观测量；/>
Figure BDA00041938674100001210
为GNSS位置的量测噪声。GNSS误差被定义为经过GNSS天线杆臂补偿的载体导航坐标系位置信息与GNSS在导航坐标系定位量测值的差值，其表达式为：
Figure BDA00041938674100001211
其中，
Figure BDA00041938674100001212
为k时刻载体在导航坐标系n的位置信息，/>
Figure BDA00041938674100001213
为k时刻载体坐标系b相对于导航坐标系n的旋转矩阵，/>
Figure BDA00041938674100001214
为载体坐标系b下的GNSS杆臂信息。
利用上述给出的误差函数，可以计算状态估计误差，从而可计算估计损失。
在一种实施方式中，对损失进行修正，步骤包括：
估计的可信度与影响力结合进行度量或仅采用影响力进行度量，从而对每个状态估计误差进行修正；
其中，影响力为不同时刻***状态对当前状态估计的影响程度。
在一种实施方式中，状态估计的影响力度量值可以为设定值，也可以是计算值。一种计算的实施方式是：对传感器集合中的传感器c，为其设定对应的滑动时窗，统计滑动时窗内的第i个状态估计频次，将其记作
Figure BDA0004193867410000131
通过下述函数计算状态估计影响力/>
Figure BDA0004193867410000132
Figure BDA0004193867410000133
式中，β0为设置的最低影响力；a为影响力变化速率。在一实施方式中，β0值为0.1，a为1.2。影响力度量函数参见图2。
上述影响力计算中采用的滑动时窗，还可以满足对车辆实时定位的需求。参见图3，由于随着时间推移，观测信息不断积累，如果状态估计量过多，则会增加计算负担，如果状态估计量过少，则可能会影响定位精度。通过设置滑动时窗，根据计算能力，调整滑动时窗大小，仅对滑动时窗内的状态节点进行估计，满足车辆实时定位需求。
在一种实施方式中，将滑动时窗大小设置为20s，该滑动时窗中的第一个状态节点在21s时，该状态节点不再被估计。当车辆运动过程中，滑动时窗内的运动状态节点被估计次数越多，对应状态量将具有更高的估计精度，因此设定影响力对该节点的误差进行修正。
作为进一步改进，不仅对状态节点估计的影响力进行度量，还对状态节点估计的误差可信度进行度量。由于状态节点估计的误差是由传感器的定位精度或异常带来的，通过对状态节点估计的误差可信度度量可降低传感器带来的误差影响。可信度可以为设置值，也可以是计算值。在计算时，可为各个传感器建立可信度模型来进行计算。下面示例出上述传感器集合中的传感器建立的可信度模型。
(2.2.1)GNSS可信度模型
由于自动驾驶车辆运行环境的不确定性，通常GNSS会由于障碍物遮挡、周跳偏差和多径效应等原因导致定位精度下降甚至拒止情况出现。为了提高自动驾驶车辆的定位鲁棒性，即在复杂环境中自动驾驶车辆应具有GNSS自主完好性监测能力，具备异常值剔除功能。
GNSS接收机接收来自导航卫星发出的无线电信号，需要至少4颗卫星才能满足载***置计算条件，为了能够进行GNSS接收机自主完好性监测，需要来自更多导航卫星的冗余观测量剔除异常观测值。
在对GNSS接收到的导航卫星伪距观测值进行可信度判别，采用伪距最小二乘残差判别算法，根据伪距残差的大小判定定位结果的可信度。定位伪距残差可以表示为：
wk+1＝(I-G(GTG)-1GT)wk
其中：w为伪距残差向量，I为单位矩阵，G为观测矩阵，其表达式为：
Figure BDA0004193867410000151
其中，ρi为卫星i的伪距信息，(xi yi zi)为卫星i位置信息，i的取值为[1 2 3 4]，(xi yi zi)为接收机位置信息。
本发明将伪距残差向量的平方和设定为GNSS传感器的可信度衡量因素，随着伪距残差的增大，可认为GNSS量测量中存在异常信息，应减小GNSS状态估计的可信度。通过对当前时刻的伪距残差向量设定失准门限值，以剔除伪距残差不满足要求的导航卫星观测异常信息。因此，构建k时刻基于伪距残差的GNSS状态估计可信度模型如下，其函数图像如图4所示：
Figure BDA0004193867410000152
(2.2.2)INS、ODO可信度模型
自动驾驶车辆一般采用MEMS惯导***，在自动驾驶车辆实际运行的复杂环境中，基于MEMS的INS性能会随着环境的变化而导致精度波动，如INS在自动驾驶车辆进行匀速直线行驶过程时无法有效测量。
INS和ODO传感器均通过航迹推算获取载***置信息。为应对载体在不同运行环境中的INS和ODO异常量测信息，本发明设计一种针对测量中异常值扰动的动态权重函数，以降低INS和ODO异常量测对多源传感器融合定位***的影响，提高***定位鲁棒性。
由于INS和ODO的高频特性，在传感器数据采集间隔范围内，自动驾驶车辆普遍处于相对稳定的运动状态，可以通过前一时刻状态量测值得到当前时刻的状态预测值，假设INS与ODO传感器数据采集间隔载体运动状态没有变化，通过预测值与量测值的残差动态确定INS与ODO的可信度，其表达式如下，函数图像如图5所示：
Figure BDA0004193867410000161
其中，s为传感器预测值与量测值的残差，s0为残差合理阈值，即预测值与量测值的残差小于s0时，认定为合理变化范围，根据不同精度的传感器确定；r为不同传感器的调节系数。
至此，得到了示例的传感器集合中各个传感器的可信度模型，通过可信度模型计算各个传感器的可信度，将传感器的可信度作为该传感器状态估计的可信度。
利用每个状态估计的影响力对状态估计的损失进行修正，即将影响力作为损失的一个修正系数；若利用每个状态估计的影响力和可信度对状态估计损失进行修正，即将影响力与可信度的积作为损失的一个修正系数。
将传感器c在k时刻状态估计误差产生的损失可表示为
Figure BDA0004193867410000162
将传感器c在k时刻状态估计损失修正系数记作/>
Figure BDA0004193867410000163
可得每个状态修正后的损失为/>
Figure BDA0004193867410000164
S30、利用每个状态修正后的损失进行定位准确率计算，进而建立如下全局定位准确率函数：
Figure BDA0004193867410000171
Figure BDA0004193867410000172
式中：
式中：D(χ)为全局代价函数，C为传感器集合，c为传感器集合中元素，
Figure BDA0004193867410000173
为传感器c在k时刻状态估计损失修正系数，/>
Figure BDA0004193867410000174
为传感器c在k时刻状态估计损失，Xc为传感器c待估计状态值，χ为待估计状态集合，f(Xc)为传感器c进行状态估计损失的准确率。
S40、获取使全局定位准确率函数取得最大值的***状态估计值，将其作为车辆最优定位值。
上述全局定位准确率函数为目标优化函数，为方便计算，对其求对数，再在两边同时乘以-2，可得一个新的目标优化函数如下：
Figure BDA0004193867410000175
式中：Sc为传感器c进行状态估计的时刻集合，
Figure BDA0004193867410000176
为传感器c在k时刻状态估计损失修正系数，C为传感器集合，c为传感器集合中元素。
在这种形式下，
Figure BDA0004193867410000177
可视为全局损失函数，当使其取得最小值时，可获得车辆最优定位信息。使用非线性优化方法获取自动驾驶车辆导航状态的最优估计值/>
Figure BDA0004193867410000178
Figure BDA0004193867410000179
在一种实施方式中，通过将上式状态参数估计转化为最小二乘问题，使用高斯牛顿非线性优化方法作为最优迭代算法，对所述目标函数进行求解，获取载体最优导航估计值
Figure BDA0004193867410000181
在本发明方法实施过程中，可以看出：
(1)本发明方法不仅考虑传感器异常对定位精度的影响，同时考虑因历史信息增加带来计算量增加，而影响状态估计影响力，通过引入传感器状态估计影响力度量，实现不同时刻的各状态估计误差自适应修正。
(2)本发明方法具有普适性，可根据实际情况选择传感器集合中传感器，可以为单种或单个，尽管单个传感器具有异常带来的估计误差，但是可以通过状态估计影响力和可信度度量对估计误差进行修正，进而减小传感器异常影响。其中，可信度度量函数能够剔除传感器异常值的作用。也可以采用多种或多个传感器组合，以充分发挥各传感器的优势，毕竟所有传感器同时异常的概率是比较低的，可提高定位的鲁棒性。
在另一实施方式中，在自动驾驶车辆中配置由实施例方法实现的***，提高车辆定位准确性。
通过以上的实施方式的描述，所属领域的技术人员可以清楚地了解到本公开方法可借助软件加必需的通用硬件的方式来实现，当然也可以通过专用硬件包括专用集成电路、专用CPU、专用存储器、专用元器件等来实现。一般情况下，凡由计算机程序完成的功能都可以很容易地用相应的硬件来实现，而且，用来实现同一功能的具体硬件结构也可以是多种多样的，例如模拟电路、数字电路或专用电路等。但是，对本公开而言更多情况下，软件程序实现是更佳的实施方式。
尽管以上结合附图对本发明的实施方案进行了描述，但本发明并不局限于上述的具体实施方案和应用领域，上述的具体实施方案仅仅是示意性的、指导性的，而不是限制性的。本领域的普通技术人员在本说明书的启示下和在不脱离本发明权利要求所保护的范围的情况下，还可以做出很多种的形式，这些均属于本发明保护之列。

Claims (10)

1.一种车辆位置估计方法，其特征在于，所述方法包括下述步骤：
确定在车辆定位中使用的传感器集合，对传感器集合中的每个传感器，确定要估计的状态集合；
对状态集合中每个状态，计算每个状态估计产生的损失，并对损失进行修正；
对每个状态修正后的损失进行定位准确率计算，进而建立如下全局定位准确率函数：
Figure FDA0004193867400000011
Figure FDA0004193867400000012
式中：D(χ)为全局代价函数，C为传感器集合，c为传感器集合中元素，
Figure FDA0004193867400000013
为传感器c在k时刻状态估计损失修正系数，/>
Figure FDA0004193867400000014
为传感器c在k时刻状态估计损失，Xc为传感器c待估计状态值，f(Xc)为传感器c进行状态估计损失的准确率；
获取使全局定位准确率函数取得最大值的***状态估计值，将其作为车辆最优定位值。
2.根据权利要求1所述的方法，其特征在于，对损失进行修正，步骤包括：
估计的可信度与影响力结合进行度量或仅采用影响力进行度量，从而对每个状态估计误差进行修正；
其中，影响力为不同时刻***状态对当前状态估计的影响程度。
3.根据权利要求2所述的方法，其特征在于，影响力为设定值或为计算值，计算步骤包括：
对传感器集合中的传感器c，为其设定对应的滑动时窗，统计滑动时窗内的第i个状态估计频次，将其记作
Figure FDA0004193867400000021
通过下述函数计算状态估计影响力
Figure FDA0004193867400000022
Figure FDA0004193867400000023
式中，β0为设置的最低影响力；a为影响力变化速率。
4.根据权利要求2所述的方法，其特征在于，传感器集合为：{全球导航卫星***(GNSS)，惯性导航***(INS)，车辆轮速里程计(ODO)}，将INS、ODO传感器时间同步为GNSS时间，采用世界坐标系确定为导航坐标系。
5.根据权利要求4所述的方法，其特征在于，可信度与影响力结合为将可信度与影响力进行相乘，可信度为计算值，计算步骤包括：
当传感器为全球导航卫星***时，其k时刻的可信度
Figure FDA0004193867400000024
通过下式计算：
Figure FDA0004193867400000025
式中：G为观测矩阵，I为单位矩阵，wk为k时刻的GNSS定位伪距残差向量，wk-1为k-1时刻的GNSS定位伪距残差向量；
当传感器为惯性导航***、车辆轮速里程计时，其k时刻的状态估计可信度
Figure FDA0004193867400000026
通过下式计算：
Figure FDA0004193867400000031
式中：s为惯性导航***或车辆轮速里程计的预测值与量测值的残差，s0为设定的残差阈值，r为不同传感器的调节系数。
6.根据权利要求1所述的方法，其特征在于，全局定位准确率函数将量测值作为迭代初始值，通过最优迭代算法缩小预测值和量测值之间的误差而提高定位准确率，从而获得最优***状态估计值。
7.根据权利要求6所述的方法，其特征在于，用于全球导航卫星***在k时刻状态估计误差计算的量测值为通过GNSS的原始位置观测值通过噪声补偿获得，预测值为经过GNSS天线杆臂补偿的车辆导航坐标系位置信息。
8.根据权利要求6所述的方法，其特征在于，惯性导航***采用惯性预积分构建[k-1，k]时间段的自动驾驶车辆相对位置、姿态与速度，作为k时刻的位置、姿态与速度的量测值。
9.根据权利要求6所述的方法，其特征在于，轮式里程计采用轮式里程计预积分构建[k-1，k]时间段内的位置增量，作为k时刻状态估计误差计算的量测值。
10.一种计算机可读存储介质，其特征在于：存储有能够被处理器加载并执行如权利要求1至9中任一种方法的计算机程序。
CN202310440555.7A 2023-04-23 2023-04-23 一种车辆位置估计方法 Pending CN116399351A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310440555.7A CN116399351A (zh) 2023-04-23 2023-04-23 一种车辆位置估计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310440555.7A CN116399351A (zh) 2023-04-23 2023-04-23 一种车辆位置估计方法

Publications (1)

Publication Number Publication Date
CN116399351A true CN116399351A (zh) 2023-07-07

Family

ID=87014126

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310440555.7A Pending CN116399351A (zh) 2023-04-23 2023-04-23 一种车辆位置估计方法

Country Status (1)

Country Link
CN (1) CN116399351A (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117310756A (zh) * 2023-11-30 2023-12-29 宁波路特斯机器人有限公司 多传感器融合定位方法及***、机器可读存储介质
CN117761745A (zh) * 2024-02-22 2024-03-26 中国科学院空天信息创新研究院 一种用于基于历史信息融合的少星定位的信息优选方法

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117310756A (zh) * 2023-11-30 2023-12-29 宁波路特斯机器人有限公司 多传感器融合定位方法及***、机器可读存储介质
CN117310756B (zh) * 2023-11-30 2024-03-29 宁波路特斯机器人有限公司 多传感器融合定位方法及***、机器可读存储介质
CN117761745A (zh) * 2024-02-22 2024-03-26 中国科学院空天信息创新研究院 一种用于基于历史信息融合的少星定位的信息优选方法
CN117761745B (zh) * 2024-02-22 2024-04-30 中国科学院空天信息创新研究院 一种用于基于历史信息融合的少星定位的信息优选方法

Similar Documents

Publication Publication Date Title
CN111721289B (zh) 自动驾驶中车辆定位方法、装置、设备、存储介质及车辆
CN112268559B (zh) 复杂环境下融合slam技术的移动测量方法
CN110823217B (zh) 一种基于自适应联邦强跟踪滤波的组合导航容错方法
CN111156994B (zh) 一种基于mems惯性组件的ins/dr&gnss松组合导航方法
CN116399351A (zh) 一种车辆位置估计方法
CN113203418B (zh) 基于序贯卡尔曼滤波的gnssins视觉融合定位方法及***
US7058505B1 (en) System for navigation redundancy
CN112505737B (zh) 一种gnss/ins组合导航方法
WO2022133294A1 (en) System and method for fusing dead reckoning and gnss data streams
CN113465628B (zh) 惯性测量单元数据补偿方法及***
CN115143954B (zh) 一种基于多源信息融合的无人车导航方法
CN112697138A (zh) 一种基于因子图优化的仿生偏振同步定位与构图的方法
CN113252048B (zh) 一种导航定位方法、导航定位***及计算机可读存储介质
CN112946681B (zh) 融合组合导航信息的激光雷达定位方法
JP5664059B2 (ja) 車両用軌跡推定装置
CN114915913A (zh) 一种基于滑窗因子图的uwb-imu组合室内定位方法
CN113566850B (zh) 惯性测量单元的安装角度标定方法、装置和计算机设备
Wang et al. A robust backtracking CKF based on Krein space theory for in-motion alignment process
US8566055B1 (en) Gyro indexing compensation method and system
CN109459769B (zh) 一种自主定位方法与***
CN116772837A (zh) 基于交互式多模型的gnss/sins组合导航方法
CN116150565A (zh) 一种用于单频gnss/mems-imu/里程计低功耗实时组合导航的改进自适应抗差卡尔曼滤波算法及***
CN114001730A (zh) 融合定位方法、装置、计算机设备和存储介质
CN113566849B (zh) 惯性测量单元的安装角度标定方法、装置和计算机设备
CN117948986B (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