CN111551175A - 一种航姿参考系统的互补滤波姿态解算方法 - Google Patents

一种航姿参考系统的互补滤波姿态解算方法 Download PDF

Info

Publication number
CN111551175A
CN111551175A CN202010464589.6A CN202010464589A CN111551175A CN 111551175 A CN111551175 A CN 111551175A CN 202010464589 A CN202010464589 A CN 202010464589A CN 111551175 A CN111551175 A CN 111551175A
Authority
CN
China
Prior art keywords
attitude
calculating
output
acceleration
angle
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
CN202010464589.6A
Other languages
English (en)
Other versions
CN111551175B (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 Institute of Computer Technology and Applications
Original Assignee
Beijing Institute of Computer Technology and Applications
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 Institute of Computer Technology and Applications filed Critical Beijing Institute of Computer Technology and Applications
Priority to CN202010464589.6A priority Critical patent/CN111551175B/zh
Publication of CN111551175A publication Critical patent/CN111551175A/zh
Application granted granted Critical
Publication of CN111551175B publication Critical patent/CN111551175B/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/165—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 combined with non-inertial navigation instruments

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)

Abstract

本发明涉及一种航姿参考系统的互补滤波姿态解算方法，其中，包括：采集陀螺仪输出、加速度输出和磁强计输出，并进行归一化；利用第一组加速度计数据进行水平对准，计算俯仰角θ、滚转角γ和本体系到水平系的姿态转换矩阵
Figure DDA0002510118990000011
利用第一组磁强计数据进行方位对准，计算初始航向角
Figure DDA0002510118990000012
采集陀螺仪输出、加速度输出和磁强计输出数据；利用加速度和磁强计输出数据计算陀螺仪误差
Figure DDA0002510118990000013
计算当地导航系磁场强度参考值
Figure DDA0002510118990000014
计算加速度输出在导航系的投影值fn，计算加速度叉乘结果fX，计算磁矢量叉乘结果
Figure DDA0002510118990000015
计算误差
Figure DDA0002510118990000016
计算陀螺仪数据误差修正值
Figure DDA0002510118990000017
利用陀螺仪的输出修正后进行姿态更新；根据更新后的姿态信息计算姿态角。

Description

