CN110986925B - 初始姿态最优估计方法 - Google Patents

初始姿态最优估计方法 Download PDF

Info

Publication number
CN110986925B
CN110986925B CN201911211612.4A CN201911211612A CN110986925B CN 110986925 B CN110986925 B CN 110986925B CN 201911211612 A CN201911211612 A CN 201911211612A CN 110986925 B CN110986925 B CN 110986925B
Authority
CN
China
Prior art keywords
matrix
calculating
initial attitude
loss function
adjoint
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
CN201911211612.4A
Other languages
English (en)
Other versions
CN110986925A (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.)
Beijing Automation Control Equipment Institute BACEI
Original Assignee
Beijing Automation Control Equipment Institute BACEI
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 Beijing Automation Control Equipment Institute BACEI filed Critical Beijing Automation Control Equipment Institute BACEI
Priority to CN201911211612.4A priority Critical patent/CN110986925B/zh
Publication of CN110986925A publication Critical patent/CN110986925A/zh
Application granted granted Critical
Publication of CN110986925B publication Critical patent/CN110986925B/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
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/04—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means
    • G01C21/08—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means involving use of the magnetic field of the earth
    • 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

Abstract

本发明涉及惯性导航技术领域，公开了一种初始姿态最优估计方法。其中，该方法包括：计算重力矢量在体坐标系下的测量值
Figure DDA0002298301660000011
和重力矢量在参考坐标系下的测量值
Figure DDA0002298301660000012
基于测量值
Figure DDA0002298301660000013
和测量值
Figure DDA0002298301660000014
计算损失函数构造矩阵K；计算损失函数构造矩阵K的特征值λi和特征向量qi；根据特征值λi和特征向量qi计算扩展矩阵H及其伴随矩阵H*；基于伴随矩阵H*得到初始姿态最优解。由此，能够有效提高单位置条件下的初始姿态估计精度。

Description

