CN116381760B - Gnss rtk/ins紧耦合定位方法、装置及介质 - Google Patents
Gnss rtk/ins紧耦合定位方法、装置及介质 Download PDFInfo
- Publication number
- CN116381760B CN116381760B CN202310653249.1A CN202310653249A CN116381760B CN 116381760 B CN116381760 B CN 116381760B CN 202310653249 A CN202310653249 A CN 202310653249A CN 116381760 B CN116381760 B CN 116381760B
- Authority
- CN
- China
- Prior art keywords
- satellite
- window
- estimation
- double
- difference
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 57
- 230000008878 coupling Effects 0.000 title claims abstract description 39
- 238000010168 coupling process Methods 0.000 title claims abstract description 39
- 238000005859 coupling reaction Methods 0.000 title claims abstract description 39
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 58
- 238000005457 optimization Methods 0.000 claims abstract description 29
- 230000010354 integration Effects 0.000 claims abstract description 15
- 238000004364 calculation method Methods 0.000 claims abstract description 10
- 239000011159 matrix material Substances 0.000 claims description 88
- 238000009499 grossing Methods 0.000 claims description 44
- 230000003044 adaptive effect Effects 0.000 claims description 30
- 230000006870 function Effects 0.000 claims description 24
- 238000003860 storage Methods 0.000 claims description 13
- 238000005520 cutting process Methods 0.000 claims description 10
- 238000013507 mapping Methods 0.000 claims description 9
- 238000007667 floating Methods 0.000 claims description 6
- 238000013016 damping Methods 0.000 claims description 4
- 238000011002 quantification Methods 0.000 claims description 4
- 230000001133 acceleration Effects 0.000 claims description 3
- 230000005484 gravity Effects 0.000 claims description 3
- 230000004927 fusion Effects 0.000 abstract description 5
- 238000010586 diagram Methods 0.000 description 11
- 230000008859 change Effects 0.000 description 8
- 238000004590 computer program Methods 0.000 description 7
- 238000005259 measurement Methods 0.000 description 5
- 230000008569 process Effects 0.000 description 5
- 238000012545 processing Methods 0.000 description 5
- 238000005516 engineering process Methods 0.000 description 4
- 230000003287 optical effect Effects 0.000 description 3
- 241001391944 Commicarpus scandens Species 0.000 description 2
- 230000005540 biological transmission Effects 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000000903 blocking effect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 238000001914 filtration Methods 0.000 description 1
- 230000005251 gamma ray Effects 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000010606 normalization Methods 0.000 description 1
- 238000012805 post-processing Methods 0.000 description 1
- 230000009467 reduction Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000013077 scoring method Methods 0.000 description 1
- 230000035945 sensitivity Effects 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
Classifications
-
- 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
-
- 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/43—Determining position using carrier phase measurements, e.g. kinematic positioning; using long or short baseline interferometry
- G01S19/44—Carrier phase ambiguity resolution; Floating ambiguity; LAMBDA [Least-squares AMBiguity Decorrelation Adjustment] method
-
- 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
本发明涉及一种GNSS RTK/INS紧耦合定位方法、装置及介质，属于多传感器融合定位领域，其中方法包括：对IMU观测量进行预积分确定运动学导航方程；初始化当前时刻位置估计；估计未知整周模糊度；初始化估计窗长度；基于窗内双差卫星观测可用性评分算法选取参考卫星，并自适应调整估计窗窗长；在自适应滑动估计窗内，基于卫星与参考卫星之间的差分观测量，构建双差卫星观测约束，结合运动学导航方程确定的约束，构建成本函数，并通过LM迭代算法求解非线性最小二乘问题，对窗内的所有待估状态进行批量平滑优化，实现RTK/INS紧耦合定位。与现有技术相比，本发明具有定位精度高、降低了计算成本等优点。
Description
技术领域
本发明涉及多传感器融合定位领域，尤其是涉及一种基于自适应窗长平滑的GNSSRTK/INS紧耦合定位方法、装置及介质。
背景技术
高精度导航定位是自动驾驶领域的关键技术之一。现有的定位技术可利用多种传感器的测量信息来估计帧间运动，包括视觉传感器、激光雷达传感器、全球导航卫星系统（Global Navigation Satellite System, GNSS）以及惯性测量单元（InertialMeasurement Unit, IMU）等。智能车辆在长距离行驶时，通常需要GNSS来定期校准位置以消除累积误差。然而，在城市场景或者山地路段，由于卫星信号易被障碍物遮挡，GNSS定位精度将会下降，此时需要其他传感器提供辅助导航信息。惯性导航系统（InertialNavigation System, INS）对外部干扰敏感性低且输出频率高，与GNSS优势互补，因此常与GNSS做组合导航。当前，基于紧耦合结构的GNSS/INS导航系统在智能车辆的长距离定位中被广泛应用，紧耦合结构能够同时利用不同传感器提供的约束信息，有效提高系统整体的可用性。
GNSS中的实时动态（Real-time Kinematic, RTK）载波相位差分技术通过处理卫星信号的双差载波相位观测来实现高精度定位，在开阔的露天场景中，其精度可以达到厘米级甚至毫米级。然而，RTK技术引入了关于卫星整周模糊度的混合整数规划问题，一旦卫星可见性状态发生变化，就需要重新计算并固定其对应整周模糊度，甚至改变双差观测的参考卫星，因此将其融合到连续的紧耦合结构中存在一定挑战。过去基于滑动窗平滑方法的RTK/INS紧耦合算法研究往往假设卫星可见性保持不变，或者只在双差载波观测充足且模糊度有固定解的区间做后处理融合，这不能满足需要一定实时性的长距离高精度定位需求。而现有的将RTK作为因子嵌入到因子图优化框架并通过滑动窗平滑来进行融合估计的方法，也并未充分考虑连续捕获卫星的整周模糊度固定解的不变性约束，以及参考卫星可见性状态变化时参考卫星选取的变化等问题，因此在定位状态估计的非线性优化过程中，仍有提高计算效率、改善系统稳定性的空间。
发明内容
本发明的目的就是为了提供一种基于自适应窗长平滑的GNSS RTK/INS紧耦合定位方法、装置及介质，针对城市场景或山地路段卫星信号被遮挡、卫星可见性状态变化的情况，考虑连续捕获卫星的整周模糊度固定解的不变性约束，并通过一种基于卫星可见性的评分机制，选取出当前时刻的最佳参考卫星并有效地截断估计窗内不必要的数据，从而在长距离定位中保持定位精度并降低计算成本。
本发明的目的可以通过以下技术方案来实现：
一种基于自适应窗长平滑的GNSS RTK/INS紧耦合定位方法，包括以下步骤：
步骤1）利用GNSS定位结果对INS位姿进行初始化，对IMU观测量进行预积分确定运动学导航方程；
步骤2）基于GNSS RTK定位算法或运动学导航方程初始化当前时刻位置估计；
步骤3）对于被移动站和基站连续捕获的卫星，用上一时刻的整周模糊度固定解作为当前时刻整周模糊度的估计；对于首次或重新捕获的卫星，基于当前位置估计的初始值进行卫星未知整周模糊度解算；
步骤4）初始化估计窗长度；
步骤5）基于窗内双差卫星观测可用性评分算法选取参考卫星，并自适应的调整估计窗窗长；
步骤6）在步骤5）确定的自适应滑动估计窗内，基于各个卫星与参考卫星之间的差分观测量，构建双差卫星观测约束，结合步骤1）的运动学导航方程确定运动学约束，构建用以确定估计窗内待估状态最大后验估计的成本函数，并通过LM（Levenberg-Marquardt）迭代算法求解非线性最小二乘问题，对窗内的所有待估状态进行批量平滑优化，实现RTK/INS紧耦合定位。
所述步骤1）具体为：在初始时刻，基于GNSS定位结果对INS进行位姿初始化，根据IMU加速度计和陀螺仪的零偏估计值以及IMU采集到的原始比力和角速度观测量，假设一个GNSS周期内零偏估计值不变，预先将该周期内的所有相邻IMU测量时刻的惯性导航方程进行欧拉积分，确定与GNSS频率一致的运动学导航方程。
由时刻i的状态递推求得时刻j状态的运动学导航方程为：
，
其中，C，v，p分别为表示姿态（旋转矩阵）、速度、位置，g表示重力加速度，Δt ij 表示时刻i与时刻j的间隔时间，ΔC ij 、Δv ij 、Δp ij 分别为对时刻i到时刻j间的惯性导航方程进行积分得到的用旋转矩阵表示的等效姿态增量、等效速度增量、等效位置增量。
所述步骤2）具体为：判断是否有超过预设颗数的同时被移动站和基站连续捕获并已经得到整周模糊度固定解的卫星，若有，则表明当前有充足的有效高精度卫星测距信息，基于单历元的GNSS RTK定位算法确定当前时刻的初始位置估计及初始位置协方差矩阵；否则，表明没有充足的高精度卫星测距信息，通过INS辅助导航，基于运动学导航方程确定当前时刻的初始位置估计及初始位置协方差矩阵。
所述基于单历元的GNSS RTK定位算法估计当前时刻的初始位置，即求解当前时刻导航卫星系统提供的双差伪距、双差伪距率、双差载波相位约束的非线性最小二乘问题，并给出相应的状态协方差矩阵。
所述步骤3）中，对于首次或重新捕获的卫星，基于当前时刻的初始位置估计，计算整周模糊度浮点解，并提供求解整数解的搜索域，利用最小二乘模糊度去相关调整算法求解模糊度整数解；若单差模糊度整数解为固定解，则将对应卫星的载波相位观测纳入到用于构建双差卫星观测约束的观测量中，否则不纳入。
所述步骤4）具体为：利用初始位置协方差矩阵的迹作为当前位置状态估计不确定度，通过截断线性映射将其对应到设置的估计窗长区间内，作为估计窗的初始长度，其中，初始位置协方差矩阵的迹指协方差矩阵对角元素之和，截断线性映射使得超过某个预设阈值的不确定度都将映射到初始估计窗长度的上限。
所述步骤5）具体为：根据初始估计窗内卫星的可见性状况，选取出失锁次数最少的卫星作为备选参考卫星，对于每个备选参考卫星考察估计窗内的双差卫星观测的可用性，基于评分算法评估估计窗内可用双差观测的连续性，并考虑对估计窗进行截断，剔除与当前状态估计关联性小的历史数据，选择评分算法量化得分最高的备选参考卫星和估计窗长度的组合，分别作为计算卫星间差分的参考卫星和自适应估计窗的窗长。
所述步骤5）包括以下步骤：
步骤5.1）将初始估计窗内的卫星可见性状态用卫星可见性矩阵表示，矩阵的行表示估计窗内出现的不同卫星，列表示估计窗内的时刻，卫星可见性矩阵中卫星失锁时刻对应的元素设为0，其他位置元素表示该行对应卫星在估计窗内被连续捕获的序数，选择最后一列元素最小的卫星作为备选参考卫星；
步骤5.2）对于每个备选参考卫星，分别计算双差观测可用性矩阵，双差观测可用性矩阵的行表示估计窗内包含的双差卫星观测，列表示估计窗内的时刻，矩阵的0元素表示该时刻该卫星的双差观测不可用，非零元素表示双差观测可用，根据双差观测可用性矩阵中非零元素的连续性，计算每个估计窗的卫星可见性得分；
步骤5.3）对于每个备选参考卫星，判断是否在卫星失锁时刻截断估计窗，判断标准是原始估计窗的卫星可见性得分是否低于截断后估计窗的卫星可见性得分，若低于，则截断，即删除备选参考卫星失锁时刻以及之前的数据，减小估计窗的长度，反之不截断；
步骤5.4）遍历所有备选参考卫星，确定卫星可见性得分最高的参考卫星和估计窗长度的组合，分别作为计算卫星间差分的参考卫星和自适应估计窗的窗长，用于当前时刻的双差卫星观测计算。
所述步骤6）具体为：
记步骤5）确定的自适应滑动估计窗长度为K，参考卫星为c，除参考卫星外在估计窗内观测到M颗卫星，记批量待估状态为，单个时刻的状态变量包括位置、速度、姿态、加速度计零偏、陀螺仪零偏的三维信息，共15维信息，定义成本函数C为：，
其中，U表示一个GNSS周期内的所有IMU观测量，f表示基于IMU预积分的运动学导航函数，z表示双频双差的卫星观测量，即其中包含来自L1和L2载波信号的双差卫星观测量，γ k 表示相应的观测函数， Q k 表示运动学约束中的噪声协方差矩阵，R k 表示双差卫星观测中的噪声协方差矩阵，则x1:K的最大后验估计为：
，
通过LM算法对x1:K进行迭代优化，更新方程为：
，
其中，Δx表示状态迭代增量，μ表示LM算法的阻尼因子，I表示单位矩阵，表示用于平滑的雅克比矩阵，分块对角矩阵W表示权重矩阵，A表示平滑的残差向量：
，
通过LM算法的迭代对窗内的所有待估状态进行批量平滑优化，实现RTK/INS紧耦合定位。
一种基于自适应窗长平滑的GNSS RTK/INS紧耦合定位装置，包括：
IMU预积分模块，利用GNSS定位结果对INS位姿进行初始化，对IMU观测量进行预积分确定运动学导航方程；
初始化位置估计模块，基于GNSS RTK定位算法或运动学导航方程初始化当前时刻位置估计，并确定初始位置协方差矩阵；
未知整周模糊度解算模块，对于被移动站和基站连续捕获的卫星，未知整周模糊度解算模块用上一时刻的整周模糊度固定解作为当前时刻整周模糊度的估计；对于首次或重新捕获的卫星，未知整周模糊度解算模块基于当前位置估计的初始值进行卫星未知整周模糊度解算，并基于解算结果判断是否将对应卫星的载波相位观测纳入到用于构建双差卫星观测约束的观测量中；
初始化估计窗长度模块，用于基于初始位置协方差矩阵初始化估计窗长度；
参考卫星选取与自适应窗长确定模块，用于基于窗内双差卫星观测可用性评分算法选取参考卫星，并自适应的调整估计窗窗长；
待估状态平滑优化模块，在参考卫星选取与自适应窗长确定模块确定的自适应滑动估计窗内，基于各个卫星与参考卫星之间的差分观测量，构建双差卫星观测约束，结合IMU预积分模块确定的运动学导航方程确定运动学约束，构建用以确定估计窗内待估状态最大后验估计的成本函数，并通过LM（Levenberg-Marquardt）迭代算法求解非线性最小二乘问题，对窗内的所有待估状态进行批量平滑优化，实现RTK/INS紧耦合定位。
一种基于自适应窗长平滑的GNSS RTK/INS紧耦合定位装置，包括存储器、处理器，以及存储于所述存储器中的程序，所述处理器执行所述程序时实现如上述所述的方法。
一种存储介质，其上存储有程序，所述程序被执行时实现如上述所述的方法。
与现有技术相比，本发明具有以下有益效果：
（1）针对城市、山地场景中卫星信号容易中断、RTK双差载波相位观测的整周模糊度需频繁重新估计的问题，本发明在GNSS RTK/INS紧耦合框架中充分利用了连续捕获的卫星整周模糊度不变的性质，减少了在平滑算法中迭代优化卫星整周模糊度状态的计算成本。
（2）本发明提出了一种评估一段时间窗内卫星可见性状况的评分算法，从而在进行基于RTK的批量平滑优化时能够选取出合适的参考卫星。
（3）本发明设计了一种窗长可自适应变化的平滑器来进行状态估计的批量优化，自适应窗长的选取考虑了当前状态估计的不确定性，以及当前一段时间窗口内卫星可见性状况，相比于固定窗长的滑动窗平滑方法更加灵活，能够灵活调整估计窗长度并截去其中不必要的数据，减少了非线性优化迭代算法中需要处理的状态维数。
（4）本发明相比于基于扩展卡尔曼滤波器的RTK/INS紧耦合算法具有更高的定位准确性，而相比于窗长较长的固定窗长平滑器的RTK/INS紧耦合算法在保持相同定位精度的同时降低了计算成本，提高了系统的实时性，更适用于智能车辆的长距离高精度定位。
附图说明
图1为本发明一种基于自适应窗长平滑的GNSS RTK/INS紧耦合定位方法的流程图。
图2为本发明一种实施例中的卫星可见性变化场景的示意图。
图3为本发明一种实施例中两种信号遮挡场景下卫星可见性变化的示意图。
图4为本发明一种实施例中自适应窗长平滑器在每个GNSS历元选取的滑动窗长度示意图。
图5为本发明一种基于自适应窗长平滑的GNSS RTK/INS紧耦合定位装置的结构示意图，图中附图标记为，A-IMU预积分模块， B-初始化位置估计模块， C-未知整周模糊度解算模块， D-初始化估计窗长度模块，E- 参考卫星选取与自适应窗长确定模块，F-待估状态平滑优化模块。
具体实施方式
下面结合附图和具体实施例对本发明进行详细说明。本实施例以本发明技术方案为前提进行实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范围不限于下述的实施例。
本实施例提供一种基于自适应窗长平滑的GNSS RTK/INS紧耦合定位方法，如图1所示，包括以下步骤：
步骤1）利用GNSS定位结果对INS位姿进行初始化，对IMU观测量进行预积分确定运动学导航方程。
在初始时刻，基于GNSS定位结果对INS进行位姿初始化。在一个GNSS周期内，为避免每次更新线性化点时重新计算高频惯性测量值的积分，假设传感器零偏估计值不变，根据IMU加速度计和陀螺仪的零偏估计值以及IMU采集到的原始比力和角速度观测量，预先将该周期内的所有相邻IMU测量时刻的惯性导航方程进行欧拉积分，确定与GNSS频率一致的运动学导航方程。
例如，将时刻i与时刻j的惯性导航方程进行积分，得到姿态、速度、位置的等效运动增量分别为：用旋转矩阵表示的等效姿态增量ΔC ij 、等效速度增量Δv ij 、等效位置增量Δp ij 。利用预积分等价观测量由时刻i的状态，递推求得到时刻j状态的运动学导航方程为：
，
其中，C，v，p分别为表示姿态（旋转矩阵）、速度、位置，g表示重力加速度，Δt ij 表示时刻i与时刻j的间隔时间。
步骤2）基于GNSS RTK定位算法或运动学导航方程初始化当前时刻位置估计。
为了步骤3）中估计当前时刻的卫星整周模糊度，步骤4）中确定滑动估计窗的初始长度，以及为步骤6）状态平滑优化迭代算法提供初始值，需要首先为当前时刻的位置状态确定一个初值。
在本步骤中，首先判断当前是否有充足的有效高精度卫星测距信息。若当前可观测到的卫星中有超过四颗同时被基站与移动站连续捕获，并且已经得到整周模糊度的固定解，则表明当前有充足的有效高精度卫星测距信息，否则，表明当前时刻没有充足的高精度卫星测距信息。
当前有充足的有效高精度卫星测距信息时，用卫星观测来初始化当前位置状态。具体的，可以利用基于单历元的GNSS RTK定位算法确定当前时刻的初始位置估计及初始位置协方差矩阵，即求解当前时刻导航卫星系统提供的双差伪距、双差伪距率、双差载波相位约束的非线性最小二乘问题，并给出相应的状态协方差矩阵。
当前没有充足的有效高精度卫星测距信息时，通过INS辅助导航，基于IMU预积分确定的运动学导航方程确定当前时刻的初始位置估计及初始位置协方差矩阵。
步骤3）解算当前时刻得到的载波相位观测中的未知整周模糊度。
对于被移动站和基站连续捕获的卫星，其未知整周模糊度不变，在上一时刻已经解出其固定解，因此，用上一时刻的整周模糊度固定解作为当前时刻整周模糊度的估计。
对于首次或失锁后重新捕获的卫星，需要首次对其未知整周模糊度做估计，具体的：基于步骤2）得到当前时刻的初始位置估计和初始位置协方差矩阵，计算的浮点数估计值，以及相应的协方差矩阵，然后利用LAMBDA算法将问题建模成一个整数最小二乘估计，利用模糊度浮点数估计值完成整数解搜索，其中模糊度椭球搜索空间的形态和范围由模糊度浮点数估计值的协方差矩阵确定。最后判断搜索出的模糊度整数解是否是固定解，若单差模糊度整数解为固定解，则将对应卫星的带整周模糊度固定解的载波相位观测纳入到平滑优化中用于构建双差卫星观测约束的观测量中；若当前整周模糊度没有固定解，则舍去整周模糊度的浮点解，在平滑优化时不纳入这些卫星的载波相位观测提供的约束。
步骤4）初始化估计窗长度。
利用步骤2）得到的初始位置协方差矩阵的迹作为当前位置状态估计不确定度，通过截断线性映射将其对应到设置的估计窗长区间内，作为平滑估计窗的初始长度，其中，初始位置协方差矩阵的迹指协方差矩阵对角元素之和，截断线性映射意味着给不确定度设置一个阈值，超过这个阈值的不确定度都将映射到初始估计窗长度的上限。
本实施例中，记步骤2）得到的当前状态初始估计的协方差矩阵为 ，将协方差矩阵的迹/>，即三个对角元素之和作为当前位置估计不确定性的度量。/>越大，则说明当前状态估计初值的波动性越大，需对应更长时间窗口的数据来对其进行平滑优化。设置一个的/>阈值2δ，将做截断线性映射从[0, 2δ]映射到[a, b]，其中a，b分别为初始滑动窗长的上下限，当/>超过2δ时映射到b。记初始窗长为K0，则/>。在本发明进行的仿真实验中，初始窗长的上下限为a=10，b=30。
步骤5）基于窗内双差卫星观测可用性评分算法选取参考卫星，并自适应地调整估计窗窗长。
根据初始估计窗内卫星的可见性状况，选取出失锁次数最少的卫星作为备选参考卫星，对于每个备选参考卫星考察估计窗内的双差卫星观测的可用性，基于评分算法评估估计窗内可用双差观测的连续性，并考虑对估计窗进行截断，剔除与当前状态估计关联性小的历史数据，选择评分算法量化得分最高的备选参考卫星和估计窗长度的组合，分别作为计算卫星间差分的参考卫星和自适应估计窗的窗长。
具体的，步骤5）包括以下步骤：
步骤5.1）将初始估计窗内的卫星可见性状态用卫星可见性矩阵表示，矩阵的行表示估计窗内出现的不同卫星，列表示估计窗内的时刻，卫星可见性矩阵中卫星失锁时刻对应的元素设为0，其他位置元素表示该行对应卫星在估计窗内被连续捕获的序数，选择最后一列元素最小的卫星作为备选参考卫星。
若最后一列元素最小的卫星只有一个，则该卫星直接被作为参考卫星，并执行步骤5.2）-5.4），确定自适应滑动估计窗窗长。若最后一列元素最小的卫星有多个，则对每一个备选参考卫星执行步骤5.2）-5.4），从中选出参考卫星和估计窗长度的最优组合。
步骤5.2）对于每个备选参考卫星，分别计算双差观测可用性矩阵，双差观测可用性矩阵的行表示估计窗内包含的双差卫星观测，列表示估计窗内的时刻，矩阵的0元素表示该时刻该卫星的双差观测不可用，非零元素表示双差观测可用，根据双差观测可用性矩阵中非零元素的连续性，计算每个估计窗的卫星可见性得分。
步骤5.3）对于每个备选参考卫星，判断是否在卫星失锁时刻截断估计窗，判断标准是原始估计窗的卫星可见性得分是否低于截断后估计窗的卫星可见性得分，若低于，则截断，即删除备选参考卫星失锁时刻以及之前的数据，减小估计窗的长度，反之不截断。
步骤5.4）遍历所有备选参考卫星，确定卫星可见性得分最高的参考卫星和估计窗长度的组合，分别作为计算卫星间差分的参考卫星和自适应估计窗的窗长，用于当前时刻的双差卫星观测计算。
本实施例结合图2对各子步骤进行说明：
步骤5.1）图2展示了60秒内七颗GPS卫星的可见性变化情况，其中每颗GPS对应行的跳点表示该时刻卫星失锁。假设当前为第15秒时刻，根据步骤2）和步骤4）得到的初始估计窗长为15，即包含第1秒到第15秒内的所有卫星观测，将卫星可见性状态表示为表1所示的卫星可用性矩阵。矩阵中卫星失锁时刻对应的元素设为0，其他位置元素表示该行对应卫星在估计窗内第几次被连续捕获中。由于GPS10、GPS9、GPS6的最后一列元素为2最小，说明它们在选定的时间窗口内仅失锁一次，因此选择它们作为备选的参考星。
表1 卫星可见性矩阵示例
| 卫星 | 时刻1 | 时刻2 | 时刻3 | 时刻4 | 时刻5 | 时刻6 | 时刻7 | 时刻8 | 时刻9 | 时刻10 | 时刻11 | 时刻12 | 时刻13 | 时刻14 | 时刻15 |
| GPS30 | 1 | 1 | 1 | 1 | 1 | 0 | 2 | 2 | 2 | 2 | 2 | 2 | 2 | 0 | 3 |
| GPS15 | 1 | 1 | 1 | 1 | 1 | 1 | 0 | 2 | 0 | 3 | 3 | 3 | 0 | 4 | 4 |
| GPS11 | 1 | 1 | 1 | 1 | 0 | 2 | 2 | 2 | 2 | 0 | 3 | 3 | 3 | 3 | 3 |
| GPS10 | 1 | 1 | 1 | 1 | 1 | 1 | 1 | 0 | 2 | 2 | 2 | 2 | 2 | 2 | 2 |
| GPS9 | 1 | 1 | 1 | 1 | 1 | 1 | 1 | 1 | 1 | 1 | 1 | 1 | 0 | 2 | 2 |
| GPS6 | 1 | 1 | 1 | 1 | 1 | 1 | 1 | 0 | 2 | 2 | 2 | 2 | 2 | 2 | 2 |
| GPS5 | 1 | 1 | 0 | 2 | 2 | 2 | 2 | 2 | 2 | 0 | 3 | 3 | 3 | 3 | 3 |
步骤5.2）分别以GPS10、GPS9、GPS6作为参考卫星，计算双差卫星观测的可用性矩阵并给相应的平滑方案评分。以GPS10为例，计算双差观测可用性矩阵如表2所示，表中行表示先做基站与移动站之间的差分，再以GPS10为参考星做星间差分的双差载波相位观测，每行的数值为对应卫星与GPS10在表1中行的哈达玛积。表中0元素表示该时刻该双差观测不可用，非零元素表示双差观测可用。
表2 双差观测可用性矩阵
每个双差观测可用性矩阵对应一种参考卫星和估计窗长度组合的平滑方案，基于卫星可见性状况，本发明提出一种评估平滑方案有效性的评分算法，每个平滑方案的得分由当前时刻选择的参考卫星和估计窗长确定，其分值在(0,1]之间。具体评分方式是将每个卫星的双差观测持续可用历元数相加再做归一化。这里的持续可用历元数是指，若某双差观测在一段时间内连续n次可用，则这段时间内的持续可用历元数为n-1。如在1到5秒内和9到13秒内均连续5次可用，则计持续可用历元数为4+4=8。而归一化指的是将每个卫星的双差观测持续可用历元数之和除以窗长减1和卫星数减1的积。因此，表2对应的以GPS10为参考星，估计窗长为15的平滑方案的得分为：S10,0=(8+8+8+10+12+8)/(14×6)=0.643，同样地，以GPS9为参考星的方案的得分为S9,0=0.631；以GPS6为参考星的方案的得分为S6,0=0.643。
步骤5.3）对于每个备选参考星，依据评分方案判断是否对估计窗进行截断。以GPS10为例，如表2中所示，在第8秒参考卫星失锁，因此考虑截断第9秒之前的数据，保留9到15秒的估计窗。截断估计窗后，以GPS10为参考星的平滑方案的得分为S10,1=0.694>S10,0，则截断估计窗；同样地，以GPS9为参考星，保留14到15秒估计窗的平滑方案的得分为S9,1=0.833；以GPS6为参考星，保留9到15秒估计窗的平滑方案的得分为S6,1=0.583<S6,0，则不对估计窗进行截断。
步骤5.4）比较不同备选参考星下的平滑方案的得分，由于S9,1=0.833最大，当前时刻选取的平滑方案是：以GPS9为参考星，可变滑动窗长度选择2。
步骤6）在步骤5）确定的自适应滑动估计窗内，基于各个卫星与参考卫星之间的差分观测量，构建双差卫星观测（包括码伪距、伪距率、载波测距）的约束，结合步骤1）的运动学导航方程确定运动学约束，构建用以确定估计窗内待估状态最大后验估计的成本函数，并通过LM（Levenberg-Marquardt）迭代算法求解非线性最小二乘问题，对窗内的所有待估状态进行批量平滑优化，实现RTK/INS紧耦合定位。
记步骤5）确定的自适应滑动估计窗长度为K，参考卫星为c，除参考卫星外在估计窗内共观测到M颗卫星，记批量待估状态为，单个时刻的状态变量包括位置、速度、姿态、加速度计零偏、陀螺仪零偏的三维信息，共15维信息。
定义成本函数C为：
，
其中，U表示一个GNSS周期内的所有IMU观测量；f表示基于IMU预积分的运动学导航函数（也是15维）；z表示双频双差的卫星观测量，即其中包含来自L1和L2载波信号的双差卫星观测量；γ k 表示相应的观测函数 ；Q k 表示运动学约束中的噪声协方差矩阵；R k 表示双差卫星观测中的噪声协方差矩阵。则x1:K的最大后验估计为：，
通过LM算法对x1:K进行迭代优化，更新方程为：
，
其中，Δx表示状态迭代增量，μ表示LM算法的阻尼因子，I表示单位矩阵，表示用于平滑的雅克比矩阵，分块对角矩阵W表示权重矩阵，A表示平滑的残差向量：
，
通过LM算法的迭代对窗内的所有待估状态进行批量平滑优化，实现RTK/INS紧耦合定位。
综上，本实施例构建了一种GNSS RTK/INS紧耦合的融合定位框架，首先集成了RTK技术定位的高精度和INS定位的抗外部环境干扰的优点，使得两个子系统在整个定位框架中相互辅助；然后针对城市场景和山地道路中卫星信号容易中断，RTK双差载波相位观测的整周模糊度需要频繁重新估计的问题，本发明充分利用了连续捕获卫星的整周模糊度的不变形约束来减少混合整数规划的计算成本；在基于GNSS RTK和INS两个子系统提供的约束对待估状态进行批量平滑优化时，设计了基于估计窗内卫星可见性的评分方法来选取用于做卫星观测星间差分的参考卫星，并自适应地调整估计窗的长度，合理截断估计窗内对估计当前状态影响不大的观测，降低了非线性优化迭代算法中需要处理的状态维数，提高了计算效率。
为了验证本发明的有效性，将本发明中的方法同现有的RTK/INS定位算法进行对比，比较它们在加入了卫星信号随机遮挡条件的虚幻引擎4（UE4）仿真山地道路场景1800秒轨迹数据中的定位精度。对比方法包括：单历元RTK、基于扩展卡尔曼滤波（EKF）的RTK/INS紧耦合、基于固定窗长平滑的RTK/INS紧耦合（窗长固定为30帧/10帧），实验结果如表3所示，其中场景A为卫星可见性不变的理想条件，场景B和场景C均加入了卫星信号随机遮挡条件，且场景C卫星失锁比例更高，局部的卫星可见性变化情况和观测到的卫星数的如图3所示。
从表3中可以看出：在理想场景A中，各种方法的估计精度相近；在场景B和场景C中，单历元RTK和RTK/INS-EKF方法的最大定位误差和均方根定位误差将明显高于基于平滑的方法，而从均方根误差来看，本发明提出的方法与30帧固定窗长平滑方法具有相同定位精度，且固定率的水平相近。从整体上看，相比于EKF方法，本发明提出的方法在定位精度上提高约27%；而相比于具有30帧固定窗长的平滑算法，在计算成本上降低约30%。在实验过程中，本发明的自适应窗长的变化情况如图4所示，表明本发明在实施过程中，会自适应的对估计窗窗长进行调整，更加灵活。
表3实验结果对比
本实施例还提供一种基于自适应窗长平滑的GNSS RTK/INS紧耦合定位装置，如图5所示，包括：
（1）IMU预积分模块A，利用GNSS定位结果对INS位姿进行初始化，对IMU观测量进行预积分确定运动学导航方程。
（2）初始化位置估计模块B，基于GNSS RTK定位算法或运动学导航方程初始化当前时刻位置估计，并确定初始位置协方差矩阵。
（3）未知整周模糊度解算模块C，对于被移动站和基站连续捕获的卫星，未知整周模糊度解算模块用上一时刻的整周模糊度固定解作为当前时刻整周模糊度的估计；对于首次或重新捕获的卫星，未知整周模糊度解算模块基于位置估计的初始值进行卫星未知整周模糊度解算，并基于解算结果判断是否将对应卫星的载波相位观测纳入到用于构建双差卫星观测约束的观测量中。
（4）初始化估计窗长度模块D，用于基于初始位置协方差矩阵初始化估计窗长度。
（5）参考卫星选取与自适应窗长确定模块E，用于基于窗内双差卫星观测可用性评分算法选取参考卫星，并自适应的调整估计窗窗长。
（6）待估状态平滑优化模块F，在参考卫星选取与自适应窗长确定模块确定的自适应滑动估计窗内，基于各个卫星与参考卫星之间的差分观测量，构建双差卫星观测约束，结合IMU预积分模块确定的运动学导航方程确定运动学约束，构建用以确定估计窗内待估状态最大后验估计的成本函数，并通过LM迭代算法求解非线性最小二乘问题，对窗内的所有待估状态进行批量平滑优化，实现RTK/INS紧耦合定位。
各个模块执行时实现如上述所述的一种基于自适应窗长平滑的GNSS RTK/INS紧耦合定位方法。
为了描述的方便，描述以上装置时以功能分为各种模块分别描述。当然，在实施本发明时可以把各模块的功能在同一个或多个软件和/或硬件中实现。
本领域内的技术人员应明白，本发明的实施例可提供为方法、装置（系统）、或计算机程序产品。因此，本发明可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质（包括但不限于磁盘存储器、CD-ROM、光学存储器等）上实施的计算机程序产品的形式。
本发明是参照根据本发明实施例的方法、装置、和计算机程序产品的流程图和／或方框图来描述的。应理解可由计算机程序指令实现流程图和／或方框图中的每一流程和／或方框、以及流程图和／或方框图中的流程和／或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和／或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和／或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和／或方框图一个方框或多个方框中指定的功能的步骤。
上述功能如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备（可以是个人计算机，服务器，或者网络设备等）执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器（ROM，Read-Only Memory）、随机存取存储器（RAM，Random Access Memory）、磁碟或者光盘等各种可以存储程序代码的介质。
计算机可读介质包括永久性和非永久性、可移动和非可移动媒体可以由任何方法或技术来实现信息存储。信息可以是计算机可读指令、数据结构、程序的模块或其他数据。计算机的存储介质的例子包括，但不限于相变内存(PRAM)、静态随机存取存储器(SRAM)、动态随机存取存储器(DRAM)、其他类型的随机存取存储器(RAM)、只读存储器(ROM)、电可擦除可编程只读存储器(EEPROM)、快闪记忆体或其他内存技术、只读光盘只读存储器(CD-ROM)、数字多功能光盘(DVD)或其他光学存储、磁盒式磁带，磁带磁磁盘存储或其他磁性存储设备或任何其他非传输介质，可用于存储可以被计算设备访问的信息。按照本文中的界定，计算机可读介质不包括暂存电脑可读媒体(transitory media)，如调制的数据信号和载波。
以上详细描述了本发明的较佳具体实施例。应当理解，本领域的普通技术人员无需创造性劳动就可以根据本发明的构思做出诸多修改和变化。因此，凡本技术领域中技术人员依据本发明的构思在现有技术的基础上通过逻辑分析、推理、或者有限的实验可以得到的技术方案，皆应在权利要求书所确定的保护范围内。
Claims (11)
1.一种基于自适应窗长平滑的GNSS RTK/INS紧耦合定位方法，其特征在于，包括以下步骤：
步骤1）利用GNSS定位结果对INS位姿进行初始化，对IMU观测量进行预积分确定运动学导航方程；
步骤2）基于GNSS RTK定位算法或运动学导航方程初始化当前时刻位置估计；
步骤3）对于被移动站和基站连续捕获的卫星，用上一时刻的整周模糊度固定解作为当前时刻整周模糊度的估计；对于首次或重新捕获的卫星，基于当前位置估计的初始值进行卫星未知整周模糊度解算；
步骤4）初始化估计窗长度；
步骤5）基于窗内双差卫星观测可用性评分算法选取参考卫星，并自适应的调整估计窗窗长；
步骤6）在步骤5）确定的自适应滑动估计窗内，基于各个卫星与参考卫星之间的差分观测量，构建双差卫星观测约束，结合步骤1）的运动学导航方程确定运动学约束，构建用以确定估计窗内待估状态最大后验估计的成本函数，并通过LM迭代算法求解非线性最小二乘问题，对窗内的所有待估状态进行批量平滑优化，实现RTK/INS紧耦合定位；
所述步骤5）具体为：根据初始估计窗内卫星的可见性状况，选取出失锁次数最少的卫星作为备选参考卫星，对于每个备选参考卫星，考察估计窗内的双差卫星观测的可用性，基于评分算法评估估计窗内可用双差观测的连续性，并考虑对估计窗进行截断，剔除与当前状态估计关联性小的历史数据，选择评分算法量化得分最高的备选参考卫星和估计窗长度的组合，分别作为计算卫星间差分的参考卫星和自适应估计窗的窗长；
所述步骤6）具体为：
记步骤5）确定的自适应滑动估计窗长度为K，参考卫星为c，除参考卫星外在估计窗内观测到M颗卫星，记批量待估状态为，单个时刻的状态变量包括位置、速度、姿态、加速度计零偏、陀螺仪零偏的三维信息，共15维信息，定义成本函数C为：
，
其中，U表示一个GNSS周期内的所有IMU观测量，f表示基于IMU预积分的运动学导航函数，z表示双频双差的卫星观测量，即其中包含来自L1和L2载波信号的双差卫星观测量，γ k 表示相应的观测函数，Q k 表示运动学约束中的噪声协方差矩阵，R k 表示双差卫星观测中的噪声协方差矩阵，则x1:K的最大后验估计为：
，
通过LM算法对x1:K进行迭代优化，更新方程为：
，
其中，Δx表示状态迭代增量，μ表示LM算法的阻尼因子，I表示单位矩阵，表示用于平滑的雅克比矩阵，分块对角矩阵W表示权重矩阵，A表示平滑的残差向量：
，通过LM算法的迭代对窗内的所有待估状态进行批量平滑优化，实现RTK/INS紧耦合定位。
2.根据权利要求1所述的一种基于自适应窗长平滑的GNSS RTK/INS紧耦合定位方法，其特征在于，所述步骤1）具体为：在初始时刻，基于GNSS定位结果对INS进行位姿初始化，根据IMU加速度计和陀螺仪的零偏估计值以及IMU采集到的原始比力和角速度观测量，假设一个GNSS周期内零偏估计值不变，预先将该周期内的所有相邻IMU测量时刻的惯性导航方程进行欧拉积分，确定与GNSS频率一致的运动学导航方程。
3.根据权利要求2所述的一种基于自适应窗长平滑的GNSS RTK/INS紧耦合定位方法，其特征在于，由时刻i的状态递推求得时刻j状态的运动学导航方程为：
，
其中，C，v，p分别为表示姿态、速度、位置，g表示重力加速度，Δt ij 表示时刻i与时刻j的间隔时间，ΔC ij 、Δv ij 、Δp ij 分别为对时刻i到时刻j间的惯性导航方程进行积分得到的用旋转矩阵表示的等效姿态增量、等效速度增量、等效位置增量。
4.根据权利要求1所述的一种基于自适应窗长平滑的GNSS RTK/INS紧耦合定位方法，其特征在于，所述步骤2）具体为：判断是否有超过预设颗数的同时被移动站和基站连续捕获并已经得到整周模糊度固定解的卫星，若有，则表明当前有充足的有效高精度卫星测距信息，基于单历元的GNSS RTK定位算法确定当前时刻的初始位置估计及初始位置协方差矩阵；否则，表明没有充足的高精度卫星测距信息，通过INS辅助导航，基于运动学导航方程确定当前时刻的初始位置估计及初始位置协方差矩阵。
5.根据权利要求4所述的一种基于自适应窗长平滑的GNSS RTK/INS紧耦合定位方法，其特征在于，所述基于单历元的GNSS RTK定位算法估计当前时刻的初始位置，即求解当前时刻导航卫星系统提供的双差伪距、双差伪距率、双差载波相位约束的非线性最小二乘问题，并给出相应的状态协方差矩阵。
6.根据权利要求1所述的一种基于自适应窗长平滑的GNSS RTK/INS紧耦合定位方法，其特征在于，所述步骤3）中，对于首次或重新捕获的卫星，基于当前时刻的初始位置估计，计算整周模糊度浮点解，并提供求解整数解的搜索域，利用最小二乘模糊度去相关调整算法求解模糊度整数解；若单差模糊度整数解为固定解，则将对应卫星的载波相位观测纳入到用于构建双差卫星观测约束的观测量中，否则不纳入。
7.根据权利要求4所述的一种基于自适应窗长平滑的GNSS RTK/INS紧耦合定位方法，其特征在于，所述步骤4）具体为：利用初始位置协方差矩阵的迹作为当前位置状态估计不确定度，通过截断线性映射将其对应到设置的估计窗长区间内，作为估计窗的初始长度，其中，初始位置协方差矩阵的迹指协方差矩阵对角元素之和，截断线性映射使得超过某个预设阈值的不确定度都将映射到初始估计窗长度的上限。
8.根据权利要求1所述的一种基于自适应窗长平滑的GNSS RTK/INS紧耦合定位方法，其特征在于，所述步骤5）包括以下步骤：
步骤5.1）将初始估计窗内的卫星可见性状态用卫星可见性矩阵表示，矩阵的行表示估计窗内出现的不同卫星，列表示估计窗内的时刻，卫星可见性矩阵中卫星失锁时刻对应的元素设为0，其他位置元素表示该行对应卫星在估计窗内被连续捕获的序数，选择最后一列元素最小的卫星作为备选参考卫星；
步骤5.2）对于每个备选参考卫星，分别计算双差观测可用性矩阵，双差观测可用性矩阵的行表示估计窗内包含的双差卫星观测，列表示估计窗内的时刻，矩阵的0元素表示该时刻该卫星的双差观测不可用，非零元素表示双差观测可用，根据双差观测可用性矩阵中非零元素的连续性，计算每个估计窗的卫星可见性得分；
步骤5.3）对于每个备选参考卫星，判断是否在卫星失锁时刻截断估计窗，判断标准是原始估计窗的卫星可见性得分是否低于截断后估计窗的卫星可见性得分，若低于，则截断，即删除备选参考卫星失锁时刻以及之前的数据，减小估计窗的长度，反之不截断；
步骤5.4）遍历所有备选参考卫星，确定卫星可见性得分最高的参考卫星和估计窗长度的组合，分别作为计算卫星间差分的参考卫星和自适应估计窗的窗长，用于当前时刻的双差卫星观测计算。
9.一种基于自适应窗长平滑的GNSS RTK/INS紧耦合定位装置，其特征在于，包括：
IMU预积分模块，利用GNSS定位结果对INS位姿进行初始化，对IMU观测量进行预积分确定运动学导航方程；
初始化位置估计模块，基于GNSS RTK定位算法或运动学导航方程初始化当前时刻位置估计，并确定初始位置协方差矩阵；
未知整周模糊度解算模块，对于被移动站和基站连续捕获的卫星，未知整周模糊度解算模块用上一时刻的整周模糊度固定解作为当前时刻整周模糊度的估计；对于首次或重新捕获的卫星，未知整周模糊度解算模块基于当前位置估计的初始值进行卫星未知整周模糊度解算，并基于解算结果判断是否将对应卫星的载波相位观测纳入到用于构建双差卫星观测约束的观测量中；
初始化估计窗长度模块，用于基于初始位置协方差矩阵初始化估计窗长度；
参考卫星选取与自适应窗长确定模块，用于基于窗内双差卫星观测可用性评分算法选取参考卫星，并自适应地调整估计窗窗长，具体为：根据初始估计窗内卫星的可见性状况，选取出失锁次数最少的卫星作为备选参考卫星，对于每个备选参考卫星，考察估计窗内的双差卫星观测的可用性，基于评分算法评估估计窗内可用双差观测的连续性，并考虑对估计窗进行截断，剔除与当前状态估计关联性小的历史数据，选择评分算法量化得分最高的备选参考卫星和估计窗长度的组合，分别作为计算卫星间差分的参考卫星和自适应估计窗的窗长；
待估状态平滑优化模块，在参考卫星选取与自适应窗长确定模块确定的自适应滑动估计窗内，基于各个卫星与参考卫星之间的差分观测量，构建双差卫星观测约束，结合IMU预积分模块确定的运动学导航方程确定运动学约束，构建用以确定估计窗内待估状态最大后验估计的成本函数，并通过LM迭代算法求解非线性最小二乘问题，对窗内的所有待估状态进行批量平滑优化，实现RTK/INS紧耦合定位，具体为：记参考卫星选取与自适应窗长确定模块确定的自适应滑动估计窗长度为K，参考卫星为c，除参考卫星外在估计窗内观测到M颗卫星，记批量待估状态为，单个时刻的状态变量包括位置、速度、姿态、加速度计零偏、陀螺仪零偏的三维信息，共15维信息，定义成本函数C为：
，
其中，U表示一个GNSS周期内的所有IMU观测量，f表示基于IMU预积分的运动学导航函数，z表示双频双差的卫星观测量，即其中包含来自L1和L2载波信号的双差卫星观测量，γ k 表示相应的观测函数，Q k 表示运动学约束中的噪声协方差矩阵，R k 表示双差卫星观测中的噪声协方差矩阵，则x1:K的最大后验估计为：
，
通过LM算法对x1:K进行迭代优化，更新方程为：
，
其中，Δx表示状态迭代增量，μ表示LM算法的阻尼因子，I表示单位矩阵，表示用于平滑的雅克比矩阵，分块对角矩阵W表示权重矩阵，A表示平滑的残差向量：
，通过LM算法的迭代对窗内的所有待估状态进行批量平滑优化，实现RTK/INS紧耦合定位。
10.一种基于自适应窗长平滑的GNSS RTK/INS紧耦合定位装置，包括存储器、处理器，以及存储于所述存储器中的程序，其特征在于，所述处理器执行所述程序时实现如权利要求1-8中任一所述的方法。
11.一种存储介质，其上存储有程序，其特征在于，所述程序被执行时实现如权利要求1-8中任一所述的方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310653249.1A CN116381760B (zh) | 2023-06-05 | 2023-06-05 | Gnss rtk/ins紧耦合定位方法、装置及介质 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310653249.1A CN116381760B (zh) | 2023-06-05 | 2023-06-05 | Gnss rtk/ins紧耦合定位方法、装置及介质 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116381760A CN116381760A (zh) | 2023-07-04 |
| CN116381760B true CN116381760B (zh) | 2023-08-15 |
Family
ID=86969784
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310653249.1A Active CN116381760B (zh) | 2023-06-05 | 2023-06-05 | Gnss rtk/ins紧耦合定位方法、装置及介质 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116381760B (zh) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116819585B (zh) * | 2023-08-31 | 2023-12-29 | 长沙金维信息技术有限公司 | 基于非线性优化的gnss单点定位方法及导航方法 |
Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2005071431A1 (en) * | 2004-01-23 | 2005-08-04 | Novatel Inc. | Inertial gps navigation system with modified kalman filter |
| CN103675844A (zh) * | 2013-11-18 | 2014-03-26 | 航天恒星科技有限公司 | 一种gnss/ins组合导航同步模拟系统 |
| CN111578935A (zh) * | 2020-05-08 | 2020-08-25 | 北京航空航天大学 | 一种利用惯导位置增量辅助gnss模糊度固定的方法 |
| CN113267796A (zh) * | 2021-05-13 | 2021-08-17 | 中国人民解放军92859部队 | 一种双天线gnss、rtk定位及测向方法 |
| CN113960648A (zh) * | 2021-09-27 | 2022-01-21 | 北京三快在线科技有限公司 | 定位方法、装置、电子设备及计算机可读存储介质 |
| CN114646993A (zh) * | 2022-03-16 | 2022-06-21 | 同济大学 | 一种基于gnss、视觉以及imu进行精确定位的数据融合算法 |
| CN115435779A (zh) * | 2022-08-17 | 2022-12-06 | 南京航空航天大学 | 一种基于gnss/imu/光流信息融合的智能体位姿估计方法 |
| CN115451955A (zh) * | 2022-09-29 | 2022-12-09 | 北京交通大学 | 基于分布鲁棒滤波的ins/gps紧耦合导航方法及系统 |
| CN115683094A (zh) * | 2022-03-17 | 2023-02-03 | 山东建筑大学 | 一种复杂环境下车载双天线紧耦合定位方法及系统 |
| WO2023082050A1 (zh) * | 2021-11-09 | 2023-05-19 | 浙江大学 | 一种基于双层滤波框架的高精度里程估计方法 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US10132933B2 (en) * | 2016-02-02 | 2018-11-20 | Qualcomm Incorporated | Alignment of visual inertial odometry and satellite positioning system reference frames |
-
2023
- 2023-06-05 CN CN202310653249.1A patent/CN116381760B/zh active Active
Patent Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2005071431A1 (en) * | 2004-01-23 | 2005-08-04 | Novatel Inc. | Inertial gps navigation system with modified kalman filter |
| CN103675844A (zh) * | 2013-11-18 | 2014-03-26 | 航天恒星科技有限公司 | 一种gnss/ins组合导航同步模拟系统 |
| CN111578935A (zh) * | 2020-05-08 | 2020-08-25 | 北京航空航天大学 | 一种利用惯导位置增量辅助gnss模糊度固定的方法 |
| CN113267796A (zh) * | 2021-05-13 | 2021-08-17 | 中国人民解放军92859部队 | 一种双天线gnss、rtk定位及测向方法 |
| CN113960648A (zh) * | 2021-09-27 | 2022-01-21 | 北京三快在线科技有限公司 | 定位方法、装置、电子设备及计算机可读存储介质 |
| WO2023082050A1 (zh) * | 2021-11-09 | 2023-05-19 | 浙江大学 | 一种基于双层滤波框架的高精度里程估计方法 |
| CN114646993A (zh) * | 2022-03-16 | 2022-06-21 | 同济大学 | 一种基于gnss、视觉以及imu进行精确定位的数据融合算法 |
| CN115683094A (zh) * | 2022-03-17 | 2023-02-03 | 山东建筑大学 | 一种复杂环境下车载双天线紧耦合定位方法及系统 |
| CN115435779A (zh) * | 2022-08-17 | 2022-12-06 | 南京航空航天大学 | 一种基于gnss/imu/光流信息融合的智能体位姿估计方法 |
| CN115451955A (zh) * | 2022-09-29 | 2022-12-09 | 北京交通大学 | 基于分布鲁棒滤波的ins/gps紧耦合导航方法及系统 |
Non-Patent Citations (1)
| Title |
|---|
| Improving Vehicle Positioning Performance in Urban Environment with Tight Integration of Multi-GNSS PPP-RTK/INS;Luguang Lai et al.;《remote sensing》;第14卷(第21期);1-19 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116381760A (zh) | 2023-07-04 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Sun et al. | A new IMU-aided multiple GNSS fault detection and exclusion algorithm for integrated navigation in urban environments | |
| US10018729B2 (en) | Selected aspects of advanced receiver autonomous integrity monitoring application to kalman filter based navigation filter | |
| CN111780755B (zh) | 一种基于因子图和可观测度分析的多源融合导航方法 | |
| CN110140065B (zh) | Gnss接收机保护等级 | |
| CN107341819B (zh) | 目标跟踪方法及存储介质 | |
| Zhao et al. | High-precision vehicle navigation in urban environments using an MEM's IMU and single-frequency GPS receiver | |
| US8818722B2 (en) | Rapid lidar image correlation for ground navigation | |
| WO2020048623A1 (en) | Estimation of a pose of a robot | |
| Wen et al. | 3D LiDAR aided GNSS NLOS mitigation in urban canyons | |
| CN110006427B (zh) | 一种低动态高振动环境下的bds/ins紧组合导航方法 | |
| CN116381760B (zh) | Gnss rtk/ins紧耦合定位方法、装置及介质 | |
| CN114562992A (zh) | 一种基于因子图及场景约束的多径环境组合导航方法 | |
| CN113465628A (zh) | 惯性测量单元数据补偿方法及系统 | |
| Wen | 3D LiDAR aided GNSS and its tightly coupled integration with INS via factor graph optimization | |
| Iqbal et al. | Experimental results on an integrated GPS and multisensor system for land vehicle positioning | |
| CN110208843B (zh) | 一种基于增广伪距信息辅助的容错导航方法 | |
| CN113375664B (zh) | 基于点云地图动态加载的自主移动装置定位方法 | |
| CN115728796A (zh) | 定位方法及其系统 | |
| EP4143507A1 (en) | Navigation apparatus and method in which measurement quantization errors are modeled as states | |
| CN111142133B (zh) | 基于多个连续运行参考站的后处理定位方法和系统 | |
| EP2113785B1 (en) | Estimation of probability of lambda failure through employment of lookup table | |
| Atia et al. | A novel systems integration approach for multi-sensor integrated navigation systems | |
| Li et al. | An ambiguity-free smoothing algorithm for multipath mitigation in INS/RTK tightly-coupled integration | |
| CN111882494B (zh) | 位姿图处理方法、装置、计算机设备和存储介质 | |
| US11914054B2 (en) | System and methods for estimating attitude and heading based on GNSS carrier phase measurements with assured integrity |
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 |