一种航姿参考系统的互补滤波姿态解算方法
技术领域
本发明涉及导航技术，特别涉及一种航姿参考系统的互补滤波姿态解算方法。
背景技术
随着智能制造和尖端芯片的发展，无人机在军用和民用领域都得到广泛使用。为了得到稳定的无人机飞行控制，导航系统成为不可或缺的机载设备之一。一般情况下，导航系统由惯性导航和卫星导航配合磁强计等传感器组成。正常情况下，导航系统能不间断提供位置、速度和姿态角(姿态角包括俯仰角、滚转角和航向角)。但由于卫星导航系统易受干扰造成信号中断，为了保证飞机的安全，需要采用航姿参考系统作为飞机姿态传感器。另外，对于一些消费级无人机或视距内的小型无人机，如果不需要获得位置信息，可以采用航姿参考系统作为姿态传感器，减少卫星导航带来的系统成本。
航姿参考系统由陀螺仪、加速度计、磁强计、相关电路、结构和线缆等组成，利用互补滤波算法，实现载体航向角、俯仰角和滚转角的解算。算法的原理是：没有位置和速度量测信息时，飞机会保持匀速直线运动或盘旋运动，线加速度变化较小，此时加速度计测量的信息为重力加速度和干扰加速度的叠加，利用陀螺仪输出的角速度积分可以得到系统的姿态角，利用磁强计输出计算导航坐标系磁场分量。忽略重力加速度在不同位置的变化和干扰加速度，利用导航系到本体系的姿态矩阵将导航系下重力加速度和磁场分量投影到本体坐标系上，与加速度计和磁强计实际输出作叉乘，得到的结果作为陀螺仪漂移修正本体系陀螺仪角速度输出，并进一步实现姿态更新。传统互补滤波姿态解算方法框图如图1所示。
其中，导航系的定义为：x轴和y轴在当地水平面，x轴指向正东，y轴指向正北，z轴垂直于当地水平面向上。本体系的定义为：y轴沿着载体纵轴指向前，x轴沿着载体横轴指向右，z轴符合右手定则。
目前，传统航姿参考系统的互补滤波姿态解算方法有两处缺陷。第一，根据磁场分布特性，东向磁场分量为0，传统互补滤波姿态解算方法将姿态角全部初始化为0，考虑到叉乘计算方法，当航向角为180°时，磁强计失去了对航向角的修正作用，存在修正盲点。特别是初始方位为180°时，航姿参考系统解算航向角将始终维持在0°，姿态角解算结果错误。第二，磁强计叉乘结果直接反馈到本体系的三个轴，在本体系下同时修正三个轴的陀螺仪，磁强计的测量误差耦合进俯仰角和滚转角的计算，造成俯仰角和滚转角的计算误差。
目前，现有的航姿参考系统的互补滤波姿态解算方法有两处缺陷。第一，根据磁场分布特性，东向磁场分量为0，传统互补滤波姿态解算方法将姿态角全部初始化为0，考虑到叉乘计算方法，当航向角为180°时，磁强计失去了对航向角的修正作用，存在修正盲点。特别是初始方位为180°时，航姿参考系统解算航向角将始终维持在0°，姿态角解算结果错误。第二，磁强计叉乘结果直接反馈到本体系的三个轴，在本体系下同时修正三个轴的陀螺仪，磁强计的测量误差耦合进俯仰角和滚转角的计算，造成俯仰角和滚转角的计算误差。
发明内容
本发明的目的在于提供一种航姿参考系统的互补滤波姿态解算方法，用于解决上述现有技术的问题。
本发明一种航姿参考系统的互补滤波姿态解算方法，其中，包括：步骤一：采集陀螺仪输出、加速度输出和磁强计输出，并进行归一化；步骤二：利用第一组加速度计数据进行水平对准，计算俯仰角θ、滚转角γ和本体系到水平系的姿态转换矩阵
Figure BDA0002510118970000031
第三步：利用第一组磁强计数据进行方位对准，计算初始航向角
Figure BDA0002510118970000032
并初始化四元数Qk＝[Qk,1 Qk,2 Qk,3Qk,4]T和本体系到导航系的姿态转换矩阵
Figure BDA0002510118970000033
第四步：采集陀螺仪输出、加速度输出和磁强计输出数据；第五步：利用加速度和磁强计输出数据计算陀螺仪误差
Figure BDA0002510118970000034
计算当地导航系磁场强度参考值
Figure BDA0002510118970000035
计算加速度输出在导航系的投影值fn，计算加速度叉乘结果fX，计算磁矢量叉乘结果
Figure BDA0002510118970000036
计算误差
Figure BDA0002510118970000037
第六步：计算陀螺仪数据误差修正值
Figure BDA0002510118970000038
第七步：利用陀螺仪的输出修正后进行姿态更新；第八步：根据更新后的姿态信息计算姿态角；第九步：重复第四步到第八步，直到采样时刻结束。
根据本发明的航姿参考系统的互补滤波姿态解算方法的一实施例，其中，第一步的计算过程具体包括：陀螺仪输出的角度增量为：
Figure BDA0002510118970000039
加速度计输出的速度增量为：
Figure BDA00025101189700000310
磁强计输出为：
Figure BDA00025101189700000311
归一化导航系重力加速度gn＝[0 0 1]T；
对采集的数据进行归一化为：
Figure BDA0002510118970000041
Figure BDA0002510118970000042
Figure BDA0002510118970000043
Figure BDA0002510118970000044
根据本发明的航姿参考系统的互补滤波姿态解算方法的一实施例，其中，利用第一组加速度计数据进行水平对准，计算俯仰角θ、滚转角γ和本体系到水平系的姿态转换矩阵
Figure BDA0002510118970000045
包括：
Figure BDA0002510118970000046
Figure BDA0002510118970000047
根据本发明的航姿参考系统的互补滤波姿态解算方法的一实施例，其中，第三步具体包括：
定义水平系为：导航系绕z轴转过航向角后得到的坐标系，记磁强计在水平系的分量为
Figure BDA0002510118970000048
Figure BDA0002510118970000049
计算航向角
Figure BDA00025101189700000410
Figure BDA00025101189700000411
初始化四元数Qk：
Figure BDA0002510118970000051
计算姿态转换矩阵
Figure BDA0002510118970000052
和
Figure BDA0002510118970000053
Figure BDA0002510118970000054
Figure BDA0002510118970000055
根据本发明的航姿参考系统的互补滤波姿态解算方法的一实施例，其中，第五步：利用加速度和磁强计输出数据计算陀螺仪误差
Figure BDA0002510118970000056
计算当地导航系磁场强度参考值
Figure BDA0002510118970000057
Figure BDA0002510118970000058
计算加速度输出在导航系的投影值fn：
Figure BDA0002510118970000059
计算加速度叉乘结果fX：
Figure BDA00025101189700000510
计算磁矢量叉乘结果
Figure BDA00025101189700000511
Figure BDA00025101189700000512
其中，符号×表示叉乘，即对于a＝[ax ay az]T，b＝[bx by bz]T
Figure BDA0002510118970000061
取加速度叉乘fX前两项和磁叉乘
Figure BDA0002510118970000062
第三项，作为误差
Figure BDA0002510118970000063
Figure BDA0002510118970000064
根据本发明的航姿参考系统的互补滤波姿态解算方法的一实施例，其中，第六步具体包括：设定比例系数kp＝10，积分系数ki＝0.01，初始采样时刻积分误差修正值
Figure BDA0002510118970000065
计算比例误差修正值
Figure BDA0002510118970000066
Figure BDA0002510118970000067
计算积分误差修正值
Figure BDA0002510118970000068
Figure BDA0002510118970000069
计算一个采样周期dT内陀螺仪误差修正值
Figure BDA00025101189700000610
Figure BDA00025101189700000611
根据本发明的航姿参考系统的互补滤波姿态解算方法的一实施例，其中，第七步具体包括：
一个采样周期dT内角度增量：
Figure BDA00025101189700000612
等效转过的角度：
Figure BDA00025101189700000613
采用三阶毕卡逼近更新当前采用时刻的四元数：Qk+1＝[Qk+1,1 Qk+1,2 Qk+1,3 Qk+1,4]T，并进行归一化；
Figure BDA00025101189700000614
其中：
Figure BDA0002510118970000071
根据本发明的航姿参考系统的互补滤波姿态解算方法的一实施例，其中，第八步：计算姿态角包括：
根据更新的四元数Qk+1更新转换矩阵
Figure BDA0002510118970000072
和
Figure BDA0002510118970000073
Figure BDA0002510118970000074
Figure BDA0002510118970000075
根据姿态矩阵计算姿态角：
Figure BDA0002510118970000076
其中：
Figure BDA0002510118970000077
为
Figure BDA0002510118970000078
的第i行第j列。
本发明针对目前航姿参考系统传统互补滤波姿态解算方法存在修正盲点和误差耦合的技术缺陷，设计了一种改进的互补滤波姿态解算方法，可实现任意姿态角下加速度计和磁强计对陀螺仪的角速度修正，同时在导航系下计算陀螺仪修正值，分离俯仰角、滚转角和航向角通道，避免了磁强计误差对俯仰角和滚转角的耦合误差。
附图说明
图1为现有的互补滤波姿态解算框图；
图2本发明的互补滤波姿态解算框图。
具体实施方式
为使本发明的目的、内容、和优点更加清楚，下面结合附图和实施例，对本发明的具体实施方式作进一步详细描述。
图2本发明的互补滤波姿态解算框图，如图2所示，航姿参考系统的互补滤波姿态解算方法包括：
第一步：采集陀螺仪输出、加速度输出和磁强计输出，并进行归一化：
陀螺仪输出的角度增量为：
Figure BDA0002510118970000081
加速度计输出的速度增量为：
Figure BDA0002510118970000082
磁强计输出为：
Figure BDA0002510118970000083
归一化导航系重力加速度gn＝[0 0 1]T。
对采集的数据进行归一化：
Figure BDA0002510118970000084
Figure BDA0002510118970000085
Figure BDA0002510118970000086
Figure BDA0002510118970000087
第二步：利用第一组加速度计数据进行水平对准，计算俯仰角θ、滚转角γ和本体系到水平系的姿态转换矩阵
Figure BDA0002510118970000088
Figure BDA0002510118970000091
Figure BDA0002510118970000092
第三步：利用第一组磁强计数据进行方位对准，计算初始航向角
Figure BDA0002510118970000093
并初始化四元数Qk＝[Qk,1 Qk,2 Qk,3 Qk,4]T和本体系到导航系的姿态转换矩阵
Figure BDA0002510118970000094
定义水平系为：导航系绕z轴转过航向角后得到的坐标系。记磁强计在水平系的分量为
Figure BDA0002510118970000095
Figure BDA0002510118970000096
计算航向角
Figure BDA0002510118970000097
Figure BDA0002510118970000098
初始化四元数Qk：
Figure BDA0002510118970000099
计算姿态转换矩阵
Figure BDA00025101189700000910
和
Figure BDA00025101189700000911
Figure BDA00025101189700000912
Figure BDA00025101189700000913
第四步：采集陀螺仪输出、加速度输出和磁强计输出数据。
第五步：利用加速度和磁强计输出数据计算陀螺仪误差
Figure BDA0002510118970000101
计算当地导航系磁场强度参考值
Figure BDA0002510118970000102
Figure BDA0002510118970000103
Figure BDA0002510118970000104
计算加速度输出在导航系的投影值fn：
Figure BDA0002510118970000105
计算加速度叉乘结果fX：
Figure BDA0002510118970000106
计算磁矢量叉乘结果
Figure BDA0002510118970000107
Figure BDA0002510118970000108
其中，符号×表示叉乘，即对于a＝[ax ay az]T，b＝[bx by bz]T
Figure BDA0002510118970000109
取加速度叉乘fX前两项和磁叉乘
Figure BDA00025101189700001010
第三项，作为角速度误差
Figure BDA00025101189700001011
Figure BDA00025101189700001012
第六步：计算陀螺仪数据误差修正值
Figure BDA00025101189700001013
设定比例系数kp＝10，积分系数ki＝0.01，初始采样时刻积分误差修正值
Figure BDA00025101189700001014
计算比例误差修正值
Figure BDA00025101189700001015
Figure BDA00025101189700001016
计算积分误差修正值
Figure BDA0002510118970000111
Figure BDA0002510118970000112
计算一个采样周期dT内陀螺仪误差修正值
Figure BDA0002510118970000113
Figure BDA0002510118970000114
第七步：利用陀螺仪的输出修正后进行姿态更新
一个采样周期dT内角度增量：
Figure BDA0002510118970000115
等效转过的角度：
Figure BDA0002510118970000116
采用三阶毕卡逼近更新当前采用时刻的四元数：Qk+1＝[Qk+1,1 Qk+1,2 Qk+1,3 Qk+1,4]T，并进行归一化。
Figure BDA0002510118970000117
Qk+1＝Qk+1/||Qk+1||
其中：
Figure BDA0002510118970000118
Figure BDA0002510118970000119
第八步：计算姿态角
根据更新的四元数Qk+1更新转换矩阵
Figure BDA00025101189700001110
和
Figure BDA00025101189700001111
Figure BDA0002510118970000121
Figure BDA0002510118970000122
根据姿态矩阵计算姿态角：
Figure BDA0002510118970000123
其中：
Figure BDA0002510118970000124
为
Figure BDA0002510118970000125
的第i行第j列。
第九步：重复第四步到第八步，直到采样时刻结束。
此时即完成了改进的互补滤波姿态解算。
本发明的关键点，利用初始对准实现初值姿态测量，避免了航向角的修正盲点；同时在导航系下计算陀螺仪修正值，避免了俯仰角、滚转角与航向角的交叉耦合，从而提高了姿态角的解算精度。
本发明设计了一种改进的互补滤波方法，该方法优点为：传统互补滤波姿态解算存在航向角修正盲点，同时，磁强计误差被耦合进俯仰角和滚转角的解算，造成了交叉耦合。本方法通过初始对准提供初始姿态角和初始四元数，可实现任意姿态角下陀螺仪的修正，保证了姿态角的正确性。同时，陀螺仪修正值计算更换到导航坐标系进行，方便分离加速度计通道和磁强计通道的耦合，提高了俯仰角和滚转角的测量精度。
本发明针对目前航姿参考系统传统互补滤波姿态解算方法，存在修正盲点和误差耦合的技术缺陷，设计了一种改进的互补滤波姿态解算方法，可实现任意姿态角下加速度计和磁强计对陀螺仪的角速度修正，同时在导航系下计算陀螺仪修正值，分离俯仰角、滚转角和航向角通道，避免了磁强计误差对俯仰角和滚转角的耦合误差。
以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变形，这些改进和变形也应视为本发明的保护范围。

