CN112146655B - 一种BeiDou/SINS紧组合导航系统弹性模型设计方法 - Google Patents

一种BeiDou/SINS紧组合导航系统弹性模型设计方法 Download PDF

Info

Publication number
CN112146655B
CN112146655B CN202010894005.9A CN202010894005A CN112146655B CN 112146655 B CN112146655 B CN 112146655B CN 202010894005 A CN202010894005 A CN 202010894005A CN 112146655 B CN112146655 B CN 112146655B
Authority
CN
China
Prior art keywords
representing
error
sins
vector
matrix
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
CN202010894005.9A
Other languages
English (en)
Other versions
CN112146655A (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.)
Indufei Intelligent Equipment Co ltd
Original Assignee
Zhengzhou University of Light Industry
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 Zhengzhou University of Light Industry filed Critical Zhengzhou University of Light Industry
Priority to CN202010894005.9A priority Critical patent/CN112146655B/zh
Publication of CN112146655A publication Critical patent/CN112146655A/zh
Application granted granted Critical
Publication of CN112146655B publication Critical patent/CN112146655B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • 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

Abstract

本发明提出了一种全源弹性BeiDou/SINS紧组合导航系统的弹性模型设计方法，用于解决全源导航数据快速融合定位计算技术问题。本发明利用全源弹性PNT服务体系的导航定位系统概念框架，以精确SINS子系统为核心，以多源BeiDou导航系统、磁力计等导航设备，设计了全源弹性PNT组合导航系统姿态旋转和平移运动非线性弹性观测器方程，把多源导航设备数据构建融合为系统模型方程的注入项算子和弹性修正函数，提出新型全源弹性PNT导航定位系统的级联式非线性姿态‑平移运动弹性观测器理论与算法解决方案，满足水面无人飞行器运动对象的复杂应用环境下全源弹性PNT组合导航定位系统快速精确计算的技术性能要求。

Description