初始姿态最优估计方法
技术领域
本发明涉及惯性导航技术领域，尤其涉及一种初始姿态最优估计方法。
背景技术
惯导系统的初始对准是影响系统使用性能的关键技术之一，对准的精度与速度直接关系到惯性系统的精度与启动特性。根据惯导系统原理，若要完全消除误差需要进行多位置对准计算，但在绝大多数情况下，这个要求无法满足，一般只能在一个位置进行对准。因此，提高单位置初始对准精度具有重要应用价值。
当前常用的方法是基于重力矢量的对准方法，通过计算重力矢量在惯性空间内旋转的角度求取惯导系统的初始姿态，该方法具有估计精度高，鲁棒性强的特点。使用卡尔曼滤波、递推最小二乘等递推算法，可以实现姿态的实时最优估计。但传统的重力矢量方法在计算时对系统姿态进行了线性化假设，即表示姿态的四元数
Figure BDA0002298301640000011
中的常数项q＝0，因此当转动角度为180°时会无法收敛。
发明内容
本发明的目的在于克服现有技术不足，提供了一种初始姿态最优估计方法，能够解决现有技术中的问题。
本发明的技术解决方案：一种初始姿态最优估计方法，其中，该方法包括：
计算重力矢量在体坐标系下的测量值
Figure BDA0002298301640000012
和重力矢量在参考坐标系下的测量值
Figure BDA0002298301640000013
基于测量值
Figure BDA0002298301640000021
和测量值
Figure BDA0002298301640000022
计算损失函数构造矩阵K；
计算损失函数构造矩阵K的特征值λi和特征向量qi；
根据特征值λi和特征向量qi计算扩展矩阵H及其伴随矩阵H*；
基于伴随矩阵H*得到初始姿态最优解。
优选地，通过下式基于测量值
Figure BDA0002298301640000023
和测量值
Figure BDA0002298301640000024
计算损失函数构造矩阵K：
Figure BDA0002298301640000025
Figure BDA0002298301640000026
Figure BDA0002298301640000027
S＝B+BT，
Figure BDA0002298301640000028
其中，αi为权重系数，且
Figure BDA0002298301640000029
共n个系数，对应n组重力矢量测量值。
优选地，通过下式计算损失函数构造矩阵K的特征值λi和特征向量qi：
Figure BDA00022983016400000210
其中，i＝1,2,3,4，λi和qi分别表示4个特征值和4个特征向量。
优选地，通过下式根据特征值λi和特征向量qi计算扩展矩阵H：
Figure BDA00022983016400000211
优选地，通过下式计算伴随矩阵H*：
Figure BDA00022983016400000212
其中，λj,k,l为不同于λi的其他三个特征值。
优选地，基于伴随矩阵H*得到初始姿态最优解包括：
令伴随矩阵H*中λ＝λmax＝λ1，则伴随矩阵H*中除了第一项外全部为0，得到下式，并通过下式得到初始姿态最优解：
Figure BDA0002298301640000031
其中，qopt为初始姿态最优解，λmax为损失函数构造矩阵K的所有特征值中最大的一个值。
通过上述技术方案，可以基于重力矢量在体坐标系下的测量值
Figure BDA0002298301640000033
和重力矢量在参考坐标系下的测量值
Figure BDA0002298301640000032
计算损失函数构造矩阵K，然后可以通过损失函数构造矩阵K的特征值λi和特征向量qi计算扩展矩阵H及其伴随矩阵H*，进而可以根据伴随矩阵H*得到初始姿态最优解。由此，能够有效提高单位置条件下的初始姿态估计精度。
附图说明
所包括的附图用来提供对本发明实施例的进一步的理解，其构成了说明书的一部分，用于例示本发明的实施例，并与文字描述一起来阐释本发明的原理。显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本发明实施例提供的一种初始姿态最优估计方法的流程图。
具体实施方式
下面将结合附图对本发明的具体实施例进行详细说明。在下面的描述中，出于解释而非限制性的目的，阐述了具体细节，以帮助全面地理解本发明。然而，对本领域技术人员来说显而易见的是，也可以在脱离了这些具体细节的其它实施例中实践本发明。
在此需要说明的是，为了避免因不必要的细节而模糊了本发明，在附图中仅仅示出了与根据本发明的方案密切相关的设备结构和/或处理步骤，而省略了与本发明关系不大的其他细节。
图1为本发明实施例提供的一种初始姿态最优估计方法的流程图。
如图1所示，本发明实施例提供了一种初始姿态最优估计方法，其中，该方法包括：
S100，计算重力矢量在体坐标系下的测量值
Figure BDA0002298301640000041
和重力矢量在参考坐标系下的测量值
Figure BDA0002298301640000042
S102，基于测量值
Figure BDA0002298301640000043
和测量值
Figure BDA0002298301640000044
计算损失函数构造矩阵K；
S104，计算损失函数构造矩阵K的特征值λi和特征向量qi；
S106，根据特征值λi和特征向量qi计算扩展矩阵H及其伴随矩阵H*；
S108，基于伴随矩阵H*得到初始姿态最优解。
通过上述技术方案，可以基于重力矢量在体坐标系下的测量值
Figure BDA0002298301640000045
和重力矢量在参考坐标系下的测量值
Figure BDA0002298301640000046
计算损失函数构造矩阵K，然后可以通过损失函数构造矩阵K的特征值λi和特征向量qi计算扩展矩阵H及其伴随矩阵H*，进而可以根据伴随矩阵H*得到初始姿态最优解。由此，能够有效提高单位置条件下的初始姿态估计精度。
根据本发明一种实施例，通过下式基于测量值
Figure BDA0002298301640000047
和测量值
Figure BDA0002298301640000048
计算损失函数构造矩阵K：
Figure BDA0002298301640000049
Figure BDA00022983016400000410
Figure BDA0002298301640000051
S＝B+BT， (4)
Figure BDA0002298301640000052
其中，αi为权重系数，且
Figure BDA0002298301640000053
共n个系数，对应n组重力矢量测量值。
每组重力矢量测量值包括重力矢量在体坐标系下的测量值和重力矢量在参考坐标系下的测量值。
上述的式(1)-(4)为中间变量，用于简化计算过程。也就是，根据(1)-(4)计算损失函数构造矩阵K。
根据本发明一种实施例，通过下式计算损失函数构造矩阵K的特征值λi和特征向量qi：
Figure BDA0002298301640000054
其中，i＝1,2,3,4，λi和qi分别表示4个特征值和4个特征向量。
在本发明中，可以根据实对称阵的特性，得到上述式(6)。
根据本发明一种实施例，通过下式根据特征值λi和特征向量qi计算扩展矩阵H：
Figure BDA0002298301640000055
根据本发明一种实施例，通过下式计算伴随矩阵H*：
Figure BDA0002298301640000056
其中，λj,k,l为不同于λi的其他三个特征值。
根据本发明一种实施例，基于伴随矩阵H*得到初始姿态最优解包括：
令伴随矩阵H*中λ＝λmax＝λ1，则伴随矩阵H*中除了第一项外全部为0，得到下式，并通过下式得到初始姿态最优解：
Figure BDA0002298301640000061
其中，qopt为初始姿态最优解，λmax为损失函数构造矩阵K的所有特征值中最大的一个值。
由此可知，H*中的每一组列向量都是待求的(乘以了一个系数后的)最优四元数。从实际情况来看，求取应该使用范数最大的一列，这是因为如果qopt中某一项趋近于0，那么会使得H*中乘以该项的一列由于计算机字长限制，使得解算出的四元数产生很大的量化误差。由于K是实对称阵，因此只要选择H*对角线上的元素是最大值一列就可以得到初始姿态最优解。
下面对通过本发明上述的方法得到的初始姿态最优解进行验证。
姿态余弦矩阵A与姿态四元数字
Figure BDA0002298301640000062
的转换关系为：
Figure BDA0002298301640000063
Figure BDA0002298301640000064
其中，θ为旋转角，
Figure BDA0002298301640000065
为旋转轴的单位向量。
为实现姿态最优估计，定义如下损失函数：
Figure BDA0002298301640000066
则有：
g(A)＝1-L(A)＝tr[ABT] (13)
为实现损失函数最小，只需满足转换后的损失函数g(A)最大即可。
将式(10)带入式(13)可得：
Figure BDA0002298301640000067
四元数的元素具有唯一约束：
Figure BDA0002298301640000071
为求取式(14)在式(15)约束条件下对最大值，重新构建方程：
Figure BDA0002298301640000072
令
Figure BDA0002298301640000073
可得：
Figure BDA0002298301640000074
由上式分析可知，λ是K的一个特征根，
Figure BDA0002298301640000075
则是对应的特征向量，因此式(9)的结果是一种最优估计。
本领域技术人员应当理解，上述验证过程仅仅是一种验证本发明所述的方法的方式，并不作为本发明技术方案的一部分，也可以采用现有技术中其他的验证方式对本发明所述的方法进行验证。
如上针对一种实施例描述和/或示出的特征可以以相同或类似的方式在一个或更多个其它实施例中使用，和/或与其它实施例中的特征相结合或替代其它实施例中的特征使用。
应该强调，术语“包括/包含”在本文使用时指特征、整件、步骤或组件的存在，但并不排除一个或更多个其它特征、整件、步骤、组件或其组合的存在或附加。
本发明以上的方法可以由硬件实现，也可以由硬件结合软件实现。本发明涉及这样的计算机可读程序，当该程序被逻辑部件所执行时，能够使该逻辑部件实现上文所述的装置或构成部件，或使该逻辑部件实现上文所述的各种方法或步骤。本发明还涉及用于存储以上程序的存储介质，如硬盘、磁盘、光盘、DVD、flash存储器等。
这些实施例的许多特征和优点根据该详细描述是清楚的，因此所附权利要求旨在覆盖这些实施例的落入其真实精神和范围内的所有这些特征和优点。此外，由于本领域的技术人员容易想到很多修改和改变，因此不是要将本发明的实施例限于所例示和描述的精确结构和操作，而是可以涵盖落入其范围内的所有合适修改和等同物。
本发明未详细说明部分为本领域技术人员公知技术。

