CN114485641A - 一种基于惯导卫导方位融合的姿态解算方法及装置 - Google Patents

一种基于惯导卫导方位融合的姿态解算方法及装置 Download PDF

Info

Publication number
CN114485641A
CN114485641A CN202210083308.1A CN202210083308A CN114485641A CN 114485641 A CN114485641 A CN 114485641A CN 202210083308 A CN202210083308 A CN 202210083308A CN 114485641 A CN114485641 A CN 114485641A
Authority
CN
China
Prior art keywords
attitude
gyroscope
azimuth
value
acceleration
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
Application number
CN202210083308.1A
Other languages
English (en)
Other versions
CN114485641B (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.)
Wuhan Mengxin Technology Co ltd
Original Assignee
Wuhan Mengxin Technology Co ltd
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 Wuhan Mengxin Technology Co ltd filed Critical Wuhan Mengxin Technology Co ltd
Priority to CN202210083308.1A priority Critical patent/CN114485641B/zh
Publication of CN114485641A publication Critical patent/CN114485641A/zh
Application granted granted Critical
Publication of CN114485641B publication Critical patent/CN114485641B/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/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
    • G01C21/18—Stabilised platforms, e.g. by gyroscope
    • 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
    • G01C21/183—Compensation of inertial measurements, e.g. for temperature effects
    • G01C21/188—Compensation of inertial measurements, e.g. for temperature effects for accumulated errors, e.g. by coupling inertial systems with absolute positioning systems
    • 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/20—Instruments for performing navigational calculations

Abstract

本发明涉及一种基于惯导卫导方位融合的姿态解算方法及装置，属于姿态角测量技术领域；本发明采集载体静止状态的陀螺数据估算陀螺零偏、采集静止状态加速度数据进行水平对准，采用卫导方位角进行方位初始化并计算初始姿态转换矩阵；利用加速度输出在载体系的测量值和投影值，计算陀螺水平方向误差，利用卫导方位角测量值与估算值的偏差计算陀螺z轴误差，根据陀螺误差、零偏、陀螺测量值估算陀螺真值，并更新姿态转换矩阵；根据更新后的姿态转换矩阵计算姿态角。本发明对载体环境要求低，能够较好的代替磁力计进行融合解算，获得稳定可靠的姿态角数据。

Description

