CN108645404B - 一种小型多旋翼无人机姿态解算方法 - Google Patents

一种小型多旋翼无人机姿态解算方法 Download PDF

Info

Publication number
CN108645404B
CN108645404B CN201810275931.0A CN201810275931A CN108645404B CN 108645404 B CN108645404 B CN 108645404B CN 201810275931 A CN201810275931 A CN 201810275931A CN 108645404 B CN108645404 B CN 108645404B
Authority
CN
China
Prior art keywords
state
covariance
stage
calculating
quaternion
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
CN201810275931.0A
Other languages
English (en)
Other versions
CN108645404A (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.)
Xian University of Architecture and Technology
Original Assignee
Xian University of Architecture and Technology
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 Xian University of Architecture and Technology filed Critical Xian University of Architecture and Technology
Priority to CN201810275931.0A priority Critical patent/CN108645404B/zh
Publication of CN108645404A publication Critical patent/CN108645404A/zh
Application granted granted Critical
Publication of CN108645404B publication Critical patent/CN108645404B/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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/04—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means
    • G01C21/08—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means involving use of the magnetic field of the earth
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • 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
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices

Abstract

本发明公开了一种小型多旋翼无人机姿态解算方法，采用四元数作为状态向量，使用陀螺仪数据对状态向量进行更新，将加速度数据和磁力计数据分为两个阶段进行处理。第一阶段，使用改进的无迹卡尔曼滤波算法结合加速度计数据对四元数状态向量进行初步校正；第二阶段，使用改进的无迹卡尔曼滤波算法结合磁力计数据对四元数状态向量进行进一步的校正。该方法将加速度计和磁力计数据分为两个阶段来处理，降低了量测向量的维度，可使用较小的矩阵进行计算，所需的计算能力较少，还减小了磁异常对横滚角和俯仰角估计精度的影响。

Description

