CN111982126A - 一种全源BeiDou/SINS弹性状态观测器模型设计方法 - Google Patents

一种全源BeiDou/SINS弹性状态观测器模型设计方法 Download PDF

Info

Publication number
CN111982126A
CN111982126A CN202010894489.7A CN202010894489A CN111982126A CN 111982126 A CN111982126 A CN 111982126A CN 202010894489 A CN202010894489 A CN 202010894489A CN 111982126 A CN111982126 A CN 111982126A
Authority
CN
China
Prior art keywords
representing
matrix
model
beidou
vector
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
CN202010894489.7A
Other languages
English (en)
Other versions
CN111982126B (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 CN202010894489.7A priority Critical patent/CN111982126B/zh
Publication of CN111982126A publication Critical patent/CN111982126A/zh
Application granted granted Critical
Publication of CN111982126B publication Critical patent/CN111982126B/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/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
    • G01S19/49—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an inertial position system, e.g. loosely-coupled

Abstract

本发明提出了一种全源BeiDou/SINS弹性状态观测器模型设计方法，利用全源弹性PNT服务体系的导航定位系统概念框架，以无人飞行器平台应用的全源弹性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组合导航系统模型设计复杂性的技术问题，利用全源弹性PNT服务体系的导航定位系统概念框架，本发明提出以无人飞行器平台应用的全源弹性PNT导航定位系统为对象，以精确SINS子系统为核心，以多源BeiDou导航系统、磁力计等导航设备提出了一种全源BeiDou/SINS弹性状态观测器模型设计方法，设计全源弹性PNT组合导航系统姿态旋转和平移运动非线性弹性观测器方程，把多源导航设备数据构建融合为系统模型方程的注入项算子和弹性修正函数，提出新型全源弹性PNT导航定位系统的级联式非线性姿态-平移运动弹性观测器理论与算法解决方案，满足无人飞行器运动对象的复杂应用环境下全源弹性PNT组合导航定位系统快速精确计算的技术性能要求。
本发明的技术方案是这样实现的：
一种全源BeiDou/SINS弹性状态观测器模型设计方法，其步骤如下：
步骤一、根据BeiDou/SINS组合导航系统多源传感器配置，设计松组合模式无人机载体有界姿态模型方程，并根据多源传感器中磁力计传感设备测量的数据计算注入项算子，根据多源传感器中陀螺仪测量的数据计算陀螺仪偏差；
步骤二、分别对注入项算子和陀螺仪偏差进行离散化计算，根据注入项算子和陀螺仪偏差的离散化结果对松组合模式无人机载体有界姿态模型方程进行离散化计算；
步骤三、构建BeiDou接收机位置和速度测量误差模型，并引入辅助向量构建BeiDou/SINS组合导航系统的无人机平移运动观测器模型；
步骤四、根据BeiDou/SINS组合导航系统的无人机平移运动观测器模型构建BeiDou/SINS组合导航系统的连续线性系统模型，并将BeiDou/SINS组合导航系统的连续线性系统模型转化为平移运动观测器的LTV模型；
步骤五、根据平移运动观测器的LTV模型预测平移运动观测器的LTV模型系统状态变量的预测值；
步骤六、根据平移运动观测器的LTV模型系统状态变量的预测值对平移运动观测器的LTV模型系统状态变量进行更新。
所述松组合模式无人机载体有界姿态模型方程为：
Figure BDA0002658000780000031
其中，
Figure BDA0002658000780000032
表示载体坐标系相对于ECEF系的旋转四元数微分，
Figure BDA0002658000780000033
表示无人机从载体坐标系b到ECEF系的旋转姿态，
Figure BDA0002658000780000034
表示陀螺仪测量角速度，
Figure BDA0002658000780000035
表示陀螺仪偏差，
Figure BDA0002658000780000036
表示扩展注入项算子，
Figure BDA0002658000780000037
表示地球自转角速度ωie的四维扩展向量，
Figure BDA0002658000780000038
表示陀螺仪偏差随机游走量，
Figure BDA0002658000780000039
表示陀螺仪测量偏差，
Figure BDA00026580007800000310
表示注入项算子，
Figure BDA00026580007800000311
表示陀螺仪偏差定界值，Proj(·)表示投影模型，
Figure BDA00026580007800000312
表示比力测量值，
Figure BDA00026580007800000313
表示旋转矩阵，
Figure BDA00026580007800000314
表示饱和算子，κ1表示陀螺仪角速率偏差估计弹性增益系数，κ2表示注入项算子
Figure BDA00026580007800000315
的各个观测/参考矢量对的弹性比例系数，
Figure BDA00026580007800000316
表示载体系下的磁力计测量，me表示地球磁场参考矢量，
Figure BDA00026580007800000317
表示加速度计的比力矢量；
分别对比力测量值
Figure BDA00026580007800000318
加速度计的比力矢量
Figure BDA00026580007800000319
载体系下的磁力计测量值
Figure BDA00026580007800000320
地球磁场参考矢量me进行规范化计算，得到：
Figure BDA00026580007800000321
其中，f b表示载体系下的规范化比力，f e表示ECEF系下的规范化测量比力，m b表示载体系下的规范化磁力计测量值，m e表示ECEF系下的规范化磁力计测量值；
注入项算子
Figure BDA00026580007800000322
的规范化形式为：
Figure BDA00026580007800000323
所述对注入项算子
Figure BDA00026580007800000324
进行离散化计算的方法为：
Figure BDA00026580007800000325
若i＝1，
Figure BDA00026580007800000326
执行
Figure BDA00026580007800000327
计算，
Figure BDA00026580007800000328
否则，
Figure BDA0002658000780000041
若i＝2，
Figure BDA0002658000780000042
执行
Figure BDA0002658000780000043
计算，
Figure BDA0002658000780000044
否则，
Figure BDA0002658000780000045
其中，δtacc表示加速度计可用时的采样时间间隔，δtmag表示磁力计可用时的采样时间间隔，T表示积分间隔，
Figure BDA0002658000780000046
表示k时刻的总的注入项算子，
Figure BDA0002658000780000047
表示k时刻的比力计算的注入项部分，
Figure BDA0002658000780000048
表示k时刻的磁力计测量的注入项部分，k1(k)表示k时刻的增益系数，f b(k)表示k时刻的载体系下的比力规范值，
Figure BDA0002658000780000049
表示k-1时刻的四元数表述的旋转矩阵，f e(k)表示ECEF系下的k时刻的比力规范化值，k表示时刻，m b(k)表示载体系下的k时刻的磁力计规范化测量值，m e(k)表示ECEF系下的k时刻的磁力计规范化测量值；
所述对陀螺仪偏差
Figure BDA00026580007800000410
进行离散化计算的方法为：
将陀螺仪偏差
Figure BDA00026580007800000411
的投影模型转化为：
Figure BDA00026580007800000412
其中，
Figure BDA00026580007800000413
表示注入项最小化取值算子，Mb是陀螺仪测量角速率偏差的上界，则陀螺仪偏差
Figure BDA00026580007800000414
的离散化表达式为：
Figure BDA00026580007800000415
其中，
Figure BDA00026580007800000416
表示k时刻的陀螺仪偏差计算值，
Figure BDA00026580007800000417
表示k-1时刻的陀螺仪偏差计算值，κ1(k)表示k时刻的陀螺仪偏差计算的增益系数，I3表示3维单位向量。
所述根据注入项算子和陀螺仪偏差的离散化结果对松组合模式无人机载体有界姿态模型方程进行离散化计算的方法为：
Figure BDA00026580007800000418
其中，
Figure BDA00026580007800000419
表示斜对称矩阵的指数计算，
Figure BDA0002658000780000051
表示负斜对称矩阵的指数计算，
Figure BDA0002658000780000052
表示k时刻的计算角速度，
Figure BDA0002658000780000053
表示k时刻的角速度，
Figure BDA0002658000780000054
表示角速度，
Figure BDA0002658000780000055
表示地球自转角速度，
Figure BDA0002658000780000056
表示k-1时刻的角速度，I4表示4维单位阵，sinc()表示反正弦函数。
所述BeiDou接收机位置和速度测量误差模型为：
Figure BDA0002658000780000057
Figure BDA0002658000780000058
其中，
Figure BDA0002658000780000059
表示BeiDou接收机位置向量微分，zp表示BeiDou接收机位置向量，Fp表示位置向量转移矩阵，Gp表示位置噪声矩阵，np表示位置噪声，δp表示无人机载体位置误差观测噪声，Hp表示位置误差转移矩阵，
Figure BDA00026580007800000510
表示位置误差噪声，
Figure BDA00026580007800000511
表示BeiDou接收机速度向量，Fv表示BeiDou接收机测量速度转移矩阵，zv表示速度观测向量，Gv表示速度噪声矩阵，δv表示速度观测噪声，Hv表示速度误差转移矩阵，
Figure BDA00026580007800000512
表示速度误差噪声。
所述BeiDou/SINS组合导航系统的无人机平移运动观测器模型为：
Figure BDA00026580007800000513
其中，
Figure BDA00026580007800000514
表示无人机平移运动的位置微分，
Figure BDA00026580007800000515
表示无人机平移运动的位置，
Figure BDA00026580007800000516
表示无人机平移运动的速度向量，θ表示弹性系数，可取值为1，
Figure BDA00026580007800000517
表示位置变量增益系数，
Figure BDA00026580007800000518
表示BeiDou接收机在ECEF系下的位置向量，
Figure BDA00026580007800000519
表示位置和速度关联增益系数，
Figure BDA00026580007800000520
表示BeiDou接收机在ECEF系下的速度向量，
Figure BDA00026580007800000521
表示扩展的BeiDou接收机测量的垂向位置变量微分，
Figure BDA00026580007800000522
表示扩展的BeiDou接收机测量的垂向位置变量，F表示扩展垂向位移转移矩阵，Kpz表示BeiDou接收机位置和扩展垂向变量关联增益系数，
Figure BDA00026580007800000523
表示扩展垂向误差变量，Kvz表示速度与扩展垂向位移关联增益系数，
Figure BDA0002658000780000061
表示扩展垂向速度误差噪声，
Figure BDA0002658000780000062
表示ECEF系下的速度矢量微分，
Figure BDA0002658000780000063
表示地球自转角速度表示的旋转矩阵，
Figure BDA0002658000780000064
表示加速度计比力估计向量，
Figure BDA0002658000780000065
表示计算位置表达的重力矢量，
Figure BDA0002658000780000066
表示位置速度关联增益系数，
Figure BDA0002658000780000067
表示速度增益系数，Ψ表示辅助向量，
Figure BDA0002658000780000068
表示辅助向量微分，
Figure BDA0002658000780000069
表示四元数
Figure BDA00026580007800000610
的旋转矩阵，
Figure BDA00026580007800000611
表示注入项算子表述的旋转矩阵，
Figure BDA00026580007800000612
表示位置和辅助变量关联增益系数，
Figure BDA00026580007800000613
表示速度与辅助变量关联增益系数，
Figure BDA00026580007800000614
表示比力测量值。
所述BeiDou/SINS组合导航系统的连续线性系统模型为：
Figure BDA00026580007800000615
其中，
Figure BDA00026580007800000616
表示BeiDou/SINS组合导航系统的状态变量微分，n表示系统噪声，
Figure BDA00026580007800000617
为BeiDou/SINS组合导航系统的状态变量，
Figure BDA00026580007800000618
为BeiDou/SINS组合导航系统的输入量，状态转移矩阵
Figure BDA00026580007800000619
表示A的子矩阵，
Figure BDA00026580007800000620
表示输入量系数矩阵，
Figure BDA00026580007800000621
表示B1子矩阵，
Figure BDA00026580007800000622
表示过程噪声系数矩阵，B＝(B1,B2)，
Figure BDA00026580007800000623
表示获得的LTV方程中的辅助项；
所述平移运动观测器的LTV模型为：
Figure BDA00026580007800000624
其中，C＝(C*,H)表示观测方程的一阶微分Jaccobian矩阵，
Figure BDA00026580007800000625
表示系数矩阵，
Figure BDA00026580007800000626
表示增益矩阵，
Figure BDA00026580007800000627
表示由系统状态变量间的关联增益系数与弹性系数组建的弹性转移矩阵，Kz表示扩展的弹性转移矩阵，t表示连续系统时间变量，y表示观测量。
所述预测平移运动观测器的LTV模型系统状态变量的方法为：
针对平移运动观测器的LTV模型，LTV模型系统状态变量的下一步预测计算为，
Figure BDA0002658000780000071
其中，x-(k+1)表示k+1时刻的系统状态变量预测，x+(k)表示系统状态变量的上一步估计计算数据，定义矩阵指数函数为Ad，
Figure BDA0002658000780000072
B1(τ)表示输入量系数矩阵积分，u*(τ)表示输入量，D(τ)表示LTV系统方程的辅助项，τ∈[kT,(k+1)T]表示积分时间算子；
在采样间隔时间内比力输入量、姿态旋转矩阵和重力向量均保持常值，也即
Figure BDA0002658000780000073
则定义，
Figure BDA0002658000780000074
可得，
得到Bd(k)＝(Bd,1(k)Bd,2(k))；
定义
Figure BDA0002658000780000075
则平移运动观测器的LTV模型系统状态变量的预测表达式为，
x-(k+1)＝Ad(k)x+(k)+Bd,1(k)u*(k)+D(k)，
平移运动观测器的LTV模型系统状态变量的预测表达式对应的过程噪声方差矩阵迭代计算公式为，
Figure BDA0002658000780000076
其中，Qd(k)＝Q·T；
根据Kalman滤波计算方法计算增益矩阵Kd(k)，
Kd(k)＝P-(k)CT(k)(C(k)P-(k)CT(k)+R(k))-1，
其中，C(k)表示系统观测方程的观测矩阵。
所述根据平移运动观测器的LTV模型系统状态变量的预测值对平移运动观测器的LTV模型系统状态变量进行更新的方法为：
x+(k)＝x-(k)+Kd(k)[y(k)-C(k)x-(k)]，
其中，x+(k)表示k时刻的系统状态变量估计值，x-(k)表示k时刻的系统状态变量预测值，y(k)表示k时刻的系统观测量；
LTV系统状态变量的估计方差矩阵计算为，
P+(k)＝[I9+l-Kd(k)C(k)]P-(k)，
其中，P+(k)表示系统状态变量k时刻的估计误差方差矩阵，P-(k)表示k时刻的预测系统状态误差方差阵，I9+l表示9+l维的单位矩阵。
本技术方案能产生的有益效果：本发明采用了注入项算子策略，充分利用组合系统的多源传感数据，设计无人机载体的姿态计算模型、平移运动观测器模型，利用综合形成LTV系统方程及其系统状态向量方差的Riccati方程，通过姿态计算模型和平移运动模型的交互操作，整定姿态计算模型和平移运动观测器模型中的弹性系数与弹性增益矩阵，采用直接法实现系统姿态和平移向量的快速有效计算，提高BeiDou/SINS松组合导航系统状态参数估计的计算效率，并且有效改善系统状态向量参数的计算精度。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的图作简单地介绍，显而易见地，下面描述中的图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些图获得其他的图。
图1是本发明BeiDou/SINS松组合系统弹性观测器模型结构图。
图2是本发明BeiDou/SINS松组合系统弹性观测器模型算法计算流程图。
图3是本发明的无人机载体位置计算误差数据图。
图4是本发明的无人机载体位置计算数据图。
图5是本发明的无人机载体速度计算数据图。
图6是本发明的无人机载体姿态计算数据图。
具体实施方式
下面将结合本发明实施例中的图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有付出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
无人机运动载体的BeiDou/INS组合导航定位系统主要由BeiDou接收机获得无人机位置和速度信息、IMU组件包括加速度计和陀螺仪以及磁力计测量无人机运动载体的加速度、旋转角速度等信息，在不同坐标系中无人机载体的动力学模型有不同的表达式，如在惯性坐标系(ECI)、在地球协议坐标系(ECEF)、在当地水平坐标系(NED)以及切向坐标系(T系)等，无人机姿态可由不同的姿态角参数表示，如四元数、欧拉角以及罗德里格尔斯参数等，选择ECEF系和四元数构建无人机载体的PVA动力学模型方程，
Figure BDA0002658000780000091
其中，Pe，Ve，fe∈R3分别表示无人机载体在ECEF系下的位置、速度和比力，单位四元数
Figure BDA0002658000780000092
描述无人机从载体坐标系b到ECEF系的旋转姿态，
Figure BDA0002658000780000093
是无人机载体相对于ECI惯性系的旋转角速度的四维扩展向量，
Figure BDA0002658000780000094
表示地球自转角速度ωie的四维扩展向量，
Figure BDA0002658000780000095
表示利用ECEF系下的地球自转角速度组成的斜对称矩阵，ge(Pe)表示无人所在位置的地球重力加速度。利用单位四元数描述的姿态旋转矩阵可表示为，
Figure BDA0002658000780000096
捷联惯性导航系统(SINS)的惯性传感组件IMU中包含了加速度计、陀螺仪以及磁力计等，在载体坐标系b中构建其模型为，
Figure BDA0002658000780000097
其中，
Figure BDA0002658000780000098
是加速度计相对于地球测量的比力经过坐标转换到载体坐标系中的比力矢量，
Figure BDA0002658000780000101
是陀螺仪测量的角速率偏差，加速度计偏差或者漂移已经过在线或者离线标定补偿，磁力计提供无人机航向参考信息，那么惯性组件IMU中的测量误差量
Figure BDA0002658000780000102
BeiDou接收机测量无人机载体位置和速度数据，和惯性导航系统SINS测量数据进行数据融合，一般来说GNSS和SINS组合有松组合、紧组合和超紧组合模式，松组合策略采用无人机位置和速度数据融合，BeiDou接收机位置和速度测量模型在ECEF系中可表示为，
Figure BDA0002658000780000103
其中，δ*表示测量噪声或者测量误差。
为了解决现有BeiDou/SINS组合导航系统模型设计问题，基于非线性稳定性控制理论，本发明提出一类BeiDou/SINS松组合导航系统的弹性观测器模型设计方法，它采用了注入项算子策略，充分利用组合系统的多源传感数据，设计无人机载体的姿态计算模型、平移运动观测器模型，利用综合形成LTV系统方程及其系统状态向量方差的Riccati方程，通过姿态计算模型和平移运动模型的交互操作，整定姿态计算模型和平移运动观测器模型中的弹性系数与弹性增益矩阵，采用直接法实现系统姿态和平移向量的快速有效计算，提高BeiDou/SINS松组合导航系统状态参数估计的计算效率，并且有效改善系统状态向量参数的计算精度。
本发明充分利用无人机载的导航定位传感设备，BeiDou接收机、SINS组件以及磁力计，甚至还有视觉相机和激光雷达等设备有界物理模型，考虑多个传感设备输出的多对非平行观测向量及其参考矢量，构建系统姿态的注入项算子，对无人机姿态计算数据展开弹性修正计算；利用投影原理对陀螺仪偏差向量进行定界计算操作，其主要优势在于多个传感设备感测数据可在姿态计算模型中采用注入项算子形式开放式输入模型对其进行修正计算，可以有效改善无人机姿态计算精度，从而利用四元数构建无人机运动中的高精度弹性姿态计算模型方程。
利用松组合模式中BeiDou接收机观测的无人机位置和速度向量，充分考虑BeiDou接收机观测数据特性，构建BeiDou接收机观测误差模型方程，充分考虑无人机运动状态对加速度计测量的比力影响，设计辅助向量体现位置和速度状态向量对比力影响的计算方程，设计BeiDou/SINS组合导航系统平移运动观测器的位置计算方程和速度计算方程，平移运动观测器模型的状态变量定义为位置、速度和辅助变量，把加速度计比力计算作为输入向量，BeiDou接收机的位置和速度向量作为输出向量，其中引入位置、速度、辅助向量和比力方程的弹性系数，构建平移运动观测器的弹性系数矩阵。综合平移运动观测器为连续时间线性系统模型，设计平移运动观测器的过程噪声和观测噪声矩阵，利用Riccati方程迭代整定计算系统方差矩阵，获得系统Kalman增益矩阵。BeiDou/SINS松组合导航系统弹性观测器模型的优势在于，它不同于传统的观测器设计思路，它将系统姿态计算方程模型和平移运动观测器方程分开计算，但是二者间通过辅助变量计算加速度计比力的估计数据实现交互级联操作，本发明设计的BeiDou/SINS松组合导航定位系统弹性观测器模型结构如图1所示；它有效避免了传统观测器模型的线性化操作，改善了观测器模型的计算精度与计算稳定性。通过实验数据仿真计算验证了本发明模型设计的正确性和高效计算精度与计算稳定性特点，并且它采用了开放式设计模式，可以根据系统传感设备配置情况，随机修改添加系统姿态注入项算子，从而获得一种BeiDou/SINS松组合导航系统的弹性观测器模型，本发明设计的弹性观测器模型实施算法的计算流程如图2所示，实现了一种弹性组合导航定位系统设计新方法。
一种全源BeiDou/SINS弹性状态观测器模型设计方法，具体步骤如下：
步骤一、结合BeiDou/SINS组合导航系统传感器配置，设计四元数描述的松组合模式无人机载体有界姿态模型方程，并根据松组合导航系统多源传感器中磁力计传感设备和加速度计测量的数据计算注入项算子和陀螺仪偏差；
所述松组合模式无人机载体有界姿态模型方程为：
Figure BDA0002658000780000111
其中，
Figure BDA0002658000780000112
表示载体坐标系相对于ECEF系的旋转四元数微分，
Figure BDA0002658000780000113
表示无人机从载体坐标系b到ECEF系的旋转姿态，
Figure BDA0002658000780000114
表示陀螺仪测量角速度，
Figure BDA0002658000780000115
表示陀螺仪偏差，
Figure BDA0002658000780000116
表示扩展注入项算子，
Figure BDA0002658000780000117
表示地球自转角速度ωie的四维扩展向量，
Figure BDA0002658000780000118
表示陀螺仪偏差随机游走量，
Figure BDA0002658000780000119
表示陀螺仪测量偏差，
Figure BDA00026580007800001110
表示注入项算子，
Figure BDA00026580007800001111
表示陀螺仪偏差定界值，Proj(·)表示投影算子来确保陀螺角速率偏差是有界的，
Figure BDA00026580007800001112
表示比力测量值，
Figure BDA00026580007800001113
表示旋转矩阵，
Figure BDA00026580007800001114
表示饱和算子，κI表示在非线性姿态观测器组合效应作用下的陀螺仪角速率偏差估计弹性增益系数，κ2表示注入项算子
Figure BDA00026580007800001115
的各个观测/参考矢量对的弹性比例系数，
Figure BDA00026580007800001116
表示载体系下的磁力计测量，me表示地球磁场参考矢量，
Figure BDA00026580007800001117
表示加速度计的比力矢量；利用
Figure BDA00026580007800001118
获得
Figure BDA00026580007800001119
的估计
Figure BDA00026580007800001120
计算。在弹性姿态观测器设计中引入的注入项算子
Figure BDA00026580007800001121
它是基于载体坐标系b中的非平行观测向量比较计算获得的，执行陀螺仪偏差的补偿和姿态修正计算，在本模型设计中利用磁力计观测数据和加速度计比力测量数据构成非平行矢量开展计算，需要给出在ECEF系中相应的磁力计和加速度计参考矢量数据，利用旋转矩阵
Figure BDA0002658000780000121
将其转换到载体坐标系b中，当两组矢量不平行时候，那么注入项算子就自动补偿非平行矢量导致的误差，对姿态数据做出补偿修正，其中me是已知的地球磁场参考矢量，
Figure BDA0002658000780000122
是加速度计的比力矢量，经由平移运动观测器计算获得的，这样即使是在无人机加速运动状态也可以获得比力的精确估计计算，
Figure BDA0002658000780000123
是一个饱和算子，确保比力计算是有界的，
Figure BDA0002658000780000124
这样设计的优势在于，相比于传统的利用载体所在位置数据计算重力加速度模型，
Figure BDA0002658000780000125
要求运动载体必须在一定时间内是不能加速的，本发明设计模型则没有这方面的限制要求；另外本发明模型的注入项算子采用了两对矢量对加速度观测/参考矢量对和地球磁场测量/参考矢量对开展计算的，其实本发明模型设计中还可以根据组合系统传感器配置，开放式添加其他观测/参考矢量对获得的注入项算子。应该说明的是利用非平行矢量/参考矢量对设计注入项算子要确保至少两组系统传感器参与注入项算子计算，来保证模型计算收敛性。
本发明模型设计中的加速度计测量及其参考矢量对，磁力计测量/地球磁场参考矢量对比力测量值
Figure BDA0002658000780000126
加速度计的比力矢量
Figure BDA0002658000780000127
载体系下的磁力计测量值
Figure BDA0002658000780000128
地球磁场参考矢量me进行规范化计算，得到：
Figure BDA0002658000780000129
其中，f b表示载体系下的规范化比力，f e表示ECEF系下的规范化测量比力，m b表示载体系下的规范化磁力计测量值，m e表示ECEF系下的规范化磁力计测量值。
首先利用本发明松组合导航定位系统配置的加速度计和磁力计传感设备，加速度计测量比力矢量及其参考矢量(来自于平移运动观测器的前步迭代数据)，磁力计测量的地球磁场矢量以及已知的地球磁场参考数据，对其进行规范化操作，构建注入项算子，考虑利用规范化非平行观测/参考矢量对参与无人机载体姿态修正计算，注入项算子
Figure BDA00026580007800001210
的规范化形式为：
Figure BDA00026580007800001211
对陀螺仪角速率偏差向量执行投影计算，确保陀螺角速率偏差是定界的，在此过程中引入弹性姿态模型的弹性比例系数κ1、κ2，以及陀螺仪角速率偏差的弹性整定参数κI。从而进一步增强姿态估计计算的计算速度和稳定性。其中的κ1、κ2是注入项算子
Figure BDA00026580007800001212
的各个观测/参考矢量对的弹性比例系数，用来确定各个非平行矢量对对注入项算子的影响大小，一般来说第一个分量的影响是最大的，其余分量作用会逐步减小。
步骤二、分别对注入项算子和陀螺仪偏差进行离散化计算，根据注入项算子和陀螺仪偏差的离散化结果对松组合模式无人机载体有界姿态模型方程进行离散化计算；
对弹性姿态有界模型方程执行离散化计算，以速率陀螺仪测量速率f＝1/T执行离散化计算，可认为采样期间角速度测量数据
Figure BDA0002658000780000131
不变，那么四元数计算公式为，
Figure BDA0002658000780000132
其中，
Figure BDA0002658000780000133
表示k时刻的计算角速度，
Figure BDA0002658000780000134
表示k时刻的角速度列写出的斜对称矩阵，
Figure BDA0002658000780000135
表示地球自转角速度列写出的斜对称矩阵，
Figure BDA0002658000780000136
表示斜对称矩阵的指数计算，
Figure BDA0002658000780000137
表示负斜对称矩阵的指数计算，
Figure BDA0002658000780000138
表示k时刻的角速度，
Figure BDA0002658000780000139
表示角速度，S(·)表示由角速度计算出来的向量，
Figure BDA00026580007800001310
表示地球自转角速度，
Figure BDA00026580007800001311
表示k-1时刻的角速度，I4表示4维单位阵，sinc()表示反正弦函数。
在四元数运算过程中要求保证四元数的规范化，计算公式为
Figure BDA00026580007800001312
相应的陀螺仪偏差速率更新计算为，
Figure BDA00026580007800001313
其中弹性系数κI表示在非线性姿态观测器组合效应作用下的陀螺仪角速率偏差估计弹性增益系数，Proj(·)表示投影算子来确保陀螺角速率偏差是有界的，
Figure BDA00026580007800001314
Mb是陀螺仪测量角速率偏差的预定义上界，将陀螺仪偏差
Figure BDA00026580007800001315
的投影转化为：
Figure BDA00026580007800001316
其中，
Figure BDA0002658000780000141
表示注入项最小化取值算子，Mb是陀螺仪测量角速率偏差的上界，则陀螺仪偏差
Figure BDA0002658000780000142
的离散化表达式为：
Figure BDA0002658000780000143
其中，
Figure BDA0002658000780000144
表示k时刻的陀螺仪偏差计算值，
Figure BDA0002658000780000145
表示k-1时刻的陀螺仪偏差计算值，κI(k)表示k时刻的陀螺仪偏差计算的增益系数。
值得注意的是这里引入多传感设备构建的注入项算子计算，当时刻k时候可以获得可靠的矢量观测数据，可把注入项算子利用投影算子直接添加操作；若此时没有有效的观测矢量，则不执行注入项算子添加操作，那么注入项算子离散化为，
Figure BDA0002658000780000146
若i＝1，
Figure BDA0002658000780000147
执行
Figure BDA0002658000780000148
计算，
Figure BDA0002658000780000149
否则，
Figure BDA00026580007800001410
若i＝2，
Figure BDA00026580007800001411
执行
Figure BDA00026580007800001412
计算，
Figure BDA00026580007800001413
否则，
Figure BDA00026580007800001414
其中，δtacc表示加速度计可用时的采样时间间隔，δtmag表示磁力计可用时的采样时间间隔，T表示积分间隔，
Figure BDA00026580007800001415
表示k时刻的总的注入项算子，
Figure BDA00026580007800001416
表示k时刻的比力计算的注入项部分，
Figure BDA00026580007800001417
表示k时刻的磁力计测量的注入项部分，k1(k)表示k时刻的增益系数，f b(k)表示k时刻的载体系下的比力规范值，
Figure BDA00026580007800001418
表示k-1时刻的四元数表述的旋转矩阵，f e(k)表示ECEF系下的k时刻的比力规范化值，k表示时刻，m b(k)表示载体系下的k时刻的磁力计规范化值，m e(k)表示ECEF系下的k时刻的磁力计规范化值；实际上加速度计采样时间δtacc＝T。
步骤三、构建BeiDou接收机位置和速度测量误差模型，并引入辅助向量构建BeiDou/SINS组合导航系统的无人机平移运动观测器模型；
在无人机载体弹性姿态模型设计基础上，考虑无人机平移运动观测器设计任务。这里综合BeiDou接收机和惯性组件IMU模型，考虑无人机载体的平移运动向量，位置
Figure BDA0002658000780000151
和速度向量
Figure BDA0002658000780000152
还有平移运动计算的加速度计比力估计向量
Figure BDA0002658000780000153
为了方便模型编制，和计算比力估计数据，引入辅助向量Ψ，给出无人机平移运动观测器模型为，
Figure BDA0002658000780000154
其中，旋转矩阵
Figure BDA0002658000780000155
其中的四元数
Figure BDA0002658000780000156
表示为实部sq和虚部rq，平移运动模型中的比力估计值再反馈到姿态估计器中参与姿态和平移运动观测器的交互计算。
BeiDou接收机获得的无人机载体位置和速度测量数据会受到三个方面的时变误差影响，包括卫星误差、卫星信号传播误差和接收机误差等，对此在BeiDou/SINS松组合模型中采用m阶线性误差模型设计为，
Figure BDA0002658000780000157
其中δ＝(δp,δv)∈R6表示无人机载体位置和速度观测误差，
Figure BDA0002658000780000158
和n∈R6表示具有单位白噪声，满足n～(0，1)和
Figure BDA0002658000780000159
分布特性，因此接收机测量的位置和速度估计可表示为，
Figure BDA00026580007800001510
定义BeiDou接收机位置和速度测量误差向量z:＝(zp；zv)T，那么可以系统化表示BeiDou接收机动态位置误差表达式为，
Figure BDA00026580007800001511
其中
Figure BDA00026580007800001512
Tp是相关时间常数，Gp可选择为驱动噪声的适宜的标准偏差；
相应的BeiDou接收机测量的载体速度可表示为，
Figure BDA00026580007800001513
其中，
Figure BDA00026580007800001514
表示BeiDou接收机位置向量微分，zp表示BeiDou接收机位置向量，Fp表示位置向量转移矩阵，Gp表示位置噪声矩阵，np表示位置噪声，δp表示无人机载体位置误差观测噪声，Hp表示位置误差转移矩阵，
Figure BDA0002658000780000161
表示位置误差噪声，
Figure BDA0002658000780000162
表示BeiDou接收机速度向量，Fv表示BeiDou接收机测量速度转移矩阵，zv表示速度观测向量，Gv表示速度噪声矩阵，δv表示速度观测噪声，Hv表示速度误差转移矩阵，
Figure BDA0002658000780000163
表示速度误差噪声。参数
Figure BDA0002658000780000164
Tv是相关时间常数，Gv可选择为驱动噪声的适宜的标准偏差。
考虑BeiDou/SINS松组合模式下的位置和速度观测特性，以及弹性姿态观测器中的加速度计比力影响，在设计无人机平移运动观测器模型中引入一个辅助变量Ψ，它联系了BeiDou接收机位置和速度观测量对加速度计比力测量的影响作用，融合BeiDou接收机的位置和速度观测误差模型方程，构建BeiDou/SINS组合导航系统的无人机平移运动观测器模型为：
Figure BDA0002658000780000165
其中，
Figure BDA0002658000780000166
表示无人机平移运动的位置微分，
Figure BDA0002658000780000167
表示无人机平移运动的位置，
Figure BDA0002658000780000168
表示无人机平移运动的速度向量，θ表示弹性系数，可取值为1，
Figure BDA0002658000780000169
表示位置变量增益系数，
Figure BDA00026580007800001610
表示BeiDou接收机在ECEF系下的位置向量，
Figure BDA00026580007800001611
表示位置和速度关联增益系数，
Figure BDA00026580007800001612
表示BeiDou接收机在ECEF系下的速度向量，
Figure BDA00026580007800001613
表示扩展的BeiDou接收机测量的垂向位置变量微分，
Figure BDA00026580007800001614
表示扩展的BeiDou接收机测量的垂向位置变量，F表示扩展垂向位移转移矩阵，Kpz表示BeiDou接收机位置和扩展垂向变量关联增益系数，
Figure BDA00026580007800001615
表示扩展垂向误差变量，Kvz表示速度与扩展垂向位移关联增益系数，
Figure BDA00026580007800001616
表示扩展垂向速度误差噪声，
Figure BDA00026580007800001617
表示ECEF系下的速度矢量微分，
Figure BDA00026580007800001618
表示地球自转角速度表示的旋转矩阵，
Figure BDA00026580007800001619
表示加速度计比力估计向量，
Figure BDA00026580007800001620
表示计算位置表达的重力矢量，
Figure BDA00026580007800001621
表示位置速度关联增益系数，
Figure BDA00026580007800001622
表示速度增益系数，Ψ表示辅助向量，
Figure BDA00026580007800001623
表示辅助向量微分，
Figure BDA00026580007800001624
表示四元数
Figure BDA00026580007800001625
的旋转矩阵，
Figure BDA00026580007800001626
表示注入项算子表述的旋转矩阵，
Figure BDA00026580007800001627
表示位置和辅助变量关联增益系数，
Figure BDA00026580007800001628
表示速度与辅助变量关联增益系数，
Figure BDA00026580007800001629
表示比力测量值。
步骤四、根据BeiDou/SINS组合导航系统的无人机平移运动观测器模型构建BeiDou/SINS组合导航系统的连续线性系统模型，并将BeiDou/SINS组合导航系统的连续线性系统模型转化为平移运动观测器的LTV模型；
执行BeiDou/SINS松组合导航系统平移运动观测器模型弹性参数整定计算，对系统状态变量方差矩阵实施Riccati矩阵方程的迭代计算；从而平移运动观测器状态向量定义为
Figure BDA0002658000780000171
系统驱动向量
Figure BDA0002658000780000172
输出向量定义为
Figure BDA0002658000780000173
所述BeiDou/SINS组合导航系统的连续线性系统模型为：
Figure BDA0002658000780000174
其中，
Figure BDA0002658000780000175
表示BeiDou/SINS组合导航系统的状态变量微分，n表示系统噪声，
Figure BDA0002658000780000176
为BeiDou/SINS组合导航系统的状态变量，
Figure BDA0002658000780000177
为BeiDou/SINS组合导航系统的输入量，矩阵
Figure BDA0002658000780000178
表示状态转移矩阵A的子矩阵，
Figure BDA0002658000780000179
表示输入量系数矩阵，
Figure BDA00026580007800001710
表示输入量系数矩阵的子矩阵，
Figure BDA00026580007800001711
表示过程噪声系数矩阵，B＝(B1,B2)，
Figure BDA00026580007800001712
表示获得的LTV方程中的辅助项；
所述平移运动观测器的LTV模型为：
Figure BDA00026580007800001713
其中，
Figure BDA00026580007800001714
表示系统状态变量计算值微分，
Figure BDA00026580007800001715
表示系统状态变量计算值，C＝(C*,H)表示观测方程的一阶微分Jaccobian矩阵，
Figure BDA00026580007800001716
表示系数矩阵，
Figure BDA00026580007800001717
表示增益矩阵，
Figure BDA00026580007800001718
表示由系统状态变量间的关联增益系数与弹性系数组建的弹性转移矩阵，Kz表示扩展的弹性转移矩阵，t表示连续系统时间变量，y表示观测量。
利用矩阵B计算弹性增益矩阵K，根据Riccati方程迭代方法首先计算K0＝PCTR-1，计算Riccati方程，
Figure BDA0002658000780000181
其中，
Figure BDA0002658000780000182
矩阵
Figure BDA0002658000780000183
表示矩阵C的转置矩阵，从而可以计算出弹性增益矩阵
Figure BDA0002658000780000184
lp和lv分别表示BeiDou接收机位置和速度误差模型的维数，可取为3，过程噪声Q可表示为，
Figure BDA0002658000780000185
观测噪声方差可表示为，
R＝blockdiag(Sp,Sv) (22)。
步骤五、对平移运动观测器的LTV模型进行离散化，预测平移运动观测器的LTV模型系统状态变量；
针对平移运动观测器的LTV模型，LTV模型系统状态变量的下一步预测计算为，
Figure BDA0002658000780000186
其中，x-(k+1)表示k+1时刻的系统状态变量预测，x+(k)表示系统状态变量的上一步估计计算数据，定义矩阵指数函数为Ad，
Figure BDA0002658000780000187
B1(τ)表示输入量系数矩阵积分，u*(τ)表示输入量，D(τ)表示LTV系统方程的辅助项，τ∈[kT,(k+1)T]表示积分时间算子；定义矩阵指数函数为Ad，根据平移运动观测器模型式(19)的参数设置，在采样间隔时间内比力输入量、姿态旋转矩阵和重力向量均保持常值，也即
Figure BDA0002658000780000188
Figure BDA0002658000780000189
则定义，
Figure BDA00026580007800001810
可得，
得到Bd(k)＝(Bd,1(k)Bd,2(k))；
定义
Figure BDA0002658000780000191
则平移运动观测器的LTV模型系统状态变量的预测表达式为，
x-(k+1)＝Ad(k)x+(k)+Bd,1(k)u*(k)+D(k) (24)，
平移运动观测器的LTV模型系统状态变量的预测表达式对应的过程噪声方差矩阵迭代计算公式为，
Figure BDA0002658000780000192
其中，Qd(k)＝Q·T；
若所有的输出向量y(k)都有效情况下，根据Kalman滤波计算方法计算增益矩阵Kd(k)，
Kd(k)＝P-(k)CT(k)(C(k)P-(k)CT(k)+R(k))-1 (26)，
其中，C(k)表示观测方程的一阶微分Jaccobian矩阵。
步骤六、根据平移运动观测器的LTV模型系统状态变量的预测值对平移运动观测器的LTV模型系统状态变量进行更新。平移运动观测器的LTV模型系统状态变量的更新计算表达式为，
x+(k)＝x-(k)+Kd(k)[y(k)-C(k)x-(k)] (27)，
其中，x+(k)表示k时刻的系统状态变量估计值，x-(k)表示k时刻的系统状态变量预测值，y(k)表示k时刻的系统观测量；
LTV系统状态变量的估计方差矩阵计算为，
P+(k)＝[I9+l-Kd(k)C(k)]P-(k) (28)，
其中，P+(k)表示系统状态变量k时刻的估计误差方差矩阵，P-(k)表示k时刻的预测系统状态误差方差阵，I9+l表示9+l维的单位矩阵。
应用实例
为了验证本发明提出的BeiDou/SINS松组合导航定位系统的弹性观测器模型算法的有效性及其计算优势，这里给出仿真验证测试数据。首先本发明假设惯性组件IMU和BeiDou接收机子系统的测量数据包括位置和速度数据具有有色普常数特性的白噪声干扰误差，考虑系统弹性增益系数及其增益矩阵是时变性的，最终仿真数据都转换为NED坐标系中显示出来，从ECEF系转换到NED坐标系需要从位置估计数据
Figure BDA0002658000780000201
获得无人机在NED坐标系中的经度
Figure BDA0002658000780000202
和纬度
Figure BDA0002658000780000203
估计数据，利用四元数
Figure BDA0002658000780000204
其中
Figure BDA0002658000780000205
Figure BDA0002658000780000206
IMU组件的噪声特性表现为，陀螺仪偏差噪声满足εω～n(0,0.00252)，加速度计噪声满足εf～n(0,0.052)；BeiDou接收机的位置观测噪声为
Figure BDA0002658000780000207
另外在NED坐标系中BeiDou接收机测量的包含噪声误差的位置和速度数据可表示为，
Figure BDA0002658000780000208
那么BeiDou接收机测量的位置和速度误差模型参数中，
F＝blockdiag(FP,FV),G＝blockdiag(GP,GV) (30)，
且满足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 (9)

1.一种全源BeiDou/SINS弹性状态观测器模型设计方法，其特征在于，其步骤如下：
步骤一、根据BeiDou/SINS组合导航系统多源传感器配置，设计松组合模式无人机载体有界姿态模型方程，并根据多源传感器中磁力计传感设备测量的数据计算注入项算子，根据多源传感器中陀螺仪测量的数据计算陀螺仪偏差；
步骤二、分别对注入项算子和陀螺仪偏差进行离散化计算，根据注入项算子和陀螺仪偏差的离散化结果对松组合模式无人机载体有界姿态模型方程进行离散化计算；
步骤三、构建BeiDou接收机位置和速度测量误差模型，并引入辅助向量构建BeiDou/SINS组合导航系统的无人机平移运动观测器模型；
步骤四、根据BeiDou/SINS组合导航系统的无人机平移运动观测器模型构建BeiDou/SINS组合导航系统的连续线性系统模型，并将BeiDou/SINS组合导航系统的连续线性系统模型转化为平移运动观测器的LTV模型；
步骤五、根据平移运动观测器的LTV模型预测平移运动观测器的LTV模型系统状态变量的预测值；
步骤六、根据平移运动观测器的LTV模型系统状态变量的预测值对平移运动观测器的LTV模型系统状态变量进行更新。
2.根据权利要求1所述的全源BeiDou/SINS弹性状态观测器模型设计方法，其特征在于，所述松组合模式无人机载体有界姿态模型方程为：
Figure FDA0002658000770000011
其中，
Figure FDA0002658000770000012
表示载体坐标系相对于ECEF系的旋转四元数微分，
Figure FDA0002658000770000013
表示无人机从载体坐标系b到ECEF系的旋转姿态，
Figure FDA0002658000770000014
表示陀螺仪测量角速度，
Figure FDA0002658000770000015
表示陀螺仪偏差，
Figure FDA0002658000770000016
表示扩展注入项算子，
Figure FDA0002658000770000017
表示地球自转角速度ωie的四维扩展向量，
Figure FDA0002658000770000018
表示陀螺仪偏差随机游走量，
Figure FDA0002658000770000019
表示陀螺仪测量偏差，
Figure FDA00026580007700000110
表示注入项算子，
Figure FDA00026580007700000111
表示陀螺仪偏差定界值，Proj(·)表示投影模型，
Figure FDA00026580007700000112
表示比力测量值，
Figure FDA00026580007700000113
表示旋转矩阵，
Figure FDA00026580007700000114
表示饱和算子，κ1表示陀螺仪角速率偏差估计弹性增益系数，κ2表示注入项算子
Figure FDA00026580007700000115
的各个观测/参考矢量对的弹性比例系数，
Figure FDA00026580007700000116
表示载体系下的磁力计测量，me表示地球磁场参考矢量，
Figure FDA00026580007700000117
表示加速度计的比力矢量；
分别对比力测量值
Figure FDA0002658000770000021
加速度计的比力矢量
Figure FDA0002658000770000022
载体系下的磁力计测量值
Figure FDA0002658000770000023
地球磁场参考矢量me进行规范化计算，得到：
Figure FDA0002658000770000024
其中，f b表示载体系下的规范化比力，f e表示ECEF系下的规范化测量比力，m b表示载体系下的规范化磁力计测量值，m e表示ECEF系下的规范化磁力计测量值；
注入项算子
Figure FDA0002658000770000025
的规范化形式为：
Figure FDA0002658000770000026
3.根据权利要求2所述的全源BeiDou/SINS弹性状态观测器模型设计方法，其特征在于，所述对注入项算子
Figure FDA0002658000770000027
进行离散化计算的方法为：
Figure FDA0002658000770000028
若i＝1，
Figure FDA0002658000770000029
执行
Figure FDA00026580007700000210
计算，
Figure FDA00026580007700000211
否则，
Figure FDA00026580007700000212
若i＝2，
Figure FDA00026580007700000213
执行
Figure FDA00026580007700000214
计算，
Figure FDA00026580007700000215
否则，
Figure FDA00026580007700000216
其中，δtacc表示加速度计可用时的采样时间间隔，δtmag表示磁力计可用时的采样时间间隔，T表示积分间隔，
Figure FDA00026580007700000217
表示k时刻的总的注入项算子，
Figure FDA00026580007700000218
表示k时刻的比力计算的注入项部分，
Figure FDA00026580007700000219
表示k时刻的磁力计测量的注入项部分，k1(k)表示k时刻的增益系数，f b(k)表示k时刻的载体系下的比力规范值，
Figure FDA00026580007700000220
表示k-1时刻的四元数表述的旋转矩阵，f e(k)表示ECEF系下的k时刻的比力规范化值，k表示时刻，m b(k)表示载体系下的k时刻的磁力计规范化测量值，m e(k)表示ECEF系下的k时刻的磁力计规范化测量值；
所述对陀螺仪偏差
Figure FDA00026580007700000221
进行离散化计算的方法为：
将陀螺仪偏差
Figure FDA00026580007700000222
的投影模型转化为：
Figure FDA0002658000770000031
其中，
Figure FDA0002658000770000032
表示注入项最小化取值算子，Mb是陀螺仪测量角速率偏差的上界，则陀螺仪偏差
Figure FDA0002658000770000033
的离散化表达式为：
Figure FDA0002658000770000034
其中，
Figure FDA0002658000770000035
表示k时刻的陀螺仪偏差计算值，
Figure FDA0002658000770000036
表示k-1时刻的陀螺仪偏差计算值，κ1(k)表示k时刻的陀螺仪偏差计算的增益系数，I3表示3维单位向量。
4.根据权利要求3所述的全源BeiDou/SINS弹性状态观测器模型设计方法，其特征在于，所述根据注入项算子和陀螺仪偏差的离散化结果对松组合模式无人机载体有界姿态模型方程进行离散化计算的方法为：
Figure FDA0002658000770000037
其中，
Figure FDA0002658000770000038
表示斜对称矩阵的指数计算，
Figure FDA0002658000770000039
表示负斜对称矩阵的指数计算，
Figure FDA00026580007700000310
表示k时刻的计算角速度，
Figure FDA00026580007700000311
表示k时刻的角速度，
Figure FDA00026580007700000312
表示角速度，
Figure FDA00026580007700000313
表示地球自转角速度，
Figure FDA00026580007700000314
表示k-1时刻的角速度，I4表示4维单位阵，sinc()表示反正弦函数。
5.根据权利要求1所述的全源BeiDou/SINS弹性状态观测器模型设计方法，其特征在于，所述BeiDou接收机位置和速度测量误差模型为：
Figure FDA00026580007700000315
Figure FDA00026580007700000316
其中，
Figure FDA00026580007700000317
表示BeiDou接收机位置向量微分，zp表示BeiDou接收机位置向量，Fp表示位置向量转移矩阵，Gp表示位置噪声矩阵，np表示位置噪声，δp表示无人机载体位置误差观测噪声，Hp表示位置误差转移矩阵，
Figure FDA0002658000770000041
表示位置误差噪声，
Figure FDA0002658000770000042
表示BeiDou接收机速度向量，Fv表示BeiDou接收机测量速度转移矩阵，zv表示速度观测向量，Gv表示速度噪声矩阵，δv表示速度观测噪声，Hv表示速度误差转移矩阵，
Figure FDA0002658000770000043
表示速度误差噪声。
6.根据权利要求5所述的全源BeiDou/SINS弹性状态观测器模型设计方法，其特征在于，所述BeiDou/SINS组合导航系统的无人机平移运动观测器模型为：
Figure FDA0002658000770000044
其中，
Figure FDA0002658000770000045
表示无人机平移运动的位置微分，
Figure FDA0002658000770000046
表示无人机平移运动的位置，
Figure FDA0002658000770000047
表示无人机平移运动的速度向量，θ表示弹性系数，可取值为1，
Figure FDA0002658000770000048
表示位置变量增益系数，
Figure FDA0002658000770000049
表示BeiDou接收机在ECEF系下的位置向量，
Figure FDA00026580007700000410
表示位置和速度关联增益系数，
Figure FDA00026580007700000411
表示BeiDou接收机在ECEF系下的速度向量，
Figure FDA00026580007700000412
表示扩展的BeiDou接收机测量的垂向位置变量微分，
Figure FDA00026580007700000413
表示扩展的BeiDou接收机测量的垂向位置变量，F表示扩展垂向位移转移矩阵，Kpz表示BeiDou接收机位置和扩展垂向变量关联增益系数，
Figure FDA00026580007700000414
表示扩展垂向误差变量，Kvz表示速度与扩展垂向位移关联增益系数，
Figure FDA00026580007700000415
表示扩展垂向速度误差噪声，
Figure FDA00026580007700000416
表示ECEF系下的速度矢量微分，
Figure FDA00026580007700000417
表示地球自转角速度表示的旋转矩阵，
Figure FDA00026580007700000418
表示加速度计比力估计向量，
Figure FDA00026580007700000419
表示计算位置表达的重力矢量，
Figure FDA00026580007700000420
表示位置速度关联增益系数，
Figure FDA00026580007700000421
表示速度增益系数，Ψ表示辅助向量，
Figure FDA00026580007700000422
表示辅助向量微分，
Figure FDA00026580007700000423
表示四元数
Figure FDA00026580007700000424
的旋转矩阵，
Figure FDA00026580007700000425
表示注入项算子表述的旋转矩阵，
Figure FDA00026580007700000426
表示位置和辅助变量关联增益系数，
Figure FDA00026580007700000427
表示速度与辅助变量关联增益系数，
Figure FDA00026580007700000428
表示比力测量值。
7.根据权利要求6所述的全源BeiDou/SINS弹性状态观测器模型设计方法，其特征在于，所述BeiDou/SINS组合导航系统的连续线性系统模型为：
Figure FDA00026580007700000429
其中，
Figure FDA00026580007700000430
表示BeiDou/SINS组合导航系统的状态变量微分，n表示系统噪声，
Figure FDA00026580007700000431
为BeiDou/SINS组合导航系统的状态变量，
Figure FDA00026580007700000432
为BeiDou/SINS组合导航系统的输入量，状态转移矩阵
Figure FDA0002658000770000051
表示A的子矩阵，
Figure FDA0002658000770000052
表示输入量系数矩阵，
Figure FDA0002658000770000053
表示B1子矩阵，
Figure FDA0002658000770000054
表示过程噪声系数矩阵，B＝(B1,B2)，
Figure FDA0002658000770000055
表示获得的LTV方程中的辅助项；
所述平移运动观测器的LTV模型为：
Figure FDA0002658000770000056
其中，C＝(C*,H)表示观测方程的一阶微分Jaccobian矩阵，
Figure FDA0002658000770000057
表示系数矩阵，
Figure FDA0002658000770000058
表示增益矩阵，
Figure FDA0002658000770000059
表示由系统状态变量间的关联增益系数与弹性系数组建的弹性转移矩阵，Kz表示扩展的弹性转移矩阵，t表示连续系统时间变量，y表示观测量。
8.根据权利要求7所述的全源BeiDou/SINS弹性状态观测器模型设计方法，其特征在于，所述预测平移运动观测器的LTV模型系统状态变量的方法为：
针对平移运动观测器的LTV模型，LTV模型系统状态变量的下一步预测计算为，
Figure FDA00026580007700000510
其中，x-(k+1)表示k+1时刻的系统状态变量预测，x+(k)表示系统状态变量的上一步估计计算数据，定义矩阵指数函数为Ad，
Figure FDA00026580007700000511
B1(τ)表示输入量系数矩阵积分，u*(τ)表示输入量，D(τ)表示LTV系统方程的辅助项，τ∈[kT,(k+1)T]表示积分时间算子；
在采样间隔时间内比力输入量、姿态旋转矩阵和重力向量均保持常值，也即
Figure FDA0002658000770000061
则定义，
Figure FDA0002658000770000062
可得，
得到Bd(k)＝(Bd,1(k)Bd,2(k))；
定义
Figure FDA0002658000770000063
则平移运动观测器的LTV模型系统状态变量的预测表达式为，
x-(k+1)＝Ad(k)x+(k)+Bd,1(k)u*(k)+D(k)，
平移运动观测器的LTV模型系统状态变量的预测表达式对应的过程噪声方差矩阵迭代计算公式为，
Figure FDA0002658000770000064
其中，Qd(k)＝Q·T；
根据Kalman滤波计算方法计算增益矩阵Kd(k)，
Kd(k)＝P-(k)CT(k)(C(k)P-(k)CT(k)+R(k))-1，
其中，C(k)表示系统观测方程的观测矩阵。
9.根据权利要求8所述的全源BeiDou/SINS弹性状态观测器模型设计方法，其特征在于，所述根据平移运动观测器的LTV模型系统状态变量的预测值对平移运动观测器的LTV模型系统状态变量进行更新的方法为：
x+(k)＝x-(k)+Kd(k)[y(k)-C(k)x-(k)]，
其中，x+(k)表示k时刻的系统状态变量估计值，x-(k)表示k时刻的系统状态变量预测值，y(k)表示k时刻的系统观测量；
LTV系统状态变量的估计方差矩阵计算为，
P+(k)＝[I9+l-Kd(k)C(k)]P-(k)，
其中，P+(k)表示系统状态变量k时刻的估计误差方差矩阵，P-(k)表示k时刻的预测系统状态误差方差阵，I9+l表示9+l维的单位矩阵。
CN202010894489.7A 2020-08-31 2020-08-31 一种全源BeiDou/SINS弹性状态观测器模型设计方法 Active CN111982126B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010894489.7A CN111982126B (zh) 2020-08-31 2020-08-31 一种全源BeiDou/SINS弹性状态观测器模型设计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010894489.7A CN111982126B (zh) 2020-08-31 2020-08-31 一种全源BeiDou/SINS弹性状态观测器模型设计方法

Publications (2)

Publication Number Publication Date
CN111982126A true CN111982126A (zh) 2020-11-24
CN111982126B CN111982126B (zh) 2023-03-17

Family

ID=73441504

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010894489.7A Active CN111982126B (zh) 2020-08-31 2020-08-31 一种全源BeiDou/SINS弹性状态观测器模型设计方法

Country Status (1)

Country Link
CN (1) CN111982126B (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113009816A (zh) * 2021-03-08 2021-06-22 北京信息科技大学 时间同步误差的确定方法及装置、存储介质及电子装置
CN114674313A (zh) * 2022-03-31 2022-06-28 淮阴工学院 一种基于ckf算法的gps/bds和sins融合的无人配送车导航定位方法

Citations (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20030149528A1 (en) * 2002-02-06 2003-08-07 Ching-Fang Lin Positioning and navigation method and system thereof
US20050022402A1 (en) * 2003-08-01 2005-02-03 Ash Michael E. Compact navigation system and method
US20050240347A1 (en) * 2004-04-23 2005-10-27 Yun-Chun Yang Method and apparatus for adaptive filter based attitude updating
US20140372026A1 (en) * 2011-09-14 2014-12-18 Trusted Positioning Inc. Method and apparatus for navigation with nonlinear models
CN106767797A (zh) * 2017-03-23 2017-05-31 南京航空航天大学 一种基于对偶四元数的惯性/gps组合导航方法
CN109141436A (zh) * 2018-09-30 2019-01-04 东南大学 改进的无迹卡尔曼滤波算法在水下组合导航中的应用方法
CN109781098A (zh) * 2019-03-08 2019-05-21 兰州交通大学 一种列车定位的方法和系统
CN110220512A (zh) * 2019-05-16 2019-09-10 武汉新瑞通达信息技术有限公司 一种全站仪组合惯性测量单元的动态定位系统
CN110285810A (zh) * 2019-06-13 2019-09-27 兖矿集团有限公司 一种基于惯性导航数据的采煤机自主定位方法及装置
CN111190207A (zh) * 2020-01-09 2020-05-22 郑州轻工业大学 基于pstcsdref算法的无人机ins bds组合导航方法
CN111189442A (zh) * 2020-01-11 2020-05-22 郑州轻工业大学 基于cepf的无人机多源导航信息状态预测方法
CN111351482A (zh) * 2020-03-19 2020-06-30 南京理工大学 基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法

Patent Citations (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20030149528A1 (en) * 2002-02-06 2003-08-07 Ching-Fang Lin Positioning and navigation method and system thereof
US20050022402A1 (en) * 2003-08-01 2005-02-03 Ash Michael E. Compact navigation system and method
US20050240347A1 (en) * 2004-04-23 2005-10-27 Yun-Chun Yang Method and apparatus for adaptive filter based attitude updating
US20140372026A1 (en) * 2011-09-14 2014-12-18 Trusted Positioning Inc. Method and apparatus for navigation with nonlinear models
CN106767797A (zh) * 2017-03-23 2017-05-31 南京航空航天大学 一种基于对偶四元数的惯性/gps组合导航方法
CN109141436A (zh) * 2018-09-30 2019-01-04 东南大学 改进的无迹卡尔曼滤波算法在水下组合导航中的应用方法
CN109781098A (zh) * 2019-03-08 2019-05-21 兰州交通大学 一种列车定位的方法和系统
CN110220512A (zh) * 2019-05-16 2019-09-10 武汉新瑞通达信息技术有限公司 一种全站仪组合惯性测量单元的动态定位系统
CN110285810A (zh) * 2019-06-13 2019-09-27 兖矿集团有限公司 一种基于惯性导航数据的采煤机自主定位方法及装置
CN111190207A (zh) * 2020-01-09 2020-05-22 郑州轻工业大学 基于pstcsdref算法的无人机ins bds组合导航方法
CN111189442A (zh) * 2020-01-11 2020-05-22 郑州轻工业大学 基于cepf的无人机多源导航信息状态预测方法
CN111351482A (zh) * 2020-03-19 2020-06-30 南京理工大学 基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
CHUNLING LIU; CHENG WANG; JING WANG: "A bandwidth adaptive pseudo-code tracking loop design for BD/INS integrated navigation", 《2016 2ND INTERNATIONAL CONFERENCE ON CONTROL SCIENCE AND SYSTEMS ENGINEERING》 *
田方浩;姚敏立;伍宗伟;王标标;赵鹏;: "低成本动中通姿态估计算法" *
蔡安江，刘凯峰，郭师虹，等: "基于四元数衍生无迹卡尔曼滤波的二段式多旋翼无人机姿态估计算法", 《控制理论与应用》 *
马霞等: "SINS/GPS/TACAN机载综合导航定位系统设计", 《电讯技术》 *

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113009816A (zh) * 2021-03-08 2021-06-22 北京信息科技大学 时间同步误差的确定方法及装置、存储介质及电子装置
CN114674313A (zh) * 2022-03-31 2022-06-28 淮阴工学院 一种基于ckf算法的gps/bds和sins融合的无人配送车导航定位方法

Also Published As

Publication number Publication date
CN111982126B (zh) 2023-03-17

Similar Documents

Publication Publication Date Title
CN108226980B (zh) 基于惯性测量单元的差分gnss与ins自适应紧耦合导航方法
CN112146655B (zh) 一种BeiDou/SINS紧组合导航系统弹性模型设计方法
CN111323050B (zh) 一种捷联惯导和多普勒组合系统标定方法
Bryne et al. Nonlinear observers for integrated INS\/GNSS navigation: implementation aspects
CN108827310A (zh) 一种船用星敏感器辅助陀螺仪在线标定方法
CN105136145A (zh) 一种基于卡尔曼滤波的四旋翼无人机姿态数据融合的方法
EP1585939A2 (en) Attitude change kalman filter measurement apparatus and method
Crocoll et al. Model‐aided navigation for a quadrotor helicopter: A novel navigation system and first experimental results
CN110849360B (zh) 面向多机协同编队飞行的分布式相对导航方法
CN111189442B (zh) 基于cepf的无人机多源导航信息状态预测方法
CN111982126B (zh) 一种全源BeiDou/SINS弹性状态观测器模型设计方法
CN111190207B (zh) 基于pstcsdref算法的无人机ins bds组合导航方法
Ko et al. Lie group approach to dynamic-model-aided navigation of multirotor unmanned aerial vehicles
CN111578931B (zh) 基于在线滚动时域估计的高动态飞行器自主姿态估计方法
CN111220151B (zh) 载体系下考虑温度模型的惯性和里程计组合导航方法
Taghizadeh et al. A low-cost integrated navigation system based on factor graph nonlinear optimization for autonomous flight
CN114111840B (zh) 一种基于组合导航的dvl误差参数在线标定方法
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
CN113916226B (zh) 一种基于最小方差的组合导航系统抗扰滤波方法
Khoder et al. A quaternion scaled unscented kalman estimator for inertial navigation states determination using ins/gps/magnetometer fusion
CN112683265B (zh) 一种基于快速iss集员滤波的mimu/gps组合导航方法
Do et al. An Improvement of 3D DR/INS/GNSS Integrated System using Inequality Constrained EKF
CN111473786A (zh) 一种基于局部反馈的两层分布式多传感器组合导航滤波方法
Vasconcelos et al. Inertial navigation system aided by GPS and selective frequency contents of vector measurements
CN115164886B (zh) 车载gnss/ins组合导航系统比例因子误差补偿方法

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: 20231126

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