CN113670314A - 基于pi自适应两级卡尔曼滤波的无人机姿态估计方法 - Google Patents
基于pi自适应两级卡尔曼滤波的无人机姿态估计方法 Download PDFInfo
- Publication number
- CN113670314A CN113670314A CN202110958930.8A CN202110958930A CN113670314A CN 113670314 A CN113670314 A CN 113670314A CN 202110958930 A CN202110958930 A CN 202110958930A CN 113670314 A CN113670314 A CN 113670314A
- Authority
- CN
- China
- Prior art keywords
- filtering
- stage
- matrix
- observation
- calculating
- 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
Links
- 238000001914 filtration Methods 0.000 title claims abstract description 111
- 238000000034 method Methods 0.000 title claims abstract description 39
- 239000011159 matrix material Substances 0.000 claims abstract description 103
- 238000012937 correction Methods 0.000 claims abstract description 29
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 24
- 230000008859 change Effects 0.000 claims abstract description 10
- 238000005259 measurement Methods 0.000 claims description 27
- 230000001133 acceleration Effects 0.000 claims description 22
- 230000008569 process Effects 0.000 claims description 15
- 230000007704 transition Effects 0.000 claims description 15
- 238000004364 calculation method Methods 0.000 claims description 13
- 239000013598 vector Substances 0.000 claims description 11
- 230000005484 gravity Effects 0.000 claims description 8
- 230000006978 adaptation Effects 0.000 claims description 3
- 230000004069 differentiation Effects 0.000 claims description 3
- 230000006698 induction Effects 0.000 claims description 3
- 230000010354 integration Effects 0.000 claims description 3
- 238000013178 mathematical model Methods 0.000 claims description 3
- 238000000926 separation method Methods 0.000 claims description 3
- 238000012360 testing method Methods 0.000 claims description 2
- 230000000295 complement effect Effects 0.000 description 8
- 238000011478 gradient descent method Methods 0.000 description 5
- RZVHIXYEVGDQDX-UHFFFAOYSA-N 9,10-anthraquinone Chemical compound C1=CC=C2C(=O)C3=CC=CC=C3C(=O)C2=C1 RZVHIXYEVGDQDX-UHFFFAOYSA-N 0.000 description 4
- 230000000694 effects Effects 0.000 description 3
- 238000011160 research Methods 0.000 description 3
- 230000003068 static effect Effects 0.000 description 3
- 230000003044 adaptive effect Effects 0.000 description 2
- 238000013461 design Methods 0.000 description 2
- 238000011161 development Methods 0.000 description 2
- 238000002474 experimental method Methods 0.000 description 2
- 238000004088 simulation Methods 0.000 description 2
- 238000012612 static experiment Methods 0.000 description 2
- 238000013459 approach Methods 0.000 description 1
- 238000013473 artificial intelligence Methods 0.000 description 1
- 230000002567 autonomic effect Effects 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 230000008878 coupling Effects 0.000 description 1
- 238000010168 coupling process Methods 0.000 description 1
- 238000005859 coupling reaction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000007599 discharging Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 230000002265 prevention Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
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/20—Instruments for performing navigational calculations
-
- 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
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Manufacturing & Machinery (AREA)
- Navigation (AREA)
Abstract
本发明公开了一种基于PI自适应两级卡尔曼滤波的无人机姿态估计方法，包括：无人机因陀螺仪零位漂移和加速度计噪声特性发生变化而导致滤波发散、滤波精度降低的问题时，将卡尔曼滤波器设计为两级滤波，第一级滤波使用加速度计数据，通过PI自适应算法实时修正观测噪声协方差矩阵，解决由于加速度计噪声特性发生变化导致滤波发散的问题。第二级滤波使用磁力计数据对姿态角进行校正，利用校正因子修正陀螺仪零位漂移引起的误差，提高无人机姿态估计精度。
Description
技术领域
本发明涉及无人机姿态估计技术领域，特别涉及一种基于PI自适应两级卡尔 曼滤波的无人机姿态估计方法。
背景技术
四旋翼无人机越来越多的进入人们的生活当中，比起传统的直升机，四旋翼 无人机具有结构简单，体积较小，质量较轻，造价便宜，可全自主飞行控制和定 点悬停，垂直起降等优势。四旋翼能够节约资源，且便于携带，能进入更多的狭 小空间采集所需数据。所以近几年来，四旋翼无人机应用越来越广泛，如物流运 输、影视航拍、通讯中继、精准农业、环境保护、地理勘测、排弹作业等。尤其 在人工智能技术研究热潮中，四旋翼无人机技术的研究成果也越来越丰富，此外， 在疫情防控期间，无人机物流配送，以及无人机高空喊话等，给人民带来了很大 的便利。因而，四旋翼无人机的研究是现今发展的趋势之一。四旋翼无人机系统 是一种典型的非线性、欠驱动、强耦合系统，在实际飞行过程中易受到空气动力、重力、陀螺效应等因素影响，还可能受到外界气流干扰，所以无人机的姿态估计 是热门研究问题，因为姿态估计的精准性对无人机的飞行控制至关重要，解算精 度的高低将直接影响无人机的控制稳定性与飞行安全性。一般情况下，常用捷联 惯性测量单元(InertialMeasurement Unit,IMU)测量无人机的姿态，其中，陀螺仪 用来测量角速率，长时间工作后由于累计误差会发生漂移，同时在受到载体振动 噪声、运动加速度和磁场环境等因素影响后将导致姿态角解算精度低、稳定性差。 加速度计的测量值不仅只有重力加速度还包括了运动加速度信息，飞行过程中产 生的高频误差会使加速度计的短时精度受到较大影响。磁力计的测量误差主要来 自观测噪声，且在实际飞行过程中磁场容易受到干扰，对测量造成影响。因此， 融合惯性传感器数据，提高姿态估计精度，实现更高精度的导航，是捷联惯性导 航的重要研究方向。
目前姿态估计方法主要有扩展卡尔曼滤波，梯度下降法滤波，自适应卡尔曼 滤波和互补滤波等。扩展卡尔曼滤波是固定的观测噪声协方差矩阵，在无人机实 际飞行过程中会导致滤波精度下降，姿态估计误差较大；梯度下降法易受磁场干 扰，自适应卡尔曼滤波和互补滤波计算繁琐，工程量大。
文献[1]提出了一种基于扩展卡尔曼滤波器的算法，结合加速度计与陀螺仪数 据进行姿态估计，虽然该算法对横滚角与俯仰角具有良好的估计，但仅结合加速 度数据，对偏航角的校正精度偏低。文献[2]通过梯度下降法计算姿态最优估计， 虽然利用变步长动量梯度下降的方法消除了陀螺仪累计误差，但当出现磁场干扰 时，不能够有效滤波，计算结果误差较大。文献[3]将互补滤波器与梯度下降法进 行融合，提高了解算精度，但只适用于无人机平稳运动的场合，如果无人机做加 速运动，且姿态角变化剧烈，该算法的解算精度会大幅下降。文献[4]设计了一种 四元数描述符滤波器(Quaternion DescriptorFilter)，可以解决任意方向存在运动加 速度的情况。该算法以四元数和运动加速度的函数为状态向量构建状态方程，采 用加权最小二乘法对状态进行估计，在加速度剧烈变化的情况下也有较高的精度， 但需要计算加权矩阵，计算过程繁琐。文献[5]结合互补滤波算法，提出了一种 改进的自适应增量滤波算法，把经过互补滤波后的数据增量作为卡尔曼滤波器观 测量，解算精度有所提高，但计算量较大，工程上较难实现。文献[6-7]基于互补 滤波对姿态进行解算，计算量小，但算法中固定的滤波系数不能解决传感器噪声 对姿态估计结果的影响，导致姿态估计精度不高。
参考文献
[1]Ghobadi,Mostafa,Singla,等.Robust Att-itude Estimation fromUncertain Observations of Inertial Sensors Using Covariance Inflate-dMultiplicative Extended Kalman Filter[J].IEEE Transactions onInstrumentation&Measurement,2018.
[2]张帅,郑芳,李霞,等.基于变步长动量梯度下降法的姿态估计算法[J].电光与制,2020,27(9)：66-70,111.
[3]Wu J,Zhou Z,Fourati H,et al.Generalized Linear QuaternionComplementary Filter for Attitude Estimation From Multise-nsor Observations:An Optimization Approach[J].IEEE Transactions on Automation Science a-ndEngineering,2019:1330-1343.
[4]Makni A,Kibangou A Y,Fourati H.Data fusion-based descriptorapproach for attitude estimation under accelerated maneuv-ers[J].AsianJournal of Control, 2019(4).
[5]刘字,杨晓辉,郭俊启,等.一种基于AIKF的姿态测量算法[J].压电与声 光,2018,40(3):454-459.
[6]Wu Z,Sun Z,Zhang W.A Novel App-roach Forattitude EstimationBasedon MFMS Inertial Sensors Using Nonlinear Complem-entary Filters[J].IEEESensors Journal,2016,16(10):3856-386.
[7]杜瑾,赵华超,郑哲,等.捷联惯导互补滤波姿态融合算法设计[J].传感技术学报,2018,31(10):91-96.
发明内容
本发明针对现有技术的缺陷，提供了一种基于PI自适应两级卡尔曼滤波的无 人机姿态估计方法。
为了实现以上发明目的，本发明采取的技术方案如下：
一种基于PI自适应两级卡尔曼滤波的无人机姿态估计方法，包括以下步骤：
S1、建立导航坐标系与机体坐标系，并基于牛顿-欧拉法建立了如下的非线性 数学模型：
式中：x,y,z是导航坐标系下无人机的位置，p,q,r是机体坐标下的角速度，m 为机身质量，U1为四旋翼提供的总推力，Ix,Iy,Iz和U2,U3，U4分别为绕机体坐标系 3个坐标轴的转动惯量和力矩。
S2、以四选取四元数q＝(q0，q1，q2,q3)T作为状态向量，第一级滤波的观测变 量为重力加速度G＝(ax,ay，az)T，由加速度计测得。第二级滤波的观测变量为磁 感应强度M＝(mx,my,mz)T，由磁力计测得。状态方程与观测方程为：
式中：A为状态转移矩阵，
为第一级滤波器的观测矩阵，
为第二级滤 波器的观测矩阵。wk-1为系统的过程噪声，vk1，
分别为观测噪声，它们均服 从零均值高斯分布。
S3、求解状态转移矩阵和观测矩阵。将状态方程用四元数微分形式表示为：
式中：
其中，ωx，ωy，ωz是陀螺仪测得的角速度。将状态方程从连续时间形式转换 为离散时间形式，通过差分近似代替微分求得：
进而得到状态转移矩阵为:
使用方向余弦矩阵结合加速度数据得到第一级滤波阶段观测矩阵：
其中，
式中：h1(qk)为系统第一级滤波观测方程，qk为k时刻的状态变量，
为方 向余弦矩阵。h1(qk)是非线性的，需要通过扩展卡尔曼滤波将系统线性化，用雅 克比矩阵代替观测矩阵。雅克比矩阵为：
同理，第二级滤波阶段观测矩阵为：
线性化得
S4、先验估计：
先验状态估计值是滤波的中间计算结果，即根据(k-1)时刻的最优估计预测k 时刻的结果。计算步骤如下：
S41、读取陀螺仪传感器数据，获得角速度ωx，ωy，ωz。
S42、计算状态转移矩阵Ak。
S43、计算系统的先验状态估计：
S44、计算先验噪声协方差矩阵。为了计算卡尔曼增益Kk，首先计算先验误 差协方差矩阵
式中：Qk是过程噪声协方差矩阵，它表示陀螺仪噪声及陀螺仪其他误差源。
S5、第一级滤波，步骤如下：
S51、线性化h1(qk)计算雅可比矩阵Hk1。
S52、计算卡尔曼增益Kk1。由于观测矩阵h1(qk)是非线性的，所以通过扩展 卡尔曼滤波获得卡尔曼滤波增益Kk1为：
式中：Rk1是测量噪声协方差矩阵，表示加速度计噪声和加速度计的其他误差 源。Vk1是关于非线性方程组h1(qk)的观测噪声vk1的偏导数的雅克比矩阵。
S53、获取加速度数据，zk1＝G
S54、计算第一级滤波阶段观测方程
S55、计算校正因子。先验状态估计
仅使用陀螺仪数据更新角位置的先验等 式，需要通过校正因子得到角位置的后验方程式，根据卡尔曼滤波理论，卡尔曼 滤波器的校正方程：
式中：zk代表实际测量值，在该算法中，第一级滤波使用加速度计数据，第 二级滤波使用磁力计数据。
是从先验系统状态计算出的预期测量值。用卡尔 曼增益Kk对残差加权，以计算系统状态的后验估计
第一级滤波使用加速度计数据校正，校正因子为：
式中：zk1是加速度计测量值，
是根据先验系统状态计算出的重力加速 度预测值，Kk1是第一级滤波的卡尔曼增益。通过测量重力加速度，仅可以校正横 滚角和俯仰角。为了保证偏航角在校正中不受影响，将校正四元数q的第三矢量部 分设置为零。
S56、计算后验状态估计。
S57、计算后验误差协方差矩阵。
S6、第二级滤波，步骤如下：
S61、线性化h2(qk)计算雅可比矩阵Hk2。
S62、计算卡尔曼增益Kk2。
式中：Rk2是测量噪声协方差矩阵，表示磁力计噪声及其他误差源。Vk2是观 测噪声vk2的雅克比矩阵。
S63、读取加速度计数据，zk2＝M
S64、计算第二级滤波阶段观测方程
S65、计算校正因子。
q∈2,1＝0,q∈2，2＝0
式中：zk2是磁力计测量值，
是根据先验系统状态计算出的预期磁场强 度，Kk2是第二级滤波卡尔曼增益。为了使磁异常不影响横滚角和俯仰角的估计而 仅影响偏航角，令校正四元数q∈2的第一、二两个部分矢量为零。
S66、计算后验状态估计。
S67、计算后验误差协方差矩阵。
Pk＝(I-Kk2Hk2)Pk1
S7、基于PI自适应的参数调节
第k次迭代后的第一级滤波观测噪声协方差矩阵的估计值为：
Rk1＝(1+αk)Rk1-1
式中：αk为观测噪声协方差矩阵的调整系数。
定义残差方差的理论值为：
定义残差方差的实际值为：
残差方差的理论值与实际值之差为：
Ek＝trace(Dk)-trace(Ck)
式中：trace()表示矩阵的迹。
卡尔曼滤波器正常工作时，如果Ek较大，则说明加速度计噪声特性发生了变 化，通过调节αk间接调节Rk1来改变Dk，使Dk与Ck保持一致，从而使Ek维持在0 附近，达到实时修正观测噪声协方差矩阵Rk1的目的。
式中：Kp为比例系数，Ki为积分系数。β为积分开关系数，决定是否使用积 分环节。采用积分分离式PI控制算法，计算αk。
式中：ε为开关阈值，当误差大于阈值时，不使用积分环节，防止饱和；当 误差小于阈值时，积分环节开始介入，消除余差。
与现有技术相比，本发明的优点在于：
将卡尔曼滤波器设计为两级滤波，可以以分离磁异常对横滚角和俯仰角估计 精度的影响，同时也减小了计算量。第一级滤波使用加速度计数据，通过PI自适 应算法实时修正观测噪声协方差矩阵，解决了由于加速度计噪声特性发生变化导 致滤波发散的问题。第二级滤波使用磁力计数据对姿态角进行校正，利用校正因 子修正了陀螺仪零位漂移引起的误差，提高了姿态估计的精度和鲁棒性。
附图说明
图1是本发明实施例导航坐标系与机体坐标系图；
图2是本发明实施例基于PI自适应两级卡尔曼滤波的无人机姿态估计方法流 程图；
图3是本发明实施例静态实验结果图，其中(a)表示横滚角，(b)表示俯仰角， (c)表示偏航角；
图4是本发明实施例仿真结果图，其中(a)表示横滚角，(b)表示俯仰角，(c) 表示偏航角；
图5是本发明实施例三个姿态角的角度误差图，其中(a)表示横滚角误差，(b) 表示俯仰角误差，(c)表示偏航角误差。
具体实施方式
为使本发明的目的、技术方案及优点更加清楚明白，以下根据附图并列举实 施例，对本发明做进一步详细说明。
本实施例的姿态估计方法是基于牛顿-欧拉法建立四旋翼无人机的非线性系 统模型，选取四元数q＝(q0,q1,q2,q3)T作为状态向量，将卡尔曼滤波器设计为两 级滤波，第一级滤波使用加速度计数据，通过PI自适应算法实时修正观测噪声协 方差矩阵。第二级滤波使用磁力计数据对姿态角进行校正。
首先，建立导航坐标系与机体坐标系如图1所示。
并基于牛顿-欧拉法建立了如下所示的非线性数学模型：
式中：x,y,z是导航坐标系下无人机的位置，p,q,r是机体坐标下的角速度，m 为机身质量，U1为四旋翼提供的总推力，Ix,Iy,Iz和U2,U3，U4分别为绕机体坐标系 3个坐标轴的转动惯量和力矩。
以四选取四元数q＝(q0，q1,q2,q3)T作为状态向量，第一级滤波的观测变量为 重力加速度G＝(ax,ay,az)T，由加速度计测得。第二级滤波的观测变量为磁感应 强度M＝(mx,my,mz)T，由磁力计测得。状态方程与观测方程为：
式中：A为状态转移矩阵，
为第一级滤波器的观测矩阵，
为第二级滤 波器的观测矩阵。wk-1为系统的过程噪声，vk1，
分别为观测噪声，它们均服 从零均值高斯分布。
为了后续滤波阶段，先求解状态转移矩阵和观测矩阵。将状态方程用四元数 微分形式表示为：
式中：
其中，ωx，ωy，ωz是陀螺仪测得的角速度。将状态方程从连续时间形式转换 为离散时间形式，通过差分近似代替微分求得：
进而得到状态转移矩阵为:
使用方向余弦矩阵结合加速度数据得到第一级滤波阶段观测矩阵：
其中，
式中：h1(qk)为系统第一级滤波观测方程，qk为k时刻的状态变量，
为方 向余弦矩阵。h1(qk)是非线性的，需要通过扩展卡尔曼滤波将系统线性化，用雅 克比矩阵代替观测矩阵。雅克比矩阵为：
同理，第二级滤波阶段观测矩阵为：
线性化得
接下来是滤波器的设计，设计的基于PI自适应两级卡尔曼滤波的无人机姿态 估计方法，首先是将陀螺仪数据与加速度计数据融合进行第一级滤波，其中引入 PI控制算法实时修正观测噪声协方差矩阵。第一级滤波后的校正因子结合陀螺仪 与磁力计数据进行第二级滤波以得到更好的姿态角估计值，重复上述迭代过程直 到获得最优估计值，流程如图2所示。
系统的先验估计：
先验状态估计值是滤波的中间计算结果，即根据(k-1)时刻的最优估计预测k 时刻的结果。计算步骤如下所示：
1)读取陀螺仪传感器数据，获得角速度ωx，ωy，ωz。
2)计算状态转移矩阵Ak。
3)计算系统的先验状态估计：
4)计算先验噪声协方差矩阵。为了计算卡尔曼增益Kk，首先计算先验误差协 方差矩阵
式中：Qk是过程噪声协方差矩阵，它表示陀螺仪噪声及陀螺仪其他误差源。
(1)第一级滤波。
第一级滤波是将陀螺仪数据与加速度计数据融合进行滤波，步骤如下：
1)线性化h1(qk)计算雅可比矩阵Hk1。
2)计算卡尔曼增益Kk1。由于观测矩阵h1(qk)是非线性的，所以通过扩展卡尔 曼滤波获得卡尔曼滤波增益Kk1为：
式中：Rk1是测量噪声协方差矩阵，表示加速度计噪声和加速度计的其他误差 源。Vk1是关于非线性方程组h1(qk)的观测噪声vk1的偏导数的雅克比矩阵。
3)获取加速度数据，zk1＝G
4)计算第一级滤波阶段观测方程
5)计算校正因子。先验状态估计
仅使用陀螺仪数据更新角位置的先验等式， 需要通过校正因子得到角位置的后验方程式，根据卡尔曼滤波理论，卡尔曼滤波 器的校正方程：
式中：zk代表实际测量值，在该算法中，第一级滤波使用加速度计数据，第 二级滤波使用磁力计数据。
是从先验系统状态计算出的预期测量值。用卡尔 曼增益Kk对残差加权，以计算系统状态的后验估计
在扩展卡尔曼两级滤波算法中，有两个校正方程式。第一级滤波使用加速度 计数据校正，校正因子为：
式中：zk1是加速度计测量值，
是根据先验系统状态计算出的重力加速 度预测值，Kk1是第一级滤波的卡尔曼增益。通过测量重力加速度，仅可以校正横 滚角和俯仰角。为了保证偏航角在校正中不受影响，将校正四元数q的第三矢量部 分设置为零。
6)计算后验状态估计。
7)计算后验误差协方差矩阵。
(2)第二级滤波。
第二级滤波是将陀螺仪数据与磁力计数据融合进行滤波，步骤如下：
1)线性化h2(qk)计算雅可比矩阵Hk2。
2)计算卡尔曼增益Kk2。
式中：Rk2是测量噪声协方差矩阵，表示磁力计噪声及其他误差源。Vk2是观 测噪声vk2的雅克比矩阵。
3)读取加速度计数据，zk2＝M
4)计算第二级滤波阶段观测方程
5)计算校正因子。
q∈2，1＝0,q∈2，2＝0
式中：zk2是磁力计测量值，
是根据先验系统状态计算出的预期磁场强 度，Kk2是第二级滤波卡尔曼增益。为了使磁异常不影响横滚角和俯仰角的估计而 仅影响偏航角，令校正四元数q∈2的第一、二两个部分矢量为零。
6)计算后验状态估计。
7)计算后验误差协方差矩阵。
Pk＝(I-Kk2Hk2)Pk1
(3)基于PI自适应的参数调节
第k次迭代后的第一级滤波观测噪声协方差矩阵的估计值为：
Rk1＝(1+αk)Rk1-1
式中：αk为观测噪声协方差矩阵的调整系数。
定义残差方差的理论值为：
定义残差方差的实际值为：
残差方差的理论值与实际值之差为：
Ek＝trace(Dk)-trace(Ck)
式中：trace()表示矩阵的迹。
卡尔曼滤波器正常工作时，如果Ek较大，则说明加速度计噪声特性发生了变 化，通过调节αk间接调节Rk1来改变Dk，使Dk与Ck保持一致，从而使Ek维持在0 附近，达到实时修正观测噪声协方差矩阵Rk1的目的。
式中：Kp为比例系数，Ki为积分系数。β为积分开关系数，决定是否使用积 分环节。采用积分分离式PI控制算法，计算αk。
式中：ε为开关阈值，当误差大于阈值时，不使用积分环节，防止饱和；当 误差小于阈值时，积分环节开始介入，消除余差。
为了验证所研究滤波方法的滤波效果，使用匿名拓空者pro飞控，搭建轴距 为450mm的四旋翼无人机实验平台。将滤波方法编程并写进飞控中，利用数传将 传感器的数据通过串口发送到上位机，采样时间为0.01s。将上位机数据导入到 MATLAB中处理，分别通过静态实验和动态实验分析横滚角φ、俯仰角θ和偏航 角ψ的滤波效果。
(1)静态实验
姿态角初始值为(0,0,0)，陀螺仪常值漂移为0.05，初始误差协方差矩阵P(0)＝10-4×I4×4，过程噪声协方差矩阵Q＝10-6×I4×4,第一级滤波中加速度计测量噪 声协方差矩阵R1＝2×I3×3，第二级滤波中磁力计测量噪声协方差矩阵R2＝I3×3。 过程噪声和测量噪声均为高斯噪声，wk，vk1，vk2根据Q，R1，R2随机赋值，实 验结果如图3所示，姿态误差数据如表1所示。
表1静态时姿态误差数据
(2)动态实验验证
为模拟无人机实际飞行过程姿态角变化，对无人机姿态角随机赋值，其他参 数与静态实验保持一致，将PI自适应两级卡尔曼滤波算法与扩展卡尔曼滤波算法 进行对比，仿真结果如图4、图5所示，图4分别为横滚角、俯仰角、偏航角的 解算数据。图5分别为三个姿态角的角度误差。
从实验结果可以看出，PI自适应两级卡尔曼滤波算法与传统的扩展卡尔曼滤 波算法都能够跟随姿态角变化，但基于PI自适应两级卡尔曼滤波算法的姿态角解 算值比通过传统的扩展卡尔曼滤波算法得到的姿态角解算值更加接近真实值。即 使在姿态角快速变化时，所研究姿态估计方法也能使姿态角在较小误差范围内变 化，从而提高了解算精度。
本领域的普通技术人员将会意识到，这里所述的实施例是为了帮助读者理解 本发明的实施方法，应被理解为本发明的保护范围并不局限于这样的特别陈述和 实施例。本领域的普通技术人员可以根据本发明公开的这些技术启示做出各种不 脱离本发明实质的其它各种具体变形和组合，这些变形和组合仍然在本发明的保 护范围内。
Claims (1)
1.一种基于PI自适应两级卡尔曼滤波的无人机姿态估计方法，其特征在于，包括以下步骤：
S1、建立导航坐标系与机体坐标系，并基于牛顿-欧拉法建立了如下的非线性数学模型：
式中：x,y,z是导航坐标系下无人机的位置，p,q,r是机体坐标下的角速度，m为机身质量，U1为四旋翼提供的总推力，Ix,Iy,Iz和U2,U3，U4分别为绕机体坐标系3个坐标轴的转动惯量和力矩；
S2、以四选取四元数q＝(q0，q1，q2，q3)T作为状态向量，第一级滤波的观测变量为重力加速度G＝(ax,ay,az)T，由加速度计测得；第二级滤波的观测变量为磁感应强度M＝(mx,my,mz)T，由磁力计测得；状态方程与观测方程为：
式中：A为状态转移矩阵，
为第一级滤波器的观测矩阵，
为第二级滤波器的观测矩阵；wk-1为系统的过程噪声，
分别为观测噪声，它们均服从零均值高斯分布；
S3、求解状态转移矩阵和观测矩阵；将状态方程用四元数微分形式表示为：
式中：
其中，ωx，ωy，ωz是陀螺仪测得的角速度；将状态方程从连续时间形式转换为离散时间形式，通过差分近似代替微分求得：
进而得到状态转移矩阵为:
使用方向余弦矩阵结合加速度数据得到第一级滤波阶段观测矩阵：
其中，
式中：h1(qk)为系统第一级滤波观测方程，qk为k时刻的状态变量，
为方向余弦矩阵；h1(qk)是非线性的，需要通过扩展卡尔曼滤波将系统线性化，用雅克比矩阵代替观测矩阵；雅克比矩阵为：
同理，第二级滤波阶段观测矩阵为：
线性化得
S4、先验估计：
先验状态估计值是滤波的中间计算结果，即根据(k-1)时刻的最优估计预测k时刻的结果；计算步骤如下：
S41、读取陀螺仪传感器数据，获得角速度ωx，ωy，ωz；
S42、计算状态转移矩阵Ak；
S43、计算系统的先验状态估计：
S44、计算先验噪声协方差矩阵；为了计算卡尔曼增益Kk，首先计算先验误差协方差矩阵
式中：Qk是过程噪声协方差矩阵，它表示陀螺仪噪声及陀螺仪其他误差源；
S5、第一级滤波，步骤如下：
S51、线性化h1(qk)计算雅可比矩阵Hk1；
S52、计算卡尔曼增益Kk1；由于观测矩阵h1(qk)是非线性的，所以通过扩展卡尔曼滤波获得卡尔曼滤波增益Kk1为：
式中：Rk1是测量噪声协方差矩阵，表示加速度计噪声和加速度计的其他误差源；Vk1是关于非线性方程组h1(qk)的观测噪声vk1的偏导数的雅克比矩阵；
S53、获取加速度数据，zk1＝G
S54、计算第一级滤波阶段观测方程
S55、计算校正因子；先验状态估计
仅使用陀螺仪数据更新角位置的先验等式，需要通过校正因子得到角位置的后验方程式，根据卡尔曼滤波理论，卡尔曼滤波器的校正方程：
式中：zk代表实际测量值，在该算法中，第一级滤波使用加速度计数据，第二级滤波使用磁力计数据；
是从先验系统状态计算出的预期测量值；用卡尔曼增益Kk对残差加权，以计算系统状态的后验估计
第一级滤波使用加速度计数据校正，校正因子为：
式中：zk1是加速度计测量值，
是根据先验系统状态计算出的重力加速度预测值，Kk1是第一级滤波的卡尔曼增益；通过测量重力加速度，仅可以校正横滚角和俯仰角；为了保证偏航角在校正中不受影响，将校正四元数q的第三矢量部分设置为零；
S56、计算后验状态估计；
S57、计算后验误差协方差矩阵；
S6、第二级滤波，步骤如下：
S61、线性化h2(qk)计算雅可比矩阵Hk2；
S62、计算卡尔曼增益Kk2；
式中：Rk2是测量噪声协方差矩阵，表示磁力计噪声及其他误差源；Vk2是观测噪声vk2的雅克比矩阵；
S63、读取加速度计数据，zk2＝M
S64、计算第二级滤波阶段观测方程
S65、计算校正因子；
q∈2,1＝0,q∈2，2＝0
式中：zk2是磁力计测量值，
是根据先验系统状态计算出的预期磁场强度，Kk2是第二级滤波卡尔曼增益；为了使磁异常不影响横滚角和俯仰角的估计而仅影响偏航角，令校正四元数q∈2的第一、二两个部分矢量为零；
S66、计算后验状态估计；
S67、计算后验误差协方差矩阵；
Pk＝(I-Kk2Hk2)Pk1
S7、基于PI自适应的参数调节
第k次迭代后的第一级滤波观测噪声协方差矩阵的估计值为：
Rk1＝(1+αk)Rk1-1
式中：αk为观测噪声协方差矩阵的调整系数；
定义残差方差的理论值为：
定义残差方差的实际值为：
残差方差的理论值与实际值之差为：
Ek＝trace(Dk)-trace(Ck)
式中：trace()表示矩阵的迹；
卡尔曼滤波器正常工作时，如果Ek较大，则说明加速度计噪声特性发生了变化，通过调节αk间接调节Rk1来改变Dk，使Dk与Ck保持一致，从而使Ek维持在0附近，达到实时修正观测噪声协方差矩阵Rk1的目的；
式中：Kp为比例系数，Ki为积分系数；β为积分开关系数，决定是否使用积分环节；采用积分分离式PI控制算法，计算αk；
式中：ε为开关阈值，当误差大于阈值时，不使用积分环节，防止饱和；当误差小于阈值时，积分环节开始介入，消除余差。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110958930.8A CN113670314B (zh) | 2021-08-20 | 2021-08-20 | 基于pi自适应两级卡尔曼滤波的无人机姿态估计方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110958930.8A CN113670314B (zh) | 2021-08-20 | 2021-08-20 | 基于pi自适应两级卡尔曼滤波的无人机姿态估计方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113670314A true CN113670314A (zh) | 2021-11-19 |
| CN113670314B CN113670314B (zh) | 2023-05-09 |
Family
ID=78544350
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110958930.8A Active CN113670314B (zh) | 2021-08-20 | 2021-08-20 | 基于pi自适应两级卡尔曼滤波的无人机姿态估计方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113670314B (zh) |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114526756A (zh) * | 2022-01-04 | 2022-05-24 | 华南理工大学 | 一种无人机机载多传感器校正方法、装置及存储介质 |
| CN114880874A (zh) * | 2022-06-07 | 2022-08-09 | 东南大学 | 一种水面无人船参数自适应鲁棒估计方法与系统 |
| WO2024104446A1 (zh) * | 2022-11-18 | 2024-05-23 | 优奈柯恩(北京)科技有限公司 | 用于估计设备姿态的方法和装置 |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103033186A (zh) * | 2012-12-30 | 2013-04-10 | 东南大学 | 一种用于水下滑翔器的高精度组合导航定位方法 |
| CN108398128A (zh) * | 2018-01-22 | 2018-08-14 | 北京大学深圳研究生院 | 一种姿态角的融合解算方法和装置 |
| CN110441691A (zh) * | 2018-05-02 | 2019-11-12 | 西南科技大学 | 一种基于精简粒子无迹变换的soc估算方法 |
| CN111895988A (zh) * | 2019-12-20 | 2020-11-06 | 北京空天技术研究所 | 无人机导航信息更新方法及装置 |
| CN112013836A (zh) * | 2020-08-14 | 2020-12-01 | 北京航空航天大学 | 一种基于改进自适应卡尔曼滤波的航姿参考系统算法 |
| CN113155129A (zh) * | 2021-04-02 | 2021-07-23 | 北京大学 | 一种基于扩展卡尔曼滤波的云台姿态估计方法 |
-
2021
- 2021-08-20 CN CN202110958930.8A patent/CN113670314B/zh active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103033186A (zh) * | 2012-12-30 | 2013-04-10 | 东南大学 | 一种用于水下滑翔器的高精度组合导航定位方法 |
| CN108398128A (zh) * | 2018-01-22 | 2018-08-14 | 北京大学深圳研究生院 | 一种姿态角的融合解算方法和装置 |
| CN110441691A (zh) * | 2018-05-02 | 2019-11-12 | 西南科技大学 | 一种基于精简粒子无迹变换的soc估算方法 |
| CN111895988A (zh) * | 2019-12-20 | 2020-11-06 | 北京空天技术研究所 | 无人机导航信息更新方法及装置 |
| CN112013836A (zh) * | 2020-08-14 | 2020-12-01 | 北京航空航天大学 | 一种基于改进自适应卡尔曼滤波的航姿参考系统算法 |
| CN113155129A (zh) * | 2021-04-02 | 2021-07-23 | 北京大学 | 一种基于扩展卡尔曼滤波的云台姿态估计方法 |
Non-Patent Citations (9)
| Title |
|---|
| SIMONE SABATELLI等: "A double stage Kalman filter for sensor fusion and orientation tracking in 9D IMU.IEEE Conference on Sensors Applications Symposium", 《2012 IEEE SENSORS APPLICATIONS SYMPOSIUM PROCEEDINGS》 * |
| 严丹;邓志红;张雁鹏;: "一种微惯性与磁组合测量单元的姿态解算方法", 兵工学报 * |
| 严丹等: "一种微惯性与磁组合测量单元的姿态解算方法", 《兵工学报》 * |
| 卢艳军等: "四旋翼飞行器姿态解算算法试验研究", 《电光与控制》 * |
| 吴耀;李文钧;姜华;何风行;: "基于动态Kalman滤波的多传感数据融合算法研究", 物联网技术 * |
| 毛红瑛等: "低成本惯导系统预处理和姿态解算", 《仪表技术与传感器》 * |
| 汪俊等: "基于多传感器的运动姿态测量算法", 《计算机系统应用》 * |
| 郭佳晖等: "基于PI自适应卡尔曼滤波的无人机姿态解算算法", 《传感技术学报》 * |
| 高伟;叶攀;许伟通;: "SINS/视觉组合导航系统融合算法", 压电与声光 * |
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114526756A (zh) * | 2022-01-04 | 2022-05-24 | 华南理工大学 | 一种无人机机载多传感器校正方法、装置及存储介质 |
| CN114526756B (zh) * | 2022-01-04 | 2023-06-16 | 华南理工大学 | 一种无人机机载多传感器校正方法、装置及存储介质 |
| CN114880874A (zh) * | 2022-06-07 | 2022-08-09 | 东南大学 | 一种水面无人船参数自适应鲁棒估计方法与系统 |
| WO2023236247A1 (zh) * | 2022-06-07 | 2023-12-14 | 东南大学 | 一种水面无人船参数自适应鲁棒估计方法与系统 |
| CN114880874B (zh) * | 2022-06-07 | 2024-03-12 | 东南大学 | 一种水面无人船参数自适应鲁棒估计方法与系统 |
| WO2024104446A1 (zh) * | 2022-11-18 | 2024-05-23 | 优奈柯恩(北京)科技有限公司 | 用于估计设备姿态的方法和装置 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113670314B (zh) | 2023-05-09 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113670314A (zh) | 基于pi自适应两级卡尔曼滤波的无人机姿态估计方法 | |
| CN108731676B (zh) | 一种基于惯性导航技术的姿态融合增强测量方法及系统 | |
| Chang et al. | Analysis and design of second-order sliding-mode algorithms for quadrotor roll and pitch estimation | |
| CN112683269B (zh) | 一种附有运动加速度补偿的marg姿态计算方法 | |
| CN110793515A (zh) | 一种基于单天线gps和imu的大机动条件下无人机姿态估计方法 | |
| US11725947B2 (en) | Alignment of electrical devices using inertial measurement units | |
| CN116147624B (zh) | 一种基于低成本mems航姿参考系统的船舶运动姿态解算方法 | |
| Allibert et al. | Velocity aided attitude estimation for aerial robotic vehicles using latent rotation scaling | |
| CN111189442A (zh) | 基于cepf的无人机多源导航信息状态预测方法 | |
| CN112068444A (zh) | 一种采用非线性自适应滑模的飞行器攻角控制方法 | |
| CN110595434B (zh) | 基于mems传感器的四元数融合姿态估计方法 | |
| Chou et al. | Two‐step optimal filter design for the low‐cost attitude and heading reference systems | |
| CN113063416B (zh) | 一种基于自适应参数互补滤波的机器人姿态融合方法 | |
| CN110442023B (zh) | 一种mems陀螺仪驱动与检测模态预设性能抗干扰控制方法 | |
| CN109674480B (zh) | 一种基于改进互补滤波的人体运动姿态解算方法 | |
| Wang et al. | Accurate Attitude Determination Based on Adaptive UKF and RBF Neural Network Using Fusion Methodology for Micro‐IMU Applied to Rotating Environment | |
| CN113219820B (zh) | 一种利用无拖曳控制提取惯性传感器负刚度力零位的方法 | |
| Chu et al. | Magnetic orientation system based on magnetometer, accelerometer and gyroscope | |
| CN112197768B (zh) | 一种测量侧向过载的飞行器反演干扰观测转弯控制方法 | |
| CN113701755B (zh) | 一种无高精度陀螺的光学遥感卫星姿态确定方法 | |
| CN111290279B (zh) | 一种基于误差转换函数的神经网络滑模控制方法 | |
| CN112061379B (zh) | 一种测量加速度提供阻尼的飞行器转弯控制方法 | |
| CN114594675B (zh) | 一种改进型pid的四旋翼飞行器控制系统及方法 | |
| CN111025915B (zh) | 一种基于干扰观测器的z轴陀螺仪神经网络滑模控制方法 | |
| Huang et al. | The design of attitude heading reference system based on MEMS sensing technology |
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 |