Claims (8)

1.一种航姿参考系统的互补滤波姿态解算方法，其特征在于，包括：
步骤一：采集陀螺仪输出、加速度输出和磁强计输出，并进行归一化；
步骤二：利用第一组加速度计数据进行水平对准，计算俯仰角θ、滚转角γ和本体系到水平系的姿态转换矩阵
Figure FDA0002510118960000011
第三步：利用第一组磁强计数据进行方位对准，计算初始航向角
Figure FDA0002510118960000012
并初始化四元数Qk＝[Qk,1 Qk,2 Qk,3 Qk,4]T和本体系到导航系的姿态转换矩阵
Figure FDA0002510118960000013
第四步：采集陀螺仪输出、加速度输出和磁强计输出数据；
第五步：利用加速度和磁强计输出数据计算陀螺仪误差
Figure FDA0002510118960000014
计算当地导航系磁场强度参考值
Figure FDA0002510118960000015
计算加速度输出在导航系的投影值fn，计算加速度叉乘结果fX，计算磁矢量叉乘结果
Figure FDA0002510118960000016
计算误差
Figure FDA0002510118960000017
第六步：计算陀螺仪数据误差修正值
Figure FDA0002510118960000018
第七步：利用陀螺仪的输出修正后进行姿态更新；
第八步：根据更新后的姿态信息计算姿态角；
第九步：重复第四步到第八步，直到采样时刻结束。
2.如权利要求1所述的航姿参考系统的互补滤波姿态解算方法，其特征在于，第一步的计算过程具体包括：
陀螺仪输出的角度增量为：
Figure FDA0002510118960000019
加速度计输出的速度增量为：
Figure FDA00025101189600000110
磁强计输出为：
Figure FDA0002510118960000021
归一化导航系重力加速度gn＝[0 0 1]T；
对采集的数据进行归一化为：
Figure FDA0002510118960000022
Figure FDA0002510118960000023
Figure FDA0002510118960000024
Figure FDA0002510118960000025
3.如权利要求2所述的航姿参考系统的互补滤波姿态解算方法，其特征在于，利用第一组加速度计数据进行水平对准，计算俯仰角θ、滚转角γ和本体系到水平系的姿态转换矩阵
Figure FDA0002510118960000026
包括：
Figure FDA0002510118960000027
Figure FDA0002510118960000028
4.如权利要求3所述的航姿参考系统的互补滤波姿态解算方法，其特征在于，第三步具体包括：
定义水平系为：导航系绕z轴转过航向角后得到的坐标系，记磁强计在水平系的分量为
Figure FDA0002510118960000029
Figure FDA00025101189600000210
计算航向角
Figure FDA00025101189600000211
Figure FDA00025101189600000212
初始化四元数Qk：
Figure FDA0002510118960000031
计算姿态转换矩阵
Figure FDA0002510118960000032
和
Figure FDA0002510118960000033
Figure FDA0002510118960000034
Figure FDA0002510118960000035
5.如权利要求4所述的航姿参考系统的互补滤波姿态解算方法，其特征在于，
第五步：利用加速度和磁强计输出数据计算陀螺仪误差
Figure FDA0002510118960000036
计算当地导航系磁场强度参考值
Figure FDA0002510118960000037
Figure FDA0002510118960000038
计算加速度输出在导航系的投影值fn：
Figure FDA0002510118960000039
计算加速度叉乘结果fX：
Figure FDA00025101189600000310
计算磁矢量叉乘结果
Figure FDA00025101189600000311
Figure FDA00025101189600000312
其中，符号×表示叉乘，即对于a＝[ax ay az]T，b＝[bx by bz]T
Figure FDA0002510118960000041
取加速度叉乘fX前两项和磁叉乘
Figure FDA0002510118960000042
第三项，作为误差
Figure FDA0002510118960000043
Figure FDA0002510118960000044
6.如权利要求5所述的航姿参考系统的互补滤波姿态解算方法，其特征在于，第六步具体包括：：
设定比例系数kp＝10，积分系数ki＝0.01，初始采样时刻积分误差修正值
Figure FDA0002510118960000045
计算比例误差修正值
Figure FDA0002510118960000046
Figure FDA0002510118960000047
计算积分误差修正值
Figure FDA0002510118960000048
Figure FDA0002510118960000049
计算一个采样周期dT内陀螺仪误差修正值
Figure FDA00025101189600000410
Figure FDA00025101189600000411
7.如权利要求6所述的航姿参考系统的互补滤波姿态解算方法，其特征在于，第七步具体包括：
一个采样周期dT内角度增量：
Figure FDA00025101189600000412
等效转过的角度：
Figure FDA00025101189600000413
采用三阶毕卡逼近更新当前采用时刻的四元数：Qk+1＝[Qk+1,1 Qk+1,2 Qk+1,3 Qk+1,4]T，并进行归一化；
Figure FDA0002510118960000051
Qk+1＝Qk+1/||Qk+1||
其中：
Figure FDA0002510118960000052
8.如权利要求1所述的航姿参考系统的互补滤波姿态解算方法，其特征在于，第八步：计算姿态角包括：
根据更新的四元数Qk+1更新转换矩阵
Figure FDA0002510118960000053
和
Figure FDA0002510118960000054
Figure FDA0002510118960000055
Figure FDA0002510118960000056
根据姿态矩阵计算姿态角：
Figure FDA0002510118960000057
其中：
Figure FDA0002510118960000058
为
Figure FDA0002510118960000059
的第i行第j列。
CN202010464589.6A 2020-05-27 2020-05-27 一种航姿参考系统的互补滤波姿态解算方法 Active CN111551175B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010464589.6A CN111551175B (zh) 2020-05-27 2020-05-27 一种航姿参考系统的互补滤波姿态解算方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010464589.6A CN111551175B (zh) 2020-05-27 2020-05-27 一种航姿参考系统的互补滤波姿态解算方法