一种小型多旋翼无人机姿态解算方法
技术领域
本发明属于无人机控制及惯性导航领域，更具体地，涉及一种小型多旋翼无人机姿态解算方法。
背景技术
虽然多旋翼无人机的应用已经非常广泛，但是在多旋翼无人机的研究过程中依旧存在着许多的问题。其中一个很重要的问题就是多旋翼无人机通常尺寸较小，无法使用传统的体积和重量都比较大的高精度的传感器，而是使用的是价格低廉的MEMS惯性传感器，但是目前的MEMS惯性传感器相较于那些军用级别的高精度惯性传感器来说精度低、稳定性较差并且其系统误差随时间积累比较快，无法满足多旋翼无人机姿态测量的要求。然而姿态估计的精度是影响多旋翼飞行器性能的关键因素，因此为了解决在姿态估计中系统迅速发散的问题，通常采用的办法是综合多个传感器的优点，设计基于多传感器的姿态估计系统。目前多旋翼无人机的导航通常采用MEMS磁力计、加速度计和陀螺仪的组合(MARG)来作为其姿态估计系统。MARG传感器可看作具有一定冗余度的传感器配置，从而产生了数据融合的问题，如何将各传感器的数据取长补短、提高姿态解算精度具有重要的实际意义和研究价值。
出于对制造成本、功耗和体积尺寸等多方面的考虑，现在的小型多旋翼无人机多使用低成本的微控制器作为主控芯片，这种低成本微控制器计算能力有限。无人机作为一种复杂的嵌入式系统，其姿态解算算法对实时性有很高的要求，所以无人机姿态解算算法除了需要达到足够的解算精度外，也需要有较少的解算时间。
发明内容
针对上述现有技术存在的缺陷或不足，本发明的目的在于，提供一种小型多旋翼无人机姿态解算方法。
为了实现上述任务，本发明采用如下的技术解决方案：
一种小型多旋翼无人机姿态解算方法，其特征在于，该方法采用四元数作为状态向量，使用陀螺仪数据对状态向量进行更新，将加速度数据和磁力计数据分为两个阶段进行处理，在第一阶段，使用一种改进的无迹卡尔曼滤波器结合加速度计数据对四元数状态向量进行初步校正；在第二阶段，使用改进的无迹卡尔曼滤波算法结合磁力计数据对四元数状态向量进行进一步的校正。
根据本发明，具体按以下步骤进行：
步骤1：确定初始状态估计值
Figure BDA0001613650390000021
及其协方差
Figure BDA0001613650390000022
读取陀螺仪数据wx，wy和wz，计算离散时间状态转移矩阵Ak/k-1，计算状态预测值
Figure BDA0001613650390000023
及其协方差
Figure BDA0001613650390000024
步骤2：Sigma点选取：
根据状态预测值及其协方差选择一组加权Sigma点；
步骤3：第一阶段量测更新：
计算经过非线性量测方程h1(·)变换后的Sigma点，计算量测预测值
Figure BDA0001613650390000025
及其协方差
Figure BDA0001613650390000026
计算状态预测值与量测预测值间的互协方差
Figure BDA0001613650390000027
确定第一阶段卡尔曼增益矩阵Kk1，读取加速度计数据
Figure BDA0001613650390000028
计算第一阶段校正参数qε1，更新第一阶段状态估计值
Figure BDA0001613650390000029
及其协方差
Figure BDA00016136503900000210
步骤4：第二阶段量测更新：
计算经过非线性量测方程h2(·)变换后的Sigma点，计算量测预测值
Figure BDA00016136503900000211
及其协方差
Figure BDA00016136503900000212
计算状态预测值
Figure BDA00016136503900000213
与量测预测值间的互协方差
Figure BDA0001613650390000031
确定第二阶段卡尔曼增益矩阵Kk2，读取磁力计数据
Figure BDA0001613650390000032
计算第二阶段校正参数qε2，其中，
Figure BDA0001613650390000033
更新第二阶段状态估计值
Figure BDA0001613650390000034
及其协方差
Figure BDA0001613650390000035
步骤5：完成一个滤波周期的计算，得到滤波校正后的四元数，将其转换为姿态角；
步骤6：重复步骤1到步骤5的过程。
本发明的小型多旋翼无人机姿态解算方法，与现有技术相比，带来的技术效果是：
(1)四元数状态向量的状态方程具有线性特征，使用传统的无迹卡尔曼滤波进行滤波解算时，在求取状态预测值及其方差时，会产生大量的冗余计算，严重影响系统的实时性，使用改进的无迹卡尔曼滤波器可以克服传统无迹卡尔曼滤波的这个缺陷。
(2)在环境中存在磁异常时，可以选择是否打开第二阶段以开启或关闭磁力计校正，以屏蔽磁异常对姿态角估计的影响，两段式滤波器提供了更大的操作灵活性。
(3)使用低维度四元数作为状态向量，并且将加速度计和磁力计数据分为两个阶段来处理，降低了量测向量的维度，可以使用较小的矩阵进行操作，减少了一个滤波周期的的解算时间，提高了系统的实时性。
(4)使用这种两段式滤波器可以消除加速度计数据对偏航角估计的干扰以及磁力计数据对横滚角和俯仰角估计的干扰。
附图说明
图1是本发明的小型多旋翼无人机姿态解算方法流程图。
图2为本发明所述算法(TDUKF)和无人机自带算法(EKF)解算的横滚角比较图。
图3为本发明所述算法(TDUKF)和无人机自带算法(EKF)解算的俯仰角比较图。
图4为本发明所述算法(TDUKF)和无人机自带算法(EKF)解算的航向角比较图。
以下结合附图和实施例对本发明作进一步的详细说明。
具体实施方式
本实施例给出一种小型多旋翼无人机姿态解算方法，方法采用四元数作为状态向量，使用陀螺仪数据对状态向量进行更新，将加速度数据和磁力计数据分为两个阶段进行处理，在第一阶段，使用一种改进的无迹卡尔曼滤波器结合加速度计数据对四元数状态向量进行初步校正；在第二阶段，使用改进的无迹卡尔曼滤波算法结合磁力计数据对四元数状态向量进行进一步的校正。
具体按以下步骤实施：
步骤1：确定初始状态估计值
Figure BDA0001613650390000042
及其协方差
Figure BDA0001613650390000041
读取陀螺仪数据wx，wy和wz，计算离散时间状态转移矩阵Ak/k-1，计算状态预测值
Figure BDA0001613650390000043
及其协方差
Figure BDA0001613650390000044
步骤2：Sigma点选取：
根据状态预测值及其协方差选择一组加权Sigma点。
步骤3：第一阶段量测更新：
计算经过非线性量测方程h1(·)变换后的Sigma点，计算量测预测值
Figure BDA0001613650390000045
及其协方差
Figure BDA0001613650390000046
计算状态预测值与量测预测值间的互协方差
Figure BDA0001613650390000047
确定第一阶段卡尔曼增益矩阵Kk1，读取加速度计数据
Figure BDA0001613650390000048
计算第一阶段校正参数qε1，更新第一阶段状态估计值
Figure BDA0001613650390000049
及其协方差
Figure BDA00016136503900000410
步骤4：第二阶段量测更新：
计算经过非线性量测方程h2(·)变换后的Sigma点，计算量测预测值
Figure BDA0001613650390000058
及其协方差
Figure BDA0001613650390000059
计算状态预测值与量测预测值间的互协方差
Figure BDA00016136503900000510
确定第二阶段卡尔曼增益矩阵Kk2，读取磁力计数据
Figure BDA00016136503900000511
Figure BDA00016136503900000512
计算第二阶段校正参数qε2，更新第二阶段状态估计值
Figure BDA00016136503900000513
及其协方差
Figure BDA00016136503900000514
步骤5：完成一个滤波周期的计算，得到滤波校正后的四元数，将其转换为姿态角。
步骤6：重复步骤1到步骤5的过程。
本实施例中，所述四元数状态向量的方程为：
qk＝Ak/k-1qk-1+wk (1)
Figure BDA0001613650390000051
Figure BDA0001613650390000052
其中，qk∈Rn，为k时刻的四元数状态向量，
Figure BDA00016136503900000515
和
Figure BDA00016136503900000516
为k时刻的量测向量，Ak/k-1为四元数状态向量方程的状态转移矩阵，h1(·)和h2(·)为描述量测方程的非线性函数，wk，
Figure BDA00016136503900000517
和
Figure BDA00016136503900000518
为互不相关的零均值高斯白噪声过程，其方差为：
Figure BDA0001613650390000053
Figure BDA0001613650390000054
Figure BDA0001613650390000055
所述的状态转移矩阵为：
Figure BDA0001613650390000056
其中I为单位阵，T为解算周期，Ω为陀螺仪数据组成的矩阵，且：
Figure BDA0001613650390000057
其中，wx，wy和wz分别为k-1时刻陀螺仪测得机体坐标系下角速度分量。
所述的非线性量测方程h1(·)和非线性量测方程h2(·)均为非线性函数，其表达式分别为：
Figure BDA0001613650390000061
Figure BDA0001613650390000062
其中：
Figure BDA0001613650390000067
和
Figure BDA0001613650390000068
分别表示为重力场和地磁场在机体坐标系下的估计量，g表示重力加速度，
Figure BDA0001613650390000069
为使用四元数表示的地球坐标系到载体坐标系之间的变换矩阵，其表达式为：
Figure BDA0001613650390000063
所述四元数状态向量的状态方程为线性，状态预测值及其协方差的计算与线性卡尔曼滤波相同，即：
Figure BDA0001613650390000064
Figure BDA0001613650390000065
所述步骤2中选取的sigma点为：
Figure BDA0001613650390000066
其中，a∈R为调节参数，控制Sigma点在
Figure BDA00016136503900000610
周围的分布，
Figure BDA00016136503900000711
为矩阵
Figure BDA00016136503900000712
均方根的第i列，n为状态向量的维数。
所述经过非线性量测方程h1(·)和非线性量测方程h2(·)变换后的Sigma点分别为：
Figure BDA0001613650390000071
Figure BDA0001613650390000072
步骤3、4中所述的计算量测预测值及其互协方差和计算状态预测值与量测预测值间的互协方差的计算公式分别为：
Figure BDA0001613650390000073
Figure BDA0001613650390000074
Figure BDA0001613650390000075
Figure BDA0001613650390000076
Figure BDA0001613650390000077
Figure BDA0001613650390000078
其中，
Figure BDA0001613650390000079
步骤3和步骤4中所述的卡尔曼增益矩阵Kk1和卡尔曼增益矩阵Kk2分别为：
Figure BDA00016136503900000710
Figure BDA0001613650390000081
为保证偏航角不被加速度计校正过程影响，校正参数四元数qε的第三个矢量部分被置为0；步骤3中，计算第一阶段校正参数qε1，更新状态估计值
Figure BDA00016136503900000811
及其协方差
Figure BDA00016136503900000812
所用公式分别为：
Figure BDA0001613650390000082
qε1＝[qε,0 qε,1 qε,2 0·qε,3]T (27)
Figure BDA0001613650390000083
Figure BDA0001613650390000084
其中qε,0,qε,1,qε,2和qε,3分别为四元数qε的四个元素；
为保证横滚角和俯仰角不被磁力计校正过程影响，校正参数qε的第一和第二个矢量部分被置为0；步骤4中，计算第二阶段校正参数qε2，更新状态估计值
Figure BDA00016136503900000813
及其协方差
Figure BDA00016136503900000814
所用公式分别为：
Figure BDA0001613650390000085
qε2＝[qε,0 0·qε,1 0·qε,2 qε,3]T (31)
Figure BDA0001613650390000086
Figure BDA0001613650390000087
其中qε,0,qε,1,qε,2和qε,3分别为四元数qε的四个元素。
所述步骤5中四元数转姿态角的公式为：
Figure BDA0001613650390000088
Figure BDA0001613650390000089
Figure BDA00016136503900000810
以下给出一个解算周期的完整算法流程的实施例。
如图1所示，本实施例给出一种小型多旋翼无人机姿态解算方法，系统组成包括有陀螺仪、加速度计、磁力计和改进的二段式无迹卡尔曼滤波器，具体计算流程如下：
1、计算系统先验估计
(1)确定上个解算周期的状态估计值
Figure BDA0001613650390000097
及其协方差
Figure BDA0001613650390000096
(2)读取陀螺仪数据wx，wy和wz。
(3)计算离散时间状态转移矩阵
Figure BDA0001613650390000091
其中I为单位阵，T为解算周期，Ω为陀螺仪数据组成的矩阵：
Figure BDA0001613650390000092
(4)时间更新:
由于四元数状态向量的状态方程为线性，状态预测值及其协方差的计算与线性卡尔曼滤波相同，即
Figure BDA0001613650390000093
Figure BDA0001613650390000094
(5)Sigma点选取：
根据状态预测值及其协方差选择一组加权Sigma点。选取的Sigma点为：
Figure BDA0001613650390000095
其中，a∈R为调节参数，控制Sigma点在
Figure BDA0001613650390000098
周围的分布，
Figure BDA0001613650390000099
为矩阵
Figure BDA00016136503900000910
均方根的第i列，n为状态向量的维数。
2、开始第一阶段
(1)量测更新
经过非线性量测方程h1(·)变换后的Sigma点为：
Figure BDA0001613650390000101
(2)计算量测预测值及其互协方差
Figure BDA0001613650390000102
计算状态预测值与量测预测值间的互协方差
Figure BDA0001613650390000103
其中，
Figure BDA0001613650390000104
(3)确定第一阶段卡尔曼增益矩阵
Figure BDA0001613650390000105
(4)读取加速度计数据zk1＝[axayaz]T。
(5)计算第一阶段校正参数
Figure BDA0001613650390000106
为了保证偏航角不被该校正过程影响，校正参数四元数qε的第三个矢量部分被置为0，即有：
qε1＝[qε,0 qε,1 qε,2 0·qε,3]T
(6)更新状态估计值
Figure BDA0001613650390000109
及其协方差
Figure BDA00016136503900001010
Figure BDA0001613650390000107
Figure BDA0001613650390000108
3、开始第二阶段(可根据需要选择是否开启第二阶段)
(1)量测更新
经过非线性量测方程h2(·)变换后的Sigma点为：
Figure BDA0001613650390000111
(2)计算量测预测值及其互协方差：
Figure BDA0001613650390000112
Figure BDA0001613650390000113
计算状态预测值与量测预测值间的互协方差：
Figure BDA0001613650390000114
其中：
Figure BDA0001613650390000115
(3)确定第二阶段卡尔曼增益矩阵
Figure BDA0001613650390000116
(4)读取磁力计数据zk2＝[mxmymz]T。
(5)计算第二阶段校正参数
Figure BDA0001613650390000117
为了保证横滚角和俯仰角不被该校正过程影响，校正参数qε的第一和第二个矢量部分被置为0，即有：
qε2＝[qε,0 0·qε,1 0·qε,2 qε,3]T
(6)更新状态估计值
Figure BDA00016136503900001111
及其协方差
Figure BDA00016136503900001110
Figure BDA0001613650390000118
Figure BDA0001613650390000119
4、将校正后的状态四元数转换为姿态角
Figure BDA0001613650390000121
Figure BDA0001613650390000122
Figure BDA0001613650390000123
本实施例中，由于四元数状态向量的状态方程存在线性特征，所以在时间更新阶段，按照线性卡尔曼滤波方式进行处理而不进行无迹变换处理；量测方程存在非线性特征，在量测更新阶段时，使用无迹变换进行处理。
本实施例使用低维度四元数作为状态向量，并且将加速度计和磁力计数据分为两个阶段来处理，降低了量测向量的维度，可以使用较小的矩阵进行计算；同时采用改进的无迹卡尔曼滤波器，所需的计算能力较少，有利于增加系统的实时性；在环境中存在磁异常时，可选择是否使用第二阶段以开启或关闭磁力计校正，以屏蔽磁异常对姿态角估计的影响。且二段式滤波器具有更大的操作灵活性；使用加速度计数据，能正确校正横滚角和俯仰角，使用磁力计数据，能正确校正偏航角。这种两段式滤波器还可以减小磁异常对横滚角和俯仰角估计精度的影响。
为验证本发明的小型多旋翼无人机姿态解算方法的有效性，发明人通过采集静止状态无人机的传感器的数据，将经本发明所述的小型多旋翼无人机姿态解算方法(TDUKF)处理的姿态角数据和无人机自带算法(EKF)处理的姿态角进行比较。比较结果如图2、3、4所示，本发明所述小型多旋翼无人机姿态解算方法的横滚角、俯仰角和航行角的估计精度均在0.1度左右，与EKF算法相比有明显提高；且通过20次的蒙特卡洛仿真实验表明，与EKF相比本发明所述算法计算速度提高了约10％左右。

