CN107179084A - Gnss兼容机伪距与加表组合导航及漂移估计方法 - Google Patents

Gnss兼容机伪距与加表组合导航及漂移估计方法 Download PDF

Info

Publication number
CN107179084A
CN107179084A CN201710501159.5A CN201710501159A CN107179084A CN 107179084 A CN107179084 A CN 107179084A CN 201710501159 A CN201710501159 A CN 201710501159A CN 107179084 A CN107179084 A CN 107179084A
Authority
CN
China
Prior art keywords
mrow
msub
mtd
mover
msup
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
Application number
CN201710501159.5A
Other languages
English (en)
Other versions
CN107179084B (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.)
Shanghai Aerospace Control Technology Institute
Original Assignee
Shanghai Aerospace Control Technology 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 Shanghai Aerospace Control Technology Institute filed Critical Shanghai Aerospace Control Technology Institute
Priority to CN201710501159.5A priority Critical patent/CN107179084B/zh
Publication of CN107179084A publication Critical patent/CN107179084A/zh
Application granted granted Critical
Publication of CN107179084B publication Critical patent/CN107179084B/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/24—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for cosmonautical navigation
    • 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
    • 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/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • 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

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Astronomy & Astrophysics (AREA)
  • Manufacturing & Machinery (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Navigation (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)
  • Medicines Containing Antibodies Or Antigens For Use As Internal Diagnostic Agents (AREA)

Abstract

本发明公开了一种GNSS兼容机伪距与加表组合导航及漂移估计方法，包括如下步骤：步骤1，基于伪距观测误差估计三轴位置误差；步骤2，基于三轴位置误差估计位置/速度修正量；步骤3，基于速度误差修正量和PI滤波估计加表漂移；步骤4，基于位置/速度误差修正量和加表漂移进行轨道积分推算。本发明基于GNSS兼容机伪距估计位置/速度修正量，基于速度修正量和PI滤波估计加表漂移，该GNSS兼容机伪距与加表组合导航及漂移估计算法简单，易于工程应用。

Description

GNSS兼容机伪距与加表组合导航及漂移估计方法
技术领域
本发明涉及空间飞行器组合导航技术领域，具体是一种GNSS兼容机伪距与加表组合导航及漂移估计方法，用于空间飞行器自主导航。
背景技术
现有的空间飞行器一般直接基于GNSS兼容机输出的位置/速度与加表进行组合导航，飞行器上轨道推算输出的位置/速度与GNSS兼容机输出位置/速度的作差获得位置/速度误差，可以直接用于修正组合导航位置/速度误差。
高轨GNSS兼容机接收地球另一侧的导航星信号，这些导航星信号较弱，可探测的导航星数有限；空间站下方对接的卫星受空间站遮挡影响，收到的导航星数有限。探测的导航星数较少时，不能直接定位，只能输出少数星的伪距，飞行器需要基于伪距进行组合导航。
基于GNSS兼容机输出的伪距与加表组合导航，伪距误差与位置/速度误差存在非线性关系，一般采用卡尔曼滤波进行组合导航，位置/速度估计共6维，计算量较大，如进行加表漂移估计，维数增加至9维，计算量更大。如何根据伪距误差估计位置/速度误差，并基于位置/速度误差直接修正组合导航位置/速度误差，减小计算量，易于工程应用。
发明内容
本发明针对现有技术中存在的上述不足，提供了一种GNSS兼容机伪距与加表组合导航及漂移估计方法，该方法基于GNSS兼容机伪距估计位置/速度修正量，基于速度修正量和PI滤波估计加表漂移，该GNSS兼容机伪距与加表组合导航及漂移估计算法简单，易于工程应用。
本发明是通过以下技术方案实现的。
一种GNSS兼容机伪距与加表组合导航及漂移估计方法，包括如下步骤：
步骤1，基于伪距观测误差估计三轴位置误差：
-计算伪距观测误差；
-估计三轴位置误差；
步骤2，基于三轴位置误差估计位置/速度修正量：
-位置误差修正量估计；
-速度误差修正量估计；
步骤3，基于速度误差修正量和PI滤波估计加表漂移：
步骤4，基于位置/速度误差修正量和加表漂移轨道推算：
所述步骤1具体包括如下步骤：
步骤1.1，计算伪距观测误差：
根据推算的卫星位置(x,y,z)和第i颗GNSS卫星位置，求得伪距估计值ρi′：
其中：(xi,yi,zi)表示第i颗GNSS卫星在J2000系位置和速度。
Δρi＝ρi′-ρi＝|r′sni|-|rsni|
Δρi为伪距观测误差，ρi为GNSS兼容机输出的卫星相对第i颗GNSS卫星的伪距测量值。
步骤1.2，估计三轴位置误差：
由图1得卫星位置误差方程如下：
drsi＝r′si-rsi
其中：r′si为推算的卫星位置，rsi为卫星真实位置。
星地矢量与星星矢量夹角：
其中：r′si为推算的J2000惯性系卫星位置相对地心位置矢量，r′sni为推算的J2000惯性系卫星位置相对GNSS导航星位置矢量。
推算卫星位置误差矢量，如果|αi|≤α0或|π-αi|≤α0：
drsi＝0
否则：
令：
drsi＝[dxsi dysi dzsi]T
对于n颗导航星基于伪距求卫星推算误差如下：
求得在J2000惯性系位置误差：
Δri＝[dxs dys dzs]T
在惯导积分过程中逐步增加位置/速度误差修正量Δri,k，可以确保误差修正的平稳性。
所述步骤2具体包括如下步骤：
步骤2.1，位置误差修正量估计：
在惯导解算过程对位置和速度误差修正量进行积分，为防止积分饱和，需要对积分进行限幅，实际工程应用中可以对位置误差Δri,k进行限幅，设对Δri,k限幅后位置/速度误差为
基于GNSS兼容机伪距求得的限幅后位置误差估计位置误差修正量dri,k如下：
其中：
kp,r为位置/速度误差修正量估计比例系数，kp,r为3×3对角阵，可以独立估计位置/速度误差修正量。
令：
dri,k＝[dxi,k dyi,k dzi,k]T
三轴独立估计位置误差修正量形式如下：
其中：
kp,rx、kp,ry、kp,rz为三轴位置误差修正量估计比例系数。
步骤2.2，速度误差修正量估计：
考虑到位置误差也反映速度误差，基于位置误差修正量dri,k估计速度误差修正量。
由位置误差修正量dri,k估计速度误差修正量dvi,k，为防积分饱和也需要对dri，k限幅，设对dri，k限幅后的位置误差修正量为得速度误差修正量dvi,k如下：
其中：
kp,v为速度误差修正量估计比例系数，kp,v为3×3对角阵，可以独立估计速度误差修正量。
令；
dvi,k＝[dvxi,k dvyi,k dvzi,k]T
三轴独立估计速度误差修正量形式如下：
其中：
kp,vx、kp,vy、kp,vz为三轴速度误差修正量估计比例系数。
所述步骤3具体包括如下步骤：
将速度误差修正量dvi,k转换到加表坐标系：
dva,k＝Aai·dvi,k
其中：
Aai为J2000惯性系到加表坐标系的姿态转换矩阵。
由速度误差修正量dva,k估计加表漂移，为防积分饱和也需要对dva,k限幅，设对dva,k限幅后的位置误差修正量为基于PI滤波估计加表加速度漂移算法如下：
其中：
daa,k为第k步估计的加表漂移，kp,a为PI滤波估计比例系数，ki,a为PI滤波估计积分系数，kp,a、ki,a为3×3对角阵，可以独立估计加速度漂移。
令：
daa,k＝[daxa,k daya,k daza,k]T
三轴独立估计加速度漂移形式如下：
其中：
kp,ax、kp,ay、kp,az为三轴加速度漂移估计比例系数，ki,ax、ki,ay、ki,az为三轴加速度漂移估计积分系数。
所述步骤4具体包括如下步骤：
设加表坐标系到J2000惯性系的姿态转换矩阵为Aia，求得J2000惯性系下卫星非惯性加速度fi如下：
fi＝Aia·(fs-dfs)
其中：
fs为加表测得的比力，dfs为加表漂移估计值。
令：
fi＝[fx fy fz]T
考虑J2项摄动的影响，在J2000惯性系卫星的运动方程可以表示为：
其中，fx、fy、fz为加表测得的比力扣除漂移转换到J2000惯性系的三轴分量。
令：
ai＝[ax ay az]T
在J2000惯性系扣除位置/速度误差修正量，应用简化积分算法进行惯导解算如下：
vi,k＝vi,k-1+[ai,k-1+(ai,k-ai,k-1)/2]·T-dvi,k-1
ri,k＝ri,k-1+[vi,k-1+(vi,k-vi,k-1)/2]·T-dri,k-1
其中：
ai,k-1为第k-1步扣除加表漂移的加速度。
ai,k为第k步扣除加表漂移的加速度。
dvi,k-1为第k-1步估计的速度误差修正量。
vi,k-1为第k-1步扣除速度误差修正量的速度。
vi,k为第k步扣除速度误差修正量的速度。
dri,k-1为第k-1步估计的位置误差修正量。
ri,k-1为第k-1步扣除位置误差修正量的位置。
ri,k为第k步扣除位置误差修正量的位置。
T为导航周期。
本发明提供的GNSS兼容机伪距与加表组合导航及漂移估计方法，与现有技术相比，具有以下优点和有益效果：
1、基于伪距观测误差估计三轴位置误差；
2、基于三轴位置误差估计位置/速度误差修正量；
3、基于速度误差修正量和PI滤波估计加表漂移；
4、组合导航算法简单，易于工程应用。
附图说明
通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：
图1是本发明导航星和卫星相对位置关系示意图；
图2是本发明组合导航过程。
具体实施方式
下面对本发明的实施例作详细说明：本实施例在以本发明技术方案为前提下进行实施，给出了详细的实施方式和具体的操作过程。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。
请同时参阅图1至图2。
本发明是通过以下技术方案实现的。
一种GNSS兼容机伪距与加表组合导航及漂移估计方法，包括如下步骤：
步骤1，基于伪距观测误差估计三轴位置误差：
-计算伪距观测误差；
-估计三轴位置误差；
步骤2，基于三轴位置误差估计位置/速度修正量：
-位置误差修正量估计；
-速度误差修正量估计；
步骤3，基于速度误差修正量和PI滤波估计加表漂移；
步骤4，基于位置/速度误差修正量和加表漂移轨道推算。
所述步骤1具体包括如下步骤：
步骤1.1，计算伪距观测误差：
根据推算的卫星位置(x,y,z)和第i颗GNSS卫星位置，求得伪距估计值ρi′：
其中：(xi,yi,zi)表示第i颗GNSS卫星在J2000系位置和速度。
Δρi＝ρi′-ρi＝|r′sni|-|rsni|
Δρi为伪距观测误差，ρi为GNSS兼容机输出的卫星相对第i颗GNSS卫星的伪距测量值。
步骤1.2，估计三轴位置误差：
由图1得卫星位置误差方程如下：
drsi＝r′si-rsi
其中：r′si为推算的卫星位置，rsi为卫星真实位置。
星地矢量与星星矢量夹角：
其中：r′si为推算的J2000惯性系卫星位置相对地心位置矢量，r′sni为推算的J2000惯性系卫星位置相对GNSS导航星位置矢量。
推算卫星位置误差矢量，如果|αi|≤α0或|π-αi|≤α0：
drsi＝0
否则：
令：
drsi＝[dxsi dysi dzsi]T
对于n颗导航星基于伪距求卫星推算误差如下：
求得在J2000惯性系位置误差：
Δri＝[dxs dys dzs]T
在惯导积分过程中逐步增加位置/速度误差修正量Δri,k，可以确保误差修正的平稳性。
所述步骤2具体包括如下步骤：
步骤2.1，位置误差修正量估计：
在惯导解算过程对位置和速度误差修正量进行积分，为防止积分饱和，需要对积分进行限幅，实际工程应用中可以对位置误差Δri,k进行限幅，设对Δri,k限幅后位置/速度误差为
基于GNSS兼容机伪距求得的限幅后位置误差估计位置误差修正量dri,k如下：
其中：
kp,r为位置/速度误差修正量估计比例系数，kp,r为3×3对角阵，可以独立估计位置/速度误差修正量。
令：
dri,k＝[dxi,k dyi,k dzi,k]T
三轴独立估计位置误差修正量形式如下：
其中：
kp,rx、kp,ry、kp,rz为三轴位置误差修正量估计比例系数。
步骤2.2，速度误差修正量估计：
考虑到位置误差也反映速度误差，基于位置误差修正量dri,k估计速度误差修正量。
由位置误差修正量dri,k估计速度误差修正量dvi,k，为防积分饱和也需要对dri,k限幅，设对dri,k限幅后的位置误差修正量为得速度误差修正量dvi,k如下：
其中：
kp,v为速度误差修正量估计比例系数，kp,v为3×3对角阵，可以独立估计速度误差修正量。
令；
dvi,k＝[dvxi,k dvyi,k dvzi,k]T
三轴独立估计速度误差修正量形式如下：
其中：
kp,vx、kp,vy、kp,vz为三轴速度误差修正量估计比例系数。
所述步骤3具体包括如下步骤：
将速度误差修正量dvi,k转换到加表坐标系：
dva,k＝Aai·dvi,k
其中：
Aai为J2000惯性系到加表坐标系的姿态转换矩阵。
由速度误差修正量dva,k估计加表漂移，为防积分饱和也需要对dva,k限幅，设对dva,k限幅后的位置误差修正量为基于PI滤波估计加表加速度漂移算法如下：
其中：
daa,k为第k步估计的加表漂移，kp,a为PI滤波估计比例系数，ki,a为PI滤波估计积分系数，kp,a、ki,a为3×3对角阵，可以独立估计加速度漂移。
令：
daa,k＝[daxa,k daya,k daza,k]T
三轴独立估计加速度漂移形式如下：
其中：
kp,ax、kp,ay、kp,az为三轴加速度漂移估计比例系数，ki,ax、ki,ay、ki,az为三轴加速度漂移估计积分系数。
所述步骤4具体包括如下步骤：
设加表坐标系到J2000惯性系的姿态转换矩阵为Aia，求得J2000惯性系下卫星非惯性加速度fi如下：
fi＝Aia·(fs-dfs)
其中：
fs为加表测得的比力，dfs为加表漂移估计值。
令：
fi＝[fx fy fz]T
考虑J2项摄动的影响，在J2000惯性系卫星的运动方程可以表示为：
其中，fx、fy、fz为加表测得的比力扣除漂移转换到J2000惯性系的三轴分量。
令：
ai＝[ax ay az]T
在J2000惯性系扣除位置/速度误差修正量，应用简化积分算法进行惯导解算如下：
vi,k＝vi,k-1+[ai,k-1+(ai,k-ai,k-1)/2]·T-dvi,k-1
ri,k＝ri,k-1+[vi,k-1+(vi,k-vi,k-1)/2]·T-dri,k-1
其中：
ai,k-1为第k-1步扣除加表漂移的加速度。
ai,k为第k步扣除加表漂移的加速度。
dvi,k-1为第k-1步估计的速度误差修正量。
vi,k-1为第k-1步扣除速度误差修正量的速度。
vi,k为第k步扣除速度误差修正量的速度。
dri,k-1为第k-1步估计的位置误差修正量。
ri,k-1为第k-1步扣除位置误差修正量的位置。
ri,k为第k步扣除位置误差修正量的位置。
T为导航周期。
本实施例基于GNSS兼容机伪距估计位置/速度修正量，基于速度修正量和PI滤波估计加表漂移，适用于空间飞行器自主导航，该GNSS兼容机伪距与加表组合导航算法简单，易于工程应用。
以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变形或修改，这并不影响本发明的实质内容。

Claims (5)

1.一种GNSS兼容机伪距与加表组合导航及漂移估计方法，其特征在于，包括如下步骤：
步骤1，基于伪距观测误差估计三轴位置误差；
步骤2，基于三轴位置误差估计位置/速度误差修正量；
步骤3，基于速度误差修正量和PI滤波估计加表漂移；
步骤4，基于位置/速度误差修正量和加表漂移进行轨道积分推算。
2.根据权利要求1所述的GNSS兼容机伪距与加表组合导航及漂移估计方法，其特征在于，所述步骤1进一步包括如下步骤：
步骤1.1，计算伪距观测误差：
根据推算的卫星位置(x,y,z)和第i颗GNSS卫星位置，求得伪距估计值ρi′：
<mrow> <msubsup> <mi>&amp;rho;</mi> <mi>i</mi> <mo>&amp;prime;</mo> </msubsup> <mo>=</mo> <msqrt> <mrow> <msup> <mrow> <mo>(</mo> <mi>x</mi> <mo>-</mo> <msub> <mi>x</mi> <mi>i</mi> </msub> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mo>+</mo> <msup> <mrow> <mo>(</mo> <mi>y</mi> <mo>-</mo> <msub> <mi>y</mi> <mi>i</mi> </msub> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mo>+</mo> <msup> <mrow> <mo>(</mo> <mi>z</mi> <mo>-</mo> <msub> <mi>z</mi> <mi>i</mi> </msub> <mo>)</mo> </mrow> <mn>2</mn> </msup> </mrow> </msqrt> </mrow>
其中：(xi,yi,zi)表示第i颗GNSS卫星在J2000系位置和速度；
Δρi＝ρ′i-ρi＝|r′sni|-|rsni|
Δρi为伪距观测误差，ρi为GNSS兼容机输出的卫星相对第i颗GNSS卫星的伪距测量值；
步骤1.2，估计三轴位置误差：
卫星位置误差为：
drsi＝r′si-rsi
其中：r′si为推算的卫星位置，rsi为卫星真实位置；
星地矢量与星星矢量夹角：
<mrow> <mi>c</mi> <mi>o</mi> <mi>s</mi> <mi>&amp;alpha;</mi> <mo>=</mo> <mfrac> <mrow> <msubsup> <mi>r</mi> <mrow> <mi>s</mi> <mi>i</mi> </mrow> <mo>&amp;prime;</mo> </msubsup> <mo>&amp;CenterDot;</mo> <msubsup> <mi>r</mi> <mrow> <mi>s</mi> <mi>n</mi> <mi>i</mi> </mrow> <mo>&amp;prime;</mo> </msubsup> </mrow> <mrow> <mo>|</mo> <msubsup> <mi>r</mi> <mrow> <mi>s</mi> <mi>i</mi> </mrow> <mo>&amp;prime;</mo> </msubsup> <mo>|</mo> <mo>&amp;CenterDot;</mo> <mo>|</mo> <msubsup> <mi>r</mi> <mrow> <mi>s</mi> <mi>n</mi> <mi>i</mi> </mrow> <mo>&amp;prime;</mo> </msubsup> <mo>|</mo> </mrow> </mfrac> </mrow>
其中：r′si为推算的J2000惯性系卫星位置相对地心位置矢量，r′sni为推算的J2000惯性系卫星位置相对GNSS导航星位置矢量；
推算卫星位置误差矢量，如果|αi|≤α0或|π-αi|≤α0：
drsi＝0
否则：
<mrow> <msub> <mi>dr</mi> <mrow> <mi>s</mi> <mi>i</mi> </mrow> </msub> <mo>=</mo> <mfrac> <mrow> <msub> <mi>&amp;Delta;&amp;rho;</mi> <mi>i</mi> </msub> </mrow> <mrow> <msub> <mi>sin&amp;alpha;</mi> <mi>i</mi> </msub> </mrow> </mfrac> <mo>&amp;CenterDot;</mo> <mfrac> <mrow> <mrow> <mo>(</mo> <mrow> <msup> <msub> <mi>r</mi> <mrow> <mi>s</mi> <mi>i</mi> </mrow> </msub> <mo>&amp;prime;</mo> </msup> <mo>&amp;times;</mo> <msubsup> <mi>r</mi> <mrow> <mi>s</mi> <mi>n</mi> <mi>i</mi> </mrow> <mo>&amp;prime;</mo> </msubsup> </mrow> <mo>)</mo> </mrow> <mo>&amp;times;</mo> <msup> <msub> <mi>r</mi> <mrow> <mi>s</mi> <mi>i</mi> </mrow> </msub> <mo>&amp;prime;</mo> </msup> </mrow> <mrow> <mo>|</mo> <mrow> <mo>(</mo> <mrow> <msup> <msub> <mi>r</mi> <mrow> <mi>s</mi> <mi>i</mi> </mrow> </msub> <mo>&amp;prime;</mo> </msup> <mo>&amp;times;</mo> <msubsup> <mi>r</mi> <mrow> <mi>s</mi> <mi>n</mi> <mi>i</mi> </mrow> <mo>&amp;prime;</mo> </msubsup> </mrow> <mo>)</mo> </mrow> <mo>&amp;times;</mo> <msup> <msub> <mi>r</mi> <mrow> <mi>s</mi> <mi>i</mi> </mrow> </msub> <mo>&amp;prime;</mo> </msup> <mo>|</mo> </mrow> </mfrac> </mrow>
令：
drsi＝[dxsi dysi dzsi]T
对于n颗导航星基于伪距求卫星推算误差如下：
<mrow> <msub> <mi>dx</mi> <mi>s</mi> </msub> <mo>=</mo> <mi>s</mi> <mi>i</mi> <mi>g</mi> <mi>n</mi> <mrow> <mo>(</mo> <msub> <mi>dx</mi> <mrow> <mi>s</mi> <mi>j</mi> </mrow> </msub> <mo>)</mo> </mrow> <mo>&amp;CenterDot;</mo> <munderover> <mrow> <mi>m</mi> <mi>a</mi> <mi>x</mi> </mrow> <mi>j</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>1...</mn> <mi>n</mi> </mrow> </munderover> <mo>{</mo> <mo>|</mo> <msub> <mi>dx</mi> <mrow> <mi>s</mi> <mn>1</mn> </mrow> </msub> <mo>|</mo> <mo>,</mo> <mo>|</mo> <msub> <mi>dx</mi> <mrow> <mi>s</mi> <mn>2</mn> </mrow> </msub> <mo>|</mo> <mo>,</mo> <mn>...</mn> <mo>,</mo> <mo>|</mo> <msub> <mi>dx</mi> <mrow> <mi>s</mi> <mi>i</mi> </mrow> </msub> <mo>|</mo> <mo>,</mo> <mn>...</mn> <mo>,</mo> <mo>|</mo> <msub> <mi>dx</mi> <mrow> <mi>s</mi> <mi>n</mi> </mrow> </msub> <mo>|</mo> <mo>}</mo> </mrow>
<mrow> <msub> <mi>dy</mi> <mi>s</mi> </msub> <mo>=</mo> <mi>s</mi> <mi>i</mi> <mi>g</mi> <mi>n</mi> <mrow> <mo>(</mo> <msub> <mi>dy</mi> <mrow> <mi>s</mi> <mi>k</mi> </mrow> </msub> <mo>)</mo> </mrow> <mo>&amp;CenterDot;</mo> <munderover> <mrow> <mi>m</mi> <mi>a</mi> <mi>x</mi> </mrow> <mi>k</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>1...</mn> <mi>n</mi> </mrow> </munderover> <mo>{</mo> <mo>|</mo> <msub> <mi>dy</mi> <mrow> <mi>s</mi> <mn>1</mn> </mrow> </msub> <mo>|</mo> <mo>,</mo> <mo>|</mo> <msub> <mi>dy</mi> <mrow> <mi>s</mi> <mn>2</mn> </mrow> </msub> <mo>|</mo> <mo>,</mo> <mo>...</mo> <mo>,</mo> <mo>|</mo> <msub> <mi>dy</mi> <mrow> <mi>s</mi> <mi>i</mi> </mrow> </msub> <mo>|</mo> <mo>,</mo> <mo>...</mo> <mo>,</mo> <mo>|</mo> <msub> <mi>dy</mi> <mrow> <mi>s</mi> <mi>n</mi> </mrow> </msub> <mo>|</mo> <mo>}</mo> </mrow> 1
<mrow> <msub> <mi>dz</mi> <mi>s</mi> </msub> <mo>=</mo> <mi>s</mi> <mi>i</mi> <mi>g</mi> <mi>n</mi> <mrow> <mo>(</mo> <msub> <mi>dz</mi> <mrow> <mi>s</mi> <mi>l</mi> </mrow> </msub> <mo>)</mo> </mrow> <mo>&amp;CenterDot;</mo> <munderover> <mrow> <mi>m</mi> <mi>a</mi> <mi>x</mi> </mrow> <mi>l</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>1...</mn> <mi>n</mi> </mrow> </munderover> <mo>{</mo> <mo>|</mo> <msub> <mi>dz</mi> <mrow> <mi>s</mi> <mn>1</mn> </mrow> </msub> <mo>|</mo> <mo>,</mo> <mo>|</mo> <msub> <mi>dz</mi> <mrow> <mi>s</mi> <mn>2</mn> </mrow> </msub> <mo>|</mo> <mo>,</mo> <mo>...</mo> <mo>,</mo> <mo>|</mo> <msub> <mi>dz</mi> <mrow> <mi>s</mi> <mi>i</mi> </mrow> </msub> <mo>|</mo> <mo>,</mo> <mo>...</mo> <mo>,</mo> <mo>|</mo> <msub> <mi>dz</mi> <mrow> <mi>s</mi> <mi>n</mi> </mrow> </msub> <mo>|</mo> <mo>}</mo> </mrow>
求得在J2000惯性系位置误差：
Δri＝[dxs dys dzs]T
在惯导积分过程中逐步增加位置/速度误差修正量Δri,k。
3.根据权利要求2所述的GNSS兼容机伪距与加表组合导航及漂移估计方法，其特征在于，所述步骤2进一步包括如下步骤：
步骤2.1，位置误差修正量估计：
在惯导解算过程对位置和速度误差修正量进行积分，设对位置误差Δri,k限幅后位置/速度误差为
基于GNSS兼容机伪距求得的限幅后位置/速度误差估计位置误差修正量dri,k如下：
<mrow> <msub> <mi>dr</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <msub> <mi>k</mi> <mrow> <mi>p</mi> <mo>,</mo> <mi>r</mi> </mrow> </msub> <mo>&amp;CenterDot;</mo> <mi>&amp;Delta;</mi> <msub> <mover> <mi>r</mi> <mo>&amp;OverBar;</mo> </mover> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> </mrow>
其中：
kp,r为位置/速度误差修正量估计比例系数，kp,r为3×3对角阵，用以独立估计位置/速度误差修正量；
令：
dri,k＝[dxi,k dyi,k dzi,k]T
<mrow> <mi>&amp;Delta;</mi> <msub> <mover> <mi>r</mi> <mo>&amp;OverBar;</mo> </mover> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <msup> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <mi>&amp;Delta;</mi> <msub> <mover> <mi>x</mi> <mo>&amp;OverBar;</mo> </mover> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> </mrow> </mtd> <mtd> <mrow> <mi>&amp;Delta;</mi> <msub> <mover> <mi>y</mi> <mo>&amp;OverBar;</mo> </mover> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> </mrow> </mtd> <mtd> <mrow> <mi>&amp;Delta;</mi> <msub> <mover> <mi>z</mi> <mo>&amp;OverBar;</mo> </mover> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> </mrow>
<mrow> <msub> <mi>k</mi> <mrow> <mi>p</mi> <mo>,</mo> <mi>r</mi> </mrow> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>k</mi> <mrow> <mi>p</mi> <mo>,</mo> <mi>r</mi> <mi>x</mi> </mrow> </msub> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <msub> <mi>k</mi> <mrow> <mi>p</mi> <mo>,</mo> <mi>r</mi> <mi>y</mi> </mrow> </msub> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <msub> <mi>k</mi> <mrow> <mi>p</mi> <mo>,</mo> <mi>r</mi> <mi>z</mi> </mrow> </msub> </mtd> </mtr> </mtable> </mfenced> </mrow>
三轴独立估计位置误差修正量形式如下：
<mrow> <msub> <mi>dx</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <msub> <mi>k</mi> <mrow> <mi>p</mi> <mo>,</mo> <mi>r</mi> <mi>x</mi> </mrow> </msub> <mo>&amp;CenterDot;</mo> <mi>&amp;Delta;</mi> <msub> <mover> <mi>x</mi> <mo>&amp;OverBar;</mo> </mover> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> </mrow>
<mrow> <msub> <mi>dy</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <msub> <mi>k</mi> <mrow> <mi>p</mi> <mo>,</mo> <mi>r</mi> <mi>y</mi> </mrow> </msub> <mo>&amp;CenterDot;</mo> <mi>&amp;Delta;</mi> <msub> <mover> <mi>y</mi> <mo>&amp;OverBar;</mo> </mover> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> </mrow>
<mrow> <msub> <mi>dz</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <msub> <mi>k</mi> <mrow> <mi>p</mi> <mo>,</mo> <mi>r</mi> <mi>z</mi> </mrow> </msub> <mo>&amp;CenterDot;</mo> <mi>&amp;Delta;</mi> <msub> <mover> <mi>z</mi> <mo>&amp;OverBar;</mo> </mover> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> </mrow>
步骤2.2，速度误差修正量估计：
位置误差对应于速度误差，故基于位置误差修正量dri,k估计速度误差修正量dvi,k，设对dri,k限幅后的位置误差修正量为
<mrow> <msub> <mi>dv</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <msub> <mi>k</mi> <mrow> <mi>p</mi> <mo>,</mo> <mi>v</mi> </mrow> </msub> <mo>&amp;CenterDot;</mo> <mi>d</mi> <msub> <mover> <mi>r</mi> <mo>&amp;OverBar;</mo> </mover> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> </mrow>
令；
dvi,k＝[dvxi,k dvyi,k dvzi,k]T
<mrow> <mi>d</mi> <msub> <mover> <mi>r</mi> <mo>&amp;OverBar;</mo> </mover> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <msup> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <mi>d</mi> <msub> <mover> <mi>x</mi> <mo>&amp;OverBar;</mo> </mover> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> </mrow> </mtd> <mtd> <mrow> <mi>d</mi> <msub> <mover> <mi>y</mi> <mo>&amp;OverBar;</mo> </mover> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> </mrow> </mtd> <mtd> <mrow> <mi>d</mi> <msub> <mover> <mi>z</mi> <mo>&amp;OverBar;</mo> </mover> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> </mrow> 2
三轴独立估计速度误差修正量形式如下：
<mrow> <msub> <mi>dvx</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <msub> <mi>k</mi> <mrow> <mi>p</mi> <mo>,</mo> <mi>v</mi> <mi>x</mi> </mrow> </msub> <mo>&amp;CenterDot;</mo> <mi>d</mi> <msub> <mover> <mi>x</mi> <mo>&amp;OverBar;</mo> </mover> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> </mrow>
<mrow> <msub> <mi>dvy</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <msub> <mi>k</mi> <mrow> <mi>p</mi> <mo>,</mo> <mi>v</mi> <mi>y</mi> </mrow> </msub> <mo>&amp;CenterDot;</mo> <mi>d</mi> <msub> <mover> <mi>y</mi> <mo>&amp;OverBar;</mo> </mover> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> </mrow>
<mrow> <msub> <mi>dvz</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <msub> <mi>k</mi> <mrow> <mi>p</mi> <mo>,</mo> <mi>v</mi> <mi>z</mi> </mrow> </msub> <mo>&amp;CenterDot;</mo> <mi>d</mi> <msub> <mover> <mi>z</mi> <mo>&amp;OverBar;</mo> </mover> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> </mrow>
其中：
kp,vx、kp,vy、kp,vz为三轴速度误差修正量估计比例系数。
4.根据权利要求3所述的GNSS兼容机伪距与加表组合导航及漂移估计方法，其特征在于，所述步骤3基于速度误差修正量和PI滤波估计加表漂移：
将速度误差修正量dvi,k转换到加表坐标系：
dva,k＝Aai·dvi,k
其中：
Aai为J2000惯性系到加表坐标系的姿态转换矩阵；
由速度误差修正量dva,k估计加表漂移，设对dva,k限幅后的位置误差修正量为基于PI滤波估计加表加速度漂移算法如下：
<mrow> <msub> <mi>da</mi> <mrow> <mi>a</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <msub> <mi>k</mi> <mrow> <mi>p</mi> <mo>,</mo> <mi>a</mi> </mrow> </msub> <mo>&amp;CenterDot;</mo> <mi>d</mi> <msub> <mover> <mi>v</mi> <mo>&amp;OverBar;</mo> </mover> <mrow> <mi>a</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>+</mo> <mo>&amp;Integral;</mo> <msub> <mi>k</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>a</mi> </mrow> </msub> <mo>&amp;CenterDot;</mo> <mi>d</mi> <msub> <mover> <mi>v</mi> <mo>&amp;OverBar;</mo> </mover> <mrow> <mi>a</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>&amp;CenterDot;</mo> <mi>d</mi> <mi>t</mi> </mrow>
其中：
daa,k为第k步估计的加表漂移，kp,a为PI滤波估计比例系数，ki,a为PI滤波估计积分系数，kp,a、ki,a为3×3对角阵，用以独立估计加速度漂移；
令：
daa,k＝[daxa,k daya,k daza,k]T
<mrow> <mi>d</mi> <msub> <mover> <mi>v</mi> <mo>&amp;OverBar;</mo> </mover> <mrow> <mi>a</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <msup> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <mi>d</mi> <msub> <mover> <mrow> <mi>v</mi> <mi>x</mi> </mrow> <mo>&amp;OverBar;</mo> </mover> <mrow> <mi>a</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> </mrow> </mtd> <mtd> <mrow> <mi>d</mi> <msub> <mover> <mrow> <mi>v</mi> <mi>y</mi> </mrow> <mo>&amp;OverBar;</mo> </mover> <mrow> <mi>a</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> </mrow> </mtd> <mtd> <mrow> <mi>d</mi> <mover> <mi>v</mi> <mo>&amp;OverBar;</mo> </mover> <msub> <mover> <mi>z</mi> <mo>&amp;OverBar;</mo> </mover> <mrow> <mi>a</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> </mrow>
<mrow> <msub> <mi>k</mi> <mrow> <mi>p</mi> <mo>,</mo> <mi>a</mi> </mrow> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>k</mi> <mrow> <mi>p</mi> <mo>,</mo> <mi>a</mi> <mi>x</mi> </mrow> </msub> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <msub> <mi>k</mi> <mrow> <mi>p</mi> <mo>,</mo> <mi>a</mi> <mi>y</mi> </mrow> </msub> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <msub> <mi>k</mi> <mrow> <mi>p</mi> <mo>,</mo> <mi>a</mi> <mi>z</mi> </mrow> </msub> </mtd> </mtr> </mtable> </mfenced> </mrow>
<mrow> <msub> <mi>k</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>a</mi> </mrow> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>k</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>a</mi> <mi>x</mi> </mrow> </msub> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <msub> <mi>k</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>a</mi> <mi>y</mi> </mrow> </msub> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <msub> <mi>k</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>a</mi> <mi>z</mi> </mrow> </msub> </mtd> </mtr> </mtable> </mfenced> </mrow>
三轴独立估计加速度漂移形式如下：
<mrow> <msub> <mi>dax</mi> <mrow> <mi>a</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <msub> <mi>k</mi> <mrow> <mi>p</mi> <mo>,</mo> <mi>a</mi> <mi>x</mi> </mrow> </msub> <mo>&amp;CenterDot;</mo> <mi>d</mi> <msub> <mover> <mrow> <mi>v</mi> <mi>x</mi> </mrow> <mo>&amp;OverBar;</mo> </mover> <mrow> <mi>a</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>+</mo> <mo>&amp;Integral;</mo> <msub> <mi>k</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>a</mi> <mi>x</mi> </mrow> </msub> <mo>&amp;CenterDot;</mo> <mi>d</mi> <msub> <mover> <mrow> <mi>v</mi> <mi>x</mi> </mrow> <mo>&amp;OverBar;</mo> </mover> <mrow> <mi>a</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>&amp;CenterDot;</mo> <mi>d</mi> <mi>t</mi> </mrow>
<mrow> <msub> <mi>day</mi> <mrow> <mi>a</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <msub> <mi>k</mi> <mrow> <mi>p</mi> <mo>,</mo> <mi>a</mi> <mi>y</mi> </mrow> </msub> <mo>&amp;CenterDot;</mo> <mi>d</mi> <msub> <mover> <mrow> <mi>v</mi> <mi>y</mi> </mrow> <mo>&amp;OverBar;</mo> </mover> <mrow> <mi>a</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>+</mo> <mo>&amp;Integral;</mo> <msub> <mi>k</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>a</mi> <mi>y</mi> </mrow> </msub> <mo>&amp;CenterDot;</mo> <mi>d</mi> <msub> <mover> <mrow> <mi>v</mi> <mi>y</mi> </mrow> <mo>&amp;OverBar;</mo> </mover> <mrow> <mi>a</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>&amp;CenterDot;</mo> <mi>d</mi> <mi>t</mi> </mrow> 3
<mrow> <msub> <mi>daz</mi> <mrow> <mi>a</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <msub> <mi>k</mi> <mrow> <mi>p</mi> <mo>,</mo> <mi>a</mi> <mi>z</mi> </mrow> </msub> <mo>&amp;CenterDot;</mo> <mi>d</mi> <mover> <mi>v</mi> <mo>&amp;OverBar;</mo> </mover> <msub> <mover> <mi>z</mi> <mo>&amp;OverBar;</mo> </mover> <mrow> <mi>a</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>+</mo> <mo>&amp;Integral;</mo> <msub> <mi>k</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>a</mi> <mi>z</mi> </mrow> </msub> <mo>&amp;CenterDot;</mo> <mi>d</mi> <mover> <mi>v</mi> <mo>&amp;OverBar;</mo> </mover> <msub> <mover> <mi>z</mi> <mo>&amp;OverBar;</mo> </mover> <mrow> <mi>a</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>&amp;CenterDot;</mo> <mi>d</mi> <mi>t</mi> </mrow>
其中：
kp,ax、kp,ay、kp,az为三轴加速度漂移估计比例系数，ki,ax、ki,ay、ki,az为三轴加速度漂移估计积分系数。
5.根据权利要求4所述的GNSS兼容机伪距与加表组合导航及漂移估计方法，其特征在于，所述步骤4基于位置/速度误差修正量和加表漂移轨道推算：
设加表坐标系到J2000惯性系的姿态转换矩阵为Aia，求得J2000惯性系下卫星非惯性加速度fi如下：
fi＝Aia·(fs-dfs)
其中：
fs为加表测得的比力，dfs为加表漂移估计值；
令：
fi＝[fx fy fz]T
考虑J2项摄动的影响，在J2000惯性系卫星的运动方程表示为：
<mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msub> <mi>a</mi> <mi>x</mi> </msub> <mo>=</mo> <mover> <mi>x</mi> <mo>&amp;CenterDot;&amp;CenterDot;</mo> </mover> <mo>=</mo> <mo>-</mo> <mfrac> <mi>&amp;mu;</mi> <msup> <mi>r</mi> <mn>3</mn> </msup> </mfrac> <mi>x</mi> <mo>-</mo> <mfrac> <mn>3</mn> <mn>2</mn> </mfrac> <msub> <mi>J</mi> <mn>2</mn> </msub> <mo>&amp;CenterDot;</mo> <msubsup> <mi>R</mi> <mi>E</mi> <mn>2</mn> </msubsup> <mo>&amp;CenterDot;</mo> <mi>&amp;mu;</mi> <mfrac> <mi>x</mi> <msup> <mi>r</mi> <mn>5</mn> </msup> </mfrac> <mrow> <mo>(</mo> <mrow> <mn>1</mn> <mo>-</mo> <mfrac> <mrow> <mn>5</mn> <msup> <mi>z</mi> <mn>2</mn> </msup> </mrow> <msup> <mi>r</mi> <mn>2</mn> </msup> </mfrac> </mrow> <mo>)</mo> </mrow> <mo>+</mo> <msub> <mi>f</mi> <mi>x</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>a</mi> <mi>y</mi> </msub> <mo>=</mo> <mover> <mi>y</mi> <mo>&amp;CenterDot;&amp;CenterDot;</mo> </mover> <mo>=</mo> <mo>-</mo> <mfrac> <mi>&amp;mu;</mi> <msup> <mi>r</mi> <mn>3</mn> </msup> </mfrac> <mi>y</mi> <mo>-</mo> <mfrac> <mn>3</mn> <mn>2</mn> </mfrac> <msub> <mi>J</mi> <mn>2</mn> </msub> <mo>&amp;CenterDot;</mo> <msubsup> <mi>R</mi> <mi>E</mi> <mn>2</mn> </msubsup> <mo>&amp;CenterDot;</mo> <mi>&amp;mu;</mi> <mfrac> <mi>y</mi> <msup> <mi>r</mi> <mn>5</mn> </msup> </mfrac> <mrow> <mo>(</mo> <mrow> <mn>1</mn> <mo>-</mo> <mfrac> <mrow> <mn>5</mn> <msup> <mi>z</mi> <mn>2</mn> </msup> </mrow> <msup> <mi>r</mi> <mn>2</mn> </msup> </mfrac> </mrow> <mo>)</mo> </mrow> <mo>+</mo> <msub> <mi>f</mi> <mi>y</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>a</mi> <mi>z</mi> </msub> <mo>=</mo> <mover> <mi>z</mi> <mo>&amp;CenterDot;&amp;CenterDot;</mo> </mover> <mo>=</mo> <mo>-</mo> <mfrac> <mi>&amp;mu;</mi> <msup> <mi>r</mi> <mn>3</mn> </msup> </mfrac> <mi>z</mi> <mo>-</mo> <mfrac> <mn>3</mn> <mn>2</mn> </mfrac> <msub> <mi>J</mi> <mn>2</mn> </msub> <mo>&amp;CenterDot;</mo> <msubsup> <mi>R</mi> <mi>E</mi> <mn>2</mn> </msubsup> <mo>&amp;CenterDot;</mo> <mi>&amp;mu;</mi> <mfrac> <mi>z</mi> <msup> <mi>r</mi> <mn>5</mn> </msup> </mfrac> <mrow> <mo>(</mo> <mrow> <mn>3</mn> <mo>-</mo> <mfrac> <mrow> <mn>5</mn> <msup> <mi>z</mi> <mn>2</mn> </msup> </mrow> <msup> <mi>r</mi> <mn>2</mn> </msup> </mfrac> </mrow> <mo>)</mo> </mrow> <mo>+</mo> <msub> <mi>f</mi> <mi>z</mi> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced>
其中，fx、fy、fz为加表测得的比力扣除漂移转换到J2000惯性系的三轴分量；
令：
ai＝[ax ay az]T
在J2000惯性系扣除位置/速度误差修正量，应用简化积分算法进行惯导解算如下：
vi,k＝vi,k-1+[ai,k-1+(ai,k-ai,k-1)/2]·T-dvi,k-1
ri,k＝ri,k-1+[vi,k-1+(vi,k-vi,k-1)/2]·T-dri,k-1
其中：
ai,k-1为第k-1步扣除加表漂移的加速度；
ai,k为第k步扣除加表漂移的加速度；
dvi,k-1为第k-1步估计的速度误差修正量；
vi,k-1为第k-1步扣除速度误差修正量的速度；
vi,k为第k步扣除速度误差修正量的速度；
dri,k-1为第k-1步估计的位置误差修正量；
ri,k-1为第k-1步扣除位置误差修正量的位置；
ri,k为第k步扣除位置误差修正量的位置；
T为导航周期。
CN201710501159.5A 2017-06-27 2017-06-27 Gnss兼容机伪距与加表组合导航及漂移估计方法 Active CN107179084B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201710501159.5A CN107179084B (zh) 2017-06-27 2017-06-27 Gnss兼容机伪距与加表组合导航及漂移估计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710501159.5A CN107179084B (zh) 2017-06-27 2017-06-27 Gnss兼容机伪距与加表组合导航及漂移估计方法

Publications (2)

Publication Number Publication Date
CN107179084A true CN107179084A (zh) 2017-09-19
CN107179084B CN107179084B (zh) 2020-08-11

Family

ID=59845414

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710501159.5A Active CN107179084B (zh) 2017-06-27 2017-06-27 Gnss兼容机伪距与加表组合导航及漂移估计方法

Country Status (1)

Country Link
CN (1) CN107179084B (zh)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110596426A (zh) * 2019-09-25 2019-12-20 上海航天控制技术研究所 一种加速度计组合数据漂移自主辨识方法
CN111174779A (zh) * 2019-11-29 2020-05-19 上海航天控制技术研究所 一种深空探测飞行器惯性-天文组合导航方法
CN111256731A (zh) * 2020-02-28 2020-06-09 上海航天控制技术研究所 一种不受历史数据影响快速收敛的地面静态对准方法
CN113987407A (zh) * 2021-10-28 2022-01-28 中国西安卫星测控中心 一种地球静止卫星非球形引力切向漂移加速度计算方法

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103299209A (zh) * 2011-01-07 2013-09-11 三星电子株式会社 用于使用导航算法检测位置信息的设备和方法
US8849565B1 (en) * 2007-06-28 2014-09-30 The Boeing Company Navigation system based on neutrino detection
CN105424041A (zh) * 2016-01-18 2016-03-23 重庆邮电大学 一种基于bd/ins紧耦合的行人定位算法
CN106153051A (zh) * 2016-06-29 2016-11-23 上海航天控制技术研究所 一种航天器组合导航方法
CN106707322A (zh) * 2016-12-30 2017-05-24 立得空间信息技术股份有限公司 基于rtk/sins的高动态定位定姿系统及方法
CN106767787A (zh) * 2016-12-29 2017-05-31 北京时代民芯科技有限公司 一种紧耦合gnss/ins组合导航装置

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8849565B1 (en) * 2007-06-28 2014-09-30 The Boeing Company Navigation system based on neutrino detection
CN103299209A (zh) * 2011-01-07 2013-09-11 三星电子株式会社 用于使用导航算法检测位置信息的设备和方法
CN105424041A (zh) * 2016-01-18 2016-03-23 重庆邮电大学 一种基于bd/ins紧耦合的行人定位算法
CN106153051A (zh) * 2016-06-29 2016-11-23 上海航天控制技术研究所 一种航天器组合导航方法
CN106767787A (zh) * 2016-12-29 2017-05-31 北京时代民芯科技有限公司 一种紧耦合gnss/ins组合导航装置
CN106707322A (zh) * 2016-12-30 2017-05-24 立得空间信息技术股份有限公司 基于rtk/sins的高动态定位定姿系统及方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
王健: "提高SINS/GNSS组合导航系统定位精度的方法研究", 《现代导航》 *
王献忠 等: "基于地平仪的惯性-天文组合导航", 《导航定位学报》 *

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110596426A (zh) * 2019-09-25 2019-12-20 上海航天控制技术研究所 一种加速度计组合数据漂移自主辨识方法
CN111174779A (zh) * 2019-11-29 2020-05-19 上海航天控制技术研究所 一种深空探测飞行器惯性-天文组合导航方法
CN111256731A (zh) * 2020-02-28 2020-06-09 上海航天控制技术研究所 一种不受历史数据影响快速收敛的地面静态对准方法
CN113987407A (zh) * 2021-10-28 2022-01-28 中国西安卫星测控中心 一种地球静止卫星非球形引力切向漂移加速度计算方法
CN113987407B (zh) * 2021-10-28 2022-07-08 中国西安卫星测控中心 一种地球静止卫星非球形引力切向漂移加速度计算方法

Also Published As

Publication number Publication date
CN107179084B (zh) 2020-08-11

Similar Documents

Publication Publication Date Title
CN107179084A (zh) Gnss兼容机伪距与加表组合导航及漂移估计方法
CN104597471B (zh) 面向时钟同步多天线gnss接收机的定向测姿方法
CN106291639B (zh) 一种gnss接收机实现定位的方法及装置
CN107588769B (zh) 一种车载捷联惯导、里程计及高程计组合导航方法
CN101881619B (zh) 基于姿态测量的船用捷联惯导与天文定位方法
CN104501838B (zh) 捷联惯导系统初始对准方法
CN101825467B (zh) 捷联惯性导航系统与天文导航系统实现组合导航的方法
CN102519485B (zh) 一种引入陀螺信息的二位置捷联惯性导航系统初始对准方法
CN103278165B (zh) 基于剩磁标定的磁测及星光备份的自主导航方法
CN102508277A (zh) 精密单点定位与惯性测量紧组合导航系统及数据处理方法
CN103675861A (zh) 一种基于星载gnss多天线的卫星自主定轨方法
CN103792561B (zh) 一种基于gnss通道差分的紧组合降维滤波方法
CN103217174B (zh) 一种基于低精度微机电系统的捷联惯导系统初始对准方法
CN107270893A (zh) 面向不动产测量的杆臂、时间不同步误差估计与补偿方法
CN102853837B (zh) 一种mimu和gnss信息融合的方法
CN103968844B (zh) 基于低轨平台跟踪测量的大椭圆机动航天器自主导航方法
CN103868514A (zh) 一种在轨飞行器自主导航系统
CN112880669B (zh) 一种航天器星光折射和单轴旋转调制惯性组合导航方法
CN104316943B (zh) 一种伪距离和多普勒组合差分定位系统及方法
CN108088443A (zh) 一种定位定向设备速度补偿方法
CN105157724B (zh) 一种基于速度加姿态匹配的传递对准时间延迟估计与补偿方法
CN103529459A (zh) 一种采用单频gps和glonass组合精准定位的方法及其系统
CN103941274A (zh) 一种导航方法及导航终端
CN103454665A (zh) 一种双差gps/sins组合导航姿态测量方法
CN105606093B (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
GR01 Patent grant
GR01 Patent grant