CN110108301B - 模值检测动基座鲁棒对准方法 - Google Patents

模值检测动基座鲁棒对准方法 Download PDF

Info

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
Application number
CN201910399911.9A
Other languages
English (en)
Other versions
CN110108301A (zh
Inventor
徐祥
徐大诚
白春风
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Suzhou University
Original Assignee
Suzhou University
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Suzhou University filed Critical Suzhou University
Priority to CN201910399911.9A priority Critical patent/CN110108301B/zh
Publication of CN110108301A publication Critical patent/CN110108301A/zh
Application granted granted Critical
Publication of CN110108301B publication Critical patent/CN110108301B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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具体包括：由惯性测量单元测量模型可知：
Figure BDA0002059414640000021
Figure BDA0002059414640000022
式中，
Figure BDA0002059414640000023
表示测量加速度；fb表示测量真实加速度；▽b表示加速度测量偏置误差；
Figure BDA0002059414640000024
表示加速度计测量噪声；
Figure BDA0002059414640000025
表示陀螺仪测量角速度；
Figure BDA0002059414640000026
表示测量真实角速度；εb表示陀螺仪测量偏置；
Figure BDA0002059414640000027
表示陀螺仪测量噪声；
由姿态更新方程可知：
Figure BDA0002059414640000028
Figure BDA0002059414640000029
式中，
Figure BDA00020594146400000210
表示测量真实角速度；
Figure BDA00020594146400000211
表示载体系到初始载体系的方向余弦矩阵；
Figure BDA00020594146400000212
表示表示载体系到初始载体系的方向余弦矩阵的微分；
Figure BDA00020594146400000213
表示导航系相对于惯性系的旋转角速度在导航系的投影；
Figure BDA00020594146400000214
表示导航系相对于初始导航系的方向余弦矩阵；
Figure BDA00020594146400000215
表示导航系相对于初始导航系的方向余弦矩阵的微分。
在其中一个实施例中，步骤2具体包括：由DVL测量模型可知：
Figure BDA00020594146400000216
式中，
Figure BDA00020594146400000217
表示DVL测量速度；vb表示真实速度；
Figure BDA00020594146400000218
表示测量速度误差；
由比力方程可知
Figure BDA0002059414640000031
式中，
Figure BDA0002059414640000032
表示导航系速度的微分；
Figure BDA0002059414640000033
表示载体系相对于导航系变化的方向余弦矩阵；fb表示测量真实加速度；
Figure BDA0002059414640000034
表示地球系相对于惯性系的旋转角速度在导航系的投影；
Figure BDA0002059414640000035
表示导航系相对于地球系的转动角速度在导航系的投影；×表示矢量叉乘运算；vn表示导航系速度；gn表示重力加速度在导航系下的投影；
由比力方程及方向余弦变化关系可知：
Figure BDA0002059414640000036
式中，
Figure BDA0002059414640000037
表示载体系相对于导航系变化的方向余弦矩阵；
Figure BDA0002059414640000038
表示载体系相对于导航系的旋转角速度在载体系的投影；vb表示载体系速度；
Figure BDA0002059414640000039
表示载体系速度的微分；
Figure BDA00020594146400000310
表示初始导航系相对于导航系的方向余弦矩阵；
Figure BDA00020594146400000311
表示初始载体系到初始导航系之间的方向余弦矩阵；
Figure BDA00020594146400000312
表示载体系到初始载体系之间的方向余弦矩阵；fb表示测量真实加速度；
Figure BDA00020594146400000313
表示地球系相对于惯性系的旋转角速度在导航系的投影；
Figure BDA00020594146400000314
表示导航系相对于地球系的转动角速度在导航系的投影；×表示矢量叉乘运算；vn表示导航系速度；gn表示重力加速度在导航系下的投影；
对上式进行整理可得
Figure BDA00020594146400000315
式中，
Figure BDA00020594146400000316
表示载体系到初始载体系之间的方向余弦矩阵；
Figure BDA00020594146400000317
表示地球系相对于惯性系的旋转角速度在载体系的投影；
Figure BDA00020594146400000318
表示测量真实角速度；vb表示载体系速度；
Figure BDA00020594146400000319
表示载体系速度微分；fb表示测量真实加速度；
Figure BDA00020594146400000320
表示初始导航系到初始载体系的方向余弦矩阵；
Figure BDA00020594146400000321
表示导航系到初始导航系的方向余弦矩阵；gn表示重力加速度在导航系下的投影；
对上式两边同时进行积分可得
Figure BDA0002059414640000041
Figure BDA0002059414640000042
式中，β表示观测矢量；α表示参考矢量；
Figure BDA0002059414640000043
表示载体系到初始载体系之间的方向余弦矩阵；
Figure BDA0002059414640000044
表示地球系相对于惯性系的旋转角速度在载体系的投影；
Figure BDA0002059414640000045
表示测量真实角速度；vb表示载体系速度；
Figure BDA0002059414640000046
表示载体系速度微分；fb表示测量真实加速度；
Figure BDA0002059414640000047
表示初始导航系到初始载体系的方向余弦矩阵；
Figure BDA0002059414640000048
表示导航系到初始导航系的方向余弦矩阵；gn表示重力加速度在导航系下的投影；
将DVL测量速度代入观测矢量可得
Figure BDA0002059414640000049
式中，
Figure BDA00020594146400000410
表示采用DVL测量速度构造的含噪声的观测矢量；β表示观测矢量；
Figure BDA00020594146400000411
表示DVL测量误差在初始载体系的映射。
在其中一个实施例中，步骤3具体包括：
对观测矢量进行模值计算可得
Figure BDA00020594146400000412
式中，
Figure BDA00020594146400000413
表示采用DVL测量速度构造的含噪声的观测矢量；β表示观测矢量；
Figure BDA00020594146400000414
表示DVL测量误差在初始载体系的映射；T表示转置运算；
有观测矢量和参考矢量之间的关系可知
Figure BDA00020594146400000415
式中，β表示观测矢量；α表示参考矢量；
Figure BDA00020594146400000416
表示初始导航系到初始载体系的方向余弦矩阵；
因此，可以得到DVL测量速度构造的含噪声矢量噪声模值为
Figure BDA0002059414640000051
式中，χ表示噪声模值；
Figure BDA0002059414640000052
表示采用DVL测量速度构造的含噪声的观测矢量；β表示观测矢量；α表示参考矢量；
Figure BDA0002059414640000053
表示DVL测量误差在初始载体系的映射；T表示转置运算；
噪声模值求解期望和方差可以得到
Figure BDA0002059414640000054
Figure BDA0002059414640000055
式中，E[·]表示期望运算；
Figure BDA0002059414640000056
表示DVL测量速度噪声的方差；β表示观测矢量；α表示参考矢量。
在其中一个实施例中，步骤4具体包括：由上面噪声模值的方差可以定义权值系数为
Figure BDA0002059414640000057
式中，
Figure BDA0002059414640000058
表示归一化之后的权值；
Figure BDA0002059414640000059
表示采用DVL测量速度构造的含噪声的观测矢量；α表示参考矢量；σv表示DVL测量速度噪声的标准差；
利用OBA姿态确定算法实现自适应矢量姿态确定
Figure BDA00020594146400000510
Figure BDA00020594146400000511
Figure BDA00020594146400000512
式中，Kk表示k时刻的构造矩阵；Kk-1表示k-1时刻的构造矩阵；
Figure BDA00020594146400000513
表示采用DVL测量速度构造的含噪声的观测矢量；α表示参考矢量；
Figure BDA00020594146400000514
表示归一化之后的权值。
在其中一个实施例中，步骤1中，陀螺仪量测常值漂移误差为
Figure BDA0002059414640000061
陀螺仪量测随机游走误差为
Figure BDA0002059414640000062
输出频率为200Hz；加速度计量测常值漂移误差为▽b＝[500500500]Tμg，加速度计量测随机游走误差为
Figure BDA0002059414640000063
输出频率为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：获取惯性传感器实时数据并进行姿态更新；
由惯性测量单元测量模型可知：
Figure BDA0002059414640000071
Figure BDA0002059414640000072
式中，
Figure BDA0002059414640000073
表示测量加速度；fb表示测量真实加速度；▽b表示加速度测量偏置误差；
Figure BDA0002059414640000074
表示加速度计测量噪声；
Figure BDA0002059414640000075
表示陀螺仪测量角速度；
Figure BDA0002059414640000076
表示测量真实角速度；εb表示陀螺仪测量偏置；
Figure BDA0002059414640000077
表示陀螺仪测量噪声；
由姿态更新方程可知：
Figure BDA0002059414640000081
Figure BDA0002059414640000082
式中，
Figure BDA0002059414640000083
表示测量真实角速度；
Figure BDA0002059414640000084
表示载体系到初始载体系的方向余弦矩阵；
Figure BDA0002059414640000085
表示表示载体系到初始载体系的方向余弦矩阵的微分；
Figure BDA0002059414640000086
表示导航系相对于惯性系的旋转角速度在导航系的投影；
Figure BDA0002059414640000087
表示导航系相对于初始导航系的方向余弦矩阵；
Figure BDA0002059414640000088
表示导航系相对于初始导航系的方向余弦矩阵的微分；
步骤2：获取辅助传感器信息，构造矢量观测器；
由DVL测量模型可知：
Figure BDA0002059414640000089
式中，
Figure BDA00020594146400000810
表示DVL测量速度；vb表示真实速度；
Figure BDA00020594146400000811
表示测量速度误差；
由比力方程可知
Figure BDA00020594146400000812
式中，
Figure BDA00020594146400000813
表示导航系速度的微分；
Figure BDA00020594146400000814
表示载体系相对于导航系变化的方向余弦矩阵；fb表示测量真实加速度；
Figure BDA00020594146400000815
表示地球系相对于惯性系的旋转角速度在导航系的投影；
Figure BDA00020594146400000816
表示导航系相对于地球系的转动角速度在导航系的投影；×表示矢量叉乘运算；vn表示导航系速度；gn表示重力加速度在导航系下的投影；
由比力方程及方向余弦变化关系可知：
Figure BDA00020594146400000817
式中，
Figure BDA00020594146400000818
表示载体系相对于导航系变化的方向余弦矩阵；
Figure BDA00020594146400000819
表示载体系相对于导航系的旋转角速度在载体系的投影；vb表示载体系速度；
Figure BDA00020594146400000820
表示载体系速度的微分；
Figure BDA00020594146400000821
表示初始导航系相对于导航系的方向余弦矩阵；
Figure BDA00020594146400000822
表示初始载体系到初始导航系之间的方向余弦矩阵；
Figure BDA00020594146400000823
表示载体系到初始载体系之间的方向余弦矩阵；fb表示测量真实加速度；
Figure BDA0002059414640000091
表示地球系相对于惯性系的旋转角速度在导航系的投影；
Figure BDA0002059414640000092
表示导航系相对于地球系的转动角速度在导航系的投影；×表示矢量叉乘运算；vn表示导航系速度；gn表示重力加速度在导航系下的投影；
对上式进行整理可得
Figure BDA0002059414640000093
式中，
Figure BDA0002059414640000094
表示载体系到初始载体系之间的方向余弦矩阵；
Figure BDA0002059414640000095
表示地球系相对于惯性系的旋转角速度在载体系的投影；
Figure BDA0002059414640000096
表示测量真实角速度；vb表示载体系速度；
Figure BDA0002059414640000097
表示载体系速度微分；fb表示测量真实加速度；
Figure BDA0002059414640000098
表示初始导航系到初始载体系的方向余弦矩阵；
Figure BDA0002059414640000099
表示导航系到初始导航系的方向余弦矩阵；gn表示重力加速度在导航系下的投影；
对上式两边同时进行积分可得
Figure BDA00020594146400000910
Figure BDA00020594146400000911
式中，β表示观测矢量；α表示参考矢量；
Figure BDA00020594146400000912
表示载体系到初始载体系之间的方向余弦矩阵；
Figure BDA00020594146400000913
表示地球系相对于惯性系的旋转角速度在载体系的投影；
Figure BDA00020594146400000914
表示测量真实角速度；vb表示载体系速度；
Figure BDA00020594146400000915
表示载体系速度微分；fb表示测量真实加速度；
Figure BDA00020594146400000916
表示初始导航系到初始载体系的方向余弦矩阵；
Figure BDA00020594146400000917
表示导航系到初始导航系的方向余弦矩阵；gn表示重力加速度在导航系下的投影；
将DVL测量速度代入观测矢量可得
Figure BDA00020594146400000918
式中，
Figure BDA00020594146400000919
表示采用DVL测量速度构造的含噪声的观测矢量；β表示观测矢量；
Figure BDA00020594146400000920
表示DVL测量误差在初始载体系的映射；
步骤3：对矢量观测器进行模值计算，并利用标准模值进行异常检测；
对观测矢量进行模值计算可得
Figure BDA0002059414640000101
式中，
Figure BDA0002059414640000102
表示采用DVL测量速度构造的含噪声的观测矢量；β表示观测矢量；
Figure BDA0002059414640000103
表示DVL测量误差在初始载体系的映射；T表示转置运算；
有观测矢量和参考矢量之间的关系可知
Figure BDA0002059414640000104
式中，β表示观测矢量；α表示参考矢量；
Figure BDA0002059414640000105
表示初始导航系到初始载体系的方向余弦矩阵；
因此，可以得到DVL测量速度构造的含噪声矢量噪声模值为
Figure BDA0002059414640000106
式中，χ表示噪声模值；
Figure BDA0002059414640000107
表示采用DVL测量速度构造的含噪声的观测矢量；β表示观测矢量；α表示参考矢量；
Figure BDA0002059414640000108
表示DVL测量误差在初始载体系的映射；T表示转置运算；
噪声模值求解期望和方差可以得到
Figure BDA0002059414640000109
Figure BDA00020594146400001010
式中，E[·]表示期望运算；
Figure BDA00020594146400001011
表示DVL测量速度噪声的方差；β表示观测矢量；α表示参考矢量；
步骤4：通过模值检测削弱异常噪声影响，构造自适应矢量姿态确定算法；
由上面噪声模值的方差可以定义权值系数为
Figure BDA00020594146400001012
式中，
Figure BDA0002059414640000111
表示归一化之后的权值；
Figure BDA0002059414640000112
表示采用DVL测量速度构造的含噪声的观测矢量；α表示参考矢量；σv表示DVL测量速度噪声的标准差；
利用OBA姿态确定算法实现自适应矢量姿态确定
Figure BDA0002059414640000113
Figure BDA0002059414640000114
Figure BDA0002059414640000115
式中，Kk表示k时刻的构造矩阵；Kk-1表示k-1时刻的构造矩阵；
Figure BDA0002059414640000116
表示采用DVL测量速度构造的含噪声的观测矢量；α表示参考矢量；
Figure BDA0002059414640000117
表示归一化之后的权值；
对本发明的有益效果说明如下：
MATLAB仿真实验，在以下的仿真条件下，对该方法进行仿真实验：
陀螺仪量测常值漂移误差为
Figure BDA0002059414640000118
陀螺仪量测随机游走误差为
Figure BDA0002059414640000119
输出频率为200Hz；加速度计量测常值漂移误差为▽b＝[500500500]Tμg，加速度计量测随机游走误差为
Figure BDA00020594146400001110
输出频率为200Hz。DVL采样周期为1s，量测标准差为0.1m/s。对准时间为M＝300s。
以上所述实施例仅是为充分说明本发明而所举的较佳的实施例，本发明的保护范围不限于此。本技术领域的技术人员在本发明基础上所作的等同替代或变换，均在本发明的保护范围之内。本发明的保护范围以权利要求书为准。

Claims (6)

1.一种模值检测动基座鲁棒对准方法，其特征在于，包括：
步骤1：获取惯性传感器实时数据并进行姿态更新；
步骤2：获取辅助传感器信息，构造矢量观测器；
步骤3：对矢量观测器进行模值计算，并利用标准模值进行异常检测；
步骤4：通过模值检测削弱异常噪声影响，构造自适应矢量姿态确定算法；初始对准过程运行时间为M，若姿态变化次数k＝M，则输出对准结果，完成初始对准过程，若姿态变化次数k＜M，表示初始对准过程未完成，则重复上述步骤直到初始对准过程结束；
步骤1具体包括：由惯性测量单元测量模型可知：
Figure FDA0002643920460000011
Figure FDA0002643920460000012
式中，
Figure FDA0002643920460000013
表示测量加速度；fb表示测量真实加速度；
Figure FDA0002643920460000014
表示加速度测量偏置误差；
Figure FDA0002643920460000015
表示加速度计测量噪声；
Figure FDA0002643920460000016
表示陀螺仪测量角速度；
Figure FDA0002643920460000017
表示测量真实角速度；εb表示陀螺仪测量偏置；
Figure FDA0002643920460000018
表示陀螺仪测量噪声；
由姿态更新方程可知：
Figure FDA0002643920460000019
Figure FDA00026439204600000110
式中，
Figure FDA00026439204600000111
表示测量真实角速度；
Figure FDA00026439204600000112
表示载体系到初始载体系的方向余弦矩阵；
Figure FDA00026439204600000113
表示表示载体系到初始载体系的方向余弦矩阵的微分；
Figure FDA00026439204600000114
表示导航系相对于惯性系的旋转角速度在导航系的投影；
Figure FDA00026439204600000115
表示导航系相对于初始导航系的方向余弦矩阵；
Figure FDA00026439204600000116
表示导航系相对于初始导航系的方向余弦矩阵的微分；
步骤2具体包括：由DVL测量模型可知：
Figure FDA00026439204600000117
式中，
Figure FDA0002643920460000021
表示DVL测量速度；vb表示真实速度；
Figure FDA0002643920460000022
表示测量速度误差；
由比力方程可知
Figure FDA0002643920460000023
式中，
Figure FDA0002643920460000024
表示导航系速度的微分；
Figure FDA0002643920460000025
表示载体系相对于导航系变化的方向余弦矩阵；fb表示测量真实加速度；
Figure FDA0002643920460000026
表示地球系相对于惯性系的旋转角速度在导航系的投影；
Figure FDA0002643920460000027
表示导航系相对于地球系的转动角速度在导航系的投影；×表示矢量叉乘运算；vn表示导航系速度；gn表示重力加速度在导航系下的投影；
由比力方程及方向余弦变化关系可知：
Figure FDA0002643920460000028
式中，
Figure FDA0002643920460000029
表示载体系相对于导航系变化的方向余弦矩阵；
Figure FDA00026439204600000210
表示载体系相对于导航系的旋转角速度在载体系的投影；vb表示载体系速度；
Figure FDA00026439204600000211
表示载体系速度的微分；
Figure FDA00026439204600000212
表示初始导航系相对于导航系的方向余弦矩阵；
Figure FDA00026439204600000213
表示初始载体系到初始导航系之间的方向余弦矩阵；
Figure FDA00026439204600000214
表示载体系到初始载体系之间的方向余弦矩阵；fb表示测量真实加速度；
Figure FDA00026439204600000215
表示地球系相对于惯性系的旋转角速度在导航系的投影；
Figure FDA00026439204600000216
表示导航系相对于地球系的转动角速度在导航系的投影；×表示矢量叉乘运算；vn表示导航系速度；gn表示重力加速度在导航系下的投影；
对上式进行整理可得
Figure FDA00026439204600000217
式中，
Figure FDA00026439204600000218
表示载体系到初始载体系之间的方向余弦矩阵；
Figure FDA00026439204600000219
表示地球系相对于惯性系的旋转角速度在载体系的投影；
Figure FDA00026439204600000220
表示测量真实角速度；vb表示载体系速度；
Figure FDA00026439204600000221
表示载体系速度微分；fb表示测量真实加速度；
Figure FDA00026439204600000222
表示初始导航系到初始载体系的方向余弦矩阵；
Figure FDA00026439204600000223
表示导航系到初始导航系的方向余弦矩阵；gn表示重力加速度在导航系下的投影；
对上式两边同时进行积分可得
Figure FDA0002643920460000031
Figure FDA0002643920460000032
式中，β表示观测矢量；α表示参考矢量；
Figure FDA0002643920460000033
表示载体系到初始载体系之间的方向余弦矩阵；
Figure FDA0002643920460000034
表示地球系相对于惯性系的旋转角速度在载体系的投影；
Figure FDA0002643920460000035
表示测量真实角速度；vb表示载体系速度；
Figure FDA0002643920460000036
表示载体系速度微分；fb表示测量真实加速度；
Figure FDA0002643920460000037
表示初始导航系到初始载体系的方向余弦矩阵；
Figure FDA0002643920460000038
表示导航系到初始导航系的方向余弦矩阵；gn表示重力加速度在导航系下的投影；
将DVL测量速度代入观测矢量可得
Figure FDA0002643920460000039
式中，
Figure FDA00026439204600000310
表示采用DVL测量速度构造的含噪声的观测矢量；β表示观测矢量；
Figure FDA00026439204600000311
表示DVL测量误差在初始载体系的映射；
步骤3具体包括：
对观测矢量进行模值计算可得
Figure FDA00026439204600000312
式中，
Figure FDA00026439204600000313
表示采用DVL测量速度构造的含噪声的观测矢量；β表示观测矢量；
Figure FDA00026439204600000314
表示DVL测量误差在初始载体系的映射；T表示转置运算；
有观测矢量和参考矢量之间的关系可知
Figure FDA00026439204600000315
式中，β表示观测矢量；α表示参考矢量；
Figure FDA00026439204600000316
表示初始导航系到初始载体系的方向余弦矩阵；
因此，可以得到DVL测量速度构造的含噪声矢量噪声模值为
Figure FDA0002643920460000041
式中，χ表示噪声模值；
Figure FDA0002643920460000042
表示采用DVL测量速度构造的含噪声的观测矢量；β表示观测矢量；α表示参考矢量；
Figure FDA0002643920460000043
表示DVL测量误差在初始载体系的映射；T表示转置运算；
噪声模值求解期望和方差可以得到
Figure FDA0002643920460000044
Figure FDA0002643920460000045
式中，E[·]表示期望运算；
Figure FDA0002643920460000046
表示DVL测量速度噪声的方差；β表示观测矢量；α表示参考矢量；
步骤4具体包括：由上面噪声模值的方差可以定义权值系数为
Figure FDA0002643920460000047
式中，
Figure FDA0002643920460000048
表示归一化之后的权值；
Figure FDA0002643920460000049
表示采用DVL测量速度构造的含噪声的观测矢量；α表示参考矢量；σv表示DVL测量速度噪声的标准差；
利用OBA姿态确定算法实现自适应矢量姿态确定
Figure FDA00026439204600000410
Figure FDA00026439204600000411
Figure FDA00026439204600000412
式中，Kk表示k时刻的构造矩阵；Kk-1表示k-1时刻的构造矩阵；
Figure FDA00026439204600000413
表示采用DVL测量速度构造的含噪声的观测矢量；α表示参考矢量；
Figure FDA00026439204600000414
表示归一化之后的权值。
2.如权利要求1所述的模值检测动基座鲁棒对准方法，其特征在于，步骤1中，陀螺仪量测常值漂移误差为εb＝[0.02 0.02 0.02]T°/h，陀螺仪量测随机游走误差为
Figure FDA0002643920460000051
输出频率为200Hz；加速度计量测常值漂移误差为
Figure FDA0002643920460000052
加速度计量测随机游走误差为
Figure FDA0002643920460000053
输出频率为200Hz。
3.如权利要求1所述的模值检测动基座鲁棒对准方法，其特征在于，步骤2中，DVL采样周期为1s，量测标准差为0.1m/s。
4.一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时实现权利要求1到3任一项所述方法的步骤。
5.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行时实现权利要求1到3任一项所述方法的步骤。
6.一种处理器，其特征在于，所述处理器用于运行程序，其中，所述程序运行时执行权利要求1到3任一项所述的方法。
CN201910399911.9A 2019-05-14 2019-05-14 模值检测动基座鲁棒对准方法 Active CN110108301B (zh)

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)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110987018B (zh) * 2019-12-19 2023-11-24 苏州大学 比力微分的位置法dvl误差标定方法及***

