CN112254723B - 基于自适应ekf算法的小型无人机marg航姿估计方法 - Google Patents

基于自适应ekf算法的小型无人机marg航姿估计方法 Download PDF

Info

Publication number
CN112254723B
CN112254723B CN202011092956.0A CN202011092956A CN112254723B CN 112254723 B CN112254723 B CN 112254723B CN 202011092956 A CN202011092956 A CN 202011092956A CN 112254723 B CN112254723 B CN 112254723B
Authority
CN
China
Prior art keywords
acc
mag
measurement
equation
magnetometer
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
CN202011092956.0A
Other languages
English (en)
Other versions
CN112254723A (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.)
Tianjin Jinhang Computing Technology Research Institute
Original Assignee
Tianjin Jinhang Computing Technology Research Institute
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 Tianjin Jinhang Computing Technology Research Institute filed Critical Tianjin Jinhang Computing Technology Research Institute
Priority to CN202011092956.0A priority Critical patent/CN112254723B/zh
Publication of CN112254723A publication Critical patent/CN112254723A/zh
Application granted granted Critical
Publication of CN112254723B publication Critical patent/CN112254723B/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/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
    • 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
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T90/00—Enabling technologies or technologies with a potential or indirect contribution to GHG emissions mitigation

Landscapes

  • Engineering & Computer Science (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Mathematical Physics (AREA)
  • Mathematical Analysis (AREA)
  • Mathematical Optimization (AREA)
  • Theoretical Computer Science (AREA)
  • Data Mining & Analysis (AREA)
  • Computational Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Pure & Applied Mathematics (AREA)
  • Computing Systems (AREA)
  • Environmental & Geological Engineering (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Algebra (AREA)
  • General Life Sciences & Earth Sciences (AREA)
  • Databases & Information Systems (AREA)
  • Software Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Geology (AREA)
  • Navigation (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明涉及一种基于自适应EKF算法的小型无人机MARG航姿估计方法，属于组合导航技术领域。本发明中，首先，将磁力计和加速度计量测更新分开进行，增加了磁力计误差评估，在磁力计误差较大时，能够隔离磁力计误差，避免对水平姿态估计产生干扰。其次，本发明还提出了针对外部加速度的自适应滤波算法，为了提高估计精度，不直接判断三轴加速度模值，而是对三轴的残差进行分析，再对相应的量测噪声进行自适应调整，避免损失有用的加速度信息，提高姿态估计精度。

Description

基于自适应EKF算法的小型无人机MARG航姿估计方法
技术领域
本发明属于组合导航技术领域，具体涉及一种基于自适应EKF算法的小型无人机MARG航姿估计方法。
背景技术
小型无人机由于其体积和成本限制，通常使用低成本的MEMS惯性器件作为其导航***的主要传感器，结合磁力计和GPS卫星定位***进行组合导航。在飞行中遇到卫星数不足或在室内飞行的情况下，需要切换到航姿模式(AHRS)提供姿态信息。常用的AHRS姿态算法有互补滤波法、梯度下降法和卡尔曼滤波等，都是基于MARG(magnetic,angular rate,and gravity)传感器测量模型进行姿态估计。前两种算法实现起来比较容易，无需针对***进行专门的建模，在大部分的情况下可以满足飞行要求，但遇到大的机动飞行或磁干扰等情况，会导致姿态角精度下降、误差收敛速度慢等情况，使飞机发生振荡、自旋等现象，影响飞行安全。卡尔曼滤波相对前两种方法更为复杂，需要对***进行建模，但可以对不同传感器的量测噪声进行调节，同时还能对传感器的误差进行估计。飞机的导航***方程通常都是非线性的，扩展卡尔曼滤波(Extended Kalman Filter，EKF)可将***方程和量测方程作泰勒级数展开，略去高阶项，近似为线性***，再做卡尔曼滤波估计。
发明内容
(一)要解决的技术问题
本发明要解决的技术问题是：如何设计一种小型无人机MARG航姿估计方法，提高姿态估计精度。
(二)技术方案
为了解决上述技术问题，本发明提供了一种基于自适应EKF算法的小型无人机MARG航姿估计方法，包括以下步骤：
步骤一、采集陀螺仪测量值
Figure BDA0002722772570000021
加速度计测量值
Figure BDA0002722772570000022
和磁力计的测量值
Figure BDA0002722772570000023
步骤二、更新载体姿态四元数；
姿态四元数的递推计算公式为：
Figure BDA0002722772570000024
式中，
Figure BDA0002722772570000025
表示四元数乘法，qk为当前时刻k的姿态四元数，k＝0,1,2，qnew为[t,tk+1]时间段内载体姿态变化四元数；
Figure BDA0002722772570000026
式中，Δθx,Δθy,Δθz是[t,tk+1]时间段内x,y,z陀螺的角增量输出，Δθ是[t,tk+1]时间段内的角增量向量，
Figure BDA0002722772570000027
步骤三、导航坐标系选用采用北-东-地坐标系，计算姿态矩阵；
由所述姿态四元数得到机体坐标系至导航坐标系的姿态矩阵为：
Figure BDA0002722772570000028
其中，q0,q1,q2,q3为姿态四元数；
步骤四、选取所述姿态四元数的扩展卡尔曼滤波***状态向量为:
X＝[q0 q1 q2 q3 bwx bwy bwz]
式中，bwx,bwy,bwz为三轴的陀螺漂移；
步骤五、基于步骤四建立***状态方程；
***离散时间下的状态空间方程为：
Figure BDA0002722772570000031
式中，Xk是k时刻的状态向量；f(Xk)为状态更新方程，是7维非线性向量函数，h(Xk-1)是量测矩阵，为6维非线性向量函数；Wk是协方差矩阵为Q的零均值白噪声；Vk协方差矩阵为R的观测噪声；Zk是k时刻的观测向量；
步骤六、基于步骤五进行Q-EKF滤波状态更新；
***状态更新方程为：
Figure BDA0002722772570000032
bw＝[bwx,bwy,bwz]，对***状态方程f(·)在参考状态Xk-1邻域附近进行泰勒展开并取一阶近似项可得***的状态转移矩阵为：
Figure BDA0002722772570000033
四元数的EKF滤波状态更新方程如下：
Figure BDA0002722772570000034
式中，Pk为均方误差矩阵，Qk为***噪声矩阵：
Figure BDA0002722772570000035
Qquat为姿态四元数***噪声，Qwb为角速度漂移***噪声。
优选地，还包括步骤七、自适应调节加速度量测噪声；
当飞机进行加减速或机动飞行时，加速度的残差yacc的值会发生较大的变化，利用三轴的残差值来分析外部加速度的影响，动态调节三轴加速度的量测噪声，设计量测噪声自适应调节方程如下：
Figure BDA0002722772570000041
式中，n＝x,y,z代表机体坐标系的三个方向，Racc,n为自适应调节后的加速度量测噪声，Racc,ori是滤波器初始化设置的加速度量测噪声，ymax是残差的阈值，当yacc,n超过阈值时，说明当前方向的外部加速度较大，则需要对量测噪声进行调整，ρn为yacc,n的影响因子。
优选地，还包括步骤八、基于步骤三、步骤五、步骤七进行加速度量测更新；
量测方程分为两部分，如下：
Figure BDA0002722772570000042
式中，ZAcc,ZMag为观测向量，Vk,acc,Vk,mag为观测噪声；
加速度量测方程为：
Figure BDA0002722772570000043
式中，
Figure BDA0002722772570000044
为导航坐标系至机体坐标系的转换矩阵，
Figure BDA0002722772570000045
gn＝[0 0 g]T为重力在导航坐标系下的投影，对***量测方程hacc进行泰勒展开并取一阶近似项得到加速度量测矩阵为：
Figure BDA0002722772570000051
则加速度量测更新方程为：
Figure BDA0002722772570000052
Xk＝Xk/k-1+Kacc,kYacc,k
Pk＝(I-Kacc,kHacc,k)Pk/k-1
式中，Kacc为加速度滤波增益矩阵，Yacc,k为加速度残差，Racc,k为加速度计量测噪声，Yacc,k＝Zacc,k-hacc,k(Xk/k-1)，其中Zacc,k为当前时刻加速度计的测量值。
优选地，还包括步骤九、基于步骤一判断磁力计精度。
优选地，步骤九具体为：
当满足以下任一条件时，不进行地磁量测更新：
Figure BDA0002722772570000053
式中，||mb||为当前时刻地磁强度的模值，
Figure BDA0002722772570000054
为起飞前地磁强度的模值，
Figure BDA0002722772570000055
为前Δt时间内仅使用四元数更新得到的航向值，
Figure BDA0002722772570000056
为前Δt时间内使用磁力计解算得到的航向变化值，
Figure BDA0002722772570000057
磁航向计算方法如下：
Figure BDA0002722772570000058
Figure BDA0002722772570000059
为地磁强度在x轴的分量的计算值，
Figure BDA00027227725700000510
为地磁强度在y轴的分量的计算值。
优选地，步骤十、基于步骤三、步骤六进行地磁量测更新。
优选地，步骤十具体为：
设
Figure BDA00027227725700000511
为机体坐标系下的地磁测量值在导航坐标系下的投影，则
Figure BDA00027227725700000512
设mn为理想情况下导航坐标系的地磁分量，则：
Figure BDA0002722772570000061
Figure BDA0002722772570000062
为地磁强度在z轴的分量的计算值；
根据上述关系，地磁量测方程为：
Figure BDA0002722772570000063
对***量测方程hmag进行泰勒展开并取一阶近似项得到地磁量测矩阵为：
Figure BDA0002722772570000064
则地磁量测更新方程为：
Figure BDA0002722772570000065
Xk＝Xk/k-1+Kmag,kYmag,k
Pk＝(I-Kmag,kHmag,k)Pk/k-1
式中，Kmag为磁力计滤波增益矩阵，Ymag,k为磁力计残差Rmag为磁力计量测噪声，Ymag,k＝Zmag,k-hmag,k(Xk/k-1)，Zmag,k为当前时刻磁力计的测量值。
优选地，Δt＝1s。
优选地，所述方法基于MARG的传感器测量模型进行姿态估计。
本发明还提供了一种所述的方法在组合导航技术领域中的应用。
(三)有益效果
本发明中，首先，将磁力计和加速度计量测更新分开进行，增加了磁力计误差评估，在磁力计误差较大时，能够隔离磁力计误差，避免对水平姿态估计产生干扰。其次，本发明还提出了针对外部加速度的自适应滤波算法，当飞行时外部加速度较大时，会导致姿态估计精度下降。通常的方法是将三轴加速度的模值与标准重力值g进行比较，如果差值较大，增大加速度的量测噪声，这样相当于舍弃三轴的加速度信息，无法对加速度测量值进行充分利用。本发明为了提高估计精度，不直接判断三轴加速度模值，而是对三轴的残差进行分析，再对相应的量测噪声进行自适应调整，避免损失有用的加速度信息，提高姿态估计精度。
附图说明
图1是本发明的提供的卡尔曼滤波流程图；
图2是本发明提出改进Q-EKF和普通Q-EKF的姿态误差对比图。
具体实施方式
为使本发明的目的、内容、和优点更加清楚，下面结合附图和实施例，对本发明的具体实施方式作进一步详细描述。
本发明提供了一种基于自适应EKF算法的小型无人机MARG航姿估计方法，基于MARG的传感器测量模型进行姿态估计。采用四元数的扩展卡尔曼滤波算法(QuaternionExtended Kalman Filter，Q-EKF)，并对Q-EKF滤波器进行了改进。首先，将磁力计和加速度计量测更新分开进行，增加磁力计误差评估，在磁力计误差较大时，能够隔离磁力计误差，避免对水平姿态估计产生干扰。其次，本发明提出了针对外部加速度的自适应滤波算法，针对飞机加减速以及振动对水平姿态估计的影响，利用三轴的残差信息，设计了量测噪声自适应调节方程,提高姿态估计精度。具体包括如下步骤：
步骤一、采集陀螺仪测量值
Figure BDA0002722772570000071
加速度计测量值
Figure BDA0002722772570000072
和磁力计的测量值
Figure BDA0002722772570000073
步骤二、更新载体姿态四元数，四元数递推计算公式为：
Figure BDA0002722772570000081
式中，
Figure BDA0002722772570000082
表示四元数乘法，qk为当前时刻k的姿态四元数，k＝0,1,2，qnew为[t,tk+1]时间段内载体姿态变化四元数；
Figure BDA0002722772570000083
式中，Δθx,Δθy,Δθz是[t,tk+1]时间段内x,y,z陀螺的角增量输出(陀螺仪测量值)，Δθ是[t,tk+1]时间段内的角增量向量，
Figure BDA0002722772570000084
步骤三、导航坐标系(n系)选用采用北-东-地坐标系，计算姿态矩阵，由所述姿态四元数得到机体坐标系至导航坐标系的姿态矩阵为：
Figure BDA0002722772570000085
其中，q0,q1,q2,q3为姿态四元数；
步骤四、选取所述姿态四元数的扩展卡尔曼滤波***状态向量为:
X＝[q0 q1 q2 q3 bwx bwy bwz]
式中，bwx,bwy,bwz为三轴的陀螺漂移；
步骤五、基于步骤四建立***状态方程；
***离散时间下的状态空间方程为：
Figure BDA0002722772570000091
式中，Xk是k时刻的状态向量；f(Xk)为状态更新方程，是7维非线性向量函数，h(Xk-1)是量测矩阵，为6维非线性向量函数；Wk是协方差矩阵为Q的零均值白噪声；Vk协方差矩阵为R的观测噪声；Zk是k时刻的观测向量；
步骤六、基于步骤五进行Q-EKF滤波状态更新；
***状态更新方程为：
Figure BDA0002722772570000092
bw＝[bwx,bwy,bwz]，对***状态方程f(·)在参考状态Xk-1邻域附近进行泰勒展开并取一阶近似项可得***的状态转移矩阵为：
Figure BDA0002722772570000093
四元数的EKF滤波状态更新方程如下：
Figure BDA0002722772570000094
式中，Pk为均方误差矩阵，Qk为***噪声矩阵：
Figure BDA0002722772570000095
Qquat为姿态四元数***噪声，Qwb为角速度漂移***噪声；
步骤七、自适应调节加速度量测噪声；
当飞机进行加减速或机动飞行时，加速度的残差yacc的值会发生较大的变化，利用三轴的残差值来分析外部加速度的影响，动态调节三轴加速度的量测噪声，设计量测噪声自适应调节方程如下：
Figure BDA0002722772570000101
式中，n＝x,y,z代表机体坐标系的三个方向，Racc,n为自适应调节后的加速度量测噪声，Racc,ori是滤波器初始化设置的加速度量测噪声，ymax是残差的阈值，当yacc,n超过阈值时，说明当前方向的外部加速度较大，需要对量测噪声进行调整，ρn为yacc,n的影响因子，可加快量测噪声的调节速度；
步骤八、基于步骤三、步骤五、步骤七进行加速度量测更新；
量测方程分为两部分，如下：
Figure BDA0002722772570000102
式中，ZAcc,ZMag为观测向量，Vk,acc,Vk,mag为观测噪声；
加速度量测方程为：
Figure BDA0002722772570000103
式中，
Figure BDA0002722772570000104
为导航坐标系至机体坐标系的转换矩阵，
Figure BDA0002722772570000105
gn＝[0 0 g]T为重力在导航坐标系下的投影，对***量测方程hacc进行泰勒展开并取一阶近似项得到加速度量测矩阵为：
Figure BDA0002722772570000106
则加速度量测更新方程为：
Figure BDA0002722772570000107
Xk＝Xk/k-1+Kacc,kYacc,k
Pk＝(I-Kacc,kHacc,k)Pk/k-1
式中，Kacc为加速度滤波增益矩阵，Yacc,k为加速度残差，Racc,k为加速度计量测噪声，Yacc,k＝Zacc,k-hacc,k(Xk/k-1)，其中Zacc,k为当前时刻加速度计的测量值；
步骤九、基于步骤一判断磁力计精度；
无人机在一定范围内飞行时，地磁强度可视为固定值，当无人机所处的外部环境存在干扰时，磁力计测量的地磁强度会产生波动，对航向造成影响；当满足以下任一条件时，说明磁力计受到了较大的干扰，不进行地磁量测更新：
Figure BDA0002722772570000111
式中，||mb||为当前时刻地磁强度的模值，
Figure BDA0002722772570000112
为起飞前地磁强度的模值，
Figure BDA0002722772570000113
为前Δt时间内仅使用四元数更新得到的航向值，
Figure BDA0002722772570000114
为前Δt时间内使用磁力计解算得到的航向变化值，
Figure BDA0002722772570000115
本实施例中Δt＝1s，磁航向计算方法如下：
Figure BDA0002722772570000116
Figure BDA0002722772570000117
为地磁强度在x轴的分量的计算值，
Figure BDA0002722772570000118
为地磁强度在y轴的分量的计算值；
步骤十、基于步骤三、步骤六进行地磁量测更新；
设
Figure BDA0002722772570000119
为机体坐标系下的地磁测量值在导航坐标系下的投影，则
Figure BDA00027227725700001110
设mn为理想情况下导航坐标系的地磁分量，则：
Figure BDA00027227725700001111
Figure BDA00027227725700001112
为地磁强度在z轴的分量的计算值；
，根据上述关系，地磁量测方程为：
Figure BDA00027227725700001113
对***量测方程hmag进行泰勒展开并取一阶近似项可得地磁量测矩阵为：
Figure BDA0002722772570000121
则地磁量测更新方程为：
Figure BDA0002722772570000122
Xk＝Xk/k-1+Kmag,kYmag,k
Pk＝(I-Kmag,kHmag,k)Pk/k-1
式中，Kmag为磁力计滤波增益矩阵，Ymag,k为磁力计残差Rmag为磁力计量测噪声，Ymag,k＝Zmag,k-hmag,k(Xk/k-1)，Zmag,k为当前时刻磁力计的测量值。
以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变形，这些改进和变形也应视为本发明的保护范围。

Claims (5)

1.一种基于自适应EKF算法的小型无人机MARG航姿估计方法，其特征在于，包括以下步骤：
步骤一、采集陀螺仪测量值
Figure FDA0003715853480000011
加速度计测量值
Figure FDA0003715853480000012
和磁力计的测量值
Figure FDA0003715853480000013
步骤二、更新载体姿态四元数；
姿态四元数的递推计算公式为：
Figure FDA0003715853480000014
式中，
Figure FDA0003715853480000015
表示四元数乘法，qk为当前时刻k的姿态四元数，k＝0,1,2，qnew为[t,tk+1]时间段内载体姿态变化四元数；
Figure FDA0003715853480000016
式中，Δθx,Δθy,Δθz是[t,tk+1]时间段内x,y,z陀螺的角增量输出，Δθ是[t,tk+1]时间段内的角增量向量，
Figure FDA0003715853480000017
步骤三、导航坐标系选用采用北-东-地坐标系，计算姿态矩阵；
由所述姿态四元数得到机体坐标系至导航坐标系的姿态矩阵为：
Figure FDA0003715853480000018
其中，q0,q1,q2,q3为姿态四元数；
步骤四、选取所述姿态四元数的扩展卡尔曼滤波***状态向量为:
X＝[q0 q1 q2 q3 bwx bwy bwz]
式中，bwx,bwy,bwz为三轴的陀螺漂移；
步骤五、基于步骤四建立***状态方程；
***离散时间下的状态空间方程为：
Figure FDA0003715853480000021
式中，Xk是k时刻的状态向量；f(Xk)为状态更新方程，是7维非线性向量函数，h(Xk-1)是量测矩阵，为6维非线性向量函数；Wk是协方差矩阵为Q的零均值白噪声；Vk是协方差矩阵为R的观测噪声；Zk是k时刻的观测向量；
步骤六、基于步骤五进行Q-EKF滤波状态更新；
***状态更新方程为：
Figure FDA0003715853480000022
bw＝[bwx,bwy,bwz]，对***状态方程f(·)在参考状态Xk-1邻域附近进行泰勒展开并取一阶近似项可得***的状态转移矩阵为：
Figure FDA0003715853480000023
四元数的EKF滤波状态更新方程如下：
Figure FDA0003715853480000024
式中，Pk为均方误差矩阵，Qk为***噪声矩阵：
Figure FDA0003715853480000025
Qquat为姿态四元数***噪声，Qwb为角速度漂移***噪声；
还包括步骤七、自适应调节加速度量测噪声；
量测噪声自适应调节方程如下：
Figure FDA0003715853480000031
式中，n＝x,y,z代表机体坐标系的三个方向，Racc,n为自适应调节后的加速度量测噪声，Racc,ori是滤波器初始化设置的加速度量测噪声，ymax是残差的阈值，ρn为yacc,n的影响因子；
还包括步骤八、基于步骤三、步骤五、步骤七进行加速度量测更新；
量测方程分为两部分，如下：
Figure FDA0003715853480000032
式中，ZAcc,ZMag分别为加速度计观测向量、磁力计观测向量，Vk,acc,Vk,mag为加速度计观测噪声、磁力计观测噪声；
加速度量测方程为：
Figure FDA0003715853480000033
式中，
Figure FDA0003715853480000034
为导航坐标系至机体坐标系的转换矩阵，
Figure FDA0003715853480000035
gn＝[0 0 g]T为重力在导航坐标系下的投影，对***量测方程hacc进行泰勒展开并取一阶近似项得到加速度量测矩阵为：
Figure FDA0003715853480000036
则加速度量测更新方程为：
Figure FDA0003715853480000041
Xk＝Xk/k-1+Kacc,kYacc,k
Pk＝(I-Kacc,kHacc,k)Pk/k-1
式中，Kacc为加速度滤波增益矩阵，Yacc,k为加速度残差，Racc,k为加速度计量测噪声，Yacc,k＝Zacc,k-hacc,k(Xk/k-1)，其中Zacc,k为当前时刻加速度计的测量值；
还包括步骤九、基于步骤一判断磁力计精度；
步骤九具体为：
当满足以下任一条件时，不进行地磁量测更新：
Figure FDA0003715853480000042
式中，||mb||为当前时刻地磁强度的模值，
Figure FDA0003715853480000043
为起飞前地磁强度的模值，
Figure FDA0003715853480000044
为前Δt时间内仅使用四元数更新得到的航向值，
Figure FDA0003715853480000045
为前Δt时间内使用磁力计解算得到的航向变化值，
Figure FDA0003715853480000046
磁航向计算方法如下：
Figure FDA0003715853480000047
Figure FDA0003715853480000048
为地磁强度在x轴的分量的计算值，
Figure FDA0003715853480000049
为地磁强度在y轴的分量的计算值。
2.如权利要求1所述的方法，其特征在于，步骤十、基于步骤三、步骤六进行地磁量测更新。
3.如权利要求2所述的方法，其特征在于，步骤十具体为：
设
Figure FDA00037158534800000410
为机体坐标系下的地磁测量值在导航坐标系下的投影，则
Figure FDA00037158534800000411
设mn为理想情况下导航坐标系的地磁分量，则：
Figure FDA00037158534800000412
Figure FDA0003715853480000051
为地磁强度在z轴的分量的计算值；
地磁量测方程为：
Figure FDA0003715853480000052
对***量测方程hmag进行泰勒展开并取一阶近似项得到地磁量测矩阵为：
Figure FDA0003715853480000053
则地磁量测更新方程为：
Figure FDA0003715853480000054
Xk＝Xk/k-1+Kmag,kYmag,k
Pk＝(I-Kmag,kHmag,k)Pk/k-1
式中，Kmag为磁力计滤波增益矩阵，Ymag,k为磁力计残差Rmag为磁力计量测噪声，Ymag,k＝Zmag,k-hmag,k(Xk/k-1)，Zmag,k为当前时刻磁力计的测量值。
4.如权利要求1所述的方法，其特征在于，Δt＝1s。
5.如权利要求1至4中任一项所述的方法，其特征在于，所述方法基于MARG的传感器测量模型进行姿态估计。
CN202011092956.0A 2020-10-13 2020-10-13 基于自适应ekf算法的小型无人机marg航姿估计方法 Active CN112254723B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011092956.0A CN112254723B (zh) 2020-10-13 2020-10-13 基于自适应ekf算法的小型无人机marg航姿估计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011092956.0A CN112254723B (zh) 2020-10-13 2020-10-13 基于自适应ekf算法的小型无人机marg航姿估计方法

Publications (2)

Publication Number Publication Date
CN112254723A CN112254723A (zh) 2021-01-22
CN112254723B true CN112254723B (zh) 2022-08-16

Family

ID=74243145

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011092956.0A Active CN112254723B (zh) 2020-10-13 2020-10-13 基于自适应ekf算法的小型无人机marg航姿估计方法

Country Status (1)

Country Link
CN (1) CN112254723B (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113063416B (zh) * 2021-02-05 2023-08-08 重庆大学 一种基于自适应参数互补滤波的机器人姿态融合方法
CN113175926B (zh) * 2021-04-21 2022-06-21 哈尔滨工程大学 一种基于运动状态监测的自适应水平姿态测量方法

Family Cites Families (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20020008661A1 (en) * 2000-07-20 2002-01-24 Mccall Hiram Micro integrated global positioning system/inertial measurement unit system
CN101782391A (zh) * 2009-06-22 2010-07-21 北京航空航天大学 机动加速度辅助的扩展卡尔曼滤波航姿***姿态估计方法
DE102012011632A1 (de) * 2011-07-28 2013-01-31 Memsic Inc. System und Verfahren zum Erfassen einer Raumstellung und Winkelrate unter Verwendung eines Magnetfeldsensors und Akzelerometers für tragbare elektronische Vorrichtungen
CN103822633B (zh) * 2014-02-11 2016-12-07 哈尔滨工程大学 一种基于二阶量测更新的低成本姿态估计方法
CN108645404B (zh) * 2018-03-30 2021-05-11 西安建筑科技大学 一种小型多旋翼无人机姿态解算方法
CN109974714B (zh) * 2019-04-29 2023-07-04 南京航空航天大学 一种Sage-Husa自适应无迹卡尔曼滤波姿态数据融合方法
CN110174121A (zh) * 2019-04-30 2019-08-27 西北工业大学 一种基于地磁场自适应修正的航姿***姿态解算方法
CN111426318B (zh) * 2020-04-22 2024-01-26 中北大学 基于四元数-扩展卡尔曼滤波的低成本ahrs航向角补偿方法

Also Published As

Publication number Publication date
CN112254723A (zh) 2021-01-22

Similar Documents

Publication Publication Date Title
CN111351482B (zh) 基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法
CN109813311B (zh) 一种无人机编队协同导航方法
CN106959110B (zh) 一种云台姿态检测方法及装置
JP4782111B2 (ja) 輸送手段の位置、姿勢、および／または飛行方向を推定するシステムおよび方法
Kingston et al. Real-time attitude and position estimation for small UAVs using low-cost sensors
CN110207691B (zh) 一种基于数据链测距的多无人车协同导航方法
CN111207745B (zh) 一种适用于大机动无人机垂直陀螺仪的惯性测量方法
Mahony et al. A non-linear observer for attitude estimation of a fixed-wing unmanned aerial vehicle without GPS measurements
CN104880189B (zh) 一种动中通天线低成本跟踪抗干扰方法
CN112254723B (zh) 基于自适应ekf算法的小型无人机marg航姿估计方法
CN112066985B (zh) 一种组合导航***初始化方法、装置、介质及电子设备
US20220155800A1 (en) Method and apparatus for yaw fusion and aircraft
CN111238469B (zh) 一种基于惯性/数据链的无人机编队相对导航方法
CN111189442B (zh) 基于cepf的无人机多源导航信息状态预测方法
CN108627152B (zh) 一种微型无人机基于多传感器数据融合的导航方法
CN110793515A (zh) 一种基于单天线gps和imu的大机动条件下无人机姿态估计方法
CN110849360A (zh) 面向多机协同编队飞行的分布式相对导航方法
Goppert et al. Invariant Kalman filter application to optical flow based visual odometry for UAVs
CN105928519B (zh) 基于ins惯性导航与gps导航以及磁力计的导航算法
CN112284388B (zh) 一种无人机多源信息融合导航方法
Emran et al. A cascaded approach for quadrotor's attitude estimation
CN111486841A (zh) 一种基于激光定位***的无人机导航定位方法
Yang et al. Model-free integrated navigation of small fixed-wing UAVs full state estimation in wind disturbance
CN104864868B (zh) 一种基于近距离地标测距的组合导航方法
Xu et al. EKF based multiple-mode attitude estimator for quadrotor using inertial measurement unit

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