一种BeiDou/SINS紧组合导航系统弹性模型设计方法
技术领域
本发明涉及航空航天航海领域的导航定位授时(Positioning、Naviagtion andTiming,PNT)服务中的系统信息处理技术领域，特别是指一种BeiDou/SINS紧组合导航系统弹性模型设计方法。
背景技术
无人机导航定位主要有惯性导航系统(Inertial navigation system,SINS)，为运载体提供位置、速度和姿态数据信息，目前采用最多的是捷联(Strip)惯性导航系统(SINS)，利用三轴加速度计和陀螺仪传感器；全球卫星导航系统(Global NavigationSatellite System,GNSS)，包括我国的BeiDou导航系统，美国GPS系统和Galileo系统等，GNSS以导航卫星为基站，能够提供精确的三维位置、速度和时间信息，但是存在着GNSS信号被遮蔽或者人为干扰等缺陷，同时SINS系统存在着导航误差会随时间累积，惯性器件精度受到工艺水平和成本限制，普通精度纯惯导系统不能满足长航时导航应用要求。很明显目前单一导航方式难以满足运载体高精度长航时稳定导航的技术需求，组合导航技术与系统成为无人机飞行器导航定位技术发展的主要方向。但是随着微传感器技术、物联网通信技术、计算机技术以及现代控制理论发展，越来越多的实时定位与导航传感数据可以有效融合到GNSS和INS组合导航系统中去，构成了分布式多源组合导航定位系统架构，由此杨元喜院士提出了弹性PNT框架概念，目前国家自然科学基金支持了水下潜器的弹性PNT体系算法与系统研究工作。
全源组合定位导航系统离不开多源传感数据信息融合问题，目前多传感器信息融合技术已经在导航定位领域获得广泛应用，传统组合导航系统利用多源传感器物理模型来构建运载体定位导航系统的观测器模型，采用Kalman滤波理论与算法开展运载体导航系统状态变量估计计算，从而为导航制导律提供状态数据，随后以Bayesian滤波理论框架构建的随机Kalman最优滤波算法获得快速发展和完善，如EKF算法、UKF算法、CDKF算法、CKF算法和PF算法，以及SMF算法等等，但是非线性Bayesian滤波框架下的滤波算法都存在着收敛性不清晰，算法计算精度受到高阶截断误差影响，滤波参数整定困难，同时计算量很大，滤波计算效能较低的缺陷。因此近年来基于非线性稳定性理论发展起来的非线性观测器方法逐步引起学者注意，非线性观测器理论是一种具有全局指数稳定的确定性建模方法，它没有假设系统噪声具体特性，观测器估计数据对于干扰噪声和初始条件不确定性具有较强鲁棒性；它利用多源导航传感数据设计系统姿态和平移运动误差注入项算子围包系统状态测量值和估计值的差值驱动系统状态变量逼近系统状态真实值，利用线性系统理论方法展开模型计算处理过程，这样可以有效避免EKF算法的线性化操作；组合导航系统的非线性观测器分为系统姿态观测器和平移运动观测器两部分，对于姿态观测器可以由获得的姿态直接测量值或者是向量测量值和已知的参考向量间的比较值开展姿态建模设计，平移运动观测器则是根据GNSS/INS组合模式，采用不同的观测量如位置向量，若引入机器人地面起伏运动的虚拟垂直参考系统，垂向位移矢量和GNSS接收机水平面内的两个分量组合构成三维位移向量；松组合模式中列出速度向量方程，而紧组合模式中需要列出钟差量误差方程；双差分GNSS模式中需要考虑频偏误差模型方程等。
发明内容
针对上述背景技术中存在的不足，本发明提出了一种BeiDou/SINS紧组合导航系统弹性模型设计方法，解决了BeiDou卫星导航系统的接收机测量的垂直位移测量不准确的技术问题。
本发明的技术方案是这样实现的：
一种BeiDou/SINS紧组合导航系统弹性模型设计方法，其步骤如下：
步骤一、根据全源弹性BeiDou/SINS紧组合导航系统多源传感器配置，设计紧组合模式无人机载体有界姿态模型方程，并根据多源传感器中磁力计传感设备测量的数据计算注入项算子，根据多源传感器中陀螺仪测量的数据计算陀螺仪偏差；
步骤二、分别对注入项算子和陀螺仪偏差进行离散化计算，根据注入项算子和陀螺仪偏差的离散化结果对紧组合模式无人机载体有界姿态模型方程进行离散化计算；
步骤三、根据代数法计算紧组合导航系统中卫星的伪距观测量数据，并引入辅助向量构建BeiDou/SINS紧组合导航系统的无人机平移运动观测器模型；
步骤四、将BeiDou/SINS紧组合导航系统的无人机平移运动观测器模型转化为BeiDou/SINS紧组合导航系统的LTV误差动力学模型，并对BeiDou/SINS紧组合导航系统的LTV误差动力学模型进行弹性参数整定和变换，得到BeiDou/SINS紧组合导航系统的LTV误差状态变量的Riccati方程；
步骤五、利用SINS滤波原理对BeiDou/SINS紧组合导航系统的LTV误差动力学模型进行离散化，得到BeiDou/SINS紧组合导航系统的误差状态变量的观测量离散化方程；
步骤六、对BeiDou/SINS紧组合导航系统的误差状态变量的观测量离散化方程进行LTV误差状态估计，根据LTV误差状态估计对LTV误差状态变量进行更新。
所述紧组合模式无人机载体有界姿态模型方程为：
Figure BDA0002657856370000031
其中，
Figure BDA0002657856370000032
表示载体坐标系b相对于ECEF坐标系的旋转四元数微分，/>
Figure BDA0002657856370000033
表示无人机从载体坐标系b到ECEF坐标系的旋转姿态四元数，/>
Figure BDA0002657856370000034
表示陀螺仪测量角速度，/>
Figure BDA0002657856370000035
表示陀螺仪偏差，/>
Figure BDA0002657856370000036
表示扩展注入项算子，/>
Figure BDA0002657856370000037
表示地球自转角速度ωie的四维扩展向量，/>
Figure BDA0002657856370000038
表示陀螺仪偏差随机游走量，/>
Figure BDA0002657856370000039
表示陀螺仪测量偏差，/>
Figure BDA00026578563700000310
表示注入项算子，/>
Figure BDA00026578563700000311
表示陀螺仪偏差定界值，Proj(·)表示投影模型，/>
Figure BDA00026578563700000312
表示比力测量值，/>
Figure BDA00026578563700000313
表示旋转矩阵的转置矩阵，/>
Figure BDA00026578563700000314
表示饱和算子，κI表示在非线性姿态观测器组合效应作用下的陀螺仪角速率偏差估计弹性增益系数，κ2表示注入项算子/>
Figure BDA00026578563700000315
的各个观测/参考矢量对的弹性比例系数，
Figure BDA00026578563700000316
表示载体坐标系b下的磁力计测量值，me表示地球磁场参考矢量，/>
Figure BDA00026578563700000317
表示加速度计的比力矢量；
分别对比力测量值
Figure BDA00026578563700000318
加速度计的比力矢量/>
Figure BDA00026578563700000319
载体坐标系b下的磁力计测量值
Figure BDA00026578563700000320
地球磁场参考矢量me进行规范化计算，得到：
Figure BDA00026578563700000321
其中，f b表示载体坐标系下的规范化比力，f e表示ECEF坐标系下的规范化测量比力，m b表示载体坐标系下的规范化磁力计测量值，m e表示ECEF坐标系下的规范化磁力计；
注入项算子
Figure BDA00026578563700000322
的规范化形式为：
Figure BDA00026578563700000323
所述对注入项算子
Figure BDA00026578563700000324
进行离散化计算的方法为：
Figure BDA00026578563700000325
若i＝1，
Figure BDA00026578563700000326
执行/>
Figure BDA00026578563700000327
计算，
Figure BDA00026578563700000328
否则，
Figure BDA00026578563700000329
若i＝2，
Figure BDA0002657856370000041
执行/>
Figure BDA0002657856370000042
计算，
Figure BDA0002657856370000043
否则，
Figure BDA0002657856370000044
其中，δtacc表示加速度计可用时的采样时间间隔，δtmag表示磁力计可用时的采样时间间隔，T表示积分间隔，
Figure BDA0002657856370000045
表示k时刻的总的注入项算子，/>
Figure BDA0002657856370000046
表示k时刻的比力计算的注入项部分，/>
Figure BDA0002657856370000047
表示k时刻的磁力计测量的注入项部分，k1(k)表示k时刻的增益系数，f b(k)表示k时刻的载体坐标系下的比力规范值，/>
Figure BDA0002657856370000048
表示k-1时刻的四元数表述的旋转矩阵，f e(k)表示ECEF坐标系下的k时刻的比力规范化值，k表示时刻，m b(k)表示载体坐标系下的k时刻的磁力计规范化值，m e(k)表示ECEF坐标系下的k时刻的磁力计规范化值；
所述对陀螺仪偏差
Figure BDA0002657856370000049
进行离散化计算的方法为：
将陀螺仪偏差
Figure BDA00026578563700000410
的投影模型转化为，/>
Figure BDA00026578563700000411
其中，
Figure BDA00026578563700000412
表示注入项最小化取值算子，Mb是陀螺仪测量角速率偏差的上界；
陀螺仪偏差
Figure BDA00026578563700000413
的离散化表达式为，
Figure BDA00026578563700000414
其中，
Figure BDA00026578563700000415
表示k时刻的陀螺仪偏差计算值，/>
Figure BDA00026578563700000416
表示k-1时刻的陀螺仪偏差计算值，κI(k)表示k时刻的陀螺仪偏差计算的增益系数。
所述根据注入项算子和陀螺仪偏差的离散化结果对紧组合模式无人机载体有界姿态模型方程进行离散化计算的方法为：
Figure BDA00026578563700000417
其中，
Figure BDA0002657856370000051
表示斜对称矩阵的指数计算，/>
Figure BDA0002657856370000052
表示负斜对称矩阵的指数计算，/>
Figure BDA0002657856370000053
表示k时刻的计算角速度，/>
Figure BDA0002657856370000054
表示k时刻的角速度，/>
Figure BDA0002657856370000055
表示角速度，/>
Figure BDA0002657856370000056
表示地球自转角速度，/>
Figure BDA0002657856370000057
表示k-1时刻的角速度，I4表示4维单位阵。
所述根据代数法计算紧组合导航系统中卫星的伪距观测量数据的方法为：
S31、无人机参考位置矢量
Figure BDA0002657856370000058
每一颗卫星的视线向量/>
Figure BDA0002657856370000059
四颗卫星的伪距观测量为ρ1、ρ2、ρ3和ρ4，且其中前三颗卫星的视线向量/>
Figure BDA00026578563700000510
和/>
Figure BDA00026578563700000511
都是相互线性独立，前三颗卫星的视线向量组成视线矩阵/>
Figure BDA00026578563700000512
满足/>
Figure BDA00026578563700000513
设置伪距观测噪声误差矩阵Г＝1，定义辅助变量/>
Figure BDA00026578563700000514
S32、由四颗卫星的伪距观测量和视线向量组成矩阵
Figure BDA00026578563700000515
则
Figure BDA00026578563700000516
辅助变量z表达式为：
Figure BDA00026578563700000517
/>
其中，
Figure BDA00026578563700000518
表示由向量/>
Figure BDA00026578563700000519
计算的转换向量，/>
Figure BDA00026578563700000520
表示由向量/>
Figure BDA00026578563700000521
计算的转换向量，/>
Figure BDA00026578563700000522
表示观测距离向量，/>
Figure BDA00026578563700000523
表示单位向量，/>
Figure BDA00026578563700000524
表示视线向量，/>
Figure BDA00026578563700000525
且/>
Figure BDA00026578563700000526
表示视线距离矢量，i'＝1,2,3,4，M＝diag(1,1,1，-1)表示对角阵。
所述BeiDou/SINS紧组合导航系统的无人机平移运动观测器模型为：
Figure BDA0002657856370000061
其中，
Figure BDA0002657856370000062
表示无人机平移运动的位置矢量，/>
Figure BDA0002657856370000063
表示机器人平移运动的速度向量，
Figure BDA0002657856370000064
表示加速度计比力估计向量，Ψ表示辅助向量，/>
Figure BDA0002657856370000065
表示位置微分，/>
Figure BDA0002657856370000066
表示速度微分，/>
Figure BDA0002657856370000067
表示辅助变量微分，/>
Figure BDA0002657856370000068
表示钟差微分，eρ,i表示平移运动注入项信号，ev,i表示平移运动注入项信号，m表示可观测卫星数，/>
Figure BDA0002657856370000069
表示计算位置表达的重力矢量，/>
Figure BDA00026578563700000610
表示位置伪距增益系数，/>
Figure BDA00026578563700000611
表示位置速度增益系数，/>
Figure BDA00026578563700000612
表示速度伪距增益系数，/>
Figure BDA00026578563700000613
表示伪距率增益系数，/>
Figure BDA00026578563700000614
表示辅助变量伪距增益系数，/>
Figure BDA00026578563700000615
表示辅助变量伪距率增益系数，/>
Figure BDA00026578563700000616
表示钟差伪距增益系数，/>
Figure BDA00026578563700000617
表示钟差伪距率增益系数。
所述平移运动注入项信号eρ,i、ev,i分别为：
Figure BDA00026578563700000618
则估计观测值的表达式为：
Figure BDA00026578563700000619
Figure BDA00026578563700000620
其中，ρi表示观测值，
Figure BDA00026578563700000621
表示估计观测值，vi表示观测值，/>
Figure BDA00026578563700000622
表示估计观测值，/>
Figure BDA00026578563700000623
是伪距误差建模参数，Г表示伪距测量的观测噪声误差矩阵；第i颗卫星的位置和速度分别表示为/>
Figure BDA00026578563700000624
和/>
Figure BDA00026578563700000625
第i颗卫星和接收机间的几何距离为/>
Figure BDA00026578563700000626
加速度计比力误差
Figure BDA00026578563700000627
位置误差定义为/>
Figure BDA00026578563700000628
速度误差定义为/>
Figure BDA00026578563700000629
时钟偏差误差定义/>
Figure BDA00026578563700000630
Figure BDA00026578563700000631
表示速度伪距率观测矩阵Π的转置；
当估计计算卫星观测和几何距离时，已知卫星的位置和速度，可以获得注入项算子计算式为：
Figure BDA0002657856370000071
Figure BDA0002657856370000072
其中，伪距噪声定界为，
Figure BDA0002657856370000073
Figure BDA0002657856370000074
表示位置误差下界值，伪距率噪声定界为，/>
Figure BDA0002657856370000075
Figure BDA0002657856370000076
表示伪距率上界值。
所述将BeiDou/SINS紧组合导航系统的无人机平移运动观测器模型转化为BeiDou/SINS紧组合导航系统的LTV误差动力学模型的方法为：
定义误差状态变量
Figure BDA0002657856370000077
采用Taylor级数分别对注入项算子计算式进行线性化，可得误差状态方程：
eρ,i＝Cρ,ix+ερ,i，
eν,i＝Cν,ix+εν,i，
其中，Cρ,i、Cν,i均表示时变矩阵，且时变矩阵Cρ,i和Cν,i的行数为2m；
将时变矩阵Cρ,i和Cν,i级联为综合时变矩阵C为，
Figure BDA0002657856370000078
其中
Figure BDA0002657856370000079
视线向量估计为/>
Figure BDA00026578563700000710
规范化的相对速度估计为/>
Figure BDA00026578563700000711
所述综合时变矩阵C可表示为：
Figure BDA00026578563700000712
其中，
Figure BDA00026578563700000713
根据综合时变矩阵C构建BeiDou/SINS紧组合导航系统的LTV误差动力学模型为：
Figure BDA00026578563700000714
其中，矩阵
Figure BDA00026578563700000715
矩阵/>
Figure BDA00026578563700000716
ρ1(t,x)、ρ2(t,χ)和ρ3(t,x)均为干扰项，/>
Figure BDA0002657856370000089
表示LTV系统状态变量微分，x表示LTV系统状态变量，t表示连续系统时间变量，I3表示3维单位矩阵；
所述干扰项ρ1(t,x)、ρ2(t,χ)和ρ3(t,x)分别为：
Figure BDA0002657856370000081
Figure BDA0002657856370000082
ρ3(t,x)＝Kε(t,x)，
其中，
Figure BDA0002657856370000083
ε(t,x)＝[ερ,1；…；ερ,m；εv,1；…；εv,m]是误差状态变量函数，θ≥1弹性增益选择系数，R(·)表示旋转矩阵。
所述对BeiDou/SINS紧组合导航系统的LTV误差动力学模型进行弹性参数整定和变换，得到BeiDou/SINS紧组合导航系统的LTV误差状态变量的Riccati方程的方法为：
为确保LTV误差动力学方程的收敛性，引入非奇异状态变换矩阵Lθ：
Figure BDA0002657856370000084
其中，I3表示3维单位矩阵，In表示n维单位矩阵；
利用非奇异状态变换矩阵Lθ将LTV误差状态变量变换为：
η＝Lθx；
对于任意的矩阵K0∈R(9+n)×2m，在给定弹性增益选择系数θ≥1的θ常值参数设置下，设置
Figure BDA0002657856370000085
其中时变矩阵Eθ∈R2m×2m满足EθC＝CLθ，那么LTV误差动力学方程可变换为：
Figure BDA0002657856370000086
由于速度误差项
Figure BDA0002657856370000087
可能会超出确定限界，时变矩阵C可能不是定界的，此时令K0＝PCTR-1，获得误差系统状态方差矩阵的Riccati方程为：
Figure BDA0002657856370000088
所述利用SINS滤波原理对BeiDou/SINS紧组合导航系统的LTV误差动力学模型进行离散化，得到BeiDou/SINS紧组合导航系统的误差状态变量的观测量离散化方程的方法为：
利用传统的SINS滤波原理，观测器状态向量是辅助参考向量和SINS测量向量间的误差向量以及有色噪声向量组成，定义SINS误差向量δx*为：
Figure BDA0002657856370000091
其中，δP表示位置误差，δV表示速度误差，δΨ表示辅助变量误差，Pe表示位置矢量，Ve表示速度矢量，Ψ表示辅助变量，
Figure BDA0002657856370000092
表示SINS计算的位置，/>
Figure BDA0002657856370000093
表示SINS计算的速度，ΨSINS表示SINS计算的辅助变量；离散化的SINS误差向量可表示为：
Figure BDA0002657856370000094
其中，z[k]表示紧组合模式下的钟差误差向量，z[k]＝β[k]；从而LTV误差动力学模型相应可离散化表示为：
δx[k]＝Ad[k-1]δx[k-1]-Rd[k-1]w[k-1]，
δy[k]＝C[k]δx[k]-εpv[k]，
其中，w[k-1]的表示式为，
Figure BDA0002657856370000095
矩阵C[k]∈R4×m的离散化表达式为，
Figure BDA0002657856370000096
定义SINS状态向量为，
Figure BDA0002657856370000097
那么BeiDou/SINS紧组合导航系统的误差状态变量的观测量离散化方程为，
Figure BDA0002657856370000098
所述对BeiDou/SINS紧组合导航系统的误差状态变量的观测量离散化方程进行LTV误差状态估计，根据LTV误差状态估计对LTV误差状态变量进行更新的方法为：
误差状态向量时间更新为：
δx-[k]＝Ad[k-1]δx+[k-1]，
其中，δx-[k]表示k时刻的系统状态预测值，δx+[k-1]表示k-1时刻的系统状态估计值；
误差状态向量方差矩阵时间更新计算为，
Figure BDA0002657856370000101
/>
其中，
Figure BDA0002657856370000102
Qd[k-1]＝Qδt，δt＝t[k]-t[k-1]，P-[k]表示k时刻的预测系统状态误差方差阵，P+[k-1]表示k-1时刻的估计系统状态误差方差，Bd[k-1]表示k-1时刻的噪声系数矩阵；
将误差状态向量和预测方差矩阵修订为，
Figure BDA0002657856370000103
其中，
Figure BDA0002657856370000104
表示k时刻的系统状态估计，δx-[k]表示k时刻的系统状态预测，P+[k]表示k时刻的系统状态估计方差矩阵；
实施SINS系统状态向量修正计算，
Figure BDA0002657856370000105
其中，
Figure BDA0002657856370000106
表示k时刻由SINS计算的系统状态，/>
Figure BDA0002657856370000107
表示k时刻由SINS计算的系统状态估计方差矩阵，/>
Figure BDA0002657856370000108
表示估计方差矩阵修订量，/>
Figure BDA0002657856370000109
表示k时刻由SINS计算的系统速度，/>
Figure BDA00026578563700001010
表示k时刻由SINS计算的系统速度修订量，ΨSINS[k]表示k时刻由SINS计算的系统辅助变量，δΨ+[k]表示k时刻由SINS计算的系统辅助变量修订量；
把误差状态变量重置为：
Figure BDA00026578563700001011
考虑BieDou系统信号误差的时间更新计算为，
z-[k]＝Λz[k-1]z+[k-1],Λz[k-1]＝eFδt，
其中，
Figure BDA0002657856370000111
表示BeiDou系统信号k时刻的估计值，z-[k]表示BeiDou系统信号k时刻的预测值，Λz[k-1]表示BeiDou信号状态转移矩阵，z+[k-1]表示BeiDou系统信号k-1时刻的估计值，eFδt表示BeiDou信号状态转移矩阵计算表达式；
部分误差变量
Figure BDA0002657856370000112
满足以下方程式，
Figure BDA0002657856370000113
/>
其中，K*[k]表示k时刻的迭代增益矩阵；
由于(x*)+[k-1]≡0，对误差状态变量表达式实施传统的Kalman滤波更新计算为，
Figure BDA0002657856370000114
其中，Kz[k]表示BeiDou信号的Kalman滤波更新增益矩阵，H[k]表示观测方程一阶偏微分矩阵。
本技术方案能产生的有益效果：利用全源弹性PNT服务体系的导航定位系统概念框架，本发明提出以水面无人飞行器平台应用的全源弹性PNT导航定位系统为对象，以精确SINS子系统为核心，以多源BeiDou导航系统、磁力计等导航设备提出一种全源弹性PNT导航定位系统数据融合方案的创新性研究方法，本发明设计全源弹性PNT组合导航系统姿态旋转和平移运动非线性弹性观测器方程，把多源导航设备数据构建融合为系统模型方程的注入项算子和弹性修正函数，提出新型全源弹性PNT导航定位系统的级联式非线性姿态-平移运动弹性观测器理论与算法解决方案，满足水面无人飞行器运动对象的复杂应用环境下全源弹性PNT组合导航定位系统快速精确计算的技术性能要求。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1是本发明BeiDou/SINS松组合系统弹性观测器模型结构图。
图2是本发明BeiDou/SINS松组合系统弹性观测器模型算法计算流程图。
图3是本发明的无人机载体位置计算误差数据图。
图4是本发明的无人机载体位置计算数据图。
图5是本发明的无人机载体速度计算数据图。
图6是本发明的无人机载体姿态计算数据图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有付出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
无人机运动载体的BeiDou/INS组合导航定位系统主要由BeiDou接收机获得无人机位置和速度信息、IMU组件包括加速度计和陀螺仪以及磁力计测量无人机运动载体的加速度、旋转角速度等信息，在不同坐标系中无人机载体的动力学模型有不同的表达式，如在惯性坐标系(ECI)、在地球协议坐标系(ECEF)、在当地水平坐标系(NED)以及切向坐标系(T系)等，无人机姿态可由不同的姿态角参数表示，如四元数、欧拉角以及罗德里格尔斯参数等，选择ECEF坐标系和四元数构建无人机载体的PVA动力学模型方程，
Figure BDA0002657856370000121
其中，Pe，Ve，fe∈R3分别表示无人机载体在ECEF坐标系下的位置、速度和比力，单位四元数
Figure BDA0002657856370000122
描述无人机从载体坐标系b到ECEF坐标系的旋转姿态，/>
Figure BDA0002657856370000123
是无人机载体相对于ECI惯性系的旋转角速度的四维扩展向量，/>
Figure BDA0002657856370000124
表示地球自转角速度ωie的四维扩展向量，/>
Figure BDA0002657856370000125
表示利用ECEF坐标系下的地球自转角速度组成的斜对称矩阵，ge(Pe)表示无人所在位置的地球重力加速度。利用单位四元数描述的姿态旋转矩阵可表示为，
Figure BDA0002657856370000126
捷联惯性导航系统(SINS)的惯性传感组件IMU中包含了加速度计、陀螺仪以及磁力计等，在载体坐标系b中构建其模型为，
Figure BDA0002657856370000131
其中，
Figure BDA0002657856370000132
是加速度计相对于地球测量的比力经过坐标转换到载体坐标系中的比力矢量，/>
Figure BDA0002657856370000133
是陀螺仪测量的角速率偏差，加速度计偏差或者漂移已经过在线或者离线标定补偿，磁力计提供无人机航向参考信息，那么惯性组件IMU中的测量误差量/>
Figure BDA0002657856370000134
BeiDou接收机测量无人机载体位置和速度数据，和惯性导航系统SINS测量数据进行数据融合，一般来说GNSS和SINS组合有松组合、紧组合和超紧组合模式，松组合策略采用无人机位置和速度数据融合，BeiDou接收机位置和速度测量模型在ECEF坐标系中可表示为，
Figure BDA0002657856370000135
其中，δ*表示测量噪声或者测量误差。
这里综合惯性组件IMU观测数据给出无人机载体的姿态观测器表达式为，
Figure BDA0002657856370000136
其中κI表示在非线性姿态观测器组合效应作用下的陀螺仪角速率偏差估计弹性增益系数，Proj()表示投影算子来确保陀螺角速率偏差是有界的，
Figure BDA0002657856370000137
利用
Figure BDA0002657856370000138
获得/>
Figure BDA0002657856370000139
的估计/>
Figure BDA00026578563700001310
计算。在弹性姿态观测器设计中引入的注入项算子/>
Figure BDA00026578563700001311
它是基于载体坐标系b中的非平行观测向量比较计算获得的，执行陀螺仪偏差的补偿和姿态修正计算，在本模型设计中利用磁力计观测数据和加速度计比力测量数据构成非平行矢量开展计算，需要给出在ECEF坐标系中相应的磁力计和加速度计参考矢量数据，利用旋转矩阵
Figure BDA00026578563700001312
将其转换到载体坐标系b中，当两组矢量不平行时候，那么注入项算子就自动补偿非平行矢量导致的误差，对姿态数据做出补偿修正，其中me是已知的地球磁场参考矢量，/>
Figure BDA00026578563700001313
是加速度计的比力矢量，经由平移运动观测器计算获得的，这样即使是在无人机加速运动状态也可以获得比力的精确估计计算，/>
Figure BDA00026578563700001314
是一个饱和算子，确保比力计算是有界的，
Figure BDA00026578563700001315
这样设计的优势在于，相比于传统的利用载体所在位置数据计算重力加速度模型，/>
Figure BDA0002657856370000141
要求运动载体必须在一定时间内是不能加速的，本发明设计模型则没有这方面的限制要求；另外本发明模型的注入项算子采用了两对矢量对加速度观测/参考矢量对和地球磁场测量/参考矢量对开展计算的，其实本发明模型设计中还可以根据组合系统传感器配置，开放式添加其他观测/参考矢量对获得的注入项算子。应该说明的是利用非平行矢量/参考矢量对设计注入项算子要确保至少两组系统传感器参与注入项算子计算，来保证模型计算收敛性。
为了解决现有BeiDou/SINS组合导航系统模型设计问题，基于非线性稳定性控制理论，本发明提出一类BeiDou/SINS紧组合模式下的导航系统弹性观测器模型设计方法，它采用了注入项算子策略，充分利用组合系统的多源传感数据，设计无人机载体的弹性姿态计算模型、弹性平移运动观测器模型，在弹性平移运动观测器中在BeiDou接收机信号有效情况下充分利用卫星信号伪距和伪距率测量数据，本发明利用至少四颗卫星信号数据，设计一种伪距代数计算算法实现伪距误差以及钟差误差计算，在此基础上对其进行模型转换综合形成LTV系统方程及其系统状态向量方差的Riccati方程，设计辅助变量实现加速度计比力估计在弹性姿态计算模型和弹性平移运动模型的交互操作，整定姿态计算模型和平移运动观测器模型中的弹性系数与弹性增益矩阵，采用直接法实现系统姿态和平移向量的快速有效计算，提高BeiDou/SINS紧组合导航系统状态参数估计的计算效率，并且有效改善系统状态向量参数的计算精度。
本发明充分利用无人机载的导航定位传感设备，BeiDou接收机、SINS组件以及磁力计，甚至还有视觉相机和激光雷达等设备有界物理模型，考虑多个传感设备输出的多对非平行观测向量及其参考矢量，构建系统姿态的注入项算子，对无人机姿态计算数据展开弹性修正计算；利用投影原理对陀螺仪偏差向量进行定界计算操作，其主要优势在于多个传感设备感测数据可在姿态计算模型中采用注入项算子形式开放式输入模型对其进行修正计算，可以有效改善无人机姿态计算精度，从而利用四元数构建无人机运动中的高精度弹性姿态计算模型方程。
利用紧组合模式中BeiDou接收机观测的无人机伪距和伪距率数据，充分考虑BeiDou接收机观测数据特性，构建BeiDou接收机观测误差模型方程，充分考虑无人机运动状态对加速度计测量的比力影响，设计辅助向量体现伪距和伪距率状态向量对比力计算的影响作用计算方程，设计BeiDou/SINS紧组合模式下导航系统平移运动观测器的伪距和伪距率注入项算子设计位置计算方程和速度计算方程。弹性平移运动观测器模型的状态变量定义为位置、速度和辅助变量，把加速度计比力计算作为输入向量，BeiDou接收机的位置和速度向量作为输出向量，其中引入位置、速度、辅助向量和比力方程的弹性系数，构建平移运动观测器的弹性系数矩阵。在至少四颗卫星观测情况下，设计BeiDou接收机伪距和伪距率测量数据的代数计算算法，获得伪距测量误差以及时钟误差的综合解析解。在此基础上实施弹性平移运动观测器转换，获得弹性平移运动观测器的连续时间线性系统LTV模型，设计平移运动观测器的过程噪声和观测噪声矩阵，利用Riccati方程迭代整定计算系统方差矩阵，获得系统Kalman增益矩阵。BeiDou/SINS紧组合导航系统弹性观测器模型的优势在于，它不同于传统的观测器设计思路，它将系统姿态计算方程模型和平移运动观测器方程分开计算，但是二者间通过辅助变量估计加速度计比力计算数据实现交互级联操作，本发明设计的BeiDou/SINS紧组合导航定位系统弹性观测器模型结构如附图1所示；它优势在于能够有效避免传统观测器模型的线性化操作，改善了观测器模型的计算精度与计算稳定性。通过实验数据仿真计算验证了本发明模型设计的正确性和高效计算精度与计算稳定性特点，并且它采用了开放式设计模式，可以根据系统传感设备配置情况，随机修改添加系统姿态注入项算子，从而获得一种BeiDou/SINS紧组合导航系统的弹性观测器模型。
如图1和图2所示，本发明实施例提供了一种BeiDou/SINS紧组合导航系统弹性模型设计方法，具体步骤如下：
步骤一、结合BeiDou/SINS紧组合导航系统多源传感器配置，设计四元数描述的紧组合模式无人机载体有界姿态模型方程，并根据紧组合导航系统多源传感器中磁力计传感设备和加速度计测量的数据计算注入项算子和陀螺仪偏差；
所述四元数描述的紧组合模式无人机载体有界姿态模型方程为：
Figure BDA0002657856370000151
其中，
Figure BDA0002657856370000152
表示载体坐标系相对于ECEF坐标系的旋转四元数微分，/>
Figure BDA0002657856370000153
表示无人机从载体坐标系b到ECEF坐标系的旋转姿态四元数，/>
Figure BDA0002657856370000154
表示陀螺仪测量角速度，/>
Figure BDA0002657856370000155
表示陀螺仪偏差，/>
Figure BDA0002657856370000156
表示扩展注入项算子，/>
Figure BDA0002657856370000157
表示地球自转角速度ωie的四维扩展向量，/>
Figure BDA0002657856370000158
表示陀螺仪偏差随机游走量，/>
Figure BDA0002657856370000159
表示陀螺仪测量偏差，/>
Figure BDA00026578563700001510
表示注入项算子，/>
Figure BDA00026578563700001511
表示陀螺仪偏差定界值，Proj(·)表示投影算子，/>
Figure BDA00026578563700001512
表示比力测量值，/>
Figure BDA00026578563700001513
表示旋转矩阵，/>
Figure BDA00026578563700001514
表示饱和算子，κI表示在非线性姿态观测器组合效应作用下的陀螺仪角速率偏差估计弹性增益系数，κ2表示注入项算子/>
Figure BDA00026578563700001515
的各个观测/参考矢量对的弹性比例系数，/>
Figure BDA00026578563700001516
表示载体坐标系下的磁力计测量值，me表示地球磁场参考矢量，/>
Figure BDA0002657856370000161
表示加速度计的比力矢量；
首先利用本发明紧组合导航定位系统配置的加速度计和磁力计传感设备，加速度计测量比力矢量及其参考矢量(来自于平移运动观测器的前步迭代数据)，磁力计测量的地球磁场矢量以及已知的地球磁场参考数据，分别对比力测量值
Figure BDA0002657856370000162
加速度计的比力矢量
Figure BDA0002657856370000163
载体坐标系下的磁力计测量值/>
Figure BDA0002657856370000164
地球磁场参考矢量me进行规范化计算，得到：
Figure BDA0002657856370000165
其中，f b表示载体坐标系下的规范化比力，f e表示ECEF坐标系下的规范化测量比力，m b表示载体坐标系下的规范化磁力计测量值，m e表示ECEF坐标系下的规范化磁力计；
考虑利用规范化非平行观测/参考矢量对参与无人机载体姿态修正计算，因此注入项算子
Figure BDA0002657856370000166
的规范化形式为：
Figure BDA0002657856370000167
对陀螺仪角速率偏差向量执行投影计算，确保陀螺角速率偏差是定界的，在此过程中引入弹性姿态模型的弹性比例系数κ1、κ2，以及陀螺仪角速率偏差的弹性整定参数κI。
步骤二、分别对注入项算子和陀螺仪偏差进行离散化计算，根据注入项算子和陀螺仪偏差的离散化结果对紧组合模式无人机载体有界姿态模型方程进行离散化计算；
对弹性姿态有界模型方程执行离散化计算，以速率陀螺仪测量速率f＝1/T执行离散化计算，可认为采样期间角速度测量数据
Figure BDA0002657856370000168
不变，那么四元数计算公式为：
Figure BDA0002657856370000169
其中，
Figure BDA00026578563700001610
表示k时刻的计算角速度，
Figure BDA00026578563700001611
表示k时刻的角速度列写出的斜对称矩阵，/>
Figure BDA00026578563700001612
表示地球自转角速度列写出的斜对称矩阵，/>
Figure BDA00026578563700001613
表示斜对称矩阵的指数计算，/>
Figure BDA00026578563700001614
表示负斜对称矩阵的指数计算，/>
Figure BDA00026578563700001615
表示k时刻的角速度，/>
Figure BDA00026578563700001616
表示角速度，S(·)表示由角速度计算出来的向量，/>
Figure BDA0002657856370000171
表示地球自转角速度，/>
Figure BDA0002657856370000172
表示k-1时刻的角速度，I4表示4维单位阵。
根据设计获得的弹性姿态观测器模型式(6)中陀螺仪偏差模型表达式为，
Figure BDA0002657856370000173
其中弹性系数κI表示在非线性姿态观测器组合效应作用下的陀螺仪角速率偏差估计弹性增益系数，Proj(·)表示投影算子来确保陀螺角速率偏差是有界的，
Figure BDA0002657856370000174
Mb是陀螺仪测量角速率偏差的预定义上界，将陀螺仪偏差/>
Figure BDA0002657856370000175
的投影转化为：
Figure BDA0002657856370000176
其中，
Figure BDA0002657856370000177
表示注入项最小化取值算子，Mb是陀螺仪测量角速率偏差的上界，在四元数运算过程中要求保证四元数的规范化，计算公式为
Figure BDA0002657856370000178
则陀螺仪偏差/>
Figure BDA0002657856370000179
的离散化表达式为：
Figure BDA00026578563700001710
其中，
Figure BDA00026578563700001711
表示k时刻的陀螺仪偏差计算值，/>
Figure BDA00026578563700001712
表示k-1时刻的陀螺仪偏差计算值，κI(k)表示k时刻的陀螺仪偏差计算的增益系数。
值得注意的是这里引入多传感设备构建的注入项算子计算，当时刻为k时可以获得可靠的矢量观测数据，可把注入项算子利用投影算子直接添加操作；若此时没有有效的观测矢量，则不执行注入项算子添加操作，对注入项算子
Figure BDA00026578563700001713
进行离散化计算的方法为：
Figure BDA00026578563700001714
若i＝1，
Figure BDA00026578563700001715
执行/>
Figure BDA00026578563700001716
计算，
Figure BDA00026578563700001717
/>
否则，
Figure BDA00026578563700001718
若i＝2，
Figure BDA00026578563700001719
执行/>
Figure BDA00026578563700001720
计算，
Figure BDA0002657856370000181
否则，
Figure BDA0002657856370000182
其中，δtacc表示加速度计可用时的采样时间间隔，δtmag表示磁力计可用时的采样时间间隔，T表示积分间隔，
Figure BDA0002657856370000183
表示k时刻的总的注入项算子，/>
Figure BDA0002657856370000184
表示k时刻的比力计算的注入项部分，/>
Figure BDA0002657856370000185
表示k时刻的磁力计测量的注入项部分，k1(k)表示k时刻的增益系数，f b(k)表示k时刻的载体坐标系下的比力规范值，/>
Figure BDA0002657856370000186
表示k-1时刻的四元数表述的旋转矩阵，f e(k)表示k时刻的ECEF坐标系下的比力规范化值，k表示时刻，m b(k)表示k时刻的载体坐标系下的磁力计规范化值，m e(k)表示k时刻的ECEF坐标系下的磁力计规范化值；实际上加速度计采样时间δtacc＝T。
步骤三、根据代数法计算紧组合导航系统中卫星的伪距观测量数据，并引入辅助向量构建BeiDou/SINS紧组合导航系统的无人机平移运动观测器模型；
所述根据代数法计算紧组合导航系统中卫星的伪距观测量数据的方法为：
S31、给定无人机参考位置矢量
Figure BDA0002657856370000187
每一颗卫星的视线向量/>
Figure BDA0002657856370000188
四颗卫星的伪距观测量为ρ1、ρ2、ρ3和ρ4，且其中前三颗卫星的视线向量/>
Figure BDA0002657856370000189
和/>
Figure BDA00026578563700001810
都是相互线性独立，前三颗卫星的视线向量组成视线矩阵/>
Figure BDA00026578563700001811
满足/>
Figure BDA00026578563700001812
设置伪距观测噪声误差矩阵Г＝1，定义辅助变量/>
Figure BDA00026578563700001813
S32、由四颗卫星的伪距观测量和视线向量组成矩阵
Figure BDA00026578563700001814
则
Figure BDA00026578563700001815
辅助变量z表达式为：
Figure BDA00026578563700001816
其中，
Figure BDA00026578563700001817
表示由向量/>
Figure BDA00026578563700001818
计算的转换向量，/>
Figure BDA00026578563700001819
表示由向量/>
Figure BDA00026578563700001820
计算的转换向量，/>
Figure BDA00026578563700001821
表示观测距离向量，/>
Figure BDA00026578563700001822
表示单位向量，/>
Figure BDA00026578563700001823
表示视线向量，/>
Figure BDA00026578563700001824
且/>
Figure BDA00026578563700001825
表示视线距离矢量，i'＝1,2,3,4，M＝diag(1,1,1，-1)表示对角阵。。
构建BeiDou/SINS紧组合模式下的无人机平移运动观测器模型。在BeiDou/SINS紧组合模式下，组合导航定位系统中BeiDou接收机观测的粗数据，也就是距离和距离变化率观测量，展开与SINS系统的位置和速度组合，构造紧组合模式的BeiDou/SINS组合导航系统的平移运动观测量的注入项算子来设计BeiDou/SINS紧组合系统的平移运动观测器模型。但是距离和距离变化率观测量会受到很明显的外界干扰，如BeiDou导航卫星时钟和BeiDou接收机时钟间的距离钟差误差等因素，因此时钟误差需要在平移运动观测器中进行估计计算；还有卫星测量中的电离层和对流层对卫星信号在信号传输路径上的延迟影响作用，一般采用双频接收机来抵消延迟效应误差，但是不可避免地增加系统测量噪声。在本发明中假设至少配置了四颗可观测到的卫星，那么可以设计BeiDou/SINS组合导航系统的平移运动方程的弹性注入项算子，这里的弹性是指平移运动注入项算子的系数是弹性的随时间可变化的，并且在组合导航系统中的传感器失效情况下，系统能够自动保持注入项算子系数为0，从而消除掉失效传感器对组合导航系统平移运动的影响和作用。在无人机载体弹性姿态模型设计基础上，考虑无人机平移运动观测器设计任务。这里综合BeiDou接收机和惯性组件IMU模型，考虑无人机载体的平移运动向量，位置
Figure BDA0002657856370000191
和速度向量/>
Figure BDA0002657856370000192
还有平移运动计算的加速度计比力估计向量/>
Figure BDA0002657856370000193
为了方便模型编制，和计算比力估计数据，引入辅助向量Ψ，给出BeiDou/SINS紧组合导航系统的无人机平移运动观测器模型为：
Figure BDA0002657856370000194
其中，
Figure BDA0002657856370000195
表示机器人平移运动的位置，/>
Figure BDA0002657856370000196
表示机器人平移运动的速度向量，/>
Figure BDA0002657856370000197
表示加速度计比力估计向量，Ψ表示辅助向量，/>
Figure BDA0002657856370000198
表示位置微分，/>
Figure BDA0002657856370000199
表示速度微分，/>
Figure BDA00026578563700001910
表示辅助变量微分，/>
Figure BDA00026578563700001911
表示钟差微分，eρ,i表示平移运动注入项信号，ev,i表示平移运动注入项信号，m表示可观测卫星数，/>
Figure BDA00026578563700001912
表示计算位置表达的重力矢量，/>
Figure BDA00026578563700001913
表示位置伪距增益系数，/>
Figure BDA00026578563700001914
表示位置速度增益系数，/>
Figure BDA00026578563700001915
表示速度伪距增益系数，/>
Figure BDA00026578563700001916
表示伪距率增益系数，
Figure BDA00026578563700001917
表示辅助变量伪距增益系数，/>
Figure BDA00026578563700001918
表示辅助变量伪距率增益系数，/>
Figure BDA00026578563700001919
表示钟差伪距增益系数，/>
Figure BDA00026578563700001920
表示钟差伪距率增益系数，m表示可观测卫星数；
在紧组合模式导航定位系统的平移运动观测器模型中，加入了卫星时钟与接收机时钟间的钟差观测量及其注入项微分表达方程，其实正是由于时钟偏差误差存在，才需要至少四颗卫星从伪距观测中确定三个位置向量，时钟误差一般表示为时变函数，β:＝c△c，其中c表示光速，△c表示时钟误差值。但是在本发明时钟误差由设计平移运动注入项信号eρ,i和ev,i决定的，注入项信号中包含着噪声，所述平移运动注入项信号eρ,i、ev,i分别为：
Figure BDA0002657856370000201
Figure BDA0002657856370000202
则估计观测值的表达式为：
Figure BDA0002657856370000203
Figure BDA0002657856370000204
其中，ρi表示观测值，
Figure BDA0002657856370000205
表示估计观测值，vi表示观测值，/>
Figure BDA0002657856370000206
表示估计观测值，/>
Figure BDA0002657856370000207
是伪距误差建模参数，Г表示伪距测量的观测噪声误差矩阵；第i颗卫星的位置和速度分别表示为/>
Figure BDA0002657856370000208
和/>
Figure BDA0002657856370000209
第i颗卫星和接收机间的几何距离为/>
Figure BDA00026578563700002010
加速度计比力误差
Figure BDA00026578563700002011
位置误差定义为/>
Figure BDA00026578563700002012
速度误差定义为/>
Figure BDA00026578563700002013
时钟偏差误差定义
Figure BDA00026578563700002014
Figure BDA00026578563700002015
表示速度伪距率观测矩阵Π的转置；
当估计计算卫星观测和几何距离时，已知卫星的位置和速度，从而可以获得注入项算子计算式为：
Figure BDA00026578563700002016
Figure BDA00026578563700002017
其中，伪距噪声定界为，
Figure BDA00026578563700002018
Figure BDA00026578563700002019
表示位置误差下界值，伪距率噪声定界为，/>
Figure BDA00026578563700002020
Figure BDA00026578563700002021
表示伪距率上界值。
步骤四、将BeiDou/SINS紧组合导航系统的无人机平移运动观测器模型转化为BeiDou/SINS紧组合导航系统的LTV误差动力学模型，并对BeiDou/SINS紧组合导航系统的LTV误差动力学模型进行弹性参数整定和变换，得到BeiDou/SINS紧组合导航系统的LTV误差状态变量的Riccati方程；
首先对无人机平移运动观测器模型实施系统误差LTV动力学模型变换。定义误差状态变量
Figure BDA0002657856370000211
采用Taylor级数分别对注入项算子计算式进行线性化，可得误差状态方程：
eρ,i＝Cρ,ix+ερ,i (22)，
eν,i＝Cν,ix+εν,i (23)，
其中，Cρ,i、Cν,i均表示时变矩阵，且时变矩阵Cρ,i和Cν,i的行数为2m；
将时变矩阵Cρ,i和Cν,i级联为综合时变矩阵C为，C:＝[Cρ,1；…；Cρ,m；Cv,1；…；Cv,m]，其中
Figure BDA0002657856370000212
视线向量估计为/>
Figure BDA0002657856370000213
规范化的相对速度估计为/>
Figure BDA0002657856370000214
所述综合时变矩阵C可表示为：
Figure BDA0002657856370000215
其中，
Figure BDA0002657856370000216
Dρ＝[Г1,…,Гm]，Dρ＝[Π1,…,Πm]；
时变矩阵C在当前时刻是已知的，根据综合时变矩阵C可知BeiDou/SINS紧组合导航系统的LTV误差动力学模型为：
Figure BDA0002657856370000217
其中，矩阵
Figure BDA0002657856370000218
矩阵/>
Figure BDA0002657856370000219
ρ1(t,x)、ρ2(t,χ)和ρ3(t,x)均为干扰项，/>
Figure BDA00026578563700002112
表示LTV系统状态变量微分，x表示LTV系统状态变量，t表示连续系统时间变量；
所述干扰项ρ1(t,x)、ρ2(t,χ)和ρ3(t,x)分别为：
Figure BDA00026578563700002110
Figure BDA00026578563700002111
ρ3(t,x)＝Kε(t,x) (27)，
其中，
Figure BDA0002657856370000221
ε(t,x)＝[ερ,1；…；ερ,m；εv,1；…；εv,m]是误差状态变量函数，θ≥1弹性增益选择系数。
为确保LTV误差动力学方程的收敛性，引入非奇异状态变换矩阵Lθ：
Figure BDA0002657856370000222
其中，I3表示3维单位矩阵，In表示n维单位矩阵；
根据非奇异状态变换矩阵Lθ将则LTV误差状态变量变换为：
η＝Lθx (29)；
对于任意的矩阵K0∈R(9+n)×2m，在给定弹性增益选择系数θ≥1的θ常值参数设置下，设置
Figure BDA0002657856370000223
其中时变矩阵Eθ∈R2m×2m满足EθC＝CLθ，那么LTV误差动力学方程可变换为：
Figure BDA0002657856370000224
由于速度误差项
Figure BDA0002657856370000225
可能会超出确定限界，时变矩阵C可能不是定界的，此时令K0＝PCTR-1，获得误差系统状态方差矩阵的Riccati方程为：
Figure BDA0002657856370000226
步骤五、利用SINS滤波原理对BeiDou/SINS紧组合导航系统的LTV误差动力学模型进行离散化，得到BeiDou/SINS紧组合导航系统的误差状态变量的观测量离散化方程；
利用传统的SINS滤波原理，观测器状态向量是辅助参考向量和SINS测量向量间的误差向量以及有色噪声向量组成，定义SINS误差向量δx*为：
Figure BDA0002657856370000227
其中，δP表示位置误差，δV表示速度误差，δΨ表示辅助变量误差，Pe表示位置矢量，Ve表示速度矢量，Ψ表示辅助变量，
Figure BDA0002657856370000228
表示SINS计算的位置，/>
Figure BDA0002657856370000229
表示SINS计算的速度，ΨSINS表示SINS计算的辅助变量；离散化的SINS误差向量可表示为：
Figure BDA0002657856370000231
其中，z[k]表示紧组合模式下的钟差误差向量，z[k]＝β[k]；从而LTV误差动力学模型可离散化表示为：
δx[k]＝Ad[k-1]δx[k-1]-Rd[k-1]w[k-1] (34)，
δy[k]＝C[k]δx[k]-εpv[k] (35)，
其中，w[k-1]的表示式为，
Figure BDA0002657856370000232
矩阵C[k]∈R4×m的离散化表达式为，
Figure BDA0002657856370000233
定义SINS状态向量为，
Figure BDA0002657856370000234
那么BeiDou/SINS紧组合导航系统的误差状态变量的观测量离散化方程为，/>
Figure BDA0002657856370000235
步骤六、对BeiDou/SINS紧组合导航系统的误差状态变量的观测量离散化方程进行LTV误差状态估计，根据LTV误差状态估计对LTV误差状态变量进行更新。
误差状态向量时间更新为：
δx-[k]＝Ad[k-1]δx+[k-1] (38)，
其中，δx-[k]表示k时刻的系统状态预测值，δx+[k-1]表示k-1时刻的系统状态估计值；
误差状态向量方差矩阵时间更新计算为，
Figure BDA0002657856370000236
其中，
Figure BDA0002657856370000237
Qd[k-1]＝Qδt，δt＝t[k]-t[k-1]，P-[k]表示k时刻的预测系统状态误差方差阵，P+[k-1]表示k-1时刻的估计系统状态误差方差，Bd[k-1]表示k-1时刻的噪声系数矩阵；；
将误差状态向量和预测方差矩阵修订为，
Figure BDA0002657856370000241
其中，
Figure BDA0002657856370000242
表示k时刻的系统状态估计，δx-[k]表示k时刻的系统状态预测，P+[k]表示k时刻的系统状态估计方差矩阵；
实施SINS系统状态向量修正计算，
Figure BDA0002657856370000243
其中，
Figure BDA0002657856370000244
表示k时刻由SINS计算的系统状态，/>
Figure BDA0002657856370000245
表示k时刻由SINS计算的系统状态估计方差矩阵，/>
Figure BDA0002657856370000246
表示k时刻估计方差矩阵修订量，/>
Figure BDA0002657856370000247
表示k时刻由SINS计算的系统速度，/>
Figure BDA0002657856370000248
表示k时刻由SINS计算的系统速度修订量，ΨSINS[k]表示k时刻由SINS计算的系统辅助变量，δΨ+[k]表示k时刻由SINS计算的系统辅助变量修订量；
把误差状态变量重置为：
Figure BDA0002657856370000249
考虑BieDou系统信号误差的时间更新计算为，
z-[k]＝Λz[k-1]z+[k-1], Λz[k-1]＝eFδt (43)，
其中，
Figure BDA00026578563700002410
表示BeiDou系统信号k时刻的估计值，z-[k]表示BeiDou系统信号k时刻的预测值，Λz[k-1]表示BeiDou信号状态转移矩阵，z+[k-1]表示BeiDou系统信号k-1时刻的估计值，eFδt表示BeiDou信号状态转移矩阵计算表达式；；
部分误差变量
Figure BDA00026578563700002411
满足以下方程式，
Figure BDA00026578563700002412
其中，K*[k]表示k时刻的迭代增益矩阵；
由于(x*)+[k-1]≡0，对式(43)实施传统的Kalman滤波更新计算为，
Figure BDA0002657856370000251
其中，Kz[k]表示BeiDou信号的Kalman滤波更新增益矩阵，C[k]表示系统状态变量误差系数矩阵，H[k]表示观测方程一阶偏微分矩阵。
应用实例
为了验证本发明提出的BeiDou/SINS紧组合模式下的无人机导航定位系统的弹性观测器模型算法的有效性及其计算优势，这里给出仿真验证测试数据。首先本发明假设惯性组件IMU和BeiDou接收机子系统的测量数据包括位置和速度数据具有有色普常数特性的白噪声干扰误差，考虑系统弹性增益系数及其增益矩阵是时变性的，最终仿真数据都转换为NED坐标系中显示出来，从ECEF坐标系转换到NED坐标系需要从位置估计数据
Figure BDA0002657856370000252
获得无人机在NED坐标系中的经度/>
Figure BDA0002657856370000253
和纬度/>
Figure BDA0002657856370000254
估计数据，利用四元数/>
Figure BDA0002657856370000255
其中
Figure BDA0002657856370000256
IMU组件的噪声特性表现为，陀螺仪偏差噪声满足εω～n(0,0.00252)，加速度计噪声满足εf～n(0,0.052)；非线性观测器模型参数设置为，k1＝0.25，k2＝0.75，kI＝0.004，BeiDou接收机的位置观测噪声为
Figure BDA0002657856370000257
另外在NED坐标系中BeiDou接收机测量的包含噪声误差的位置和速度数据可表示为，
Figure BDA0002657856370000258
那么BeiDou接收机测量的位置和速度误差模型参数中
F＝blockdiag(FP,FV),G＝blockdiag(GP,GV) (47)
且满足FP＝-1/TP·I3,FV＝-1/TV·I3，GP＝diag(1.2,0.7,2),GV＝diag(1,1,2)，位置误差相关时间常数TP＝1100s，速度时间常数设置为TV＝2s，应该清楚的是BeiDou导航系统测量的无人机位置数据中水平测量比垂直方向测量的位置数据精确，且在高纬度地区东向位置测量比北向位置数据精确。假设无人机启动时的航向角速率维持常数，纵摇角和横摇角分别为φ＝-3°，θ＝2°。通过仿真计算获得的BeiDou/SINS紧组合模式下的导航定位系统中无人机(UAV)位置计算误差数据如附图3所示以及位置计算数据如附图4所示，相应的无人机速度计算数据如附图5和姿态计算数据如附图6所示。仿真数据验证了本发明的BeiDou/SINS松组合系统弹性观测器模型算法的计算效能，和常规EKF算法开展相比，很明显，本发明提出的弹性观测器模型算法的计算精度明显优于常规的EKF算法，并且位置量估计误差获得明显改善且曲线平滑稳定，并且速度误差量收敛很快，导航效果稳定。
以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (8)

