本发明涉及惯性导航中惯性传感器误差标定技术领域,具体涉及一种船用星敏感器辅助陀螺仪在线标定方法。
背景技术
陀螺仪是惯性导航系统中姿态测量的核心传感器,可以直接敏感载体系相对惯性系的角速度,但其输出误差是导致惯性导航系统的导航误差主要原因之一,若可以在载体航行过程中准确的估算出陀螺输出误差,此部分导航误差就可以得到较好的抑制,因此对陀螺仪进行在线标定是重要的。传统的陀螺仪在线标定是利用gps信号作为外部信息源估计陀螺误差,但gps信号易受环境的干扰和欺骗而失灵。星敏感器是一种通过观测星体获得载体相对惯性系姿态的高精度传感器,其精度可达到角秒级别,具有动态稳定性好,抗干扰能力强,其测量误差不随时间累积等优点,使得在航海领域得到广泛的应用。因此,以星敏感器输出信息为基准,对陀螺仪进行在线标定,进而提高惯性导航系统姿态精度,使其长期、连续输出高精度姿态信息。然而,舰船航行过程中外界环境发生变化,进而引起星敏感器测量噪声统计特性发生变化,造成滤波器工作不稳定,降低滤波精度。
为了提高滤波精度,常采用简化的sage-husa滤波方法对组合导航系统进行滤波,该方法利用测量信息不断修正预测值,对测量噪声统计参数进行估计与修正,有效的解决了实际系统中噪声统计特性发生变化的问题,提高了系统的环境适应性。虽然简化的sage-husa滤波方法能够较好的抑制测量噪声统计特性发生变化时的滤波精度下降的问题,但是当系统环境变化情况复杂时,由于量测噪声统计特性参数修正时间短,进而导致噪声统计特性未得到较好的跟踪,造成滤波发散的现象。
《系统工程与电子技术》2012年第34卷第八期由高伟等人撰写的《ccd星敏感器辅助光纤陀螺在线标定技术》一文中,利用星敏感器输出高精度的姿态信息,将模型预测滤波与ekf结合可在线估计出陀螺输出误差,提高导航精度;《sensors》2017年17卷第一期由杨延强等人撰写的《localobservabilityanalysisofstarsensorinstallationerrorsinasins/cnsintegrationsystemfornear-earthflightvehicles》一文中,基于sins与星敏感器之间的安装误差一直是制约sins/cns实际精度的主要因素之一,利用卡尔曼滤波对星敏感器安装误差进行精确估计,提高导航系统精度;公开号cn101943585a的中国发明专利在2011年1月12日公开的《一种基于ccd星敏感器的标定方法》,该方法在短时间内可以达到稳定的标定结果,不需要进行任何机动措施,便可以估计出陀螺常值漂移和加速度计零偏。《宇航学报》2011年32卷第五期由杨波等人撰写的《长航时环境下高精度组合导航方法研究与仿真》一文中针对长航时环境下可能面临量测噪声统计特性的不确定问题,采用简化的sage-husa自适应滤波算法进行组合导航滤波设计,提高系统的精度及鲁棒性;《计算机系统应用》2015年第24卷第八期由孙会敏等人撰写的《有色噪声下的平方根ukf在天文自主导航中的应用》一文中针对传统的平方根ukf不能很好地解决测量噪声为有色噪声情况下的非线性滤波问题,提出了一种有色噪声情况下的平方根ukf方法,能够很好地解决估计精度低的问题。以上文献都在利用星敏感器辅助陀螺仪进行在线标定,以及抑制星敏感器测量噪声发生变化时的估计误差,并没有提及环境变化复杂时对陀螺仪在线标定精度及适应性的影响。
技术实现要素:
本发明的目的在于解决:针对舰船长航时,自主导航过程中,外界环境的改变造成星敏感器测量噪声统计特性未知,引起标准卡尔曼滤波发散而无法进行陀螺仪在线标定的问题,提出一种船用星敏感器辅助陀螺仪在线标定方法。
一种船用星敏感器辅助陀螺仪在线标定方法,包括以下步骤:
(1)上电,初始化星敏感器/惯性组合导航系统;
(2)系统实时采集惯性器件和星敏感器输出数据,包括陀螺仪输出的三轴角速度
(3)利用(2)中得到的陀螺仪输出信息进行姿态解算,得到姿态四元数的计算值
(4)利用(2)中得到的星敏感器输出数据进行惯性系下姿态解算,得到载体姿态四元数的真实值q;
(5)利用模糊逻辑控制方法确定简化sage-husa自适应滤波方法中遗忘因子b;构造简化sage-husa自适应滤波器,以(3)中解算得到的四元数计算值
(6)将(5)中ε和δk补偿陀螺仪输出信息
(7)将(6)中得到的载体导航信息存储并输出。
步骤(1)中所述导航初始化,需初始化系统以下信息:
导航信息初始化:舰船位置信息
初始化常值参量:当地经纬度λ、
简化sage-husa自适应滤波器参数初值:状态变量初值x0=[δe0δk0ε0]t,协方差阵p0,系统噪声方差阵q0,量测噪声方差阵r0;
其中,δe0表示误差四元数矢量部分的初始值;δk0表示陀螺仪刻度因数的初始值;ε0表示陀螺仪漂移的初始值;
初始转换矩阵计算如下:
其中,b表示载体坐标系,n表示导航坐标系,
初始化四元数q0计算如下:
令
其中,cij为矩阵
步骤(2)中所述系统实时采集惯性器件和星敏感器的输出数据,包括陀螺仪输出的三轴角速度
步骤(3)中所述利用陀螺仪采集到的角速度
角速度更新:
其中,e表示地球坐标系,p表示平台坐标系;
载体姿态四元数计算值的更新:
设任意时刻载体坐标系相对平台坐标系的转动四元数为:
其中,
四元数
其中,
根据k时刻载体坐标系相对平台坐标系的转动四元数q(k)、e1(k)、e2(k)、e3(k),求取k时刻转动四元数的变化率为:
在k+1时刻载体的转动四元数具体为:
至此,根据上式得到载体姿态四元数的计算值
载体姿态、速度及位置的更新:
利用得到的q(k+1)、e1(k+1)、e2(k+1)、e3(k+1),更新捷联矩阵
其中,上式中的q、ei,i=1,2,3为k+1时刻载体的转动四元数中q(k+1)、ei(k+1),i=1,2,3,即上式中省略了(k+1);
更新姿态信息:
利用转换关系式
其中,
根据k时刻的载体的三轴速度vx(k)、vy(k)与vz(k),求取k时刻的载体速度变化率
其中,r表示地球半径;当k=1时,vx(1)、vy(1)、vz(1)为步骤一中初始化系统时获得的载体初始速度,
至此,得到载体的姿态角、速度和位置。
步骤(4)中所述利用(2)中得到的星敏感器输出数据进行惯性系下姿态解算,得到载体姿态四元数的真实值q,即
其中,cib(i,j)表示矩阵
至此,根据上式得到载体姿态四元数的真实值q。
步骤(5)中所述的利用模糊逻辑控制方法确定k时刻的遗忘因子b,构造简化sage-husa自适应滤波器,具体包括以下步骤:
1)根据(3)中载体姿态四元数的计算值与(4)中载体姿态四元数的真实值解算得到误差四元数,过程如下:
其中,δq=[δqδe]t表示误差四元数,δq表示误差四元数的实数部分,δe表示误差四元数的虚数部分;(·)-1表示矩阵的逆;
2)利用1)中误差四元数的第3个分量δe2的绝对值对星敏感器测量噪声统计特性进行分析,离散的将星敏感器测量噪声状态定义为若干级,形式为:
u={nnlnhn}
确定δe2绝对值的论域集并划分论域,基本论域为[ac],通过量化因子转化的模糊论域为[a1c1],划分后的论域为:
其中,nn表示星敏感器无测量噪声,ln表示星敏感器测量噪声为低噪声,hn表示星敏感器测量噪声为高噪声;
3)根据δe2绝对值的实际情况建立模糊论域各元素对模糊语言的隶属度大小,即相应的隶属函数,表达形式如下:
三角形函数为:
梯形函数为:
其中,参数f,v,g,d,m,p,y,e,n分别表示模糊集合中的元素;
4)将δe2的绝对值作为模糊逻辑控制的输入信息,结合3)中隶属函数,经过模糊逻辑运算,进而推测出k时刻星敏感器测量噪声状态。其中模糊逻辑规则的具体表达式为:
5)依据4)中推测出的k时刻星敏感器测量噪声状态,采用重心法进行反模糊化得到b的精确值,进而选择k时刻相应的遗忘因子b,选择方法具体如下:
其中,b的范围为(01),重心法计算式为
6)构造简化sage-husa自适应滤波器,滤波状态量为x=[δeδkε]t,观测量为z=δe,滤波过程具体如下:
利用下列微方程建立系统方程:
其中,δe为误差四元数,且δe=[δe1δe2δe3];δk为陀螺刻度因数,且δk=[δkxδkyδkz];ε为陀螺漂移,且ε=[εxεyεz];
基于上述误差模型,采用简化sage-husa自适应滤波器,对陀螺误差进行在线标定,具体算法如下:
dk=(1-b)/(1-bk+1)
xk/k-1=fk,k-1xk-1
其中,dk为k时刻的调节因子,fk,k-1为k-1时刻到k时刻的状态转移矩阵,gk为陀螺角速度信息构成的对角阵,ι为单位阵,ο3×3为零矩阵;
根据测量信息计算k时刻的新息,再通过指数加权法不断估计k时刻的测量噪声r阵:
vk=zk-hxk/k-1
计算滤波增益、状态估值及更新pk:
kk=pk/k-1ht(hpk/k-1ht+rk)-1
xk=xkk-1+kkvk
pk=(ι-kkh)pk/k-1
其中,xk为k时刻的状态量,当k=1时,状态量x=[δe(1)δk(1)ε(1)]为初始化系统时获得的初始状态量x0。
步骤(6)中所述利用(5)中估计出的ε和δk补偿陀螺仪输出信息
陀螺仪输出角速度信息补偿:
其中,
将
本发明的有益效果在于:
本发明针对因星敏感器测量噪声统计特性发生变化而影响滤波精度的问题,利用模糊逻辑控制方法具有表达界限不清晰的定性知识与经验、推理解决常规方法难于解决的规则型模糊信息问题的优点,对自适应滤波的遗忘因子进行选择,实现陀螺误差的在线估计,既可以保证星敏感器信息的有效利用,又可实时修正了陀螺仪误差,提高舰船导航系统的精度。本发明方法增强了舰船在复杂环境中星敏感器辅助陀螺仪在线标定的适用性,减小了因测量噪声导致的误差估计不充分的问题;有效提高了星敏感器的定姿精度。
附图说明
图1为本发明方法的流程图;
图2为利用本发明进行的仿真结果图,图2(a)为陀螺漂移估计值比较曲线,图2(b)为刻度因数误差估计值比较曲线,图2(c)为补偿前后定位路径对比曲线;
图3为模糊逻辑控制方法输入量的隶属函数。
具体实施方式
下面将结合附图和实施例对本发明作进一步的详细说明。
本发明是一种基于遗忘因子自适应选择sage-husa滤波的船用星敏感器辅助陀螺仪在线标定方法,方法流程如图1所示,包括以下步骤:
步骤一:接通电源,对星敏感器/惯性组合导航系统进行初始化。导航初始时刻,需初始化系统:
(1)导航信息初始化:舰船位置信息
(2)初始化常值参量:当地经纬度λ、
(3)简化sage-husa自适应滤波器参数初值:状态变量初值x0=[δe0δk0ε0]t,协方差阵p0,系统噪声方差阵q0,量测噪声方差阵r0;
其中,δe0表示误差四元数矢量部分的初始值;δk0表示陀螺仪刻度因数的初始值;ε0表示陀螺仪漂移的初始值。
初始转换矩阵计算如下:
其中,b表示载体坐标系,n表示导航坐标系,
初始化四元数q0计算如下:
令
其中,cij为矩阵
标定过程中,利用该初始信息进行更新,得到陀螺仪的角速度信息。
步骤二:系统实时采集惯性器件和星敏感器的输出数据,包括陀螺仪输出的三轴角速度
步骤三:系统根据步骤二中陀螺仪采集到的角速度
(1)角速度更新:
其中,e表示地球坐标系,p表示平台坐标系;
(2)载体姿态四元数计算值的更新:
设任意时刻载体坐标系相对平台坐标系的转动四元数为:
其中,
四元数
其中,
根据k时刻载体坐标系相对平台坐标系的转动四元数q(k)、e1(k)、e2(k)、e3(k),求取k时刻转动四元数的变化率为:
在k+1时刻载体的转动四元数具体为:
至此,根据上式得到载体姿态四元数的计算值
(3)载体姿态、速度及位置的更新:
利用得到的q(k+1)、e1(k+1)、e2(k+1)、e3(k+1),更新捷联矩阵
其中,上式中的q、ei,i=1,2,3为k+1时刻载体的转动四元数中q(k+1)、ei(k+1),i=1,2,3,上式中省略了k+1。
更新姿态信息:
利用转换关系式
其中,
根据k时刻的载体的三轴速度vx(k)、vy(k)与vz(k),求取k时刻的载体速度变化率
其中,r表示地球半径;当k=1时,vx(1)、vy(1)、vz(1)为步骤一中初始化系统时获得的载体初始速度,
至此得到载体的姿态角、速度和位置。
步骤四:由步骤二中星敏感器输出相对于惯性空间的姿态矩阵
其中,cib(i,j)表示矩阵
至此,根据上式得到载体姿态四元数的真实值q。
步骤五:利用模糊逻辑控制方法确定k时刻的简化sage-husa自适应滤波方法中遗忘因子b,构造简化sage-husa自适应滤波器,具体过程如下:
(1)根据步骤三中载体姿态四元数的计算值与步骤四中载体姿态四元数的真实值解算得到误差四元数,过程如下:
其中,δq=[δqδe]t表示误差四元数,δq表示误差四元数的实数部分,δe表示误差四元数的虚数部分;(·)-1表示矩阵的逆。
(2)利用(1)中误差四元数的第3个分量δe2的绝对值对星敏感器测量噪声统计特性进行分析,并将δe2的绝对值作为模糊逻辑控制器的输入,离散的将星敏感器测量噪声状态定义为若干级,形式为:
u={nnlnhn}
确定δe2绝对值的论域集并划分论域,基本论域为[ac],通过量化因子转化的模糊论域为[a1c1],划分后的论域为:
其中,nn表示星敏感器无测量噪声;ln表示星敏感器测量噪声为低噪声;hn表示星敏感器测量噪声为高噪声。
(3)根据δe2绝对值的实际情况建立模糊论域各元素对模糊语言的隶属度大小,即相应的隶属函数,表达形式如下:
三角形函数为:
梯形函数为:
其中,参数f,v,g,d,m,p,y,e,n分别表示模糊集合中的元素。
(4)将δe2的绝对值作为模糊逻辑控制的输入信息,结合(3)中隶属函数,经过模糊逻辑运算,进而推测出k时刻星敏感器测量噪声状态。其中模糊逻辑控制方法采用的是“if-then”规则来形成基于数据的规则,具体表达式为:
(5)依据(4)中推测出的k时刻星敏感器测量噪声状态,采用重心法进行反模糊化得到b的精确值,进而选择k时刻相应的遗忘因子b,选择方法具体如下:
其中,b的范围为(01);重心法计算式为
(6)构造简化sage-husa自适应滤波器,滤波状态量为x=[δeδkε]t,观测量为z=δe,滤波过程具体如下:
利用下列微方程建立系统方程:
其中,δe为误差四元数,且δe=[δe1δe2δe3];δk为陀螺刻度因数,且δk=[δkxδkyδkz];ε为陀螺漂移,且ε=[εxεyεz];
根据上述误差模型,利用简化sage-husa自适应滤波器,对陀螺误差进行在线标定,具体算法如下:
dk=(1-b)/(1-bk+1)
xk/k-1=fk,k-1xk-1
其中,dk为k时刻的调节因子,fk,k-1为k-1时刻到k时刻的状态转移矩阵,且
根据测量信息计算k时刻的新息,再通过指数加权法不断估计k时刻的测量噪声r阵:
vk=zk-hxk/k-1
计算滤波增益、状态估值及更新pk:
kk=pk/k-1ht(hpk/k-1ht+rk)-1
xk=xkk-1+kkvk
pk=(ι-kkh)pk/k-1
其中,xk为k时刻的状态量,当k=1时,状态量x=[δe(1)δk(1)ε(1)]为步骤一中初始化系统时获得的初始状态量x0。再对系统进行估计的同时,估计量测噪声的协方差阵r,以提高滤波精度。
步骤六:利用步骤五中估计出的ε和δk补偿陀螺仪输出信息
陀螺仪输出角速度信息补偿:
其中,
将
步骤七:将步骤六中得到的补偿后载体的姿态、速度及位置信息存储并输出。
对本发明有益效果进行验证方式如下:
仿真实验:
舰船以10m/s的速度匀速直线航行
当地经纬度:
地球半径:r=6378393.0m
由万有引力可得的地球表面重力加速度:g=9.78049m/s2
地球自转角速度:ωie=7.2931158×10-5rad/s
因受风浪等因素影响,载体姿态呈周期性变化,模拟载体姿态如:
陀螺参数:
陀螺常值漂移:0.01rad/h
陀螺刻度因数误差:5×10-4
陀螺白噪声:1×10-5
加速度计参数:
加速度计零偏:10-5g
加速度计白噪声:10-6g
星敏感器参数:
星敏感器的测量噪声看成零均值的高斯白噪声;
200s~400s时,星敏感器测量噪声方差变为5″;
600s~800s时,星敏感器测量噪声方差变为60″;
1000s~1200s时,星敏感器测量噪声方差变为30″;
1400s~1600s时,星敏感器测量噪声方差变为10″;
其他时刻星敏感器无噪声。
模糊逻辑控制方法参数设置:
基本论域:[ac]=[0120]
模糊论域:[a1c1]=[03]
划分后的论域:
不同噪声下的遗传因子:
简化sage-husa自适应滤波器参数设置:
p0=diag([10-610-610-66.25×10-66.25×10-66.25×10-610-1010-1010-10]2)
q0=diag([2.97×10-72.97×10-72.97×10-7000000]2)
r0=diag([2×10-32×10-32×10-3]2)
采样频率:1s
仿真时间:1800s
利用发明所述方法,得到星敏感器测量噪声统计特性发生变化情况下标准卡尔曼滤波和基于遗传因子自适应选择sage-husa滤波方法估计陀螺漂移和刻度因数的比较曲线。图2为仿真结果比较曲线,图3为δe2绝对值的隶属函数。根据图2结果可看出,利用本发明提出的基于遗忘因子自适应选择sage-husa滤波的船用星敏感器辅助陀螺在线标定方法较好的跟踪测量噪声统计特性,修正滤波器的测量噪声参数,估计出了陀螺漂移和刻度因数误差,提高滤波精度,改善导航系统精度,提高系统的环境适用性,满足实际需求。