CN110672124A - 离线杠杆臂估计方法 - Google Patents

离线杠杆臂估计方法 Download PDF

Info

Publication number
CN110672124A
CN110672124A CN201910923950.4A CN201910923950A CN110672124A CN 110672124 A CN110672124 A CN 110672124A CN 201910923950 A CN201910923950 A CN 201910923950A CN 110672124 A CN110672124 A CN 110672124A
Authority
CN
China
Prior art keywords
error
representing
vector
obtaining
lever arm
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.)
Pending
Application number
CN201910923950.4A
Other languages
English (en)
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.)
Beijing Neville Times Technology Co Ltd
Original Assignee
Beijing Neville Times Technology Co Ltd
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 Beijing Neville Times Technology Co Ltd filed Critical Beijing Neville Times Technology Co Ltd
Priority to CN201910923950.4A priority Critical patent/CN110672124A/zh
Publication of CN110672124A publication Critical patent/CN110672124A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • 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
    • G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices

Landscapes

  • Engineering & Computer Science (AREA)
  • Manufacturing & Machinery (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Navigation (AREA)

Abstract

本发明公开一种离线杠杆臂估计方法，该方法的一个具体实施方式包括以下步骤：从全球导航卫星系统获取目标的速度和位置数据并将所述数据传输到所述目标的惯性导航系统；获取所述惯性导航系统的系统误差，根据所述系统误差确定系统状态；获取所述惯性导航系统和所述全球导航卫星系统的可量测信息；通过卡尔曼滤波器，利用固定间隔平滑算法，根据所述系统状态和所述可量测信息获取对离线杠杆臂的优化估计。该实施方式可以有效的估计INS与卫星天线的杠杆距离，提高INS/GNSS卫星组合导航系统的导航精度。

Description

离线杠杆臂估计方法
技术领域
本发明涉及数字信号处理技术领域。更具体的，涉及一种离线杠杆臂估计方法。
背景技术
惯性导航是二十世纪发展起来的一种导航方法，其利用惯性元件陀螺仪来测量运动载体的旋转角速度，依据牛顿惯性原理，通过一次积分运算实时获得运动载体的姿态角，进而更新姿态矩阵；加速度计则用来测量运动载体的运动加速度，根据牛顿惯性原理，通过一次积分运算实时获得运动载体的速度、通过二次积分运算实时获得载体的位置，实现导航定位功能。
INS(Inertial Navigation System，惯性导航系统)的自主性强，不接收也不向外发射任何电磁波，抗干扰能力强；实时导航数据更新率高；在短时间内具有高精度和稳定性；随着MEMS(Micro-Electro-Mechanical System，微机电系统)技术的发展，MINS(Micro-INS，Micro-Inertial-Navigation System，微惯性导航系统)还具有体积更小、重量更轻、耗电量更小、易集成、能大批量生产等特点。然而误差随着时间的增加而逐渐积累是MINS导航面临的最大难题，因此人们采用INS/GNSS(Global Navigation Satellite System，简称GNSS，全球导航卫星系统)组合导航的方式。
然而，在车载环境下，由于INS与卫星天线很难安装在同一个位置，组合导航系统中存在杠杆臂误差，严重影响了导航精度，所以，需要对杠杆臂误差进行估计，以有利于消除误差。又由于INS与卫星天线安装位置相对固定，因此，可以提供一种基于固定间隔平滑滤波的离线杠杆臂估计方法，通过采集INS与卫星数据，对杠杆臂进行离线估计，有利于通过对杠杆臂误差进行补偿，提高组合导航系统导航精度。
发明内容
本发明的目的在于提供一种离线杠杆臂估计方法，以解决现有技术存在的问题中的至少一个。
为达到上述目的，本发明采用下述技术方案：
本发明提供一种离线杠杆臂估计方法，包括以下步骤：从全球导航卫星系统获取目标的速度和位置数据并将所述数据传输到所述目标的惯性导航系统；获取所述惯性导航系统的系统误差，根据所述系统误差确定系统状态；获取所述惯性导航系统和所述全球导航卫星系统的可量测信息；通过卡尔曼滤波器，利用固定间隔平滑算法，根据所述系统状态和所述可量测信息获取对离线杠杆臂的优化估计。
可选的，所述获取所述惯性导航系统的系统误差包括：获取所述惯性导航系统中的平台误差角误差、速度误差、位置误差、传感器误差和杠杆臂误差。
可选的，所述获取平台误差角误差进一步包括根据下述公式计算所述平台误差角误差：
Figure BDA0002218378620000021
其中，
Figure BDA0002218378620000022
和
Figure BDA0002218378620000023
分别表示导航坐标系下x、y和z方向的平台误差角导数，
Figure BDA0002218378620000024
和
Figure BDA0002218378620000025
分别表示导航坐标系下x、y和z方向的平台误差角，
Figure BDA0002218378620000026
和
Figure BDA0002218378620000027
分别表示导航坐标系下东和北方向速度，和
Figure BDA0002218378620000029
分别表示导航坐标系下东和北方向速度误差，
Figure BDA00022183786200000210
和
Figure BDA00022183786200000211
分别表示导航坐标系下x、y和z方向陀螺常值误差，Rn表示地球卯酉圈半径，Re表示地球子午圈半径，ωie表示地球自转角速率，L表示纬度，H表示高度，δL表示纬度误差；
所述获取速度误差进一步包括根据下述公式计算所述速度误差：
Figure BDA00022183786200000212
，其中，
Figure BDA0002218378620000031
和
Figure BDA0002218378620000032
分别表示东、北、天方向的速度误差的导数，
Figure BDA0002218378620000033
和
Figure BDA0002218378620000034
分别表示导航坐标系下x、y和z方向的平台误差角，
Figure BDA0002218378620000035
和
Figure BDA0002218378620000036
分别表示导航坐标系下x、y和z方向的比力，vx、vy和vz分别表示东、北和天方向的速度，Rn表示地球卯酉圈半径，Re表示地球子午圈半径，ωie表示地球自转角速率，L表示纬度，H表示高度，δL表示纬度误差，δvx、δvy和δvz分别表示东、北和天方向的速度误差，
Figure BDA0002218378620000037
和
Figure BDA0002218378620000038
分别表示导航坐标系下x、y和z方向加速度计常值误差；
所述获取位置误差进一步包括根据下述公式计算所述位置误差：
Figure BDA0002218378620000039
其中，λ表示经度，L表示纬度，H表示高度，
Figure BDA00022183786200000310
表示纬度误差导数，
Figure BDA00022183786200000311
表示经度误差导数，
Figure BDA00022183786200000312
表示高度误差导数，δvx、δvy和δvz分别表示东、北和天方向的速度误差，Rn表示地球卯酉圈半径，Re表示地球子午圈半径，δL表示纬度误差，vx表示东方向的速度；
所述获取传感器误差包括获取陀螺常值误差向量和加速度计常值误差向量，其中，所述获取陀螺常值误差向量进一步包括根据下述公式计算陀螺常值误差向量：
Figure BDA00022183786200000313
其中，表示所述陀螺常值误差向量的导数，εc表示所述陀螺常值误差向量，
Figure BDA00022183786200000315
和
Figure BDA00022183786200000316
分别表示导航坐标系下x、y和z方向陀螺常值误差，所述获取加速度计常值误差向量进一步包括根据下述公式计算所述加速度计常值误差向量：
其中，
Figure BDA00022183786200000318
表示所述加速度计常值误差向量的导数，
Figure BDA00022183786200000319
表示所述加速度计常值误差向量，
Figure BDA00022183786200000320
和
Figure BDA00022183786200000321
分别表示导航坐标系下x、y和z方向加速度计常值误差；
所述获取杠杆臂误差进一步包括根据下述公式计算所述杠杆臂误差：
其中，
Figure BDA00022183786200000323
表示所述杠杆臂误差向量的导数，δla表示所述杠杆臂误差向量，δlax、δlay和δlaz分别表示导航坐标系下x、y和z方向的杠杆臂误差。
可选的，所述根据所述系统误差确定系统状态进一步包括：根据所述系统误差获取系统的状态向量X(k)，所述状态向量X(k)为
Figure BDA0002218378620000041
其中，
Figure BDA0002218378620000042
和
Figure BDA0002218378620000043
分别表示导航坐标系下x、y和z方向的平台误差角导数，
Figure BDA0002218378620000044
和
Figure BDA0002218378620000045
分别表示东、北、天方向的速度误差的导数，表示纬度误差导数，表示经度误差导数，
Figure BDA0002218378620000048
表示高度误差导数，
Figure BDA0002218378620000049
和分别表示导航坐标系下x、y和z方向的杠杆臂误差的导数。
所述系统的状态向量X(k)满足下述状态方程：
其中，
Figure BDA00022183786200000412
表示状态向量导数，X(t)表示状态向量，F(t)表示状态系数矩阵，W(t)表示状态噪声矩阵。
可选的，所述获取所述惯性导航系统和所述全球导航卫星系统的可量测信息包括：获取速度观测误差、位置观测误差和平台误差角。
可选的，所述获取速度观测误差进一步包括根据下述公式计算所述速度观测误差：
其中，vxI、vyI和vzI分别表示惯性导航系统输出的导航坐标系下x、y和z方向速度，vxG、vyG和vzG分别表示卫星导航系统输出的导航坐标系下x、y和z方向速度，δvx、δvy和δvz分别表示东、北和天方向的速度误差，Mx、My和Mz分别表示x、y和z方向的观测噪声；
所述获取位置观测误差进一步包括根据下述公式计算所述位置误差：
Figure BDA00022183786200000414
其中，λI、LI和HI分别表示惯性导航系统输出的经度、纬度和高度信息，λG、LG和HG分别表示卫星导航系统输出的经度、纬度和高度信息，δλ、δL和δH分别表示经度误差、纬度误差和高度误差，
Figure BDA00022183786200000415
表示载体坐标系b到导航坐标系n的转移矩阵，δla×表示杠杆臂误差向量对称阵，
Figure BDA0002218378620000051
表示平台误差角向量的对称阵，
Figure BDA0002218378620000052
×表示杠杆臂误差向量的导数的对称阵；
所述获取平台误差角进一步包括根据下述公式计算所述平台误差角：
其中，yawI表示惯性导航系统输出的偏航角，yawM表示通过地磁计算的偏航角，
Figure BDA0002218378620000054
表示导航坐标系下z方向的平台误差角，Ryaw表示偏航角误差的观测噪声。
可选的，所述获取所述惯性导航系统和所述全球导航卫星系统的可量测信息进一步包括：结合所述速度观测误差、所述位置观测误差和所述平台误差角根据下述公式计算所述系统的可量测信息所满足的观测方程：
Zk＝HkXk+Vk，
其中，Zk表示在k时刻的m维量测矢量，Hk表示k时刻的量测系数矩阵，Xk表示系统在k时刻的m维状态向量，Vk表示k时刻的m维量测噪声矢量。
可选的，所述通过卡尔曼滤波器，利用固定间隔平滑算法，根据所述系统状态和所述可量测信息获取对离线杠杆臂的优化估计进一步包括：通过卡尔曼滤波器，利用固定间隔平滑算法，根据所述状态方程和所述观测方程获取对所述状态向量x(k)的优化估计作为对离线杠杆臂的优化估计。
可选的，所述通过卡尔曼滤波器进一步包括：所述卡尔曼滤波估计满足下述公式：
在离散系统方程中，其中，Φk，k-1表示(k-1)时刻到k时刻系统状态的转移矩阵，Xk-1表示(k-1)时刻的状态向量，Γk-1表示(k-1)时刻的噪声系数矩阵，Wk-1表示(k-1)时刻的噪声矢量，Zk表示在k时刻的m维量测矢量，Hk表示k时刻的量测系数矩阵，Xk表示系统在k时刻的m维状态向量，Vk表示k时刻的m维量测噪声矢量。
本发明的有益效果如下：
本发明可以有效的估计INS与卫星天线的杠杆距离，提高INS/GNSS卫星组合导航系统的导航精度。
附图说明
下面结合附图对本发明的具体实施方式作进一步详细的说明；
图1示出根据本发明的离线杠杆臂估计方法的一个实施例的流程示意图。
具体实施方式
为了更清楚地说明本发明，下面结合优选实施例和附图对本发明做进一步的说明。附图中相似的部件以相同的附图标记进行表示。本领域技术人员应当理解，下面所具体描述的内容是说明性的而非限制性的，不应以此限制本发明的保护范围。
如图1所示，本发明的一个实施例提供了一种离线杠杆臂估计方法，包括以下步骤：
S100：从全球导航卫星系统获取目标的速度和位置数据并将数据传输到目标的惯性导航系统；
S200：获取惯性导航系统的系统误差，根据系统误差确定系统状态；
S300：获取惯性导航系统和全球导航卫星系统的可量测信息；
S400：通过卡尔曼滤波器，利用固定间隔平滑算法，根据系统状态和系统的可量测信息获取对离线杠杆臂的优化估计。
其中，获取惯性导航系统的系统误差包括：获取惯性导航系统中的平台误差角误差、速度误差、位置误差、传感器误差和杠杆臂误差。
其中，获取平台误差角误差进一步包括根据下述公式计算平台误差角误差：
Figure BDA0002218378620000061
其中，
Figure BDA0002218378620000062
和
Figure BDA0002218378620000063
分别表示导航坐标系下x、y和z方向的平台误差角导数，
Figure BDA0002218378620000064
和
Figure BDA0002218378620000065
分别表示导航坐标系下x、y和z方向的平台误差角，
Figure BDA0002218378620000066
和
Figure BDA0002218378620000067
分别表示导航坐标系下东和北方向速度，和
Figure BDA0002218378620000069
分别表示导航坐标系下东和北方向速度误差，
Figure BDA00022183786200000610
和
Figure BDA00022183786200000611
分别表示导航坐标系下x、y和z方向陀螺常值误差，Rn表示地球卯酉圈半径，Re表示地球子午圈半径，ωie表示地球自转角速率，L表示纬度，H表示高度，δL表示纬度误差；
获取速度误差进一步包括根据下述公式计算速度误差：
，其中，和
Figure BDA0002218378620000073
分别表示东、北、天方向的速度误差的导数，
Figure BDA0002218378620000074
和
Figure BDA0002218378620000075
分别表示导航坐标系下x、y和z方向的平台误差角，和
Figure BDA0002218378620000077
分别表示导航坐标系下x、y和z方向的比力，vx、vy和vz分别表示东、北和天方向的速度，Rn表示地球卯酉圈半径，Re表示地球子午圈半径，ωie表示地球自转角速率，L表示纬度，H表示高度，δL表示纬度误差，δvx、δvy和δvz分别表示东、北和天方向的速度误差，
Figure BDA0002218378620000078
和
Figure BDA0002218378620000079
分别表示导航坐标系下x、y和z方向加速度计常值误差；
获取位置误差进一步包括根据下述公式计算位置误差：
Figure BDA00022183786200000710
其中，λ表示经度，L表示纬度，H表示高度，
Figure BDA00022183786200000711
表示纬度误差导数，
Figure BDA00022183786200000712
表示经度误差导数，
Figure BDA00022183786200000713
表示高度误差导数，δvx、δvy和δvz分别表示东、北和天方向的速度误差，Rn表示地球卯酉圈半径，Re表示地球子午圈半径，δL表示纬度误差，vx表示东方向的速度；
获取传感器误差包括获取陀螺常值误差向量和加速度计常值误差向量，其中，获取陀螺常值误差向量进一步包括根据下述公式计算陀螺常值误差向量：
Figure BDA00022183786200000714
其中，
Figure BDA00022183786200000715
表示陀螺常值误差向量的导数，εc表示陀螺常值误差向量，
Figure BDA00022183786200000716
Figure BDA00022183786200000717
和
Figure BDA00022183786200000718
分别表示导航坐标系下x、y和z方向陀螺常值误差，获取加速度计常值误差向量进一步包括根据下述公式计算加速度计常值误差向量：
Figure BDA00022183786200000719
其中，
Figure BDA00022183786200000720
表示加速度计常值误差向量的导数，
Figure BDA00022183786200000721
表示加速度计常值误差向量，
Figure BDA0002218378620000081
和
Figure BDA0002218378620000082
分别表示导航坐标系下x、y和z方向加速度计常值误差；
获取杠杆臂误差进一步包括根据下述公式计算杠杆臂误差：
Figure BDA0002218378620000083
其中，
Figure BDA0002218378620000084
表示杠杆臂误差向量的导数，δla表示杠杆臂误差向量，δlax、δlay和δlaz分别表示导航坐标系下x、y和z方向的杠杆臂误差。
其中，根据系统误差确定系统状态进一步包括：
根据系统误差获取系统的状态向量X(k)，状态向量X(k)为
Figure BDA0002218378620000085
为12维状态向量，其中，
Figure BDA0002218378620000086
和
Figure BDA0002218378620000087
分别表示导航坐标系下x、y和z方向的平台误差角导数，
Figure BDA0002218378620000088
和
Figure BDA0002218378620000089
分别表示东、北、天方向的速度误差的导数，表示纬度误差导数，
Figure BDA00022183786200000811
表示经度误差导数，
Figure BDA00022183786200000812
表示高度误差导数，
Figure BDA00022183786200000813
和
Figure BDA00022183786200000814
分别表示导航坐标系下x、y和z方向的杠杆臂误差的导数。
系统的状态向量X(k)满足下述状态方程：
Figure BDA00022183786200000815
其中，
Figure BDA00022183786200000816
表示状态向量导数，X(t)表示状态向量，F(t)表示状态系数矩阵，W(t)表示状态噪声矩阵。
其中，状态系数矩阵F(t)满足关系式：
Figure BDA00022183786200000817
其中，F1是一个9*9的系数矩阵，以下为列举的非零系数：
Figure BDA00022183786200000818
Figure BDA00022183786200000820
Figure BDA00022183786200000821
Figure BDA00022183786200000822
Figure BDA00022183786200000823
Figure BDA00022183786200000824
Figure BDA00022183786200000825
Figure BDA00022183786200000826
Figure BDA0002218378620000091
Figure BDA0002218378620000092
其中，获取惯性导航系统和全球导航卫星系统的可量测信息包括：获取速度观测误差、位置观测误差和平台误差角。
获取速度观测误差进一步包括根据下述公式计算速度观测误差：
Figure BDA0002218378620000093
其中，vxI、vyI和vzI分别表示惯性导航系统输出的导航坐标系下x、y和z方向速度，vxG、vyG和vzG分别表示卫星导航系统输出的导航坐标系下x、y和z方向速度，δvx、δvy和δvz分别表示东、北和天方向的速度误差，Mx、My和Mz分别表示x、y和z方向的观测噪声；
获取位置观测误差进一步包括根据下述公式计算位置误差：
Figure BDA0002218378620000094
其中，λI、LI和HI分别表示惯性导航系统输出的经度、纬度和高度信息，λG、LG和HG分别表示卫星导航系统输出的经度、纬度和高度信息，δλ、δL和δH分别表示经度误差、纬度误差和高度误差，
Figure BDA0002218378620000095
表示载体坐标系b到导航坐标系n的转移矩阵，δla×表示杠杆臂误差向量对称阵，
Figure BDA0002218378620000096
表示平台误差角向量的对称阵，
Figure BDA0002218378620000097
×表示杠杆臂误差向量的导数。
用磁传感器辅助确定航向角，获取平台误差角进一步包括根据下述公式计算平台误差角：
Figure BDA0002218378620000098
其中，yawI表示惯性导航系统输出的偏航角，yawM表示表示通过地磁计算的偏航角，表示导航坐标系下z方向的平台误差角，Ryaw表示偏航角误差的观测噪声。
其中，获取惯性导航系统和全球导航卫星系统的可量测信息进一步包括：结合速度观测误差、位置观测误差和平台误差角根据下述公式计算系统的可量测信息所满足的观测方程：
Zk＝HkXk+Vk，
其中，Zk表示在k时刻的m维量测矢量，Hk表示k时刻的量测系数矩阵，Xk表示系统在k时刻的m维状态向量，Vk表示k时刻的m维量测噪声矢量。
作为一种可选的实施方式，通过卡尔曼滤波器，利用固定间隔平滑算法，根据系统状态和可量测信息获取对离线杠杆臂的优化估计进一步包括：通过卡尔曼滤波器，利用固定间隔平滑算法，根据状态方程和观测方程获取对状态向量x(k)的优化估计作为对离线杠杆臂的优化估计。
其中，通过卡尔曼滤波器进一步包括：
卡尔曼滤波估计满足下述公式：
Figure BDA0002218378620000101
在离散系统方程中，其中，Φk，k-1表示(k-1)时刻到k时刻系统状态的转移矩阵，Xk-1表示(k-1)时刻的状态向量，Γk-1表示(k-1)时刻的噪声系数矩阵，Wk-1表示(k-1)时刻的噪声矢量，Zk表示在k时刻的m维量测矢量，Hk表示k时刻的量测系数矩阵，Xk表示系统在k时刻的m维状态向量，Vk表示k时刻的m维量测噪声矢量。
这里的固定间隔平滑算法，是假设在区间[0，N]中的测量值均为已知的情况下，对该区间内的所有时间步长K求出状态向量X(k)的最优状态估计。
固定间隔平滑算法由两个部分构成，即由前向滤波和后向过滤构成。无论是前向滤波还是后向滤波均是利用最优卡尔曼滤波以计算状态向量x(k)的估计值。以后向滤波为例的计算过程如下：
状态一步预测方程：
Figure BDA0002218378620000102
其中，
Figure BDA0002218378620000103
表示状态一步预测向量，Φk，k+1表示系统状态的转移矩阵，
Figure BDA0002218378620000104
表示状态向量；
一步预测均方误差方程：
Figure BDA0002218378620000105
其中，Pk，k+1表示一步预测均方误差矩阵，Φk，k+1表示系统状态的转移矩阵，Pk，k表示均方误差矩阵，
Figure BDA0002218378620000106
表示系统状态的转移矩阵的转置，Γk表示噪声系数矩阵，
Figure BDA0002218378620000107
表示噪声系数矩阵Γk的转置，Qk表示状态噪声矩阵；
残差向量：αk＝Zk-HkXk，k+1，其中，αk表示残差向量，Zk表示观测向量，Hk表示量测系数矩阵，Xk，k+1表示状态一步预测向量；
最优滤波增益方程：
Figure BDA0002218378620000108
其中，Kk表示最优滤波增益矩阵，Pk，k+1表示一步预测均方误差矩阵，Hk表示量测系数矩阵，
Figure BDA0002218378620000109
表示量测系数矩阵Hk的转置，Rk表示观测噪声矩阵；
状态向量X(k)的估计方程：
Figure BDA00022183786200001010
其中，
Figure BDA00022183786200001011
表示状态向量估计值，表示状态一步预测向量，Kk表示最优滤波增益矩阵，αk表示残差向量；
状态向量X(k)的估计均方误差方程：
Figure BDA0002218378620000111
Figure BDA0002218378620000112
其中，Pk+1表示均方误差矩阵，Kk表示最优滤波增益矩阵，Hk表示量测系数矩阵，Pk，k+1表示一步预测均方误差矩阵，Rk表示观测噪声矩阵，
Figure BDA0002218378620000113
表示最优滤波增益矩阵Kk的转置。
并对每一步的计算结果进行存储。再以X(N|N)、P(N|N)为初始值完成前向滤波，得到最终的滤波结果。
前向滤波利用最优卡尔曼滤波以计算状态向量X(k)的估计值，并对每一步得到的结果进行存储，其中包括状态向量估计值
Figure BDA0002218378620000114
状态一步预测值
Figure BDA0002218378620000115
估计均方误差Pk和一步预测均方误差Pk，k-1，以便后续步骤使用。
平滑增益矩阵A(k)，满足下述方程：
A(k)＝PkΦT k，k+1Pk，k+1 -1，k＝N-1，N-2，...，0，
其中，Pk表示均方误差矩阵，ΦT k，k+1表示系统状态的转移矩阵的转置，Pk，k+1 -1表示一步预测均方误差矩阵的逆；
固定区间平滑误差协方差矩阵为：
Pk，N＝Pk+A(k)[Pk+1，N-Pk+1，k]AT(k)，k＝N-1，N-2，...，0，
其中，Pk，N表示k时刻到N时刻的预测均方误差矩阵，Pk表示均方误差矩阵，Pk+1，N表示k+1时刻到N时刻的预测均方误差矩阵，Pk+1，k表示一步预测均方误差矩阵，AT(k)表示平滑增益矩阵A(k)的转置；
计算得到，固定区间平滑过程中状态向量X(k)的最优估计如下：
Figure BDA0002218378620000116
其中，
Figure BDA0002218378620000117
表示k时刻到N时刻的预测状态向量，
Figure BDA0002218378620000118
表示常规Kalman状态估计结果，表示k+1时刻到N时刻的预测状态向量，
Figure BDA00022183786200001110
表示一步预测状态向量。
获取状态向量X(k)的最优估计后，便得到了惯性系统与卫星系统之间的杠杆臂误差。
本发明可以有效的估计INS与卫星天线的杠杆距离，提高INS/GNSS卫星组合导航系统的导航精度。
在本发明的描述中，需要说明的是，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本发明中的具体含义。
还需要说明的是，在本发明的描述中，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。
显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定，对于本领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动，这里无法对所有的实施方式予以穷举，凡是属于本发明的技术方案所引伸出的显而易见的变化或变动仍处于本发明的保护范围之列。

Claims (9)

1.一种离线杠杆臂估计方法，其特征在于，包括以下步骤：
从全球导航卫星系统获取目标的速度和位置数据并将所述数据传输到所述目标的惯性导航系统；
获取所述惯性导航系统的系统误差，根据所述系统误差确定系统状态；
获取所述惯性导航系统和所述全球导航卫星系统的可量测信息；
通过卡尔曼滤波器，利用固定间隔平滑算法，根据所述系统状态和所述可量测信息获取对离线杠杆臂的优化估计。
2.根据权利要求1所述的方法，其特征在于，所述获取所述惯性导航系统的系统误差包括：
获取所述惯性导航系统中的平台误差角误差、速度误差、位置误差、传感器误差和杠杆臂误差。
3.根据权利要求2所述的方法，其特征在于，
所述获取平台误差角误差进一步包括根据下述公式计算所述平台误差角误差：
Figure FDA0002218378610000011
其中，
Figure FDA0002218378610000012
和
Figure FDA0002218378610000013
分别表示导航坐标系下x、y和z方向的平台误差角导数，和
Figure FDA0002218378610000015
分别表示导航坐标系下x、y和z方向的平台误差角，
Figure FDA0002218378610000016
和分别表示导航坐标系下东和北方向速度，
Figure FDA0002218378610000018
和
Figure FDA0002218378610000019
分别表示导航坐标系下东和北方向速度误差，
Figure FDA00022183786100000110
和
Figure FDA00022183786100000111
分别表示导航坐标系下x、y和z方向陀螺常值误差，Rn表示地球卯酉圈半径，Re表示地球子午圈半径，ωie表示地球自转角速率，L表示纬度，H表示高度，δL表示纬度误差；
所述获取速度误差进一步包括根据下述公式计算所述速度误差：
Figure FDA0002218378610000021
，其中，
Figure FDA0002218378610000022
和
Figure FDA0002218378610000023
分别表示东、北、天方向的速度误差的导数，
Figure FDA0002218378610000024
和
Figure FDA0002218378610000025
分别表示导航坐标系下x、y和z方向的平台误差角，
Figure FDA0002218378610000026
和
Figure FDA0002218378610000027
分别表示导航坐标系下x、y和z方向的比力，vx、vy和vz分别表示东、北和天方向的速度，Rn表示地球卯酉圈半径，Re表示地球子午圈半径，ωie表示地球自转角速率，L表示纬度，H表示高度，δL表示纬度误差，δvx、δvy和δvz分别表示东、北和天方向的速度误差，
Figure FDA0002218378610000028
和
Figure FDA0002218378610000029
分别表示导航坐标系下x、y和z方向加速度计常值误差；
所述获取位置误差进一步包括根据下述公式计算所述位置误差：
Figure FDA00022183786100000210
其中，λ表示经度，L表示纬度，H表示高度，表示纬度误差导数，
Figure FDA00022183786100000212
表示经度误差导数，
Figure FDA00022183786100000213
表示高度误差导数，δvx、δvy和δvz分别表示东、北和天方向的速度误差，Rn表示地球卯酉圈半径，Re表示地球子午圈半径，δL表示纬度误差，vx表示东方向的速度；
所述获取传感器误差包括获取陀螺常值误差向量和加速度计常值误差向量，其中，所述获取陀螺常值误差向量进一步包括根据下述公式计算陀螺常值误差向量：
Figure FDA00022183786100000214
其中，
Figure FDA00022183786100000215
表示所述陀螺常值误差向量的导数，εc表示所述陀螺常值误差向量，
Figure FDA00022183786100000216
和分别表示导航坐标系下x、y和z方向陀螺常值误差，
所述获取加速度计常值误差向量进一步包括根据下述公式计算所述加速度计常值误差向量：
Figure FDA00022183786100000218
其中，
Figure FDA0002218378610000031
表示所述加速度计常值误差向量的导数，
Figure FDA0002218378610000032
表示所述加速度计常值误差向量，
Figure FDA0002218378610000033
和
Figure FDA0002218378610000034
分别表示导航坐标系下x、y和z方向加速度计常值误差；
所述获取杠杆臂误差进一步包括根据下述公式计算所述杠杆臂误差：
Figure FDA0002218378610000035
其中，
Figure FDA0002218378610000036
表示所述杠杆臂误差向量的导数，δla表示所述杠杆臂误差向量，δlax、δlay和δlaz分别表示导航坐标系下x、y和z方向的杠杆臂误差。
4.根据权利要求1或3所述的方法，其特征在于，所述根据所述系统误差确定系统状态进一步包括：
根据所述系统误差获取系统的状态向量X(k)，所述状态向量X(k)为
Figure FDA0002218378610000037
其中，和
Figure FDA0002218378610000039
分别表示导航坐标系下x、y和z方向的平台误差角导数，
Figure FDA00022183786100000310
和
Figure FDA00022183786100000311
分别表示东、北、天方向的速度误差的导数，
Figure FDA00022183786100000312
表示纬度误差导数，
Figure FDA00022183786100000313
表示经度误差导数，
Figure FDA00022183786100000314
表示高度误差导数，
Figure FDA00022183786100000315
和
Figure FDA00022183786100000316
分别表示导航坐标系下x、y和z方向的杠杆臂误差的导数；
所述系统的状态向量X(k)满足下述状态方程：
其中，
Figure FDA00022183786100000318
表示状态向量导数，X(t)表示状态向量，F(t)表示状态系数矩阵，W(t)表示状态噪声矩阵。
5.根据权利要求1所述的方法，其特征在于，所述获取所述惯性导航系统和所述全球导航卫星系统的可量测信息包括：
获取速度观测误差、位置观测误差和平台误差角。
6.根据权利要求5所述的方法，其特征在于，
所述获取速度观测误差进一步包括根据下述公式计算所述速度观测误差：
其中，vxI、vyI和vzI分别表示惯性导航系统输出的导航坐标系下x、y和z方向速度，vxG、vyG和vzG分别表示卫星导航系统输出的导航坐标系下x、y和z方向速度，δvx、δvy和δvz分别表示东、北和天方向的速度误差，Mx、My和Mz分别表示x、y和z方向的观测噪声；
所述获取位置观测误差进一步包括根据下述公式计算所述位置误差：
Figure FDA0002218378610000041
其中，λI、LI和HI分别表示惯性导航系统输出的经度、纬度和高度信息，λG、LG和HG分别表示卫星导航系统输出的经度、纬度和高度信息，δλ、δL和δH分别表示经度误差、纬度误差和高度误差，
Figure FDA0002218378610000042
表示载体坐标系b到导航坐标系n的转移矩阵，δla×表示杠杆臂误差向量对称阵，
Figure FDA0002218378610000043
表示平台误差角向量的对称阵；
Figure FDA0002218378610000044
表示杠杆臂误差向量的导数的对称阵；
所述获取平台误差角进一步包括根据下述公式计算所述平台误差角：
Figure FDA0002218378610000045
其中，yawI表示惯性导航系统输出的偏航角，yawM表示通过地磁计算的偏航角，
Figure FDA0002218378610000046
表示导航坐标系下z方向的平台误差角，Ryaw表示偏航角误差的观测噪声。
7.根据权利要求5或6所述的方法，其特征在于，所述获取所述惯性导航系统和所述全球导航卫星系统的可量测信息进一步包括：
结合所述速度观测误差、所述位置观测误差和所述平台误差角根据下述公式计算所述系统的可量测信息所满足的观测方程：
Zk＝HkXk+Vk，
其中，Zk表示在k时刻的m维量测矢量，Hk表示k时刻的量测系数矩阵，Xk表示系统在k时刻的m维状态向量，Vk表示k时刻的m维量测噪声矢量。
8.根据权利要求1所述的方法，其特征在于，所述通过卡尔曼滤波器，利用固定间隔平滑算法，根据所述系统状态和所述可量测信息获取对离线杠杆臂的优化估计进一步包括：
通过卡尔曼滤波器，利用固定间隔平滑算法，根据所述状态方程和所述观测方程获取对所述状态向量x(k)的优化估计作为对离线杠杆臂的优化估计。
9.根据权利要求8所述的方法，其特征在于，所述通过卡尔曼滤波器进一步包括：
所述卡尔曼滤波估计满足下述公式：
Figure FDA0002218378610000051
在离散系统方程中，其中，Фk,k-1表示(k-1)时刻到k时刻系统状态的转移矩阵，Xk-1表示(k-1)时刻的状态向量，Гk-1表示(k-1)时刻的噪声系数矩阵，Wk-1表示(k-1)时刻的噪声矢量，Zk表示在k时刻的m维量测矢量，Hk表示k时刻的量测系数矩阵，Xk表示系统在k时刻的m维状态向量，Vk表示k时刻的m维量测噪声矢量。
CN201910923950.4A 2019-09-27 2019-09-27 离线杠杆臂估计方法 Pending CN110672124A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910923950.4A CN110672124A (zh) 2019-09-27 2019-09-27 离线杠杆臂估计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910923950.4A CN110672124A (zh) 2019-09-27 2019-09-27 离线杠杆臂估计方法

Publications (1)

Publication Number Publication Date
CN110672124A true CN110672124A (zh) 2020-01-10

Family

ID=69079771

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910923950.4A Pending CN110672124A (zh) 2019-09-27 2019-09-27 离线杠杆臂估计方法

Country Status (1)

Country Link
CN (1) CN110672124A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114563004A (zh) * 2022-04-29 2022-05-31 季华实验室 组合导航系统杆臂值在线测量方法及系统

Citations (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103344259A (zh) * 2013-07-11 2013-10-09 北京航空航天大学 一种基于杆臂估计的ins/gps组合导航系统反馈校正方法
CN104019828A (zh) * 2014-05-12 2014-09-03 南京航空航天大学 高动态环境下惯性导航系统杆臂效应误差在线标定方法
CN104949687A (zh) * 2014-03-31 2015-09-30 北京自动化控制设备研究所 一种长时间导航系统全参数精度评估方法
US20150276413A1 (en) * 2014-03-31 2015-10-01 Honeywell International Inc. Global positioning system (gps) self-calibrating lever arm function
CN105571578A (zh) * 2015-12-14 2016-05-11 武汉大学 一种利用伪观测取代精密转台的原地旋转调制寻北方法
CN106482746A (zh) * 2016-11-25 2017-03-08 北京航空航天大学 一种用于混合式惯导系统的加速度计内杆臂标定与补偿方法
CN107656301A (zh) * 2017-09-20 2018-02-02 北京航天发射技术研究所 一种基于多源信息融合的车载定位方法
CN107727097A (zh) * 2017-09-18 2018-02-23 北京航空航天大学 基于机载分布式位置姿态测量系统的信息融合方法和装置
CN108225312A (zh) * 2017-12-27 2018-06-29 中国电子科技集团公司第五十四研究所 一种gnss/ins松组合中杆臂估计以及补偿方法
CN108549397A (zh) * 2018-04-19 2018-09-18 武汉大学 基于二维码和惯导辅助的无人机自主降落方法及系统
CN110221333A (zh) * 2019-04-11 2019-09-10 同济大学 一种车载ins/od组合导航系统的量测误差补偿方法
CN110221332A (zh) * 2019-04-11 2019-09-10 同济大学 一种车载gnss/ins组合导航的动态杆臂误差估计和补偿方法

Patent Citations (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103344259A (zh) * 2013-07-11 2013-10-09 北京航空航天大学 一种基于杆臂估计的ins/gps组合导航系统反馈校正方法
CN104949687A (zh) * 2014-03-31 2015-09-30 北京自动化控制设备研究所 一种长时间导航系统全参数精度评估方法
US20150276413A1 (en) * 2014-03-31 2015-10-01 Honeywell International Inc. Global positioning system (gps) self-calibrating lever arm function
CN104019828A (zh) * 2014-05-12 2014-09-03 南京航空航天大学 高动态环境下惯性导航系统杆臂效应误差在线标定方法
CN105571578A (zh) * 2015-12-14 2016-05-11 武汉大学 一种利用伪观测取代精密转台的原地旋转调制寻北方法
CN106482746A (zh) * 2016-11-25 2017-03-08 北京航空航天大学 一种用于混合式惯导系统的加速度计内杆臂标定与补偿方法
CN107727097A (zh) * 2017-09-18 2018-02-23 北京航空航天大学 基于机载分布式位置姿态测量系统的信息融合方法和装置
CN107656301A (zh) * 2017-09-20 2018-02-02 北京航天发射技术研究所 一种基于多源信息融合的车载定位方法
CN108225312A (zh) * 2017-12-27 2018-06-29 中国电子科技集团公司第五十四研究所 一种gnss/ins松组合中杆臂估计以及补偿方法
CN108549397A (zh) * 2018-04-19 2018-09-18 武汉大学 基于二维码和惯导辅助的无人机自主降落方法及系统
CN110221333A (zh) * 2019-04-11 2019-09-10 同济大学 一种车载ins/od组合导航系统的量测误差补偿方法
CN110221332A (zh) * 2019-04-11 2019-09-10 同济大学 一种车载gnss/ins组合导航的动态杆臂误差估计和补偿方法

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
南京航空航天大学科技部: "《南京航空航天大学论文集 2006年 第13册 自动化学院 第3分册》", 31 March 2007 *
南京航空航天大学科技部: "《南京航空航天大学论文集 2010年 第13册 自动化学院 第1分册》", 31 May 2011, 南京航空航天大学科技部 *
张红良等: "《陆用高精度激光陀螺捷联惯导系统误差参数估计方法研究》", 28 February 2017, 国防工业出版社 *
高社生等: "《组合导航原理及应用》", 30 September 2012, 西北工业大学出版社 *

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114563004A (zh) * 2022-04-29 2022-05-31 季华实验室 组合导航系统杆臂值在线测量方法及系统
CN114563004B (zh) * 2022-04-29 2022-07-22 季华实验室 组合导航系统杆臂值在线测量方法及系统

Similar Documents

Publication Publication Date Title
CN112629538B (zh) 基于融合互补滤波和卡尔曼滤波的舰船水平姿态测量方法
CN110221332B (zh) 一种车载gnss/ins组合导航的动态杆臂误差估计和补偿方法
CN108827310B (zh) 一种船用星敏感器辅助陀螺仪在线标定方法
US6459990B1 (en) Self-contained positioning method and system thereof for water and land vehicles
EP1585939B1 (en) Attitude change kalman filter measurement apparatus and method
CN109000640B (zh) 基于离散灰色神经网络模型的车辆gnss/ins组合导航方法
CN101949703B (zh) 一种捷联惯性/卫星组合导航滤波方法
US20100121601A1 (en) Method and system for estimation of inertial sensor errors in remote inertial measurement unit
CN112504275B (zh) 一种基于级联卡尔曼滤波算法的水面舰船水平姿态测量方法
WO2007059134A1 (en) Dead reckoning system
CN111121766B (zh) 一种基于星光矢量的天文与惯性组合导航方法
CN102829777A (zh) 自主式水下机器人组合导航系统及方法
JP2001221652A (ja) 自動車用ナビゲーション・システムのための慣性誘導装置及び方法
CN104697520B (zh) 一体化无陀螺捷联惯导系统与gps系统组合导航方法
CN111323012B (zh) 载体高动态环境下的ins辅助dvl测速误差补偿方法
CN117053782A (zh) 一种水陆两栖机器人组合导航方法
JP5164645B2 (ja) カルマンフィルタ処理における繰り返し演算制御方法及び装置
CN112432642A (zh) 一种重力灯塔与惯性导航融合定位方法及系统
CN114964222A (zh) 一种车载imu姿态初始化方法、安装角估计方法及装置
CN111220151B (zh) 载体系下考虑温度模型的惯性和里程计组合导航方法
CN113503892A (zh) 一种基于里程计和回溯导航的惯导系统动基座初始对准方法
CN116222551A (zh) 一种融合多种数据的水下导航方法及装置
CN112197765B (zh) 一种实现水下机器人精细导航的方法
CN111982126B (zh) 一种全源BeiDou/SINS弹性状态观测器模型设计方法
CN111812737B (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
RJ01 Rejection of invention patent application after publication

Application publication date: 20200110

RJ01 Rejection of invention patent application after publication