1.一种BeiDou/SINS紧组合导航系统弹性模型设计方法，其特征在于，其步骤如下：
步骤一、根据全源弹性BeiDou/SINS紧组合导航系统多源传感器配置，设计紧组合模式无人机载体有界姿态模型方程，并根据多源传感器中磁力计传感设备测量的数据计算注入项算子，根据多源传感器中陀螺仪测量的数据计算陀螺仪偏差；
步骤二、分别对注入项算子和陀螺仪偏差进行离散化计算，根据注入项算子和陀螺仪偏差的离散化结果对紧组合模式无人机载体有界姿态模型方程进行离散化计算；
步骤三、根据代数法计算紧组合导航系统中卫星的伪距观测量数据，并引入辅助向量构建BeiDou/SINS紧组合导航系统的无人机平移运动观测器模型；
步骤四、将BeiDou/SINS紧组合导航系统的无人机平移运动观测器模型转化为BeiDou/SINS紧组合导航系统的LTV误差动力学模型，并对BeiDou/SINS紧组合导航系统的LTV误差动力学模型进行弹性参数整定和变换，得到BeiDou/SINS紧组合导航系统的LTV误差状态变量的Riccati方程；
所述将BeiDou/SINS紧组合导航系统的无人机平移运动观测器模型转化为BeiDou/SINS紧组合导航系统的LTV误差动力学模型的方法为：
定义误差状态变量
Figure FDA0004067963560000011
采用Taylor级数分别对注入项算子计算式进行线性化，可得误差状态方程：
eρ,i＝Cρ,ix+ερ,i，
eν,i＝Cν,ix+εν,i，
其中，Cρ,i、Cν,i均表示时变矩阵，且时变矩阵Cρ,i和Cν,i的行数为2m；
Figure FDA0004067963560000012
为位置误差，/>
Figure FDA0004067963560000013
为速度误差，/>
Figure FDA0004067963560000014
为加速度计比力误差，/>
Figure FDA0004067963560000015
为时钟偏差误差；eρ,i表示伪距注入项算子信号，ev,i表示伪距率注入项算子信号；ερ,i为伪距噪声，εν,i为伪距率噪声；
将时变矩阵Cρ,i和Cν,i级联为综合时变矩阵C为，C:＝[Cρ,1；…；Cρ,m；Cv,1；…；Cv,m]，其中
Figure FDA0004067963560000016
视线向量估计为/>
Figure FDA0004067963560000017
规范化的相对速度估计为/>
Figure FDA0004067963560000018
Гi表示伪距测量的观测噪声误差矩阵；/>
Figure FDA0004067963560000019
表示速度伪距率观测矩阵Π的转置；/>
Figure FDA00040679635600000110
为无人机参考位置矢量，/>
Figure FDA00040679635600000111
为每一颗卫星的视线向量，Pi e为第i颗卫星的位置，/>
Figure FDA00040679635600000112
表示估计观测值；/>
Figure FDA00040679635600000113
为第i颗卫星的速度；
所述综合时变矩阵C可表示为：
Figure FDA0004067963560000021
其中，
Figure FDA0004067963560000022
Dρ＝[Г1,…,Гm]，Dv＝[Π1,…,Πm]；
根据综合时变矩阵C构建BeiDou/SINS紧组合导航系统的LTV误差动力学模型为：
Figure FDA0004067963560000023
/>
其中，矩阵
Figure FDA0004067963560000024
矩阵/>
Figure FDA0004067963560000025
ρ1(t,x)、ρ2(t,χ)和ρ3(t,x)均为干扰项，/>
Figure FDA0004067963560000026
表示LTV系统状态变量微分，x表示LTV系统状态变量，t表示连续系统时间变量，I3表示3维单位矩阵；/>
Figure FDA0004067963560000027
表示位置伪距增益系数，/>
Figure FDA0004067963560000028
表示位置速度增益系数，
Figure FDA0004067963560000029
表示速度伪距增益系数，/>
Figure FDA00040679635600000219
表示伪距率增益系数，/>
Figure FDA00040679635600000210
表示辅助变量伪距增益系数，
Figure FDA00040679635600000211
表示辅助变量伪距率增益系数，/>
Figure FDA00040679635600000212
表示钟差伪距增益系数，/>
Figure FDA00040679635600000213
表示钟差伪距率增益系数；
干扰项ρ1(t,x)、ρ2(t,χ)和ρ3(t,x)分别为：
Figure FDA00040679635600000214
Figure FDA00040679635600000215
ρ3(t,x)＝Kε(t,x)，
其中，
Figure FDA00040679635600000216
ε(t,x)＝[ερ,1；…；ερ,m；εv,1；…；εv,m]是误差状态变量函数，θ≥1弹性增益选择系数，R(·)表示旋转矩阵；/>
Figure FDA00040679635600000217
表示地球自转角速度；
所述对BeiDou/SINS紧组合导航系统的LTV误差动力学模型进行弹性参数整定和变换，得到BeiDou/SINS紧组合导航系统的LTV误差状态变量的Riccati方程的方法为：
为确保LTV误差动力学方程的收敛性，引入非奇异状态变换矩阵Lθ：
Figure FDA00040679635600000218
其中，I3表示3维单位矩阵，In表示n维单位矩阵；θ为弹性增益选择系数；
利用非奇异状态变换矩阵Lθ将LTV误差状态变量变换为：
η＝Lθx；
对于任意的矩阵K0∈R(9+n)×2m，在给定弹性增益选择系数θ≥1的θ常值参数设置下，设置
Figure FDA0004067963560000031
其中时变矩阵Eθ∈R2m×2m满足EθC＝CLθ，那么LTV误差动力学方程可变换为：
Figure FDA0004067963560000032
由于速度误差项
Figure FDA0004067963560000033
可能会超出确定限界，时变矩阵C可能不是定界的，此时令K0＝PCTR-1，获得误差系统状态方差矩阵的Riccati方程为：/>
Figure FDA0004067963560000034
步骤五、利用SINS滤波原理对BeiDou/SINS紧组合导航系统的LTV误差动力学模型进行离散化，得到BeiDou/SINS紧组合导航系统的误差状态变量的观测量离散化方程；
步骤六、对BeiDou/SINS紧组合导航系统的误差状态变量的观测量离散化方程进行LTV误差状态估计，根据LTV误差状态估计对LTV误差状态变量进行更新。
2.根据权利要求1所述的BeiDou/SINS紧组合导航系统弹性模型设计方法，其特征在于，所述紧组合模式无人机载体有界姿态模型方程为：
Figure FDA0004067963560000035
其中，
Figure FDA0004067963560000036
表示载体坐标系b相对于ECEF坐标系的旋转四元数微分，/>
Figure FDA0004067963560000037
表示无人机从载体坐标系b到ECEF坐标系的旋转姿态四元数，/>
Figure FDA0004067963560000038
表示陀螺仪测量角速度，/>
Figure FDA0004067963560000039
表示陀螺仪偏差，/>
Figure FDA00040679635600000310
表示扩展注入项算子，/>
Figure FDA00040679635600000311
表示地球自转角速度ωie的四维扩展向量，/>
Figure FDA00040679635600000312
表示陀螺仪偏差随机游走量，/>
Figure FDA00040679635600000313
表示陀螺仪测量偏差，/>
Figure FDA00040679635600000314
表示注入项算子，/>
Figure FDA00040679635600000315
表示陀螺仪偏差定界值，Proj(·)表示投影模型，/>
Figure FDA00040679635600000316
表示比力测量值，/>
Figure FDA00040679635600000317
表示旋转矩阵的转置矩阵，
Figure FDA00040679635600000318
表示饱和算子，κ1表示在非线性姿态观测器组合效应作用下的陀螺仪角速率偏差估计弹性增益系数，κ2表示注入项算子/>
Figure FDA0004067963560000041
的各个观测/参考矢量对的弹性比例系数，/>
Figure FDA0004067963560000042
表示载体坐标系b下的磁力计测量值，me表示地球磁场参考矢量，/>
Figure FDA0004067963560000043
表示加速度计的比力矢量；
分别对比力测量值
Figure FDA0004067963560000044
加速度计的比力矢量/>
Figure FDA0004067963560000045
载体坐标系b下的磁力计测量值
Figure FDA0004067963560000046
地球磁场参考矢量me进行规范化计算，得到：
Figure FDA0004067963560000047
其中，f b表示载体坐标系下的规范化比力，f e表示ECEF坐标系下的规范化测量比力，m b表示载体坐标系下的规范化磁力计测量值，m e表示ECEF坐标系下的规范化磁力计；
注入项算子
Figure FDA0004067963560000048
的规范化形式为：
Figure FDA0004067963560000049
3.根据权利要求2所述的BeiDou/SINS紧组合导航系统弹性模型设计方法，其特征在于，对注入项算子
Figure FDA00040679635600000410
进行离散化计算的方法为：/>
Figure FDA00040679635600000411
若i＝1，
Figure FDA00040679635600000412
执行/>
Figure FDA00040679635600000413
计算，
Figure FDA00040679635600000414
否则，
Figure FDA00040679635600000415
若i＝2，
Figure FDA00040679635600000416
执行/>
Figure FDA00040679635600000417
计算，
Figure FDA00040679635600000418
否则，
Figure FDA00040679635600000419
其中，δtacc表示加速度计可用时的采样时间间隔，δtmag表示磁力计可用时的采样时间间隔，T表示积分间隔，
Figure FDA00040679635600000420
表示k时刻的总的注入项算子，/>
Figure FDA00040679635600000421
表示k时刻的比力计算的注入项部分，/>
Figure FDA00040679635600000422
表示k时刻的磁力计测量的注入项部分，k1(k)表示k时刻的增益系数，f b(k)表示k时刻的载体坐标系下的比力规范值，/>
Figure FDA00040679635600000423
表示k-1时刻的四元数表述的旋转矩阵，f e(k)表示ECEF坐标系下的k时刻的比力规范化值，k表示时刻，m b(k)表示载体坐标系下的k时刻的磁力计规范化值，m e(k)表示ECEF坐标系下的k时刻的磁力计规范化值；
对陀螺仪测量偏差
Figure FDA0004067963560000051
进行离散化计算的方法为：
将陀螺仪测量偏差
Figure FDA0004067963560000052
的投影模型转化为，
Figure FDA0004067963560000053
其中，
Figure FDA0004067963560000054
表示注入项最小化取值算子，Mb是陀螺仪测量角速率偏差的上界；
陀螺仪测量偏差
Figure FDA0004067963560000055
的离散化表达式为，
Figure FDA0004067963560000056
其中，
Figure FDA0004067963560000057
表示k时刻的陀螺仪测量偏差计算值，/>
Figure FDA0004067963560000058
表示k-1时刻的陀螺仪测量偏差计算值，κ1(k)表示k时刻的陀螺仪偏差计算的增益系数。
4.根据权利要求3所述的BeiDou/SINS紧组合导航系统弹性模型设计方法，其特征在于，所述根据注入项算子和陀螺仪偏差的离散化结果对紧组合模式无人机载体有界姿态模型方程进行离散化计算的方法为：
Figure FDA0004067963560000059
其中，
Figure FDA00040679635600000510
表示斜对称矩阵的指数计算，/>
Figure FDA00040679635600000511
表示负斜对称矩阵的指数计算，/>
Figure FDA00040679635600000512
表示k-1时刻的计算角速度，/>
Figure FDA00040679635600000513
表示k时刻的角速度，/>
Figure FDA00040679635600000514
表示角速度，/>
Figure FDA00040679635600000515
表示地球自转角速度，I4表示4维单位阵。
5.根据权利要求4所述的BeiDou/SINS紧组合导航系统弹性模型设计方法，其特征在于，所述根据代数法计算紧组合导航系统中卫星的伪距观测量数据的方法为：
S31、无人机参考位置矢量
Figure FDA00040679635600000516
每一颗卫星的视线向量/>
Figure FDA00040679635600000517
四颗卫星的伪距观测量为ρ1、ρ2、ρ3和ρ4，且其中前三颗卫星的视线向量/>
Figure FDA00040679635600000518
和/>
Figure FDA00040679635600000519
都是相互线性独立，前三颗卫星的视线向量组成视线矩阵/>
Figure FDA0004067963560000061
满足/>
Figure FDA0004067963560000062
设置伪距观测噪声误差矩阵Г＝1，定义辅助变量/>
Figure FDA0004067963560000063
S32、由四颗卫星的伪距观测量和视线向量组成矩阵
Figure FDA0004067963560000064
则/>
Figure FDA0004067963560000065
辅助变量z表达式为：
Figure FDA0004067963560000066
其中，
Figure FDA0004067963560000067
表示由向量/>
Figure FDA0004067963560000068
计算的转换向量，/>
Figure FDA0004067963560000069
表示由向量/>
Figure FDA00040679635600000610
计算的转换向量，
Figure FDA00040679635600000611
表示观测距离向量，/>
Figure FDA00040679635600000612
表示单位向量，
Figure FDA00040679635600000613
表示视线向量，/>
Figure FDA00040679635600000614
且/>
Figure FDA00040679635600000615
表示视线距离矢量，i'＝1,2,3,4，M＝diag(1,1,1，-1)表示对角阵。
6.根据权利要求5所述的BeiDou/SINS紧组合导航系统弹性模型设计方法，其特征在于，所述BeiDou/SINS紧组合导航系统的无人机平移运动观测器模型为：
Figure FDA00040679635600000616
其中，
Figure FDA00040679635600000617
表示无人机平移运动的位置矢量，/>
Figure FDA00040679635600000618
表示机器人平移运动的速度向量，/>
Figure FDA00040679635600000619
表示加速度计比力估计向量，Ψ表示辅助向量，/>
Figure FDA00040679635600000620
表示位置微分，/>
Figure FDA00040679635600000621
表示速度微分，/>
Figure FDA00040679635600000622
表示辅助变量微分，/>
Figure FDA00040679635600000623
表示钟差微分，eρ,i表示伪距注入项算子信号，ev,i表示伪距率注入项算子信号，m表示可观测卫星数，/>
Figure FDA00040679635600000624
表示计算位置表达的重力矢量，/>
Figure FDA00040679635600000625
表示位置伪距增益系数，/>
Figure FDA00040679635600000626
表示位置速度增益系数，/>
Figure FDA00040679635600000627
表示速度伪距增益系数，/>
Figure FDA00040679635600000628
表示伪距率增益系数，
Figure FDA00040679635600000629
表示辅助变量伪距增益系数，/>
Figure FDA00040679635600000630
表示辅助变量伪距率增益系数，/>
Figure FDA00040679635600000631
表示钟差伪距增益系数，/>
Figure FDA0004067963560000071
表示钟差伪距率增益系数，/>
Figure FDA0004067963560000072
表示地球自转角速度；
伪距/伪距率注入项算子信号eρ,i、ev,i分别为：
Figure FDA0004067963560000073
则估计观测值的表达式为：
Figure FDA0004067963560000074
Figure FDA0004067963560000075
其中，ρi表示观测值，
Figure FDA0004067963560000076
表示估计观测值，vi表示观测值，/>
Figure FDA0004067963560000077
表示估计观测值，/>
Figure FDA0004067963560000078
是伪距误差建模参数，Г表示伪距测量的观测噪声误差矩阵；第i颗卫星的位置和速度分别表示为Pi e和/>
Figure FDA00040679635600000720
第i颗卫星和接收机间的几何距离为/>
Figure FDA0004067963560000079
加速度计比力误差/>
Figure FDA00040679635600000710
位置误差定义为/>
Figure FDA00040679635600000711
速度误差定义为/>
Figure FDA00040679635600000712
时钟偏差误差定义/>
Figure FDA00040679635600000713
Figure FDA00040679635600000722
表示速度伪距率观测矩阵Π的转置；
当估计计算卫星观测和几何距离时，已知卫星的位置和速度，可以获得注入项算子计算式为：
Figure FDA00040679635600000714
Figure FDA00040679635600000715
/>
其中，伪距噪声定界为，
Figure FDA00040679635600000716
Figure FDA00040679635600000719
表示位置误差下界值，伪距率噪声定界为，
Figure FDA00040679635600000717
Figure FDA00040679635600000721
表示伪距率上界值。
7.根据权利要求6所述的BeiDou/SINS紧组合导航系统弹性模型设计方法，其特征在于，所述利用SINS滤波原理对BeiDou/SINS紧组合导航系统的LTV误差动力学模型进行离散化，得到BeiDou/SINS紧组合导航系统的误差状态变量的观测量离散化方程的方法为：
利用传统的SINS滤波原理，观测器状态向量是辅助参考向量和SINS测量向量间的误差向量以及有色噪声向量组成，定义SINS误差向量δx*为：
Figure FDA00040679635600000718
其中，δP表示位置误差，δV表示速度误差，δΨ表示辅助变量误差，Pe表示位置矢量，Ve表示速度矢量，Ψ表示辅助变量，
Figure FDA0004067963560000081
表示SINS计算的位置，/>
Figure FDA0004067963560000082
表示SINS计算的速度，ΨSINS表示SINS计算的辅助变量；离散化的SINS误差向量可表示为：
Figure FDA0004067963560000083
其中，z[k]表示紧组合模式下的钟差误差向量，z[k]＝β[k]；从而LTV误差动力学模型相应可离散化表示为：
δx[k]＝Ad[k-1]δx[k-1]-Rd[k-1]w[k-1]，
δy[k]＝C[k]δx[k]-εpv[k]，
其中，w[k-1]的表示式为，
Figure FDA0004067963560000084
矩阵C[k]∈R4×m的离散化表达式为，
Figure FDA0004067963560000085
定义SINS状态向量为，
Figure FDA0004067963560000086
那么BeiDou/SINS紧组合导航系统的误差状态变量的观测量离散化方程为，
Figure FDA0004067963560000087
8.根据权利要求7所述的BeiDou/SINS紧组合导航系统弹性模型设计方法，其特征在于，所述对BeiDou/SINS紧组合导航系统的误差状态变量的观测量离散化方程进行LTV误差状态估计，根据LTV误差状态估计对LTV误差状态变量进行更新的方法为：
误差状态向量时间更新为：
δx-[k]＝Ad[k-1]δx+[k-1]，
其中，δx-[k]表示k时刻的系统状态预测值，δx+[k-1]表示k-1时刻的系统状态估计值；
误差状态向量方差矩阵时间更新计算为，
Figure FDA0004067963560000088
其中，
Figure FDA0004067963560000091
Qd[k-1]＝Qδt，δt＝t[k]-t[k-1]，P-[k]表示k时刻的预测系统状态误差方差阵，P+[k-1]表示k-1时刻的估计系统状态误差方差，Bd[k-1]表示k-1时刻的噪声系数矩阵；
将误差状态向量和预测方差矩阵修订为，
Figure FDA0004067963560000092
其中，
Figure FDA0004067963560000093
表示k时刻的系统状态估计，δx-[k]表示k时刻的系统状态预测，P+[k]表示k时刻的系统状态估计方差矩阵；
实施SINS系统状态向量修正计算，
Figure FDA0004067963560000094
其中，
Figure FDA0004067963560000095
表示k时刻由SINS计算的系统状态，/>
Figure FDA0004067963560000096
表示k时刻由SINS计算的系统状态估计方差矩阵，/>
Figure FDA0004067963560000097
表示估计方差矩阵修订量，/>
Figure FDA0004067963560000098
表示k时刻由SINS计算的系统速度，/>
Figure FDA0004067963560000099
表示k时刻由SINS计算的系统速度修订量，ΨSINS[k]表示k时刻由SINS计算的系统辅助变量，δΨ+[k]表示k时刻由SINS计算的系统辅助变量修订量；
把k时刻的系统状态估计的误差状态变量重置为：
Figure FDA00040679635600000910
考虑BieDou系统信号误差的时间更新计算为，
z-[k]＝Λz[k-1]z+[k-1],Λz[k-1]＝eFδt，
其中，
Figure FDA00040679635600000911
表示BeiDou系统信号k时刻的估计值，z-[k]表示BeiDou系统信号k时刻的预测值，Λz[k-1]表示BeiDou信号状态转移矩阵，z+[k-1]表示BeiDou系统信号k-1时刻的估计值，eFδt表示BeiDou信号状态转移矩阵计算表达式；
部分误差变量
Figure FDA0004067963560000101
满足以下方程式，
Figure FDA0004067963560000102
其中，K*[k]表示k时刻的迭代增益矩阵；
由于(x*)+[k-1]≡0，对误差状态变量表达式实施传统的Kalman滤波更新计算为，
Figure FDA0004067963560000103
其中，Kz[k]表示BeiDou信号的Kalman滤波更新增益矩阵，H[k]表示观测方程一阶偏微分矩阵。
CN202010894005.9A 2020-08-31 2020-08-31 一种BeiDou/SINS紧组合导航系统弹性模型设计方法 Active CN112146655B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010894005.9A CN112146655B (zh) 2020-08-31 2020-08-31 一种BeiDou/SINS紧组合导航系统弹性模型设计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010894005.9A CN112146655B (zh) 2020-08-31 2020-08-31 一种BeiDou/SINS紧组合导航系统弹性模型设计方法