Claims (9)

1.一种小型多旋翼无人机姿态解算方法，其特征在于，该方法采用四元数作为状态向量，使用陀螺仪数据对状态向量进行更新，将加速度数据和磁力计数据分为两个阶段进行处理，在第一阶段，使用一种改进的无迹卡尔曼滤波器结合加速度计数据对四元数状态向量进行初步校正；在第二阶段，使用改进的无迹卡尔曼滤波算法结合磁力计数据对四元数状态向量进行进一步的校正；具体按以下步骤进行：
步骤1：确定初始状态估计值
Figure FDA0002967174490000011
及其协方差
Figure FDA0002967174490000012
读取陀螺仪数据wx，wy和wz，计算离散时间状态转移矩阵Ak/k-1，计算状态预测值
Figure FDA0002967174490000013
及其协方差
Figure FDA0002967174490000014
步骤2：Sigma点选取：
根据状态预测值及其协方差选择一组加权Sigma点；
步骤3：第一阶段量测更新：
计算经过非线性量测方程h1(·)变换后的Sigma点，计算量测预测值
Figure FDA0002967174490000015
及其协方差
Figure FDA0002967174490000016
计算状态预测值与量测预测值间的互协方差
Figure FDA0002967174490000017
确定第一阶段卡尔曼增益矩阵Kk1，读取加速度计数据
Figure FDA0002967174490000018
计算第一阶段校正参数qε1，更新第一阶段状态估计值
Figure FDA0002967174490000019
及其协方差
Figure FDA00029671744900000110
步骤4：第二阶段量测更新：
计算经过非线性量测方程h2(·)变换后的Sigma点，计算量测预测值
Figure FDA00029671744900000111
及其协方差
Figure FDA00029671744900000112
计算状态预测值
Figure FDA00029671744900000113
与量测预测值间的互协方差
Figure FDA00029671744900000114
确定第二阶段卡尔曼增益矩阵Kk2，读取磁力计数据
Figure FDA00029671744900000115
计算第二阶段校正参数qε2，其中，
Figure FDA00029671744900000116
更新第二阶段状态估计值
Figure FDA00029671744900000117
及其协方差
Figure FDA00029671744900000118
步骤5：完成一个滤波周期的计算，得到滤波校正后的四元数，将其转换为姿态角；
步骤6：重复步骤1到步骤5的过程。
2.如权利要求1所述的方法，其特征在于：所述四元数状态向量的方程为：
qk＝Ak/k-1qk-1+wk
Figure FDA0002967174490000021
Figure FDA0002967174490000022
其中，qk∈Rn，为k时刻的四元数状态向量，
Figure FDA0002967174490000023
和
Figure FDA0002967174490000024
为k时刻的量测向量，Ak/k-1为四元数状态向量方程的状态转移矩阵，h1(·)和h2(·)为描述量测方程的非线性函数，wk，
Figure FDA0002967174490000025
和
Figure FDA0002967174490000026
为互不相关的零均值高斯白噪声过程，其方差为：
Figure FDA0002967174490000027
Figure FDA0002967174490000028
Figure FDA0002967174490000029
所述的状态转移矩阵为：
Figure FDA00029671744900000210
其中I为单位阵，T为解算周期，Ω为陀螺仪数据组成的矩阵，且：
Figure FDA00029671744900000211
其中，wx，wy和wz分别为k-1时刻陀螺仪测得机体坐标系下角速度分量。
3.如权利要求1所述的方法，其特征在于，所述的非线性量测方程h1(·)和非线性量测方程h2(·)均为非线性函数，其表达式分别为：
Figure FDA0002967174490000031
Figure FDA0002967174490000032
其中：
Figure FDA0002967174490000033
和
Figure FDA0002967174490000034
分别表示为重力场和地磁场在机体坐标系下的估计量，g表示重力加速度，
Figure FDA0002967174490000035
为使用四元数表示的地球坐标系到载体坐标系之间的变换矩阵，其表达式为：
Figure FDA0002967174490000036
4.如权利要求1所述的方法，其特征在于，所述四元数状态向量的状态方程为线性，状态预测值及其协方差的计算与线性卡尔曼滤波相同，即：
Figure FDA0002967174490000037
Figure FDA0002967174490000038
5.如权利要求1所述的方法，其特征在于，所述步骤2中选取的sigma点为：
Figure FDA0002967174490000039
其中，a∈R为调节参数，控制Sigma点在
Figure FDA00029671744900000310
周围的分布，
Figure FDA00029671744900000311
为矩阵
Figure FDA00029671744900000312
均方根的第i列，n为状态向量的维数。
6.如权利要求1所述的方法，其特征在于，所述经过非线性量测方程h1(·)和非线性量测方程h2(·)变换后的Sigma点分别为：
Figure FDA0002967174490000041
Figure FDA0002967174490000042
7.如权利要求1所述的方法，其特征在于，步骤3、4中所述的计算量测预测值及其互协方差和计算状态预测值与量测预测值间的互协方差的计算公式分别为：
Figure FDA0002967174490000043
Figure FDA0002967174490000044
Figure FDA0002967174490000045
Figure FDA0002967174490000046
Figure FDA0002967174490000047
Figure FDA0002967174490000048
其中，
Figure FDA0002967174490000049
步骤3和步骤4中所述的卡尔曼增益矩阵Kk1和卡尔曼增益矩阵Kk2分别为：
Figure FDA00029671744900000410
Figure FDA00029671744900000411
8.如权利要求1所述的方法，其特征在于，为保证偏航角不被加速度计校正过程影响，校正参数四元数qε的第三个矢量部分被置为0；步骤3中，计算第一阶段校正参数qε1，更新状态估计值
Figure FDA0002967174490000051
及其协方差
Figure FDA0002967174490000052
所用公式分别为：
Figure FDA0002967174490000053
qε1＝[qε,0 qε,1 qε,2 0·qε,3]T
Figure FDA0002967174490000054
Figure FDA0002967174490000055
其中qε,0,qε,1,qε,2和qε,3分别为四元数qε的四个元素；
为保证横滚角和俯仰角不被磁力计校正过程影响，校正参数qε的第一和第二个矢量部分被置为0；步骤4中，计算第二阶段校正参数qε2，更新状态估计值
Figure FDA0002967174490000056
及其协方差
Figure FDA0002967174490000057
所用公式分别为：
Figure FDA0002967174490000058
qε2＝[qε,0 0·qε,1 0·qε,2 qε,3]T
Figure FDA0002967174490000059
Figure FDA00029671744900000510
其中qε,0,qε,1,qε,2和qε,3分别为四元数qε的四个元素。
9.如权利要求1所述的方法，其特征在于，所述步骤5中四元数转姿态角的公式为：
Figure FDA00029671744900000511
Figure FDA00029671744900000512
Figure FDA00029671744900000513
CN201810275931.0A 2018-03-30 2018-03-30 一种小型多旋翼无人机姿态解算方法 Active CN108645404B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810275931.0A CN108645404B (zh) 2018-03-30 2018-03-30 一种小型多旋翼无人机姿态解算方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810275931.0A CN108645404B (zh) 2018-03-30 2018-03-30 一种小型多旋翼无人机姿态解算方法