Publications (2)

Publication Number Publication Date
CN111551175A true CN111551175A (zh) 2020-08-18
CN111551175B CN111551175B (zh) 2024-03-15

Family

ID=72006739

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010464589.6A Active CN111551175B (zh) 2020-05-27 2020-05-27 一种航姿参考系统的互补滤波姿态解算方法

Country Status (1)

Country Link
CN (1) CN111551175B (zh)

Cited By (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112066984A (zh) * 2020-09-17 2020-12-11 深圳维特智能科技有限公司 姿态角度解算方法、装置、处理设备和存储介质
CN112179380A (zh) * 2020-09-25 2021-01-05 西北工业大学 一种动态情况下速率陀螺误差补偿方法
CN112630813A (zh) * 2020-11-24 2021-04-09 中国人民解放军国防科技大学 基于捷联惯导和北斗卫星导航系统的无人机姿态测量方法
CN113063416A (zh) * 2021-02-05 2021-07-02 重庆大学 一种基于自适应参数互补滤波的机器人姿态融合方法
CN113532477A (zh) * 2021-07-15 2021-10-22 青岛迈金智能科技有限公司 一种骑行码表设备及骑行码表初始姿态自动校准方法
CN114485641A (zh) * 2022-01-24 2022-05-13 武汉梦芯科技有限公司 一种基于惯导卫导方位融合的姿态解算方法及装置
CN115855038A (zh) * 2022-11-22 2023-03-28 哈尔滨工程大学 一种短时高精度姿态保持方法
CN116817896A (zh) * 2023-04-03 2023-09-29 盐城数智科技有限公司 一种基于扩展卡尔曼滤波的姿态解算方法

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102654404A (zh) * 2011-03-02 2012-09-05 浙江中科无线授时与定位研发中心 一种提高航姿参考系统解算精度和系统抗干扰能力的方法
CN105698792A (zh) * 2016-01-26 2016-06-22 上海实汇机电科技有限公司 一种基于自适应鲁邦融合算法的动态mems惯性姿态测量系统
US20170363428A1 (en) * 2016-06-20 2017-12-21 Atlantic Inertial Systems Limited Inertial navigation system
CN108592918A (zh) * 2018-04-26 2018-09-28 清华大学 摇摆基座下mems imu的全姿态解算方法
CN109163721A (zh) * 2018-09-18 2019-01-08 河北美泰电子科技有限公司 姿态测量方法及终端设备
CN111024091A (zh) * 2019-12-31 2020-04-17 北京智新翼行科技有限公司 视觉辅助微型无人机室内飞行三维姿态算法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102654404A (zh) * 2011-03-02 2012-09-05 浙江中科无线授时与定位研发中心 一种提高航姿参考系统解算精度和系统抗干扰能力的方法
CN105698792A (zh) * 2016-01-26 2016-06-22 上海实汇机电科技有限公司 一种基于自适应鲁邦融合算法的动态mems惯性姿态测量系统
US20170363428A1 (en) * 2016-06-20 2017-12-21 Atlantic Inertial Systems Limited Inertial navigation system
CN108592918A (zh) * 2018-04-26 2018-09-28 清华大学 摇摆基座下mems imu的全姿态解算方法
CN109163721A (zh) * 2018-09-18 2019-01-08 河北美泰电子科技有限公司 姿态测量方法及终端设备
CN111024091A (zh) * 2019-12-31 2020-04-17 北京智新翼行科技有限公司 视觉辅助微型无人机室内飞行三维姿态算法

Cited By (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112066984A (zh) * 2020-09-17 2020-12-11 深圳维特智能科技有限公司 姿态角度解算方法、装置、处理设备和存储介质
CN112179380B (zh) * 2020-09-25 2022-09-30 西北工业大学 一种动态情况下速率陀螺误差补偿方法
CN112179380A (zh) * 2020-09-25 2021-01-05 西北工业大学 一种动态情况下速率陀螺误差补偿方法
CN112630813A (zh) * 2020-11-24 2021-04-09 中国人民解放军国防科技大学 基于捷联惯导和北斗卫星导航系统的无人机姿态测量方法
CN112630813B (zh) * 2020-11-24 2024-05-03 中国人民解放军国防科技大学 基于捷联惯导和北斗卫星导航系统的无人机姿态测量方法
CN113063416A (zh) * 2021-02-05 2021-07-02 重庆大学 一种基于自适应参数互补滤波的机器人姿态融合方法
CN113063416B (zh) * 2021-02-05 2023-08-08 重庆大学 一种基于自适应参数互补滤波的机器人姿态融合方法
CN113532477A (zh) * 2021-07-15 2021-10-22 青岛迈金智能科技有限公司 一种骑行码表设备及骑行码表初始姿态自动校准方法
CN114485641A (zh) * 2022-01-24 2022-05-13 武汉梦芯科技有限公司 一种基于惯导卫导方位融合的姿态解算方法及装置
CN114485641B (zh) * 2022-01-24 2024-03-26 武汉梦芯科技有限公司 一种基于惯导卫导方位融合的姿态解算方法及装置
CN115855038A (zh) * 2022-11-22 2023-03-28 哈尔滨工程大学 一种短时高精度姿态保持方法
CN116817896A (zh) * 2023-04-03 2023-09-29 盐城数智科技有限公司 一种基于扩展卡尔曼滤波的姿态解算方法
CN116817896B (zh) * 2023-04-03 2024-04-16 盐城数智科技有限公司 一种基于扩展卡尔曼滤波的姿态解算方法

Also Published As

Publication number Publication date
CN111551175B (zh) 2024-03-15

Similar Documents

Publication Publication Date Title
CN111551175B (zh) 一种航姿参考系统的互补滤波姿态解算方法
CN111351482B (zh) 基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法
CN102692225B (zh) 一种用于低成本小型无人机的姿态航向参考系统
US4930085A (en) Method for determining the heading of an aircraft
CN111780756A (zh) 车辆航位推算方法、装置、设备以及存储介质
CN111207745B (zh) 一种适用于大机动无人机垂直陀螺仪的惯性测量方法
CN105203098A (zh) 基于九轴mems传感器的农业机械全姿态角更新方法
CN103063216B (zh) 一种基于星像坐标建模的惯性与天文组合导航方法
JP2011227017A (ja) 慣性センサ，磁気センサおよび速度計を用いた移動体の姿勢推定装置および姿勢推定方法
CN105865455B (zh) 一种利用gps与加速度计计算飞行器姿态角的方法
CN105157705A (zh) 一种半捷联雷达导引头视线角速度提取方法
CN106767767A (zh) 一种微纳多模星敏感器系统及其数据融合方法
CN110296719B (zh) 一种在轨标定方法
CN111238469B (zh) 一种基于惯性/数据链的无人机编队相对导航方法
CN111189442A (zh) 基于cepf的无人机多源导航信息状态预测方法
CN105928515A (zh) 一种无人机导航系统
CN107764268B (zh) 一种机载分布式pos传递对准的方法和装置
CN113703019B (zh) 一种导航系统的故障处理方法、电子设备及存储介质
CN114111768B (zh) 一种基于偏角异常补偿的轨道线型拟合方法
CN115950419A (zh) 超小型无人机用组合导航方法、装置及系统
CN110286688B (zh) 一种带有机械臂的水下四旋翼无人机控制方法
Toda et al. Simulation design of thermopile and magnetometer aided INS/GPS navigation system for UAV navigation
CN113447025A (zh) 基于克雷洛夫角的惯性导航高精度姿态角解算方法和系统
CN112649823A (zh) 无人机导航定位方法及装置
Kamali et al. Multiplicative error state Kalman filter vs nonlinear complimentary filter for a high performance aircraft attitude estimation

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