CN109443378A - 速度辅助行进间回溯初始对准方法 - Google Patents
速度辅助行进间回溯初始对准方法 Download PDFInfo
- Publication number
- CN109443378A CN109443378A CN201811107681.6A CN201811107681A CN109443378A CN 109443378 A CN109443378 A CN 109443378A CN 201811107681 A CN201811107681 A CN 201811107681A CN 109443378 A CN109443378 A CN 109443378A
- Authority
- CN
- China
- Prior art keywords
- indicate
- matrix
- direction cosine
- relative
- cosine matrix
- 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
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Manufacturing & Machinery (AREA)
- Automation & Control Theory (AREA)
- Navigation (AREA)
Abstract
本发明涉及一种速度辅助行进间回溯初始对准方法,解决了捷联惯导***行进间快速初始对准的问题。本发明的主要步骤为:步骤一:获取传感器实时数据;步骤二:进行基于速度辅助的矢量观测行进间粗对准并存储中间变量;步骤三:利用存储的中间变量,构建基于初始导航系的行进间正向‑正向回溯初始对准;步骤四:将行进间正向‑正向回溯初始对准估计的初始姿态反馈,估计当前姿态角;步骤五:行进间正向‑正向回溯过程次数为M,若k=M,则输出估计的姿态结果,完成初始对准过程,若k<M,表示回溯过程未完成,则重复上述步骤一至步骤五,直至行进间初始对准过程结束。
Description
技术领域
本发明涉及联惯性导航***行进间初始对准领域,特别是涉及速度辅助行 进间回溯初始对准方法。
背景技术
捷联惯性导航***行进间初始对准技术是捷联惯性导航定位的关键技术之 一,采用外部参考速度辅助惯性测量单元进行行进间初始对准具有测姿精度高、 可靠性好、自主性强等优点。当前,行进间初始对准可以分为粗对准和精对准 两个过程,其中粗对准主要实现粗略的姿态估计;精对准则是在粗对准的基础 上进行姿态精估计。众多学者都对捷联惯导***行进间初始对准技术进行了深 入的研究,但是研究均有将粗对准和精对准分别研究的缺点,精对准过程没有 充分利用粗对准过程的有效信息,导致对准时间较长,进而延长了捷联惯导准 备时间,不利于***应用。
传统技术存在以下技术问题:
发明内容
基于此,有必要针对上述技术问题,提供一种速度辅助行进间回溯初始对 准方法,发明采用基于速度辅助的矢量观测器行进间粗对准方法,并保存对准 过程中的有效数据,采用基于初始导航系的***误差模型实现行进间正向-正向 初始对准过程,减小了初始对准时间,实现了快速对准。
一种速度辅助行进间回溯初始对准方法,包括:
获取传感器实时数据,所述传感器实时数据包括陀螺仪数据、加速度计数 据以及多普勒测速仪数据;
进行基于速度辅助的矢量观测行进间粗对准并存储中间变量;
利用存储的中间变量,构建基于初始导航系的行进间正向-正向回溯初始 对准;
将行进间正向-正向回溯初始对准估计的初始姿态反馈,估计当前姿态角;
行进间正向-正向回溯过程次数为M,若k=M,则输出估计的姿态结果,完 成初始对准过程,若k<M,表示回溯过程未完成,则重复上述步骤,直至行进 间初始对准过程结束。
上述速度辅助行进间回溯初始对准方法,采用基于初始导航系的***误差 模型,具有计算简便的优点;估计姿态误差为初始时刻姿态,为实现行进间正 向-正向回溯过程提供了理论支撑;本发明采用行进间正向-正向回溯计算,充分 利用了计算机的计算效能,极大的加快了初始对准的时间。
在另外的一个实施例中,“进行基于速度辅助的矢量观测行进间粗对准并 存储中间变量;”具体包括:
定义解算所需的参考坐标系如下:
b—载体坐标系,表示捷联惯性导航***三轴正交坐标系,其x轴、y轴和z 轴分别指向载体的右-前-上;
n—导航坐标系,表示载体所在位置的地理坐标系,其三轴分别指向当地 东向、北向和天向;
e—地球坐标系,表示原点在地心,x轴为地心指向本初子午线与赤道交点, z轴为地心指向北极点,y轴与x轴和z轴构成右手坐标系;
i—惯性坐标系,表示惯性空间非旋转坐标系;
b0—初始载体坐标系,表示惯导***开机运行时刻的载体坐标系,并在整 个对准过程中相对于惯性空间保持静止;
n0—初始导航坐标系,表示惯导***开机运行时刻的导航坐标系,并在整 个对准过程中相对于惯性空间保持静止;
e0—初始地球坐标系,表示惯导***开机运行时刻的地球坐标系,并在整 个对准过程中相对于惯性空间保持静止;
由坐标变换可知:
式中:vn表示导航系速度;vb表示载体系速度;表示b系到n系转换的方 向余弦矩阵;
对等式两边进行微分,可得:
式中:表示n系速度微分;表示b系到n系转换的方向余弦矩阵微分;vb表示载体系速度;表示b系到n系转换的方向余弦矩阵;表示b系速度微分;
其中,
式中:表示n系速度微分;表示b系到n系转换的方向余弦矩阵;fb表 示比力;表示e系相对于i系的转动角速度在n系的投影;表示n系相对于e 系的转动角速度在n系的投影;vn表示n系速度;gn表示重力加速度在n系的投 影;
由上面推导可知:
式中:表示b系到n系转换的方向余弦矩阵微分;vb表示载体系速度;表示b系到n系转换的方向余弦矩阵;表示b系速度微分;表示b系相对 于n系的转动角速度在b系的投影所对应的叉乘矩阵;表示n0系相对于n系 的方向余弦矩阵;表示b0系相对于n0系的方向余弦矩阵;表示b系相对 于b0系的方向余弦矩阵;fb表示比力;表示e系相对于i系的转动角速度在n系 的投影;表示n系相对于e系的转动角速度在n系的投影;vn表示n系速度;gn表示重力加速度在n系的投影;
对等式两边同时乘以可得:
式中:表示b0系相对于n0系的方向余弦矩阵;表示b系相对于b0系 的方向余弦矩阵;表示b系相对于n系的转动角速度在b系的投影所对应 的叉乘矩阵;vb表示载体系速度;表示b系速度微分;fb表示比力;表示 n0系相对于n系的方向余弦矩阵;表示e系相对于i系的转动角速度在n系的投 影;表示n系相对于e系的转动角速度在n系的投影;vn表示n系速度;gn表 示重力加速度在n系的投影;
对等式两边积分并进行运算可得:
式中:表示b0系相对于n0系的方向余弦矩阵;表示b系相对于b0系 的方向余弦矩阵;vb表示载体系速度;fb表示比力;表示n0系相对于n系的 方向余弦矩阵;表示e系相对于i系的转动角速度在n系的投影;vn表示n系速 度;gn表示重力加速度在n系的投影;
因此可得矢量观测模型:
式中:β表示观测矢量;表示b0系相对于n0系的方向余弦矩阵;α表示 参考矢量;
观测矢量和参考矢量可以表示为:
式中:表示b系相对于b0系的方向余弦矩阵;vb表示载体系速度;fb表 示比力;表示n0系相对于n系的方向余弦矩阵;表示e系相对于i系的转 动角速度在n系的投影;vn表示n系速度;gn表示重力加速度在n系的投影;
由最优基姿态确定算法可知:
式中:K表示构造矩阵;β表示观测矢量;α表示参考矢量;和[α⊙]分 别表示为:
式中:β表示观测矢量;α表示参考矢量;[·×]表示矢量转换成斜对称矩阵;
通过提取K矩阵最小特征值对应的特征矢量可以计算方向余弦从而实 现速度辅助行进间粗对准;
存储中间变量为:量测比力在初始载体系上的映射载体系相对于初 始载体系变化的方向余弦矩阵
在另外的一个实施例中,陀螺仪量测常值漂移误差为 εb=[0.02 0.02 0.02]To/h,陀螺仪量测随机游走误差为 输出频率为200Hz;加速度计量测常值漂移误差 为陀螺仪量测随机游走误差为输出频率为200Hz。
在另外的一个实施例中,“利用存储的中间变量,构建基于初始导航系的行 进间正向-正向回溯初始对准;”具体包括:
(1)姿态误差方程
由粗对准可知:
式中,表示粗对准结束时刻方向余弦矩阵;表示初始时刻方向余弦 矩阵;表示b系相对于b0系变化的方向余弦矩阵;表示n0系与n系之间的 方向余弦矩阵;
由方向余弦误差方程可知:
式中,表示初始时刻方向余弦矩阵;表示初始时刻方向余弦矩阵真 值;表示初始姿态误差在n0系上的映射;I3表示3维单位矩阵;[·×]表示矢 量转换成斜对称矩阵;表示b系相对于b0系变化的方向余弦矩阵;表示载 体系相对于b0系变化的方向余弦矩阵真值;表示陀螺量测造成的姿态更新 误差;
由上面推导可知:
式中,表示粗对准结束时刻方向余弦矩阵;表示初始时刻方向余弦 矩阵真值;表示b系相对于b0系变化的方向余弦矩阵真值;表示初始姿 态误差在n0系上的映射;表示陀螺量测造成的姿态更新误差;I3表示3维 单位矩阵;[·×]表示矢量转换成斜对称矩阵;表示n0系与n系之间的方向余 弦矩阵;
简化计算得到:
式中,表示粗对准结束时刻方向余弦矩阵;表示初始时刻方向余弦 矩阵真值;表示b系相对于初始载体系变化的方向余弦矩阵真值;表示 初始姿态误差在n0系上的映射;表示陀螺量测造成的姿态更新误差;I3表 示3维单位矩阵;[·×]表示矢量转换成斜对称矩阵;表示n系与n0系之间的 方向余弦矩阵;
由姿态误差方程可知:
式中,表示初始姿态误差的微分在n0系上的映射;表示陀螺量测造 成的姿态更新误差的微分;εb表示陀螺仪量测常值漂移误差;表示陀螺仪量 测随机游走误差;表示初始时刻方向余弦矩阵;表示b系相对于b0系变化 的方向余弦矩阵;
有上面计算可知:
式中,表示姿态误差的微分在n0系上的映射;εb表示陀螺仪量测常值 漂移误差;表示陀螺仪量测随机游走误差;表示初始时刻方向余弦矩阵; 表示b系相对于b0系变化的方向余弦矩阵;
(2)速度误差方程
由惯性导航比力方程可知:
式中,表示n0系速度微分;表示b系与n0系之间的方向余弦矩阵;fb表示比力真值;表示地球自转角速度在n0系的映射;表示n0系相对于e 系的转动角速度在n0系上的映射;vn0表示载体运动速度在n0系的投影;gn0表 示重力矢量在n0系上的映射;
根据角速度映射关系:
式中,表示n0系相对于i系的转动角速度在n0系上的映射;表示e系 相对于i系的转动角速度在n0系上的映射;表示n0系相对于e系的转动角速 度在n0系上的映射;
因此速度误差微分方程可以表示为:
式中,表示n0系速度微分在初始导航系的映射;表示b0系与n0系之 间的方向余弦矩阵;表示b系相对于b0系变化的方向余弦矩阵;表示加速 度计测量值;φn0表示姿态误差;表示地球自转角速度在n0系的映射;δvn0表示速度误差在n0系的投影;表示加速度零偏;表示加速度随机游走噪 声;
(3)量测方程
当测量速度为DVL速度时,此时量测方程可以表示为:
式中,表示量测量;表示b0系与n0系之间的方向余弦矩阵;表示 b系相对于b0系变化的方向余弦矩阵;表示DVL测量速度;φn0表示安装误差 角;δvn0表示速度误差;δvb表示测量速度随机噪声;
(4)***模型
由速度误差方程和姿态误差方程可得***模型为:
式中,表示***状态;表示***过程噪声;z表示量测速度误差;表示量测噪声;***矩阵F和量测矩阵H分别表示为:
***模型离散化之后表示为:
式中,k表示离散化时刻;Φk表示离散化***矩阵;xk+1和xk表示k+1时刻 和k时刻***状态;zk表示k时刻量测;Hk表示量测矩阵;Wk表示k时刻过程噪 声;Vk表示k时刻量测噪声;
(4)Kalman滤波
利用离散化之后的***模型得到Kalman滤波如下:
式中,表示k-1时刻的最优状态估计;表示k时刻状态一步预 测;Φk表示离散化***矩阵;Pk-1|k-1表示k-1时刻的状态误差协方差;Qk-1表 示k-1时刻的过程噪声协方差;Pk|k-1表示k时刻一步预测状态误差协方差;Hk表示量测矩阵;Rk表示k时刻量测噪声协方差矩阵;Kk表示k时刻滤波增益矩 阵;zk表示k时刻量测;表示k时刻的最优状态估计;Pk|k表示k时刻的状态 误差协方差。
在另外的一个实施例中,Kalman滤波参数初值为 P0|0=diag([0.1 0.11 0.1 0.1 0.1 0.02e3 500e3])2e3=[1 1 1]。
在另外的一个实施例中,M=4。
一种计算机设备,包括存储器、处理器及存储在存储器上并可在处理器上 运行的计算机程序,所述处理器执行所述程序时实现任一项所述方法的步骤。
一种计算机可读存储介质,其上存储有计算机程序,该程序被处理器执行 时实现任一项所述方法的步骤。
一种处理器,所述处理器用于运行程序,其中,所述程序运行时执行任一 项所述的方法。
附图说明
图1为本申请实施例提供的一种速度辅助行进间回溯初始对准方法的流程 图。
图2为本申请实施例提供的一种速度辅助行进间回溯初始对准方法中的回 溯对准纵摇角误差图。
图3为本申请实施例提供的一种速度辅助行进间回溯初始对准方法中的回 溯对准横摇角误差图。
图4为本申请实施例提供的一种速度辅助行进间回溯初始对准方法中的回 溯对准航向角误差图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白,以下结合附图及实 施例,对本发明进行进一步详细说明。应当理解,此处所描述的具体实施例仅 仅用以解释本发明,并不用于限定本发明。
本实施例将本发明提出的一种速度辅助行进间回溯初始对准方法通过 Matlab仿真软件进行仿真验证,从而证明行进间正向-正向回溯过程的快速性。 仿真硬件环境均为Intel(R)Core(TM)T9600 CPU 2.80GHz,4G RAM,Windows 7操作***。如图2和图3所示,为行进间正向-正向回溯对准纵摇角误差图和 横摇角误差图,一共进行四次回溯对准,对准过程中单次运行时间为1s,总对 准时间为4s(不包括行进间粗对准时间)。图4为行进间正向-正向回溯对准航 向角误差图,从图中可以看出,经过两次回溯之后,航向角对准误差小于0.1°, 达到了极限对准精度。综合行进间粗对准采集数据过程,仿真中为300s,整个 对准过程持续时间为304s左右,相较于传统对准方法,在同样***指标的要求 下大大提升了初始对准过程的速度。
本发明是一种速度辅助行进间回溯初始对准方法,算法结构如图1所示, 包括以下几个步骤:
步骤1:获取传感器实时数据,所述传感器实时数据包括陀螺仪数据、加 速度计数据以及多普勒测速仪数据;
步骤2:进行基于速度辅助的矢量观测行进间粗对准并存储中间变量;
定义解算所需的参考坐标系如下:
b—载体坐标系,表示捷联惯性导航***三轴正交坐标系,其x轴、y轴和z 轴分别指向载体的右-前-上;
n—导航坐标系,表示载体所在位置的地理坐标系,其三轴分别指向当地 东向、北向和天向;
e—地球坐标系,表示原点在地心,x轴为地心指向本初子午线与赤道交点, z轴为地心指向北极点,y轴与x轴和z轴构成右手坐标系;
i—惯性坐标系,表示惯性空间非旋转坐标系;
b0—初始载体坐标系,表示惯导***开机运行时刻的载体坐标系,并在整 个对准过程中相对于惯性空间保持静止;
n0—初始导航坐标系,表示惯导***开机运行时刻的导航坐标系,并在整 个对准过程中相对于惯性空间保持静止;
e0—初始地球坐标系,表示惯导***开机运行时刻的地球坐标系,并在整 个对准过程中相对于惯性空间保持静止;
由坐标变换可知:
式中:vn表示导航系速度;vb表示载体系速度;表示b系到n系转换的方 向余弦矩阵;
对等式两边进行微分,可得:
式中:表示n系速度微分;表示b系到n系转换的方向余弦矩阵微分;vb表示载体系速度;表示b系到n系转换的方向余弦矩阵;表示b系速度微分;
其中,
式中:表示n系速度微分;表示b系到n系转换的方向余弦矩阵;fb表 示比力;表示e系相对于i系的转动角速度在n系的投影;表示n系相对于e 系的转动角速度在n系的投影;vn表示n系速度;gn表示重力加速度在n系的投 影;
由上面推导可知:
式中:表示b系到n系转换的方向余弦矩阵微分;vb表示载体系速度;表示b系到n系转换的方向余弦矩阵;表示b系速度微分;表示b系相对 于n系的转动角速度在b系的投影所对应的叉乘矩阵;表示n0系相对于n系 的方向余弦矩阵;表示b0系相对于n0系的方向余弦矩阵;表示b系相对 于b0系的方向余弦矩阵;fb表示比力;表示e系相对于i系的转动角速度在n系 的投影;表示n系相对于e系的转动角速度在n系的投影;vn表示n系速度;gn表示重力加速度在n系的投影;
对等式两边同时乘以可得:
式中:表示b0系相对于n0系的方向余弦矩阵;表示b系相对于b0系 的方向余弦矩阵;表示b系相对于n系的转动角速度在b系的投影所对应 的叉乘矩阵;vb表示载体系速度;表示b系速度微分;fb表示比力;表示 n0系相对于n系的方向余弦矩阵;表示e系相对于i系的转动角速度在n系的投 影;表示n系相对于e系的转动角速度在n系的投影;vn表示n系速度;gn表 示重力加速度在n系的投影;
对等式两边积分并进行运算可得:
式中:表示b0系相对于n0系的方向余弦矩阵;表示b系相对于b0系 的方向余弦矩阵;vb表示载体系速度;fb表示比力;表示n0系相对于n系的 方向余弦矩阵;表示e系相对于i系的转动角速度在n系的投影;vn表示n系速 度;gn表示重力加速度在n系的投影;
因此可得矢量观测模型:
式中:β表示观测矢量;表示b0系相对于n0系的方向余弦矩阵;α表示 参考矢量;
观测矢量和参考矢量可以表示为:
式中:表示b系相对于b0系的方向余弦矩阵;vb表示载体系速度;fb表 示比力;表示n0系相对于n系的方向余弦矩阵;表示e系相对于i系的转 动角速度在n系的投影;vn表示n系速度;gn表示重力加速度在n系的投影;
由最优基姿态确定算法可知:
式中:K表示构造矩阵;β表示观测矢量;α表示参考矢量;和[α⊙]分 别表示为:
式中:β表示观测矢量;α表示参考矢量;[·×]表示矢量转换成斜对称矩阵;
通过提取K矩阵最小特征值对应的特征矢量可以计算方向余弦从而实 现速度辅助行进间粗对准;
存储中间变量为:量测比力在初始载体系上的映射载体系相对于初 始载体系变化的方向余弦矩阵
步骤3:利用存储的中间变量,构建基于初始导航系的行进间正向-正向 回溯初始对准;
(1)姿态误差方程
由粗对准可知:
式中,表示粗对准结束时刻方向余弦矩阵;表示初始时刻方向余弦 矩阵;表示b系相对于b0系变化的方向余弦矩阵;表示n0系与n系之间的 方向余弦矩阵;
由方向余弦误差方程可知:
式中,表示初始时刻方向余弦矩阵;表示初始时刻方向余弦矩阵真 值;表示初始姿态误差在n0系上的映射;I3表示3维单位矩阵;[·×]表示矢 量转换成斜对称矩阵;表示b系相对于b0系变化的方向余弦矩阵;表示载 体系相对于b0系变化的方向余弦矩阵真值;表示陀螺量测造成的姿态更新 误差;
由上面推导可知:
式中,表示粗对准结束时刻方向余弦矩阵;表示初始时刻方向余弦 矩阵真值;表示b系相对于b0系变化的方向余弦矩阵真值;表示初始姿 态误差在n0系上的映射;表示陀螺量测造成的姿态更新误差;I3表示3维 单位矩阵;[·×]表示矢量转换成斜对称矩阵;表示n0系与n系之间的方向余 弦矩阵;
简化计算得到:
式中,表示粗对准结束时刻方向余弦矩阵;表示初始时刻方向余弦 矩阵真值;表示b系相对于初始载体系变化的方向余弦矩阵真值;表示 初始姿态误差在n0系上的映射;表示陀螺量测造成的姿态更新误差;I3表 示3维单位矩阵;[·×]表示矢量转换成斜对称矩阵;表示n系与n0系之间的 方向余弦矩阵;
由姿态误差方程可知:
式中,表示初始姿态误差的微分在n0系上的映射;表示陀螺量测造 成的姿态更新误差的微分;εb表示陀螺仪量测常值漂移误差;表示陀螺仪量 测随机游走误差;表示初始时刻方向余弦矩阵;表示b系相对于b0系变化 的方向余弦矩阵;
有上面计算可知:
式中,表示姿态误差的微分在n0系上的映射;εb表示陀螺仪量测常值 漂移误差;表示陀螺仪量测随机游走误差;表示初始时刻方向余弦矩阵; 表示b系相对于b0系变化的方向余弦矩阵;
(2)速度误差方程
由惯性导航比力方程可知:
式中,表示n0系速度微分;表示b系与n0系之间的方向余弦矩阵;fb表示比力真值;表示地球自转角速度在n0系的映射;表示n0系相对于e 系的转动角速度在n0系上的映射;vn0表示载体运动速度在n0系的投影;gn0表 示重力矢量在n0系上的映射;
根据角速度映射关系:
式中,表示n0系相对于i系的转动角速度在n0系上的映射;表示e系 相对于i系的转动角速度在n0系上的映射;表示n0系相对于e系的转动角速 度在n0系上的映射;
因此速度误差微分方程可以表示为:
式中,表示n0系速度微分在初始导航系的映射;表示b0系与n0系之 间的方向余弦矩阵;表示b系相对于b0系变化的方向余弦矩阵;表示加速 度计测量值;φn0表示姿态误差;表示地球自转角速度在n0系的映射;δvn0表示速度误差在n0系的投影;表示加速度零偏;表示加速度随机游走噪 声;
(3)量测方程
当测量速度为DVL速度时,此时量测方程可以表示为:
式中,表示量测量;表示b0系与n0系之间的方向余弦矩阵;表示 b系相对于b0系变化的方向余弦矩阵;表示DVL测量速度;φn0表示安装误差 角;δvn0表示速度误差;δvb表示测量速度随机噪声;
(4)***模型
由速度误差方程和姿态误差方程可得***模型为:
式中,表示***状态;表示***过程噪声;z表示量测速度误差;表示量测噪声;***矩阵F和量测矩阵H分别表示为:
***模型离散化之后表示为:
式中,k表示离散化时刻;φk表示离散化***矩阵;xk+1和xk表示k+1时刻 和k时刻***状态;zk表示k时刻量测;Hk表示量测矩阵;Wk表示k时刻过程噪 声;Vk表示k时刻量测噪声;
(4)Kalman滤波
利用离散化之后的***模型得到Kalman滤波如下:
Pk|k=Pk|k-1-KkHkPk|k-1
式中,表示k-1时刻的最优状态估计;表示k时刻状态一步预 测;φk表示离散化***矩阵;Pk-1|k-1表示k-1时刻的状态误差协方差;Qk-1表 示k-1时刻的过程噪声协方差;Pk|k-1表示k时刻一步预测状态误差协方差;Hk表示量测矩阵;Rk表示k时刻量测噪声协方差矩阵;Kk表示k时刻滤波增益矩 阵;zk表示k时刻量测;表示k时刻的最优状态估计;Pk|k表示k时刻的状态 误差协方差。
步骤4:将行进间正向-正向回溯初始对准估计的初始姿态反馈,估计当前 姿态角。
步骤5:行进间正向-正向回溯过程次数为M,若k=M,则输出估计的姿态 结果,完成初始对准过程,若k<M,表示回溯过程未完成,则重复上述步骤, 直至行进间初始对准过程结束。
一种计算机设备,包括存储器、处理器及存储在存储器上并可在处理器上 运行的计算机程序,所述处理器执行所述程序时实现任一项所述方法的步骤。
一种计算机可读存储介质,其上存储有计算机程序,该程序被处理器执行 时实现任一项所述方法的步骤。
一种处理器,所述处理器用于运行程序,其中,所述程序运行时执行任一 项所述的方法。
对本发明的有益效果说明如下:
MATLAB仿真实验,在以下的仿真条件下,对该方法进行仿真实验:
陀螺仪量测常值漂移误差为εb=[0.02 0.02 0.02]To/h,陀螺仪量测随机游 走误差为输出频率为200Hz;加速度计量测常值 漂移误差为陀螺仪量测随机游走误差为 输出频率为200Hz。Kalman滤波参数初值为 P0|0=diag([0.1 0.1 1 0.1 0.1 0.10.02e3 500e3])2,e3=[1 1 1]。步骤5中, M=4。
以上所述实施例的各技术特征可以进行任意的组合,为使描述简洁,未对 上述实施例中的各个技术特征所有可能的组合都进行描述,然而,只要这些技 术特征的组合不存在矛盾,都应当认为是本说明书记载的范围。
以上所述实施例仅表达了本发明的几种实施方式,其描述较为具体和详细, 但并不能因此而理解为对发明专利范围的限制。应当指出的是,对于本领域的 普通技术人员来说,在不脱离本发明构思的前提下,还可以做出若干变形和改 进,这些都属于本发明的保护范围。因此,本发明专利的保护范围应以所附权 利要求为准。
Claims (9)
1.一种速度辅助行进间回溯初始对准方法,其特征在于,包括:
获取所述传感器实时数据,所述传感器实时数据包括陀螺仪数据、加速度计数据以及多普勒测速仪数据;
进行基于速度辅助的矢量观测行进间粗对准并存储中间变量;
利用存储的中间变量,构建基于初始导航系的行进间正向-正向回溯初始对准;
将行进间正向-正向回溯初始对准估计的初始姿态反馈,估计当前姿态角;
行进间正向-正向回溯过程次数为M,若k=M,则输出估计的姿态结果,完成初始对准过程,若k<M,表示回溯过程未完成,则重复上述步骤,直至行进间初始对准过程结束。
2.根据权利要求1所述的速度辅助行进间回溯初始对准方法,其特征在于,“进行基于速度辅助的矢量观测行进间粗对准并存储中间变量;”具体包括:
定义解算所需的参考坐标系如下:
b—载体坐标系,表示捷联惯性导航***三轴正交坐标系,其x轴、y轴和z轴分别指向载体的右-前-上;
n—导航坐标系,表示载体所在位置的地理坐标系,其三轴分别指向当地东向、北向和天向;
e—地球坐标系,表示原点在地心,x轴为地心指向本初子午线与赤道交点,z轴为地心指向北极点,y轴与x轴和z轴构成右手坐标系;
i—惯性坐标系,表示惯性空间非旋转坐标系;
b0—初始载体坐标系,表示惯导***开机运行时刻的载体坐标系,并在整个对准过程中相对于惯性空间保持静止;
n0—初始导航坐标系,表示惯导***开机运行时刻的导航坐标系,并在整个对准过程中相对于惯性空间保持静止;
e0—初始地球坐标系,表示惯导***开机运行时刻的地球坐标系,并在整个对准过程中相对于惯性空间保持静止;
由坐标变换可知:
式中:vn表示导航系速度;vb表示载体系速度;表示b系到n系转换的方向余弦矩阵;
对等式两边进行微分,可得:
式中:表示n系速度微分;表示b系到n系转换的方向余弦矩阵微分;vb表示载体系速度;表示b系到n系转换的方向余弦矩阵;表示b系速度微分;
其中,
式中:表示n系速度微分;表示b系到n系转换的方向余弦矩阵;fb表示比力;表示e系相对于i系的转动角速度在n系的投影;表示n系相对于e系的转动角速度在n系的投影;vn表示n系速度;gn表示重力加速度在n系的投影;
由上面推导可知:
式中:表示b系到n系转换的方向余弦矩阵微分;vb表示载体系速度;表示b系到n系转换的方向余弦矩阵;表示b系速度微分;表示b系相对于n系的转动角速度在b系的投影所对应的叉乘矩阵;表示n0系相对于n系的方向余弦矩阵;表示b0系相对于n0系的方向余弦矩阵;表示b系相对于b0系的方向余弦矩阵;fb表示比力;表示e系相对于i系的转动角速度在n系的投影;表示n系相对于e系的转动角速度在n系的投影;vn表示n系速度;gn表示重力加速度在n系的投影;
对等式两边同时乘以可得:
式中:表示b0系相对于n0系的方向余弦矩阵;表示b系相对于b0系的方向余弦矩阵;表示b系相对于n系的转动角速度在b系的投影所对应的叉乘矩阵;vb表示载体系速度;表示b系速度微分;fb表示比力;表示n0系相对于n系的方向余弦矩阵;表示e系相对于i系的转动角速度在n系的投影;表示n系相对于e系的转动角速度在n系的投影;vn表示n系速度;gn表示重力加速度在n系的投影;
对等式两边积分并进行运算可得:
式中:表示b0系相对于n0系的方向余弦矩阵;表示b系相对于b0系的方向余弦矩阵;vb表示载体系速度;fb表示比力;表示n0系相对于n系的方向余弦矩阵;表示e系相对于i系的转动角速度在n系的投影;vn表示n系速度;gn表示重力加速度在n系的投影;
因此可得矢量观测模型:
式中:β表示观测矢量;表示b0系相对于n0系的方向余弦矩阵;α表示参考矢量;
观测矢量和参考矢量可以表示为:
式中:表示b系相对于b0系的方向余弦矩阵;vb表示载体系速度;fb表示比力;表示n0系相对于n系的方向余弦矩阵;表示e系相对于i系的转动角速度在n系的投影;vn表示n系速度;gn表示重力加速度在n系的投影;
由最优基姿态确定算法可知:
式中:K表示构造矩阵;β表示观测矢量;α表示参考矢量;和[α⊙]分别表示为:
式中:β表示观测矢量;α表示参考矢量;[·×]表示矢量转换成斜对称矩阵;
通过提取K矩阵最小特征值对应的特征矢量可以计算方向余弦从而实现速度辅助行进间粗对准;
存储中间变量为:量测比力在初始载体系上的映射载体系相对于初始载体系变化的方向余弦矩阵
3.根据权利要求2所述的速度辅助行进间回溯初始对准方法,其特征在于,陀螺仪量测常值漂移误差为εb=[0.02 0.02 0.02]To/h,陀螺仪量测随机游走误差为输出频率为200Hz;加速度计量测常值漂移误差为陀螺仪量测随机游走误差为输出频率为200Hz。
4.根据权利要求1所述的速度辅助行进间回溯初始对准方法,其特征在于,“利用存储的中间变量,构建基于初始导航系的行进间正向-正向回溯初始对准;”具体包括:
(1)姿态误差方程
由粗对准可知:
式中,表示粗对准结束时刻方向余弦矩阵;表示初始时刻方向余弦矩阵;表示b系相对于b0系变化的方向余弦矩阵;表示n0系与n系之间的方向余弦矩阵;
由方向余弦误差方程可知:
式中,表示初始时刻方向余弦矩阵;表示初始时刻方向余弦矩阵真值;表示初始姿态误差在n0系上的映射;I3表示3维单位矩阵;[·×]表示矢量转换成斜对称矩阵;表示b系相对于b0系变化的方向余弦矩阵;表示载体系相对于b0系变化的方向余弦矩阵真值;表示陀螺量测造成的姿态更新误差;
由上面推导可知:
式中,表示粗对准结束时刻方向余弦矩阵;表示初始时刻方向余弦矩阵真值;表示b系相对于b0系变化的方向余弦矩阵真值;表示初始姿态误差在n0系上的映射;表示陀螺量测造成的姿态更新误差;I3表示3维单位矩阵;[·×]表示矢量转换成斜对称矩阵;表示n0系与n系之间的方向余弦矩阵;
简化计算得到:
式中,表示粗对准结束时刻方向余弦矩阵;表示初始时刻方向余弦矩阵真值;表示b系相对于初始载体系变化的方向余弦矩阵真值;表示初始姿态误差在n0系上的映射;表示陀螺量测造成的姿态更新误差;I3表示3维单位矩阵;[·×]表示矢量转换成斜对称矩阵;表示n系与n0系之间的方向余弦矩阵;
由姿态误差方程可知:
式中,表示初始姿态误差的微分在n0系上的映射;表示陀螺量测造成的姿态更新误差的微分;εb表示陀螺仪量测常值漂移误差;表示陀螺仪量测随机游走误差;表示初始时刻方向余弦矩阵;表示b系相对于b0系变化的方向余弦矩阵;
有上面计算可知:
式中,表示姿态误差的微分在n0系上的映射;εb表示陀螺仪量测常值漂移误差;表示陀螺仪量测随机游走误差;表示初始时刻方向余弦矩阵;表示b系相对于b0系变化的方向余弦矩阵;
(2)速度误差方程
由惯性导航比力方程可知:
式中,表示n0系速度微分;表示b系与n0系之间的方向余弦矩阵;fb表示比力真值;表示地球自转角速度在n0系的映射;表示n0系相对于e系的转动角速度在n0系上的映射;vn0表示载体运动速度在n0系的投影;gn0表示重力矢量在n0系上的映射;
根据角速度映射关系:
式中,表示n0系相对于i系的转动角速度在n0系上的映射;表示e系相对于i系的转动角速度在n0系上的映射;表示n0系相对于e系的转动角速度在n0系上的映射;
因此速度误差微分方程可以表示为:
式中,表示n0系速度微分在初始导航系的映射;表示b0系与n0系之间的方向余弦矩阵;表示b系相对于b0系变化的方向余弦矩阵;表示加速度计测量值;φn0表示姿态误差;表示地球自转角速度在n0系的映射;δvn0表示速度误差在n0系的投影;表示加速度零偏;表示加速度随机游走噪声;
(3)量测方程
当测量速度为DVL速度时,此时量测方程可以表示为:
式中,表示量测量;表示b0系与n0系之间的方向余弦矩阵;表示b系相对于b0系变化的方向余弦矩阵;表示DVL测量速度;φn0表示安装误差角;δvn0表示速度误差;δvb表示测量速度随机噪声;
(4)***模型
由速度误差方程和姿态误差方程可得***模型为:
式中,表示***状态;表示***过程噪声;z表示量测速度误差;表示量测噪声;***矩阵F和量测矩阵H分别表示为:
***模型离散化之后表示为:
式中,k表示离散化时刻;Φk表示离散化***矩阵;xk+1和xk表示k+1时刻和k时刻***状态;zk表示k时刻量测;Hk表示量测矩阵;Wk表示k时刻过程噪声;Vk表示k时刻量测噪声;
(4)Kalman滤波
利用离散化之后的***模型得到Kalman滤波如下:
pk|k=Pk|k-1-KkHkPk|k-1
式中,表示k-1时刻的最优状态估计;表示k时刻状态一步预测;Φk表示离散化***矩阵;Pk-1|k-1表示k-1时刻的状态误差协方差;Qk-1表示k-1时刻的过程噪声协方差;Pk|k-1表示k时刻一步预测状态误差协方差;Hk表示量测矩阵;Rk表示k时刻量测噪声协方差矩阵;Kk表示k时刻滤波增益矩阵;zk表示k时刻量测;表示k时刻的最优状态估计;Pk|k表示k时刻的状态误差协方差。
5.根据权利要求4所述的速度辅助行进间回溯初始对准方法,其特征在于,Kalman滤波参数初值为P0|0=diag([0.1 0.1 1 0.1 0.1 0.1 0.02e3 500e3])2,e3=[1 11]。
6.根据权利要求1所述的速度辅助行进间回溯初始对准方法,其特征在于,M=4。
7.一种计算机设备,包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序,其特征在于,所述处理器执行所述程序时实现权利要求1到6任一项所述方法的步骤。
8.一种计算机可读存储介质,其上存储有计算机程序,其特征在于,该程序被处理器执行时实现权利要求1到6任一项所述方法的步骤。
9.一种处理器,其特征在于,所述处理器用于运行程序,其中,所述程序运行时执行权利要求1到6任一项所述的方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201811107681.6A CN109443378B (zh) | 2018-09-21 | 2018-09-21 | 速度辅助行进间回溯初始对准方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201811107681.6A CN109443378B (zh) | 2018-09-21 | 2018-09-21 | 速度辅助行进间回溯初始对准方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN109443378A true CN109443378A (zh) | 2019-03-08 |
CN109443378B CN109443378B (zh) | 2020-05-29 |
Family
ID=65530907
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201811107681.6A Active CN109443378B (zh) | 2018-09-21 | 2018-09-21 | 速度辅助行进间回溯初始对准方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN109443378B (zh) |
Cited By (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111947685A (zh) * | 2020-07-30 | 2020-11-17 | 苏州大学 | 一种极区格网坐标系动基座粗对准方法 |
CN113670335A (zh) * | 2021-08-18 | 2021-11-19 | 河海大学 | 基于dvl辅助和向量截断化k矩阵的水下载体初始对准方法 |
CN113959462A (zh) * | 2021-10-21 | 2022-01-21 | 北京机电工程研究所 | 一种基于四元数的惯性导航***自对准方法 |
Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101514900A (zh) * | 2009-04-08 | 2009-08-26 | 哈尔滨工程大学 | 一种单轴旋转的捷联惯导***初始对准方法 |
CN106595711A (zh) * | 2016-12-21 | 2017-04-26 | 东南大学 | 一种基于递推四元数的捷联惯性导航***粗对准方法 |
CN107664511A (zh) * | 2017-09-06 | 2018-02-06 | 哈尔滨工程大学 | 基于速度信息的摇摆基座粗对准方法 |
-
2018
- 2018-09-21 CN CN201811107681.6A patent/CN109443378B/zh active Active
Patent Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101514900A (zh) * | 2009-04-08 | 2009-08-26 | 哈尔滨工程大学 | 一种单轴旋转的捷联惯导***初始对准方法 |
CN106595711A (zh) * | 2016-12-21 | 2017-04-26 | 东南大学 | 一种基于递推四元数的捷联惯性导航***粗对准方法 |
CN107664511A (zh) * | 2017-09-06 | 2018-02-06 | 哈尔滨工程大学 | 基于速度信息的摇摆基座粗对准方法 |
Non-Patent Citations (2)
Title |
---|
YUANXIN WU 等: "Velocity/Position Integration Formula (I):Application to In-flight Coarse Alignment", 《IEEE TRANS. AEROSP. ELECTRON.》 * |
成研 等: "单轴旋转捷联惯导***回溯式对准方法", 《航空精密制造技术》 * |
Cited By (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111947685A (zh) * | 2020-07-30 | 2020-11-17 | 苏州大学 | 一种极区格网坐标系动基座粗对准方法 |
CN111947685B (zh) * | 2020-07-30 | 2021-12-07 | 苏州大学 | 一种极区格网坐标系动基座粗对准方法 |
CN113670335A (zh) * | 2021-08-18 | 2021-11-19 | 河海大学 | 基于dvl辅助和向量截断化k矩阵的水下载体初始对准方法 |
CN113670335B (zh) * | 2021-08-18 | 2023-10-24 | 河海大学 | 基于dvl辅助和向量截断化k矩阵的水下载体初始对准方法 |
CN113959462A (zh) * | 2021-10-21 | 2022-01-21 | 北京机电工程研究所 | 一种基于四元数的惯性导航***自对准方法 |
CN113959462B (zh) * | 2021-10-21 | 2023-09-12 | 北京机电工程研究所 | 一种基于四元数的惯性导航***自对准方法 |
Also Published As
Publication number | Publication date |
---|---|
CN109443378B (zh) | 2020-05-29 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109163735B (zh) | 一种晃动基座正向-正向回溯初始对准方法 | |
Li et al. | A fast SINS initial alignment scheme for underwater vehicle applications | |
WO2020220729A1 (zh) | 基于角加速度计/陀螺/加速度计的惯性导航解算方法 | |
EP2740256B1 (en) | Moving direction determination with noisy signals from inertial navigation systems on mobile devices | |
CN104215259B (zh) | 一种基于地磁模量梯度和粒子滤波的惯导误差校正方法 | |
CN110398257A (zh) | Gps辅助的sins***快速动基座初始对准方法 | |
CN103776446B (zh) | 一种基于双mems-imu的行人自主导航解算算法 | |
Li et al. | Optimization-based INS in-motion alignment approach for underwater vehicles | |
CN103344260B (zh) | 基于rbckf的捷联惯导***大方位失准角初始对准方法 | |
CN109931955B (zh) | 基于状态相关李群滤波的捷联惯性导航***初始对准方法 | |
CN109141475A (zh) | 一种dvl辅助sins鲁棒行进间初始对准方法 | |
CN109596144A (zh) | Gnss位置辅助sins行进间初始对准方法 | |
CN103398713A (zh) | 一种星敏感器/光纤惯性设备量测数据同步方法 | |
CN101839719A (zh) | 一种基于陀螺、地磁传感器的惯性测量装置 | |
CN109443378A (zh) | 速度辅助行进间回溯初始对准方法 | |
CN104457446A (zh) | 一种自旋制导炮弹的空中自对准方法 | |
CN103644910A (zh) | 基于分段rts平滑算法的个人自主导航***定位方法 | |
CN103076026A (zh) | 一种捷联惯导***中确定多普勒计程仪测速误差的方法 | |
Liang et al. | A solution to the attitude problem using two rotation units of micromechanical gyroscopes | |
Sun et al. | Coarse alignment based on IMU rotational motion for surface ship | |
CN109764870A (zh) | 基于变换估计量建模方案的载体初始航向估算方法 | |
Yuan et al. | Indoor pedestrian navigation using miniaturized low-cost MEMS inertial measurement units | |
Yuan et al. | Improved SITAN algorithm in the application of aided inertial navigation | |
CN113108781B (zh) | 一种应用于无人船行进间的改进粗对准方法 | |
Bai et al. | Optimal Path Planning Method for IMU System-Level Calibration Based on Improved Dijkstra’s Algorithm |
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 |