Publications (2)

Publication Number Publication Date
CN108645404A CN108645404A (zh) 2018-10-12
CN108645404B true CN108645404B (zh) 2021-05-11

Family

ID=63744949

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810275931.0A Active CN108645404B (zh) 2018-03-30 2018-03-30 一种小型多旋翼无人机姿态解算方法

Country Status (1)

Country Link
CN (1) CN108645404B (zh)

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109990776B (zh) * 2019-04-12 2021-09-24 武汉深海蓝科技有限公司 一种姿态测量方法及装置
CN110081878B (zh) * 2019-05-17 2023-01-24 东北大学 一种多旋翼无人机的姿态及位置确定方法
CN110377058B (zh) * 2019-08-30 2021-11-09 深圳市道通智能航空技术股份有限公司 一种飞行器的偏航角修正方法、装置及飞行器
CN112254723B (zh) * 2020-10-13 2022-08-16 天津津航计算技术研究所 基于自适应ekf算法的小型无人机marg航姿估计方法
CN115685067B (zh) * 2022-11-07 2023-06-06 江西理工大学 用于多旋翼无人机定位跟踪的常模信号盲估计方法及系统

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101726295A (zh) * 2008-10-24 2010-06-09 中国科学院自动化研究所 考虑加速度补偿和基于无迹卡尔曼滤波的惯性位姿跟踪方法
CN106643737A (zh) * 2017-02-07 2017-05-10 大连大学 风力干扰环境下四旋翼飞行器姿态解算方法
CN106708066A (zh) * 2015-12-20 2017-05-24 中国电子科技集团公司第二十研究所 基于视觉/惯导的无人机自主着陆方法
US10773330B2 (en) * 2015-01-05 2020-09-15 University Of Kentucky Research Foundation Measurement of three-dimensional welding torch orientation for manual arc welding process

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101726295A (zh) * 2008-10-24 2010-06-09 中国科学院自动化研究所 考虑加速度补偿和基于无迹卡尔曼滤波的惯性位姿跟踪方法
US10773330B2 (en) * 2015-01-05 2020-09-15 University Of Kentucky Research Foundation Measurement of three-dimensional welding torch orientation for manual arc welding process
CN106708066A (zh) * 2015-12-20 2017-05-24 中国电子科技集团公司第二十研究所 基于视觉/惯导的无人机自主着陆方法
CN106643737A (zh) * 2017-02-07 2017-05-10 大连大学 风力干扰环境下四旋翼飞行器姿态解算方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
A derivative UKF for tightly coupled INS/GPS integrated navigation;Gaoge Hu等;《ISA Transactions》;20150530;第56卷;第2节第1段，图1 *
A Double-Stage Kalman Filter for Orientation Tracking With an Integrated Processor in 9-D IMU;Simone Sabatelli等;《IEEE TRANSACTIONS ON INSTRUMENTATION AND MEASUREMENT》;20130331;第62卷(第3期);第2节第2、5段，第593页右栏第3段-594页左栏，图1、2 *

