CN113188565B - 一种机载分布式pos传递对准量测异常处理方法 - Google Patents
一种机载分布式pos传递对准量测异常处理方法 Download PDFInfo
- Publication number
- CN113188565B CN113188565B CN202110308355.7A CN202110308355A CN113188565B CN 113188565 B CN113188565 B CN 113188565B CN 202110308355 A CN202110308355 A CN 202110308355A CN 113188565 B CN113188565 B CN 113188565B
- Authority
- CN
- China
- Prior art keywords
- imu
- sub
- error
- model
- relative
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000005259 measurement Methods 0.000 title claims abstract description 232
- 238000000034 method Methods 0.000 title claims abstract description 56
- 238000012546 transfer Methods 0.000 title claims abstract description 47
- 239000000835 fiber Substances 0.000 claims abstract description 90
- 238000013178 mathematical model Methods 0.000 claims abstract description 12
- 238000001914 filtration Methods 0.000 claims abstract description 9
- 238000012545 processing Methods 0.000 claims abstract description 8
- 238000004364 calculation method Methods 0.000 claims description 66
- 238000012937 correction Methods 0.000 claims description 44
- 239000011159 matrix material Substances 0.000 claims description 36
- 101000912561 Bos taurus Fibrinogen gamma-B chain Proteins 0.000 claims description 10
- 230000005540 biological transmission Effects 0.000 claims description 9
- 238000006243 chemical reaction Methods 0.000 claims description 7
- 102000002274 Matrix Metalloproteinases Human genes 0.000 claims description 6
- 108010000684 Matrix Metalloproteinases Proteins 0.000 claims description 6
- 238000006073 displacement reaction Methods 0.000 claims description 5
- 238000003491 array Methods 0.000 claims description 3
- 230000005489 elastic deformation Effects 0.000 description 16
- 230000005856 abnormality Effects 0.000 description 6
- 238000003384 imaging method Methods 0.000 description 5
- 230000002159 abnormal effect Effects 0.000 description 2
- 238000004422 calculation algorithm Methods 0.000 description 2
- 230000007547 defect Effects 0.000 description 2
- 238000009434 installation Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 238000003672 processing method Methods 0.000 description 1
- 238000005309 stochastic process Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C23/00—Combined instruments indicating more than one navigational value, e.g. for aircraft; Combined measuring devices for measuring two or more variables of movement, e.g. distance, speed or acceleration
- G01C23/005—Flight directors
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Aviation & Aerospace Engineering (AREA)
- Navigation (AREA)
Abstract
本公开提供了一种机载分布式POS传递对准量测异常处理方法。该方法计算子IMU处光纤光栅传感器的相对位置和相对姿态测量误差;建立相对位置和相对姿态测量误差的各阶次多项式函数模型;采用最小二乘法确定各模型的系数,并计算出各模型的拟合残差,将拟合残差最小的模型作为最终的相对位置测量误差模型和相对姿态测量误差模型;修正主POS的位置和姿态测量信息,确定子IMU传递对准的量测量,并建立数学模型;采用卡尔曼滤波得到子IMU处更高精度的运动参数。该方法通过建立光纤光栅传感器的测量误差模型,以此修正高精度主POS的测量信息,进而建立出更加准确的量测方程,提高了系统量测模型的准确性,从而提高了传递对准的精度。
Description
技术领域
本发明涉及导航系统领域,具体涉及光纤光栅传感器测量异常导致机载分布式POS传递对准量测异常的处理方法。也可用于解决各种采用光纤光栅传感器测量弹性变形,并通过传递对准技术来确定位置、速度和姿态的捷联惯性导航系统。
背景技术
随着航空对地观测技术的进步,机载对地观测系统已从传统的单个观测载荷观测向多个或多类观测载荷联合成像的方向发展。对于装备了多个或多种观测载荷的高性能综合航空遥感系统,要实现高精度成像,需要获取各载荷安置点的高精度运动参数。机载分布式POS(Position and Orientation System,POS)可以通过与遥感载荷集成安装于统一平台,为机载综合对地观测系统提供各遥感载荷所需的高精度运动参数,从而辅助遥感载荷实现高精度成像。机载分布式POS已经成为提升机载对地观测系统成像精度的制约性因素。
机载分布式POS通常由四个部分组成:一个高精度主POS、多个中/低精度子IMU(Inertial Measurement Unit,IMU)、数据处理计算机和后处理软件。其中,高精度主POS也称之为主系统,一般位于机舱内;子IMU也称之为子系统,尽可能地与各遥感载荷靠近安装,一般分布安装在两侧机翼上,依靠高精度主POS的位置、速度、姿态等运动参数对其进行传递对准,以实现所在处运动信息的准确测量。然而,在实际成像过程中,由于多个或多种观测载荷安装在飞机的不同位置,飞机的弹性变形将导致各载荷间的相对空间关系随时间变化。机载分布式POS的主POS与子IMU间除了确定性的杆臂误差和安装误差角以外,还存在随时间变化的弹性变形。弹性变形的估计/测量精度直接决定主POS与子IMU间传递对准的精度,即决定了分布式POS各节点运动参数的测量精度。
针对机翼弹性变形的研究主要分为两个方面:一是建模估计,二是基于传感器的精确测量。在机翼弹性变形的建模估计方面,有学者将机翼的柔性变形等效为随机的二阶或三阶Markov过程,而Markov过程模型的参数不是根据系统的实际安装方式以及所受外力情况确定的,而是凭经验设定或依赖弹性变形的估计信息来确定,因此该模型往往缺乏准确性。例如专利号为201410168711.X的专利利用卡尔曼滤波估计出的弹性变形角和弹性变形角速度来计算马尔科夫过程参数,由于并没有弹性变形的直接观测信息,因此估计结果的精度有限,直接限制了马尔科夫过程参数的计算精度。此外,也可以基于弹性力学理论对机翼弹性变形进行建模。但是由于该方法所建立的模型没有考虑到外挂载荷对飞机弹性变形的影响,建立的模型难以应对实际情况的变化,因此缺乏广泛适用性。相比于对机翼弹性变形进行建模估计,通过传感器直接测量各子IMU安装点处相对于主系统的弹性变形,并在传递对准中对主POS与子IMU之间的转换关系进行补偿的方式,能够直接且有效的提高各子IMU所在处运动参数的估计精度。光纤光栅传感器技术是近年来逐步兴起的一种接触式测量技术,能够直接测量各子IMU安装点处相对于主系统的弹性变形。光纤光栅传感器体积小、重量轻,几乎不会对被测对象产生额外的负载作用,广泛用于桥梁、舰船结构及航空结构变形的监测。但光纤光栅传感器测量也有自身的不足,它通过测量所在处的结构应变来计算位移和挠度时,需要进行积分运算,这就导致形变测量误差随空间积累,因此光纤光栅传感器的测量精度受变形幅度的影响。即,结构变形越大的地方,光纤光栅传感器的测量精度越低。当被测对象剧烈振动、振动幅值大时,光纤光栅传感器会输出误差较大的位移(相对位置)和挠度(相对姿态)异常值,即出现测量异常。由于在传递对准中,是将子IMU的运动参数与经过光纤光栅传感器测量值修正后的主POS运动参数之差作为量测量,因此光纤光栅传感器的测量异常将直接导致传递对准的量测出现异常。如果不对光纤光栅传感器的测量异常值进行补偿,将直接降低传递对准的精度,甚至导致传递对准异常。
发明内容
本发明的技术解决问题是:克服现有技术的不足,提出一种机载分布式POS传递对准量测异常处理方法,该方法可提高机载分布式POS传递对准的精度。
本发明的技术解决方案为:一种机载分布式POS传递对准量测异常处理方法。其具体步骤如下:
(1)计算子IMU处光纤光栅传感器的相对位置和相对姿态测量误差;
(2)建立光纤光栅传感器相对位置、相对姿态测量误差的各阶次多项式函数模型;
(3)采用最小二乘方法,确定出各阶次多项式函数模型的待定系数,并计算各模型的拟合残差,将拟合残差最小的多项式函数模型作为最终的光纤光栅传感器测量误差模型,包括相对位置测量误差模型和相对姿态测量误差模型;
(4)利用光纤光栅传感器最终的相对位置和相对姿态测量误差模型,修正主POS的位置和姿态;
(5)确定子IMU传递对准的量测量,建立子IMU传递对准的数学模型;
(6)运用卡尔曼滤波,估计出子IMU的位置误差、速度误差和姿态误差,并对子IMU的捷联解算结果进行校正,得到子IMU处更高精度的运动参数,包括位置、速度和姿态。
上述步骤(1)中子IMU处光纤光栅传感器的相对位置和相对姿态测量误差的具体计算步骤为:
光纤光栅传感器输出的位移和挠度分别又称为相对位置和相对姿态。
1)计算相对位置测量误差
Δx′=Δx-Δxjizhun,Δy′=Δy-Δyjizhun,Δz′=Δz-Δzjizhun
其中,Δx、Δy、Δz分别是子IMU处光纤光栅传感器测量的相对位置在主POS载体坐标系x轴、y轴和z轴上的分量;Δx′、Δy′、Δz′分别是Δx、Δy、Δz的误差,即光纤光栅传感器相对位置的测量误差;Δxjizhun、Δyjihzhun、Δzjizhun是由高精度双目相机提供的主POS载体坐标系x轴、y轴和z轴上相对位置的基准数据。
2)计算相对姿态测量误差
Δψ′=Δψ-Δψjizhun,Δθ′=Δθ-Δθjizhun,Δγ′=Δγ-Δγjizhun
其中,Δψ、Δθ、Δγ是子IMU处光纤光栅传感器测量的相对姿态,即相对航向角、相对俯仰角、相对横滚角;Δψ′、Δθ′、Δγ′分别是Δψ、Δθ、Δγ的测量误差;Δψjizhun、Δθjizhun、Δγjizhun是主POS与子IMU处高精度基准POS二者的航向角之差、俯仰角之差和横滚角之差,作为相对姿态基准数据。
上述步骤(2)中建立光纤光栅传感器相对位置、相对姿态测量误差的各阶次多项式函数模型的具体步骤为:
1)建立相对姿态测量误差的各阶次多项式函数模型
光纤光栅传感器相对姿态测量误差Δψ′、Δθ′、Δγ′与子IMU处光纤光栅传感器测量的相对姿态Δψ、Δθ、Δγ呈近似线性关系,因此建立相对姿态测量误差的一阶和二阶多项式函数模型。
a)一阶多项式函数模型
Δψ′=a1Δψ+a0,Δθ′=a3Δθ+a2,Δγ′=a5Δγ+a4
式中,(a1,a0)、(a3,a2)、(a5,a4)分别为相对姿态测量误差Δψ′、Δθ′、Δγ′的一阶多项式函数模型待定系数。
b)二阶多项式函数模型
Δψ′=b2Δψ2+b1Δψ+b0,Δθ′=b5Δθ2+b4Δθ+b3,Δγ′=b8Δγ2+b7Δγ+b6
式中,(b2,b1,b0)、(b5,b4,b3)、(b8,b7,b6)分别为相对姿态测量误差Δψ′、Δθ′、Δγ′的二阶多项式函数模型待定系数。
2)建立相对位置测量误差的各阶次多项式函数模型
光纤光栅传感器相对位置测量误差Δx′、Δy′、Δz′与子IMU处光纤光栅传感器测量的相对位置Δx、Δy、Δz呈近似线性关系,因此建立相对位置测量误差的一阶和二阶多项式函数模型。
a)一阶多项式函数模型
Δx′=a′1Δx+a′0,Δy′=a′3Δy+a′2,Δz′=a′5Δz+a′4
式中,(a′1,a′0)、(a′3,a′2)、(a′5,a′4)分别为相对位置测量误差Δx′、Δy′、Δz′的一阶多项式函数模型待定系数。
b)二阶多项式函数模型
Δx′=b′2Δx2+b′1Δx+b′0,Δy′=b′5Δy2+b′4Δy+b′3,Δz′=b′8Δz2+b′7Δz+b′6
式中,(b′2,b′1,b′0)、(b′5,b′4,b′3)、(b′8,b′7,b′6)分别为相对位置测量误差Δx′、Δy′、Δz′的二阶多项式函数模型待定系数。
上述步骤(3)采用最小二乘方法,确定出各阶次多项式函数模型的待定系数,并计算各模型的拟合残差,将拟合残差最小的多项式函数模型作为最终的光纤光栅传感器测量误差模型,具体步骤为:
1)求解各阶次多项式函数模型的待定系数
将光纤光栅传感器相对姿态测量误差、相对位置测量误差数据与子IMU处光纤光栅传感器测量的相对姿态、相对位置数据代入步骤(2)建立的各阶次多项式函数模型。
a)求解相对姿态测量误差各阶次多项式函数模型的待定系数
对于相对航向角Δψ测量误差的一阶多项式函数模型,令该模型的拟合残差则a1,a0的计算公式为:
其中n1为Δψ测量误差数据的总个数;
对于相对航向角Δψ测量误差的二阶多项式函数模型,令该模型的拟合残差则b2,b1,b0的计算公式为:
对于相对俯仰角Δθ测量误差的一阶多项式函数模型,令该模型的拟合残差则a3,a2的计算公式为:
其中n2为代入的Δθ测量误差数据的个数;
对于相对俯仰角Δθ测量误差的二阶多项式函数模型,令该模型的拟合残差则b5,b4,b3的计算公式为:
对于相对横滚角Δγ测量误差的一阶多项式函数模型,令该模型的拟合残差则a5,a4的计算公式为:
其中n3为代入的Δγ测量误差数据的个数;
对于相对横滚角Δγ测量误差二阶多项式函数模型,令该模型的拟合残差则b8,b7,b6的计算公式为:
b)求解相对位置测量误差各阶次多项式函数模型的待定系数
对于x轴相对位置Δx测量误差的一阶多项式函数模型,令该模型的拟合残差则a′1,a′0的计算公式为:
其中n4为代入的Δx测量误差数据的个数;
对于x轴相对位置Δx测量误差二阶多项式函数模型,令该模型的拟合残差则b′2,b′1,b′0的计算公式为:
对于y轴相对位置Δy测量误差一阶多项式函数模型,令该模型的拟合残差则a′3,a′2的计算公式为:
其中n5为代入的Δy测量误差数据的个数;
对于y轴相对位置Δy测量误差二阶多项式函数模型,令该模型的拟合残差则b′5,b′4,b′3的计算公式为:
对于z轴相对位置Δz测量误差一阶多项式函数模型,令该模型的拟合残差则a′5,a′4的计算公式为:
其中n6为代入的Δz测量误差数据的个数;
对于z轴相对位置Δz测量误差二阶多项式函数模型,令该模型的拟合残差则b′8,b′7,b′6的计算公式为:
2)计算各阶次多项式函数模型的拟合残差并确定最终的光纤光栅传感器测量误差模型
根据待定系数求取各多项式模型的拟合残差,多项式函数模型的拟合残差越小,该模型的整体拟合程度越好。
a)计算相对姿态测量误差各阶次多项式函数模型的拟合残差
相对航向角Δψ各阶测量误差模型拟合残差:
一阶多项式函数模型拟合残差:
二阶多项式函数模型拟合残差:
当s1min<s2min时,相对航向角Δψ最终测量误差模型为Δψ′=a1Δψ+a0;
当s1min>s2min时,相对航向角Δψ最终测量误差模型为Δψ′=b2Δψ2+b1Δψ+b0;相对俯仰角Δθ各阶测量误差模型拟合残差:
一阶多项式函数模型拟合残差:
二阶多项式函数模型拟合残差:
当s3min<s4min时,相对俯仰角Δθ最终测量误差模型为Δθ′=a3Δθ+a2;
当s3min>s4min时,相对俯仰角Δθ最终测量误差模型为Δθ′=b5Δθ2+b4Δθ+b3;
相对横滚角Δγ各阶测量误差模型拟合残差:
一阶多项式函数模型拟合残差:
二阶多项式函数模型拟合残差:
当s5min<s6min时,相对横滚角Δγ最终测量误差模型为Δγ′=a5Δγ+a4;
当s5min>s6min时,相对横滚角Δγ最终测量误差模型为Δγ′=b8Δγ2+b7Δγ+b6;
b)计算相对位置测量误差各阶次多项式函数模型的拟合残差
x轴相对位置Δx测量误差模型拟合残差:
一阶多项式函数模型拟合残差:
二阶多项式函数模型拟合残差:
当s′1min<s′2min时,x轴相对位置Δx最终测量误差模型为Δx′=a′1Δx+a′0;
当s′1min>s′2min时,x轴相对位置Δx最终测量误差模型为Δx′=b′2Δx2+b′1Δx+b′0;
y轴相对位置Δy测量误差模型拟合残差:
一阶多项式函数模型拟合残差:
二阶多项式函数模型拟合残差:
当s′3min<s′4min时,y轴相对位置Δy最终测量误差模型为Δy′=a′3Δy+a′2;
当s′3min>s′4min时,y轴相对位置Δy最终测量误差模型为Δy′=b′5Δy2+b′4Δy+b′3;
z轴相对位置Δz测量误差模型拟合残差:
一阶多项式函数模型拟合残差:
二阶多项式函数模型拟合残差:
当s′5min<s′6min时,z轴相对位置Δz最终测量误差模型为Δz′=a′5Δz+a′4;
当s′5min>s′6min时,z轴相对位置Δz最终测量误差模型为Δz′=b′8Δz2+b′7Δz+b′6。
上述步骤(4)中利用光纤光栅传感器最终的相对位置和相对姿态测量误差模型,修正主POS的位置和姿态,具体步骤为:
1)位置修正
式中,L′m、λ′m、h′m分别表示主POS经位置修正后的纬度、经度和高度;Lm、λm、hm分别表示主POS在传递对准前经光纤光栅传感器数据进行柔性变形补偿后的纬度、经度和高度,RM和RN分别表示地球子午圈和卯酉圈的主曲率半径,Δx′、Δy′、Δz′为子IMU处光纤光栅传感器的相对位置测量误差,为主POS的姿态矩阵,ψm、θm、γm分别表示主POS在传递对准前经光纤光栅传感器数据进行柔性变形补偿后的航向角、俯仰角和横滚角。
2)姿态修正
其中,为主POS经姿态修正后的姿态矩阵,CFBG为由光纤光栅传感器的相对姿态测量误差组成的姿态修正矩阵。
式中,ψ′m、θ′m、γ′m分别表示主POS经姿态修正后的航向角、俯仰角和横滚角,Δψ′、Δθ′、Δγ′是子IMU处光纤光栅传感器的相对姿态测量误差。
将记为:
其中,Clm为矩阵中第l行、第m列的元素,l=1,2,3,m=1,2,3;则主POS经姿态修正后的航向角ψ′m、俯仰角θ′m和横滚角γ′m的主值,即ψ′m主、θ′m主和ψ′m主分别为:
θ′m主=arcsin(C32),/>
航向角ψ′m、俯仰角θ′m和横滚角γ′m的取值范围分别定义为[0,2π]、 [-π,+π];那么,ψ′m、θ′m和γ′m的真值可由下式确定:
θ′m=θ′m主,/>
上述步骤(5)中确定子IMU传递对准的量测量,建立子IMU传递对准的数学模型的具体步骤为:
1)建立子IMU惯导误差模型
相关参考坐标系的定义包括:i代表地心惯性坐标系;e代表地球坐标系;主POS和子IMU导航坐标系均为东北天地理坐标系,分别用nm和n表示。b代表子IMU载体坐标系,bm代表主POS载体坐标系。
根据上述定义,子IMU惯导误差模型为:
a)姿态误差方程:
式中,φn=[φEφNφU]T为子IMU的失准角,φE、φN和φU分别为子IMU的东向、北向和天向失准角,下标E、N和U分别表示导航坐标系下的东向、北向和天向;表示子IMU导航坐标系下导航坐标系相对于惯性坐标系的转动角速度,/>为其误差;其中/>为子IMU捷联解算得到的姿态矩阵,εb为子IMU陀螺仪在子IMU载体坐标系的随机常值漂移,/>为子IMU陀螺仪在子IMU载体坐标系的随机噪声。
b)速度误差方程:
式中,Vn=[VE VN VU]T为子IMU导航坐标系下子IMU捷联解算得到的速度,VE、VN、VU分别为子IMU捷联解算得到的东向、北向和天向速度;速度误差δVn=[δVE δVN δVU]T,δVE、δVN和δVU分别为子IMU的东向、北向和天向速度误差。fb=[fx b fy b fz b]T为子IMU加速度计在子IMU载体坐标系上测量的比力,分别为比力在子IMU载体坐标系三个轴上的分量;表示子IMU导航坐标系下地球坐标系相对于惯性坐标系的转动角速度;/>表示子IMU导航坐标系下导航坐标系相对于地球坐标系的转动角速度;/>和/>分别为/>和/>的误差;/>表示子IMU加速度计在子IMU载体坐标系的随机常值偏置,/>为子IMU加速度计在子IMU载体坐标系的随机噪声;/>L、λ、h分别为子IMU捷联解算得到纬度、经度和高度,RM和RN分别表示地球子午圈和卯酉圈的主曲率半径。
c)位置误差方程:
式中,L、λ、h分别为子IMU捷联解算得到纬度、经度和高度,δL、δλ和δh分别为L、λ、h的误差,即子IMU的纬度误差、经度误差和高度误差。
d)惯性仪表误差方程:
陀螺仪的误差和加速度计的误差/>是非常复杂的随机过程,可以建为一个随机常值误差和一个高斯白噪声的叠加,具体表示为:
其中,εb和分别表示陀螺仪在子IMU载体坐标系的随机常值漂移和随机噪声,和/>分别表示加速度计在子IMU载体坐标系的随机常值偏置和随机噪声。εb和/>的数学模型为:
其中,εb=[εx εy εz]T,εx、εy和εz分别为子IMU载体坐标系三个轴向上的陀螺仪随机常值漂移,和/>分别为子IMU载体坐标系三个轴向上的加速度计随机常值偏置,脚标x、y、z分别表示子IMU载体坐标系的三个坐标轴。/>
2)建立子IMU传递对准的数学模型
状态方程为:
式中,状态变量F为系统矩阵,/>为系统噪声矩阵,其中/>为子IMU陀螺仪在子IMU载体坐标系下的随机噪声,/>和/>分别为子IMU载体系三轴上的陀螺随机噪声;/>为子IMU加速度计在子IMU载体坐标系下的随机噪声,/>和/>分别为子IMU载体系三轴上的加速度计随机噪声,G为噪声驱动矩阵。
/>
将经过光纤光栅传感器最终测量误差模型修正后的主POS位置和姿态信息,分别与子IMU捷联解算的位置和姿态的差值作为量测量,采用“位置+姿态”匹配方式,建立子IMU传递对准的量测方程:
Z=HX+v
式中,量测向量Z=[δψΔ δθΔ δγΔ δLΔ δλΔ δhΔ]T,其中,δψΔ、δθΔ、δγΔ是主POS经姿态修正后与子IMU的航向角、俯仰角和横滚角之差,δLΔ、δγΔ、δhΔ是主POS经位置修正后与子IMU的纬度、经度和高度之差。v=[vδψ vδθ vδγ vδL vδλ vδh]T为量测噪声矩阵,vδψ、vδθ、vδγ分别为主POS航向角、俯仰角、横滚角的量测噪声,vδL、vδλ、vδh分别为主POS纬度、经度、高度的量测噪声,H为量测矩阵,具体表达式为:
上述步骤(6)中运用卡尔曼滤波,估计出子IMU的位置误差、速度误差和姿态误差,并对子IMU的捷联解算结果进行校正,得到子IMU处更高精度的运动参数,包括位置、速度和姿态的具体步骤为:
1)估计子IMU的姿态误差、速度误差和位置误差
基于步骤(5)建立的子IMU传递对准数学模型,运用卡尔曼滤波估计出子IMU的失准角φE、φN、φU,速度误差δVE、δVN、δVU,位置误差δL、δλ和δh;
2)利用上述误差校正子IMU的捷联解算结果,得到更高精度的子IMU位置、速度和姿态
a)速度校正
V′E=VE-δVE,V′N=VN-δVN,V′U=VU-δVU
其中,V′E、V′N、V′U分别为校正后的子IMU东向、北向和天向速度;VE、VN、VU分别为子IMU捷联解算得到的东向、北向和天向速度;δVE、δVN、δVU分别为子IMU东向、北向和天向速度误差;
b)位置校正
L′=L-δL,λ′=λ-δλ,h′=h-δh
其中,L′、λ′、h′分别为校正后的子IMU纬度、经度和高度;L、λ、h分别为子IMU捷联解算得到纬度、经度和高度;δL、δλ和δh分别为子IMU纬度、经度和高度误差;
c)姿态校正
计算子IMU地理坐标系即导航坐标系n与计算地理坐标系n′间的转换矩阵
更新子IMU的姿态转换矩阵其中/>为子IMU捷联解算得到的姿态矩阵;
由更新后的子IMU的姿态阵计算子IMU的航向角ψs、俯仰角θs和横滚角γs。将记为:
其中Tlm为矩阵中第l行、第m列的元素,l=1,2,3,m=1,2,3;则子IMU航向角ψs、俯仰角θs和横滚角γs的主值,即ψs主、θs主和γs主分别为:
θs主=arcsin(T32),/>
航向角ψs、俯仰角θs和横滚角γs的取值范围分别定义为[0,2π]、 [-π,+π];那么,ψs、θs和γs的真值可由下式确定:
θs=θs主,/>
通过对子IMU的速度、位置和姿态进行校正,得到更高精度的子IMU安装点的速度、位置和姿态信息,完成传递对准。
上述步骤针对的是一主到一子系统的传递对准,对于含有多个子IMU的分布式POS而言,对主POS到每一个子IMU的传递对准都进行上述处理。
本发明与现有技术相比的优点在于:
针对分布式POS传递对准中,测量主、子系统间弹性变形的光纤光栅传感器量测异常的问题,利用光纤光栅传感器相对位置与相对姿态测量数据和其对应的误差,基于多项式拟合算法建立了光纤光栅传感器相对位置与相对姿态的测量误差模型,进而修正高精度主POS经柔性变形补偿后的位置与姿态测量信息,建立更加准确的量测方程,最后采用卡尔曼滤波得到各子IMU更加准确的运动参数。克服了现有传递对准方法未对光纤光栅量测异常进行修正补偿导致传递对准精度低的不足。
附图说明
图1为本发明的流程图;
图2为本发明的基于多项式拟合算法建立误差模型的流程图;
具体实施方式
如图1所示,本发明的具体方法实施如下:
1、子IMU处光纤光栅传感器的相对位置和相对姿态测量误差的具体计算步骤为:
光纤光栅传感器输出的位移和挠度分别又称为相对位置和相对姿态。
(1)计算相对位置测量误差
Δx′=Δx-Δxjizhun,Δy′=Δy-Δyjizhun,Δz′=Δz-Δzjizhun (1)
其中,Δx、Δy、Δz分别是子IMU处光纤光栅传感器测量的相对位置在主POS载体坐标系x轴、y轴和z轴上的分量;Δx′、Δy′、Δz′分别是Δx、Δy、Δz的误差,即光纤光栅传感器相对位置的测量误差;Δxjizhun、Δyjihzhun、Δzjizhun是由高精度双目相机提供的主POS载体坐标系x轴、y轴和z轴上相对位置的基准数据。
(2)计算相对姿态测量误差
Δψ′=Δψ-Δψjizhun,Δθ′=Δθ-Δθjizhun,Δγ′=Δγ-Δγjizhun (2)
其中,Δψ、Δθ、Δγ是子IMU处光纤光栅传感器测量的相对姿态,即相对航向角、相对俯仰角、相对横滚角;Δψ′、Δθ′、Δγ′分别是Δψ、Δθ、Δγ的测量误差;Δψjizhun、Δθjizhun、Δγjizhun是主POS与子IMU处高精度基准POS二者的航向角之差、俯仰角之差和横滚角之差,作为相对姿态基准数据。
2、建立光纤光栅传感器相对位置、相对姿态测量误差的各阶次多项式函数模型,具体的步骤为:
(1)建立相对姿态测量误差的各阶次多项式函数模型
光纤光栅传感器相对姿态测量误差Δψ′、Δθ′、Δγ′与子IMU处光纤光栅传感器测量的相对姿态Δψ、Δθ、Δγ呈近似线性关系,因此建立相对姿态测量误差的一阶和二阶多项式函数模型。
a)一阶多项式函数模型
Δψ′=a1Δψ+a0,Δθ′=a3Δθ+a2,Δγ′=a5Δγ+a4 (3)
式中,(a1,a0)、(a3,a2)、(a5,a4)分别为相对姿态测量误差Δψ′、Δθ′、Δγ′的一阶多项式函数模型待定系数。
b)二阶多项式函数模型
Δψ′=b2Δψ2+b1Δψ+b0,Δθ′=b5Δθ2+b4Δθ+b3,Δγ′=b8Δγ2+b7Δγ+b6 (4)
式中,(b2,b1,b0)、(b5,b4,b3)、(b8,b7,b6)分别为相对姿态测量误差Δψ′、Δθ′、Δγ′的二阶多项式函数模型待定系数。
(2)建立相对位置测量误差的各阶次多项式函数模型
光纤光栅传感器相对位置测量误差Δx′、Δy′、Δz′与子IMU处光纤光栅传感器测量的相对位置Δx、Δy、Δz呈近似线性关系,因此建立相对位置测量误差的一阶和二阶多项式函数模型。
a)一阶多项式函数模型
Δx′=a′1Δx+a′0,Δy′=a′3Δy+a′2,Δz′=a′5Δz+a′4 (5)
式中,(a′1,a′0)、(a′3,a′2)、(a′5,a′4)分别为相对位置测量误差Δx′、Δy′、Δz′的一阶多项式函数模型待定系数。
b)二阶多项式函数模型
Δx′=b′2Δx2+b′1Δx+b′0,Δy′=b′5Δy2+b′4Δy+b′3,Δz′=b′8Δz2+b′7Δz+b′6 (6)
式中,(b′2,b′1,b′0)、(b′5,b′4,b′3)、(b′8,b′7,b′6)分别为相对位置测量误差Δx′、Δy′、Δz′的二阶多项式函数模型待定系数。
3、采用最小二乘方法,确定出各阶次多项式函数模型的待定系数,并计算各模型的拟合残差,将拟合残差最小的多项式函数模型作为最终的光纤光栅传感器测量误差模型,具体步骤为:
(1)求解各阶次多项式函数模型的待定系数
将光纤光栅传感器相对姿态测量误差、相对位置测量误差数据与子IMU处光纤光栅传感器测量的相对姿态、相对位置数据代入步骤2建立的各阶次多项式函数模型。
a)求解相对姿态测量误差各阶次多项式函数模型的待定系数
对于相对航向角Δψ测量误差的一阶多项式函数模型,令该模型的拟合
其中n1为Δψ测量误差数据的总个数;
对于相对航向角Δψ测量误差的二阶多项式函数模型,令该模型的拟合残差则b2,b1,b0的计算公式为:/>
对于相对俯仰角Δθ测量误差的一阶多项式函数模型,令该模型的拟合残差则a3,a2的计算公式为:
其中n2为代入的Δθ测量误差数据的个数;
对于相对俯仰角Δθ测量误差的二阶多项式函数模型,令该模型的拟合残差则b5,b4,b3的计算公式为:
对于相对横滚角Δγ测量误差的一阶多项式函数模型,令该模型的拟合残差则a5,a4的计算公式为:
其中n3为代入的Δγ测量误差数据的个数;
对于相对横滚角Δγ测量误差二阶多项式函数模型,令该模型的拟合残差则b8,b7,b6的计算公式为:/>
b)求解相对位置测量误差各阶次多项式函数模型的待定系数
对于x轴相对位置Δx测量误差的一阶多项式函数模型,令该模型的拟合残差则a′1,a′0的计算公式为:
其中n4为代入的Δx测量误差数据的个数;
对于x轴相对位置Δx测量误差二阶多项式函数模型,令该模型的拟合残差则b′2,b′1,b′0的计算公式为:
对于y轴相对位置Δy测量误差一阶多项式函数模型,令该模型的拟合残差则a′3,a′2的计算公式为:
其中n5为代入的Δy测量误差数据的个数;
对于y轴相对位置Δy测量误差二阶多项式函数模型,令该模型的拟合残差则b′5,b′4,b′3的计算公式为:/>
对于z轴相对位置Δz测量误差一阶多项式函数模型,令该模型的拟合残差则a′5,a′4的计算公式为:
其中n6为代入的Δz测量误差数据的个数;
对于z轴相对位置Δz测量误差二阶多项式函数模型,令该模型的拟合残差则b′8,b′7,b′6的计算公式为:
(2)计算各阶次多项式函数模型的拟合残差并确定最终的光纤光栅传感器测量误差模型
根据待定系数求取各多项式模型的拟合残差,多项式函数模型的拟合残差越小,该模型的整体拟合程度越好。
a)计算相对姿态测量误差各阶次多项式函数模型的拟合残差
相对航向角Δψ各阶测量误差模型拟合残差:
一阶多项式函数模型拟合残差:
二阶多项式函数模型拟合残差:
当s1min<s2min时,相对航向角Δψ最终测量误差模型为Δψ′=a1Δψ+a0;
当s1min>s2min时,相对航向角Δψ最终测量误差模型为Δψ′=b2Δψ2+b1Δψ+b0;
相对俯仰角Δθ各阶测量误差模型拟合残差:
一阶多项式函数模型拟合残差:
二阶多项式函数模型拟合残差:
当s3min<s4min时,相对俯仰角Δθ最终测量误差模型为Δθ′=a3Δθ+a2;
当s3min>s4min时,相对俯仰角Δθ最终测量误差模型为Δθ′=b5Δθ2+b4Δθ+b3;
相对横滚角Δγ各阶测量误差模型拟合残差:
一阶多项式函数模型拟合残差:
二阶多项式函数模型拟合残差:
当s5min<s6min时,相对横滚角Δγ最终测量误差模型为Δγ′=a5Δγ+a4;
当s5min>s6min时,相对横滚角Δγ最终测量误差模型为Δγ′=b8Δγ2+b7Δγ+b6;
b)计算相对位置测量误差各阶次多项式函数模型的拟合残差
x轴相对位置Δx测量误差模型拟合残差:
一阶多项式函数模型拟合残差:
二阶多项式函数模型拟合残差:
当s′1min<s′2min时,x轴相对位置Δx最终测量误差模型为Δx′=a′1Δx+a′0;
当s′1min>s′2min时,x轴相对位置Δx最终测量误差模型为Δx′=b′2Δx2+b′1Δx+b′0;
y轴相对位置Δy测量误差模型拟合残差:
一阶多项式函数模型拟合残差:
二阶多项式函数模型拟合残差:
当s′3min<s′4min时,y轴相对位置Δy最终测量误差模型为Δy′=a′3Δy+a′2;
当s′3min>s′4min时,y轴相对位置Δy最终测量误差模型为Δy′=b′5Δy2+b′4Δy+b′3;
z轴相对位置Δz测量误差模型拟合残差:
一阶多项式函数模型拟合残差:
二阶多项式函数模型拟合残差:
当s′5min<s′6min时,z轴相对位置Δz最终测量误差模型为Δz′=a′5Δz+a′4;
当s′5min>s′6min时,z轴相对位置Δz最终测量误差模型为Δz′=b′8Δz2+b′7Δz+b′6。
4、利用光纤光栅传感器最终的相对位置和相对姿态测量误差模型,修正主POS的位置和姿态,具体步骤为:
(1)位置修正
式中,L′m、λ′m、h′m分别表示主POS经位置修正后的纬度、经度和高度,Lm、λm、hm分别表示主POS在传递对准前经光纤光栅传感器数据进行柔性变形补偿后的纬度、经度和高度,RM和RN分别表示地球子午圈和卯酉圈的主曲率半径,Δx′、Δy′、Δz′为子IMU处光纤光栅传感器相对位置测量误差,为主POS的姿态矩阵,ψm、θm、γm分别表示主POS在传递对准前经光纤光栅传感器数据进行柔性变形补偿后的航向角、俯仰角和横滚角。
(2)姿态修正
其中,为主POS经姿态修正后的姿态矩阵,CFBG为由光纤光栅传感器的相对姿态测量误差组成的姿态修正矩阵。
式中,ψ′m、θ′m、γ′m分别表示主POS经姿态修正后的航向角、俯仰角和横滚角,Δψ′、Δθ′、Δγ′是子IMU处光纤光栅传感器的相对姿态测量误差。
将记为:/>
其中Clm为矩阵中第l行、第m列的元素,l=1,2,3,m=1,2,3;则主POS经姿态修正后的航向角ψ′m、俯仰角θ′m和横滚角γ′m的主值,即ψ′m主、θ′m主和ψ′m主分别为:
航向角ψ′m、俯仰角θ′m和横滚角γ′m的取值范围分别定义为[0,2π]、 [-π,+π];那么,ψ′m、θ′m和γ′m的真值可由下式确定:
θ′m=θ′m主,/>
5、确定子IMU传递对准的量测量,建立子IMU传递对准的数学模型的具体步骤为:
(1)建立子IMU惯导误差模型
相关参考坐标系的定义包括:i代表地心惯性坐标系;e代表地球坐标系;主POS和子IMU导航坐标系均为东北天地理坐标系,分别用nm和n表示。b代表子IMU载体坐标系,bm代表主POS载体坐标系。
根据上述定义,子IMU惯导误差模型为:
a)姿态角误差方程:
式中,φn=[φE φN φU]T为子IMU的失准角,φE、φN和φU分别为子IMU的东向、北向和天向失准角,下标E、N和U分别表示导航坐标系下的东向、北向和天向;表示子IMU导航坐标系下导航坐标系相对于惯性坐标系的转动角速度,/>为其误差;其中/>为子IMU捷联解算得到的姿态矩阵,εb为子IMU陀螺仪在子IMU载体坐标系的随机常值漂移,/>为子IMU陀螺仪在子IMU载体坐标系的随机噪声。
b)速度误差方程:
式中,Vn=[VE VN VU]T为子IMU导航坐标系下子IMU捷联解算得到的速度,VE、VN、VU分别为子IMU捷联解算得到的东向、北向和天向速度;速度误差δVn=[δVE δVN δVU]T,δVE、δVN和δVU分别为子IMU的东向、北向和天向速度误差。fb=[fx b fy b fz b]T为子IMU加速度计在子IMU载体坐标系上测量的比力,分别为比力在子IMU载体坐标系三个轴上的分量;表示子IMU导航坐标系下地球坐标系相对于惯性坐标系的转动角速度;/>表示子IMU导航坐标系下导航坐标系相对于地球坐标系的转动角速度;/>和/>分别为/>和/>的误差;/>表示子IMU加速度计在子IMU载体坐标系的随机常值偏置,/>为子IMU加速度计在子IMU载体坐标系的随机噪声;/>L、λ、h分别为子IMU捷联解算得到纬度、经度和高度,RM和RN分别表示地球子午圈和卯酉圈的主曲率半径。
c)位置误差方程:
式中,L、λ、h分别为子IMU捷联解算得到纬度、经度和高度,δL、δλ和δh分别为L、λ、h的误差,即子IMU的纬度误差、经度误差和高度误差。
d)惯性仪表误差方程:
陀螺仪的误差和加速度计的误差/>是非常复杂的随机过程,可以建模为一个随机常值误差和一个高斯白噪声的叠加,具体表示为:
其中,εb和分别表示陀螺仪在子IMU载体坐标系的随机常值漂移和随机噪声,和/>分别表示加速度计在子IMU载体坐标系的随机常值偏置和随机噪声。εb和/>的数学模型为:
其中εb=[εx εy εz]T,εx、εy和εz分别为子IMU载体坐标系三个轴向上的陀螺仪随机常值漂移,和/>分别为子IMU载体坐标系三个轴向上的加速度计随机常值偏置,脚标x、y、z分别表示子IMU载体坐标系的三个坐标轴。
(2)建立子IMU传递对准的数学模型
状态方程为:
式中,状态变量F为系统矩阵,/>为系统噪声矩阵,其中/>为子IMU陀螺仪在子IMU载体坐标系下的随机噪声,/>和/>分别为子IMU载体系三轴上的陀螺随机噪声;/>为子IMU加速度计在子IMU载体坐标系下的随机噪声,/>和/>分别为子IMU载体系三轴上的加速度计随机噪声,G为噪声驱动矩阵。
/>
将经过光纤光栅传感器最终的测量误差模型修正后的主POS位置和姿态信息,分别与子IMU捷联解算的位置和姿态的差值作为量测量,采用“位置+姿态”匹配方式,建立子IMU传递对准的量测方程:
Z=HX+v (33)
式中,量测向量Z=[δψΔ δθΔ δγΔ δLΔ δλΔ δhΔ]T,其中,δψΔ、δθΔ、δγΔ是主POS经姿态修正后与子IMU的航向角、俯仰角和横滚角之差,δLΔ、δγΔ、δhΔ是主POS经位置修正后与子IMU的纬度、经度和高度之差。v=[vδψ vδθ vδγ vδL vδλ vδh]T为量测噪声矩阵,vδψ、vδθ、vδγ分别为主POS航向角、俯仰角、横滚角的量测噪声,vδL、vδλ、vδh分别为主POS纬度、经度、高度的量测噪声,为主POS经姿态修正后的姿态矩阵,H为量测矩阵,具体表达式为:
6、运用卡尔曼滤波,估计出子IMU的位置误差、速度误差和姿态误差,并对子IMU的捷联解算结果进行校正,得到子IMU处更高精度的运动参数,包括位置、速度和姿态的具体步骤为:
(1)估计子IMU的姿态误差、速度误差和位置误差
基于上述步骤5建立的子IMU传递对准数学模型,运用卡尔曼滤波估计出子IMU的平台失准角φE、φN、φU,速度误差δVE、δVN、δVU,位置误差δL、δλ和δh;
(2)利用上述误差校正子IMU的捷联解算结果,得到更高精度的的子IMU位置、速度和姿态。
a)速度校正
V′E=VE-δVE,V′N=VN-δVN,V′U=VU-δVU (34)
其中V′E、V′N、V′U分别为校正后的子IMU东向、北向和天向速度;VE、VN、VU分别为子IMU捷联解算得到的东向、北向和天向速度;δVE、δVN、δVU分别为子IMU东向、北向和天向速度误差;
b)位置校正
L′=L-δL,λ′=λ-δλ,h′=h-δh (35)
其中L′、λ′、h′分别为子IMU校正后的纬度、经度和高度;L、λ、h分别为子IMU捷联解算得到纬度、经度和高度;δL、δλ和δh分别为子IMU纬度、经度和高度误差;
c)姿态校正
计算子IMU的地理坐标系即导航坐标系n与计算地理坐标系n′间的转换矩阵
更新子IMU的姿态转换矩阵
由更新后的子IMU的姿态阵计算子IMU的航向角ψs、俯仰角θs和横滚角γs。将记为:
其中Tlm为矩阵中第l行、第m列的元素,l=1,2,3,m=1,2,3;则子IMU航向角ψs、俯仰角θs和横滚角γs的主值,即ψs主、θs主和γs主分别为:
θs主=arcsin(T32),/>
航向角ψs、俯仰角θs和横滚角γs的取值范围分别定义为[0,2π]、 [-π,+π];那么,ψs、θs和γs的真值可由下式确定:
θs=θs主,/>
通过对子IMU的速度、位置和姿态进行校正,得到更高精度的子IMU安装点的速度、位置和姿态信息,完成传递对准。
上述步骤针对的是一主到一子系统的传递对准,对于含有多个子IMU的分布式POS而言,对主POS到每一个子IMU的传递对准都进行上述处理。
本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。
Claims (7)
1.一种机载分布式POS传递对准量测异常处理方法,具体步骤为:
1.1计算子IMU处光纤光栅传感器的相对位置和相对姿态测量误差;
1.2建立光纤光栅传感器相对位置、相对姿态测量误差的各阶次多项式函数模型;
1.3采用最小二乘方法,确定出各阶次多项式函数模型的待定系数,并计算各模型的拟合残差,将拟合残差最小的多项式函数模型作为最终的光纤光栅传感器测量误差模型,包括相对位置测量误差模型和相对姿态测量误差模型;
1.4利用光纤光栅传感器最终的相对位置和相对姿态测量误差模型,修正主POS的位置和姿态;
1.5确定子IMU传递对准的量测量,建立子IMU传递对准的数学模型;
1.6运用卡尔曼滤波,估计出子IMU的位置误差、速度误差和姿态误差,并对子IMU的捷联解算结果进行校正,得到子IMU处更高精度的运动参数,包括位置、速度和姿态;
上述步骤针对的是一主到一子系统的传递对准,对于含有多个子IMU的分布式POS而言,对主POS到每一个子IMU的传递对准都进行上述处理。
2.根据权利要求1所述的机载分布式POS传递对准量测异常处理方法,其特征在于:所述的步骤1.1中计算子IMU处光纤光栅传感器的相对位置和相对姿态测量误差,具体步骤为:
光纤光栅传感器输出的位移和挠度分别又称为相对位置和相对姿态;
2.1计算相对位置测量误差
Δx'=Δx-Δxjizhun
Δy'=Δy-Δyjizhun
Δz'=Δz-Δzjizhun
其中,Δx、Δy、Δz分别是子IMU处光纤光栅传感器测量的相对位置在主POS载体坐标系x轴、y轴和z轴上的分量;Δx'、Δy'、Δz'分别是Δx、Δy、Δz的误差,即光纤光栅传感器相对位置的测量误差;Δxjizhun、Δyjihzhun、Δzjizhun是由高精度双目相机提供的主POS载体坐标系x轴、y轴和z轴上相对位置的基准数据;
2.2计算相对姿态测量误差
Δψ'=Δψ-Δψjizhun
Δθ'=Δθ-Δθjizhun
Δγ'=Δγ-Δγjizhun
其中,Δψ、Δθ、Δγ是子IMU处光纤光栅传感器测量的相对姿态,即相对航向角、相对俯仰角、相对横滚角;Δψ'、Δθ'、Δγ'分别是Δψ、Δθ、Δγ的测量误差;Δψjizhun、Δθjizhun、Δγjizhun是主POS与子IMU处高精度基准POS二者的航向角之差、俯仰角之差和横滚角之差,作为相对姿态基准数据。
3.根据权利要求2所述的机载分布式POS传递对准量测异常处理方法,其特征在于:所述的步骤1.2中光纤光栅传感器相对位置、相对姿态测量误差的各阶次多项式函数模型,具体测量误差建模步骤为:
3.1建立相对姿态测量误差的各阶次多项式函数模型
光纤光栅传感器相对姿态测量误差Δψ'、Δθ'、Δγ'与子IMU处光纤光栅传感器测量的相对姿态Δψ、Δθ、Δγ呈近似线性关系,因此建立相对姿态测量误差的一阶和二阶多项式函数模型;
a)一阶多项式函数模型
Δψ'=a1Δψ+a0
Δθ'=a3Δθ+a2
Δγ'=a5Δγ+a4
式中,[a1,a0]、[a3,a2]、[a5,a4]分别为相对姿态测量误差Δψ'、Δθ'、Δγ'的一阶多项式函数模型待定系数;
b)二阶多项式函数模型
Δψ'=b2Δψ2+b1Δψ+b0
Δθ'=b5Δθ2+b4Δθ+b3
Δγ'=b8Δγ2+b7Δγ+b6
式中,[b2,b1,b0]、[b5,b4,b3]、[b8,b7,b6]分别为相对姿态测量误差Δψ'、Δθ'、Δγ'的二阶多项式函数模型待定系数;
3.2建立相对位置测量误差的各阶次多项式函数模型
光纤光栅传感器相对位置测量误差Δx'、Δy'、Δz'与子IMU处光纤光栅传感器测量的相对位置Δx、Δy、Δz呈近似线性关系,因此建立相对位置测量误差的一阶和二阶多项式函数模型;
a)一阶多项式函数模型
Δx'=a'1Δx+a'0
Δy'=a'3Δy+a'2
Δz'=a'5Δz+a'4
式中,[a'1,a'0]、[a'3,a'2]、[a'5,a'4]分别为相对位置测量误差Δx'、Δy'、Δz'的一阶多项式函数模型待定系数;
b)二阶多项式函数模型
Δx'=b'2Δx2+b'1Δx+b'0
Δy'=b'5Δy2+b'4Δy+b'3
Δz'=b'8Δz2+b'7Δz+b'6
式中,[b'2,b'1,b'0]、[b'5,b'4,b'3]、[b'8,b'7,b'6]分别为相对位置测量误差Δx'、Δy'、Δz'的二阶多项式函数模型待定系数。
4.根据权利要求3所述的机载分布式POS传递对准量测异常处理方法,其特征在于:所述的步骤1.3中采用最小二乘方法,确定出各阶次多项式函数模型的待定系数,并计算各模型的拟合残差,将拟合残差最小的多项式函数模型作为最终的光纤光栅传感器测量误差模型,具体步骤为:
4.1求解各阶次多项式函数模型的待定系数
将光纤光栅传感器相对姿态测量误差、相对位置测量误差数据与子IMU处光纤光栅传感器测量的相对姿态、相对位置数据代入权利要求3建立的各阶次多项式函数模型;
a)求解相对姿态测量误差各阶次多项式函数模型的待定系数
对于相对航向角Δψ测量误差的一阶多项式函数模型,令该模型的拟合残差则a1,a0的计算公式为:
其中n1为Δψ测量误差数据的总个数;
对于相对航向角Δψ测量误差的二阶多项式函数模型,令该模型的拟合残差则b2,b1,b0的计算公式为:
对于相对俯仰角Δθ测量误差的一阶多项式函数模型,令该模型的拟合残差则a3,a2的计算公式为:
其中n2为代入的Δθ测量误差数据的个数;
对于相对俯仰角Δθ测量误差的二阶多项式函数模型,令该模型的拟合残差则b5,b4,b3的计算公式为:
对于相对横滚角Δγ测量误差的一阶多项式函数模型,令该模型的拟合残差则a5,a4的计算公式为:
其中n3为代入的Δγ测量误差数据的个数;
对于相对横滚角Δγ测量误差二阶多项式函数模型,令该模型的拟合残差则b8,b7,b6的计算公式为:
b)求解相对位置测量误差各阶次多项式函数模型的待定系数
对于x轴相对位置Δx测量误差的一阶多项式函数模型,令该模型的拟合残差则a'1,a'0的计算公式为:
其中n4为代入的Δx测量误差数据的个数;
对于x轴相对位置Δx测量误差二阶多项式函数模型,令该模型的拟合残差则b'2,b'1,b'0的计算公式为:
对于y轴相对位置Δy测量误差一阶多项式函数模型,令该模型的拟合残差则a'3,a'2的计算公式为:
其中n5为代入的Δy测量误差数据的个数;
对于y轴相对位置Δy测量误差二阶多项式函数模型,令该模型的拟合残差则b'5,b'4,b'3的计算公式为:
对于z轴相对位置Δz测量误差一阶多项式函数模型,令该模型的拟合残差则a'5,a'4的计算公式为:
其中n6为代入的Δz测量误差数据的个数;
对于z轴相对位置Δz测量误差二阶多项式函数模型,令该模型的拟合残差则b'8,b'7,b'6的计算公式为:
4.2计算各阶次多项式函数模型的拟合残差并确定最终的光纤光栅传感器测量误差模型
根据待定系数求取各多项式模型的拟合残差,多项式函数模型的拟合残差越小,该模型的整体拟合程度越好;
a)计算相对姿态测量误差各阶次多项式函数模型的拟合残差
相对航向角Δψ各阶测量误差模型拟合残差:
一阶多项式函数模型拟合残差:
二阶多项式函数模型拟合残差:
当s1min<s2min时,相对航向角Δψ最终测量误差模型为Δψ'=a1Δψ+a0;
当s1min>s2min时,相对航向角Δψ最终测量误差模型为Δψ'=b2Δψ2+b1Δψ+b0;
相对俯仰角Δθ各阶测量误差模型拟合残差:
一阶多项式函数模型拟合残差:
二阶多项式函数模型拟合残差:
当s3min<s4min时,相对俯仰角Δθ最终测量误差模型为Δθ'=a3Δθ+a2;
当s3min>s4min时,相对俯仰角Δθ最终测量误差模型为Δθ'=b5Δθ2+b4Δθ+b3;
相对横滚角Δγ各阶测量误差模型拟合残差:
一阶多项式函数模型拟合残差:
二阶多项式函数模型拟合残差:
当s5min<s6min时,相对横滚角Δγ最终测量误差模型为Δγ'=a5Δγ+a4;
当s5min>s6min时,相对横滚角Δγ最终测量误差模型为Δγ'=b8Δγ2+b7Δγ+b6;b)计算相对位置测量误差各阶次多项式函数模型的拟合残差x轴相对位置Δx测量误差模型拟合残差:
一阶多项式函数模型拟合残差:
二阶多项式函数模型拟合残差:
当s′1min<s′2min时,x轴相对位置Δx最终测量误差模型为Δx'=a'1Δx+a'0;
当s′1min>s′2min时,x轴相对位置Δx最终测量误差模型为Δx'=b'2Δx2+b'1Δx+b'0;
y轴相对位置Δy测量误差模型拟合残差:
一阶多项式函数模型拟合残差:
二阶多项式函数模型拟合残差:
当s′3min<s′4min时,y轴相对位置Δy最终测量误差模型为Δy'=a'3Δy+a'2;
当s′3min>s′4min时,y轴相对位置Δy最终测量误差模型为Δy'=b'5Δy2+b'4Δy+b'3;
z轴相对位置Δz测量误差模型拟合残差:
一阶多项式函数模型拟合残差:
二阶多项式函数模型拟合残差:
当s′5min<s′6min时,z轴相对位置Δz最终测量误差模型为Δz'=a'5Δz+a'4;
当s'5min>s'6min时,z轴相对位置Δz最终测量误差模型为Δz'=b'8Δz2+b'7Δz+b'6。
5.根据权利要求1所述的机载分布式POS传递对准量测异常处理方法,其特征在于:所述的步骤1.4中利用光纤光栅传感器最终的相对位置和相对姿态测量误差模型,修正主POS的位置和姿态,具体步骤为:
5.1位置修正
式中,L'm、λ'm、h'm分别表示主POS经位置修正后的纬度、经度和高度;Lm、λm、hm分别表示主POS在传递对准前经光纤光栅传感器数据进行柔性变形补偿后的纬度、经度和高度,RM和RN分别表示地球子午圈和卯酉圈的主曲率半径,Δx'、Δy'、Δz'为子IMU处光纤光栅传感器相对位置测量误差,为主POS的姿态矩阵,ψm、θm、γm分别表示主POS在传递对准前经光纤光栅传感器数据进行柔性变形补偿后的航向角、俯仰角和横滚角;
5.2姿态修正
其中,为主POS经姿态修正后的姿态矩阵,CFBG为由光纤光栅传感器的相对姿态测量误差组成的姿态修正矩阵;
式中,ψ'm、θ'm、γ'm分别表示主POS经姿态修正后的航向角、俯仰角和横滚角,Δψ'、Δθ'、Δγ'是子IMU处光纤光栅传感器的相对姿态测量误差;
将记为:
其中Clm为矩阵中第l行、第m列的元素,l=1,2,3,m=1,2,3;则主POS经姿态修正后的航向角ψ'm、俯仰角θ'm和横滚角γ'm的主值,即ψ'm主、θ'm主和ψ'm主分别为:
θ'm主=arcsin(C32)
航向角ψ'm、俯仰角θ'm和横滚角γ'm的取值范围分别定义为[0,2π]、 [-π,+π];那么,ψ'm、θ'm和γ'm的真值可由下式确定:
θ'm=θ'm主
6.根据权利要求1所述的机载分布式POS传递对准量测异常处理方法,其特征在于:所述的步骤1.5中确定子IMU传递对准的量测量,建立的子IMU传递对准数学模型的具体步骤为:
6.1建立子IMU惯导误差模型
相关参考坐标系的定义包括:i代表地心惯性坐标系;e代表地球坐标系;主POS和子IMU导航坐标系均为东北天地理坐标系,分别用nm和n表示;b代表子IMU载体坐标系,bm代表主POS载体坐标系;
根据上述定义,子IMU惯导误差模型为:
a)姿态误差方程:
式中,φn=[φE φN φU]T为子IMU的失准角误差,φE、φN和φU分别为子IMU的东向、北向和天向失准角,下标E、N和U分别表示导航坐标系下的东向、北向和天向;表示子IMU导航坐标系下导航坐标系相对于惯性坐标系的转动角速度,/>为其误差;其中/>为子IMU捷联解算得到的姿态矩阵,εb为子IMU陀螺仪在子IMU载体坐标系的随机常值漂移,/>为子IMU陀螺仪在子IMU载体坐标系的随机噪声;
b)速度误差方程:
式中,Vn=[VE VN VU]T为子IMU导航坐标系下子IMU捷联解算得到的速度,VE、VN、VU分别为子IMU捷联解算得到的东向、北向和天向速度;速度误差δVn=[δVE δVN δVU]T,δVE、δVN和δVU分别为子IMU的东向、北向和天向速度误差;fb=[fx b fy b fz b]T为子IMU加速度计在子IMU载体坐标系上测量的比力,分别为比力在子IMU载体坐标系三个轴上的分量;表示子IMU导航坐标系下地球坐标系相对于惯性坐标系的转动角速度;/>表示子IMU导航坐标系下导航坐标系相对于地球坐标系的转动角速度;/>和/>分别为/>和/>的误差;/>表示子IMU加速度计在子IMU载体坐标系的随机常值偏置,/>为子IMU加速度计在子IMU载体坐标系的随机噪声;/>L、λ、h分别为子IMU捷联解算得到纬度、经度和高度,RM和RN分别表示地球子午圈和卯酉圈的主曲率半径;
c)位置误差方程:
式中,L、λ、h分别为子IMU捷联解算得到纬度、经度和高度,δL、δλ和δh分别为L、λ、h的误差,即子IMU的纬度误差、经度误差和高度误差;
d)惯性仪表误差方程:
陀螺仪的误差和加速度计的误差/>是非常复杂的随机过程,可以建为一个随机常值误差和一个高斯白噪声的叠加,具体表示为:
其中,εb和分别表示陀螺仪在子IMU载体坐标系的随机常值漂移和随机噪声,/>和分别表示加速度计在子IMU载体坐标系的随机常值偏置和随机噪声;εb和/>的数学模型为:
其中εb=[εxεyεz]T,εx、εy和εz分别为子IMU载体坐标系三个轴向上的陀螺仪随机常值漂移, 和/>分别为表示子IMU载体坐标系三个轴向上的加速度计随机常值偏置,脚标x、y、z分别表示子IMU载体坐标系的三个坐标轴;
6.2建立子IMU传递对准的数学模型
状态方程为:
式中,状态变量F为系统矩阵,/>为系统噪声矩阵,其中/>为子IMU陀螺仪在子IMU载体坐标系下的随机噪声,/>和/>分别为子IMU载体系三轴上的陀螺随机噪声;/>为子IMU加速度计在子IMU载体坐标系下的随机噪声,/>和/>分别为子IMU载体系三轴上的加速度计随机噪声补充元素解释,G为噪声驱动矩阵;
/>
将经过光纤光栅传感器最终的测量误差模型修正后的主POS位置和姿态信息,分别与子IMU捷联解算的位置和姿态的差值作为量测量,采用“位置+姿态”匹配方式,建立子IMU传递对准的量测方程:
Z=HX+v
式中,量测向量Z=[δψΔδθΔδγΔδLΔδλΔδhΔ]T,其中,δψΔ、δθΔ、δγΔ是主POS经姿态修正后与子IMU的航向角、俯仰角和横滚角之差,δLΔ、δγΔ、δhΔ是主POS经位置修正后与子IMU的纬度、经度和高度之差;v=[vδψ vδθ vδγ vδL vδλ vδh]T为量测噪声矩阵,vδψ、vδθ、vδγ分别为主POS航向角、俯仰角、横滚角的量测噪声,vδL、vδλ、vδh分别为主POS纬度、经度、高度的量测噪声,为主POS经姿态修正后的姿态矩阵,H为量测矩阵,具体表达式为:
/>
7.根据权利要求6所述的机载分布式POS传递对准量测异常处理方法,其特征在于:运用卡尔曼滤波,估计出子IMU的位置误差、速度误差和姿态误差,并对子IMU的捷联解算结果进行校正,得到子IMU处更高精度的运动参数,包括位置、速度和姿态的具体步骤为:
7.1估计子IMU的姿态误差、速度误差和位置误差
基于权利要求6建立的子IMU传递对准数学模型,运用卡尔曼滤波估计出子IMU的平台失准角φE、φN、φU,速度误差δVE、δVN、δVU,位置误差δL、δλ和δh;
7.2利用上述误差校正子IMU的捷联解算结果,得到更高精度的子IMU位置、速度和姿态
a)速度校正
V'E=VE-δVE
V'N=VN-δVN
V'U=VU-δVU
其中V'E、V'N、V'U分别为校正后的子IMU东向、北向和天向速度;VE、VN、VU分别为子IMU捷联解算得到的东向、北向和天向速度;δVE、δVN、δVU分别为子IMU东向、北向和天向速度误差;
b)位置校正
L'=L-δL
λ'=λ-δλ
h'=h-δh
其中L'、λ'、h'分别为校正后的子IMU纬度、经度和高度;L、λ、h分别为子IMU捷联解算得到纬度、经度和高度;δL、δλ和δh分别为子IMU纬度、经度和高度误差;
c)姿态校正
计算子IMU的地理坐标系即导航坐标系n与计算地理坐标系n'间的转换矩阵
更新子IMU的姿态转换矩阵
/>
其中为子IMU捷联解算得到的姿态矩阵;
由更新后的子IMU的姿态阵计算子IMU的航向角ψs、俯仰角θs和横滚角γs;将/>记为:
其中Tlm为矩阵中第l行、第m列的元素,l=1,2,3,m=1,2,3;则子IMU航向角ψs、俯仰角θs和横滚角γs的主值,即ψs主、θs主和γs主分别为:
θs主=arcsin(T32)
航向角ψs、俯仰角θs和横滚角γs的取值范围分别定义为[0,2π]、 [-π,+π];那么,ψs、θs和γs的真值可由下式确定:
θs=θs主
通过对子IMU的速度、位置和姿态进行校正,得到更高精度的子IMU安装点的速度、位置和姿态信息,完成传递对准。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110308355.7A CN113188565B (zh) | 2021-03-23 | 2021-03-23 | 一种机载分布式pos传递对准量测异常处理方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110308355.7A CN113188565B (zh) | 2021-03-23 | 2021-03-23 | 一种机载分布式pos传递对准量测异常处理方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113188565A CN113188565A (zh) | 2021-07-30 |
CN113188565B true CN113188565B (zh) | 2023-09-29 |
Family
ID=76973672
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110308355.7A Active CN113188565B (zh) | 2021-03-23 | 2021-03-23 | 一种机载分布式pos传递对准量测异常处理方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113188565B (zh) |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114993242B (zh) * | 2022-06-17 | 2023-03-31 | 北京航空航天大学 | 一种基于加速度匹配的阵列式pos安装偏差角标定方法 |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103913181A (zh) * | 2014-04-24 | 2014-07-09 | 北京航空航天大学 | 一种基于参数辨识的机载分布式pos传递对准方法 |
CN107764268A (zh) * | 2017-10-13 | 2018-03-06 | 北京航空航天大学 | 一种机载分布式pos传递对准的方法和装置 |
CN108387227A (zh) * | 2018-02-22 | 2018-08-10 | 北京航空航天大学 | 机载分布式pos的多节点信息融合方法及系统 |
WO2020233290A1 (zh) * | 2019-05-17 | 2020-11-26 | 东南大学 | 一种动态变形下基于双重滤波器的传递对准方法 |
CN112326162A (zh) * | 2020-09-17 | 2021-02-05 | 北京航空航天大学 | 一种机载分布式pos用机翼弹性变形测量方法 |
CN112525191A (zh) * | 2021-02-08 | 2021-03-19 | 北京航空航天大学 | 一种基于相对捷联解算的机载分布式pos传递对准方法 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109141476B (zh) * | 2018-09-27 | 2019-11-08 | 东南大学 | 一种动态变形下传递对准过程中角速度解耦合方法 |
-
2021
- 2021-03-23 CN CN202110308355.7A patent/CN113188565B/zh active Active
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103913181A (zh) * | 2014-04-24 | 2014-07-09 | 北京航空航天大学 | 一种基于参数辨识的机载分布式pos传递对准方法 |
CN107764268A (zh) * | 2017-10-13 | 2018-03-06 | 北京航空航天大学 | 一种机载分布式pos传递对准的方法和装置 |
CN108387227A (zh) * | 2018-02-22 | 2018-08-10 | 北京航空航天大学 | 机载分布式pos的多节点信息融合方法及系统 |
WO2020233290A1 (zh) * | 2019-05-17 | 2020-11-26 | 东南大学 | 一种动态变形下基于双重滤波器的传递对准方法 |
CN112326162A (zh) * | 2020-09-17 | 2021-02-05 | 北京航空航天大学 | 一种机载分布式pos用机翼弹性变形测量方法 |
CN112525191A (zh) * | 2021-02-08 | 2021-03-19 | 北京航空航天大学 | 一种基于相对捷联解算的机载分布式pos传递对准方法 |
Non-Patent Citations (5)
Title |
---|
Airborne Distributed Position and Orientation System Transfer Alignment Method Based on Fiber Bragg Grating;Ye Wen et al.;《SENSORS》;第20卷(第7期);1-19 * |
Six-Dimensional Deformation Measurement of Distributed POS Based on FBG Sensors;Li Jianli et al.;《IEEE SENSORS JOURNAL》;第21卷(第6期);7849-7856 * |
基于光纤光栅的分布式POS柔性基线测量方法;顾宾等;《中国惯性技术学报》;第27卷(第3期);307-313 * |
基于最小二乘拟合法的分布式POS全局估计方法;叶文;刘艳红;王博;蔡晨光;李建利;;北京航空航天大学学报(第05期);27-34 * |
基于最小二乘的全局信息融合方法在分布式POS中的应用;莫明岗等;《中国惯性技术学报》;第28卷(第1期);35-40 * |
Also Published As
Publication number | Publication date |
---|---|
CN113188565A (zh) | 2021-07-30 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110501024B (zh) | 一种车载ins/激光雷达组合导航系统的量测误差补偿方法 | |
CN108827310B (zh) | 一种船用星敏感器辅助陀螺仪在线标定方法 | |
CN108413887B (zh) | 光纤光栅辅助分布式pos的机翼形变测量方法、装置和平台 | |
CN102621565B (zh) | 一种机载分布式pos的传递对准方法 | |
CN110221332B (zh) | 一种车载gnss/ins组合导航的动态杆臂误差估计和补偿方法 | |
CN111323050B (zh) | 一种捷联惯导和多普勒组合系统标定方法 | |
CN108387227B (zh) | 机载分布式pos的多节点信息融合方法及系统 | |
CN103245359B (zh) | 一种惯性导航系统中惯性传感器固定误差实时标定方法 | |
CN104764467B (zh) | 空天飞行器惯性传感器误差在线自适应标定方法 | |
CN112629538A (zh) | 基于融合互补滤波和卡尔曼滤波的舰船水平姿态测量方法 | |
CN110887481B (zh) | 基于mems惯性传感器的载体动态姿态估计方法 | |
CN107764261B (zh) | 一种分布式pos传递对准用模拟数据生成方法和系统 | |
CN110954102B (zh) | 用于机器人定位的磁力计辅助惯性导航系统及方法 | |
CN113063429B (zh) | 一种自适应车载组合导航定位方法 | |
CN108132060B (zh) | 一种捷联惯导系统无基准的系统级标定方法 | |
CN109612460B (zh) | 一种基于静止修正的垂线偏差测量方法 | |
CN111121766A (zh) | 一种基于星光矢量的天文与惯性组合导航方法 | |
EP3527948B1 (en) | Air data aided inertial measurement unit | |
CN109708663B (zh) | 基于空天飞机sins辅助的星敏感器在线标定方法 | |
CN111964688B (zh) | 结合无人机动力学模型和mems传感器的姿态估计方法 | |
CN116147624B (zh) | 一种基于低成本mems航姿参考系统的船舶运动姿态解算方法 | |
CN108303120B (zh) | 一种机载分布式pos的实时传递对准的方法及装置 | |
CN111189442A (zh) | 基于cepf的无人机多源导航信息状态预测方法 | |
CN117053782A (zh) | 一种水陆两栖机器人组合导航方法 | |
CN113291493A (zh) | 一种卫星多敏感器融合姿态确定方法和系统 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |