CN113029139B - 基于运动检测的机场飞行区车辆差分北斗/sins组合导航方法 - Google Patents

基于运动检测的机场飞行区车辆差分北斗/sins组合导航方法 Download PDF

Info

Publication number
CN113029139B
CN113029139B CN202110372112.XA CN202110372112A CN113029139B CN 113029139 B CN113029139 B CN 113029139B CN 202110372112 A CN202110372112 A CN 202110372112A CN 113029139 B CN113029139 B CN 113029139B
Authority
CN
China
Prior art keywords
vehicle
time
sins
speed
state
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.)
Active
Application number
CN202110372112.XA
Other languages
English (en)
Other versions
CN113029139A (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.)
CETC 28 Research Institute
Original Assignee
CETC 28 Research Institute
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 CETC 28 Research Institute filed Critical CETC 28 Research Institute
Priority to CN202110372112.XA priority Critical patent/CN113029139B/zh
Publication of CN113029139A publication Critical patent/CN113029139A/zh
Application granted granted Critical
Publication of CN113029139B publication Critical patent/CN113029139B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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/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
    • 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

Abstract

本发明公开了基于运动检测的机场飞行区车辆差分北斗/SINS组合导航方法，当卫星信号有效时，利用GNSS输出实时车辆速度和位置校正SINS误差，一旦检测到卫星信号失效，本发明提出利用SINS输出车辆运动信息实现运动状态在线检测，包括：停车、直线行驶以及拐弯行驶三种状态，并切换组合导航系统至对应的辅助约束模式，分别对应零速修正、动态零速修正或仅时间更新的动态零速修正三种运动学约束模式，同时，为了提高车辆运动状态检测准确率，本发明利用Vondrak滤波器对SINS输出角速度和加速度进行平滑预处理并通过计算SINS输出角速度、加速度以及速度信息联合广义似然函数实现车辆运动状态检测，保证飞行区车辆定位结果鲁棒性和连续性。

Description

