CN112179342A - 一种估计无人机姿态的方法及系统 - Google Patents

一种估计无人机姿态的方法及系统 Download PDF

Info

Publication number
CN112179342A
CN112179342A CN202010998123.4A CN202010998123A CN112179342A CN 112179342 A CN112179342 A CN 112179342A CN 202010998123 A CN202010998123 A CN 202010998123A CN 112179342 A CN112179342 A CN 112179342A
Authority
CN
China
Prior art keywords
time
attitude
gyroscope
accelerometer
value
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
CN202010998123.4A
Other languages
English (en)
Other versions
CN112179342B (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.)
China Academy of Safety Science and Technology CASST
Original Assignee
China Academy of Safety Science and Technology CASST
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 China Academy of Safety Science and Technology CASST filed Critical China Academy of Safety Science and Technology CASST
Priority to CN202010998123.4A priority Critical patent/CN112179342B/zh
Publication of CN112179342A publication Critical patent/CN112179342A/zh
Application granted granted Critical
Publication of CN112179342B publication Critical patent/CN112179342B/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
    • G01C1/00—Measuring angles

Landscapes

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

Abstract

本发明公开了一种估计无人机姿态的方法，所述方法包括：通过互补滤波器对加速计在t时刻的姿态估计值
Figure DDA0002693313950000011
和陀螺仪在t时刻的姿态估计值
Figure DDA0002693313950000012
进行融合处理，得到t时刻的无人机姿态估计值
Figure DDA0002693313950000013
Figure DDA0002693313950000017
0≤kt≤1，其中，kt是所述陀螺仪在t时刻的姿态估计值
Figure DDA0002693313950000014
的发散速率β的加权系数，1‑kt是所述加速计在t时刻的姿态估计值
Figure DDA0002693313950000015
的收敛速率的加权系数；通过所述加速度计在t时刻的误差ea,t对所述姿态估计值
Figure DDA0002693313950000016
的发散速率β实时调整，β＝2‑||ea,t||。本发明还公开了一种估计无人机姿态的系统。本发明能够自适应地调整参数，准确估计出无人机的真实姿态，同时解决了人为调整参数耗时耗力的问题，可以很好地应用于各种飞行载体的姿态估计。

Description