一种基于惯导卫导方位融合的姿态解算方法及装置
技术领域
本发明主要涉及姿态角测量技术领域，具体涉及一种基于惯导卫导方位融合的姿态解算方法及装置。
背景技术
姿态角是指物体的横滚角、俯仰角和方位角，用来表征载体的姿态，姿态角的测量在控制、机械、通信、航空航天等技术领域得到了广泛的应用，其主要利用惯性测量单元(Inertial Measurement Unit，IMU)进行测量。一般而言，IMU包含加速度计和陀螺仪，加速度计用于检测载体三轴的加速度，而陀螺仪用于检测载体三轴角速度信息，通过测量载体在三维空间中的角速度和加速度，并以此解算出物体的姿态。
在利用IMU解算姿态角信息时，陀螺仪自身的工作原理决定了其长时间的积分会导致误差的累积，使测量值随时间漂移，因此需要利用外部信息对陀螺仪的漂移进行修正。目前，在进行姿态解算时，主要使用加速度计和磁力计的数据修正陀螺仪的漂移。磁力计的原理是测量地球磁场，根据地磁在载体三轴上的分量确定载体朝向，但若使用的环境中存在除地球以外的磁场且这些磁场无法有效屏蔽，或离铁、镍、磁铁、发动机或其它磁性物质很近时，那么磁力计的使用就有很大的问题。
发明内容
本发明所要解决的技术问题是针对现有技术的不足，提供一种基于惯导卫导方位融合的姿态解算方法及装置。
本发明解决上述技术问题的技术方案如下：一种基于惯导卫导方位融合的姿态解算方法，包括如下步骤：
S1：通过惯导IMU器件的三轴陀螺仪和三轴加速度计，在载体坐标系中采集载体静止状态下多组陀螺仪输出数据及多组加速度输出数据；
S2：通过多组陀螺仪输出数据估算陀螺零偏，并通过多组加速度输出数据计算加速度均值，并根据所述加速度均值进行水平对准，并计算俯仰角和滚转角，并通过所述俯仰角和所述滚转角构建载体坐标系到水平系的初始姿态转换矩阵；
S3：采用卫导方位角对载体方位角进行初始化，并构建初始的姿态转换矩阵；
S4：根据各组加速度输出数据和投影值计算水平对准后三轴陀螺仪水平方向的误差值，并根据卫导方位角测量值与当前方位角估算陀螺z轴误差值；
S5：根据所述水平方向的误差值、陀螺零偏和陀螺z轴误差值估算陀螺真值，并根据所述陀螺真值更新所述姿态转换矩阵；
S6：根据更新后的姿态转换矩阵解算得到姿态角
本发明的有益效果是：对载体环境要求低，能够较好的代替磁力计进行融合解算，修正陀螺积分产生方位解偏差，获得稳定可靠的姿态角数据。
本发明解决上述技术问题的另一技术方案如下：一种基于惯导卫导方位融合的姿态解算装置，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，当所述处理器执行所述计算机程序时，实现如上所述的一种基于惯导卫导方位融合的姿态解算方法。
附图说明
图1为本发明实施例提供的姿态解算方法的流程示意图；
图2为本发明实施例提供的姿态解算方法的数据流向示意图。
具体实施方式
以下结合附图对本发明的原理和特征进行描述，所举实例只用于解释本发明，并非用于限定本发明的范围。
近年来卫星导航技术发展迅速，成本大幅下降，高精度卫导定位传感器和惯性测量单元(IMU)已经成为户外特定场景低速无人驾驶车辆、辅助驾驶车辆、位置形变监测场景必备的传感器，且这些使用场景中无法满足磁力计使用环境要求或无法满足磁力计校准条件。本发明利用卫导解算出的方位信息代替磁力计进行融合解算，修正陀螺积分产生的方位角漂移，得到准确的姿态角数据。
实施例1：
如图1-2所示，一种基于惯导卫导方位融合的姿态解算方法，包括如下步骤：
S1：通过惯导IMU器件的三轴陀螺仪和三轴加速度计，在载体坐标系中采集载体静止状态下多组陀螺仪输出数据及多组加速度输出数据；
S2：通过多组陀螺仪输出数据估算陀螺零偏，并通过多组加速度输出数据计算加速度均值，并根据所述加速度均值进行水平对准，并计算俯仰角和滚转角，并通过所述俯仰角和所述滚转角构建载体坐标系到水平系的姿态转换矩阵；
S3：采用卫导方位角对载体方位角进行初始化，并构建初始的姿态转换矩阵；
S4：根据各组加速度输出数据和投影值计算水平对准后三轴陀螺仪水平方向的误差值，并根据卫导方位角测量值与当前方位角估算陀螺z轴误差值；
S5：根据所述水平方向的误差值、陀螺零偏和陀螺z轴误差值估算陀螺真值，并根据所述陀螺真值更新所述姿态转换矩阵；
S6：根据更新后的姿态转换矩阵解算得到姿态角。
应理解地，惯导指的是IMU器件，IMU内有三轴陀螺仪和三轴加速度计，用于测量三轴角速度和三轴加速度。
上述实施例中，采集载体静止状态的陀螺数据估算陀螺零偏、采集静止状态加速度数据进行水平对准，采用卫导方位角进行方位初始化并计算初始姿态转换矩阵；利用加速度输出在载体系的测量值和投影值，计算陀螺水平方向误差，利用卫导方位角测量值与计算值的偏差估算陀螺z轴误差，根据陀螺误差、零偏、陀螺测量值估算陀螺真值，并更新姿态转换矩阵，根据更新后的姿态转换矩阵计算姿态角；本发明对载体环境的要求低，能够较好的代替磁力计进行融合解算，修正陀螺积分产生方位解偏差，获得稳定可靠的姿态角数据。
如图2所示，具体地，所述S2中通过多组陀螺仪输出数据估算陀螺零偏，并通过多组加速度输出数据计算加速度均值，具体包括：通过陀螺仪偏
公式和陀螺仪输出数据估算陀螺零偏，所述陀螺仪零偏公式为：
Figure BDA0003486779930000041
其中，
Figure BDA0003486779930000042
和
Figure BDA0003486779930000043
为相邻两组陀螺仪输出数据的平均值，且
Figure BDA0003486779930000044
与
Figure BDA0003486779930000045
差的绝对值小于设定的阀值，
Figure BDA0003486779930000046
n为正整数，
Figure BDA0003486779930000047
为陀螺仪输出的三轴角速度向量
Figure BDA0003486779930000048
通过加速度均值公式和加速度输出数据计算加速度均值，所述加速度均值公式为：
Figure BDA0003486779930000049
其中，
Figure BDA00034867799300000410
Figure BDA00034867799300000411
和
Figure BDA00034867799300000412
为相邻两组加速度数据的平均值，且
Figure BDA00034867799300000413
与
Figure BDA00034867799300000414
差的绝对值小于设定的阀值，
Figure BDA00034867799300000415
n为正整数，
Figure BDA00034867799300000416
为加速度输出的三轴加速度向量
Figure BDA00034867799300000417
根据加速度均值进行水平对准，并计算俯仰角和滚转角：
Figure BDA0003486779930000051
其中，θ为俯仰角，φ为滚转角。
通过所述俯仰角和所述滚转角构建载体坐标系到水平系的姿态转换矩阵，所述姿态转换矩阵为：
Figure BDA0003486779930000052
应理解地，三个姿态角与姿态转换矩阵为等价关系，有三个姿态角可计算姿态转换矩阵，有姿态转换矩阵可计算三个姿态角。此外这里只有两个角度，没有方位角，方位角记为0计算
Figure BDA0003486779930000053
在没有方位角的情况下，算法可正常运行计算准确的俯仰角和滚转角。
具体地，所述S3中构建载体坐标系到导航坐标系的初始姿态转换矩阵
Figure BDA0003486779930000054
具体包括：
当首次获得卫导方位角或距离上次获得卫导方位角的时间超过设定时间，则初始化或重置姿态方位角
Figure BDA0003486779930000055
Figure BDA0003486779930000056
为卫导方位角，并根据当前俯仰角θ、滚转角φ初始化或重置载体坐标系到导航坐标系的初始姿态转换矩阵
Figure BDA0003486779930000057
其中，
Figure BDA0003486779930000058
应理解地，采用卫导方位角与惯导IMU组合解算姿态角，卫导双天线与IMU器件需要固连在同一载体平台，且卫导双天线与IMU器件x轴平行，指向同一方向；若卫导双天线与IMU器件需要固连，但IMU器件方位角与卫导方位角存在固定方位偏差，在初始化和估算陀螺z轴误差值时需要用此固定方位偏差计算出IMU器件的方位后再进行初始化和误差估计。
具体地，所述S4中根据各组加速度输出数据和投影值计算水平对准后三轴陀螺仪水平方向的误差值，具体为：
根据陀螺误差值计算公式、各组加速度输出数据和投影值计算水平对准后三轴陀螺仪水平方向的误差值，所述陀螺误差值计算公式为：
Figure BDA0003486779930000061
其中，
Figure BDA0003486779930000062
Figure BDA0003486779930000063
为陀螺仪水平方向x轴、y轴的误差值，理想情况下
Figure BDA0003486779930000064
项计算结果为0，但由于实际误差的存在，该项为一个小值，为分离俯仰角、滚转角和航向角通道，避免耦合误差，
Figure BDA0003486779930000065
不计入陀螺z轴误差项；Kpa＝-0.5，Kia＝-0.1，
Figure BDA0003486779930000066
为重力加速度在载体坐标系投影矢量Gb与重力加速度在载体坐标系的测量值Gab的叉乘结果，投影矢量Gb＝[Gbx GbyGbx]T，测量矢量Gab＝[Gabx Gaby Gabx]T，对投影矢量和测量矢量进行归一化处理，记
Figure BDA0003486779930000067
归一化后的投影矢量和测量矢量分别记为Gbe、Gabe，则Gbe＝Gb/‖Gb‖，Gabe＝Gab/‖Gab‖，
记
Figure BDA0003486779930000068
具体地，所述S4中根据卫导方位角测量值与当前方位角估算陀螺z轴误差值，具体包括：
根据卫导方位角更新情况，选取Case1、Case2其中一种执行，
所述Case1为：
姿态方位角已初始化，且当前时刻获得卫导方位角
Figure BDA0003486779930000069
根据偏差公式、卫导方位角测量值和误差值估算陀螺仪z轴误差值
Figure BDA0003486779930000071
所述偏差公式为：
Figure BDA0003486779930000072
其中，
Figure BDA0003486779930000073
所述Case2为：
姿态方位角已初始化，且当前时刻卫导方位角
Figure BDA0003486779930000074
未更新，更新
Figure BDA0003486779930000075
基于更新后的
Figure BDA0003486779930000076
更新陀螺仪误差值
Figure BDA0003486779930000077
具体为：
Figure BDA0003486779930000078
其中，
Figure BDA0003486779930000079
Figure BDA00034867799300000710
为
Figure BDA00034867799300000711
的第i行第j列元素。
应理解地，每一次更新这两种情况的其中一种会发生。第一种是初始化后收到更新的方位角，第二种是因为一般情况下卫导方位角的更新频率低于IMU的更新频率，而算法的更新频率与IMU更新频率相同，在IMU数据更新而卫导方位角没有更新的时刻用case2。
如图2所示，具体地，所述S5具体包括：
根据估算公式所述水平方向的误差值、陀螺零偏和陀螺z轴误差值估算陀螺真值ωb，所述估算公式为：
Figure BDA00034867799300000712
其中，
Figure BDA00034867799300000713
为平滑后的陀螺仪输出数据，k-1和k为相邻时刻，
Figure BDA00034867799300000714
为陀螺零偏，中
Figure BDA00034867799300000715
Figure BDA00034867799300000716
分别为向量
Figure BDA00034867799300000717
的x轴和y轴分量，
Figure BDA00034867799300000718
为陀螺仪z轴误差，
计算一个采样周期Δt时间内的ωb的角度增量并更新
Figure BDA00034867799300000719
具体为：
Figure BDA00034867799300000720
其中，
Figure BDA00034867799300000721
为更新后的姿态转换矩阵，((ωb*Δt)×)表示向量V＝(ωb*Δt)＝[Vx Vy Vz]T构成的反对称矩阵，
Figure BDA0003486779930000081
具体地，所述S6具体包括：
对更新后的姿态转换矩阵进行单位化及正交化处理，得到处理后的姿态转换矩阵
Figure BDA0003486779930000082
根据第二公式组和姿态转换矩阵
Figure BDA0003486779930000083
得到姿态角数据，所述第二公式组为：
Figure BDA0003486779930000084
其中，
Figure BDA0003486779930000085
Figure BDA0003486779930000086
Figure BDA0003486779930000087
Figure BDA0003486779930000088
Figure BDA0003486779930000089
表示矩阵
Figure BDA00034867799300000810
的第i行第j列元素。
实施例2：
一种基于惯导卫导方位融合的姿态解算装置，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，当所述处理器执行所述计算机程序时，实现如上所述的一种基于惯导卫导方位融合的姿态解算方法。
需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。
所属领域的技术人员可以清楚地了解到，为了描述的方便和简洁，上述描述的系统和单元的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。
在本申请所提供的几个实施例中，应该理解到，所揭露的系统和方法，可以通过其它的方式实现。例如，以上所描述的系统实施例仅仅是示意性的，例如，单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。
作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本发明实施例方案的目的。
另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以是两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。
以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (8)

