CN110595503A - 基于李群最优估计的sins捷联惯性导航***晃动基座自对准方法 - Google Patents
基于李群最优估计的sins捷联惯性导航***晃动基座自对准方法 Download PDFInfo
- Publication number
- CN110595503A CN110595503A CN201910718946.4A CN201910718946A CN110595503A CN 110595503 A CN110595503 A CN 110595503A CN 201910718946 A CN201910718946 A CN 201910718946A CN 110595503 A CN110595503 A CN 110595503A
- Authority
- CN
- China
- Prior art keywords
- matrix
- initial
- lie group
- representing
- alignment
- 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.)
- Granted
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
- G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
Landscapes
- Engineering & Computer Science (AREA)
- Manufacturing & Machinery (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Navigation (AREA)
Abstract
本发明公开了基于李群最优估计的SINS捷联惯性导航***晃动基座自对准方法,采用李群描述代替传统四元数描述实现对SINS姿态变换的计算,利用李群微分方程建立基于李群描述的线性初始对准滤波模型,设计了李群最优估计方法确定导航所需要的初始姿态矩阵。本发明采用李群最优估计算法直接对初始姿态矩阵进行最优估计,从而将初始姿态估计问题转化为SO(3)群的最优估计问题,实现了SINS的一步直接自对准,大幅度缩短了对准时间;避免了传统四元数描述初始姿态矩阵而产生的非唯一性和非线性问题,有效提高了对准精度;避免了李群滤波方法中由于加性误差和计算误差产生的无法保证旋转矩阵正交性的问题。本发明在实际工程中具有实用价值。
Description
技术领域
本发明公开了一种基于李群最优估计的SINS捷联惯性导航***晃动基座自对准方法,该方法属于导航方法及应用技术领域。
背景技术
所谓导航,就是正确地引导载体沿着预定的航线、以要求的精度、在指定的时间内将载体引导至目的地的过程。惯性导航***根据自身传感器的输出,以牛顿第二定律为理论基础,对载体的各项导航参数进行解算。它是一种自主式的导航***,在工作时不依靠外界信息,也不向外界辐射任何能量,隐蔽性好、抗扰性强,能够全天时、全天候为载体提供完备的运动信息。
早期的惯导***以平台惯导为主,随着惯性器件的成熟和计算机技术的发展,上世纪 60年代开始出现了惯性器件与载体直接固联的捷联惯导***。与平台惯导相比,捷联惯导***省去了复杂的实体稳定平台,具有成本低、体积小、重量轻、可靠性高等优点。近年来,捷联惯导***日趋成熟,精度逐步提高,应用范围也逐渐扩大。捷联式惯性导航技术将陀螺仪和加速度计直接安装在载体上,得到载体系下的加速度和角速度,通过导航计算机将测得的数据转换至导航坐标系完成导航,它不需要实体的稳定平台,成本低、体积小、重量轻、可靠性高。
自对准过程的研究在捷联惯导***中占有重要意义,尤其是晃动基座下的自对准方法更是当下的研究热点。除了李群滤波估计方法之外,晃动基座条件下还可以应用奇异值分解和四元数卡尔曼等方法完成自对准过程。但这些方法仍然具有不可忽视的缺陷。奇异值分解是一种基于矩阵分解的最优化方法,由于其计算过程没有满足李群空间的运算规则,会在较大失准角的条件下产生奇异点,影响对准的性能。四元数卡尔曼是利用伪量测方程构建线性化滤波模型,使用卡尔曼滤波器完成对准任务的方法,这种方法虽然不会产生奇异点,但伪量测方程的构建同样会对收敛速度和对准精度产生影响。而在李群滤波对准过程中,由于构建增益矩阵和误差协方差矩阵时在李代数和李群空间的转换导致了计算误差的产生;其滤波器的设计原理是基于卡尔曼滤波改进而来,新息的定义不符合李群旋转矩阵的性质。因此会对对准精度产生影响。
为进一步提高初始对准的性能,本发明针对现有的晃动基座自对准方法存在的问题,借鉴系最优估计的思想,提出了一种新的李群最优估计算法。得益于李群理论的完善,通过改进新息的表达方式来保证状态量时刻满足特殊正交的性质。基于这种方法构建的估计模型避免了传统最优化方法中的奇异值问题,每一步的迭代计算都符合李群旋转矩阵的特性。相比于李群滤波自对准方法而言,在保留李群滤波优势的同时,提高了对准的速度,并且在精度方面也有所提升。仿真实验证明了该算法的可行性,可以作为李群滤波的上位替代进行晃动基座自对准。
发明内容
由于初始对准过程中,载体容易受到外界的各种干扰因素的影响,在对准过程中很难保持静态不动。因此,晃动基座下的自对准算法具有很高的研究意义与应用价值。本发明的目的是为了应对现有晃动基座自对准方法存在的问题:(1)本发明通过李群代替四元数描述初始姿态矩阵,避免了传统四元数描述方法的非唯一性和非线性问题;(2)本发明利用李群微分方程建立基于李群描述的线性自对准滤波模型,实现了SINS的一步直接自对准过程,相较现有的两步对准方法,能够大幅度缩短对准时间并提高对准精度;(3)本发明采用的基于李群最优估计的SINS捷联惯性导航***晃动基座自对准方法,能够有效避免现有四元数卡尔曼滤波方法中由四元数向姿态矩阵转换产生的复杂表述问题和大量计算误差,并能够避免李群滤波方法中由于加性误差和计算误差产生的无法保证旋转矩阵正交性的问题。
为了达到上述目的,本发明提供如下技术方案:
基于李群最优估计的SINS捷联惯性导航***晃动基座自对准方法,其特征在于,该方法通过下述步骤实现:
步骤(1):SINS捷联惯性导航***进行***预热准备,启动***,获得载体所在位置的经度λ、纬度L、当地重力加速度在导航系下的投影gn等基本信息,采集惯性测量单元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开机运行时刻的载体坐标系,并在整个对准过程中相对于惯性空间保持静止;
导航坐标系n′系,表示由李群最优估计算法计算得到的初始导航坐标系,本坐标系和真实导航坐标系n系之间存在转动关系;
基于李群微分方程,建立基于李群描述的线性自对准***模型:
根据SINS捷联惯性导航***原理,SINS晃动基座自对准问题转化为姿态估计问题,姿态变换为两个坐标系之间的旋转变换,导航的姿态矩阵用一个3×3的正交变换矩阵表示;该正交变换矩阵符合李群的特殊正交群SO(n)的性质,构成了三维旋转群SO(3):
其中,R∈SO(3)表示特定的导航姿态矩阵,表示3×3的向量空间,上标T表示矩阵的转置,I表示三维单位矩阵,det(R)表示为矩阵R的行列式;
晃动基座自对准姿态估计问题转化为对基于李群描述的姿态矩阵R的求解问题;根据基于李群描述的姿态矩阵的链式法则,将导航姿态矩阵分解为三个矩阵的乘积形式:
其中,t表示时间变量,表示当前载体系相对于当前导航系的姿态矩阵,表示初始导航系相对于当前导航系的姿态矩阵,初始姿态矩阵表示初始载体系相对于初始导航系的姿态矩阵,表示当前载体系相对于初始载体系的姿态矩阵;
根据李群微分方程,姿态矩阵和随时间变化更新过程为:
其中,表示初始载体系相对于当前载体系的姿态矩阵,表示导航系相对于惯性系的旋转角速率在导航系的投影,在晃动基座条件下其等于地球自转角速率L表示当地纬度,表示陀螺仪输出的载体系相对于惯性系的旋转角速率在载体系的投影,符号(·×)表示将一个三维向量转换成一个反对称矩阵的运算,运算规则如下:
由公式(2)-(5)可知,和由IMU传感器数据实时计算得到,而表示初始时刻的姿态矩阵,其不随时间变化;因此,SINS自对准过程中姿态矩阵的求解问题,转化为对基于李群描述的初始姿态矩阵的求解问题;
在晃动基座条件下,加速度计测量信息fb表示为:
其中,gb表示当地重力加速度在载体系下的投影,δfb表示其他噪声,表示由晃动引起的向心加速度在载体系下的投影,通过IMU传感器数据实时计算得到,计算过程如下:
其中,rb表示惯性传感器安装位置到晃动基座轴心的距离;
将重力加速度gb从加速度计测量信息fb中分离出来,得到:
根据SINS惯性导航基本原理和李群链式法则,gn和gb之间存在如下关系:
对公式(9)进行移项、整理操作,得:
公式(10)简化表示为:
其中,表示重力加速度在初始导航系下的投影,表示重力加速度在初始载体系下的投影;
对公式(11)在[0,t]上做积分,得:
其中,表示重力加速度对应的速度矢量,表示重力加速度对应的速度矢量;
公式(12)可以简化为:
其中
公式(12)是关于初始姿态矩阵的数学方程,和由传感器输出计算得到;公式(12)中给出的和均为连续形式,在实际计算过程中需要进行离散化处理,离散化后建立晃动基座自对准***的线性量测方程如下:
由于将求解姿态矩阵的问题转化为求解初始姿态矩阵的问题,且为符合李群特性的常值矩阵,因此建立晃动基座自对准***的线性状态方程如下:
Rk=Rk-1 (17)
根据上述内容,将姿态矩阵的求解问题转化为初始时刻惯性坐标系下的求解问题,建立具有李群结构的晃动基座自对准***方程,表示为:
步骤(3):根据李群最优估计算法,直接对基于李群描述的初始姿态矩阵进行最优估计:
状态矩阵的一步预测表示为:
其中,表示k时刻的初始姿态矩阵的一步预测,Rk-1表示k-1时刻的初始姿态矩阵的后验估计;
量测矢量的估计值表示为:
其中,表示k时刻速度矢量的估计值,表示k时刻的初始姿态矩阵的一步预测,由传感器输出数据计算得到;
在进行坐标系变换的过程中,误差的存在会导致计算得到的导航坐标系n′系和真实导航坐标系n系之间存在转动关系;θ表示n′系和n系之间相差的旋转角度,利用向量积运算性质,k时刻θ(k)可以表示为:
其中,表示k时刻速度矢量的估计值,和由传感器输出数据计算得到;
根据右手定则,θ(k)为旋转角度在n′系下的投影,将θ(k)投影到b系下得到李群最优估计算法的量测新息为:
根据基于李群描述的SINS捷联惯性导航***原理,李群最优估计算法的状态更新方程可以写为:
其中,Rk表示k时刻的初始姿态矩阵的后验估计,exp()表示从李代数空间到李群空间的指数映射关系,表示基于李群结构的姿态更新矩阵,表示矢量的反对称矩阵形式;
基于李群最优估计的SINS捷联惯性导航***晃动基座自对准算法归纳为:
步骤(4):求解导航***所需的姿态矩阵从而完成晃动基座自对准过程:
根据之前步骤中求解得到的姿态变化矩阵和信息,通过公式(2)即可求解导航姿态矩阵,完成SINS捷联惯性导航***晃动基座自对准。
与现有技术相比,本发明具有如下优点和有益效果:
(1)本发明采用李群描述初始姿态矩阵能够有效避免传统四元数描述初始姿态矩阵声生的非唯一性和非线性问题。
(2)本发明采用李群方法建立线性***模型,实现了SINS一步直接自对准过程,相较于传统的两步对准方法,不仅能够大幅度缩短对准时间,还能够提高对准的精度。
(3)本发明采用的基于李群最优估计的SINS捷联惯性导航***晃动基座自对准方法,能够有效避免传统四元数卡尔曼滤波方法中由四元数向姿态矩阵转换产生的复杂表述问题和大量计算误差,并能够避免李群滤波方法中由于加性误差和计算误差产生的无法保证旋转矩阵正交性的问题。
附图说明
图1捷联惯性导航***装置总体简图
图2捷联惯性导航***流程图
图3导航坐标系与机体坐标系之间转动关系示意图
图4计算坐标系与真实坐标系之间转角示意图
图5李群最优估计方法流程图
图6晃动基座自对准仿真结果图
图7晃动基座自对准对比实验结果图
图8上位机采集真实导航数据页面示意图
具体实施方式
本发明是基于李群最优估计的SINS捷联惯性导航***晃动基座自对准方法设计,下面结合本发明***流程图对本发明的具体实施步骤进行详细的描述:
本发明提供的基于李群最优估计的SINS捷联惯性导航***晃动基座自对准方法,首先获取传感器实时数据;对采集到的数据进行处理,基于李群微分方程,建立基于李群描述的线性自对准***模型;使用李群最优估计算法,估计得到基于李群描述的初始姿态矩阵并求解姿态矩阵在自对准期间,经过多次估计解算,最终得到精确的初始姿态矩阵和姿态矩阵完成自对准过程。
步骤1:启动并初始化SINS惯性导航***,获得载体所在位置的经度λ、纬度L、当地重力加速度gn等基本信息,采集惯性测量单元IMU中陀螺仪输出的角速率信息和加速度计输出的加速度信息fb。
步骤2:对采集到的陀螺仪和加速度计的数据进行处理,基于李群微分方程,建立基于李群描述的线性晃动基座自对准***模型,
步骤(2.1):通过更新计算
基于李群描述的姿态矩阵链式法则,姿态矩阵分解为:
由于表示导航系相对于惯性系的旋转角速率,且变化通常是十分缓慢的,根据基于李群描述的姿态矩阵更新求解过程,tk-1时刻到tk时刻的姿态矩阵近似为:
其中,
根据公式(25)-(27),姿态矩阵迭代过程近似为:
步骤(2.2):通过陀螺仪输出的角速率信息更新计算
tk-1时刻到tk时刻的姿态矩阵近似为:
其中,根据双子样旋转矢量法,可得:
其中,Δθ1和Δθ2分别表示两个相邻半采样周期内由陀螺仪输出计算所得的角增量;
根据公式(29)-(30),姿态矩阵迭代过程近似为:
步骤(2.3):建立基于李群描述的关于初始旋转矩阵的自对准***模型方式为:
在晃动基座条件下,加速度计测量信息fb表示为:
将重力加速度gb从加速度计测量信息fb中分离出来,得到:
根据SINS惯性导航基本原理和李群链式法则,gn和gb之间存在如下关系:
对公式(34)在[0,t]上做积分,整理后可得:
将公式(33)带入公式(35),整理后可得:
公式(36)简化为:
其中
公式(37)是关于初始姿态矩阵的数学方程,和均为连续形式,通过积分迭代算法解算和在的k时刻离散值和
k时刻近似为:
其中,T为采样周期,表示单位矩阵,由公式(28)迭代计算得到;
由于δfb远小于重力加速度gb,在离散过程中将δfb看作极小误差量,因此公式(39)近似为:
其中,
k时刻近似为:
其中,由公式(31)迭代计算得到;
根据上述公式,建立晃动基座自对准***的线性量测方程为:
由于将姿态矩阵的求解问题转化为初始姿态矩阵的求解问题,且为符合李群特性的常值矩阵,因此建立晃动基座自对准***的线性状态方程为:
Rk=Rk-1 (44)
根据上述内容,将姿态矩阵的求解问题转化为初始时刻惯性坐标系下的求解问题,建立了具有李群结构的晃动基座自对准***方程,表示为:
步骤(3):使用李群最优估计算法直接估计
李群最优估计算法对晃动基座自对准模型进行估计,整体过程如下:
其中,为k时刻始姿态矩阵的一步预测,Rk-1为k-1时刻初始姿态矩阵的后验估计,为k时刻量测矢量的估计值,θ(k)为n′系和n系之间的旋转角度在n′系下的投影,为b系下李群最优估计算法的量测新息,为基于李群结构的姿态更新矩阵,Rk为k时刻初始姿态矩阵的后验估计,即为所求的
步骤(4):求解姿态矩阵并解算姿态信息,
在步骤(2)中,将SINS自对准过程中的求解问题转化为对的求解问题,并将分解为三个矩阵乘积的形式:
根据公式(28)、公式(31)和公式(46)求解得到的和姿态矩阵的求解方式为:
并根据求解得到的姿态矩阵解算姿态信息。
本发明的有益效果如下:
(1)在以下的仿真条件下,对该方法进行仿真实验:
步骤(1)中,晃动基座条件下模拟载体收到风浪影响,其航向角ψ、俯仰角θ、横滚角γ作周期变化,姿态变化情况如下:
步骤(1)中,初始地理位置:东经118°,北纬40°;
步骤(1)中,传感器输出频率为100Hz;
步骤(1)中,陀螺仪漂移:三个方向轴上的陀螺常值漂移为0.02°/h,随机漂移为0.005°/h;
步骤(1)中,加速度计零位偏置:三个方向轴上的加速度计常值偏置为2×10-4g,随机偏置为
步骤(2)中,地球自转角速率7.2921158e-5rad/s;
步骤(2)中,时间间隔T为0.02s;
步骤(3)中,李群最优估计算法初始值
方法仿真结果如下:
进行了300s仿真,以姿态角的估计误差作为衡量指标,仿真结果如图(6)所示。从图中可以看出,俯仰姿态在120s左右完成对准,收敛到0.42′;横滚姿态在120s左右完成对准,收敛到0.48′;航向姿态在150s左右完成对准,收敛到1.2′。由仿真结果可知,本方法可以快速有效的完成晃动基座下的自对准任务。
(2)通过真实实验对本发明提出的基于李群最优估计的SINS捷联惯性导航***晃动基座自对准方法进行验证。真实试验中,不提任何供外界辅助信息,实验历时300s。上位导航计算机控制导航***,以100HZ的数据更新速率,115200bps的波特率,采集航向精度达0.1度、姿态精度达0.05度的实际三轴姿态信息,解算获得的载体姿态信息与本步骤中得到的高精度真实载体姿态信息做比较,证明本方法和***的可行性和有效性。
***实验结果如下:
进行了300s的真实实验,以姿态角的估计误差作为衡量对准精度的指标,结果如图(7) 所示。从图中可以看出,俯仰姿态在65s左右完成对准,收敛到-6.82′;横滚姿态在50s 左右完成对准,收敛到4.37′;航向姿态在120s左右完成对准,收敛到23.58′。本方法可以快速有效的完成晃动基座下的自对准任务,相比于四元数卡尔曼滤波方法、奇异值分解方法、李群滤波方法,超调量较小,收敛速度较快,且滤波精度较好。
本发明将晃动基座自对准问题转化为初始旋转矩阵的最优估计问题,采用基于李群方法描述初始姿态矩阵,能够有效避免传统基于四元数方法描述初始姿态矩阵产生的非唯一性和和非线性问题;本发明利用李群微分方程建立一步对准线性***模型,相较于传统的两步对准方法,能够大幅度缩短自对准时间,并提高对准的精度;本发明采用李群最优估计方法,能够避免四元数卡尔曼滤波过程中由四元数向姿态矩阵转换产生的复杂表述问题和大量计算误差,并能够避免李群卡尔曼滤波过程中由于加性误差和计算误差而产生的无法保证旋转矩阵正交性的问题。
以上所述仅是本发明的优选实施方式,并不用于限制本发明。应当指出,对于本技术领域的普通技术人员来说,在不脱离本发明原理的前提下,还可以做出若干改进和变型,这些改进和变型也应视为本发明的保护范围。
Claims (5)
1.基于李群最优估计的SINS捷联惯性导航***晃动基座自对准方法,其特征在于,该方法通过下述步骤实现:
步骤(1):SINS捷联惯性导航***进行***预热准备,启动***,获得载体所在位置的经度λ、纬度L、当地重力加速度在导航系下的投影gn基本信息,采集惯性测量单元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开机运行时刻的载体坐标系,并在整个对准过程中相对于惯性空间保持静止;
导航坐标系n′系,表示由李群最优估计算法计算得到的初始导航坐标系,本坐标系和真实导航坐标系n系之间存在转动关系;
基于李群微分方程,建立基于李群描述的线性自对准***模型:
根据SINS捷联惯性导航***原理,SINS晃动基座自对准问题转化为姿态估计问题,姿态变换为两个坐标系之间的旋转变换,导航的姿态矩阵用一个3×3的正交变换矩阵表示;该正交变换矩阵符合李群的特殊正交群SO(n)的性质,构成了三维旋转群SO(3):
其中,R∈SO(3)表示特定的导航姿态矩阵,表示3×3的向量空间,上标T表示矩阵的转置,I表示三维单位矩阵,det(R)表示为矩阵R的行列式;
晃动基座自对准姿态估计问题转化为对基于李群描述的姿态矩阵R的求解问题;根据基于李群描述的姿态矩阵的链式法则,将导航姿态矩阵分解为三个矩阵的乘积形式:
其中,t表示时间变量,表示当前载体系相对于当前导航系的姿态矩阵,表示初始导航系相对于当前导航系的姿态矩阵,初始姿态矩阵表示初始载体系相对于初始导航系的姿态矩阵,表示当前载体系相对于初始载体系的姿态矩阵;
根据李群微分方程,姿态矩阵和随时间变化更新过程为:
其中,表示初始载体系相对于当前载体系的姿态矩阵,表示导航系相对于惯性系的旋转角速率在导航系的投影,在晃动基座条件下其等于地球自转角速率L表示当地纬度,表示陀螺仪输出的载体系相对于惯性系的旋转角速率在载体系的投影,符号(·×)表示将一个三维向量转换成一个反对称矩阵的运算,运算规则如下:
由公式(2)-(5)可知,和由IMU传感器数据实时计算得到,而表示初始时刻的姿态矩阵,其不随时间变化;因此,SINS自对准过程中姿态矩阵的求解问题,转化为对基于李群描述的初始姿态矩阵的求解问题;
在晃动基座条件下,加速度计测量信息fb表示为:
其中,gb表示当地重力加速度在载体系下的投影,δfb表示其他噪声,表示由晃动引起的向心加速度在载体系下的投影,通过IMU传感器数据实时计算得到,计算过程如下:
其中,rb表示惯性传感器安装位置到晃动基座轴心的距离;
将重力加速度gb从加速度计测量信息fb中分离出来,得到:
根据SINS惯性导航基本原理和李群链式法则,gn和gb之间存在如下关系:
对公式(9)进行移项、整理操作,得:
公式(10)简化表示为:
其中,表示重力加速度在初始导航系下的投影,表示重力加速度在初始载体系下的投影;
对公式(11)在[0,t]上做积分,得:
其中,表示重力加速度对应的速度矢量,表示重力加速度对应的速度矢量;
公式(12)可以简化为:
其中
公式(12)是关于初始姿态矩阵的数学方程,和由传感器输出计算得到;公式(12)中给出的和均为连续形式,在实际计算过程中需要进行离散化处理,离散化后建立晃动基座自对准***的线性量测方程如下:
由于将求解姿态矩阵的问题转化为求解初始姿态矩阵的问题,且为符合李群特性的常值矩阵,因此建立晃动基座自对准***的线性状态方程如下:
Rk=Rk-1 (17)
根据上述内容,将姿态矩阵的求解问题转化为初始时刻惯性坐标系下的求解问题,建立具有李群结构的晃动基座自对准***方程,表示为:
步骤(3):根据李群最优估计算法,直接对基于李群描述的初始姿态矩阵进行最优估计:
状态矩阵的一步预测表示为:
其中,表示k时刻的初始姿态矩阵的一步预测,Rk-1表示k-1时刻的初始姿态矩阵的后验估计;
量测矢量的估计值表示为:
其中,表示k时刻速度矢量的估计值,表示k时刻的初始姿态矩阵的一步预测,由传感器输出数据计算得到;
在进行坐标系变换的过程中,误差的存在会导致计算得到的导航坐标系n′系和真实导航坐标系n系之间存在转动关系;θ表示n′系和n系之间相差的旋转角度,利用向量积运算性质,k时刻θ(k)可以表示为:
其中,表示k时刻速度矢量的估计值,和由传感器输出数据计算得到;
根据右手定则,θ(k)为旋转角度在n′系下的投影,将θ(k)投影到b系下得到李群最优估计算法的量测新息为:
根据基于李群描述的SINS捷联惯性导航***原理,李群最优估计算法的状态更新方程可以写为:
其中,Rk表示k时刻的初始姿态矩阵的后验估计,exp()表示从李代数空间到李群空间的指数映射关系,表示基于李群结构的姿态更新矩阵,表示矢量的反对称矩阵形式;
基于李群最优估计的SINS捷联惯性导航***晃动基座自对准算法归纳为:
步骤(4):求解导航***所需的姿态矩阵从而完成晃动基座自对准过程:
根据之前步骤中求解得到的姿态变化矩阵和信息,通过公式(2)即可求解导航姿态矩阵,完成SINS捷联惯性导航***晃动基座自对准。
2.根据权利要求1所述的基于李群最优估计的SINS捷联惯性导航***晃动基座自对准方法,其特征在于,步骤(1)中启动并初始化SINS捷联惯性导航***,获得载体所在位置的经度λ、纬度L、当地重力加速度在导航系下的投影gn基本信息,采集惯性测量单元IMU中陀螺仪输出的载体系相对于惯性系的旋转角速率信息在载体系的投影和加速度计输出的载体系加速度信息fb。
3.根据权利要求1所述的基于李群最优估计的SINS捷联惯性导航***晃动基座自对准方法,其特征在于,步骤(2)中基于李群微分方程,建立基于李群描述的线性晃动基座自对准***模型;
步骤(2.1):通过更新计算
基于李群描述的姿态矩阵链式法则,姿态矩阵分解为:
由于表示导航系相对于惯性系的旋转角速率,且变化通常是十分缓慢的,根据基于李群描述的姿态矩阵更新求解过程,tk-1时刻到tk时刻的姿态矩阵近似为:
其中,
根据公式(25)-(27),姿态矩阵迭代过程近似为:
步骤(2.2):通过陀螺仪输出的角速率信息更新计算
tk-1时刻到tk时刻的姿态矩阵近似为:
其中,根据双子样旋转矢量法,可得:
其中,Δθ1和Δθ2分别表示两个相邻半采样周期内由陀螺仪输出计算所得的角增量;
根据公式(29)-(30),姿态矩阵迭代过程近似为:
步骤(2.3):建立基于李群描述的关于初始旋转矩阵的自对准***模型方式为:
在晃动基座条件下,加速度计测量信息fb表示为:
将重力加速度gb从加速度计测量信息fb中分离出来,得到:
根据SINS惯性导航基本原理和李群链式法则,gn和gb之间存在如下关系:
对公式(34)在[0,t]上做积分,整理后可得:
将公式(33)带入公式(35),整理后可得:
公式(36)简化为:
其中
公式(37)是关于初始姿态矩阵的数学方程,和均为连续形式,通过积分迭代算法解算和在的k时刻离散值和
k时刻近似为:
其中,T为采样周期,表示单位矩阵,由公式(28)迭代计算得到;
由于δfb远小于重力加速度gb,在离散过程中将δfb看作极小误差量,因此公式(39)近似为:
其中,
k时刻近似为:
其中,由公式(31)迭代计算得到;
根据上述公式,建立晃动基座自对准***的线性量测方程为:
由于将姿态矩阵的求解问题转化为初始姿态矩阵的求解问题,且为符合李群特性的常值矩阵,因此建立晃动基座自对准***的线性状态方程为:
Rk=Rk-1(44)
根据上述内容,将姿态矩阵的求解问题转化为初始时刻惯性坐标系下的求解问题,建立了具有李群结构的晃动基座自对准***方程,表示为:
4.根据权利要求1所述的基于李群最优估计的SINS捷联惯性导航***晃动基座自对准方法,其特征在于,步骤(3)中使用李群最优估计算法直接估计
李群最优估计算法对晃动基座自对准模型进行估计,整体过程如下:
其中,为k时刻始姿态矩阵的一步预测,Rk-1为k-1时刻初始姿态矩阵的后验估计,为k时刻量测矢量的估计值,θ(k)为n′系和n系之间的旋转角度在n′系下的投影,为b系下李群最优估计算法的量测新息,为基于李群结构的姿态更新矩阵,Rk为k时刻初始姿态矩阵的后验估计,即为所求的
5.根据权利要求1所述的基于李群最优估计的SINS捷联惯性导航***晃动基座自对准方法,其特征在于,步骤(4)中求解姿态矩阵并解算姿态信息;
在步骤(2)中,将SINS自对准过程中的求解问题转化为对的求解问题,并将分解为三个矩阵乘积的形式:
根据公式(28)、公式(31)和公式(47)求解得到的和姿态矩阵的求解方式为:
并根据求解得到的姿态矩阵解算姿态信息。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910718946.4A CN110595503B (zh) | 2019-08-05 | 2019-08-05 | 基于李群最优估计的sins捷联惯性导航***晃动基座自对准方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910718946.4A CN110595503B (zh) | 2019-08-05 | 2019-08-05 | 基于李群最优估计的sins捷联惯性导航***晃动基座自对准方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN110595503A true CN110595503A (zh) | 2019-12-20 |
CN110595503B CN110595503B (zh) | 2021-01-15 |
Family
ID=68853609
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910718946.4A Active CN110595503B (zh) | 2019-08-05 | 2019-08-05 | 基于李群最优估计的sins捷联惯性导航***晃动基座自对准方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110595503B (zh) |
Cited By (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111399023A (zh) * | 2020-04-20 | 2020-07-10 | 中国人民解放军国防科技大学 | 基于李群非线性状态误差的惯性基组合导航滤波方法 |
CN112033440A (zh) * | 2020-08-20 | 2020-12-04 | 哈尔滨工业大学 | 一种基于梯度下降优化的摇摆基座下无纬度初始对准方法 |
CN112212889A (zh) * | 2020-09-16 | 2021-01-12 | 北京工业大学 | 基于特殊正交群最优估计的sins捷联惯性导航***晃动基座粗对准方法 |
CN112229421A (zh) * | 2020-09-16 | 2021-01-15 | 北京工业大学 | 基于李群最优估计的捷联惯性导航晃动基座粗对准方法 |
CN113108781A (zh) * | 2021-04-01 | 2021-07-13 | 东南大学 | 一种应用于无人船行进间的改进粗对准算法 |
CN114636357A (zh) * | 2022-03-31 | 2022-06-17 | 北京中科宇航技术有限公司 | 一种针对立式转台晃动状态瞄准试验设计方法 |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103245360A (zh) * | 2013-04-24 | 2013-08-14 | 北京工业大学 | 晃动基座下的舰载机旋转式捷联惯导***自对准方法 |
CN103727940A (zh) * | 2014-01-15 | 2014-04-16 | 东南大学 | 基于重力加速度矢量匹配的非线性初始对准方法 |
CN106871928A (zh) * | 2017-01-18 | 2017-06-20 | 北京工业大学 | 基于李群滤波的捷联惯性导航初始对准方法 |
CN107588771A (zh) * | 2017-08-28 | 2018-01-16 | 北京工业大学 | 基于李群描述的捷联惯性导航解算方法 |
CN109931955A (zh) * | 2019-03-18 | 2019-06-25 | 北京工业大学 | 基于状态相关李群滤波的捷联惯性导航***初始对准方法 |
CN109931957A (zh) * | 2019-03-24 | 2019-06-25 | 北京工业大学 | 基于lgmkf的sins捷联惯性导航***自对准方法 |
-
2019
- 2019-08-05 CN CN201910718946.4A patent/CN110595503B/zh active Active
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103245360A (zh) * | 2013-04-24 | 2013-08-14 | 北京工业大学 | 晃动基座下的舰载机旋转式捷联惯导***自对准方法 |
CN103727940A (zh) * | 2014-01-15 | 2014-04-16 | 东南大学 | 基于重力加速度矢量匹配的非线性初始对准方法 |
CN106871928A (zh) * | 2017-01-18 | 2017-06-20 | 北京工业大学 | 基于李群滤波的捷联惯性导航初始对准方法 |
CN107588771A (zh) * | 2017-08-28 | 2018-01-16 | 北京工业大学 | 基于李群描述的捷联惯性导航解算方法 |
CN109931955A (zh) * | 2019-03-18 | 2019-06-25 | 北京工业大学 | 基于状态相关李群滤波的捷联惯性导航***初始对准方法 |
CN109931957A (zh) * | 2019-03-24 | 2019-06-25 | 北京工业大学 | 基于lgmkf的sins捷联惯性导航***自对准方法 |
Non-Patent Citations (5)
Title |
---|
HAO XU等: "An In-Motion Initial Alignment Algorithm for SINS Using Adaptive Lie Group Filter", 《PROCEEDINGS OF THE 37TH CHINESE CONTROL CONFERENCE》 * |
ROBIN CHHABRA等: "A generalized exponential formula for forward and differential kinematics of open-chain multi-body systems", 《MECHANISM AND MACHINE THEORY》 * |
SILVERE BONNABEL等: "Non-linear observer on Lie Groups for left-invariant dynamics with right-left equivariant output", 《PROCEEDINGS OF THE 17TH WORLD CONGRESS THE INTERNATIONAL FEDERATION OF AUTOMATIC CONTROL》 * |
李益群等: "基于李群谱配点法的卫星姿态仿真", 《导航定位与授时》 * |
梁青琳: ",基于李群描述的捷联惯导***初始对准方法研究", 《中国优秀硕士学位论文全文数据库信息科技辑》 * |
Cited By (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111399023A (zh) * | 2020-04-20 | 2020-07-10 | 中国人民解放军国防科技大学 | 基于李群非线性状态误差的惯性基组合导航滤波方法 |
CN112033440A (zh) * | 2020-08-20 | 2020-12-04 | 哈尔滨工业大学 | 一种基于梯度下降优化的摇摆基座下无纬度初始对准方法 |
CN112212889A (zh) * | 2020-09-16 | 2021-01-12 | 北京工业大学 | 基于特殊正交群最优估计的sins捷联惯性导航***晃动基座粗对准方法 |
CN112229421A (zh) * | 2020-09-16 | 2021-01-15 | 北京工业大学 | 基于李群最优估计的捷联惯性导航晃动基座粗对准方法 |
CN112229421B (zh) * | 2020-09-16 | 2023-08-11 | 北京工业大学 | 基于李群最优估计的捷联惯性导航晃动基座粗对准方法 |
CN113108781A (zh) * | 2021-04-01 | 2021-07-13 | 东南大学 | 一种应用于无人船行进间的改进粗对准算法 |
CN114636357A (zh) * | 2022-03-31 | 2022-06-17 | 北京中科宇航技术有限公司 | 一种针对立式转台晃动状态瞄准试验设计方法 |
CN114636357B (zh) * | 2022-03-31 | 2023-11-10 | 北京中科宇航技术有限公司 | 一种针对立式转台晃动状态瞄准试验设计方法 |
Also Published As
Publication number | Publication date |
---|---|
CN110595503B (zh) | 2021-01-15 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110595503B (zh) | 基于李群最优估计的sins捷联惯性导航***晃动基座自对准方法 | |
CN110702143B (zh) | 基于李群描述的sins捷联惯性导航***动基座快速初始对准方法 | |
CN106871928B (zh) | 基于李群滤波的捷联惯性导航初始对准方法 | |
CN109931955B (zh) | 基于状态相关李群滤波的捷联惯性导航***初始对准方法 | |
CN107588771B (zh) | 基于李群描述的捷联惯性导航解算方法 | |
CN109931957B (zh) | 基于lgmkf的sins捷联惯性导航***自对准方法 | |
Jiancheng et al. | Study on innovation adaptive EKF for in-flight alignment of airborne POS | |
CN103743414B (zh) | 一种里程计辅助车载捷联惯导***行进间初始对准方法 | |
CN106767797B (zh) | 一种基于对偶四元数的惯性/gps组合导航方法 | |
CN104165640A (zh) | 基于星敏感器的近空间弹载捷联惯导***传递对准方法 | |
CN103913181A (zh) | 一种基于参数辨识的机载分布式pos传递对准方法 | |
CN104713555A (zh) | 应用全天域中性点辅助定向的车辆自主导航方法 | |
CN110567455B (zh) | 一种求积更新容积卡尔曼滤波的紧组合导航方法 | |
CN110926499A (zh) | 基于李群最优估计的sins捷联惯性导航***晃动基座自对准方法 | |
CN110849360B (zh) | 面向多机协同编队飞行的分布式相对导航方法 | |
CN104374405A (zh) | 一种基于自适应中心差分卡尔曼滤波的mems捷联惯导初始对准方法 | |
CN109211230A (zh) | 一种基于牛顿迭代法的炮弹姿态和加速度计常值误差估计方法 | |
CN109059914A (zh) | 一种基于gps和最小二乘滤波的炮弹滚转角估计方法 | |
Lambert et al. | Visual odometry aided by a sun sensor and inclinometer | |
CN111207773B (zh) | 一种用于仿生偏振光导航的姿态无约束优化求解方法 | |
CN110388942B (zh) | 一种基于角度和速度增量的车载姿态精对准*** | |
CN109211231A (zh) | 一种基于牛顿迭代法的炮弹姿态估计方法 | |
CN116222551A (zh) | 一种融合多种数据的水下导航方法及装置 | |
CN116105730A (zh) | 基于合作目标卫星甚短弧观测的仅测角光学组合导航方法 | |
CN112229421A (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 |