CN110221333A - 一种车载ins/od组合导航***的量测误差补偿方法 - Google Patents
一种车载ins/od组合导航***的量测误差补偿方法 Download PDFInfo
- Publication number
- CN110221333A CN110221333A CN201910288827.XA CN201910288827A CN110221333A CN 110221333 A CN110221333 A CN 110221333A CN 201910288827 A CN201910288827 A CN 201910288827A CN 110221333 A CN110221333 A CN 110221333A
- Authority
- CN
- China
- Prior art keywords
- error
- odometer
- vehicle
- ins
- measurement
- 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
Links
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
-
- 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/40—Correcting position, velocity or attitude
-
- 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/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
- G01S19/49—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an inertial position system, e.g. loosely-coupled
-
- 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/52—Determining velocity
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Automation & Control Theory (AREA)
- Navigation (AREA)
Abstract
本发明涉及一种车载INS/OD组合导航***的量测误差补偿方法,本发明技术解决的问题是:在分布式驱动电动智能汽车上,以车载里程计辅助惯性导航***进行车辆合导航,***考虑车辆行驶工况变化引起的里程仪刻度系数,和里程计数据可用性与提供测量补偿的判断,以及在组合导航中标定INS与左右两侧里程计***的安装偏置角和杆臂误差,并据此作为组合导航***量测方程中的测量量进行修正,构建以惯导位置、速度、姿态误差、惯性器件随机常值误差、左右里程仪刻度系数误差,杆臂为状态量的滤波***,并进行反馈校正,实现基于量测误差补偿的组合导航定位精度提高。
Description
技术领域
本发明涉及车载导航与定位技术领域,尤其是涉及一种车载INS/OD组合导航***的量测误差补偿方法。
背景技术
惯性导航***(Inertial Navigation System,INS)具有高自主性、抗干扰性、高的短期精度、高数据输出率、完备的导航信息、适应范围广等特点,但是其***误差具有周期振荡的特性,某些导航参数误差具有随时间积累的特性且初始对准所需时间较长;为了弥补INS***的误差,出现了以INS为主的组合导航***的多种组合方式,例如:惯导/里程计、惯导/卫星、惯导/地磁、惯导/多普勒等等,其中里程计方案成本低且抗干扰能力较强。但由于里程计在车轮轮心处,而INS安装在车体上,两个导航子***的安装位置不重合且存在一定安装角度,即量测误差。当车辆发生有姿态变化时,里程计输出有偏差,因为若不能精确估计出车辆车体与里程计之间相对姿态运动,则会对由INS/OD组成的车载导航***导航精度产生严重影响。同时当车辆打滑、侧滑、跳跃时候里程计则通常被视为故障,其输出不可信。
另外由于车辆行驶工况不同,轮胎半径会发生变化,此时里程计输出随之变化,即实际车速与里程计的输出值之间存在一个刻度系数,该系数的变化对于里程计输出有着很大的影响,故需要对该值进行估计与校正。目前的方法有三种:一种是将里程仪刻度系数认为是常值,与实际不符,二是设定为常值加上随机误差项组成,不能适应大部分工况,三是根据加速度和坡度等相关变量建立相应的误差模型以补偿刻度系数变化,建模变量因素不全且较为麻烦。
如专利申请号201210584022的一种提高车载SINS/OD组合导航精度的方法公开了如下内容:“根据路面坡度和车辆加速度,计算里程仪刻度系数变化和车辆底盘与车体之间相对角运动,并对里程仪输出进行补偿,以求取精确的车辆行驶速度。利用SINS解算的速度信息和OD解算的速度信息之差构造量测,通过卡尔曼滤波估计出模型参数及导航误差参数,利用求得的状态量对SINS和OD数据进行修正,以得到准确的导航参数。在进行位置修正时,考虑位置误差的可观测性较弱,不直接利用状态估计值修正位置,而采用修正后的速度推算位置。本发明考虑路面坡度和车辆加速度对车载SINS/OD组合导航精度的影响,通过建立相应的数学模型,实现了车载SINS/OD高精度组合导航。”
车载INS/OD组合导航***方案存在以下不足:
1.缺乏对安装于车辆上的惯性导航***与里程计进行量测一致性参数标定与估计;
2.未充分考虑由于车辆姿态变化导致的杆臂动态变化;
3.对于由于车辆行驶工况变化引起的里程仪刻度系数没有一个良好的估计与自适应;
4.缺少对于在车辆存在打滑、侧滑、跳跃、转弯和加减速工况,实际的里程计数据可用性与提供测量补偿的判断。
发明内容
本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种车载INS/OD组合导航***的量测误差补偿方法,在分布式驱动电动智能汽车上,以车载里程计辅助惯性导航***进行车辆合导航,***考虑车辆行驶工况变化引起的里程仪刻度系数,和里程计数据可用性与提供测量补偿的判断,以及在组合导航中标定INS与左右两侧里程计***的安装偏置角和杆臂误差,并据此作为组合导航***量测方程中的测量量进行修正,构建以惯导位置、速度、姿态误差、惯性器件随机常值误差、左右里程仪刻度系数误差,杆臂为状态量的滤波***,并进行反馈校正,实现基于量测误差补偿的组合导航定位精度提高。
本发明的目的可以通过以下技术方案来实现:
一种车载INS/OD组合导航***的量测误差补偿方法,包括以下步骤:
步骤1:车辆静止时进行INS的初始化,并利用测量仪器和车身结构对安装角误差、杆臂误差和里程仪刻度系数进行测量标定;
步骤2:对里程计数据可用性进行判断:根据车辆行驶状态判断是否打滑、侧滑、跳跃,若判断为是,则不使用里程计反馈,若否,则进行组合导航并将左右里程计的输出车速数据输入到组合滤波器中;
步骤3:采集车辆行驶过程中的INS原始导航数据;
步骤4:将INS原始导航数据进行器件补偿、姿态解算和导航计算后将得到速度增量姿态、速度和位置均输入到组合滤波器中;
步骤5:当组合滤波器的数据输入完毕后,建立组合导航***的状态方程,对状态方程进行估计,每次滤波后,利用滤波估计出的结果对INS解算结果进行反馈校正。
优选地,所述的步骤2中的打滑判断条件为:
vOD≤ωR
式中,vOD为车辆载体系下的车速,ω为车轮角速度,R为车轮半径。
优选地,所述步骤2中的侧滑判断条件为:
Δy≥vODyΔT
式中,Δy为载体系下的侧向位移增量,vODy为载体系下侧向车速,ΔT为变化时间。
优选地,所述步骤2中的跳跃判断条件为:
Δaz≥Δath
式中,Δaz为垂向加速度实际变化值,Δath为垂向加速度合理变化值。
优选地,所述步骤4中的速度增量姿态、速度和位置采用二子样圆锥误差补偿算法计算,其对应的计算方程组为:
式中,Δθm1和Δθm2为陀螺两次等间隔采样对应的角增量,T为采样时间,为以惯性坐标系作为参考基准,载体系从tm-1时刻到tm时刻的旋转变化,为以惯性坐标系作为参考基准,地理系从tm时刻到tm-1时刻的旋转变化,下标i表示惯导***解算值,上下标b表示载体系,上下标n表示地理系,(m)表示tm时刻,(m-1)表示tm-1时刻,φ带上下标表示对应姿态,I表示单位矩阵,为常值。
优选地,所述步骤5包括以下分步骤:
步骤51:建立***方程;
步骤52:建立量测方程;
步骤53:建立kalman滤波***方程并使量测方程离散化;
步骤54:利用kalman滤波***方程进行反馈校正。
优选地,所述步骤51中的***方程,其描述公式为:
式中,X为状态向量,φ1包括:φE、φN和φU分别为东-北-天地理坐标系下的姿态误差,δvn包括:δvE、δvN和δvU分别为东-北-天地理坐标系下的速度误差,δp包括:δL、δλ和δh为经度、纬度和高度的位置误差,ε包括:εx、εy和εz分别为陀螺仪三个坐标轴的零偏,包括:和分别为加速度计的三个坐标轴的零偏,δk1和δk2分别为里程计1和里程计2的刻度系数误差,包括:和分别为里程计1的三个坐标轴的杆臂值,包括:和分别为里程计2的三个坐标轴的杆臂值;
式中,为地理系相对于惯性系的角速度,为地球系相对于惯性系的角速度误差,为地理系相对于地球系的角速度误差,为载体系到地理系的坐标变换矩阵,为载体系相对于惯性系的角速度误差,为载体系相对于地理系下的惯导***加速度计的输出比力,vn为载体在地理系下的速度,为地球系相对于惯性系的角速度,为地理系相对于地球系的角速度,δvn为载体在地理系下的速度误差,为载体系相对于地理系下的惯导***加速度计的输出比力误差,δgn为重力加速度误差,RM为卯酉圈半径,h为当地海拔高度,L为当地纬度,RN为子午圈半径,单独φ表示捷联惯导***中的数学平台误差角。
优选地,所述步骤52中的量测方程,其描述公式为:
式中,上标n表示为地理系,Z为量测方程,下标INS表示惯性***,下标OD表示里程计,和分别为里程计1和里程计2相对于载体系的坐标变换矩阵,vOD1和vOD2分别为车辆中里程计1和里程计2的载体系下的车速,表示载体系相对于地球系的角速度,和分别为里程计1和里程计2的杆臂值误差,k1和k2分别为里程计1和里程计2的刻度系数。
优选地,所述步骤5还包括:将kalman滤波后的陀螺和加速度零偏反馈至器件补偿处进行校正,将姿态反馈至姿态更新补偿处,将速度、位置误差反馈至INS解算的输出值上进行校正,即:由修正后的可解得航向角ψ、俯仰角θ和滚动角γ,一次滤波反馈后,误差状态归0。
优选地,所述由修正后的可解得航向角ψ、俯仰角θ和滚动角γ,其对应的描述公式为:
式中,(数字1,数字2)表示矩阵内具体对应的矩阵元素。
本发明的原理是:
在车辆行驶前,先进行惯性导航***的初始化,再利用测量仪器初步测量INS与里程计两个***的安装偏置角和杆臂误差,和里程计刻度系数并据此作为组合导航***量测方程中的测量量进行修正,构建以惯导位置、速度、姿态误差以及惯性器件随机常值误差,里程计刻度系数误差和里程计杆臂为状态量的滤波***,并进行反馈校正。本发明分为四个阶段,第一阶段是INS初始化阶段:此阶段采用利用外部航向信息辅助初始化导航姿态角和位置;第二阶段标定安装角误差、杆臂、里程计刻度系数初值;第三阶段是数据采集与处理阶段,里程计可用性判断和INS的器件误差补偿、姿态解算和导航计算。第四阶段为组合滤波以及反馈校正阶段,即将估计的位置误差、速度误差、姿态误差、惯性器件随机常值误差反馈至INS处进行反馈补偿,里程计刻度系数误差反馈到里程计处。
与现有技术相比,本发明具有以下优点:
(1)本发明中,考虑了组合导航***中对安装于车辆上的惯性导航***与里程计进行量测一致性参数标定与估计,以及由于车辆姿态变化导致的杆臂动态变化,对于由于车辆行驶工况变化引起的里程仪刻度系数误差拓展为自适应,和提供了判断根据车辆打滑、侧滑、跳跃,即实际里程计数据可用性与提供测量补偿的判断,提高了组合导航***的可用性和精度。
附图说明
图1为本发明中INS惯组中心与摄像头集合中心相对位置的杆臂简图;
图2为本发明中组合导航***结构框图。
具体实施方式
下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例是本发明的一部分实施例,而不是全部实施例。基于本发明中的实施例,本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例,都应属于本发明保护的范围。
实施例
本发明分为四个阶段,第一阶段是INS初始化阶段,第二阶段标定安装角误差、杆臂、里程计刻度系数初值;第三阶段是数据采集与处理阶段,里程计可用性判断和INS的器件误差补偿、姿态解算和导航计算。第四阶段为组合滤波以及反馈校正阶段。
本发明的具体实施步骤如图2所示:
(1)车辆静止进行INS初始化;
(2)利用测量仪器和车辆结构对安装角误差和杆臂误差进行测量标定,如图1所示,同时也对里程仪刻度系数进行标定得到里程计1和里程计2相对于载体系的坐标变换矩阵、里程计1和里程计2的杆臂值误差和里程计1和里程计2的刻度系数;
(3)里程计数据可用性判断:根据车辆行驶的车速及加速度状态的判断是否打滑、侧滑、跳跃;如果是,则不使用里程计反馈,若否,则进行组合导航,将左右里程计***输出数据车速,输入到组合滤波器中;
车辆行驶状态的判断条件:
1.打滑(纵滑):vOD≤ωR
2.侧滑(制动侧滑和前轮滑移):前束角与车轮外倾角匹配不当时候发生,此时默认为车辆匹配适当,无不匹配导致滑移,采用额外判断:Δy≥vODyΔT,其中Δy为载体坐标系下的侧向位移增量,vODy为载体坐标系下侧向车速;
3.跳跃:车辆发生不正常的垂向振动,通过垂向加速度变化来判断:Δaz≥Δath。
(4)车辆行驶过程采集数据,惯性测量数据:三轴陀螺仪数据三轴加速度计数据
(5)将步骤(4)中的INS原始导航数据,进行器件补偿、姿态解算和导航计算,将得到速度增量姿态、速度和位置等信息,均输入到组合滤波器中;
其中姿态计算选“东-北-天(E-N-U)”地理坐标系(g系)作为捷联惯导***的导航参考坐标系,重新记为n系,则以n系作为参考系的姿态微分方程为:
其中,矩阵表示以i系(惯性坐标系)作为参考基准,b系从tm-1时刻到tm时刻的旋转变化,可由陀螺角速度确定;表示以i系作为参考基准,n系从tm时刻到tm-1时刻的旋转变化,可由计算角速度确定,和分别表示tm-1和tm时刻的捷联姿态矩阵。若陀螺在时间段[tm-1,tm]内(T=tm-tm-1)进行了两次等间隔采样,角增量分别为Δθm1和Δθm2,采用二子样圆锥误差补偿算法,有:
取四阶截断,并取近似:
导航更新周期[tm-1,tm]内,可以认为由速度和位置引起的变化很小,即可视为常值,记为则有:
(6)当(3)(5)中数据输入滤波器后,建立组合导航***的状态方程,采用误差状态向量具体包括位置、速度、姿态、陀螺随机常值漂移ε、加速度计随机常值零偏左右里程仪刻度系数误差,杆臂的共23维误差状态量,对23维误差状态向量进行估计,用两个***结算的速度,位置与姿态差值并补偿量测误差后结果作为测量。每次滤波后,利用滤波估计出的位置误差速度误差失准角误差陀螺随机常值漂移加速度计随机常值零偏对INS解算结果进行反馈校正,里程仪刻度系数误差对里程计进行反馈校正。
1.滤波解算:
建立***方程
其中:X:误差状态向量;
F:***矩阵;
G:噪声分配矩阵;
w:零均值高斯白噪声向量;
Z:量测向量;
H:量测矩阵;
V:量测噪声状态向量;
相关上下标位置处的b表示载体系,n表示地理系,e表示地球系,i表示惯性系。
式中,X为状态向量,φ1包括:φE、φN和φU分别为东-北-天地理坐标系下的姿态误差,δvn包括:δvE、δvN和δvU分别为东-北-天地理坐标系下的速度误差,δp包括:δL、δλ和δh为经度、纬度和高度的位置误差,ε包括:εx、εy和εz分别为陀螺仪三个坐标轴的零偏,包括:和分别为加速度计的三个坐标轴的零偏,δk1和δk2分别为里程计1和里程计2的刻度系数误差,包括:和分别为里程计1的三个坐标轴的杆臂值,包括:和分别为里程计2的三个坐标轴的杆臂值;
式中,为地理系相对于惯性系的角速度,为地球系相对于惯性系的角速度误差,为地理系相对于地球系的角速度误差,为载体系到地理系的坐标变换矩阵,为载体系相对于惯性系的角速度误差,为载体系相对于地理系下的惯导***加速度计的输出比力,vn为载体在地理系下的速度,为地球系相对于惯性系的角速度,为地理系相对于地球系的角速度,δvn为载体在地理系下的速度误差,为载体系相对于地理系下的惯导***加速度计的输出比力误差,δgn为重力加速度误差,RM为卯酉圈半径,h为当地海拔高度,L为当地纬度,RN为子午圈半径,单独φ表示捷联惯导***中的数学平台误差角。
以下将方程(姿态-速度-位置)依次展开:
其中
其中:
为陀螺测量误差,m带不同a、x、y、z下标表示为陀螺测量中的两轴间的交叉耦合系数,s带a、x、z下标表示为陀螺测量中的比例因子误差。
其中:
为加速度计测量误差,m带不同g、x、y、z下标表示为加速度计测量中的交叉耦合系数,s带g、x、z下标表示为加速度计测量中比例因子误差。
根据WGS-84(World Geodetic System 1984)地球系给出的地球参数为:半长轴:Re=6378137m,扁率:f=1/298.257223563,
地心引力常数(含大气层):μ=3.986004418×1014m3/s2,
地球自转角速率:ωie=7.2921151467×10-5rad/s
ge和gp分别为赤道重力和极点重力,地球重力扁率为:
β1表示与赤道重力的比值:
β2表示重力随高度下降梯度:
设定局部坐标系下的地理信息保持不变,h≈0,
经纬信息:
整理有式1-5有:
F15=03×3
F34=03×3,F35=03×3,F41=F42=F43=F44=F45=F51=F52=F53=F54=F55=03×3
F16=F26=F36=F46=F56=03×8,F61=F62=F63=F64=F65=08×3,F66=08×8
2.建立量测方程
式中,上标n表示为地理系,Z为量测方程,下标INS表示惯性***,下标OD表示里程计,和分别为里程计1和里程计2相对于载体系的坐标变换矩阵,vOD1和vOD2分别为车辆中里程计1和里程计2的载体系下的车速,表示载体系相对于地球系的角速度,和分别为里程计1和里程计2的杆臂值误差,k1和k2分别为里程计1和里程计2的刻度系数。
整理有:
V=V
3.Kalman滤波***方程和量测方程离散化
做近似离散化等效:
Xk=Φk/k-1Xk-1+Γk-1Wk-1
其中设定离散化时间间隔Ts=tk-tk-1,则状态转移矩阵取一阶截断,有:
Wk-1为***噪声向量,Vk为量测噪声向量,两者都是零均值的高斯白噪声向量序列(服从正态分布),且它们之间互不相关,即满足:
在Kalman滤波状态空间模型中对于噪声要求的基本假设,一般要求Qk是半正定的且Rk是正定的,即Qk≥0且Rk>0。Kalman滤波全套算法,可划分为五个基本公式,如下:
(1)状态一步预测
(2)状态一步预测均方误差
(3)滤波增益
(4)状态估计
(5)状态估计均方误差
Pk=(I-KkHk)Pk/k-1
4.反馈校正
将kalman滤波后的陀螺和加速度零偏反馈至器件补偿处进行校正,将姿态反馈至姿态更新补偿处,将速度、位置误差反馈至INS解算的输出值上进行校正,反馈后,误差状态归0。
以上所述,仅为本发明的具体实施方式,但本发明的保护范围并不局限于此,任何熟悉本技术领域的技术人员在本发明揭露的技术范围内,可轻易想到各种等效的修改或替换,这些修改或替换都应涵盖在本发明的保护范围之内。因此,本发明的保护范围应以权利要求的保护范围为准。
Claims (10)
1.一种车载INS/OD组合导航***的量测误差补偿方法,其特征在于,包括以下步骤:
步骤1:车辆静止时进行INS的初始化,并利用测量仪器和车身结构对安装角误差、杆臂误差和里程仪刻度系数进行测量标定;
步骤2:对里程计数据可用性进行判断:根据车辆行驶状态判断是否打滑、侧滑、跳跃,若判断为是,则不使用里程计反馈,若否,则进行组合导航并将左右里程计的输出车速数据输入到组合滤波器中;
步骤3:采集车辆行驶过程中的INS原始导航数据;
步骤4:将INS原始导航数据进行器件补偿、姿态解算和导航计算后将得到速度增量姿态、速度和位置均输入到组合滤波器中;
步骤5:当组合滤波器的数据输入完毕后,建立组合导航***的状态方程,对状态方程进行估计,每次滤波后,利用滤波估计出的结果对INS解算结果进行反馈校正。
2.根据权利要求1所述的一种车载INS/OD组合导航***的量测误差补偿方法,其特征在于,所述的步骤2中的打滑判断条件为:
vOD≤ωR
式中,vOD为车辆载体系下的车速,ω为车轮角速度,R为车轮半径。
3.根据权利要求2所述的一种车载INS/OD组合导航***的量测误差补偿方法,其特征在于,所述步骤2中的侧滑判断条件为:
Δy≥vODyΔT
式中,Δy为载体系下的侧向位移增量,vODy为载体系下侧向车速,ΔT为变化时间。
4.根据权利要求3所述的一种车载INS/OD组合导航***的量测误差补偿方法,其特征在于,所述步骤2中的跳跃判断条件为:
Δaz≥Δath
式中,Δaz为垂向加速度实际变化值,Δath为垂向加速度合理变化值。
5.根据权利要求1所述的一种车载INS/OD组合导航***的量测误差补偿方法,其特征在于,所述步骤4中的速度增量姿态、速度和位置采用二子样圆锥误差补偿算法计算,其对应的计算方程组为:
式中,Δθm1和Δθm2为陀螺两次等间隔采样对应的角增量,T为采样时间,为以惯性坐标系作为参考基准,载体系从tm-1时刻到tm时刻的旋转变化,为以惯性坐标系作为参考基准,地理系从tm时刻到tm-1时刻的旋转变化,下标i表示惯导***解算值,上下标b表示载体系,上下标n表示地理系,(m)表示tm时刻,(m-1)表示tm-1时刻,φ带上下标表示对应姿态,I表示单位矩阵,为常值。
6.根据权利要求1所述的一种车载INS/OD组合导航***的量测误差补偿方法,其特征在于,所述步骤5包括以下分步骤:
步骤51:建立***方程;
步骤52:建立量测方程;
步骤53:建立kalman滤波***方程并使量测方程离散化;
步骤54:利用kalman滤波***方程进行反馈校正。
7.根据权利要求6所述的一种车载INS/OD组合导航***的量测误差补偿方法,其特征在于,所述步骤51中的***方程,其描述公式为:
式中,X为状态向量,φ1包括:φE、φN和φU分别为东-北-天地理坐标系下的姿态误差,δvn包括:δvE、δvN和δvU分别为东-北-天地理坐标系下的速度误差,δp包括:δL、δλ和δh为经度、纬度和高度的位置误差,ε包括:εx、εy和εz分别为陀螺仪三个坐标轴的零偏,包括:和分别为加速度计的三个坐标轴的零偏,δk1和δk2分别为里程计1和里程计2的刻度系数误差,包括:和分别为里程计1的三个坐标轴的杆臂值,包括:和分别为里程计2的三个坐标轴的杆臂值;
式中,为地理系相对于惯性系的角速度,为地球系相对于惯性系的角速度误差,为地理系相对于地球系的角速度误差,为载体系到地理系的坐标变换矩阵,为载体系相对于惯性系的角速度误差,为载体系相对于地理系下的惯导***加速度计的输出比力,vn为载体在地理系下的速度,为地球系相对于惯性系的角速度,为地理系相对于地球系的角速度,δvn为载体在地理系下的速度误差,为载体系相对于地理系下的惯导***加速度计的输出比力误差,δgn为重力加速度误差,RM为卯酉圈半径,h为当地海拔高度,L为当地纬度,RN为子午圈半径,单独φ表示捷联惯导***中的数学平台误差角。
8.根据权利要求7所述的一种车载INS/OD组合导航***的量测误差补偿方法,其特征在于,所述步骤52中的量测方程,其描述公式为:
式中,上标n表示为地理系,Z为量测方程,下标INS表示惯性***,下标OD表示里程计,和分别为里程计1和里程计2相对于载体系的坐标变换矩阵,vOD1和vOD2分别为车辆中里程计1和里程计2的载体系下的车速,表示载体系相对于地球系的角速度,和分别为里程计1和里程计2的杆臂值误差,k1和k2分别为里程计1和里程计2的刻度系数。
9.根据权利要求1所述的一种车载INS/OD组合导航***的量测误差补偿方法,其特征在于,所述步骤5还包括:将kalman滤波后的陀螺和加速度零偏反馈至器件补偿处进行校正,将姿态反馈至姿态更新补偿处,将速度、位置误差反馈至INS解算的输出值上进行校正,即:由修正后的可解得航向角ψ、俯仰角θ和滚动角γ,一次滤波反馈后,误差状态归0。
10.根据权利要求9所述的一种车载INS/OD组合导航***的量测误差补偿方法,其特征在于,所述由修正后的可解得航向角ψ、俯仰角θ和滚动角γ,其对应的描述公式为:
式中,(数字1,数字2)表示矩阵内具体对应的矩阵元素。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910288827.XA CN110221333B (zh) | 2019-04-11 | 2019-04-11 | 一种车载ins/od组合导航***的量测误差补偿方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910288827.XA CN110221333B (zh) | 2019-04-11 | 2019-04-11 | 一种车载ins/od组合导航***的量测误差补偿方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN110221333A true CN110221333A (zh) | 2019-09-10 |
CN110221333B CN110221333B (zh) | 2023-02-10 |
Family
ID=67822552
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910288827.XA Active CN110221333B (zh) | 2019-04-11 | 2019-04-11 | 一种车载ins/od组合导航***的量测误差补偿方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110221333B (zh) |
Cited By (19)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110672124A (zh) * | 2019-09-27 | 2020-01-10 | 北京耐威时代科技有限公司 | 离线杠杆臂估计方法 |
CN111174974A (zh) * | 2020-02-17 | 2020-05-19 | 燕山大学 | 一种车辆悬架升沉测量方法及*** |
CN111536969A (zh) * | 2020-04-16 | 2020-08-14 | 哈尔滨工程大学 | 一种基于初始姿态角自对准的小径管道机器人定位方法 |
CN111578925A (zh) * | 2019-12-25 | 2020-08-25 | 重庆自行者科技有限公司 | 一种基于imu和码盘融合的车辆定位方法 |
CN111811537A (zh) * | 2020-07-02 | 2020-10-23 | 重庆青年职业技术学院 | 一种捷联惯性导航的误差补偿方法及导航*** |
CN112130188A (zh) * | 2020-11-23 | 2020-12-25 | 蘑菇车联信息科技有限公司 | 车辆定位方法、设备及云服务器 |
CN112129297A (zh) * | 2020-09-25 | 2020-12-25 | 重庆大学 | 一种多传感器信息融合的自适应校正室内定位方法 |
CN112197789A (zh) * | 2020-08-14 | 2021-01-08 | 北京自动化控制设备研究所 | 一种基于quest的ins/dvl安装误差标定方法 |
CN112254736A (zh) * | 2020-09-15 | 2021-01-22 | 株洲菲斯罗克光电技术有限公司 | 一种惯导与里程计组合导航的时延误差的补偿方法及*** |
CN112461236A (zh) * | 2020-11-23 | 2021-03-09 | 中国人民解放***箭军工程大学 | 一种车载高精度容错组合导航方法及*** |
CN112731483A (zh) * | 2020-12-14 | 2021-04-30 | 北京航空航天大学 | 一种自动驾驶组合导航***中rtk异常值的判断方法 |
CN113029139A (zh) * | 2021-04-07 | 2021-06-25 | 中国电子科技集团公司第二十八研究所 | 基于运动检测的机场飞行区车辆差分北斗/sins组合导航方法 |
CN113110602A (zh) * | 2021-04-08 | 2021-07-13 | 贵州电网有限责任公司 | 一种神经网络的高鲁邦性无人机电力设备自动巡检*** |
CN113252048A (zh) * | 2021-05-12 | 2021-08-13 | 高新兴物联科技有限公司 | 一种导航定位方法、导航定位***及计算机可读存储介质 |
CN113447960A (zh) * | 2020-03-26 | 2021-09-28 | 千寻位置网络有限公司 | 基于gnss/mems组合车载导航***的误差标定方法及*** |
CN114322998A (zh) * | 2021-12-02 | 2022-04-12 | 河北汉光重工有限责任公司 | 一种基于杆臂估计的sins_od组合导航校正方法 |
CN114383610A (zh) * | 2021-12-24 | 2022-04-22 | 郑州煤矿机械集团股份有限公司 | 一种基于移动三维扫描技术的抖动检测分段滤波方法 |
CN114526734A (zh) * | 2022-03-01 | 2022-05-24 | 长沙金维信息技术有限公司 | 用于车载组合导航的安装角补偿方法 |
CN114675055A (zh) * | 2022-03-29 | 2022-06-28 | 北京航空航天大学 | 一种基于惯性***的探空仪风速测量误差补偿方法 |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US6167347A (en) * | 1998-11-04 | 2000-12-26 | Lin; Ching-Fang | Vehicle positioning method and system thereof |
CN103344259A (zh) * | 2013-07-11 | 2013-10-09 | 北京航空航天大学 | 一种基于杆臂估计的ins/gps组合导航***反馈校正方法 |
CN105824039A (zh) * | 2016-03-17 | 2016-08-03 | 孙红星 | 基于里程计的克服卫星失锁时的gnss/ins车载组合定位定向算法 |
CN107588769A (zh) * | 2017-10-17 | 2018-01-16 | 北京航天发射技术研究所 | 一种车载捷联惯导、里程计及高程计组合导航方法 |
CN108731670A (zh) * | 2018-05-18 | 2018-11-02 | 南京航空航天大学 | 基于量测模型优化的惯性/视觉里程计组合导航定位方法 |
-
2019
- 2019-04-11 CN CN201910288827.XA patent/CN110221333B/zh active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US6167347A (en) * | 1998-11-04 | 2000-12-26 | Lin; Ching-Fang | Vehicle positioning method and system thereof |
CN103344259A (zh) * | 2013-07-11 | 2013-10-09 | 北京航空航天大学 | 一种基于杆臂估计的ins/gps组合导航***反馈校正方法 |
CN105824039A (zh) * | 2016-03-17 | 2016-08-03 | 孙红星 | 基于里程计的克服卫星失锁时的gnss/ins车载组合定位定向算法 |
CN107588769A (zh) * | 2017-10-17 | 2018-01-16 | 北京航天发射技术研究所 | 一种车载捷联惯导、里程计及高程计组合导航方法 |
CN108731670A (zh) * | 2018-05-18 | 2018-11-02 | 南京航空航天大学 | 基于量测模型优化的惯性/视觉里程计组合导航定位方法 |
Non-Patent Citations (3)
Title |
---|
严恭敏等: "惯导/里程仪组合导航***算法研究", 《计算机测量与控制》 * |
翁浚等: "载车SINS/OD杆臂在线补偿算法", 《传感技术学报》 * |
钱闯: "弱GNSS信号区域的GNSS/INS/LiDAR高精度定位方法及其应用研究", 《中国优秀博硕士学位论文全文数据库(博士)信息科技辑》 * |
Cited By (29)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110672124A (zh) * | 2019-09-27 | 2020-01-10 | 北京耐威时代科技有限公司 | 离线杠杆臂估计方法 |
CN111578925A (zh) * | 2019-12-25 | 2020-08-25 | 重庆自行者科技有限公司 | 一种基于imu和码盘融合的车辆定位方法 |
CN111174974B (zh) * | 2020-02-17 | 2021-07-30 | 燕山大学 | 一种车辆悬架升沉测量方法及*** |
CN111174974A (zh) * | 2020-02-17 | 2020-05-19 | 燕山大学 | 一种车辆悬架升沉测量方法及*** |
CN113447960B (zh) * | 2020-03-26 | 2022-06-24 | 千寻位置网络有限公司 | 基于gnss/mems组合车载导航***的误差标定方法及*** |
CN113447960A (zh) * | 2020-03-26 | 2021-09-28 | 千寻位置网络有限公司 | 基于gnss/mems组合车载导航***的误差标定方法及*** |
CN111536969A (zh) * | 2020-04-16 | 2020-08-14 | 哈尔滨工程大学 | 一种基于初始姿态角自对准的小径管道机器人定位方法 |
CN111536969B (zh) * | 2020-04-16 | 2022-12-13 | 哈尔滨工程大学 | 一种基于初始姿态角自对准的小径管道机器人定位方法 |
CN111811537A (zh) * | 2020-07-02 | 2020-10-23 | 重庆青年职业技术学院 | 一种捷联惯性导航的误差补偿方法及导航*** |
CN111811537B (zh) * | 2020-07-02 | 2023-09-08 | 重庆青年职业技术学院 | 一种捷联惯性导航的误差补偿方法及导航*** |
CN112197789A (zh) * | 2020-08-14 | 2021-01-08 | 北京自动化控制设备研究所 | 一种基于quest的ins/dvl安装误差标定方法 |
CN112197789B (zh) * | 2020-08-14 | 2023-09-12 | 北京自动化控制设备研究所 | 一种基于quest的ins/dvl安装误差标定方法 |
CN112254736A (zh) * | 2020-09-15 | 2021-01-22 | 株洲菲斯罗克光电技术有限公司 | 一种惯导与里程计组合导航的时延误差的补偿方法及*** |
CN112129297B (zh) * | 2020-09-25 | 2024-04-30 | 重庆大学 | 一种多传感器信息融合的自适应校正室内定位方法 |
CN112129297A (zh) * | 2020-09-25 | 2020-12-25 | 重庆大学 | 一种多传感器信息融合的自适应校正室内定位方法 |
CN112461236A (zh) * | 2020-11-23 | 2021-03-09 | 中国人民解放***箭军工程大学 | 一种车载高精度容错组合导航方法及*** |
CN112130188A (zh) * | 2020-11-23 | 2020-12-25 | 蘑菇车联信息科技有限公司 | 车辆定位方法、设备及云服务器 |
CN112731483B (zh) * | 2020-12-14 | 2024-04-09 | 北京航空航天大学 | 一种自动驾驶组合导航***中rtk异常值的判断方法 |
CN112731483A (zh) * | 2020-12-14 | 2021-04-30 | 北京航空航天大学 | 一种自动驾驶组合导航***中rtk异常值的判断方法 |
CN113029139A (zh) * | 2021-04-07 | 2021-06-25 | 中国电子科技集团公司第二十八研究所 | 基于运动检测的机场飞行区车辆差分北斗/sins组合导航方法 |
CN113029139B (zh) * | 2021-04-07 | 2023-07-28 | 中国电子科技集团公司第二十八研究所 | 基于运动检测的机场飞行区车辆差分北斗/sins组合导航方法 |
CN113110602A (zh) * | 2021-04-08 | 2021-07-13 | 贵州电网有限责任公司 | 一种神经网络的高鲁邦性无人机电力设备自动巡检*** |
CN113252048A (zh) * | 2021-05-12 | 2021-08-13 | 高新兴物联科技有限公司 | 一种导航定位方法、导航定位***及计算机可读存储介质 |
CN113252048B (zh) * | 2021-05-12 | 2023-02-28 | 高新兴物联科技股份有限公司 | 一种导航定位方法、导航定位***及计算机可读存储介质 |
CN114322998A (zh) * | 2021-12-02 | 2022-04-12 | 河北汉光重工有限责任公司 | 一种基于杆臂估计的sins_od组合导航校正方法 |
CN114383610A (zh) * | 2021-12-24 | 2022-04-22 | 郑州煤矿机械集团股份有限公司 | 一种基于移动三维扫描技术的抖动检测分段滤波方法 |
CN114526734A (zh) * | 2022-03-01 | 2022-05-24 | 长沙金维信息技术有限公司 | 用于车载组合导航的安装角补偿方法 |
CN114675055A (zh) * | 2022-03-29 | 2022-06-28 | 北京航空航天大学 | 一种基于惯性***的探空仪风速测量误差补偿方法 |
CN114675055B (zh) * | 2022-03-29 | 2023-05-05 | 北京航空航天大学 | 一种基于惯性***的探空仪风速测量误差补偿方法 |
Also Published As
Publication number | Publication date |
---|---|
CN110221333B (zh) | 2023-02-10 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110221333A (zh) | 一种车载ins/od组合导航***的量测误差补偿方法 | |
CN110501024A (zh) | 一种车载ins/激光雷达组合导航***的量测误差补偿方法 | |
EP2259023B1 (en) | Inertial navigation system error correction | |
CN111156994B (zh) | 一种基于mems惯性组件的ins/dr&gnss松组合导航方法 | |
CN110221332A (zh) | 一种车载gnss/ins组合导航的动态杆臂误差估计和补偿方法 | |
CN107560612B (zh) | 用于确定交通工具的角位置的方法和装置 | |
CN103245359B (zh) | 一种惯性导航***中惯性传感器固定误差实时标定方法 | |
CN107084743A (zh) | 利用gnss/ins数据的六自由度惯性测量单元的偏移和失准补偿 | |
Anderson et al. | Estimation of slip angles using a model based estimator and GPS | |
CN109870173A (zh) | 一种基于校验点的海底管道惯性导航***的轨迹修正方法 | |
CN101290229A (zh) | 硅微航姿***惯性/地磁组合方法 | |
CN104697526A (zh) | 用于农业机械的捷联惯导***以及控制方法 | |
CN112432642B (zh) | 一种重力灯塔与惯性导航融合定位方法及*** | |
US20160010988A1 (en) | Dump truck | |
Gao et al. | Development of precise GPS/INS/wheel speed sensor/yaw rate sensor integrated vehicular positioning system | |
CN110346824A (zh) | 一种车辆导航方法、***、装置及可读存储介质 | |
CN114061619A (zh) | 一种基于在线标定的惯导***姿态补偿方法 | |
US20230182790A1 (en) | Method for calculating an instantaneous velocity vector of a rail vehicle and corresponding system | |
Wang et al. | Performance analysis of GNSS/MIMU tight fusion positioning model with complex scene feature constraints | |
Jensen et al. | Mass estimation of ground vehicles based on longitudinal dynamics using loosely coupled integrated navigation system and CAN-bus data with model parameter estimation | |
Klein et al. | LiDAR and INS fusion in periods of GPS outages for mobile laser scanning mapping systems | |
CN110068325A (zh) | 一种车载ins/视觉组合导航***的杆臂误差补偿方法 | |
Hwang et al. | Online misalignment estimation of strapdown navigation for land vehicle under dynamic condition | |
US10859379B2 (en) | Systems and methods with dead-reckoning | |
WO2020021867A1 (ja) | ジオイド測定方法、ジオイド測定装置、ジオイド推定装置、ジオイド計算用データ収集装置 |
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 |