1.一种基于惯导卫导方位融合的姿态解算方法，其特征在于，包括如下步骤：
S1：通过惯导IMU器件的三轴陀螺仪和三轴加速度计，在载体坐标系中采集载体静止状态下多组陀螺仪输出数据及多组加速度输出数据；
S2：通过多组陀螺仪输出数据估算陀螺零偏，并通过多组加速度输出数据计算加速度均值，并根据所述加速度均值进行水平对准，并计算俯仰角和滚转角，并通过所述俯仰角和所述滚转角构建载体坐标系到水平系的初始姿态转换矩阵；
S3：采用卫导方位角对载体方位角进行初始化，并构建初始的姿态转换矩阵；
S4：根据各组加速度输出数据和投影值计算水平对准后三轴陀螺仪水平方向的误差值，并根据卫导方位角测量值与当前方位角估算陀螺z轴误差值；
S5：根据所述水平方向的误差值、陀螺零偏和陀螺z轴误差值估算陀螺真值，并根据所述陀螺真值更新所述姿态转换矩阵；
S6：根据更新后的姿态转换矩阵解算得到姿态角。
2.根据权利要求1所述的姿态解算方法，其特征在于，所述S2中通过多组陀螺仪输出数据估算陀螺零偏，并通过多组加速度输出数据计算加速度均值，具体包括：
通过陀螺仪零偏公式和陀螺仪输出数据估算陀螺零偏，所述陀螺仪零偏公式为：
Figure FDA0003486779920000011
其中，
Figure FDA0003486779920000012
和
Figure FDA0003486779920000013
为相邻两组陀螺仪输出数据的平均值，且
Figure FDA0003486779920000014
与
Figure FDA0003486779920000015
差的绝对值小于设定的阀值，
Figure FDA0003486779920000021
n为正整数，
Figure FDA0003486779920000022
为陀螺仪输出的三轴角速度向量
Figure FDA0003486779920000023
通过加速度均值公式和加速度输出数据计算加速度均值，所述加速度均值公式为：
Figure FDA0003486779920000024
其中，
Figure FDA0003486779920000025
Figure FDA0003486779920000026
和
Figure FDA0003486779920000027
为相邻两组加速度数据的平均值，且
Figure FDA0003486779920000028
与
Figure FDA0003486779920000029
差的绝对值小于设定的阀值，
Figure FDA00034867799200000210
n为正整数，
Figure FDA00034867799200000211
为加速度输出的三轴加速度向量
Figure FDA00034867799200000212
根据加速度均值进行水平对准，并计算俯仰角和滚转角：
Figure FDA00034867799200000213
其中，θ为俯仰角，φ为滚转角，
通过所述俯仰角和所述滚转角构建载体坐标系到水平系的初始姿态转换矩阵，所述初始姿态转换矩阵为：
Figure FDA00034867799200000214
3.根据权利要求2所述的姿态解算方法，其特征在于，所述S3中采用卫导方位角对载体方位角进行初始化，并构建载体坐标系到导航坐标系的初始姿态转换矩阵
Figure FDA00034867799200000215
具体为：
当首次获得卫导方位角或距离上次获得卫导方位角的时间超过设定时间，则初始化或重置姿态方位角
Figure FDA00034867799200000216
Figure FDA00034867799200000217
为卫导方位角，并根据当前俯仰角θ、滚转角φ初始化或重置载体坐标系到导航坐标系的初始姿态转换矩阵
Figure FDA00034867799200000218
其中，
Figure FDA00034867799200000219
4.根据权利要求3所述的姿态解算方法，其特征在于，所述S4中根据各组加速度输出数据和投影值计算水平对准后三轴陀螺仪水平方向的误差值，具体为：
根据陀螺误差值计算公式、各组加速度输出数据和投影值计算水平对准后三轴陀螺仪水平方向的误差值，所述陀螺误差值计算公式为：
Figure FDA0003486779920000031
其中，
Figure FDA0003486779920000032
为陀螺仪水平方向x轴、y轴的误差值，Kpa＝-0.5，Kia＝-0.1，
Figure FDA0003486779920000033
为重力加速度在载体坐标系投影矢量Gb与重力加速度在载体坐标系的测量值Gab的叉乘结果，投影矢量Gb＝[Gbx GbyGbx]T，测量矢量Gab＝[Gabx Gaby Gabx]T，对投影矢量和测量矢量进行归一化处理，记
Figure FDA0003486779920000034
归一化后的投影矢量和测量矢量分别记为Gbe、Gabe，则Gbe＝Gb/‖Gb‖，Gabe＝Gab/‖Gab‖，
记
Gbe＝[Gbxe Gbye Gbxe]T，Gabe＝
[Gabxe Gabye Gabxe]T，
Figure FDA0003486779920000037
Figure FDA0003486779920000038
5.根据权利要求4所述的姿态解算方法，其特征在于，所述S4中根据卫导方位角测量值与当前方位角估算陀螺z轴误差值，具体包括：
根据卫导方位角更新情况，选取Case1、Case2其中一种执行，
所述Case1为：
姿态方位角已初始化，且当前时刻获得
Figure FDA0003486779920000035
为卫导方位角，根据偏差公式、卫导方位角测量值和误差值估算陀螺仪z轴误差值
Figure FDA0003486779920000036
所述偏差公式为：
Figure FDA0003486779920000041
其中，
Figure FDA0003486779920000042
所述Case2为：
姿态方位角已初始化，且当前时刻
Figure FDA0003486779920000043
未更新，更新
Figure FDA0003486779920000044
基于更新后的
Figure FDA0003486779920000045
更新陀螺仪误差值
Figure FDA0003486779920000046
具体为：
Figure FDA0003486779920000047
其中，
Figure FDA0003486779920000048
Figure FDA0003486779920000049
为
Figure FDA00034867799200000410
的第i行第j列元素。
6.根据权利要求5所述的姿态解算方法，其特征在于，所述S5具体包括：
根据估算公式所述水平方向的误差值、陀螺零偏和陀螺z轴误差值估算陀螺真值ωb，所述估算公式为：
Figure FDA00034867799200000411
其中，
Figure FDA00034867799200000412
为平滑后的陀螺仪输出数据，k-1和k为相邻时刻，
Figure FDA00034867799200000413
为陀螺零偏，中
Figure FDA00034867799200000414
Figure FDA00034867799200000415
分别为向量
Figure FDA00034867799200000416
的x轴和y轴分量，
Figure FDA00034867799200000417
为陀螺仪z轴误差，
计算一个采样周期Δt时间内的ωb的角度增量并更新
Figure FDA00034867799200000418
具体为：
Figure FDA00034867799200000419
其中，
Figure FDA00034867799200000420
为更新后的姿态转换矩阵，((ωb*Δt)×表示向量V＝(ωb*Δt)＝[Vx Vy Vz]T构成的反对称矩阵，
Figure FDA00034867799200000421
7.根据权利要求6所述的姿态解算方法，其特征在于，所述S6具体包括：
对更新后的姿态转换矩阵进行单位化及正交化处理，得到处理后的姿态转换矩阵
Figure FDA0003486779920000051
根据第二公式组和姿态转换矩阵
Figure FDA0003486779920000052
得到姿态角数据，所述第二公式组为：
Figure FDA0003486779920000053
其中，
Figure FDA0003486779920000054
Figure FDA0003486779920000055
Figure FDA0003486779920000056
Figure FDA0003486779920000057
表示矩阵
Figure FDA0003486779920000058
的第i行第j列元素。
8.一种基于惯导卫导方位融合的姿态解算装置，其特征在于，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，当所述处理器执行所述计算机程序时，实现如权利要求1至7任一项所述的一种基于惯导卫导方位融合的姿态解算方法。
CN202210083308.1A 2022-01-24 2022-01-24 一种基于惯导卫导方位融合的姿态解算方法及装置 Active CN114485641B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210083308.1A CN114485641B (zh) 2022-01-24 2022-01-24 一种基于惯导卫导方位融合的姿态解算方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210083308.1A CN114485641B (zh) 2022-01-24 2022-01-24 一种基于惯导卫导方位融合的姿态解算方法及装置

Publications (2)

Publication Number Publication Date
CN114485641A true CN114485641A (zh) 2022-05-13
CN114485641B CN114485641B (zh) 2024-03-26

Family

ID=81474421

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210083308.1A Active CN114485641B (zh) 2022-01-24 2022-01-24 一种基于惯导卫导方位融合的姿态解算方法及装置

Country Status (1)

Country Link
CN (1) CN114485641B (zh)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114877892A (zh) * 2022-07-11 2022-08-09 泉州通维科技有限责任公司 一种用于光伏机器人的融合定位方法
CN115655270A (zh) * 2022-11-30 2023-01-31 湖南航天机电设备与特种材料研究所 一种惯组等效零位偏移误差补偿方法及系统
CN116499497A (zh) * 2023-06-28 2023-07-28 中国船舶集团有限公司第七〇七研究所 基于模态反转的半球谐振陀螺惯导系统误差自校准方法
CN115655270B (zh) * 2022-11-30 2024-04-19 湖南航天机电设备与特种材料研究所 一种惯组等效零位偏移误差补偿方法及系统

Citations (33)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2004239643A (ja) * 2003-02-03 2004-08-26 Furuno Electric Co Ltd ハイブリッド航法装置
US20050065727A1 (en) * 2003-09-20 2005-03-24 Guohui Hu Low cost multisensor high precision positioning and data integrated method and system thereof
CN102486377A (zh) * 2009-11-17 2012-06-06 哈尔滨工程大学 一种光纤陀螺捷联惯导系统初始航向的姿态获取方法
CN103674064A (zh) * 2013-12-06 2014-03-26 广东电网公司电力科学研究院 捷联惯性导航系统的初始标定方法
JP2014077769A (ja) * 2012-10-12 2014-05-01 Toyota Central R&D Labs Inc センサ傾斜判定装置及びプログラム
CN104374388A (zh) * 2014-11-10 2015-02-25 大连理工大学 一种基于偏振光传感器的航姿测定方法
RU2589495C1 (ru) * 2015-05-08 2016-07-10 Открытое акционерное общество Московский научно-производственный комплекс "Авионика" имени О.В. Успенского (ОАО МНПК "Авионика") Способ определения углов пространственной ориентации летательного аппарата и устройство для его осуществления
RU2603767C1 (ru) * 2015-07-31 2016-11-27 Акционерное общество "Специальное конструкторское бюро приборов подземной навигации" Способ автокомпенсации независящих от ускорения дрейфов гироскопического устройства
CN106950586A (zh) * 2017-01-22 2017-07-14 无锡卡尔曼导航技术有限公司 用于农机作业的gnss/ins/车辆组合导航方法
CN106990426A (zh) * 2017-03-16 2017-07-28 北京无线电计量测试研究所 一种导航方法和导航装置
DE102017102269A1 (de) * 2016-02-12 2017-08-17 Gm Global Technology Operations, Llc Neigungs- und fehlausrichtungsausgleich für 6-dof-imu unter verwendung von gnss-/ins-daten
CN107525503A (zh) * 2017-08-23 2017-12-29 王伟 基于双天线gps和mimu组合的自适应级联卡尔曼滤波方法
US20180017390A1 (en) * 2015-12-21 2018-01-18 Shanghai Huace Navigation Technology Ltd Method of determining gnss-ins vehicle attitude based on single antenna
CN108594283A (zh) * 2018-03-13 2018-09-28 杨勇 Gnss/mems惯性组合导航系统的自由安装方法
CN109001787A (zh) * 2018-05-25 2018-12-14 北京大学深圳研究生院 一种姿态角解算与定位的方法及其融合传感器
CN109163721A (zh) * 2018-09-18 2019-01-08 河北美泰电子科技有限公司 姿态测量方法及终端设备
CN109579870A (zh) * 2017-04-25 2019-04-05 广州市泰斗鑫信息科技有限公司 捷联惯导系统的自动对准方法和组合导航装置
CN109724579A (zh) * 2019-03-13 2019-05-07 中国工程物理研究院电子工程研究所 一种陀螺罗经标定方法、装置、计算设备及存储介质
CN110058288A (zh) * 2019-04-28 2019-07-26 北京微克智飞科技有限公司 无人机ins/gnss组合导航系统航向误差修正方法及系统
CN110332933A (zh) * 2019-07-09 2019-10-15 西安中兴物联软件有限公司 车辆定位方法、终端及计算机可读存储介质
CN110487300A (zh) * 2019-08-29 2019-11-22 南京航空航天大学 减振装置对惯性导航系统的性能影响测试方法
CN110672099A (zh) * 2019-09-09 2020-01-10 武汉元生创新科技有限公司 一种用于室内机器人导航的航向校正方法和系统
CN110764119A (zh) * 2019-11-07 2020-02-07 中国人民解放军火箭军工程大学 不依赖于卫星导航信息的卫星天线自主测控方法及系统
CN110779521A (zh) * 2019-11-12 2020-02-11 成都中科微信息技术研究院有限公司 一种多源融合的高精度定位方法与装置
CN111024071A (zh) * 2019-12-25 2020-04-17 东南大学 Gnss辅助的加速度计和陀螺仪常值漂移估算的导航方法及系统
CN111551175A (zh) * 2020-05-27 2020-08-18 北京计算机技术及应用研究所 一种航姿参考系统的互补滤波姿态解算方法
CN111776251A (zh) * 2020-07-21 2020-10-16 上海航天控制技术研究所 卫星的长期对日跟踪控制方法
CN111832690A (zh) * 2020-06-15 2020-10-27 中国人民解放军海军工程大学 基于粒子群优化算法的惯导系统的陀螺测量值计算方法
CN112378400A (zh) * 2020-10-30 2021-02-19 湖南航天机电设备与特种材料研究所 一种双天线gnss辅助的捷联惯导组合导航方法
CN112629538A (zh) * 2020-12-11 2021-04-09 哈尔滨工程大学 基于融合互补滤波和卡尔曼滤波的舰船水平姿态测量方法
CN112630813A (zh) * 2020-11-24 2021-04-09 中国人民解放军国防科技大学 基于捷联惯导和北斗卫星导航系统的无人机姿态测量方法
CN112797979A (zh) * 2020-12-31 2021-05-14 苏州精源创智能科技有限公司 一种应用于agv的惯性姿态导航系统
CN113587925A (zh) * 2021-07-16 2021-11-02 湖南航天机电设备与特种材料研究所 一种惯性导航系统及其全姿态导航解算方法与装置

Patent Citations (33)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2004239643A (ja) * 2003-02-03 2004-08-26 Furuno Electric Co Ltd ハイブリッド航法装置
US20050065727A1 (en) * 2003-09-20 2005-03-24 Guohui Hu Low cost multisensor high precision positioning and data integrated method and system thereof
CN102486377A (zh) * 2009-11-17 2012-06-06 哈尔滨工程大学 一种光纤陀螺捷联惯导系统初始航向的姿态获取方法
JP2014077769A (ja) * 2012-10-12 2014-05-01 Toyota Central R&D Labs Inc センサ傾斜判定装置及びプログラム
CN103674064A (zh) * 2013-12-06 2014-03-26 广东电网公司电力科学研究院 捷联惯性导航系统的初始标定方法
CN104374388A (zh) * 2014-11-10 2015-02-25 大连理工大学 一种基于偏振光传感器的航姿测定方法
RU2589495C1 (ru) * 2015-05-08 2016-07-10 Открытое акционерное общество Московский научно-производственный комплекс "Авионика" имени О.В. Успенского (ОАО МНПК "Авионика") Способ определения углов пространственной ориентации летательного аппарата и устройство для его осуществления
RU2603767C1 (ru) * 2015-07-31 2016-11-27 Акционерное общество "Специальное конструкторское бюро приборов подземной навигации" Способ автокомпенсации независящих от ускорения дрейфов гироскопического устройства
US20180017390A1 (en) * 2015-12-21 2018-01-18 Shanghai Huace Navigation Technology Ltd Method of determining gnss-ins vehicle attitude based on single antenna
DE102017102269A1 (de) * 2016-02-12 2017-08-17 Gm Global Technology Operations, Llc Neigungs- und fehlausrichtungsausgleich für 6-dof-imu unter verwendung von gnss-/ins-daten
CN106950586A (zh) * 2017-01-22 2017-07-14 无锡卡尔曼导航技术有限公司 用于农机作业的gnss/ins/车辆组合导航方法
CN106990426A (zh) * 2017-03-16 2017-07-28 北京无线电计量测试研究所 一种导航方法和导航装置
CN109579870A (zh) * 2017-04-25 2019-04-05 广州市泰斗鑫信息科技有限公司 捷联惯导系统的自动对准方法和组合导航装置
CN107525503A (zh) * 2017-08-23 2017-12-29 王伟 基于双天线gps和mimu组合的自适应级联卡尔曼滤波方法
CN108594283A (zh) * 2018-03-13 2018-09-28 杨勇 Gnss/mems惯性组合导航系统的自由安装方法
CN109001787A (zh) * 2018-05-25 2018-12-14 北京大学深圳研究生院 一种姿态角解算与定位的方法及其融合传感器
CN109163721A (zh) * 2018-09-18 2019-01-08 河北美泰电子科技有限公司 姿态测量方法及终端设备
CN109724579A (zh) * 2019-03-13 2019-05-07 中国工程物理研究院电子工程研究所 一种陀螺罗经标定方法、装置、计算设备及存储介质
CN110058288A (zh) * 2019-04-28 2019-07-26 北京微克智飞科技有限公司 无人机ins/gnss组合导航系统航向误差修正方法及系统
CN110332933A (zh) * 2019-07-09 2019-10-15 西安中兴物联软件有限公司 车辆定位方法、终端及计算机可读存储介质
CN110487300A (zh) * 2019-08-29 2019-11-22 南京航空航天大学 减振装置对惯性导航系统的性能影响测试方法
CN110672099A (zh) * 2019-09-09 2020-01-10 武汉元生创新科技有限公司 一种用于室内机器人导航的航向校正方法和系统
CN110764119A (zh) * 2019-11-07 2020-02-07 中国人民解放军火箭军工程大学 不依赖于卫星导航信息的卫星天线自主测控方法及系统
CN110779521A (zh) * 2019-11-12 2020-02-11 成都中科微信息技术研究院有限公司 一种多源融合的高精度定位方法与装置
CN111024071A (zh) * 2019-12-25 2020-04-17 东南大学 Gnss辅助的加速度计和陀螺仪常值漂移估算的导航方法及系统
CN111551175A (zh) * 2020-05-27 2020-08-18 北京计算机技术及应用研究所 一种航姿参考系统的互补滤波姿态解算方法
CN111832690A (zh) * 2020-06-15 2020-10-27 中国人民解放军海军工程大学 基于粒子群优化算法的惯导系统的陀螺测量值计算方法
CN111776251A (zh) * 2020-07-21 2020-10-16 上海航天控制技术研究所 卫星的长期对日跟踪控制方法
CN112378400A (zh) * 2020-10-30 2021-02-19 湖南航天机电设备与特种材料研究所 一种双天线gnss辅助的捷联惯导组合导航方法
CN112630813A (zh) * 2020-11-24 2021-04-09 中国人民解放军国防科技大学 基于捷联惯导和北斗卫星导航系统的无人机姿态测量方法
CN112629538A (zh) * 2020-12-11 2021-04-09 哈尔滨工程大学 基于融合互补滤波和卡尔曼滤波的舰船水平姿态测量方法
CN112797979A (zh) * 2020-12-31 2021-05-14 苏州精源创智能科技有限公司 一种应用于agv的惯性姿态导航系统
CN113587925A (zh) * 2021-07-16 2021-11-02 湖南航天机电设备与特种材料研究所 一种惯性导航系统及其全姿态导航解算方法与装置

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
CHOI等，: "The effects of using heading measurement during alignment of a low-cost IMU/GPS system", PROCEEDINGS OF THE 26TH INTERNATIONAL TECHNICAL MEETING OF THE SATELLITE DIVISION OF THE INSTITUTE OF NAVIGATION, pages 2175 - 2179 *
田源: "GNSS/INS定位测姿模型构建与算法研究", 中国优秀硕士学位论文全文数据库 基础科学辑, no. 2018, pages 008 - 85 *

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114877892A (zh) * 2022-07-11 2022-08-09 泉州通维科技有限责任公司 一种用于光伏机器人的融合定位方法
CN115655270A (zh) * 2022-11-30 2023-01-31 湖南航天机电设备与特种材料研究所 一种惯组等效零位偏移误差补偿方法及系统
CN115655270B (zh) * 2022-11-30 2024-04-19 湖南航天机电设备与特种材料研究所 一种惯组等效零位偏移误差补偿方法及系统
CN116499497A (zh) * 2023-06-28 2023-07-28 中国船舶集团有限公司第七〇七研究所 基于模态反转的半球谐振陀螺惯导系统误差自校准方法
CN116499497B (zh) * 2023-06-28 2023-09-05 中国船舶集团有限公司第七〇七研究所 基于模态反转的半球谐振陀螺惯导系统误差自校准方法

Also Published As

Publication number Publication date
CN114485641B (zh) 2024-03-26

Similar Documents

Publication Publication Date Title
US10061008B2 (en) Combined orbit and attitude determination system and methods
JP6094026B2 (ja) 姿勢判定方法、位置算出方法及び姿勢判定装置
CN108398128B (zh) 一种姿态角的融合解算方法和装置
CN109916395B (zh) 一种姿态自主冗余组合导航算法
CN114485641B (zh) 一种基于惯导卫导方位融合的姿态解算方法及装置
CN109596144B (zh) Gnss位置辅助sins行进间初始对准方法
CN107228674B (zh) 一种针对星敏感器和陀螺联合滤波的改进方法
WO2022160391A1 (zh) 磁力计信息辅助的mems陀螺仪标定方法及标定系统
CN103299247A (zh) 用于磁近场的动态追踪及补偿的设备和方法
US11408735B2 (en) Positioning system and positioning method
CN112562077B (zh) 一种融合pdr和先验地图的行人室内定位方法
CN111189442B (zh) 基于cepf的无人机多源导航信息状态预测方法
JP2012173190A (ja) 測位システム、測位方法
CN112683269B (zh) 一种附有运动加速度补偿的marg姿态计算方法
CN109631939B (zh) 一种基于磁强计和加速度计的快速对准方法
CN116817896B (zh) 一种基于扩展卡尔曼滤波的姿态解算方法
CN114002725A (zh) 一种车道线辅助定位方法、装置、电子设备及存储介质
CN108627152A (zh) 一种微型无人机基于多传感器数据融合的导航方法
JP2014240266A (ja) センサドリフト量推定装置及びプログラム
CN113566850B (zh) 惯性测量单元的安装角度标定方法、装置和计算机设备
CN108871319B (zh) 一种基于地球重力场与地磁场序贯修正的姿态解算方法
CN111141285B (zh) 一种航空重力测量装置
CN115143954A (zh) 一种基于多源信息融合的无人车导航方法
Kim et al. Enhanced outdoor localization of multi-GPS/INS fusion system using Mahalanobis Distance
CN109387220B (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