Publications (2)

Publication Number Publication Date
CN112146655A CN112146655A (zh) 2020-12-29
CN112146655B true CN112146655B (zh) 2023-03-31

Family

ID=73890347

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010894005.9A Active CN112146655B (zh) 2020-08-31 2020-08-31 一种BeiDou/SINS紧组合导航系统弹性模型设计方法

Country Status (1)

Country Link
CN (1) CN112146655B (zh)

Families Citing this family (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112683265B (zh) * 2021-01-20 2023-03-24 中国人民解放军火箭军工程大学 一种基于快速iss集员滤波的mimu/gps组合导航方法
CN113050143B (zh) * 2021-06-02 2021-08-10 西北工业大学 一种发射惯性坐标系下的紧耦合导航方法
CN113758489B (zh) * 2021-10-19 2024-03-12 中国电子科技集团公司第五十四研究所 一种基于多源传感器弹性融合的导航定位方法
CN114136315B (zh) * 2021-11-30 2024-04-16 山东天星北斗信息科技有限公司 一种基于单目视觉辅助惯性组合导航方法及系统
CN114563001B (zh) * 2022-03-07 2023-10-24 中国人民解放军61540部队 一种空中重力矢量计算方法及系统
CN114674313B (zh) * 2022-03-31 2023-04-04 淮阴工学院 一种基于ckf算法的gps/bds和sins融合的无人配送车导航定位方法

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6516021B1 (en) * 1999-09-14 2003-02-04 The Aerospace Corporation Global positioning systems and inertial measuring unit ultratight coupling method
CN106932804A (zh) * 2017-03-17 2017-07-07 南京航空航天大学 天文辅助的惯性/北斗紧组合导航系统及其导航方法

Family Cites Families (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20040257441A1 (en) * 2001-08-29 2004-12-23 Geovantage, Inc. Digital imaging system for airborne applications
US8589072B2 (en) * 2011-04-13 2013-11-19 Honeywell International, Inc. Optimal combination of satellite navigation system data and inertial data
CN102506857B (zh) * 2011-11-28 2014-01-22 北京航空航天大学 一种基于双imu/dgps组合的相对姿态测量实时动态滤波方法
US9612341B2 (en) * 2012-12-28 2017-04-04 Trimble Inc. GNSS receiver positioning system
US10775510B2 (en) * 2016-06-06 2020-09-15 Brian G. Agee Blind despreading of civil GNSS signals for resilient PNT applications
CN108678732B (zh) * 2018-05-10 2022-01-18 芜湖航飞科技股份有限公司 一种基于北斗导航技术的三维测绘装置
CN110285810B (zh) * 2019-06-13 2023-07-14 兖矿集团有限公司 一种基于惯性导航数据的采煤机自主定位方法及装置
CN111238467B (zh) * 2020-02-07 2021-09-03 西北工业大学 一种仿生偏振光辅助的无人作战飞行器自主导航方法

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6516021B1 (en) * 1999-09-14 2003-02-04 The Aerospace Corporation Global positioning systems and inertial measuring unit ultratight coupling method
CN106932804A (zh) * 2017-03-17 2017-07-07 南京航空航天大学 天文辅助的惯性/北斗紧组合导航系统及其导航方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
谭天乐 ; 朱春艳 ; 朱东方 ; 宋婷 ; 孙宏丽 ; 顾 ; 杨雨 ; .航天器微振动测试、隔离、抑制技术综述.上海航天.2014,31(06),全文. *

Also Published As

Publication number Publication date
CN112146655A (zh) 2020-12-29

Similar Documents

Publication Publication Date Title
CN112146655B (zh) 一种BeiDou/SINS紧组合导航系统弹性模型设计方法
CN108226980B (zh) 基于惯性测量单元的差分gnss与ins自适应紧耦合导航方法
CN108827310B (zh) 一种船用星敏感器辅助陀螺仪在线标定方法
CN104344837B (zh) 一种基于速度观测的冗余惯导系统加速度计系统级标定方法
Mu et al. A practical INS/GPS/DVL/PS integrated navigation algorithm and its application on Autonomous Underwater Vehicle
CN106500693B (zh) 一种基于自适应扩展卡尔曼滤波的ahrs算法
CN112505737B (zh) 一种gnss/ins组合导航方法
CN104019828A (zh) 高动态环境下惯性导航系统杆臂效应误差在线标定方法
CN111189442B (zh) 基于cepf的无人机多源导航信息状态预测方法
CN103884340A (zh) 一种深空探测定点软着陆过程的信息融合导航方法
CN111982126B (zh) 一种全源BeiDou/SINS弹性状态观测器模型设计方法
Jørgensen et al. Underwater position and attitude estimation using acoustic, inertial, and depth measurements
Guangcai et al. An iterative Doppler velocity log error calibration algorithm based on Newton optimization
CN104634348B (zh) 组合导航中的姿态角计算方法
CN111220151B (zh) 载体系下考虑温度模型的惯性和里程计组合导航方法
CN117053782A (zh) 一种水陆两栖机器人组合导航方法
CN116576849A (zh) 一种基于gmm辅助的车辆融合定位方法及系统
Condomines Nonlinear Kalman Filter for Multi-Sensor Navigation of Unmanned Aerial Vehicles: Application to Guidance and Navigation of Unmanned Aerial Vehicles Flying in a Complex Environment
CN116222541A (zh) 利用因子图的智能多源组合导航方法及装置
CN116222551A (zh) 一种融合多种数据的水下导航方法及装置
CN113503891B (zh) 一种sinsdvl对准校正方法、系统、介质及设备
CN113324539A (zh) 一种sins/srs/cns多源融合自主组合导航方法
CN111473786A (zh) 一种基于局部反馈的两层分布式多传感器组合导航滤波方法
Zhang et al. An improved Kalman filter for attitude determination of multi-rotor UAVs based on low-cost MEMS sensors
Zhang et al. Calibration method of DVL based on position observation information

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
TR01 Transfer of patent right
TR01 Transfer of patent right

Effective date of registration: 20231130

Address after: Room 501, 5th Floor, Building 17, East Jindian Science and Technology Industrial Park, Southwest Corner of Fuxing Road and Juxiang Road Intersection, Xincheng District, Pingdingshan City, Henan Province, 467036

Patentee after: Indufei Intelligent Equipment Co.,Ltd.

Address before: 450002 No. 5 Dongfeng Road, Jinshui District, Henan, Zhengzhou

Patentee before: Zhengzhou University of light industry