Claims (1)

1.一种初始姿态最优估计方法，其特征在于，该方法包括：
计算重力矢量在体坐标系下的测量值
Figure FDA0003692352780000011
和重力矢量在参考坐标系下的测量值
Figure FDA0003692352780000012
基于测量值
Figure FDA0003692352780000013
和测量值
Figure FDA0003692352780000014
计算损失函数构造矩阵K；
计算损失函数构造矩阵K的特征值λi和特征向量qi；
根据特征值λi和特征向量qi计算扩展矩阵H及其伴随矩阵H*；
基于伴随矩阵H*得到初始姿态最优解；
通过下式基于测量值
Figure FDA0003692352780000015
和测量值
Figure FDA0003692352780000016
计算损失函数构造矩阵K：
Figure FDA0003692352780000017
Figure FDA0003692352780000018
Figure FDA0003692352780000019
S＝B+BT，
Figure FDA00036923527800000110
其中，αi为权重系数，且
Figure FDA00036923527800000111
共n个系数，对应n组重力矢量测量值；
通过下式计算损失函数构造矩阵K的特征值λi和特征向量qi：
Figure FDA00036923527800000112
其中，i＝1,2,3,4，λi和qi分别表示4个特征值和4个特征向量；
通过下式根据特征值λi和特征向量qi计算扩展矩阵H：
Figure FDA0003692352780000021
通过下式计算伴随矩阵H*：
Figure FDA0003692352780000022
其中，λj,k,l为不同于λi的其他三个特征值；
基于伴随矩阵H*得到初始姿态最优解包括：
令伴随矩阵H*中λ＝λmax＝λ1，则伴随矩阵H*中除了第一项外全部为0，得到下式，并通过下式得到初始姿态最优解：
Figure FDA0003692352780000023
其中，qopt为初始姿态最优解，λmax为损失函数构造矩阵K的所有特征值中最大的一个值。
CN201911211612.4A 2019-12-02 2019-12-02 初始姿态最优估计方法 Active CN110986925B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911211612.4A CN110986925B (zh) 2019-12-02 2019-12-02 初始姿态最优估计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911211612.4A CN110986925B (zh) 2019-12-02 2019-12-02 初始姿态最优估计方法