基于运动检测的机场飞行区车辆差分北斗/SINS组合导航 方法
技术领域
本发明涉及卫星导航/惯性导航融合领域，具体涉及一种基于运动检测的机场飞行区车辆差分北斗/SINS组合导航方法。
背景技术
近年来，我国航空业迅速发展，机场飞行区作业车辆数量随之急剧增加，建立飞行区车辆监控管理系统有利于实现车辆可视化、数字化以及精细化管理。实现车辆监控管理的前提是车载导航系统能够实时、准确地提供车辆的行驶态势信息。车载导航定位系统中组合使用捷联式惯性导航系统(SINS，Strapdown Inertial Navigation System)与全球卫星导航系统(GNSS，Global Navigation Satellite System)可以实时获得车辆行驶速度、位置以及姿态等信息。我国北斗导航系统已完成全球组网建设，正逐步推进其在民航领域中的应用，差分北斗与SINS融合后将为飞行区车辆提供高精度定位服务。卫星信号有效时，利用SINS输出车辆运动信息与GNSS进行数据融合，能够获得车辆行驶最优速度、位置等参数。一旦车载卫星信号受到遮挡，比如，飞行区车辆经过廊桥、屋檐或者隧道等区域，卫星信号会出现短时间不可用情形，此时GNSS无法提供车辆速度与位置信息，SINS由于受惯性测量单元(IMU，Inertial Measurement Unit)漂移、安装误差等因素影响，其位置误差会随着时间积累，无法保证车载导航系统定位结果的连续性。
为了提高车载组合导航系统在卫星信号受限环境下定位结果的连续性，许多学者开展了相关研究工作，主要集中在以下3个方面。一是增加辅助传感器，如里程计、地图数据或磁强计，以便在GNSS卫星信号中断期间提供外部观测量。然而，此方法需要外部传感器来校正SINS误差，这不仅限制了导航系统的应用范围，而且增加了成本和系统复杂性。另一种解决方案是基于非线性滤波技术，比如：使用Sigma点Kalman滤波器或粒子滤波器来校正GNSS中断期间的SINS误差。这些方法只有在建立了精确的非线性运动学模型后，才能有效地估计SINS累积误差。除此之外，许多学者研究通过增加速度、姿态或高度约束来抑制GNSS卫星信号失效时的SINS累积误差，比如：零速修正技术、动态零速修正技术等。零速修正技术只适用于车辆完全静止状态，而动态零速修正技术对于直线行驶的车辆具有较好地抑制效果。车载组合导航系统GNSS卫星不可用时选择一种适合的误差修正技术是目前车载组合导航研究领域难点之一，同时，如何根据SINS输出信息准确检测车辆行驶状态也是目前该领域的一个难点。
发明内容
发明目的：为了提高机场飞行区车载差分北斗/SINS组合导航系统在GNSS卫星信号衰弱、断续甚至中断时的导航精度，提供了一种基于运动检测的机场飞行区车辆差分北斗/SINS组合导航方法，有效提升了车载组合导航系统复杂路况环境下的定位精度。
技术方案：为了解决上述技术问题，本发明提供一种基于运动检测的机场飞行区车辆差分北斗/SINS组合导航方法，包括以下步骤：
S1：根据差分北斗导航系统接收机输出卫星信号判断当前导航信息是否有效，如果北斗卫星信号有效，组合导航滤波器根据接收机输出车辆速度和位置信息进行卡尔曼(Kalman)滤波器更新，并反馈校正SINS；
S2：如果差分北斗卫星信号无效，即受到屋檐、廊桥或隧道等遮挡时，利用低通数字滤波器对SINS输出陀螺、加速度计以及速度信息进行平滑滤波预处理，并根据预处理后的陀螺、加速度计以及速度信息检测车辆运动状态，所述车辆运动状态包括：停车、直线行驶以及拐弯行驶三种状态；
S3：根据当前车辆运动状态构造相应的运动约束条件，其中，车辆停车状态对应的运动约束条件为零速修正(ZUPT，Zero Velocity Update)模式，直线行驶对应的运动约束条件为动态零速修正(DZUPT，Dynamic Zero Velocity Update)模式，车辆拐弯行驶对应的运动约束条件为Kalman滤波器仅时间更新的动态零速修正模式；
S4：利用构造得到的运动约束条件进行Kalman滤波器更新，并与SINS进行信息融合，校正SINS累积误差。
进一步的，所述步骤S2中为减小发动机振动、人员上下车辆、路面颠簸等高频信号对车辆运动状态检测准确率影响，在检测车辆运动状态前，利用Vondrak滤波器对SINS输出角速度和加速度进行平滑预处理，Vondrak滤波器能够在信号数据变化规律及其拟合函数未知的情况下进行有效平滑，相比传统数字低通滤波器而言，Vondrak滤波器延时小，SINS输出角速度和加速度经过低通滤波后具有很好的实时性，采用的4阶Vondark滤波器频率响应函数表达式为：
式中，f表示平滑滤波信号频率，ε表示平滑因子，其取值越小，则信号越平滑，反之，信号平滑程度越弱。
进一步的，根据车辆运动学原理可知，当车辆匀速运动时，SINS输出角速度和加速度值比较小，此时利用传统角速度、加速度检测方法无法有效实现车辆静止状态检测，虚警率高，即会将车辆匀速运动状态错误地判定为静止状态，从而会引起较大的系统误差，为了减小虚警率，在进行车辆停车状态检测时，所述步骤S2中引入速度信息，所述速度信息为车辆实时速度值，通过计算SINS输出角速度、加速度以及速度信息联合广义似然函数实现车辆停车运动状态检测，表达式为：
式中，
式中，T(k)表示k时刻联合检测值，M表示时间窗宽度，ω(i)、a(i)和v(i)分别表示时间窗内i时刻SINS输出的载体坐标系角速度、加速度以及速度矢量值，g表示重力加速度值，||·||表示2-范数，ma表示时间窗内SINS输出的加速度值滑动平均值， 和/>分别表示SINS输出载体运动角速度噪声方差、加速度噪声方差以及速度噪声方差；
k时刻检测结果如下：
式中，γ表示根据实际行驶路况确定得到的停车检测阈值，Zero(k)表示k时刻检测结果，1表示车辆静止，0表示车辆行驶。
当判定为车辆行驶时，根据车辆行驶速度和陀螺输出的拐弯角速度可得到车辆向心加速度为：
CA(k)＝||v(k)||×ωz(k)
式中，CA(k)表示k时刻车辆向心加速度，ωz(k)表示k时刻车辆沿垂直方向拐弯角速度，v(k)表示k时刻车辆速度矢量值；
k时刻检测结果为：
式中，λa表示根据实际行驶路况确定得到的拐弯检测阈值，Turn(k)表示k时刻检测结果，1表示车辆拐弯行驶，0表示车辆直线行驶；
进一步的，所述步骤S1中Kalman滤波器更新包括时间更新和量测更新两部分，分别表示如下：时间更新：
量测更新：
Pk＝Pk/k-1-KkHkPk/k-1
式中，分别表示k-1和k时刻滤波器状态最优估计值，/>表示k时刻状态一步预测值，Φk,k-1表示系统状态一步转移矩阵，Pk-1、Pk分别表示k-1和k时刻状态估计协方差阵，Pk/k-1表示k时刻状态一步预测协方差阵，Qk-1表示k-1时刻系统激励噪声协方差阵，Kk表示k时刻滤波器增益矩阵，Hk表示量测矩阵，/>表示量测矩阵Hk的转置，Rk表示量测噪声协方差阵，Zk表示k时刻测量值；上标-1表示矩阵求逆运算。
进一步的，所述步骤S1中Kalman滤波器更新所建立的15维状态量为：
式中，X为状态量，δL为纬度误差、δλ为经度误差、δh为高度误差，δvE为东向速度误差、δvN为北向速度误差、δvU为天向速度误差，φE为东向失准角、φN为北向失准角、φU为天向失准角，εx为x向陀螺常值漂移、εy为y向陀螺常值漂移、εz为z向陀螺常值漂移，为x向加速度计常值偏置、/>为y向加速度计常值偏置、/>为z向加速度计常值偏置；由SINS误差方程可得组合导航状态方程为：
式中，表示15维状态量X的微分表达形式，由纬度误差微分/>经度误差微分高度误差微分/>东向速度误差微分/>北向速度误差微分/>天向速度误差微分东向失准角微分/>北向失准角微分/>天向失准角微分/>x向陀螺常值漂移微分y向陀螺常值漂移微分/>z向陀螺常值漂移微分/>x向加速度计常值偏置微分/>y向加速度计常值偏置微分/>以及z向加速度计常值偏置微分/>组成，A为组合导航系统状态转移矩阵，W为组合导航系统噪声向量。
对组合导航状态方程进行离散化后可得状态方程离散表达形式为：
Xk＝Φk,k-1Xk-1+Wk-1
式中，Xk表示k时刻系统状态，Xk-1表示k-1时刻系统状态，Wk-1表示k-1时刻系统噪声向量，Φk,k-1计算表达式为：
式中，I表示单位矩阵，其对角线元素均为1，其余元素为0，T表示滤波周期，N表示状态转移矩阵A在1个滤波周期内的采样值个数，Ai-1表示第i-1个采样值，1≤i≤N。
进一步的，所述步骤S1中Kalman滤波器更新的量测更新时，利用SINS输出速度、位置与差分北斗导航系统接收机输出速度、位置做差后作为Kalman滤波器量测向量Z，此时滤波量测方程可表示为：
Z＝HX+V
式中，Z表示卫星信号有效时Kalman滤波器量测向量，H表示量测矩阵，V表示组合导航系统量测噪声向量，H表达式为：
组合导航量测方程为离散形式，因此根据Z＝HX+V可直接得到量测方程离散形式为：
Zk＝HkXk+Vk
式中，Vk表示组合导航系统k时刻量测噪声向量，Zk表示k时刻量测向量，Hk表示k时刻量测矩阵，取值与H相同。
进一步的，所述步骤S3中零速修正模式指利用车辆停车状态下导航坐标系东-北-天3个方向速度为0的特性构造Kalman滤波器量测值：
式中，ZZUPT(k)表示k时刻零速修正模式滤波器量测值，δvE,k表示k时刻东向速度误差、δvN,k表示k时刻北向速度误差，δvU,k表示k时刻天向速度误差，vE,k表示k时刻东向速度、vN,k表示k时刻北向速度、vU,k表示k时刻天向速度；
进一步的，所述步骤S3中动态零速修正模式指利用车辆运动中载体坐标系侧向和天向速度为0以及俯仰角和横滚角前后时刻变化很小的特性构造Kalman滤波器量测值：
式中，ZDZUPT(k)表示k时刻动态零速修正模式滤波器量测值，φE,k表示k时刻东向失准角，φN,k表示k时刻北向失准角，表示k时刻俯仰角，/>表示k-1时刻俯仰角，/>表示k时刻横滚角，/>表示k-1时刻横滚角，δvx,k表示k时刻载体坐标系侧向速度误差、δvz,k表示k时刻载体坐标系天向速度误差，vx,k表示k时刻载体坐标系侧向速度、vz,k表示k时刻载体坐标系天向速度；
进一步的，所述步骤S3中仅时间更新的动态零速修正模式指北斗卫星信号不可用时且车辆处于拐弯行驶状态，此时量测信息不可用，组合导航Kalman滤波器不进行量测更新，以隔离车辆拐弯产生的向心加速度对Kalman滤波器精度影响。Kalman滤波器递推流程为：
Pk＝Pk/k-1
由此实现Kalman滤波器更新，并反馈校正SINS误差。
进一步的，所述步骤S4中利用步骤S3构造得到的运动约束条件作为量测方程，并结合步骤S2中的状态方程实现Kalman滤波器更新，根据滤波器更新结果校正SINS误差。
有益效果：本发明与现有技术相比，具备如下优点：
1、GNSS卫星信号失效时，根据车辆运动特性建立了零速修正、动态零速修正以及仅时间更新的动态零速修正三种速度约束模型，能够有效抑制SINS随时间积累性误差。
2、根据SINS输出运动信息有效地实现了车辆运动状态检测，进一步根据车辆行驶状态切换速度约束模型，提高了GNSS卫星失效时车载导航系统定位精度。
3、利用Vondrak滤波器对SINS输出角速度和加速度进行平滑预处理，能够有效减小车辆发动机振动、人员上下车辆、路面颠簸等高频信号对车辆运动状态检测准确率影响。
4、通过计算SINS输出角速度、加速度以及速度信息联合广义似然函数提高了车辆运动状态检测准确率。
附图说明
下面结合附图和具体实施方式对本发明做更进一步的具体说明，本发明的上述或其他方面的优点将会变得更加清楚。
图1为本发明方法的组合导航系统框图；
图2为动态零速修正模式示意图；
图3为Kalman滤波器递推流程图；
图4为本发明方法与传统速度约束方法经度误差对比曲线；
图5为本发明方法与传统速度约束方法纬度误差对比曲线；
图6为本发明方法与传统速度约束方法高度误差对比曲线。
具体实施方式
下面结合附图和具体实施例，进一步阐明本发明。
如图1所示，本发明提供一种基于运动检测的机场飞行区车辆差分北斗/SINS组合导航方法，包括以下步骤：
S1：根据差分北斗导航系统接收机输出卫星信号判断当前导航信息是否有效，当接收机搜索到的卫星个数小于4，则判定当前导航信息无效；当接收机搜索到的卫星个数大于等于4，则判定当前导航信息有效。如果差分北斗卫星信号有效，组合导航滤波器根据接收机输出车辆速度和位置信息进行Kalman滤波器更新，并反馈校正SINS，组合导航滤波器的状态方程取15维状态量：
式中，X为状态量，δL为纬度误差、δλ为经度误差、δh为高度误差，δvE为东向速度误差、δvN为北向速度误差、δvU为天向速度误差，φE为东向失准角、φN为北向失准角、φU为天向失准角，εx为x向陀螺常值漂移、εy为y向陀螺常值漂移、εz为z向陀螺常值漂移，为x向加速度计常值偏置、/>为y向加速度计常值偏置、/>为z向加速度计常值偏置，进一步由SINS误差方程可得组合导航滤波器的状态方程为：
式中，表示15维状态量X的微分表达形式，由纬度误差微分/>经度误差微分高度误差微分/>东向速度误差微分/>北向速度误差微分/>天向速度误差微分东向失准角微分/>北向失准角微分/>天向失准角微分/>x向陀螺常值漂移微分y向陀螺常值漂移微分/>z向陀螺常值漂移微分/>x向加速度计常值偏置微分/>y向加速度计常值偏置微分/>以及z向加速度计常值偏置微分/>组成，A为组合导航系统状态转移矩阵，W为组合导航系统噪声向量。
对组合导航状态方程进行离散化后可得状态方程离散表达形式为：
Xk＝Φk,k-1Xk-1+Wk-1
式中，Xk表示k时刻系统状态，Xk-1表示k-1时刻系统状态，Wk-1表示k-1时刻系统噪声向量，Φk,k-1计算表达式为：
式中，I表示单位矩阵，其对角线元素均为1，其余元素为0，T表示滤波周期，N表示状态转移矩阵A在1个滤波周期内的采样值个数，Ai-1表示第i-1个采样值，1≤i≤N。
差分北斗信号有效时利用SINS输出速度、位置与差分北斗导航系统接收机输出速度、位置做差后作为Kalman滤波器量测向量Z，此时滤波量测方程可表示为：
Z＝HX+V
式中，Z表示卫星信号有效时量测向量，H表示量测矩阵，V表示系统量测噪声向量，H表达式为：
组合导航量测方程为离散形式，因此根据Z＝HX+V可直接得到量测方程离散形式为：
Zk＝HkXk+Vk
式中，Vk表示组合导航系统k时刻量测噪声向量，Zk表示k时刻量测向量，Hk表示k时刻量测矩阵，取值与H相同。
进一步利用Kalman滤波器进行组合导航数据融合，Kalman滤波器包括时间更新和量测更新两部分，分别表示如下：
时间更新：
量测更新：
Pk＝Pk/k-1-KkHkPk/k-1
式中，分别表示k-1和k时刻滤波器状态最优估计值，/>表示k时刻状态一步预测值，Φk,k-1表示系统状态一步转移矩阵，Pk-1、Pk分别表示k-1和k时刻状态估计协方差阵，Pk/k-1表示k时刻状态一步预测协方差阵，Qk-1表示k-1时刻系统激励噪声协方差阵，Kk表示k时刻滤波器增益矩阵，Hk表示量测矩阵，/>表示量测矩阵Hk的转置，Rk表示量测噪声协方差阵，Zk表示k时刻测量值；上标-1表示矩阵求逆运算。
当前导航信息有效时，不再执行以下步骤。
S2：如果差分北斗卫星信号无效，即受到屋檐、廊桥或隧道等遮挡，利用低通数字滤波器对SINS输出陀螺、加速度计以及速度信息进行平滑滤波预处理，并根据预处理后的陀螺、加速度计以及速度信息实现车辆运动状态检测，所述车辆运动状态包括：停车、直线行驶以及拐弯行驶三种状态。为了减小发动机振动、人员上下车辆、路面颠簸等高频信号对车辆运动状态检测准确率影响，利用Vondrak滤波器对SINS输出角速度和加速度进行平滑预处理，4阶Vondrak滤波器频率响应函数表达式为：
式中，f表示平滑滤波信号频率，ε表示平滑因子，其取值越小，则信号越平滑，反之，信号平滑程度越弱。
进一步通过计算SINS输出角速度、加速度以及速度信息联合广义似然函数实现车辆停车运动状态检测，表达式为：
式中，
式中，T(k)表示k时刻联合检测值，M表示时间窗宽度，ω(i)、a(i)和v(i)分别表示时间窗内i时刻SINS输出的载体坐标系角速度、加速度以及速度矢量值，角速度由输出陀螺获得，加速度由加速度计获得，g表示重力加速度值，||·||表示2-范数，ma表示时间窗内SINS输出的加速度值滑动平均值，和/>分别表示SINS输出载体运动角速度噪声方差、加速度噪声方差以及速度噪声方差；
k时刻检测结果如下：
式中，γ表示根据实际行驶路况确定得到的停车检测阈值，对于消费级SINS而言，其取值范围为105～107；Zero(k)表示k时刻检测结果，1表示车辆静止，0表示车辆行驶。
当判定为车辆行驶时，根据车辆行驶速度和陀螺输出的拐弯角速度可得到车辆向心加速度为：
CA(k)＝||v(k)||×ωz(k)
式中，CA(k)表示k时刻车辆向心加速度，ωz(k)表示k时刻车辆沿垂直方向拐弯角速度，v(k)表示k时刻车辆速度矢量值；
k时刻检测结果为：
式中，λa表示根据实际行驶路况确定得到的拐弯检测阈值，对于消费级SINS而言，其取值范围为0.8m/s2～5m/s2，Turn(k)表示k时刻检测结果，1表示车辆拐弯行驶，0表示车辆直线行驶。
S3：根据当前车辆运动状态构造相应的运动约束条件，其中，车辆停车状态对应的运动约束条件为零速修正模式，直线行驶对应的运动约束条件为动态零速修正模式，车辆拐弯行驶对应的运动约束条件为Kalman滤波器仅时间更新的动态零速修正模式。
零速修正模式指利用车辆停车状态下导航坐标系东-北-天3个方向速度为0的特性构造Kalman滤波器量测值：
式中，ZZUPT(k)表示k时刻零速修正模式滤波器量测值，δvE,k表示k时刻东向速度误差、δvN,k表示k时刻北向速度误差，δvU,k表示k时刻天向速度误差，vE,k表示k时刻东向速度、vN,k表示k时刻北向速度、vU,k表示k时刻天向速度；
如图2所示，动态零速修正模式指利用车辆运动中载体坐标系侧向和天向速度为0以及俯仰角和横滚角前后时刻变化很小的特性构造Kalman滤波器量测值：
式中，ZDZUPT(k)表示k时刻动态零速修正模式滤波器量测值，φE,k表示k时刻东向失准角，φN,k表示k时刻北向失准角，表示k时刻俯仰角，/>表示k-1时刻俯仰角，/>表示k时刻横滚角，/>表示k-1时刻横滚角，δvx,k表示k时刻载体坐标系侧向速度误差、δvz,k表示k时刻载体坐标系天向速度误差，vx,k表示k时刻载体坐标系侧向速度、vz,k表示k时刻载体坐标系天向速度；
仅时间更新的动态零速修正模式指北斗卫星信号不可用时且车辆处于拐弯行驶状态，此时量测信息不可用，组合导航Kalman滤波器不进行量测更新，以隔离车辆拐弯产生的向心加速度对Kalman滤波器精度影响，Kalman滤波器递推流程如图3所示，公式为：
Pk＝Pk/k-1。
S4：利用步骤S3构造得到的运动约束条件作为量测方程，并结合步骤S2中的状态方程实现Kalman滤波器更新，根据滤波器更新结果校正SINS误差。
实施例
下面以某机场飞行区一组车载GNSS/SINS实测数据为研究对准，验证本发明提出的基于运动检测的机场飞行区车辆差分北斗/SINS组合导航方法的正确性。车载组合导航实验装置由差分北斗/MEMS SINS组合导航系统、北斗差分基准站、北斗接收天线等组成，其中，SINS中采用的MEMS(Micro-Electro-Mechanical System，微机电系统)陀螺仪零偏稳定性(1s平滑，1σ，室温)优于5°/h，加速度计零偏稳定性(1s平滑，1σ，室温)优于100μg。实验中利用一台高精度Septentrio’s PolaRx5型GNSS接收机保存的星历和观测数据进行事后RTK(Real-time kinematic，实时动态)差分处理获得车辆参考行驶路线，其数据更新频率为1Hz，位置精度优于0.05m，利用专用移动通信网络实现北斗差分站与行驶车辆、行驶车辆与监控席位之间双向数字通信，图4为GNSS卫星信号失效一段时间内本发明方法与传统速度约束方法经度误差对比曲线，图5为GNSS卫星信号失效一段时间内本发明方法与传统速度约束方法纬度误差对比曲线，图6为GNSS卫星信号失效一段时间内本发明方法与传统速度约束方法高度误差对比曲线。由图4、图5和图6可以看出，本发明提出的基于运动检测的速度约束辅助方法相比传统的动态零度修改方法具有更高的定位精度。
本发明提供了基于运动检测的机场飞行区车辆差分北斗/SINS组合导航方法，具体实现该技术方案的方法和途径很多，以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。本实施例中未明确的各组成部分均可用现有技术加以实现。