Family Cites Families (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
RU2467929C2 (ru) * 2010-12-13 2012-11-27 Открытое акционерное общество "Ракетно-космическая корпорация "Энергия" имени С.П. Королева" Способ построения орбитальной ориентации пилотируемого космического аппарата
CN102506898B (zh) * 2011-11-03 2014-05-07 中国科学院自动化研究所 一种基于遗传算法的惯性/地磁传感器标定方法
CN104884902B (zh) * 2012-08-02 2018-04-06 美新公司 用于三轴磁力计和三轴加速度计的数据融合的方法和装置
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行进间初始对准方法

Also Published As

Publication number Publication date
CN110108301A (zh) 2019-08-09

Similar Documents

Publication Publication Date Title
CN108362282B (zh) 一种基于自适应零速区间调整的惯性行人定位方法
CN108827299B (zh) 一种基于改进四元数二阶互补滤波的飞行器姿态解算方法
CN112013836B (zh) 一种基于改进自适应卡尔曼滤波的航姿参考***算法
CN107560613B (zh) 基于九轴惯性传感器的机器人室内轨迹跟踪***及方法
CN101726295B (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) 一种基于惯性导航技术的姿态融合增强测量方法及***
CN109596144B (zh) Gnss位置辅助sins行进间初始对准方法
CN103822633A (zh) 一种基于二阶量测更新的低成本姿态估计方法
AU2015305864B2 (en) Earthmoving machine comprising weighted state estimator
CN110702113B (zh) 基于mems传感器的捷联惯导***数据预处理和姿态解算的方法
CN112070170B (zh) 一种动态残差阈值自适应四元数粒子滤波姿态解算数据融合方法
CN110567492A (zh) 低成本mems惯性传感器***级标定方法
KR20210013526A (ko) 관성 위치를 사용한 지형 참조 항법을 위한 장치 및 방법
Troni et al. Experimental evaluation of a MEMS inertial measurements unit for Doppler navigation of underwater vehicles
JP5164645B2 (ja) カルマンフィルタ処理における繰り返し演算制御方法及び装置
Cirillo et al. A comparison of multisensor attitude estimation algorithms
KR100643400B1 (ko) 입력 장치의 자세 측정 방법 및 그 장치
CN110108301B (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