CN110108301B - 模值检测动基座鲁棒对准方法 - Google Patents
模值检测动基座鲁棒对准方法 Download PDFInfo
- Publication number
- CN110108301B CN110108301B CN201910399911.9A CN201910399911A CN110108301B CN 110108301 B CN110108301 B CN 110108301B CN 201910399911 A CN201910399911 A CN 201910399911A CN 110108301 B CN110108301 B CN 110108301B
- Authority
- CN
- China
- Prior art keywords
- representing
- navigation system
- initial
- vector
- carrier system
- 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.)
- Active
Links
Images
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)
- Gyroscopes (AREA)
Abstract
本发明公开了一种模值检测动基座鲁棒对准方法。本发明一种模值检测动基座鲁棒对准方法,其特征在于,包括:获取惯性传感器实时数据并进行姿态更新;获取辅助传感器信息,构造矢量观测器;对矢量观测器进行模值计算,并利用标准模值进行异常检测;通过模值检测削弱异常噪声影响,构造自适应矢量姿态确定算法;初始对准过程运行时间为M,若姿态变化次数k=M,则输出对准结果,完成初始对准过程,若姿态变化次数k<M,示初始对准过程未完成,则重复上述步骤初始对准过程结束。本发明的有益效果:本发明采用模值检测方法,具有计算简便的优点;本发明设计优化权值与计算方法,具有高效的DVL测量异常检测效果。
Description
技术领域
本发明涉捷联惯性导航系统初始对准领域,属于一种模值检测动基座鲁棒对准方法,采用模值检测方法实现鲁棒对准过程,弱化了DVL测量异常噪声对对准结果的影响。
背景技术
捷联惯性导航系统是一种自主定位系统,而初始对准过程是确保捷联惯导系统能够正常工作的前提。当前,捷联惯性导航系统初始对准过程都是在静止基座或者良好的外部辅助信息下进行的,这限制了捷联惯导系统的应用范围。尤其对于水下航行器等需要在复杂环境下进行自主导航定位的系统,更需要捷联惯导系统能够对外部辅助信息有一定的抗干扰特性,这需要对外部辅助信息对初始对准过程的辅助特性进行研究,并对其影响特性进行深入分析,找到相关的噪声抑制方法,从而实现抗干扰初始对准,提高系统鲁棒性。
技术存在以下技术问题:
传统方法在外部辅助信息出现异常时无法准确进行初始对准的问题。
发明内容
本发明要解决的技术问题是提供一种模值检测动基座鲁棒对准方法,在对DVL辅助惯性系统初始对准矢量构造的基础上,通过模值检测方法计算出优化权值,并利用优化权值对姿态确定算法进行修正,实现鲁棒对准目的。
为了解决上述技术问题,本发明提供了一种模值检测动基座鲁棒对准方法,包括:
步骤1:获取惯性传感器实时数据并进行姿态更新;
步骤2:获取辅助传感器信息,构造矢量观测器;
步骤3:对矢量观测器进行模值计算,并利用标准模值进行异常检测;
步骤4:通过模值检测削弱异常噪声影响,构造自适应矢量姿态确定算法;初始对准过程运行时间为M,若姿态变化次数k=M,则输出对准结果,完成初始对准过程,若姿态变化次数k<M,示初始对准过程未完成,则重复上述步骤初始对准过程结束。
在其中一个实施例中,步骤1具体包括:由惯性测量单元测量模型可知:
由姿态更新方程可知:
式中,表示测量真实角速度;表示载体系到初始载体系的方向余弦矩阵;表示表示载体系到初始载体系的方向余弦矩阵的微分;表示导航系相对于惯性系的旋转角速度在导航系的投影;表示导航系相对于初始导航系的方向余弦矩阵;表示导航系相对于初始导航系的方向余弦矩阵的微分。
在其中一个实施例中,步骤2具体包括:由DVL测量模型可知:
由比力方程可知
式中,表示导航系速度的微分;表示载体系相对于导航系变化的方向余弦矩阵;fb表示测量真实加速度;表示地球系相对于惯性系的旋转角速度在导航系的投影;表示导航系相对于地球系的转动角速度在导航系的投影;×表示矢量叉乘运算;vn表示导航系速度;gn表示重力加速度在导航系下的投影;
由比力方程及方向余弦变化关系可知:
式中,表示载体系相对于导航系变化的方向余弦矩阵;表示载体系相对于导航系的旋转角速度在载体系的投影;vb表示载体系速度;表示载体系速度的微分;表示初始导航系相对于导航系的方向余弦矩阵;表示初始载体系到初始导航系之间的方向余弦矩阵;表示载体系到初始载体系之间的方向余弦矩阵;fb表示测量真实加速度;表示地球系相对于惯性系的旋转角速度在导航系的投影;表示导航系相对于地球系的转动角速度在导航系的投影;×表示矢量叉乘运算;vn表示导航系速度;gn表示重力加速度在导航系下的投影;
对上式进行整理可得
式中,表示载体系到初始载体系之间的方向余弦矩阵;表示地球系相对于惯性系的旋转角速度在载体系的投影;表示测量真实角速度;vb表示载体系速度;表示载体系速度微分;fb表示测量真实加速度;表示初始导航系到初始载体系的方向余弦矩阵;表示导航系到初始导航系的方向余弦矩阵;gn表示重力加速度在导航系下的投影;
对上式两边同时进行积分可得
式中,β表示观测矢量;α表示参考矢量;表示载体系到初始载体系之间的方向余弦矩阵;表示地球系相对于惯性系的旋转角速度在载体系的投影;表示测量真实角速度;vb表示载体系速度;表示载体系速度微分;fb表示测量真实加速度;表示初始导航系到初始载体系的方向余弦矩阵;表示导航系到初始导航系的方向余弦矩阵;gn表示重力加速度在导航系下的投影;
将DVL测量速度代入观测矢量可得
在其中一个实施例中,步骤3具体包括:
对观测矢量进行模值计算可得
有观测矢量和参考矢量之间的关系可知
因此,可以得到DVL测量速度构造的含噪声矢量噪声模值为
噪声模值求解期望和方差可以得到
在其中一个实施例中,步骤4具体包括:由上面噪声模值的方差可以定义权值系数为
利用OBA姿态确定算法实现自适应矢量姿态确定
在其中一个实施例中,步骤1中,陀螺仪量测常值漂移误差为陀螺仪量测随机游走误差为输出频率为200Hz;加速度计量测常值漂移误差为▽b=[500500500]Tμg,加速度计量测随机游走误差为输出频率为200Hz。
在其中一个实施例中,步骤2中,DVL采样周期为1s,量测标准差为0.1m/s。
一种计算机设备,包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序,所述处理器执行所述程序时实现任一项所述方法的步骤。
一种计算机可读存储介质,其上存储有计算机程序,该程序被处理器执行时实现任一项所述方法的步骤。
一种处理器,所述处理器用于运行程序,其中,所述程序运行时执行任一项所述的方法。
本发明的有益效果:
本发明采用模值检测方法,具有计算简便的优点;
本发明设计优化权值与计算方法,具有高效的DVL测量异常检测效果;
本发明将权值与姿态确定算法融合,实现了鲁棒姿态确定。
附图说明
图1是本发明模值检测动基座鲁棒对准方法的动基座对准流程图。
图2是本发明模值检测动基座鲁棒对准方法的动基座对准载体运行曲线图。
图3是本发明模值检测动基座鲁棒对准方法的鲁棒对准纵摇角误差图。
图4是本发明模值检测动基座鲁棒对准方法的鲁棒对准横摇角误差图。
图5是本发明模值检测动基座鲁棒对准方法的鲁棒对准航向角误差图。
具体实施方式
下面结合附图和具体实施例对本发明作进一步说明,以使本领域的技术人员可以更好地理解本发明并能予以实施,但所举实施例不作为对本发明的限定。
下面介绍本发明的一个具体应用场景:
本实施例将本发明提出的一种模值检测动基座鲁棒对准方法通过Matlab仿真软件进行仿真验证,从而证明对准过程的鲁棒化。仿真硬件环境均为Intel(R)Core(TM)T9600CPU 2.80GHz,4G RAM,Windows 7操作系统。如图2所示,为行进间对准过程载体运动曲线图。图3、图4、图5为DVL辅助SINS行进间初始对准误差图,从图中可以看出,采用模值检测技术之后,对准结果有效的抑制了量测异常噪声的干扰,航向角对准误差可以在150s左右达到1°的对准精度,而传统方法则受到外部异常噪声的干扰造成对准不稳定现象。
本发明是一种模值检测动基座鲁棒对准方法,算法流程如图1所示,包括以下几个步骤:
步骤1:获取惯性传感器实时数据并进行姿态更新;
由惯性测量单元测量模型可知:
由姿态更新方程可知:
式中,表示测量真实角速度;表示载体系到初始载体系的方向余弦矩阵;表示表示载体系到初始载体系的方向余弦矩阵的微分;表示导航系相对于惯性系的旋转角速度在导航系的投影;表示导航系相对于初始导航系的方向余弦矩阵;表示导航系相对于初始导航系的方向余弦矩阵的微分;
步骤2:获取辅助传感器信息,构造矢量观测器;
由DVL测量模型可知:
由比力方程可知
式中,表示导航系速度的微分;表示载体系相对于导航系变化的方向余弦矩阵;fb表示测量真实加速度;表示地球系相对于惯性系的旋转角速度在导航系的投影;表示导航系相对于地球系的转动角速度在导航系的投影;×表示矢量叉乘运算;vn表示导航系速度;gn表示重力加速度在导航系下的投影;
由比力方程及方向余弦变化关系可知:
式中,表示载体系相对于导航系变化的方向余弦矩阵;表示载体系相对于导航系的旋转角速度在载体系的投影;vb表示载体系速度;表示载体系速度的微分;表示初始导航系相对于导航系的方向余弦矩阵;表示初始载体系到初始导航系之间的方向余弦矩阵;表示载体系到初始载体系之间的方向余弦矩阵;fb表示测量真实加速度;表示地球系相对于惯性系的旋转角速度在导航系的投影;表示导航系相对于地球系的转动角速度在导航系的投影;×表示矢量叉乘运算;vn表示导航系速度;gn表示重力加速度在导航系下的投影;
对上式进行整理可得
式中,表示载体系到初始载体系之间的方向余弦矩阵;表示地球系相对于惯性系的旋转角速度在载体系的投影;表示测量真实角速度;vb表示载体系速度;表示载体系速度微分;fb表示测量真实加速度;表示初始导航系到初始载体系的方向余弦矩阵;表示导航系到初始导航系的方向余弦矩阵;gn表示重力加速度在导航系下的投影;
对上式两边同时进行积分可得
式中,β表示观测矢量;α表示参考矢量;表示载体系到初始载体系之间的方向余弦矩阵;表示地球系相对于惯性系的旋转角速度在载体系的投影;表示测量真实角速度;vb表示载体系速度;表示载体系速度微分;fb表示测量真实加速度;表示初始导航系到初始载体系的方向余弦矩阵;表示导航系到初始导航系的方向余弦矩阵;gn表示重力加速度在导航系下的投影;
将DVL测量速度代入观测矢量可得
步骤3:对矢量观测器进行模值计算,并利用标准模值进行异常检测;
对观测矢量进行模值计算可得
有观测矢量和参考矢量之间的关系可知
因此,可以得到DVL测量速度构造的含噪声矢量噪声模值为
噪声模值求解期望和方差可以得到
步骤4:通过模值检测削弱异常噪声影响,构造自适应矢量姿态确定算法;
由上面噪声模值的方差可以定义权值系数为
利用OBA姿态确定算法实现自适应矢量姿态确定
对本发明的有益效果说明如下:
MATLAB仿真实验,在以下的仿真条件下,对该方法进行仿真实验:
陀螺仪量测常值漂移误差为陀螺仪量测随机游走误差为输出频率为200Hz;加速度计量测常值漂移误差为▽b=[500500500]Tμg,加速度计量测随机游走误差为输出频率为200Hz。DVL采样周期为1s,量测标准差为0.1m/s。对准时间为M=300s。
以上所述实施例仅是为充分说明本发明而所举的较佳的实施例,本发明的保护范围不限于此。本技术领域的技术人员在本发明基础上所作的等同替代或变换,均在本发明的保护范围之内。本发明的保护范围以权利要求书为准。
Claims (6)
1.一种模值检测动基座鲁棒对准方法,其特征在于,包括:
步骤1:获取惯性传感器实时数据并进行姿态更新;
步骤2:获取辅助传感器信息,构造矢量观测器;
步骤3:对矢量观测器进行模值计算,并利用标准模值进行异常检测;
步骤4:通过模值检测削弱异常噪声影响,构造自适应矢量姿态确定算法;初始对准过程运行时间为M,若姿态变化次数k=M,则输出对准结果,完成初始对准过程,若姿态变化次数k<M,表示初始对准过程未完成,则重复上述步骤直到初始对准过程结束;
步骤1具体包括:由惯性测量单元测量模型可知:
由姿态更新方程可知:
式中,表示测量真实角速度;表示载体系到初始载体系的方向余弦矩阵;表示表示载体系到初始载体系的方向余弦矩阵的微分;表示导航系相对于惯性系的旋转角速度在导航系的投影;表示导航系相对于初始导航系的方向余弦矩阵;表示导航系相对于初始导航系的方向余弦矩阵的微分;
步骤2具体包括:由DVL测量模型可知:
由比力方程可知
式中,表示导航系速度的微分;表示载体系相对于导航系变化的方向余弦矩阵;fb表示测量真实加速度;表示地球系相对于惯性系的旋转角速度在导航系的投影;表示导航系相对于地球系的转动角速度在导航系的投影;×表示矢量叉乘运算;vn表示导航系速度;gn表示重力加速度在导航系下的投影;
由比力方程及方向余弦变化关系可知:
式中,表示载体系相对于导航系变化的方向余弦矩阵;表示载体系相对于导航系的旋转角速度在载体系的投影;vb表示载体系速度;表示载体系速度的微分;表示初始导航系相对于导航系的方向余弦矩阵;表示初始载体系到初始导航系之间的方向余弦矩阵;表示载体系到初始载体系之间的方向余弦矩阵;fb表示测量真实加速度;表示地球系相对于惯性系的旋转角速度在导航系的投影;表示导航系相对于地球系的转动角速度在导航系的投影;×表示矢量叉乘运算;vn表示导航系速度;gn表示重力加速度在导航系下的投影;
对上式进行整理可得
式中,表示载体系到初始载体系之间的方向余弦矩阵;表示地球系相对于惯性系的旋转角速度在载体系的投影;表示测量真实角速度;vb表示载体系速度;表示载体系速度微分;fb表示测量真实加速度;表示初始导航系到初始载体系的方向余弦矩阵;表示导航系到初始导航系的方向余弦矩阵;gn表示重力加速度在导航系下的投影;
对上式两边同时进行积分可得
式中,β表示观测矢量;α表示参考矢量;表示载体系到初始载体系之间的方向余弦矩阵;表示地球系相对于惯性系的旋转角速度在载体系的投影;表示测量真实角速度;vb表示载体系速度;表示载体系速度微分;fb表示测量真实加速度;表示初始导航系到初始载体系的方向余弦矩阵;表示导航系到初始导航系的方向余弦矩阵;gn表示重力加速度在导航系下的投影;
将DVL测量速度代入观测矢量可得
步骤3具体包括:
对观测矢量进行模值计算可得
有观测矢量和参考矢量之间的关系可知
因此,可以得到DVL测量速度构造的含噪声矢量噪声模值为
噪声模值求解期望和方差可以得到
步骤4具体包括:由上面噪声模值的方差可以定义权值系数为
利用OBA姿态确定算法实现自适应矢量姿态确定
3.如权利要求1所述的模值检测动基座鲁棒对准方法,其特征在于,步骤2中,DVL采样周期为1s,量测标准差为0.1m/s。
4.一种计算机设备,包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序,其特征在于,所述处理器执行所述程序时实现权利要求1到3任一项所述方法的步骤。
5.一种计算机可读存储介质,其上存储有计算机程序,其特征在于,该程序被处理器执行时实现权利要求1到3任一项所述方法的步骤。
6.一种处理器,其特征在于,所述处理器用于运行程序,其中,所述程序运行时执行权利要求1到3任一项所述的方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910399911.9A CN110108301B (zh) | 2019-05-14 | 2019-05-14 | 模值检测动基座鲁棒对准方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910399911.9A CN110108301B (zh) | 2019-05-14 | 2019-05-14 | 模值检测动基座鲁棒对准方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN110108301A CN110108301A (zh) | 2019-08-09 |
CN110108301B true CN110108301B (zh) | 2020-12-01 |
Family
ID=67490008
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910399911.9A Active CN110108301B (zh) | 2019-05-14 | 2019-05-14 | 模值检测动基座鲁棒对准方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110108301B (zh) |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110987018B (zh) * | 2019-12-19 | 2023-11-24 | 苏州大学 | 比力微分的位置法dvl误差标定方法及系统 |
Family Cites Families (13)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
RU2467929C2 (ru) * | 2010-12-13 | 2012-11-27 | Открытое акционерное общество "Ракетно-космическая корпорация "Энергия" имени С.П. Королева" | Способ построения орбитальной ориентации пилотируемого космического аппарата |
CN102506898B (zh) * | 2011-11-03 | 2014-05-07 | 中国科学院自动化研究所 | 一种基于遗传算法的惯性/地磁传感器标定方法 |
TW201411096A (zh) * | 2012-08-02 | 2014-03-16 | Memsic Inc | 用於一三軸磁力計及一三軸加速度計之資料融合之方法及設備 |
CN104748761B (zh) * | 2013-12-26 | 2017-09-22 | 南京理工大学 | 基于最优姿态匹配的动基座传递对准时延补偿方法 |
CN104777478B (zh) * | 2015-04-16 | 2017-10-17 | 电子科技大学 | 一种相控阵雷达搜索捕获目标的方法 |
CN105806367B (zh) * | 2016-03-25 | 2018-07-06 | 中国人民解放军国防科学技术大学 | 无陀螺惯性系统误差标定方法 |
CN106153073B (zh) * | 2016-06-21 | 2018-10-12 | 东南大学 | 一种全姿态捷联惯导系统的非线性初始对准方法 |
CN106643713B (zh) * | 2016-12-22 | 2021-01-26 | 威海北洋电气集团股份有限公司 | 对阈值平滑自适应调整的零速修正步行者轨迹的推算方法及装置 |
US10845198B2 (en) * | 2017-03-14 | 2020-11-24 | Trimble Inc. | Integrated vision-based and inertial sensor systems for use in vehicle navigation |
CN108844533B (zh) * | 2018-04-24 | 2021-01-19 | 西安交通大学 | 一种基于多传感器融合和姿态解算的自由姿态pdr定位方法 |
CN109211230B (zh) * | 2018-09-07 | 2022-02-15 | 东南大学 | 一种基于牛顿迭代法的炮弹姿态和加速度计常值误差估计方法 |
CN109141475B (zh) * | 2018-09-14 | 2020-06-05 | 苏州大学 | 一种dvl辅助sins鲁棒行进间初始对准方法 |
CN109596144B (zh) * | 2018-12-10 | 2020-07-24 | 苏州大学 | Gnss位置辅助sins行进间初始对准方法 |
-
2019
- 2019-05-14 CN CN201910399911.9A patent/CN110108301B/zh active Active
Also Published As
Publication number | Publication date |
---|---|
CN110108301A (zh) | 2019-08-09 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN108362282B (zh) | 一种基于自适应零速区间调整的惯性行人定位方法 | |
CN108827299B (zh) | 一种基于改进四元数二阶互补滤波的飞行器姿态解算方法 | |
CN107560613B (zh) | 基于九轴惯性传感器的机器人室内轨迹跟踪系统及方法 | |
CN112013836B (zh) | 一种基于改进自适应卡尔曼滤波的航姿参考系统算法 | |
US9683865B2 (en) | In-use automatic calibration methodology for sensors in mobile devices | |
Batista et al. | Accelerometer calibration and dynamic bias and gravity estimation: Analysis, design, and experimental evaluation | |
CN110398245B (zh) | 基于脚戴式惯性测量单元的室内行人导航姿态估计方法 | |
Phuong et al. | A DCM based orientation estimation algorithm with an inertial measurement unit and a magnetic compass | |
CN109141475B (zh) | 一种dvl辅助sins鲁棒行进间初始对准方法 | |
JP5328252B2 (ja) | ナビゲーションシステムの位置検出装置および位置検出方法 | |
CN108007477B (zh) | 一种基于正反向滤波的惯性行人定位系统误差抑制方法 | |
CN108731676B (zh) | 一种基于惯性导航技术的姿态融合增强测量方法及系统 | |
CN108981694A (zh) | 基于小波神经网络与ekf的姿态解算方法及系统 | |
CN109596144B (zh) | Gnss位置辅助sins行进间初始对准方法 | |
AU2015305864B2 (en) | Earthmoving machine comprising weighted state estimator | |
CN110174121A (zh) | 一种基于地磁场自适应修正的航姿系统姿态解算方法 | |
CN110132271B (zh) | 一种自适应卡尔曼滤波姿态估计算法 | |
CN110567492A (zh) | 低成本mems惯性传感器系统级标定方法 | |
KR20210013526A (ko) | 관성 위치를 사용한 지형 참조 항법을 위한 장치 및 방법 | |
Troni et al. | Experimental evaluation of a MEMS inertial measurements unit for Doppler navigation of underwater vehicles | |
CN111649747A (zh) | 一种基于imu的自适应ekf姿态测量改进方法 | |
KR100643400B1 (ko) | 입력 장치의 자세 측정 방법 및 그 장치 | |
CN110108301B (zh) | 模值检测动基座鲁棒对准方法 | |
CN112033405B (zh) | 一种室内环境磁异常实时修正与导航方法及装置 | |
CN112070170B (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 |