Claims (6)

1.基于运动检测的机场飞行区车辆差分北斗/SINS组合导航方法，其特征在于：包括以下步骤：
步骤S1：根据差分北斗导航系统接收机输出卫星信号判断当前导航信息是否有效，如果北斗卫星信号有效，组合导航滤波器根据接收机输出车辆速度和位置信息进行Kalman滤波器更新，并反馈校正SINS；
步骤S2：如果差分北斗卫星信号无效，利用低通数字滤波器对SINS输出陀螺、加速度计以及速度信息进行平滑滤波预处理，并根据预处理后的陀螺、加速度计以及速度信息检测车辆运动状态，所述车辆运动状态包括：停车、直线行驶以及拐弯行驶三种状态；
步骤S3：根据当前车辆运动状态构造相应的运动约束条件，其中，车辆停车状态对应的运动约束条件为零速修正模式，直线行驶对应的运动约束条件为动态零速修正模式，车辆拐弯行驶对应的运动约束条件为Kalman滤波器仅时间更新的动态零速修正模式；
步骤S4：利用构造得到的运动约束条件进行Kalman滤波器更新，并与SINS进行信息融合，校正SINS累积误差；
所述步骤S2中在检测车辆运动状态前，利用Vondrak滤波器对SINS输出角速度和加速度进行平滑预处理，4阶Vondark滤波器频率响应函数表达式为：
式中，f表示平滑滤波信号频率，ε表示平滑因子；
所述步骤S3中零速修正模式指利用车辆停车状态下导航坐标系东-北-天3个方向速度为0的特性构造Kalman滤波器量测值：
式中，ZZUPT(k)表示k时刻零速修正模式滤波器量测值，δvE,k表示k时刻东向速度误差、δvN,k表示k时刻北向速度误差，δvU,k表示k时刻天向速度误差，vE,k表示k时刻东向速度、vN,k表示k时刻北向速度、vU,k表示k时刻天向速度；
所述步骤S3中动态零速修正模式指利用车辆运动中载体坐标系侧向和天向速度为0以及俯仰角和横滚角前后时刻变化很小的特性构造Kalman滤波器量测值：
式中，ZDZUPT(k)表示k时刻动态零速修正模式滤波器量测值，φE,k表示k时刻东向失准角，φN,k表示k时刻北向失准角，表示k时刻俯仰角，/>表示k-1时刻俯仰角，/>表示k时刻横滚角，/>表示k-1时刻横滚角，δvx,k表示k时刻载体坐标系侧向速度误差、δvz,k表示k时刻载体坐标系天向速度误差，vx,k表示k时刻载体坐标系侧向速度、vz,k表示k时刻载体坐标系天向速度；
所述步骤S3中仅时间更新的动态零速修正模式指差分北斗卫星信号不可用时且车辆处于拐弯行驶状态，此时量测信息不可用，组合导航Kalman滤波器不进行量测更新，Kalman滤波器递推流程为：
Pk＝Pk/k-1。
2.根据权利要求1所述的基于运动检测的机场飞行区车辆差分北斗/SINS组合导航方法，其特征在于：所述步骤S2中通过计算SINS输出角速度、加速度以及速度信息联合广义似然函数实现车辆停车运动状态检测，表达式为：
式中，
式中，T(k)表示k时刻联合检测值，M表示时间窗宽度，ω(i)、a(i)和v(i)分别表示时间窗内i时刻SINS输出的载体坐标系角速度、加速度以及速度矢量值，g表示重力加速度值，||·||表示2-范数，ma表示时间窗内SINS输出的加速度值滑动平均值， 和/>分别表示SINS输出载体运动角速度噪声方差、加速度噪声方差以及速度噪声方差；
k时刻检测结果如下：
式中，γ表示根据实际行驶路况确定得到的停车检测阈值，Zero(k)表示k时刻检测结果，1表示车辆静止，0表示车辆行驶；
当判定为车辆行驶时，根据车辆行驶速度和陀螺输出的拐弯角速度可得到车辆向心加速度为：
CA(k)＝||v(k)||×ωz(k)
式中，CA(k)表示k时刻车辆向心加速度，ωz(k)表示k时刻车辆沿垂直方向拐弯角速度，v(k)表示k时刻车辆速度矢量值；
k时刻检测结果为：
式中，λa表示根据实际行驶路况确定得到的拐弯检测阈值，Turn(k)表示k时刻检测结果，1表示车辆拐弯行驶，0表示车辆直线行驶。
3.根据权利要求1所述的基于运动检测的机场飞行区车辆差分北斗/SINS组合导航方法，其特征在于：所述步骤S1中Kalman滤波器更新包括时间更新和量测更新两部分，分别表示如下：
时间更新：
量测更新：
Pk＝Pk/k-1-KkHkPk/k-1
式中，分别表示k-1和k时刻滤波器状态最优估计值，/>表示k时刻状态一步预测值，Φk,k-1表示系统状态一步转移矩阵，Pk-1、Pk分别表示k-1和k时刻状态估计协方差阵，Pk/k-1表示k时刻状态一步预测协方差阵，Qk-1表示k-1时刻系统激励噪声协方差阵，Kk表示k时刻滤波器增益矩阵，Hk表示量测矩阵，/>表示量测矩阵Hk的转置，Rk表示量测噪声协方差阵，Zk表示k时刻测量值；上标-1表示矩阵求逆运算。
4.根据权利要求3所述的基于运动检测的机场飞行区车辆差分北斗/SINS组合导航方法，其特征在于：在进行所述步骤S1中Kalman滤波器更新所建立的15维状态量为：
式中，X为状态量，δL为纬度误差、δλ为经度误差、δh为高度误差，δvE为东向速度误差、δvN为北向速度误差、δvU为天向速度误差，φE为东向失准角、φN为北向失准角、φU为天向失准角，εx为x向陀螺常值漂移、εy为y向陀螺常值漂移、εz为z向陀螺常值漂移，▽x为x向加速度计常值偏置、为y向加速度计常值偏置、/>为z向加速度计常值偏置；由SINS误差方程可得组合导航状态方程为：
式中，表示15维状态量X的微分表达形式，由纬度误差微分/>经度误差微分/>高度误差微分/>东向速度误差微分/>北向速度误差微分/>天向速度误差微分/>东向失准角微分/>北向失准角微分/>天向失准角微分/>x向陀螺常值漂移微分/>y向陀螺常值漂移微分/>z向陀螺常值漂移微分/>x向加速度计常值偏置微分/>y向加速度计常值偏置微分/>以及z向加速度计常值偏置微分/>组成，A为组合导航系统状态转移矩阵，W为组合导航系统噪声向量；
对组合导航状态方程进行离散化后可得状态方程离散表达形式为：
Xk＝Φk,k-1Xk-1+Wk-1
式中，Xk表示k时刻系统状态，Xk-1表示k-1时刻系统状态，Wk-1表示k-1时刻系统噪声向量，Φk,k-1计算表达式为：
式中，I表示单位矩阵，其对角线元素均为1，其余元素为0，T表示滤波周期，N表示状态转移矩阵A在1个滤波周期内的采样值个数，Ai-1表示第i-1个采样值，1≤i≤N。
5.根据权利要求4所述的基于运动检测的机场飞行区车辆差分北斗/SINS组合导航方法，其特征在于：在进行所述步骤S1中Kalman滤波器更新的量测更新时，利用SINS输出速度、位置与差分北斗导航系统接收机输出速度、位置做差后作为Kalman滤波器量测向量Z，此时滤波量测方程可表示为：
Z＝HX+V
式中，Z表示卫星信号有效时Kalman滤波器量测向量，H表示量测矩阵，V表示组合导航系统量测噪声向量，H表达式为：
组合导航量测方程为离散形式，因此根据Z＝HX+V可直接得到量测方程离散形式为：
Zk＝HkXk+Vk
式中，Vk表示组合导航系统k时刻量测噪声向量，Zk表示k时刻量测向量，Hk表示k时刻量测矩阵，取值与H相同。
6.根据权利要求5所述的基于运动检测的机场飞行区车辆差分北斗/SINS组合导航方法，其特征在于：所述步骤S4中利用步骤S3构造得到的运动约束条件作为量测方程，并结合步骤S2中的状态方程实现Kalman滤波器更新，根据滤波器更新结果校正SINS误差。
CN202110372112.XA 2021-04-07 2021-04-07 基于运动检测的机场飞行区车辆差分北斗/sins组合导航方法 Active CN113029139B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110372112.XA CN113029139B (zh) 2021-04-07 2021-04-07 基于运动检测的机场飞行区车辆差分北斗/sins组合导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110372112.XA CN113029139B (zh) 2021-04-07 2021-04-07 基于运动检测的机场飞行区车辆差分北斗/sins组合导航方法