一种估计无人机姿态的方法及系统
技术领域
本发明涉及无人机技术领域，具体而言，涉及一种估计无人机姿态的方法及系统。
背景技术
随着无人机在快递运输、救灾、环境监测和电力巡检等各个领域的广泛应用，其准确的飞行姿态测量变得越来越重要。当前无人机通常使用由陀螺仪和加速度计组成的低成本惯性测量单元(IMU)进行飞行姿态测量，但低成本陀螺仪具有积分累积误差，且加速度计在动态姿态测量中，容易受到高频噪声的影响，导致无人机冬天姿态测量精度较低。
针对上述问题，国内外许多学者进行了大量的研究工作。目前，主流姿态估计方法是基于卡尔曼滤波和互补滤波，或是两种结合。线性卡尔曼滤波可以降低滤波器的复杂度，但是姿态估计准确度不高。互补卡尔曼滤波需要计算大量的三角函数，并且在计算中容易产生奇异值。扩展卡尔曼滤波，无损卡尔曼滤波，粒子滤波等也被用于姿态估计，在一定程度上提高了姿态估计的准确性。但是，卡尔曼滤波容易出现发散问题，且噪声模型设计和协方差计算等也存在一些理论和实际问题。相关技术中的互补滤波由于增益是定量且不能动态调整，因此当无人机移动太快而外部干扰太大时，估计精度较差。另有基于高斯牛顿法和互补滤波的方法，虽然可以有效抑制电磁干扰和动态条件下的姿态角估计误差，但在姿态估计过程中增加了计算量。
发明内容
为解决上述问题，本发明的目的在于提供一种估计无人机姿态的方法及系统，能够自适应地调整参数，准确估计出无人机的真实姿态，同时解决了人为调整参数耗时耗力的问题，可以很好地应用于各种飞行载体的姿态估计。
本发明提供了一种估计无人机姿态的方法，所述方法包括：
通过互补滤波器对加速计在t时刻的姿态估计值
Figure BDA0002693313930000011
和陀螺仪在t时刻的姿态估计值
Figure BDA0002693313930000012
进行融合处理，得到t时刻的无人机姿态估计值
Figure BDA0002693313930000013
Figure BDA0002693313930000021
其中，kt是所述陀螺仪在t时刻的姿态估计值
Figure BDA0002693313930000022
的发散速率β的加权系数，1-kt是所述加速计在t时刻的姿态估计值
Figure BDA0002693313930000023
的收敛速率的加权系数；
通过所述加速度计在t时刻的误差ea,t对所述姿态估计值
Figure BDA0002693313930000024
的发散速率β实时调整，β＝2-||ea,t||。
作为本发明进一步的改进，采用梯度下降法获取所述加速计在t时刻的姿态估计值
Figure BDA0002693313930000025
梯度方向的步长为μt，所述互补滤波器的采样时间为Δt，
其中，所述通过互补滤波器对加速计在t时刻的姿态估计值
Figure BDA0002693313930000026
和陀螺仪在t时刻的姿态估计值
Figure BDA0002693313930000027
进行融合处理，得到t时刻的无人机姿态估计值
Figure BDA0002693313930000028
包括：
确定所述姿态估计值
Figure BDA0002693313930000029
的收敛速率
Figure BDA00026933139300000210
根据所述陀螺仪在t时刻的姿态估计值
Figure BDA00026933139300000211
的加权发散速率与所述加速计在t时刻的姿态估计值
Figure BDA00026933139300000212
的加权收敛速率，确定所述互补滤波器融合处理的加权系数，
Figure BDA00026933139300000213
作为本发明进一步的改进，加速度计在t时刻的加速度测量值bat用四元数bat＝[0ax ay az]表示，其中，ax、ay、az分别表示在无人机坐标系中x轴、y轴、z轴的加速度测量值分量，所述加速度测量值bat归一化后的加速度向量用
Figure BDA00026933139300000214
表示，
其中，所述采用梯度下降法获取所述加速计在t时刻的姿态估计值
Figure BDA00026933139300000215
包括：
基于所述加速度向量
Figure BDA00026933139300000216
和t时刻从无人机坐标系变换到导航坐标系的归一化方向向量
Figure BDA00026933139300000217
构建目标函数
Figure BDA00026933139300000218
Figure BDA00026933139300000219
其中，
Figure BDA00026933139300000220
为无人机坐标系中归一化向量
Figure BDA00026933139300000221
的参考方向，
Figure BDA00026933139300000222
Figure BDA00026933139300000223
为导航坐标系中重力向量g的归一化向量，
Figure BDA00026933139300000224
Figure BDA00026933139300000225
为
Figure BDA00026933139300000226
的共轭四元数
Figure BDA00026933139300000227
通过梯度下降法确定所述目标函数
Figure BDA00026933139300000228
的最小值，并将最小值作为所述加速计在t时刻的姿态估计值
Figure BDA00026933139300000229
作为本发明进一步的改进，所述通过梯度下降法确定所述目标函数
Figure BDA0002693313930000031
的最小值，包括：
根据梯度下降法，对
Figure BDA0002693313930000032
迭代求解，确定所述加速计在t时刻的姿态估计值
Figure BDA0002693313930000033
其中，
Figure BDA0002693313930000034
为所述互补滤波器在t-Δt时刻融合得到的无人机姿态估计值，Δt为所述互补滤波器的采样时间，
Figure BDA0002693313930000035
为所述目标函数
Figure BDA0002693313930000036
的梯度值，
Figure BDA0002693313930000037
J表示雅可比行列式，
Figure BDA0002693313930000038
Figure BDA0002693313930000039
为所述陀螺仪在t时刻的姿态估计值
Figure BDA00026933139300000310
的时间导数，α为比例因子。
作为本发明进一步的改进，所述方法还包括：
基于陀螺仪在t时刻的角度漂移值bωd,t对陀螺仪在t时刻的角速度测量值bωt进行补偿，得到补偿后的角速度值bωc,t；
基于所述补偿后的角速度值bωc,t和互补滤波器在t-Δt时刻融合得到的无人机姿态估计值
Figure BDA00026933139300000311
得到所述陀螺仪在t时刻的姿态估计值
Figure BDA00026933139300000312
其中，Δt为所述互补滤波器的采样时间。
作为本发明进一步的改进，所述角速度测量值用四元数bωt＝[0 ωx ωy ω表示，其中，ωx、ωy、ωz分别表示在无人机坐标系中x轴、y轴、z轴的角速度测量值分量，
其中，基于陀螺仪在t时刻的角度漂移值bωd,t对陀螺仪在t时刻的角速度测量值bωt进行补偿，得到补偿后的角速度值bωc,t，基于所述补偿后的角速度值bωc,t和互补滤波器在t-Δt时刻融合得到的无人机姿态估计值
Figure BDA00026933139300000313
得到所述陀螺仪在t时刻的姿态估计值
Figure BDA00026933139300000314
包括：
基于所述陀螺仪在t时刻的角度漂移值bωd,t对所述角速度测量值bωt进行补偿，确定补偿后的角速度值bωc,t，bωc,t＝bωt-bωd,t；
基于所述补偿后的角速度值bωc,t和所述互补滤波器在t-Δt时刻融合得到的无人机姿态估计值
Figure BDA00026933139300000315
确定所述陀螺仪在t时刻的姿态估计值
Figure BDA00026933139300000316
的时间导数
Figure BDA00026933139300000317
Figure BDA00026933139300000318
基于所述陀螺仪在t时刻的姿态估计值
Figure BDA00026933139300000319
的时间导数
Figure BDA00026933139300000320
确定所述陀螺仪在t时刻的姿态估计值
Figure BDA00026933139300000321
Figure BDA00026933139300000322
并将所述补偿后的角速度值bωc,t作为观测向量。
作为本发明进一步的改进，通过所述陀螺仪在t时刻的误差bωe,t
和漂移补偿参数ζ，确定所述陀螺仪在t时刻的角度漂移值bωd,t，
Figure BDA0002693313930000041
其中，
Figure BDA0002693313930000042
Figure BDA0002693313930000043
为t时刻的无人机姿态估计值
Figure BDA0002693313930000044
的时间导；
所述漂移补偿参数ζ表示消除所述陀螺仪非零平均测量误差的收敛速度：
Figure BDA0002693313930000045
其中，
Figure BDA0002693313930000046
为所述陀螺仪三轴漂移的估计速率，
Figure BDA0002693313930000047
为无人机坐标系相对于导航坐标系的方向矢量的估计值。
本发明还提供了一种估计无人机姿态的系统，所述系统包括：
融合估计模块，用于通过互补滤波器对加速计在t时刻的姿态估计值
Figure BDA0002693313930000048
和陀螺仪在t时刻的姿态估计值
Figure BDA0002693313930000049
进行融合处理，得到t时刻的无人机姿态估计值
Figure BDA00026933139300000410
Figure BDA00026933139300000411
其中，kt是所述陀螺仪在t时刻的姿态估计值
Figure BDA00026933139300000412
的发散速率β的加权系数，1-kt是所述加速计在t时刻的姿态估计值
Figure BDA00026933139300000413
的收敛速率的加权系数；
参数调整模块，用于通过所述加速度计在t时刻的误差ea,t对所述姿态估计值
Figure BDA00026933139300000414
的发散速率β实时调整，β＝2-||ea,t||。
作为本发明进一步的改进，所述融合估计模块采用梯度下降法获取所述加速计在t时刻的姿态估计值
Figure BDA00026933139300000415
梯度方向的步长为μt，所述互补滤波器的采样时间为Δt，
其中，所述通过互补滤波器对加速计在t时刻的姿态估计值
Figure BDA00026933139300000416
和陀螺仪在t时刻的姿态估计值
Figure BDA00026933139300000417
进行融合处理，得到t时刻的无人机姿态估计值
Figure BDA00026933139300000418
包括：
确定所述姿态估计值
Figure BDA00026933139300000419
的收敛速率
Figure BDA00026933139300000420
根据所述陀螺仪在t时刻的姿态估计值
Figure BDA00026933139300000421
的加权发散速率与所述加速计在t时刻的姿态估计值
Figure BDA00026933139300000422
的加权收敛速率，确定所述互补滤波器融合处理的加权系数，
Figure BDA0002693313930000051
作为本发明进一步的改进，加速度计在t时刻的加速度测量值bat用四元数bat＝[0ax ay az]表示，其中，ax、ay、az分别表示在无人机坐标系中x轴、y轴、z轴的加速度测量值分量，所述加速度测量值bat归一化后的加速度向量用
Figure BDA0002693313930000052
表示，
其中，所述采用梯度下降法获取所述加速计在t时刻的姿态估计值
Figure BDA0002693313930000053
包括：
基于所述加速度向量
Figure BDA0002693313930000054
和t时刻从无人机坐标系变换到导航坐标系的归一化方向向量
Figure BDA0002693313930000055
构建目标函数
Figure BDA0002693313930000056
Figure BDA0002693313930000057
其中，
Figure BDA0002693313930000058
为无人机坐标系中归一化向量
Figure BDA0002693313930000059
的参考方向，
Figure BDA00026933139300000510
Figure BDA00026933139300000511
为导航坐标系中重力向量g的归一化向量，
Figure BDA00026933139300000512
Figure BDA00026933139300000513
为
Figure BDA00026933139300000514
的共轭四元数
Figure BDA00026933139300000515
通过梯度下降法确定所述目标函数
Figure BDA00026933139300000516
的最小值，并将最小值作为所述加速计在t时刻的姿态估计值
Figure BDA00026933139300000517
作为本发明进一步的改进，所述通过梯度下降法确定所述目标函数
Figure BDA00026933139300000518
的最小值，包括：
根据梯度下降法，对
Figure BDA00026933139300000519
迭代求解，确定所述加速计在t时刻的姿态估计值
Figure BDA00026933139300000520
其中，
Figure BDA00026933139300000521
为所述互补滤波器在t-Δt时刻融合得到的无人机姿态估计值，Δt为所述互补滤波器的采样时间，
Figure BDA00026933139300000522
为所述目标函数
Figure BDA00026933139300000523
的梯度值，
Figure BDA00026933139300000524
J表示雅可比行列式，
Figure BDA00026933139300000525
Figure BDA00026933139300000526
为所述陀螺仪在t时刻的姿态估计值
Figure BDA00026933139300000527
的时间导数，α为比例因子。
作为本发明进一步的改进，所述融合估计模块还用于：
基于陀螺仪在t时刻的角度漂移值bωd,t对陀螺仪在t时刻的角速度测量值bωt进行补偿，得到补偿后的角速度值bωc,t；
基于所述补偿后的角速度值bωc,t和互补滤波器在t-Δt时刻融合得到的无人机姿态估计值
Figure BDA00026933139300000528
得到所述陀螺仪在t时刻的姿态估计值
Figure BDA00026933139300000529
其中，Δt为所述互补滤波器的采样时间。
作为本发明进一步的改进，所述角速度测量值用四元数bωt＝[0 ωx ωy ω表示，其中，ωx、ωy、ωz分别表示在无人机坐标系中x轴、y轴、z轴的角速度测量值分量，
其中，基于陀螺仪在t时刻的角度漂移值bωd,t对陀螺仪在t时刻的角速度测量值bωt进行补偿，得到补偿后的角速度值bωc,t，基于所述补偿后的角速度值bωc,t和互补滤波器在t-Δt时刻融合得到的无人机姿态估计值
Figure BDA0002693313930000061
得到所述陀螺仪在t时刻的姿态估计值
Figure BDA0002693313930000062
包括：
基于所述陀螺仪在t时刻的角度漂移值bωd,t对所述角速度测量值bωt进行补偿，确定补偿后的角速度值bωc,t，bωc,t＝bωt-bωd,t；
基于所述补偿后的角速度值bωc,t和所述互补滤波器在t-Δt时刻融合得到的无人机姿态估计值
Figure BDA0002693313930000063
确定所述陀螺仪在t时刻的姿态估计值
Figure BDA0002693313930000064
的时间导数
Figure BDA0002693313930000065
Figure BDA0002693313930000066
基于所述陀螺仪在t时刻的姿态估计值
Figure BDA0002693313930000067
的时间导数
Figure BDA0002693313930000068
确定所述陀螺仪在t时刻的姿态估计值
Figure BDA0002693313930000069
Figure BDA00026933139300000610
并将所述补偿后的角速度值bωc,t作为观测向量。
作为本发明进一步的改进，通过所述陀螺仪在t时刻的误差bωe,t和漂移补偿参数ζ，确定所述陀螺仪在t时刻的角度漂移值bωd,t，
Figure BDA00026933139300000611
其中，
Figure BDA00026933139300000612
Figure BDA00026933139300000613
为t时刻的无人机姿态估计值
Figure BDA00026933139300000614
的时间导；
所述漂移补偿参数ζ表示消除所述陀螺仪非零平均测量误差的收敛速度：
Figure BDA00026933139300000615
其中，
Figure BDA00026933139300000616
为所述陀螺仪三轴漂移的估计速率，
Figure BDA00026933139300000617
为无人机坐标系相对于导航坐标系的方向矢量的估计值。
本发明还提供了一种电子设备，包括存储器和处理器，所述存储器用于存储一条或多条计算机指令，其中，所述一条或多条计算机指令被处理器执行以实现所述的方法。
发明还提供了一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行以实现所述的方法。
本发明的有益效果为：采用梯度下降法对加速度计数据的姿态四元数进行优化，通过自适应增益互补滤波将优化的加速度计姿态四元数与陀螺仪数据进行融合，能够自适应地调整参数β，准确估计出无人机的真实姿态，同时解决了人为调整参数耗时耗力的问题，可以很好地应用于各种飞行载体的姿态估计。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍。显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1为本发明一示例性实施例所述的一种估计无人机姿态的方法的原理框图；
图2为本发明一示例性实施例所述的导航坐标系(n)和无人机坐标系(b)之间的关系以及基于单位四元数的相应变换的示意图；
图3为本发明一示例性实施例所述的对互补滤波器调整参数的实验结果，其中，(a)为滚动角的实验结果，(b)为俯仰角的实验结果，(c)为偏航角的实验结果，(d)为三个欧拉角插补后的实验结果；
图4为本发明一示例性实施例所述的对互补滤波器采用β＝0.1和ζ＝0.01的姿态估计结果，其中，(a)为滚动角的姿态估计结果，(b)为俯仰角的姿态估计结果，(c)为偏航角的姿态估计结果；
图5为本发明一示例性实施例所述的对互补滤波器采用β＝0.1和ζ＝0.01的姿态估计误差，其中，(a)为滚动角的姿态估计误差，(b)为俯仰角的姿态估计误差，(c)为偏航角的姿态估计误差；
图6为本发明一示例性实施例所述的对互补滤波器采用β＝1和ζ＝0.01的姿态估计结果，其中，(a)为滚动角的姿态估计结果，(b)为俯仰角的姿态估计结果，(c)为偏航角的姿态估计结果；
图7为本发明一示例性实施例所述的对互补滤波器采用β＝1和ζ＝0.01的姿态估计误差，其中，(a)为滚动角的姿态估计误差，(b)为俯仰角的姿态估计误差，(c)为偏航角的姿态估计误差。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
需要说明，若本发明实施例中有涉及方向性指示(诸如上、下、左、右、前、后……)，则该方向性指示仅用于解释在某一特定姿态(如附图所示)下各部件之间的相对位置关系、运动情况等，如果该特定姿态发生改变时，则该方向性指示也相应地随之改变。
另外，在本发明的描述中，所用术语仅用于说明目的，并非旨在限制本发明的范围。术语“包括”和/或“包含”用于指定所述元件、步骤、操作和/或组件的存在，但并不排除存在或添加一个或多个其他元件、步骤、操作和/或组件的情况。术语“第一”、“第二”等可能用于描述各种元件，不代表顺序，且不对这些元件起限定作用。此外，在本发明的描述中，除非另有说明，“多个”的含义是两个及两个以上。这些术语仅用于区分一个元素和另一个元素。结合以下附图，这些和/或其他方面变得显而易见，并且，本领域普通技术人员更容易理解关于本发明所述实施例的说明。附图仅出于说明的目的用来描绘本发明所述实施例。本领域技术人员将很容易地从以下说明中认识到，在不背离本发明所述原理的情况下，可以采用本发明所示结构和方法的替代实施例。
本发明实施例所述的一种估计无人机姿态的方法，如图1所示，所述方法包括：
通过互补滤波器对加速计在t时刻的姿态估计值
Figure BDA0002693313930000081
和陀螺仪在t时刻的姿态估计值
Figure BDA0002693313930000082
进行融合处理，得到t时刻的无人机姿态估计值
Figure BDA0002693313930000083
Figure BDA0002693313930000084
其中，kt是所述陀螺仪在t时刻的姿态估计值
Figure BDA0002693313930000091
的发散速率β的加权系数，1-kt是所述加速计在t时刻的姿态估计值
Figure BDA0002693313930000092
的收敛速率的加权系数；
通过所述加速度计在t时刻的误差ea,t对所述姿态估计值
Figure BDA0002693313930000093
的发散速率β实时调整，β＝2-||ea,t||。
其中，
Figure BDA0002693313930000094
加速度计在t时刻的加速度测量值bat归一化后的加速度向量为
Figure BDA0002693313930000095
Figure BDA0002693313930000096
为导航坐标系中重力向量g的归一化向量，
Figure BDA0002693313930000097
相关技术中，通常使用传感器来实现无人机的飞行姿态测量，例如加速度计和陀螺仪等惯性测量单元(IMU)。陀螺仪用于测量角速度，具有高动态特性，它是一个间接测量角度的器件，它测量的是角度的导数，即角速度，要将角速度对时间积分才能得到角度。由于噪声等误差影响，在积分作用下不断积累，会导致陀螺仪的低频干扰和漂移。加速度计用于测量当前加速度(包含重力加速度)的方向，对高频信号敏感，在振动环境中高频干扰较大。因此，各个传感器除了自身的测量误差外，还容易受到温度、噪声等多方面因素的影响，以致测量数据不准确。本发明所述的方法利用互补滤波器将各个传感器的数据进行融合，同时通过加速度计的误差对互补滤波器的参数β进行动态调整，以适应于无人机飞行过程中姿态急剧变化的情况，提高无人机姿态估计的精度。所述无人机例如为多旋翼无人机等，所述加速度计和所述陀螺仪固定于所述无人机的框架上，本发明所述的方法对应用的无人机类型不做具体限定。
在一种可选的实施方式中，采用梯度下降法获取所述加速计在t时刻的姿态估计值
Figure BDA0002693313930000098
梯度方向的步长为μt，所述互补滤波器的采样时间为Δt，
其中，所述通过互补滤波器对加速计在t时刻的姿态估计值
Figure BDA0002693313930000099
和陀螺仪在t时刻的姿态估计值
Figure BDA00026933139300000910
进行融合处理，得到t时刻的无人机姿态估计值
Figure BDA00026933139300000911
包括：
确定所述姿态估计值
Figure BDA00026933139300000912
的收敛速率
Figure BDA00026933139300000913
根据所述陀螺仪在t时刻的姿态估计值
Figure BDA00026933139300000914
的加权发散速率与所述加速计在t时刻的姿态估计值
Figure BDA00026933139300000915
的加权收敛速率，确定所述互补滤波器融合处理的加权系数，
Figure BDA0002693313930000101
本发明所述的方法采用计算量少的梯度下降法对加速度计的姿态数据进行优化，获取加速度计的最优姿态估计。其中，kt是的最佳值是所述陀螺仪在测量过程中因积分漂移导致的所述陀螺仪在t时刻的姿态估计值
Figure BDA0002693313930000102
的加权发散速率，其等于所述加速计在t时刻的姿态估计值
Figure BDA0002693313930000103
的加权收敛速率相等，即
Figure BDA0002693313930000104
可得
Figure BDA0002693313930000105
根据动态调整的参数β，即可对t时刻的无人机姿态估计值
Figure BDA0002693313930000106
进行动态调整，由于参数β动态可调，权重(kt，1-kt)动态可调，提高了互补滤波器的自适应调整性能，可以实现对无人机姿态的精确估计。
如前述所述，加速度计低频特性比较好，因为加速度的角度可以直解算出来，没有累积误差，所以长时间测量后也比较准确，但容易受到高频噪声和运动加速度的影响。而陀螺仪可以在短时间内提供高精度的姿态信息，但容易受到累积漂移和低频噪声的影响，会造成输出误差比较大，甚至无法使用。因此，所述互补滤波器主要针对具有互补频率特性的两个噪声源进行设计，通过所述互补滤波器可以根据各个传感器的特性进行姿态估计，通过互补滤波融合来优化姿态估计结果。所述互补滤波器包括低通滤波器和高通滤波器，其中，低通滤波器用于对加速度计进行滤波，使加速度计过滤高频信号，滤除短时的变化，高通滤波器用于对陀螺仪进行滤波，使陀螺仪过滤低频信号，滤出变化比较缓慢的信号，消除漂移，再以动态的权重(kt，1-kt)加权求和，得到整个频带的信号(姿态角度，包括偏航角、俯仰角和滚动角)。其中，互补滤波要求两个信号的干扰噪声处在不同的频率，通过设置两个滤波器的截止频率，确保融合后的信号能够覆盖需求频率。可以理解的是，互补即是将滤波器的两个部分加起来成为一个滤波器，使其能输出一个准确的线性估计。
在一种可选的实施方式中，加速度计在t时刻的加速度测量值bat用四元数bat＝[0ax ay az]表示，其中，ax、ay、az分别表示在无人机坐标系中x轴、y轴、z轴的加速度测量值分量，所述加速度测量值bat归一化后的加速度向量用
Figure BDA0002693313930000111
表示，
其中，所述采用梯度下降法获取所述加速计在t时刻的姿态估计值
Figure BDA0002693313930000112
包括：
基于所述加速度向量
Figure BDA0002693313930000113
和t时刻从无人机坐标系变换到导航坐标系的归一化方向向量
Figure BDA0002693313930000114
构建目标函数
Figure BDA0002693313930000115
Figure BDA0002693313930000116
其中，如图2所示，
Figure BDA0002693313930000117
为无人机坐标系中归一化向量
Figure BDA0002693313930000118
的参考方向，
Figure BDA0002693313930000119
Figure BDA00026933139300001110
为导航坐标系中重力向量g的归一化向量，
Figure BDA00026933139300001111
Figure BDA00026933139300001112
为
Figure BDA00026933139300001113
的共轭四元数
Figure BDA00026933139300001114
通过梯度下降法确定所述目标函数
Figure BDA00026933139300001115
的最小值，并将最小值作为所述加速计在t时刻的姿态估计值
Figure BDA00026933139300001116
其中，所述加速度计进行姿态估计时，从无人机坐标系变换到导航坐标系的转换矩阵是有误差的，本发明所述的方法利用加速度计表征的四元数矩阵误差，将该误差矩阵用所述目标函数
Figure BDA00026933139300001117
来表示，将优化问题表述为求解
Figure BDA00026933139300001118
来实现，通过梯度下降法确定目标函数
Figure BDA00026933139300001119
的极值，该方法对最小化目标函数来说易于实现和计算。
在一种可选的实施方式中，所述通过梯度下降法确定所述目标函数
Figure BDA00026933139300001120
的最小值，包括：
根据梯度下降法，对
Figure BDA00026933139300001121
迭代求解，确定所述加速计在t时刻的姿态估计值
Figure BDA00026933139300001122
其中，
Figure BDA00026933139300001123
为所述互补滤波器在t-Δt时刻融合得到的无人机姿态估计值，Δt为所述互补滤波器的采样时间，
Figure BDA00026933139300001124
为所述目标函数
Figure BDA00026933139300001125
的梯度值，
Figure BDA00026933139300001126
J表示雅可比行列式，
Figure BDA00026933139300001127
Figure BDA00026933139300001128
为所述陀螺仪在t时刻的姿态估计值
Figure BDA00026933139300001129
的时间导数，α为比例因子。
由前述所述，
Figure BDA00026933139300001130
可将所述目标函数
Figure BDA00026933139300001131
进行转化，得到：
Figure BDA0002693313930000121
上式中，
Figure BDA0002693313930000122
其中，
Figure BDA0002693313930000123
分别表示归一化方向向量
Figure BDA0002693313930000124
的四元数，将重力向量g在导航坐标系中的值旋转至无人机坐标系中，再减去加速度计当前的加速度测量值，即为通过加速度计表征旋转矩阵的误差。
其中，对上述转化后的目标函数求导即可得到对应的雅克比矩阵：
Figure BDA0002693313930000125
再利用所得雅克比矩阵的转置矩阵，根据前述梯度计算式
Figure BDA0002693313930000126
确定最终的计算目标，使用梯度下降法进行迭代计算，消除旋转矩阵误差的点，得到所述加速计在t时刻的姿态估计值
Figure BDA0002693313930000127
其中，每个样本只需要迭代一次，姿态的收敛速度由步长μt决定，步长μt和无人机陀螺仪在t时刻的姿态估计值
Figure BDA0002693313930000128
的时间导数相关，和采样时间Δt相关。
在一种可选的实施方式中，所述方法还包括：
基于陀螺仪在t时刻的角度漂移值bωd,t对陀螺仪在t时刻的角速度测量值bωt进行补偿，得到补偿后的角速度值bωc,t；
基于所述补偿后的角速度值bωc,t和互补滤波器在t-Δt时刻融合得到的无人机姿态估计值
Figure BDA0002693313930000129
得到所述陀螺仪在t时刻的姿态估计值
Figure BDA00026933139300001210
其中，Δt为所述互补滤波器的采样时间。
在一种可选的实施方式中，所述角速度测量值用四元数bωt＝[0 ωx ωy ω表示，其中，ωx、ωy、ωz分别表示在无人机坐标系中x轴、y轴、z轴的角速度测量值分量，
其中，基于陀螺仪在t时刻的角度漂移值bωd,t对陀螺仪在t时刻的角速度测量值bωt进行补偿，得到补偿后的角速度值bωc,t，基于所述补偿后的角速度值bωc,t和互补滤波器在t-Δt时刻融合得到的无人机姿态估计值
Figure BDA00026933139300001211
得到所述陀螺仪在t时刻的姿态估计值
Figure BDA00026933139300001212
包括：
基于所述陀螺仪在t时刻的角度漂移值bωd,t对所述角速度测量值bωt进行补偿，确定补偿后的角速度值bωc,t，bωc,t＝bωt-bωd,t；
基于所述补偿后的角速度值bωc,t和所述互补滤波器在t-Δt时刻融合得到的无人机姿态估计值
Figure BDA0002693313930000131
确定所述陀螺仪在t时刻的姿态估计值
Figure BDA0002693313930000132
的时间导数
Figure BDA0002693313930000133
Figure BDA0002693313930000134
基于所述陀螺仪在t时刻的姿态估计值
Figure BDA0002693313930000135
的时间导数
Figure BDA0002693313930000136
确定所述陀螺仪在t时刻的姿态估计值
Figure BDA0002693313930000137
Figure BDA0002693313930000138
并将所述补偿后的角速度值bωc,t作为观测向量。
在一种可选的实施方式中，通过所述陀螺仪在t时刻的误差bωe,t
和漂移补偿参数ζ，确定所述陀螺仪在t时刻的角度漂移值bωd,t，
Figure BDA0002693313930000139
其中，
Figure BDA00026933139300001310
Figure BDA00026933139300001311
为t时刻的无人机姿态估计值
Figure BDA00026933139300001312
的时间导；
所述漂移补偿参数ζ表示消除所述陀螺仪非零平均测量误差的收敛速度：
Figure BDA00026933139300001313
其中，
Figure BDA00026933139300001314
为所述陀螺仪三轴漂移的估计速率(可以通过数值实验数据来确定)，
Figure BDA00026933139300001315
为无人机坐标系相对于导航坐标系的方向矢量的估计值。
由前述所述，陀螺仪偏置随时间、温度和运动的变化而漂移，因此需要对陀螺偏置进行补偿。本发明所述的方法，以梯度下降法补偿后的角速度值bωc,t作为观测向量，对陀螺漂移进行补偿。
如图3所示，对互补滤波器进行了测试，其中，横坐标表示时间，纵坐标分别表示滚动角、俯仰角、偏航角、三个欧拉角(滚动角、俯仰角、偏航角)插补后的角度，图中的曲线为手动调整参数β所得到的姿态估计结果，图中的横线为本发明所述的方法根据加速度计的误差实时自动调整参数β所得到的姿态估计结果，从结果可以看出，手动调整参数无法实时获取最优的估计性能，需要将参数调整到合适的值才能获得与本发明所述的方法相同的估计性能，然而通过手动调整参数时，需要进行大量的计算，耗时耗力，不适用于实时的估计。
如图4所示，采用β＝0.1和ζ＝0.01对互补滤波器进行了测试，其中，横坐标表示时间，纵坐标表示姿态估计结果，图中与标定好的真实数据曲线变化更为一致的是本发明所述的方法根据加速度计的误差实时自动调整参数β所得到的姿态估计结果，图中与标定好的真实数据曲线变化误差较大的是通过手动调整参数β所得到的姿态估计结果。如图5所示，横坐标表示时间，纵坐标表示姿态估计误差，图中与标定好的真实数据误差较小的曲线是本发明所述的方法根据加速度计的误差实时自动调整参数β所得到姿态估计误差，图中与标定好的真实数据误差较大的曲线是通过手动调整参数β所得到的姿态估计误差。从结果可以看出，本发明所述的方法可以很好地估计横滚角、俯仰角和滚动角，手动调整参数在运动姿态变化较快时，姿态估计误差较大。
如图6所示，采用β＝1和ζ＝0.01对互补滤波器进行了测试，其中，横坐标表示时间，纵坐标表示姿态估计结果，本发明所述的方法根据加速度计的误差实时自动调整参数β所得到的姿态估计结果，以及通过手动调整参数β所得到的姿态估计结果与标定好的真实数据曲线变化都基本一致。如图7所示，横坐标表示时间，纵坐标表示姿态估计误差，本发明所述的方法根据加速度计的误差实时自动调整参数β所得到的姿态估计误差，以及通过手动调整参数β所得到的姿态估计误差相差不多。从结果可以看出，当参数β调整到一定值(例如1)时，互补滤波器获得了较佳的估计性能，本发明所述的方法能够自适应的调整参数β，以满足姿态估计的需求，解决了人为调整参数耗时耗力的问题，可以很好地应用于各种飞行载体的姿态估计。
本发明实施例所述的一种估计无人机姿态的系统，所述系统包括：
融合估计模块，用于通过互补滤波器对加速计在t时刻的姿态估计值
Figure BDA0002693313930000141
和陀螺仪在t时刻的姿态估计值
Figure BDA0002693313930000142
进行融合处理，得到t时刻的无人机姿态估计值
Figure BDA0002693313930000143
Figure BDA0002693313930000144
其中，kt是所述陀螺仪在t时刻的姿态估计值
Figure BDA0002693313930000145
的发散速率β的加权系数，1-kt是所述加速计在t时刻的姿态估计值
Figure BDA0002693313930000146
的收敛速率的加权系数；
参数调整模块，用于通过所述加速度计在t时刻的误差eat对所述姿态估计值
Figure BDA0002693313930000147
的发散速率β实时调整，β＝2-||ea,t||。
其中，
Figure BDA0002693313930000151
加速度计在t时刻的加速度测量值bat归一化后的加速度向量为
Figure BDA0002693313930000152
Figure BDA0002693313930000153
为导航坐标系中重力向量g的归一化向量，
Figure BDA0002693313930000154
相关技术中，通常使用传感器来实现无人机的飞行姿态测量，例如加速度计和陀螺仪等惯性测量单元(IMU)。陀螺仪用于测量角速度，具有高动态特性，它是一个间接测量角度的器件，它测量的是角度的导数，即角速度，要将角速度对时间积分才能得到角度。由于噪声等误差影响，在积分作用下不断积累，会导致陀螺仪的低频干扰和漂移。加速度计用于测量当前加速度(包含重力加速度)的方向，对高频信号敏感，在振动环境中高频干扰较大。因此，各个传感器除了自身的测量误差外，还容易受到温度、噪声等多方面因素的影响，以致测量数据不准确。本发明所述的系统利用互补滤波器将各个传感器的数据进行融合，同时通过加速度计的误差对互补滤波器的参数β进行动态调整，以适应于无人机飞行过程中姿态急剧变化的情况，提高无人机姿态估计的精度。所述无人机例如为多旋翼无人机等，所述加速度计和所述陀螺仪固定于所述无人机的框架上，本发明所述的系统对应用的无人机类型不做具体限定。
在一种可选的实施方式中，采用梯度下降法获取所述加速计在t时刻的姿态估计值
Figure BDA0002693313930000155
梯度方向的步长为μt，所述互补滤波器的采样时间为Δt，
其中，所述通过互补滤波器对加速计在t时刻的姿态估计值
Figure BDA0002693313930000156
和陀螺仪在t时刻的姿态估计值
Figure BDA0002693313930000157
进行融合处理，得到t时刻的无人机姿态估计值
Figure BDA0002693313930000158
包括：
确定所述姿态估计值
Figure BDA0002693313930000159
的收敛速率
Figure BDA00026933139300001510
根据所述陀螺仪在t时刻的姿态估计值
Figure BDA00026933139300001511
的加权发散速率与所述加速计在t时刻的姿态估计值
Figure BDA00026933139300001512
的加权收敛速率，确定所述互补滤波器融合处理的加权系数，
Figure BDA00026933139300001513
本发明所述的系统采用计算量少的梯度下降法对加速度计的姿态数据进行优化，获取加速度计的最优姿态估计。其中，kt是的最佳值是所述陀螺仪在测量过程中因积分漂移导致的所述陀螺仪在t时刻的姿态估计值
Figure BDA00026933139300001514
的加权发散速率，其等于所述加速计在t时刻的姿态估计值
Figure BDA0002693313930000161
的加权收敛速率相等，即
Figure BDA0002693313930000162
可得
Figure BDA0002693313930000163
根据动态调整的参数β，即可对t时刻的无人机姿态估计值
Figure BDA0002693313930000164
进行动态调整，由于参数β动态可调，权重(kt，1-kt)动态可调，提高了互补滤波器的自适应调整性能，可以实现对无人机姿态的精确估计。
如前述所述，加速度计低频特性比较好，因为加速度的角度可以直解算出来，没有累积误差，所以长时间测量后也比较准确，但容易受到高频噪声和运动加速度的影响。而陀螺仪可以在短时间内提供高精度的姿态信息，但容易受到累积漂移和低频噪声的影响，会造成输出误差比较大，甚至无法使用。因此，所述互补滤波器主要针对具有互补频率特性的两个噪声源进行设计，通过所述互补滤波器可以根据各个传感器的特性进行姿态估计，通过互补滤波融合来优化姿态估计结果。所述互补滤波器包括低通滤波器和高通滤波器，其中，低通滤波器用于对加速度计进行滤波，使加速度计过滤高频信号，滤除短时的变化，高通滤波器用于对陀螺仪进行滤波，使陀螺仪过滤低频信号，滤出变化比较缓慢的信号，消除漂移，再以动态的权重(kt，1-kt)加权求和，得到整个频带的信号(姿态角度，包括偏航角、俯仰角和滚动角)。其中，互补滤波要求两个信号的干扰噪声处在不同的频率，通过设置两个滤波器的截止频率，确保融合后的信号能够覆盖需求频率。可以理解的是，互补即是将滤波器的两个部分加起来成为一个滤波器，使其能输出一个准确的线性估计。
在一种可选的实施方式中，加速度计在t时刻的加速度测量值bat用四元数bat＝[0ax ay az]表示，其中，ax、ay、az分别表示在无人机坐标系中x轴、y轴、z轴的加速度测量值分量，所述加速度测量值bat归一化后的加速度向量用
Figure BDA0002693313930000165
表示，
其中，所述采用梯度下降法获取所述加速计在t时刻的姿态估计值
Figure BDA0002693313930000166
包括：
基于所述加速度向量
Figure BDA0002693313930000167
和t时刻从无人机坐标系变换到导航坐标系的归一化方向向量
Figure BDA0002693313930000171
构建目标函数
Figure BDA0002693313930000172
Figure BDA0002693313930000173
其中，如图2所示，
Figure BDA0002693313930000174
为无人机坐标系中归一化向量
Figure BDA0002693313930000175
的参考方向，
Figure BDA0002693313930000176
Figure BDA0002693313930000177
为导航坐标系中重力向量g的归一化向量，
Figure BDA0002693313930000178
Figure BDA0002693313930000179
为
Figure BDA00026933139300001710
的共轭四元数
Figure BDA00026933139300001711
通过梯度下降法确定所述目标函数
Figure BDA00026933139300001712
的最小值，并将最小值作为所述加速计在t时刻的姿态估计值
Figure BDA00026933139300001713
其中，所述加速度计进行姿态估计时，从无人机坐标系变换到导航坐标系的转换矩阵是有误差的，本发明所述的方法利用加速度计表征的四元数矩阵误差，将该误差矩阵用所述目标函数
Figure BDA00026933139300001714
来表示，将优化问题表述为求解
Figure BDA00026933139300001715
来实现，通过梯度下降法确定目标函数
Figure BDA00026933139300001716
的极值，该方法对最小化目标函数来说易于实现和计算。
在一种可选的实施方式中，所述通过梯度下降法确定所述目标函数
Figure BDA00026933139300001717
的最小值，包括：
根据梯度下降法，对
Figure BDA00026933139300001718
迭代求解，确定所述加速计在t时刻的姿态估计值
Figure BDA00026933139300001719
其中，
Figure BDA00026933139300001720
为所述互补滤波器在t-Δt时刻融合得到的无人机姿态估计值，Δt为所述互补滤波器的采样时间，
Figure BDA00026933139300001721
为所述目标函数
Figure BDA00026933139300001722
的梯度值，
Figure BDA00026933139300001723
J表示雅可比行列式，
Figure BDA00026933139300001724
Figure BDA00026933139300001725
为所述陀螺仪在t时刻的姿态估计值
Figure BDA00026933139300001726
的时间导数，α为比例因子。
由前述所述，
Figure BDA00026933139300001727
可将所述目标函数
Figure BDA00026933139300001728
进行转化，得到：
Figure BDA00026933139300001729
上式中，
Figure BDA00026933139300001730
其中，
Figure BDA00026933139300001731
分别表示归一化方向向量
Figure BDA00026933139300001732
的四元数，将重力向量g在导航坐标系中的值旋转至无人机坐标系中，再减去加速度计当前的加速度测量值，即为通过加速度计表征旋转矩阵的误差。
其中，对上述转化后的目标函数求导即可得到对应的雅克比矩阵：
Figure BDA0002693313930000181
再利用所得雅克比矩阵的转置矩阵，根据前述梯度计算式
Figure BDA0002693313930000182
确定最终的计算目标，使用梯度下降法进行迭代计算，消除旋转矩阵误差的点，得到所述加速计在t时刻的姿态估计值
Figure BDA0002693313930000183
其中，每个样本只需要迭代一次，姿态的收敛速度由步长μt决定，步长μt和无人机陀螺仪在t时刻的姿态估计值
Figure BDA0002693313930000184
的时间导数相关，和采样时间Δt相关。
在一种可选的实施方式中，所述融合估计模块还用于：
基于陀螺仪在t时刻的角度漂移值bωd,t对陀螺仪在t时刻的角速度测量值bωt进行补偿，得到补偿后的角速度值bωc,t；
基于所述补偿后的角速度值bωc,t和互补滤波器在t-Δt时刻融合得到的无人机姿态估计值
Figure BDA0002693313930000185
得到所述陀螺仪在t时刻的姿态估计值
Figure BDA0002693313930000186
其中，Δt为所述互补滤波器的采样时间。
在一种可选的实施方式中，所述角速度测量值用四元数bωt＝[0ωxωyω表示，其中，ωx、ωy、ωz分别表示在无人机坐标系中x轴、y轴、z轴的角速度测量值分量，
其中，基于陀螺仪在t时刻的角度漂移值bωd,t对陀螺仪在t时刻的角速度测量值bωt进行补偿，得到补偿后的角速度值bωc,t，基于所述补偿后的角速度值bωc,t和互补滤波器在t-Δt时刻融合得到的无人机姿态估计值
Figure BDA0002693313930000187
得到所述陀螺仪在t时刻的姿态估计值
Figure BDA0002693313930000188
包括：
基于所述陀螺仪在t时刻的角度漂移值bωd,t对所述角速度测量值bωt进行补偿，确定补偿后的角速度值bωc,t，bωc,t＝bωt-bωd,t；
基于所述补偿后的角速度值bωc,t和所述互补滤波器在t-Δt时刻融合得到的无人机姿态估计值
Figure BDA0002693313930000189
确定所述陀螺仪在t时刻的姿态估计值
Figure BDA00026933139300001810
的时间导数
Figure BDA00026933139300001811
Figure BDA00026933139300001812
基于所述陀螺仪在t时刻的姿态估计值
Figure BDA00026933139300001813
的时间导数
Figure BDA00026933139300001814
确定所述陀螺仪在t时刻的姿态估计值
Figure BDA00026933139300001815
Figure BDA00026933139300001816
并将所述补偿后的角速度值bωc,t作为观测向量。
在一种可选的实施方式中，通过所述陀螺仪在t时刻的误差bωe,t和漂移补偿参数ζ，确定所述陀螺仪在t时刻的角度漂移值bωd,t，
Figure BDA0002693313930000191
其中，
Figure BDA0002693313930000192
Figure BDA0002693313930000193
为t时刻的无人机姿态估计值
Figure BDA0002693313930000194
的时间导；
所述漂移补偿参数ζ表示消除所述陀螺仪非零平均测量误差的收敛速度：
Figure BDA0002693313930000195
其中，
Figure BDA0002693313930000196
为所述陀螺仪三轴漂移的估计速率(可以通过数值实验数据来确定)，
Figure BDA0002693313930000197
为无人机坐标系相对于导航坐标系的方向矢量的估计值。
由前述所述，陀螺仪偏置随时间、温度和运动的变化而漂移，因此需要对陀螺偏置进行补偿。本发明所述的方法，以梯度下降法补偿后的角速度值bωc,t作为观测向量，对陀螺漂移进行补偿。
如图3所示，对互补滤波器进行了测试，其中，横坐标表示时间，纵坐标分别表示滚动角、俯仰角、偏航角、三个欧拉角(滚动角、俯仰角、偏航角)插补后的角度，图中的曲线为手动调整参数β所得到的姿态估计结果，图中的横线为本发明所述的系统根据加速度计的误差实时自动调整参数β所得到的姿态估计结果，从结果可以看出，手动调整参数无法实时获取最优的估计性能，需要将参数调整到合适的值才能获得与本发明所述的方法相同的估计性能，然而通过手动调整参数时，需要进行大量的计算，耗时耗力，不适用于实时的估计。
如图4所示，采用β＝0.1和ζ＝0.01对互补滤波器进行了测试，其中，横坐标表示时间，纵坐标表示姿态估计结果，图中与标定好的真实数据曲线变化更为一致的是本发明所述的系统根据加速度计的误差实时自动调整参数β所得到的姿态估计结果，图中与标定好的真实数据曲线变化误差较大的是通过手动调整参数β所得到的姿态估计结果。如图5所示，横坐标表示时间，纵坐标表示姿态估计误差，图中与标定好的真实数据误差较小的曲线是本发明所述的系统根据加速度计的误差实时自动调整参数β所得到姿态估计误差，图中与标定好的真实数据误差较大的曲线是通过手动调整参数β所得到的姿态估计误差。从结果可以看出，本发明所述的系统可以很好地估计横滚角、俯仰角和滚动角，手动调整参数在运动姿态变化较快时，姿态估计误差较大。
如图6所示，采用β＝1和ζ＝0.01对互补滤波器进行了测试，其中，横坐标表示时间，纵坐标表示姿态估计结果，本发明所述的系统根据加速度计的误差实时自动调整参数β所得到的姿态估计结果，以及通过手动调整参数β所得到的姿态估计结果与标定好的真实数据曲线变化都基本一致。如图7所示，横坐标表示时间，纵坐标表示姿态估计误差，本发明所述的系统根据加速度计的误差实时自动调整参数β所得到的姿态估计误差，以及通过手动调整参数β所得到的姿态估计误差相差不多。从结果可以看出，当参数β调整到一定值(例如1)时，互补滤波器获得了较佳的估计性能，本发明所述的系统能够自适应的调整参数β，以满足姿态估计的需求，同时解决了人为调整参数耗时耗力的问题，可以很好地应用于各种飞行载体的姿态估计。
本公开还涉及一种电子设备，包括服务器、终端等。该电子设备包括：至少一个处理器；与至少一个处理器通信连接的存储器；以及与存储介质通信连接的通信组件，所述通信组件在处理器的控制下接收和发送数据；其中，存储器存储有可被至少一个处理器执行的指令，指令被至少一个处理器执行以实现上述实施例中的方法。
在一种可选的实施方式中，存储器作为一种非易失性计算机可读存储介质，可用于存储非易失性软件程序、非易失性计算机可执行程序以及模块。处理器通过运行存储在存储器中的非易失性软件程序、指令以及模块，从而执行设备的各种功能应用以及数据处理，即实现方法。
存储器可以包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需要的应用程序；存储数据区可存储选项列表等。此外，存储器可以包括高速随机存取存储器，还可以包括非易失性存储器，例如至少一个磁盘存储器件、闪存器件、或其他非易失性固态存储器件。在一些实施例中，存储器可选包括相对于处理器远程设置的存储器，这些远程存储器可以通过网络连接至外接设备。上述网络的实例包括但不限于互联网、企业内部网、局域网、移动通信网及其组合。
一个或者多个模块存储在存储器中，当被一个或者多个处理器执行时，执行上述任意方法实施例中的方法。
上述产品可执行本申请实施例所提供的方法，具备执行方法相应的功能模块和有益效果，未在本实施例中详尽描述的技术细节，可参见本申请实施例所提供的方法。
本公开还涉及一种计算机可读存储介质，用于存储计算机可读程序，所述计算机可读程序用于供计算机执行上述部分或全部的方法实施例。
即，本领域技术人员可以理解，实现上述实施例方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成，该程序存储在一个存储介质中，包括若干指令用以使得一个设备(可以是单片机，芯片等)或处理器(processor)执行本申请各实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-OnlyMemory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。
在此处所提供的说明书中，说明了大量具体细节。然而，能够理解，本发明的实施例可以在没有这些具体细节的情况下实践。在一些实例中，并未详细示出公知的方法、结构和技术，以便不模糊对本说明书的理解。
此外，本领域普通技术人员能够理解，尽管在此所述的一些实施例包括其它实施例中所包括的某些特征而不是其它特征，但是不同实施例的特征的组合意味着处于本发明的范围之内并且形成不同的实施例。例如，在权利要求书中，所要求保护的实施例的任意之一都可以以任意的组合方式来使用。
本领域技术人员应理解，尽管已经参考示例性实施例描述了本发明，但是在不脱离本发明的范围的情况下，可进行各种改变并可用等同物替换其元件。另外，在不脱离本发明的实质范围的情况下，可进行许多修改以使特定情况或材料适应本发明的教导。因此，本发明不限于所公开的特定实施例，而是本发明将包括落入所附权利要求范围内的所有实施例。

Claims (10)

1.一种估计无人机姿态的方法，其特征在于，所述方法包括：
通过互补滤波器对加速计在t时刻的姿态估计值
Figure FDA0002693313920000011
和陀螺仪在t时刻的姿态估计值
Figure FDA0002693313920000012
进行融合处理，得到t时刻的无人机姿态估计值
Figure FDA0002693313920000013
Figure FDA0002693313920000014
其中，kt是所述陀螺仪在t时刻的姿态估计值
Figure FDA0002693313920000015
的发散速率β的加权系数，1-kt是所述加速计在t时刻的姿态估计值
Figure FDA0002693313920000016
的收敛速率的加权系数；
通过所述加速度计在t时刻的误差ea,t对所述姿态估计值
Figure FDA0002693313920000017
的发散速率β实时调整，β＝2-||ea,t||。
2.如权利要求1所述的方法，其中，采用梯度下降法获取所述加速计在t时刻的姿态估计值
Figure FDA0002693313920000018
梯度方向的步长为μt，所述互补滤波器的采样时间为Δt，
其中，所述通过互补滤波器对加速计在t时刻的姿态估计值
Figure FDA0002693313920000019
和陀螺仪在t时刻的姿态估计值
Figure FDA00026933139200000110
进行融合处理，得到t时刻的无人机姿态估计值
Figure FDA00026933139200000111
包括：
确定所述姿态估计值
Figure FDA00026933139200000112
的收敛速率
Figure FDA00026933139200000113
根据所述陀螺仪在t时刻的姿态估计值
Figure FDA00026933139200000114
的加权发散速率与所述加速计在t时刻的姿态估计值
Figure FDA00026933139200000115
的加权收敛速率，确定所述互补滤波器融合处理的加权系数，
Figure FDA00026933139200000116
3.如权利要求2所述的方法，其中，加速度计在t时刻的加速度测量值bat用四元数bat＝[0 ax ay az]表示，其中，ax、ay、az分别表示在无人机坐标系中x轴、y轴、z轴的加速度测量值分量，所述加速度测量值bat归一化后的加速度向量用
Figure FDA00026933139200000117
表示，
其中，所述采用梯度下降法获取所述加速计在t时刻的姿态估计值
Figure FDA00026933139200000118
包括：
基于所述加速度向量
Figure FDA00026933139200000119
和t时刻从无人机坐标系变换到导航坐标系的归一化方向向量
Figure FDA00026933139200000120
构建目标函数
Figure FDA00026933139200000121
Figure FDA00026933139200000122
其中，
Figure FDA00026933139200000123
为无人机坐标系中归一化向量
Figure FDA00026933139200000124
的参考方向，
Figure FDA00026933139200000125
Figure FDA00026933139200000126
为导航坐标系中重力向量g的归一化向量，
Figure FDA00026933139200000127
Figure FDA00026933139200000128
为
Figure FDA00026933139200000129
的共轭四元数
Figure FDA0002693313920000021
通过梯度下降法确定所述目标函数
Figure FDA0002693313920000022
的最小值，并将最小值作为所述加速计在t时刻的姿态估计值
Figure FDA0002693313920000023
4.如权利要求3所述的方法，其中，所述通过梯度下降法确定所述目标函数
Figure FDA0002693313920000024
的最小值，包括：
根据梯度下降法，对
Figure FDA0002693313920000025
迭代求解，确定所述加速计在t时刻的姿态估计值
Figure FDA0002693313920000026
其中，
Figure FDA0002693313920000027
为所述互补滤波器在t-Δt时刻融合得到的无人机姿态估计值，Δt为所述互补滤波器的采样时间，
Figure FDA0002693313920000028
为所述目标函数
Figure FDA0002693313920000029
的梯度值，
Figure FDA00026933139200000210
J表示雅可比行列式，
Figure FDA00026933139200000211
Figure FDA00026933139200000212
为所述陀螺仪在t时刻的姿态估计值
Figure FDA00026933139200000213
的时间导数，α为比例因子。
5.如权利要求3所述的方法，其中，所述方法还包括：
基于陀螺仪在t时刻的角度漂移值bωd,t对陀螺仪在t时刻的角速度测量值bωt进行补偿，得到补偿后的角速度值bωc,t；
基于所述补偿后的角速度值bωc,t和互补滤波器在t-Δt时刻融合得到的无人机姿态估计值
Figure FDA00026933139200000214
得到所述陀螺仪在t时刻的姿态估计值
Figure FDA00026933139200000215
其中，Δt为所述互补滤波器的采样时间。
6.如权利要求5所述的方法，其中，所述角速度测量值用四元数bωt＝[0 ωx ωy ωz]表示，其中，ωx、ωy、ωz分别表示在无人机坐标系中x轴、y轴、z轴的角速度测量值分量，
其中，基于陀螺仪在t时刻的角度漂移值bωd,t对陀螺仪在t时刻的角速度测量值bωt进行补偿，得到补偿后的角速度值bωc,t，基于所述补偿后的角速度值bωc,t和互补滤波器在t-Δt时刻融合得到的无人机姿态估计值
Figure FDA00026933139200000216
得到所述陀螺仪在t时刻的姿态估计值
Figure FDA00026933139200000217
包括：
基于所述陀螺仪在t时刻的角度漂移值bωd,t对所述角速度测量值bωt进行补偿，确定补偿后的角速度值bωc,t，bωc,t＝bωt-bωd,t；
基于所述补偿后的角速度值bωc,t和所述互补滤波器在t-Δt时刻融合得到的无人机姿态估计值
Figure FDA00026933139200000218
确定所述陀螺仪在t时刻的姿态估计值
Figure FDA00026933139200000219
的时间导数
Figure FDA0002693313920000031
Figure FDA0002693313920000032
基于所述陀螺仪在t时刻的姿态估计值
Figure FDA0002693313920000033
的时间导数
Figure FDA0002693313920000034
确定所述陀螺仪在t时刻的姿态估计值
Figure FDA0002693313920000035
Figure FDA0002693313920000036
并将所述补偿后的角速度值bωc,t作为观测向量。
7.如权利要求6所述的方法，其中，通过所述陀螺仪在t时刻的误差bωe,t和漂移补偿参数ζ，确定所述陀螺仪在t时刻的角度漂移值bωd,t，
Figure FDA0002693313920000037
其中，
Figure FDA0002693313920000038
Figure FDA0002693313920000039
为t时刻的无人机姿态估计值
Figure FDA00026933139200000310
的时间导；
所述漂移补偿参数ζ表示消除所述陀螺仪非零平均测量误差的收敛速度：
Figure FDA00026933139200000311
其中，
Figure FDA00026933139200000312
为所述陀螺仪三轴漂移的估计速率，
Figure FDA00026933139200000313
为无人机坐标系相对于导航坐标系的方向矢量的估计值。
8.一种估计无人机姿态的系统，其特征在于，所述系统包括：
融合估计模块，用于通过互补滤波器对加速计在t时刻的姿态估计值
Figure FDA00026933139200000314
和陀螺仪在t时刻的姿态估计值
Figure FDA00026933139200000315
进行融合处理，得到t时刻的无人机姿态估计值
Figure FDA00026933139200000316
Figure FDA00026933139200000317
其中，kt是所述陀螺仪在t时刻的姿态估计值
Figure FDA00026933139200000318
的发散速率β的加权系数，1-kt是所述加速计在t时刻的姿态估计值
Figure FDA00026933139200000319
的收敛速率的加权系数；
参数调整模块，用于通过所述加速度计在t时刻的误差ea,t对所述姿态估计值
Figure FDA00026933139200000320
的发散速率β实时调整，β＝2-||ea,t||。
9.一种电子设备，包括存储器和处理器，其特征在于，所述存储器用于存储一条或多条计算机指令，其中，所述一条或多条计算机指令被处理器执行以实现如权利要求1-7中任一项所述的方法。
10.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行以实现如权利要求1-7中任一项所述的方法。
CN202010998123.4A 2020-09-21 2020-09-21 一种估计无人机姿态的方法及系统 Active CN112179342B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010998123.4A CN112179342B (zh) 2020-09-21 2020-09-21 一种估计无人机姿态的方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010998123.4A CN112179342B (zh) 2020-09-21 2020-09-21 一种估计无人机姿态的方法及系统

Publications (2)

Publication Number Publication Date
CN112179342A true CN112179342A (zh) 2021-01-05
CN112179342B CN112179342B (zh) 2021-08-27

Family

ID=73956407

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010998123.4A Active CN112179342B (zh) 2020-09-21 2020-09-21 一种估计无人机姿态的方法及系统

Country Status (1)

Country Link
CN (1) CN112179342B (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113282005A (zh) * 2021-05-26 2021-08-20 惠州中国科学院遥感与数字地球研究所空间信息技术研究院 无刷云台调控方法、装置、计算机设备和存储介质
CN113672137A (zh) * 2021-08-17 2021-11-19 北京字节跳动网络技术有限公司 光标位置更新方法、装置和电子设备
CN114608516A (zh) * 2022-01-28 2022-06-10 北京航天发射技术研究所 一种小型化雷达动态测姿设备

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6212476B1 (en) * 1998-05-14 2001-04-03 Baker Hughes Incorporated Apparatus to measure the earth's local gravity and magnetic field in conjunction with global positioning attitude determining
CN104914874A (zh) * 2015-06-09 2015-09-16 长安大学 一种基于自适应互补融合的无人机姿态控制系统及方法
CN108398128A (zh) * 2018-01-22 2018-08-14 北京大学深圳研究生院 一种姿态角的融合解算方法和装置
CN109682377A (zh) * 2019-03-08 2019-04-26 兰州交通大学 一种基于动态步长梯度下降的姿态估计方法
CN109883451A (zh) * 2019-04-15 2019-06-14 山东建筑大学 一种用于行人方位估计的自适应增益互补滤波方法及系统
CN110319840A (zh) * 2019-07-05 2019-10-11 东北大学秦皇岛分校 面向异常步态识别的共轭梯度姿态解算方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6212476B1 (en) * 1998-05-14 2001-04-03 Baker Hughes Incorporated Apparatus to measure the earth's local gravity and magnetic field in conjunction with global positioning attitude determining
CN104914874A (zh) * 2015-06-09 2015-09-16 长安大学 一种基于自适应互补融合的无人机姿态控制系统及方法
CN108398128A (zh) * 2018-01-22 2018-08-14 北京大学深圳研究生院 一种姿态角的融合解算方法和装置
CN109682377A (zh) * 2019-03-08 2019-04-26 兰州交通大学 一种基于动态步长梯度下降的姿态估计方法
CN109883451A (zh) * 2019-04-15 2019-06-14 山东建筑大学 一种用于行人方位估计的自适应增益互补滤波方法及系统
CN110319840A (zh) * 2019-07-05 2019-10-11 东北大学秦皇岛分校 面向异常步态识别的共轭梯度姿态解算方法

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113282005A (zh) * 2021-05-26 2021-08-20 惠州中国科学院遥感与数字地球研究所空间信息技术研究院 无刷云台调控方法、装置、计算机设备和存储介质
CN113672137A (zh) * 2021-08-17 2021-11-19 北京字节跳动网络技术有限公司 光标位置更新方法、装置和电子设备
CN114608516A (zh) * 2022-01-28 2022-06-10 北京航天发射技术研究所 一种小型化雷达动态测姿设备

Also Published As

Publication number Publication date
CN112179342B (zh) 2021-08-27

Similar Documents

Publication Publication Date Title
CN112179342B (zh) 一种估计无人机姿态的方法及系统
US20170123035A1 (en) Autonomous magnetometer calibration
CN102692225B (zh) 一种用于低成本小型无人机的姿态航向参考系统
US20150177020A1 (en) Method and apparatus for data fusion of a three-axis magnetometer and three axis accelerometer
CN109000612A (zh) 设备的角度估算方法、装置、摄像组件及飞行器
CN110081878B (zh) 一种多旋翼无人机的姿态及位置确定方法
CN110887481B (zh) 基于mems惯性传感器的载体动态姿态估计方法
CN110887480B (zh) 基于mems传感器的飞行姿态估计方法及系统
CN109579836B (zh) 一种基于mems惯性导航的室内行人方位校准方法
CN106403952A (zh) 一种动中通低成本组合姿态测量方法
EP2674781B1 (en) Latency compensation
CN102654404A (zh) 一种提高航姿参考系统解算精度和系统抗干扰能力的方法
CN106370178B (zh) 移动终端设备的姿态测量方法及装置
KR101211703B1 (ko) 시선벡터를 이용한 자장계 오차 보정방법 및 이를 이용한 통합 항법 시스템
CN112066985A (zh) 一种组合导航系统初始化方法、装置、介质及电子设备
CN110793515A (zh) 一种基于单天线gps和imu的大机动条件下无人机姿态估计方法
CN103712598A (zh) 一种小型无人机姿态确定系统与确定方法
Wang et al. Nonlinear multiple integrator and application to aircraft navigation
US20200293067A1 (en) State estimation
CN112070170A (zh) 一种动态残差阈值自适应四元数粒子滤波姿态解算数据融合方法
Ascorti An application of the extended Kalman filter to the attitude control of a quadrotor
CN113175926B (zh) 一种基于运动状态监测的自适应水平姿态测量方法
CN110375773B (zh) Mems惯导系统姿态初始化方法
CN110849364B (zh) 基于动中通的自适应卡尔曼姿态估计方法
JP7404227B2 (ja) 磁場の測定から判定された方位を特徴付ける方法および装置

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