CN116399351A - 一种车辆位置估计方法 - Google Patents
一种车辆位置估计方法 Download PDFInfo
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 35
- 238000012937 correction Methods 0.000 claims abstract description 13
- 238000005259 measurement Methods 0.000 claims description 27
- 239000011159 matrix material Substances 0.000 claims description 17
- 238000004364 calculation method Methods 0.000 claims description 11
- 230000010354 integration Effects 0.000 claims description 10
- 238000004422 calculation algorithm Methods 0.000 claims description 9
- 230000008859 change Effects 0.000 claims description 9
- 239000013598 vector Substances 0.000 claims description 9
- 238000004590 computer program Methods 0.000 claims description 3
- 230000006870 function Effects 0.000 description 32
- 230000002159 abnormal effect Effects 0.000 description 13
- 230000004927 fusion Effects 0.000 description 8
- 238000005457 optimization Methods 0.000 description 7
- 238000010586 diagram Methods 0.000 description 5
- 238000005070 sampling Methods 0.000 description 5
- 230000005856 abnormality Effects 0.000 description 3
- 230000000694 effects Effects 0.000 description 3
- 230000008569 process Effects 0.000 description 3
- 230000008901 benefit Effects 0.000 description 2
- 230000005484 gravity Effects 0.000 description 2
- 238000012544 monitoring process Methods 0.000 description 2
- 230000009467 reduction Effects 0.000 description 2
- 230000001133 acceleration Effects 0.000 description 1
- 238000013473 artificial intelligence Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
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
技术领域
本发明涉及自动驾驶车辆定位领域,尤其涉及一种车辆位置估计方法。
背景技术
在新一代信息技术和人工智能技术不断发展的背景下,自动驾驶车辆已经成为当下智能交通***研究和应用的热点。其中,自动驾驶车辆定位***能够实时、准确地提供自车的位姿信息,是自动驾驶车辆自主决策、路径规划和底层协同控制的必要前提,对于保证交通环境中交通参与者的出行安全、提升自动驾驶车辆的安全性具有重要意义。
但是,自动驾驶车辆定位***中的导航定位传感器会因运行环境复杂多变而引起精度波动,当前的融合算法并未考虑各传感器定位精度变化,而定位精度变化会引起定位信息可用性的改变,如果不对异常传感器的导航定位信息进行修正或补偿,多源传感器融合定位结果将产生漂移。除此之外,目前主流采用基于固定权重滑动时窗的图优化方法融合多源传感器导航信息,以达到***实时性需求,但固定权重滑动时窗针对不同优化次数的因子节点享有相同权重,导致定位准确低。
发明内容
本发明的目的是提供一种车辆位置估计方法,以解决或至少部分地解决现有技术中,由于自动驾驶车辆运行环境复杂多变引起的导航定位传感器的精度波动而定位不准确的问题,提高车辆定位的精度与鲁棒性。本发明的技术方案如下。
第一方面,本发明提出一种车辆位置估计方法,所述方法包括下述步骤:
确定在车辆定位中使用的传感器集合,对传感器集合中的每个传感器,确定要估计的状态集合;
对状态集合中每个状态,计算每个状态估计产生的损失,并对损失进行修正;
对每个状态修正后的损失进行定位准确率计算,进而建立如下全局定位准确率函数:
式中:D(χ)为全局代价函数,C为传感器集合,c为传感器集合中元素,为传感器c在k时刻状态估计损失修正系数,/>为传感器c在k时刻状态估计损失,Xc为传感器c待估计状态值,χ为待估计状态集合,f(Xc)为传感器c进行状态估计损失的准确率;
获取使全局定位准确率函数取得最大值的***状态估计值,将其作为车辆最优定位值。
在上述技术方案中,对损失进行修正的一种实施方式,步骤包括:
估计的可信度与影响力结合进行度量或仅采用影响力进行度量,从而对每个状态估计误差进行修正;
其中,影响力为不同时刻***状态对当前状态估计的影响程度。
在这种实施方式中,提供了两种提高定位精度的技术手段,一方面是引入状态影响力,由于要估计的状态集合包括历史状态,每个历史状态会被多次估计,随着时间更新,某个历史状态距离最新状态的距离在逐渐变大,该历史状态对最新状态估计准确度的影响力是在逐步减小的,因此引入状态影响力;另一方面是不仅考虑状态影响力,还考虑传感器的可信度,通过可信度对传感器异常信息进行修正或补偿。
在上述技术方案中,传感器集合为:{全球导航卫星***(GNSS),惯性导航***(INS),车辆轮速里程计(ODO)},将INS、ODO传感器时间同步为GNSS时间,采用世界坐标系确定为导航坐标系。通过多源传感器融合克服单一传感器的异常信息时定位结果漂移的问题,而采用多源传感器进行时空同步,以保证融合结果的准确性和融合时的稳定输入。
在上述技术方案中,影响力为设定值或为计算值,计算步骤包括:
式中,β0为设置的最低影响力;a为影响力变化速率。
在上述技术方案中,可信度与影响力结合为将可信度与影响力进行相乘,可信度为计算值,计算步骤包括:
式中:G为观测矩阵,I为单位矩阵,wk为k时刻的GNSS定位伪距残差向量,wk-1为k-1时刻的GNSS定位伪距残差向量;
式中: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系)中,形成统一坐标系下的各传感器对载体的位置约束信息。
其中,n为导航坐标系,b为载体坐标系,为k-1时刻的载体导航坐标系n下的状态量,/>为单位时间内的载体导航状态增量,gn为导航坐标系n下的重力矢量,Δt为单位时间增量,/>为k-1时刻的载体坐标系b相对于导航坐标系n的旋转矩阵。
GNSS在导航坐标系下的状态量更新可表示为:
其中,n为导航坐标系,e为地心地固坐标系,为k时刻GNSS在导航坐标系n下的定位结果,/>为地心地固坐标系e相对于导航坐标系n的变换矩阵,/>为k时刻GNSS在地心地固坐标系e下的定位结果,/>为***初始化后的GNSS在地心地固坐标系e下的0时刻的定位结果。
ODO在导航坐标系下的状态量更新可表示为:
其中,n为导航坐标系,o为轮式里程计坐标系,b为载体坐标系,为k时刻ODO在导航坐标系n下的位置更新结果,/>为k-1时刻载体在导航坐标系下的位置信息,/>为k-1时刻载体坐标系n相对于导航坐标系b的旋转矩阵,/>为轮式里程计坐标系o相对于载体坐标系b的旋转矩阵,/>为k-1时刻到k时刻ODO的载体速度量测值。
对每个传感器,确定要估计的状态集合。状态是某一个时刻传感器的定位信息输出,其可以是一个数值,也可以是一组数值,因传感器而异。状态集合中的数据信息,根据需求设定,所属时刻除了当前时刻,还可以有过去时刻,在这种情况下,是对一段时间的传感器状态进行估计。各个传感器状态输出可以是相同时刻,也可以不是相同时刻,一个传感器两个状态之间的输出间隔可以和其它传感器相同,也可以不同。因此,采用多个传感器时,每个传感器确定的状态集合大小可以相同,也可以不同。
S20、对状态集合中每个状态,计算每个状态估计产生的损失,并对损失进行修正。
在一种实施方式中,通过计算每个状态估计的误差平方和作为每个状态估计产生的损失。
下面示例性的给出上述传感器集合中的传感器状态误差估计函数。
(2.1.1)惯性导航***误差函数
先将INS各状态量统一为导航坐标系,然后构建为包含载***置等信息的预测状态与量测状态的差值,以通过最优迭代算法缩小两者差距。
INS的加速度计和陀螺仪分别获取载体在惯性空间下的比力与角速度信息,其量测模型为:
为适应INS的高频特性,保证多源传感器融合定位算法实时性,采用惯性预积分构建一段时间内的自动驾驶车辆相对位置、姿态与速度约束。此外,预积分作为INS输出状态的量测值,可避免状态约束与初始状态量的关联性,减少优化过程中由运动状态更新引起的***重复积分,以提高算法实时性。INS预积分模型如下所示:
基于INS预积分的误差表达式为:
其中,X表示***状态量,/>表示在载体坐标系b下k-1时刻到k时刻的各传感器量测信息,分别表示载体坐标系b下k-1时刻到k时刻的位置残差、速度残差、姿态残差和加速度计零偏残差与陀螺仪零偏残差,Δt为单位时间增量,gn为导航坐标系n下的重力矢量,/>为k-1时刻导航坐标系n相对于载体坐标系b的旋转四元数,/>为导航坐标系下n的k时刻位置,/>为导航坐标系下n的k-1时刻位置,/>为导航坐标系下n的k时刻速度,/>为导航坐标系下n的k-1时刻速度,/>为导航坐标系下n的k时刻姿态四元数,为导航坐标系下n的k-1时刻姿态四元数,[·]xyz表示返回四元数的虚部部分,/>表示四元数的乘法,/>为k时刻的加速度计零偏,/>为k-1时刻加速度计零偏,/>为k时刻的陀螺仪零偏,/>为k-1时刻陀螺仪零偏。
(2.1.2)轮式里程计误差函数
轮式里程计可以提供载体的速度信息,其导航坐标系下的载体速度模型为:
由于轮速计的高频输出特性使其无法利用所有里程计量测信息估计每帧里程计状态,可采用轮式里程计预积分替代原有状态推导算法。根据载体运动学原理,轮式里程计预积分仅构成相对时间内的位置增量约束,而与初始位置状态无关,其预积分表达式为:
根据轮式里程计预积分形式,为ODO构建包含载***置信息的预测状态与量测状态的误差函数,其表达式为:
其中,X为***状态量,表示在载体坐标系b下k-1时刻到k时刻的各传感器量测信息,/>为k时刻导航坐标系n下载***置信息,/>为k-1时刻导航坐标系n下载***置信息,/>为k-1时刻导航坐标系n相对于载体坐标系b的旋转矩阵。
(2.1.3)全球导航卫星***误差函数
GNSS通过解算可以得到载体的位置量测值,其量测方程为:
其中,为k时刻通过噪声补偿后的GNSS位置量测值;pk为k时刻GNSS原始位置观测量;/>为GNSS位置的量测噪声。GNSS误差被定义为经过GNSS天线杆臂补偿的载体导航坐标系位置信息与GNSS在导航坐标系定位量测值的差值,其表达式为:
利用上述给出的误差函数,可以计算状态估计误差,从而可计算估计损失。
在一种实施方式中,对损失进行修正,步骤包括:
估计的可信度与影响力结合进行度量或仅采用影响力进行度量,从而对每个状态估计误差进行修正;
其中,影响力为不同时刻***状态对当前状态估计的影响程度。
在一种实施方式中,状态估计的影响力度量值可以为设定值,也可以是计算值。一种计算的实施方式是:对传感器集合中的传感器c,为其设定对应的滑动时窗,统计滑动时窗内的第i个状态估计频次,将其记作通过下述函数计算状态估计影响力/>
式中,β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为观测矩阵,其表达式为:
其中,ρi为卫星i的伪距信息,(xi yi zi)为卫星i位置信息,i的取值为[1 2 3 4],(xi yi zi)为接收机位置信息。
本发明将伪距残差向量的平方和设定为GNSS传感器的可信度衡量因素,随着伪距残差的增大,可认为GNSS量测量中存在异常信息,应减小GNSS状态估计的可信度。通过对当前时刻的伪距残差向量设定失准门限值,以剔除伪距残差不满足要求的导航卫星观测异常信息。因此,构建k时刻基于伪距残差的GNSS状态估计可信度模型如下,其函数图像如图4所示:
(2.2.2)INS、ODO可信度模型
自动驾驶车辆一般采用MEMS惯导***,在自动驾驶车辆实际运行的复杂环境中,基于MEMS的INS性能会随着环境的变化而导致精度波动,如INS在自动驾驶车辆进行匀速直线行驶过程时无法有效测量。
INS和ODO传感器均通过航迹推算获取载***置信息。为应对载体在不同运行环境中的INS和ODO异常量测信息,本发明设计一种针对测量中异常值扰动的动态权重函数,以降低INS和ODO异常量测对多源传感器融合定位***的影响,提高***定位鲁棒性。
由于INS和ODO的高频特性,在传感器数据采集间隔范围内,自动驾驶车辆普遍处于相对稳定的运动状态,可以通过前一时刻状态量测值得到当前时刻的状态预测值,假设INS与ODO传感器数据采集间隔载体运动状态没有变化,通过预测值与量测值的残差动态确定INS与ODO的可信度,其表达式如下,函数图像如图5所示:
其中,s为传感器预测值与量测值的残差,s0为残差合理阈值,即预测值与量测值的残差小于s0时,认定为合理变化范围,根据不同精度的传感器确定;r为不同传感器的调节系数。
至此,得到了示例的传感器集合中各个传感器的可信度模型,通过可信度模型计算各个传感器的可信度,将传感器的可信度作为该传感器状态估计的可信度。
利用每个状态估计的影响力对状态估计的损失进行修正,即将影响力作为损失的一个修正系数;若利用每个状态估计的影响力和可信度对状态估计损失进行修正,即将影响力与可信度的积作为损失的一个修正系数。
S30、利用每个状态修正后的损失进行定位准确率计算,进而建立如下全局定位准确率函数:
式中:
式中:D(χ)为全局代价函数,C为传感器集合,c为传感器集合中元素,为传感器c在k时刻状态估计损失修正系数,/>为传感器c在k时刻状态估计损失,Xc为传感器c待估计状态值,χ为待估计状态集合,f(Xc)为传感器c进行状态估计损失的准确率。
S40、获取使全局定位准确率函数取得最大值的***状态估计值,将其作为车辆最优定位值。
上述全局定位准确率函数为目标优化函数,为方便计算,对其求对数,再在两边同时乘以-2,可得一个新的目标优化函数如下:
(1)本发明方法不仅考虑传感器异常对定位精度的影响,同时考虑因历史信息增加带来计算量增加,而影响状态估计影响力,通过引入传感器状态估计影响力度量,实现不同时刻的各状态估计误差自适应修正。
(2)本发明方法具有普适性,可根据实际情况选择传感器集合中传感器,可以为单种或单个,尽管单个传感器具有异常带来的估计误差,但是可以通过状态估计影响力和可信度度量对估计误差进行修正,进而减小传感器异常影响。其中,可信度度量函数能够剔除传感器异常值的作用。也可以采用多种或多个传感器组合,以充分发挥各传感器的优势,毕竟所有传感器同时异常的概率是比较低的,可提高定位的鲁棒性。
在另一实施方式中,在自动驾驶车辆中配置由实施例方法实现的***,提高车辆定位准确性。
通过以上的实施方式的描述,所属领域的技术人员可以清楚地了解到本公开方法可借助软件加必需的通用硬件的方式来实现,当然也可以通过专用硬件包括专用集成电路、专用CPU、专用存储器、专用元器件等来实现。一般情况下,凡由计算机程序完成的功能都可以很容易地用相应的硬件来实现,而且,用来实现同一功能的具体硬件结构也可以是多种多样的,例如模拟电路、数字电路或专用电路等。但是,对本公开而言更多情况下,软件程序实现是更佳的实施方式。
尽管以上结合附图对本发明的实施方案进行了描述,但本发明并不局限于上述的具体实施方案和应用领域,上述的具体实施方案仅仅是示意性的、指导性的,而不是限制性的。本领域的普通技术人员在本说明书的启示下和在不脱离本发明权利要求所保护的范围的情况下,还可以做出很多种的形式,这些均属于本发明保护之列。
Claims (10)
2.根据权利要求1所述的方法,其特征在于,对损失进行修正,步骤包括:
估计的可信度与影响力结合进行度量或仅采用影响力进行度量,从而对每个状态估计误差进行修正;
其中,影响力为不同时刻***状态对当前状态估计的影响程度。
4.根据权利要求2所述的方法,其特征在于,传感器集合为:{全球导航卫星***(GNSS),惯性导航***(INS),车辆轮速里程计(ODO)},将INS、ODO传感器时间同步为GNSS时间,采用世界坐标系确定为导航坐标系。
6.根据权利要求1所述的方法,其特征在于,全局定位准确率函数将量测值作为迭代初始值,通过最优迭代算法缩小预测值和量测值之间的误差而提高定位准确率,从而获得最优***状态估计值。
7.根据权利要求6所述的方法,其特征在于,用于全球导航卫星***在k时刻状态估计误差计算的量测值为通过GNSS的原始位置观测值通过噪声补偿获得,预测值为经过GNSS天线杆臂补偿的车辆导航坐标系位置信息。
8.根据权利要求6所述的方法,其特征在于,惯性导航***采用惯性预积分构建[k-1,k]时间段的自动驾驶车辆相对位置、姿态与速度,作为k时刻的位置、姿态与速度的量测值。
9.根据权利要求6所述的方法,其特征在于,轮式里程计采用轮式里程计预积分构建[k-1,k]时间段内的位置增量,作为k时刻状态估计误差计算的量测值。
10.一种计算机可读存储介质,其特征在于:存储有能够被处理器加载并执行如权利要求1至9中任一种方法的计算机程序。
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)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN117310756A (zh) * | 2023-11-30 | 2023-12-29 | 宁波路特斯机器人有限公司 | 多传感器融合定位方法及***、机器可读存储介质 |
CN117761745A (zh) * | 2024-02-22 | 2024-03-26 | 中国科学院空天信息创新研究院 | 一种用于基于历史信息融合的少星定位的信息优选方法 |
-
2023
- 2023-04-23 CN CN202310440555.7A patent/CN116399351A/zh active Pending
Cited By (4)
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 |