Publications (2)

Publication Number Publication Date
CN113029139A CN113029139A (zh) 2021-06-25
CN113029139B true CN113029139B (zh) 2023-07-28

Family

ID=76453915

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110372112.XA Active CN113029139B (zh) 2021-04-07 2021-04-07 基于运动检测的机场飞行区车辆差分北斗/sins组合导航方法

Country Status (1)

Country Link
CN (1) CN113029139B (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113959430B (zh) * 2021-10-13 2023-12-22 广东汇天航空航天科技有限公司 飞行汽车的航姿确定方法、装置、车载终端及存储介质
CN113654573B (zh) * 2021-10-15 2022-03-08 成都云智北斗科技有限公司 地面机动载体组合导航系统粗对准方法及其设备
CN114608571A (zh) * 2022-02-25 2022-06-10 南京航空航天大学 一种适用于运动平台场景的行人惯性导航方法

Citations (19)

* Cited by examiner, † Cited by third party
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
US6240367B1 (en) * 1998-11-27 2001-05-29 Ching-Fang Lin Full fusion positioning method for vehicle
CN103134491A (zh) * 2011-11-30 2013-06-05 上海宇航系统工程研究所 Geo轨道转移飞行器sins/cns/gnss组合导航系统
CN103389506A (zh) * 2013-07-24 2013-11-13 哈尔滨工程大学 一种用于捷联惯性/北斗卫星组合导航系统的自适应滤波方法
CN105021192A (zh) * 2015-07-30 2015-11-04 华南理工大学 一种基于零速校正的组合导航系统的实现方法
CN106932804A (zh) * 2017-03-17 2017-07-07 南京航空航天大学 天文辅助的惯性/北斗紧组合导航系统及其导航方法
CN106950586A (zh) * 2017-01-22 2017-07-14 无锡卡尔曼导航技术有限公司 用于农机作业的gnss/ins/车辆组合导航方法
CN107621264A (zh) * 2017-09-30 2018-01-23 华南理工大学 车载微惯性/卫星组合导航系统的自适应卡尔曼滤波方法
CN109000640A (zh) * 2018-05-25 2018-12-14 东南大学 基于离散灰色神经网络模型的车辆gnss/ins组合导航方法
CN109541659A (zh) * 2018-10-24 2019-03-29 中国电子科技集团公司第二十八研究所 一种基于北斗的地基增强系统载波相位平滑伪距方法
CN110095800A (zh) * 2019-04-19 2019-08-06 南京理工大学 一种多源融合的自适应容错联邦滤波组合导航方法
CN110221333A (zh) * 2019-04-11 2019-09-10 同济大学 一种车载ins/od组合导航系统的量测误差补偿方法
CN110221332A (zh) * 2019-04-11 2019-09-10 同济大学 一种车载gnss/ins组合导航的动态杆臂误差估计和补偿方法
CN110285804A (zh) * 2019-06-26 2019-09-27 南京航空航天大学 基于相对运动模型约束的车辆协同导航方法
WO2019228437A1 (zh) * 2018-06-01 2019-12-05 浙江亚特电器有限公司 用于移动载具的组合导航方法
CN110780326A (zh) * 2019-09-26 2020-02-11 上海瀚所信息技术有限公司 一种车载组合导航系统和定位方法
CN111102978A (zh) * 2019-12-05 2020-05-05 深兰科技（上海）有限公司 一种车辆运动状态确定的方法、装置及电子设备
CN111381225A (zh) * 2020-04-09 2020-07-07 中国电子科技集团公司第二十八研究所 一种基于vdb系统的民航进场着陆高精度测距系统及测距方法
CN112378400A (zh) * 2020-10-30 2021-02-19 湖南航天机电设备与特种材料研究所 一种双天线gnss辅助的捷联惯导组合导航方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9791575B2 (en) * 2016-01-27 2017-10-17 Novatel Inc. GNSS and inertial navigation system utilizing relative yaw as an observable for an ins filter

Patent Citations (19)

* Cited by examiner, † Cited by third party
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
US6240367B1 (en) * 1998-11-27 2001-05-29 Ching-Fang Lin Full fusion positioning method for vehicle
CN103134491A (zh) * 2011-11-30 2013-06-05 上海宇航系统工程研究所 Geo轨道转移飞行器sins/cns/gnss组合导航系统
CN103389506A (zh) * 2013-07-24 2013-11-13 哈尔滨工程大学 一种用于捷联惯性/北斗卫星组合导航系统的自适应滤波方法
CN105021192A (zh) * 2015-07-30 2015-11-04 华南理工大学 一种基于零速校正的组合导航系统的实现方法
CN106950586A (zh) * 2017-01-22 2017-07-14 无锡卡尔曼导航技术有限公司 用于农机作业的gnss/ins/车辆组合导航方法
CN106932804A (zh) * 2017-03-17 2017-07-07 南京航空航天大学 天文辅助的惯性/北斗紧组合导航系统及其导航方法
CN107621264A (zh) * 2017-09-30 2018-01-23 华南理工大学 车载微惯性/卫星组合导航系统的自适应卡尔曼滤波方法
CN109000640A (zh) * 2018-05-25 2018-12-14 东南大学 基于离散灰色神经网络模型的车辆gnss/ins组合导航方法
WO2019228437A1 (zh) * 2018-06-01 2019-12-05 浙江亚特电器有限公司 用于移动载具的组合导航方法
CN109541659A (zh) * 2018-10-24 2019-03-29 中国电子科技集团公司第二十八研究所 一种基于北斗的地基增强系统载波相位平滑伪距方法
CN110221333A (zh) * 2019-04-11 2019-09-10 同济大学 一种车载ins/od组合导航系统的量测误差补偿方法
CN110221332A (zh) * 2019-04-11 2019-09-10 同济大学 一种车载gnss/ins组合导航的动态杆臂误差估计和补偿方法
CN110095800A (zh) * 2019-04-19 2019-08-06 南京理工大学 一种多源融合的自适应容错联邦滤波组合导航方法
CN110285804A (zh) * 2019-06-26 2019-09-27 南京航空航天大学 基于相对运动模型约束的车辆协同导航方法
CN110780326A (zh) * 2019-09-26 2020-02-11 上海瀚所信息技术有限公司 一种车载组合导航系统和定位方法
CN111102978A (zh) * 2019-12-05 2020-05-05 深兰科技（上海）有限公司 一种车辆运动状态确定的方法、装置及电子设备
CN111381225A (zh) * 2020-04-09 2020-07-07 中国电子科技集团公司第二十八研究所 一种基于vdb系统的民航进场着陆高精度测距系统及测距方法
CN112378400A (zh) * 2020-10-30 2021-02-19 湖南航天机电设备与特种材料研究所 一种双天线gnss辅助的捷联惯导组合导航方法

Non-Patent Citations (7)

* Cited by examiner, † Cited by third party
Title
An extended Kalman filter and back propagation neural network algorithm positioning method based on anti-lock brake sensor and global navigation satellite system information;Hu, J., Wu, Z., Qin, X., Geng, H., & Gao, Z;《Sensors》;第18卷(第9期);1-4 *
Differential Positioning Algorithm for GBAS Based on Extended Kalman Filtering;Hu, J., Sun, Q., & Shi, X;《2018 13th World Congress on Intelligent Control and Automation》;296-303 *
Real-time monitoring rapid ground subsidence using GNSS and Vondrak filter;Tao, T., Liu, J., Qu, X., & Gao, F;《Acta Geophysica》(第67期);133-140 *
SINS/GPS紧耦合组合导航;郑辛;《中国惯性技术学报》;第19卷(第1期);33-37 *
基于速度约束与模糊自适应滤波的车载组合导航;胡杰;严勇杰;王子卉;;《兵工学报》;第41卷(第02期);232-236 *
载波相位平滑伪距在GPS/SINS紧组合导航系统中的应用;胡杰;《导航定位与授时》;第5卷(第5期);33-35 *
面向民用航空的SINS/DGPS组合导航融合方法;胡杰;严勇杰;石潇竹;《电光与控制》;第27卷(第04期);39-42 *

Also Published As

Publication number Publication date
CN113029139A (zh) 2021-06-25

Similar Documents

Publication Publication Date Title
CN113029139B (zh) 基于运动检测的机场飞行区车辆差分北斗/sins组合导航方法
CN111156994B (zh) 一种基于mems惯性组件的ins/dr&gnss松组合导航方法
CN112505737B (zh) 一种gnss/ins组合导航方法
CN109000640B (zh) 基于离散灰色神经网络模型的车辆gnss/ins组合导航方法
CN110780326A (zh) 一种车载组合导航系统和定位方法
US20100030471A1 (en) Position detecting apparatus and method used in navigation system
CN108931244B (zh) 基于列车运动约束的惯导误差抑制方法及系统
TWI522258B (zh) Based on electronic map, global navigation satellite system and vehicle motion detection technology Lane identification method
CN111947681B (zh) 一种gnss与惯导组合导航位置输出的滤波校正方法
US20230182790A1 (en) Method for calculating an instantaneous velocity vector of a rail vehicle and corresponding system
Meiling et al. A loosely coupled MEMS-SINS/GNSS integrated system for land vehicle navigation in urban areas
Liu et al. Interacting multiple model UAV navigation algorithm based on a robust cubature Kalman filter
Gao et al. An integrated land vehicle navigation system based on context awareness
Anbu et al. Integration of inertial navigation system with global positioning system using extended kalman filter
CN114136310B (zh) 一种惯性导航系统误差自主抑制系统及方法
CN113008229B (zh) 一种基于低成本车载传感器的分布式自主组合导航方法
CN110567456B (zh) 基于抗差卡尔曼滤波的bds/ins组合列车定位方法
CN113074757A (zh) 车载惯导安装误差角的标定方法
Kubo et al. DGPS/INS/VVheelSensor Integrationfor High Accuracy Land-Vehicle Positioning
Wang et al. A comparison of loosely-coupled mode and tightly-coupled mode for INS/VMS
CN111007542B (zh) 一种车载星基增强多模gnss/mimu组合导航中mimu安装误差角的计算方法
Amin et al. A novel vehicle stationary detection utilizing map matching and IMU sensors
Fang et al. Integrating SINS sensors with odometer measurements for land vehicle navigation system
Noureldin et al. a Framework for Multi-Sensor Positioning and Mapping for Autonomous Vehicles
Shien et al. Integrated navigation accuracy improvement algorithm based on multi-sensor fusion

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