法律状态公告日
法律状态信息
法律状态
2016-05-04
授权
授权
2014-05-21
实质审查的生效 IPC(主分类):G01C25/00 申请日:20131227
实质审查的生效
2014-04-23
公开
公开
技术领域
本发明涉及一种倾斜状态下调制寻北仪安装误差在线估计与寻北误差补偿方法。
背景技术
在人类社会生活实践中,地理方位是不可或缺的信息,寻北就是要找出载体相对地 球的北向的方位关系,即确定载体的航向角。捷联式寻北又称解析式寻北,它利用陀螺 直接敏感地球自转角速度分量,具有反应迅速、结构简单等优点,还可以直接应用先进 的光学陀螺作为理想的寻北惯性元件。捷联寻北采用的寻北方法主要有静态寻北和动态 寻北。静态寻北法包括单位置、二位置、三位置、四位置和多位置寻北,其中二位置和 四位置寻北方法较常见。目前,对静态寻北方案的研究基本成熟,在寻北方案、信号滤 波、误差分析及补偿、工程样机研制方面均取得了一定成果。
旋转调制式寻北是近年来新兴的一种动态寻北方案,在寻北过程中,惯性测量单元 随转台绕其垂直中心轴连续恒速转动,利用陀螺在水平面或倾斜面内的输出信号解算出 载体方位角。相比传统寻北方案,它将静态测量变为动态测量,利用转台的恒速旋转对 陀螺和加速度计的输出进行调制,可以有效抑制陀螺常值漂移、随机漂移和加速度计漂 移,满足了寻北的快速性和高精度定位要求。目前,相关理论、技术和方法仅局限于旋 转调制寻北的基本原理、倾斜状态时的补偿算法以及数据处理方面,并未考虑实际寻北 中陀螺的安装误差角。
在实际工程应用中,随着寻北仪工作时的湿度、温度等外界环境因素的变化,陀螺 与转台之间的机械连接会因受力的变化而变形,造成陀螺坐标系与平台坐标系之间存在 安装误差角,而且该误差角的大小与外界干扰因素密切相关,无法用确定的变化的规律 来描述。目前尚无具体方法克服由安装误差角引起的寻北误差,使得实际应用中寻北精 度受到严重制约。
发明内容
本发明目的在于提供一种倾斜状态下调制寻北仪安装误差在线估计与寻北误差补偿 方法,能够对陀螺坐标系与平台坐标系之间的安装误差角进行估计,并对寻北误差进行 补偿,有效提高寻北精度。
实现本发明目的技术方案:
一种倾斜状态下调制寻北仪安装误差在线估计与寻北误差补偿方法,其特征在于:
步骤一:建立地理坐标系、载体坐标系、平台坐标系、陀螺坐标系及四者之间的转 换关系;
步骤二:建立倾斜状态下,转台绕自身垂直中心轴恒速旋转时,陀螺存在安装误差 角时的输出模型;
步骤三:建立倾斜状态下,转台绕自身垂直中心轴恒速旋转时,加速度计存在安装 误差时的输出模型;
步骤四:对陀螺和加速度计的输出信号进行处理,并得到载体的倾斜角;
步骤五:根据步骤四中对陀螺、加速度计数据处理后的结果对安装误差角进行在线 估计;
步骤六:根据步骤五中对处理后陀螺的信号和步骤四中得到的载体倾斜角估计出寻 北误差,然后对航向角粗估计值进行修正,得到准确的航向角,完成载体的寻北。
步骤二中,陀螺存在安装误差角时的输出模型为,
ωyi=Ky{-[(cosγsinH+sinθsinγcosH)ωN-cosθsinγωH]sinαi
+(ωNcosθcosH+ωHsinθ)cosαi
-η[(cosγsinH+sinθsinγcosH)ωNcosαi-cosθsinγωHcosαi
+(ωNcosθcosH+ωHsinθ)sinαi]}+εyi
式中H为载体的航向角,θ和γ分别为载体的俯仰角和翻滚角,η为平台坐标系p与陀螺 坐标系g之间的安装误差角,角位置αi=Ω·ti,其中ti表示转台转动到第i个位置的时间, Ω为转台恒定角速度,为当地纬度,ωie为地球自转角 速度,ωyi表示转台转动中第i个位置陀螺的输出值,Ky表示陀螺的标度因数,εyi表示转 台转动中第i个位置的陀螺漂移,i=1,2,....,n。
步骤四中,对陀螺和加速度计的输出信号进行处理,并得到载体的倾斜角,通过如 下方法实现,
令
令
式中g表示地球重力加速度,fyi表示转台转动中第i个位置加速度计的输出值, i=1,2,....,n。
步骤五中,在线估计出安装误差角的大小通过如下方法实现,
式中a=ωHcosθsinγ,b=ωNcosγ,c=ωNsinθsinγ,d=ωNcosθ,e=ωHsinθ,
步骤六中,对载体航向角的修正通过如下方法实现,
n取值为3000。
本发明具有的有益效果:
本发明陀螺和加速度计作为惯性测量单元随转台绕其垂直中心连续恒速转动,陀螺 用来测量载体倾斜面内地球自转角速度分量,加速度计用来测量载体的倾斜角,通过建 立倾斜状态下陀螺、加速度计存在安装误差角时的输出模型,对陀螺、加速度计输出信 号进行处理,实时估计出陀螺的安装误差角,然后对寻北误差进行补偿,完成寻北。本 发明对陀螺坐标系与平台坐标系之间的安装误差角进行估计,修正寻北结果,可以显著 提高寻北精度。
附图说明
图1是本发明的方法流程图;
图2是实施例中本发明方法与传统忽略安装误差时的寻北解算误差曲线图。
具体实施方式
如图1所示,步骤一:建立地理坐标系、载体坐标系、平台坐标系、陀螺坐标系及 四者之间的转换关系;
坐标系的定义:地理坐标系oxnynzn为东北天坐标系,坐标原点在转台平面中心;载 体坐标系oxbybzb,坐标原点与地理坐标系原点重合,oxb轴和oyb轴在转台平面内,ozb轴 垂直于转台平面且与oxb轴、oyb轴呈右手螺旋,其中oxb轴、oyb轴不随转台的旋转而 旋转;平台坐标系oxpypzp坐标系原点与地理坐标系原点重合,初始时刻平台坐标系 oxpypzp和载体坐标系oxbybzb的x、y、z轴分别对应重合,转台转动时,oxp轴、oyp轴 随转台旋转而转动;陀螺坐标系oxgygzg的坐标原点同样也在转台平面中心,随转台的旋 转而转动。单轴陀螺寻北时,陀螺的敏感轴与oyg轴重合,此时考虑寻北仪的安装误差, 陀螺坐标系g系的ozg轴与平台坐标系p系的ozp轴完全重合,oyg轴与oyp轴、oxg轴与 oxp轴之间存在安装误差角η。
地理坐标系n到载体坐标系b的转移矩阵为
式中:H为载体的航向角,θ和γ分别为载体的俯仰角和翻滚角。
当转台以恒定角速度Ω绕ozb轴(ozp轴)逆时针转动时,角位置αi=Ω·ti,其中ti表 示转台转动到第i个位置的时间,则载体坐标系到平台坐标系的转移矩阵为
平台坐标系p与陀螺坐标系g之间的安装误差角η为小角度,所以平台坐标系到陀 螺坐标系转移矩阵为
步骤二:建立倾斜状态下,转台绕自身垂直中心轴恒速旋转时,陀螺存在安装误差 角时的输出模型;
考虑陀螺标度因数和陀螺漂移,有
其中,ωg表示陀螺坐标系下角速度,K=[Kx Ky Kz]T,Kgx、Kgy、Kgz表示陀螺各 轴的标度因数,表示地球自转角速度在地理坐标系的投影,Ω=[0 0 Ω]T为寻北仪 转位机构转动的角速度矢量,Ω为转台转动的角速度大小,ε=[εx εy εz]T,εx、εy、 εz表示陀螺各轴的陀螺漂移,[…]T表示矩阵的转置。
由于陀螺的敏感轴与oyg轴,因此陀螺输出为陀螺坐标系下角速度ωg在y轴的投影。 将式(1)、(2)、(3)代入式(4),并取ωg的y轴分量可得陀螺的输出为
ωyi=Ky{-[(cosγsinH+sinθsinγcosH)ωN-cosθsinγωH]sinαi
+(ωNcosθcosH+ωHsinθ)cosαi (5)
-η[(cosγsinH+sinθsinγcosH)ωNcosαi-cosθsinγωHcosαi
+(ωNcosθcosH+ωHsinθ)sinαi]}+εyi
式中H为载体的航向角,θ和γ分别为载体的俯仰角和翻滚角,η为平台坐标系p与陀螺 坐标系g之间的安装误差角,角位置αi=Ω·ti,其中ti表示转台转动到第i个位置的时间, Ω为转台恒定角速度,为当地纬度,ωie为地球自转角 速度,ωyi表示转台转动中第i个位置陀螺的输出值,Ky表示陀螺的标度因数,εyi=ε0+εdi表示转台转动中第i个位置的陀螺漂移,ε0是常值漂移,εdi是随机漂移,i=1,2,....,n。
步骤三:建立倾斜状态下,转台绕自身垂直中心轴恒速旋转时,加速度计存在安装 误差时的输出模型;
在旋转调制式寻北仪中,采用陀螺坐标系下水平方向y轴上的一只加速度计来测量 载体的倾斜角,加速度计测得的比力在地理坐标系的投影为fn,在陀螺坐标系的投影为 fg,重力加速度在地理坐标系下的投影为gn。有
fg=[fx fy fz]T (6)
gn=[0 0 -g]T (7)
因为载体静止,地速在地理坐标系的投影地理坐标系相对地球坐 标系的转动角速度依据惯导基本方程有
考虑加速度漂移▽=[▽x ▽y ▽z]T,依据坐标转换原理,有
将式(1)、(2)、(3)、(8)代入式(9)中,并取fg的y轴分量作为加速度计的输出
fyi=(gsinθ+ηgcosθsinγ)cosαi+(gcosθsinγ-ηgsinθ)sinαi+▽y (10)
其中fyi表示转台转动中第i个位置处oyg轴的加速度计测得的比力,i=1,2,....,n。
步骤四:对陀螺和加速度计的输出信号进行处理,并得到载体的倾斜角;
(1)陀螺输出信号数据处理
转台旋转一周,在旋转过程中,陀螺输出不断变化。选取一周中n个对称位置获得 陀螺输出采样值,当次数n足够大时,我们可以采用如下近似式来进行数据处理。
令
在这里,综合考虑计算量与计算精度,根据多次实践经验,选取n=3000。
(2)加速度计输出信号数据处理
同(1)中对陀螺输出信号的数据处理,选取转台旋转一周中n个对称位置获得加速 度计输出采样值,令
在这里,同理式(13),n=3000。
实际工作中,寻北仪的倾斜角θ、γ为小角度,安装误差角η也为小角度,因此忽略 式(14)中的二阶小量项-ηgsinθ和ηgcosθsinγ,有
根据式(15)可得载体的倾斜角
步骤五:根据步骤四中对陀螺、加速度计数据处理后的结果对安装误差角进行在线 估计。
忽略安装误差角η,根据式(13)对载体航向角进行粗估计
设航向角真值H与估计值之间的关系为
其中ΔH为小量。
将式(18)代入式(13)中,并对三角函数sinH、cosH进行泰勒展开
其中
将式(19)代入式(13)中,忽略η和ΔH的高阶小量,有
其中,a=ωHcosθsinγ,b=ωNcosγ,c=ωNsinθsinγ,d=ωNcosθ,e=ωHsinθ。
根据式(20)对安装误差角进行在线估计
步骤六:根据步骤五中对处理后陀螺的信号和步骤四中得到的载体倾斜角估计出寻 北误差,然后对航向角粗估计值进行修整,得到准确的航向角,完成载体的寻北。
寻北的实质在于根据平台坐标系中测得的地球自转角速度分量求得载体的方位角。 其中,Ωx、Ωy等价于在陀螺坐标系下测得的角速度,当安装误差角存在时,陀螺坐标系 与平台坐标系并不完全重合。而且寻北仪在实际工作中,安装误差会不可避免的出现, 此时再用Ωx、Ωy来求载体的方位角,必然会引入寻北误差,本发明可以通过估计出安装 误差角和寻北误差,得到准确的航向角,从而避免因安装误差而引起的寻北误差。
结合具体实施例,进一步对本发明进行说明。仿真时暂不考虑陀螺的标度因数误差、 转台的转位误差和温度变化等因素。设地球自转角速度ωie=15.04107°/h,当地地理纬度 安装误差角η=20',转台转动角速度Ω=1°/s,转台的倾斜角θ=5°、γ=5°, 光纤陀螺的常值漂移为0.01°/h,随机漂移为0.01°/h,采样位置数n=3000。从图2可 以看出,在寻北仪旋转一周过程中,寻北仪连续输出载体航向角,传统的忽略安装误差 时的寻北解算误差范围为[0.3° 0.4°],由此可以看出,忽略安装误差导致在要求高精度寻 北中严重影响了最终的寻北结果。而本发明方法则可以很好的解决这一问题,由图2可 以看出安装误差经过补偿后,寻北解算误差范围为[-0.2° 0.15°],比未考虑安装误差时的 寻北精度有明显提高,说明本发明方法对安装误差起到了很好的消除作用。从本实施例 可以看出,在实际工程应用中,本方法可以在寻北仪长期工作导致安装误差的情况下, 很好的补偿由安装误差引起的寻北误差,提高寻北的精度。
机译: 用寻北陀螺仪确定北向的方法
机译: 用寻北陀螺仪确定北向的方法
机译: 陀螺寻北仪系统及方法