Also Published As

Publication number Publication date
CN108645404A (zh) 2018-10-12

Similar Documents

Publication Publication Date Title
CN108645404B (zh) 一种小型多旋翼无人机姿态解算方法
CN109163721B (zh) 姿态测量方法及终端设备
CN108827299B (zh) 一种基于改进四元数二阶互补滤波的飞行器姿态解算方法
CN109974714B (zh) 一种Sage-Husa自适应无迹卡尔曼滤波姿态数据融合方法
CN101726295B (zh) 考虑加速度补偿和基于无迹卡尔曼滤波的惯性位姿跟踪方法
CN103712598B (zh) 一种小型无人机姿态确定方法
CN109931955B (zh) 基于状态相关李群滤波的捷联惯性导航系统初始对准方法
CN110017837B (zh) 一种姿态抗磁干扰的组合导航方法
CN108007477B (zh) 一种基于正反向滤波的惯性行人定位系统误差抑制方法
CN111024070A (zh) 一种基于航向自观测的惯性足绑式行人定位方法
CN110243377B (zh) 一种基于分层式结构的集群飞行器协同导航方法
CN112945225A (zh) 基于扩展卡尔曼滤波的姿态解算系统及解算方法
CN110954102B (zh) 用于机器人定位的磁力计辅助惯性导航系统及方法
CN106153042A (zh) 航向角获取方法和装置
CN111189442B (zh) 基于cepf的无人机多源导航信息状态预测方法
CN112525191B (zh) 一种基于相对捷联解算的机载分布式pos传递对准方法
CN105910606A (zh) 一种基于角速度差值的方向修正方法
CN110567492A (zh) 低成本mems惯性传感器系统级标定方法
Zhou et al. Spinning projectile’s angular measurement using crest and trough data of a geomagnetic sensor
CN110595434A (zh) 基于mems传感器的四元数融合姿态估计方法
CN108416387B (zh) 基于gps与气压计融合数据的高度滤波方法
CN111207734B (zh) 一种基于ekf的无人机组合导航方法
CN111220151B (zh) 载体系下考虑温度模型的惯性和里程计组合导航方法
CN113074753A (zh) 一种星敏感器陀螺联合定姿方法、联合定姿系统及应用
CN110737194B (zh) 一种多mems传感器组合测姿方法

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