[发明专利]基于LGMKF的SINS捷联惯性导航系统自对准方法有效
申请号: | 201910224807.6 | 申请日: | 2019-03-24 |
公开(公告)号: | CN109931957B | 公开(公告)日: | 2020-08-28 |
发明(设计)人: | 裴福俊;朱德森;蒋宁;徐浩;尹舒男 | 申请(专利权)人: | 北京工业大学 |
主分类号: | G01C25/00 | 分类号: | G01C25/00 |
代理公司: | 北京思海天达知识产权代理有限公司 11203 | 代理人: | 沈波 |
地址: | 100124 *** | 国省代码: | 北京;11 |
权利要求书: | 查看更多 | 说明书: | 查看更多 |
摘要: | 本发明公开了基于LGMKF的SINS捷联惯性导航系统自对准方法,采用李群描述代替传统四元数描述实现对SINS姿态变换的计算,利用李群微分方程建立基于李群描述的线性初始对准滤波模型,设计了LGMKF方法确定导航所需要的初始姿态矩阵。本发明采用LGMKF算法直接对初始姿态矩阵进行最优估计,从而将初始姿态估计问题转化为SO(3)群的最优估计问题,不仅实现了SINS的一步直接自对准,大幅度缩短了对准时间,并能够避免传统四元数描述初始姿态矩阵而产生的非唯一性和非线性问题,有效提高了对准精度,在实际工程中具有实用价值。 | ||
搜索关键词: | 基于 lgmkf sins 惯性 导航系统 对准 方法 | ||
【主权项】:
1.基于LGMKF的SINS捷联惯性导航系统自对准方法,其特征在于,该方法通过下述步骤实现:步骤(1):SINS进行系统预热准备,启动系统,获得载体所在位置的经度λ、纬度L基本信息,获取传感器实时数据,传感器实时数据包括惯性测量单元IMU中陀螺仪输出的载体系相对于惯性系的旋转角速率信息在载体系的投影
和加速度计输出的载体系加速度信息fb等;步骤(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)表示特定的导航姿态矩阵,
表示3×3的向量空间,上标T表示矩阵的转置,I表示三维单位矩阵,det(R)表示为矩阵R的行列式;因此,SINS中姿态变换的求解问题,转化为对基于李群描述的姿态矩阵R的求解问题;根据基于李群描述的姿态矩阵链式法则,导航姿态矩阵
分解为三个矩阵乘积形式:
其中,t表示时间变量,
表示当前载体系相对于当前导航系的姿态变换矩阵,
表示初始导航系相对于当前导航系的姿态变换矩阵,初始姿态矩阵
表示初始载体系相对于初始导航系的姿态变换矩阵,
表示当前载体系相对于初始载体系的姿态变换矩阵;根据李群微分方程,姿态矩阵
和
随时间变化更新过程为:![]()
其中,
表示初始载体系相对于当前载体系的姿态变换矩阵,![]()
表示导航系相对于惯性系的旋转角速率在导航系的投影,
表示地球系相对于惯性系的旋转角速率在导航系的投影,
表示导航系相对于地球系的旋转角速率在导航系的投影,
表示陀螺仪输出的载体系相对于惯性系的旋转角速率在载体系的投影;符号(·×)表示将一个三维向量转换成一个反对称矩阵的运算,运算规则如下:
由公式(2)‑(5)看出,
和
由传感器数据实时计算得到,而初始姿态矩阵
是一个常值矩阵;因此,SINS中姿态矩阵
的求解问题,转化为对基于李群描述的初始姿态矩阵
的求解问题;根据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)中给出的
和
均为连续形式,在实际计算中需要对其作离散处理;通过α(t)和β(t)对应的积分迭代算法具体解算α和β的具体值;在载体作姿态更新过程中,由于处于角晃动或线晃动激烈、频繁的外部环境时,单子样旋转矢量法对不可交换误差的补偿程度较低,会造成严重的算法漂移;多子样旋转矢量法可以有效避免该问题,子样数越高,算法精度越高,但计算量也随之增大;综合考虑,选择双子样旋转矢量法对α(t)和β(t)进行积分迭代计算;矢量
在tk时刻采用双子样旋转矢量法求解,近似后得到:
矢量
在tk时刻采用双子样旋转矢量法求解,近似后得到:
其中,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)由采集到的陀螺仪和加速度计数据实时计算得到;由于初始姿态矩阵
为符合三维旋转群SO(3)性质的李群矩阵,因此SINS中初始姿态矩阵
的最优估计问题,转化为对三维旋转群SO(3)群的最优估计问题;根据上述内容,建立基于李群描述的线性初始对准滤波模型为:
基于李群描述初始姿态矩阵
的SINS自对准系统,能够避免基于四元数描述初始姿态矩阵
的SINS自对准系统中由四元数构造初始姿态矩阵
产生的非唯一性问题和非线性问题;步骤(4):根据LGMKF最优估计算法,直接对基于李群描述的初始姿态矩阵
进行最优估计:由于矩阵滤波是一种对矩阵形式状态量的最小方差无偏估计方法,基于李群描述的线性初始对准滤波模型中状态量为矩阵形式,因此结合李群特性和矩阵滤波原理设计了一种基于LGMKF的SINS惯性导航系统自对准方法;LGMKF最优估计算法中,滤波新息反映的是测量矩阵与估计矩阵之间的差值,噪声矩阵的协方差矩阵可由其向量形式的协方差定义得到;LGMKF最优估计算法时间更新方程为:基于三维旋转群SO(3)群的特性和矩阵滤波原理,状态矩阵和误差协方差矩阵的一步预测可以表示为:
Pk+1/k=Pk/k+Qw (21)其中,
表示k+1时刻的初始姿态矩阵的一步预测,
表示k时刻的初始姿态矩阵的后验估计,
表示k+1时刻的误差协方差矩阵的一步预测,
表示k时刻的误差协方差矩阵的后验估计,
表示k时刻的过程噪声协方差矩阵;LGMKF最优估计算法测量更新方程为:LGMKF观测方程中量测矩阵为矢量形式,在滤波过程中不能维持SO(3)群的特性同时不满足矩阵乘法的维数要求,根据矩阵滤波原理,在保留原有量测信息基础上将αk+1扩维得到k+1时刻的量测矩阵Hk+1:
其中,
表示k+1时刻由陀螺仪和加速度计输出得到的状态信息矩阵,
表示单位矩阵,符号
表示克罗内克积,运算规则如下:
根据矩阵滤波原理和克罗内克积特性,k+1时刻的LGMKF滤波增益矩阵Kk+1可以重构为:
其中,
表示k+1时刻的误差协方差矩阵的一步预测,![]()
表示k+1时刻的量测矩阵,
表示k+1时刻的量测噪声协方差矩阵,上标‑1表示对矩阵进行求逆运算;LGMKF中滤波新息定义为测量矩阵与估计测量矩阵之间的差值,k+1时刻的滤波新息矩阵
表示为:
其中,
和
由陀螺仪和加速度计输出解算得到,
表示k+1时刻的初始姿态矩阵的一步预测;由于LGMKF中滤波新息矩阵为矢量形式,不能满足LGMKF滤波要求,为此在计算k+1时刻初始姿态矩阵的后验估计
时需要对
和Kk+1做特殊处理:
其中,
表示k+1时刻的初始姿态矩阵的一步预测,![]()
表示k+1时刻的滤波新息矩阵,
定义如下:
根据矩阵滤波原理,k+1时刻的LGMKF误差协方差矩阵的后验估计
可以表示为:
其中,
表示单位阵,
表示k+1时刻的滤波增益矩阵,
表示k+1时刻的转换矩阵,
表示k+1时刻的误差协方差矩阵的一步预测,
表示k+1时刻的量测噪声协方差矩阵;因此,LGMKF最优估计算法归纳为:
基于LGMKF的SINS自对准方法,通过将初始姿态估计问题转化为SO(3)群的最优估计问题,直接对初始姿态矩阵
进行最优估计,实现了SINS一步直接自对准过程,不仅能够大幅度缩短了对准时间,还能够提高对准的可靠性;基于LGMKF的SINS自对准方法,能够避免传统四元数滤波过程中由四元数向初始姿态矩阵
转换时存在的复杂表述问题和大量计算误差,能够有效降低计算复杂度并提高对准精度;步骤(5):根据步骤(4)所得的基于李群描述的的初始姿态矩阵
求解导航系统所需的姿态矩阵
根据之前步骤中求解得到的姿态变化矩阵
和
信息,通过公式(2)即可求解导航姿态矩阵
步骤(6):自对准过程运行时间为MT,获取传感器实时数据的时刻为kT,T为传感器采样周期,k=0,1,2,...,M,若k=M,则输出自对准解算结果,完成SINS自对准过程,若k<M,表示自对准过程未完成,则重复上述骤(1)至步骤(5),直至完成SINS自对准过程。
下载完整专利技术内容需要扣除积分,VIP会员可以免费下载。
该专利技术资料仅供研究查看技术是否侵权等信息,商用须获得专利权人授权。该专利全部权利属于北京工业大学,未经北京工业大学许可,擅自商用是侵权行为。如果您想购买此专利、获得商业授权和技术合作,请联系【客服】
本文链接:http://www.vipzhuanli.com/patent/201910224807.6/,转载请声明来源钻瓜专利网。