本发明提出了一种基于李群矩阵卡尔曼滤波(liegroupmatrixkalmanfilter,lgmkf)的sins捷联惯性导航系统自对准方法,该方法属于导航系统初始对准技术领域。
背景技术:
惯性导航系统在开始进行导航解算之前,需要进行初始对准。初始对准是惯性导航的关键技术。初始对准精度的高低很大程度上决定了导航的精度,初始对准的速度快慢一定程度上影响着导航系统的应用范围。惯性导航系统初始对准可分为粗对准和精对准,通常采用解析法、非线性滤波方法等进行粗对准,采用罗经法、非线性滤波方法等方法进行精对准。通过初始对准,获得初始姿态角,从而确定导航解算所需要的初始姿态矩阵。
对准精度和对准实时性是惯导系统进行初始对准时的两项重要技术指标。初始对准精度影响捷联惯性导航系统的性能,初始对准实时性标志着系统的快速反应能力。因此,在保证初始对准精度的同时提高初始对准的实时性,才能有效提高捷联惯性导航系统的性能。
技术实现要素:
本发明的目的是为了应对由传统四元数方法描述初始姿态矩阵
为了达到上述目的,本发明提供如下技术方案:
基于lgmkf的sins捷联惯性导航系统自对准方法,其特征在于,该方法通过下述步骤实现:
步骤(1):sins进行系统预热准备,启动系统,获得载体所在位置的经度λ、纬度l等基本信息,获取传感器实时数据,传感器实时数据包括惯性测量单元imu中陀螺仪输出的载体系相对于惯性系的旋转角速率信息在载体系的投影
步骤(2):对采集到的陀螺仪和加速度计的数据进行预处理,基于李群微分方程,建立基于李群描述的线性初始对准系统模型,所述线性初始对准系统模型包括系统线性状态模型和线性量测模型:
本方法的详细描述中坐标系定义如下:
地球坐标系e系,选取地球中心为原点,x轴位于赤道平面内,从地心指向本初子午线,z轴从地心指向地理北极,x轴、y轴和z轴构成右手坐标系,随地球自转而转动;
地心惯性坐标系i系,选取地球中心为原点,x轴位于赤道平面内,从地心指向春分点,z轴从地心指向地理北极,x轴、y轴和z轴构成右手坐标系;
导航坐标系n系,表示载体所在位置的地理坐标系,选取舰载机重心为原点,x轴指向东向e,y轴指向北向n,z轴指向天向u;本方法中导航坐标系选取为地理坐标系;
载体坐标系b系,表示捷联惯性导航系统三轴正交坐标系,选取舰载机重心为原点,x轴、y轴、z轴分别沿舰载机机体横轴指向右、沿纵轴指向前、沿立轴指向上;
初始导航坐标系n(0)系,表示sins开机运行时刻的导航坐标系,并在整个对准过程中相对于惯性空间保持静止;
初始载体坐标系b(0)系,表示sins开机运行时刻的载体坐标系,并在整个对准过程中相对于惯性空间保持静止;
基于李群微分方程,建立基于李群描述的线性初始对准系统模型:
根据sins初始对准原理,sins的自对准问题,可以转化为姿态变换的求解问题;姿态变换表示为两个坐标系之间的旋转变换,sins惯性导航系统的姿态变换可以通过一个3×3的正交变换矩阵表示;根据李群定义及其性质,该正交变换矩阵符合李群的特殊正交群so(n)的性质,构成了三维旋转群so(3):
其中,三维旋转群r∈so(3)表示特定的导航姿态矩阵,
因此,sins中姿态变换的求解问题,可以转化为对基于李群描述的姿态矩阵r的求解问题;根据基于李群描述的姿态矩阵链式法则,导航姿态矩阵
其中,t表示时间变量,
根据李群微分方程,姿态矩阵
其中,
由公式(2)-(5)可以看出,
根据sins初始对准原理,载体速度微分方程表示为:
其中,vn表示相对于地球的载体速度信息,fn表示比力在导航系下的投影,fb表示比力在载体系下的投影即加速度计输出的载体系加速度信息,gn表示当地重力加速度在导航系下的投影;
将公式(2)代入公式(6),可得:
对公式(7)两边同时左乘姿态变换矩阵
在[0,t]上对公式(8)两边同时积分,整理后可得:
其中,vn(0)表示初始时刻载体的速度信息;
公式(9)可以简化表示为:
其中
公式(10)表示为基于李群描述的sins速度微分方程在惯性坐标系的积分形式,公式(11)和公式(12)可由传感器数据实时计算得到;由于公式(10)可以看作是关于初始姿态矩阵
建立系统线性状态方程模型为:
建立系统线性量测方程模型为:
β(t)
=r(t)α(t)(14)
步骤(3):将步骤(2)所得的基于李群描述的线性初始对准系统模型做离散化处理:
根据公式(13),由于将sins自对准问题转化为对初始姿态矩阵
由于公式(14)中给出的
矢量
矢量
其中,m为系统循环次数,k=0,1,2,...,m-1,δv1和δv2分别表示两个相邻半采样周期内加速度计测量得到的速度变化量,δθ1和δθ2分别表示两个相邻半采样周期内陀螺仪测量角度变化量,t为采样周期,
由于量测方程是根据传感器实测数据计算所得,存在量测误差;根据公式(14)、公式(16)和公式(17),建立离散形式的系统线性量测方程为:
β(tk)
=r(tk)α(tk)
+q(tk)(18)
其中,q(tk)为系统量测噪声协方差矩阵,α(tk)和β(tk)可由采集到的陀螺仪和加速度计数据实时计算得到;
由于初始姿态矩阵
基于李群描述初始姿态矩阵
步骤(4):根据lgmkf最优估计算法,直接对基于李群描述的初始姿态矩阵
由于矩阵滤波是一种对矩阵形式状态量的最小方差无偏估计方法,基于李群描述的线性初始对准滤波模型中状态量为矩阵形式,因此结合李群特性和矩阵滤波原理设计了一种基于lgmkf的sins惯性导航系统自对准方法;lgmkf最优估计算法中,滤波新息反映的是测量矩阵与估计矩阵之间的差值,噪声矩阵的协方差矩阵可由其向量形式的协方差定义得到;
lgmkf最优估计算法时间更新方程为:
基于三维旋转群so(3)群的特性和矩阵滤波原理,状态矩阵和误差协方差矩阵的一步预测可以表示为:
pk+1/k
=pk/k
+qw(21)
其中,
lgmkf最优估计算法测量更新方程为:
lgmkf观测方程中量测矩阵为矢量形式,在滤波过程中不能维持so(3)群的特性同时不满足矩阵乘法的维数要求,根据矩阵滤波原理,在保留原有量测信息基础上将αk+1扩维得到k+1时刻的量测矩阵hk+1:
其中,
根据矩阵滤波原理和克罗内克积特性,k+1时刻的lgmkf滤波增益矩阵kk+1可以重构为:
其中,
lgmkf中滤波新息定义为测量矩阵与估计测量矩阵之间的差值,k+1时刻的滤波新息矩阵
其中,
由于lgmkf中滤波新息矩阵为矢量形式,不能满足lgmkf滤波要求,为此在计算k+1时刻初始姿态矩阵的后验估计
其中,
根据矩阵滤波原理,k+1时刻的lgmkf误差协方差矩阵的后验估计
其中,
因此,lgmkf最优估计算法归纳为:
基于lgmkf的sins自对准方法,通过将初始姿态估计问题转化为so(3)群的最优估计问题,直接对初始姿态矩阵
步骤(5):根据步骤(4)所得的基于李群描述的的初始姿态矩阵
根据之前步骤中求解得到的姿态变化矩阵
步骤(6):自对准过程运行时间为mt,获取传感器实时数据的时刻为kt,t为传感器采样周期,k=0,1,2,…,m,若k=m,则输出自对准解算结果,完成sins自对准过程,若k<m,表示自对准过程未完成,则重复上述骤(1)至步骤(5),直至完成sins自对准过程。
进一步的,步骤(1)中启动并初始化sins惯性导航系统,获得载体所在位置的经度λ、纬度l、载体相对于地球的速度信息vn等基本信息,采集惯性测量单元imu中陀螺仪输出的角速率信息
进一步的,步骤(2)中基于李群微分方程,建立基于李群描述的关于初始旋转矩阵
步骤(2.1):通过
基于李群描述的姿态矩阵链式法则,姿态矩阵
由于
其中
根据公式(30)-(32),姿态矩阵
步骤(2.2):通过陀螺仪输出的角速率信息
tk-1时刻到tk时刻的姿态矩阵
其中,根据双子样旋转矢量法,可得:
根据公式(34)和公式(35),姿态矩阵
步骤(2.3):建立基于李群描述的关于初始旋转矩阵
根据基于李群描述的姿态矩阵链式法则和载体速度微分方程,可知:
对公式(37)进行移项、积分、整理等操作,可得:
公式(38)表示为基于李群描述的sins速度微分方程在惯性坐标系的积分形式,并将姿态矩阵
归纳可得关于初始姿态矩阵
其中,
进一步的,步骤(3)中采样周期为ts,本步骤对基于李群描述的线性初始对准系统模型做离散化处理的方式为:
步骤(3.1):由于公式(40)中给出的
步骤(3.2):将对sins中初始姿态矩阵
基于李群描述初始姿态矩阵
进一步的,步骤(4)中根据lgmkf最优估计算法,直接对基于李群描述的初始姿态矩阵
lgmkf最优估计算法中,滤波新息
基于李群描述的sins自对准lgmkf最优估计算法为:
其中,
基于lgmkf的sins自对准方法,通过将初始姿态估计问题转化为so(3)群的最优估计问题,直接对初始姿态矩阵
进一步的,步骤(5)中求解姿态矩阵
由于在步骤(2)中,将sins中姿态矩阵
根据公式(33)、公式(36)和公式(43)求解得到的
进一步的,步骤(1)中,载体处于动态情况下,其航向角ψ、俯仰角θ、横滚角γ作周期变化,姿态变化情况如下:
进一步的,步骤(1)中,加速度计三个方向轴上的常值偏差1×10-4g,随机测量噪声为
进一步的,步骤(2)中,采样周期t为0.02s。
进一步的,步骤(4)中,滤波过程参数初始值为:
姿态矩阵初始值为:
误差协方差矩阵初始值为:
量测噪声协方差矩阵设定为:
过程噪声协方差矩阵设定为:
进一步的,步骤(6)中,m=600s。
与现有技术相比,本发明具有如下优点和有益效果:
(1)本发明采用李群描述初始姿态矩阵
(2)本发明采用的李群矩阵卡尔曼滤波(lgmkf)最优估计方法,将对初始姿态的估计问题转化为对so(3)群的最优估计问题,实现了sins一步直接自对准过程,不仅能够大幅度缩短对准时间,还能够提高对准的可靠性。
(3)本发明采用的基于lgmkf的sins捷联惯性导航系统自对准方法,能够有效避免传统四元数滤波方法中由四元数向初始姿态矩阵
附图说明
图1为基于lgmkf的sins惯性导航系统自对准方法流程图;
图2为载体坐标系到导航坐标系旋转运动示意图;
图3为lgmkf算法示意图;
图4为lgmkf算法流程图;
具体实施方式
下面结合本发明系统流程图,对本发明提供的技术方案具体实施步骤进行详细描述:
本发明提供的基于lgmkf的sins捷联惯性导航系统自对准方法,首先获取传感器实时数据;对采集到的数据进行处理,基于李群微分方程,建立基于李群描述的线性初始对准系统模型,包括线性状态模型和线性量测模型;对离散后的线性初始对准滤波模型,采用lgmkf最优估计算法,估计得到基于李群描述的初始姿态矩阵
本发明是一种基于lgmkf的sins初始对准方法,发明整体流程如图1所示,包括以下步骤:
步骤(1):启动并初始化sins惯性导航系统,获得载体所在位置的经度λ、纬度l、载体相对于地球的速度信息vn等基本信息,采集惯性测量单元imu中陀螺仪输出的角速率信息
步骤(2):对采集到的陀螺仪和加速度计的数据进行预处理,基于李群微分方程,建立基于李群描述的关于初始旋转矩阵
步骤(2.1):通过
基于李群描述的姿态矩阵链式法则,姿态矩阵
由于
其中
根据公式(30)-(32),姿态矩阵
步骤(2.2):通过陀螺仪输出的角速率信息
tk-1时刻到tk时刻的姿态矩阵
其中,根据双子样旋转矢量法,可得:
根据公式(34)和公式(35),姿态矩阵
步骤(2.3):建立基于李群描述的关于初始旋转矩阵
根据基于李群描述的姿态矩阵链式法则和载体速度微分方程,可知:
对公式(37)进行移项、积分、整理等操作,可得:
公式(38)表示为基于李群描述的sins速度微分方程在惯性坐标系的积分形式,并将姿态矩阵
归纳可得关于初始姿态矩阵
其中,
步骤(3):采样周期为ts,本步骤对基于李群描述的线性初始对准系统模型做离散化处理,
步骤(3.1):由于公式(40)中给出的
步骤(3.2):将对sins中初始姿态矩阵
基于李群描述初始姿态矩阵
步骤(4):根据lgmkf最优估计算法,直接对基于李群描述的初始姿态矩阵
lgmkf最优估计算法中,滤波新息
基于李群描述的sins自对准lgmkf最优估计算法为:
其中,
基于lgmkf的sins自对准方法,通过将初始姿态估计问题转化为so(3)群的最优估计问题,直接对初始姿态矩阵
步骤(5):求解姿态矩阵
由于在步骤(2)中,将sins中姿态矩阵
根据公式(33)、公式(36)和公式(43)求解得到的
步骤(6):自对准过程运行时间为mt,获取传感器实时数据的时刻为kt,t为传感器采样周期,k=0,1,2,...,m,若k=m,则输出自对准解算结果,完成sins自对准过程,若k<m,表示自对准过程未完成,则重复上述骤(1)至步骤(5),直至完成sins自对准过程。
在以下的仿真条件下,对该方法进行仿真实验:
步骤(1)中,载体处于动态情况下,其航向角ψ、俯仰角θ、横滚角γ作周期变化,姿态变化情况如下:
步骤(1)中,加速度计三个方向轴上的常值偏差1×10-4g,随机测量噪声为
步骤(2)中,采样周期t为0.02s。
步骤(4)中,滤波过程参数初始值为:
姿态矩阵初始值为:
误差协方差矩阵初始值为:
量测噪声协方差矩阵设定为:
过程噪声协方差矩阵设定为:
步骤(6)中,m=600s。
本发明采用李群描述初始姿态矩阵
以上所述仅是本发明的优选实施方式,并不用于限制本发明。应当指出,对于本技术领域的普通技术人员来说,在不脱离本发明原理的前提下,还可以做出若干改进和变型,这些改进和变型也应视为本发明的保护范围。