Publications (2)

Publication Number Publication Date
CN110986925A CN110986925A (zh) 2020-04-10
CN110986925B true CN110986925B (zh) 2022-09-09

Family

ID=70089238

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911211612.4A Active CN110986925B (zh) 2019-12-02 2019-12-02 初始姿态最优估计方法

Country Status (1)

Country Link
CN (1) CN110986925B (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112197789B (zh) * 2020-08-14 2023-09-12 北京自动化控制设备研究所 一种基于quest的ins/dvl安装误差标定方法
CN112923923A (zh) * 2021-01-28 2021-06-08 深圳市瑞立视多媒体科技有限公司 一种imu与刚体姿态、位置对齐的方法、装置、设备以及可读存储介质
CN112945231A (zh) * 2021-01-28 2021-06-11 深圳市瑞立视多媒体科技有限公司 一种imu与刚体姿态对齐的方法、装置、设备以及可读存储介质

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105354171A (zh) * 2015-09-17 2016-02-24 哈尔滨工程大学 一种改进特征矢量的投影子空间估计自适应波束合成方法
CN105737858A (zh) * 2016-05-04 2016-07-06 北京航空航天大学 一种机载惯导系统姿态参数校准方法与装置
CN107609541A (zh) * 2017-10-17 2018-01-19 哈尔滨理工大学 一种基于可变形卷积神经网络的人体姿态估计方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20130245984A1 (en) * 2010-11-17 2013-09-19 Hillcrest Laboratories, Inc. Apparatuses and methods for magnetometer alignment calibration without prior knowledge of the local magnetic field

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105354171A (zh) * 2015-09-17 2016-02-24 哈尔滨工程大学 一种改进特征矢量的投影子空间估计自适应波束合成方法
CN105737858A (zh) * 2016-05-04 2016-07-06 北京航空航天大学 一种机载惯导系统姿态参数校准方法与装置
CN107609541A (zh) * 2017-10-17 2018-01-19 哈尔滨理工大学 一种基于可变形卷积神经网络的人体姿态估计方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
Yongbin zheng."Coarse Alignment Using Q Method".《2013 Chinese Automation Congress》.2013, *
翁浚等.车载动基座FOAM对准算法.《系统工程与电子技术》.2013,(第07期), *

Also Published As

Publication number Publication date
CN110986925A (zh) 2020-04-10

Similar Documents

Publication Publication Date Title
CN110986925B (zh) 初始姿态最优估计方法
Al-Sharadqah et al. Error analysis for circle fitting algorithms
Hartley Lines and points in three views—a unified approach
JP4325877B2 (ja) 行列の高速高精度特異値分解方法、プログラムおよび装置
JPH06325177A (ja) 画像位置合せのためのサブエリア自動選択方法
CN105809702A (zh) 一种基于Tsai算法的改进位姿估计方法
Biegler et al. Numerical experience with a reduced Hessian method for large scale constrained optimization
WO2024011779A1 (zh) 群孔测量摆角规划方法、可读介质和设备
Feng et al. Model detection and estimation for single-index varying coefficient model
WO2019178887A1 (zh) 基于函数迭代积分的刚体姿态解算方法及系统
CN110686610A (zh) 基于自适应网格的光学变形测量方法及电子设备
Dekel et al. Optimal least-squares solution to the hand-eye calibration problem
Augustin et al. About the importance of the Runge–Walsh concept for gravitational field determination
CN112613159A (zh) 一种Stewart平台6-UHU结构运动学求解及误差标定方法
CN108416811B (zh) 一种摄影机自标定方法及装置
CN108692727B (zh) 一种带有非线性补偿滤波器的捷联惯导系统
CN107356786B (zh) 加速度计的校准方法和装置、计算机可读存储介质
CN109489656B (zh) 基于旋量的星敏定姿方法
Aubry et al. Spectrum of hypersurfaces with small extrinsic radius or large λ1 in Euclidean spaces
Chandler On an iterative procedure for estimating functions when both variables are subject to error
JP2002297678A (ja) Ｖｏｘｅｌモデルの座標決定処理方法，座標決定処理プログラムおよび座標決定処理プログラム記録媒体
CN115070731B (zh) 一种面向并联机构的几何误差标定方法、系统和电子设备
CN107313766B (zh) 姿态数据校正方法和装置
CN115049813B (zh) 一种基于一阶球谐的粗配准方法、装置及系统
Wilkinson et al. The effects of viewing angle on the inference of magnetic shear in preflare active regions

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