卡尔曼滤波在四旋翼飞行器姿态解算中的应用
测 协方 差矩 阵 ４步骤 ；更 新 阶段 包 括 计算 日 矩 阵 、计
算测量余量 、卡尔曼滤波器状态更新公式 、四元数规
范化 ４步骤。具体执行过程如下 ：
＾
Ａ
ｒ Ａ Ａ
预测阶段：（１
＋ｆ，ｆ（ ， ）ｄｔ。（２）规
范化四元数 。（３）计算 Ｆ和 Ｇ矩阵元素中的偏导数，从 而 线性 化 处 理 。 （４） ＝（，＋仃 Ｔ） 一 （，＋刀 ） Ｔ ＧＱＧ ， 预测 协方 差矩 阵 。
微型电子机械系统 ）技术 和计算 机技术 的发展 ，飞行
器姿态 的测量普遍采 用捷联惯 性测量 单元 （ｉｎｅｒｔｉａｌ
ｍｅａｓｕｒｅｍｅｎｔ ｕｎｉｔ，ＩＭＵ），其主要 由陀螺仪 、加速度计 、
地磁 传感 器 和气压 计组 成 。陀螺 仪 的零 点 存在 温度 漂
移 ，加速度计会受到载体震动的影 响 ，由于地磁传感
器是一种磁阻传感器 ，会被外部磁场干扰。所以为 了
得 到可靠 、准确 的飞行姿态 ，必须利用滤波器融合不 同传感器 的数据、滤除不必要的干扰 。
卡尔曼滤波 ，也称 为线性二次估计 ，是使用一系 列随时间推移的观测量 ，包括统计噪声和其他误差 的
算法 ，并产生往往 比基于未知变量的单估计量更精确
系 统 ， 该 硬 件 系 统 主 要 由 ３２位 ＡＲＭ 控 制 器 ｓＴＭ３２Ｆ４０７ＶＥＴ６、加 速 度 计 和 陀 螺仪 （ＭＰＵＴ６０５０）、 地磁 极 （ＨＭＣ５８８３）、气 压计 （ＭＳ５６１１）、２．４Ｇ无 线通 信
模 块 （ＮＲＦ２４Ｌ０１）、电源模块 、接 收机 、电子 调速 器 、ＳＤ 卡 等组成 。
单高 效 。 基本卡尔曼滤波器只能在线性 的情况下才能使
用 ，但是 大部 分 系 统是 非 线 性 的 ，包 括 过 程模 型 和 观
测模 型 的非 线 性 。 因此 提 出 了扩 展 形 卡 尔曼 滤 波 算
法 。该算 法 包括 预测 和更 新 ２个 阶段 。其 中 ，预 测 阶段
包 括 预测 状 态 、四元 数 规 范 化 、计 算 Ｆ和 Ｇ矩 阵 、预
图 １所示 。
图 １ 加 速 度计 和 陀螺 仪 电路
ＨＭＣ５８８３Ｌ采用 Ｉ２Ｃ通 讯 总线 接 口 ，使 用 了霍 尼 韦 尔各 向异 性磁 阻 （ＡＭＲ）技术 。因为 同一 条 Ｉ２Ｃ总 上 线 可 以挂 载 ２５５个 不 同 的设 备 ，所 以将 ＨＭＣ５８８３Ｌ 和 ＭＰＵ６０５０连 在 同一个 Ｉ２Ｃ接 口上 。地磁 传感 器 电 路 如 图 ２所 示 。
作者 简 介 ：杭 成 （１９９４一），男 ，江 苏 苏州 ，本科 。
６７
的效果 。该 滤 波 器 由其 理 论 的 主要 开 发 者 之一 鲁 道
夫 ·卡尔 曼 而得名 的 。卡尔 曼滤 波器 是 一种 在 时域 上
有 效 的递 归 滤 波 器 ，可 以从 包含 噪声 的不 准确 测 量
中 ，估 计 动态 系统 的真 实状 态 。本 文将 卡 尔曼 滤波 应
用在 四旋 翼 飞行器 的姿态解 算 中 ，通过 上 位机 显示 了
摘要：文章利用卡 尔曼滤波算法对四旋翼飞行器 中的姿态进行解算。利用 自主设计的四旋翼飞行器控制 系统 ，在加速 度计和 陀螺仪 获得 飞行 姿 态原始数 据的基础 上 ，通过 卡 尔曼滤波算 法进行 了数 据融合 。 关键词 ：四旋翼飞行器；卡 尔曼滤波 ；姿 态解算
Ｏ 引 言
随着 ＭＥＭＳ（Ｍｉｃｒ０一Ｅｌｅｃｔｒｏ—Ｍｅｃｈａｎｉｃａｌ Ｓｙｓｔｅｍｓ，
ＭＰＵ６０５０是 整合 性 ６轴 运 动处 理 组件 ，相 比于
多个传感器组合的方案 ，可 以避免组合陀螺仪 、加速 器 各轴 问角度 差 的 问题 ，同时减 少 了大量 的 ＰＣＢ板
空间和布局 的复杂度。数据可 以通过最高为 ４００ｋＨｚ 的 Ｉ２Ｃ总 线传 输 。ＭＰＵ６０５０通 过 Ｉ２Ｃ接 口连 接到 主 控 制器 ＳＴＭ３２Ｆ４０７ＶＥＴ６。加速 度计 和 陀螺仪 电路 如
更新 阶段 ：（１）计算 矩阵元素中的偏导数 ，做线
性 化处 理 。（２）计算测量余量 。（３）卡尔曼滤波器更新
公式 ：Ｋ＝ＰＫＨｒ（日 日 ＋尺） ； ＝ ＋ （ 一 ）； ＝
—
刎 ；（４）规 范 化 四元 数 。
２ 硬 件 电路 算 法 应 用 的硬 件 电路 平 台为 四旋 翼 飞行 器 控 制
第 １７期 ２０１６年 ６月
江 苏科 技信 息
Ｊｉａｎｇｓｕ Ｓｃｉｅｎｃｅ＆ Ｔｅｃｈｎｏｌｏｇｙ Ｉｎｆｏｒｍａｔｉｏｎ
Ｎｏ器姿态解算 中的应 用
杭 成 ，朱海 霞 １，２，许 毅立 １，苑 家玮
（１．南 京航 空航 天大 学金 城 学院 ，江 苏 南京 ２１１１５６；２．南京航 空航 天 大 学 ，江 苏 南京 ２１００１６）
卡尔曼滤波的数据融合前后 的波形 ，实验验证了该种
姿态 解算 算 法 的可行 性和 系统 的性 能 。
１ 卡尔曼滤波的姿态解算算法
卡 尔 曼 滤 波是 采 用 递 推方 法 寻 找 在均 方 误 差 最
小时的当前状态的最佳估计 。由于卡尔曼滤波采用递
推更新 的方法 ，并不需要之前所有的状态值 ，算法简
基于SCKF的4旋翼无人机的姿态估计
基于SCKF的4旋翼无人机的姿态估计张煌军;徐雪松;张文清;刘瑞【摘要】获得准确的姿态角对于无人机的控制来说是十分重要的.考虑到平方根容积卡尔曼滤波算法(square-root cubature Kalman filter,SCKF),既能够克服扩展卡尔曼滤波(EKF)方法因线性化带来的误差,具有更好的非线性滤波功能,又在传统容积卡尔曼滤波方法中加入了平方根技术,从而能够有效提高数值计算的稳定性,并降低了算法的复杂度.该文将SCKF算法应用于4旋翼无人机的姿态估计中,提出了一种新的4旋翼无人机的姿态估计方法,并进行了仿真实验.实验结果表明:该方法相比传统的EKF方法滤波精度更高,相比较传统的容积卡尔曼滤波(CKF)、无迹卡尔曼滤波(UKF)方法计算时间更短.【期刊名称】《江西师范大学学报（自然科学版）》【年(卷),期】2019(043)002【总页数】6页(P154-159)【关键词】平方根容积卡尔曼滤波;四旋翼无人机;姿态估计【作者】张煌军;徐雪松;张文清;刘瑞【作者单位】华东交通大学电气与自动化工程学院,江西南昌 330013;华东交通大学电气与自动化工程学院,江西南昌 330013;华东交通大学电气与自动化工程学院,江西南昌 330013;华东交通大学电气与自动化工程学院,江西南昌 330013【正文语种】中文【中图分类】TP2730 引言近年来,无人机技术取得了空前的发展和巨大的进步.旋翼无人机因为具有结构简单、可靠性高、成本低廉、操作方便、受地形的影响较小等优势,已经被普遍应用于军事与民用领域[1].正是由于旋翼无人机的应用范围越来越广,对其控制的稳定性提出了更高的要求.准确的姿态角[2-3]估计是实现更好的无人机控制的先决条件,因此目前有许多学者致力于无人机姿态角估计方法的研究.针对无人机的姿态估计问题,国内外学者提出了多种方法.扩展卡尔曼滤波[4-5](extend Kalman filter,EKF)是最常见的无人机姿态估计方法.EKF通过在系统工作点附近线性化获得近似线性模型,然后采用经典卡尔曼滤波[6]方法进行状态估计,可以解决非线性动态过程的状态估计问题.不过EKF在线性化处理时需要求解雅克比(Jacobian)矩阵[7],求解过程比较复杂.同时,线性化的过程也降低了姿态角的估计精度.由于无人机动态过程具有较强的非线性,基于EKF的姿态估计误差较大,为了提高估计精度,人们将无迹卡尔曼滤波[8-9](unscented Kalman filter,UKF)方法引入无人机的姿态估计中.UKF通过无迹变换[10](Unscented Transform,UT)可以减小EKF因线性化而造成的误差,从而提高估计精度,而且能够避免复杂的雅可比矩阵求解.不过,UKF在递推过程中,UT变换的中心点权值可能为负,会导致滤波数值不稳定.粒子滤波[11](Particle filter,PF)是一种十分有效的非线性滤波方法,所以也被应用于无人机的姿态估计问题求解.PF基于状态空间中传播的加权随机样本集来逼近后验概率密度函数,对于非线性系统的状态估计求解十分有效.PF滤波算法的主要问题在于存在粒子衰竭、维数灾难.容积卡尔曼滤波[12](cubature Kalman filter,CKF)算法是在卡尔曼滤波基础上拓展而来的一种非线性滤波算法,采用基于3阶球面-径向容积准则[13]的CKF进行姿态估计,能有效提高姿态估计精度.相比较于UKF,CKF 的采样点少1个,且这些采样点的权值相同,均为正值,不会出现当维数过大时权值为负的情况.EKF将非线性问题线性化,因此其估计精度不高,对于强非线性系统还会导致滤波结果发散.UKF、CKF通过选择确定数量的采样点[14]去近似状态向量的均值和方差,能够较好地解决EKF线性化引起的误差.然而在无人机姿态角的滤波过程中会带入计算误差,使得误差协方差的正定性[15-16]会逐渐丧失和矩阵开方运算的失败,最终还会致使整个滤波算法的失效.SCKF在CKF的基础上增加了平方根技术,通过传播误差协方差的平方根形式,避免了矩阵的开方运算,保证了数值计算的稳定性.同时,还降低了滤波算法的复杂度,在保持CKF的滤波效果的基础上,还可以减少计算时间[17-19].因此,本文将SCKF滤波方法应用于4旋翼无人机的姿态估计问题求解,提出了一种基于SCKF的姿态估计方法,并采取了对比实验方案.实验结果表明:该算法相比较EKF、UKF和传统的CKF来说能够获得更高的估计精度,而与UKF、CKF相比,SCKF所需的运算时间更短.1 4旋翼无人机的数学模型4旋翼无人机在机体的前后左右分布着4个桨翼,且其结构为十字形.4旋翼无人机通过调整各个电机的速度来变更旋翼的转速,从而改变旋翼升力的大小,通过升力的变化来控制无人机的姿态,其结构图如图1所示.图1 无人机结构图4旋翼无人机的前后2个旋翼顺时针转动的同时,左右2个旋翼按照逆时针旋转,所以当无人机的状态处于平衡时,抵消了陀螺效应和空气动力扭矩效应.当4旋翼无人机的前后旋翼转速不相同时,引起的力矩差使机体绕y轴转动,实现无人机的俯仰运动,转过的角度为俯仰角(θ).当4旋翼无人机的左右旋翼转速不相同时,引起的力矩差使机体绕x轴转动,实现无人机的横滚运动,转过的角度为横滚角(φ).若每个旋翼转速一样的情况下,反扭矩相互抵消,无人机保持稳定,不转动;若各个旋翼转速不完全一样时,反扭矩无法抵消将引起无人机绕z轴转动,实现无人机的偏航运动,转过的角度为偏航角(ψ).无人机的姿态信息系统为非线性系统,其离散时间的状态空间模型为其中为6×1维状态向量;Wk-1=[W0 W1 W2 W3 W4 W5]T为6×1维系统噪声向量;Zk=[ωx ωyωz]T为3×1维量测向量,Zk为4旋翼无人机机体坐标系下相对于地理坐标系下的角速度向量;Vk=[V0 V1 V2]T为3×1维量测噪声向量,W、V都为零均值的高斯白噪声.系统的控制输入向量U[20]为其中KL 是升力系数,KQ为反扭矩系数.U2、U3、U4分别为横滚、俯仰、偏航力矩.Ω1,Ω2,Ω3,Ω4分别为4个旋翼的转速.离散后系统的状态方程为(1)其中a1=((JY-JZ)x4(k-1)x6(k-1)/JX+KDXx2(k-1)/JX+lU2(k-1)/JX)T,a2=((JZ-JX)x2(k-1)x6(k-1)/JY+KDYx4(k-1)/JY+lU3(k-1)/JY)T,a3=((JX-JY)x2(k-1)x4(k-1)/JZ+KDZx6(k-1)/JZ+U4(k-1)/JZ)T,这里l是无人机4个轴的臂长,KDX、KDY、KDZ分别为机体坐标系下3个轴上的阻力系数,JX、JY、JZ分别是绕着x、y、z轴旋转的转动惯量.姿态角的信息可以由各种传感器来取得.最常使用的是通过陀螺仪获得无人机的3个姿态角的角速度.由于陀螺仪得到的是机体坐标系下的三轴角速度,应该将其变换到导航坐标系中,其量测方程为(2)其中ωx(k)=x2(k)-sin(x3(k))x6(k),ωy(k)=cos(x1(k))x4(k)+sin(x1(k))cos(x3(k))x6(k),ωz(k)=cos(x1( k))cos(x3(k))x6(k)-sin(x1(k))x4(k)由状态方程和量测方程可知无人机的姿态估计问题存在着较强的非线性.2 基于SCKF的姿态估计方法SCKF直接传播误差协方差的平方根形式,不需要矩阵的开方运算,不但能克服EKF 精度低的问题,还可解决UKF、CKF因误差协方差失去正定性而无法开方的问题.因此,本文将SCKF方法应用于4旋翼无人机的姿态角估计中,整个方法流程图如图2所示.图2 SCKF算法流程图整个姿态估计算法中的一个周期可以分为2大部分:第1部分为时间更新;第2部分为量测更新.其具体计算方法如下:Step 1 初始化.引入初始条件p0|0和Step 2 时间更新.利用系统的状态方程,根据上一个周期的状态估计本周期状态值. Step 2.1 计算采样点.利用状态误差协方差的平方根形式sk-1|k-1和上一周期的状态获得非线性滤波所需要的采样点集Step 2.2 各采样点的状态预估.计算各采样点经过状态方程传递后的预测值Step 2.3 预估状态合成.根据各采样点的预测情况,计算状态的一步预测值Step 2.4 预测误差协方差的平方根计算.根据各采样点预估情况,计算状态估计预测误差协方差阵的平方根形式Step 3 量测更新.根据陀螺仪测量得到的3轴角速度向量,对状态估计进行修正. Step 3.1 计算采样点.根据Step 2.3和Step 2.4的结果,计算量测方程的输入采样点Step 3.2 各采样点的量测输出估计.根据量测方程,预测各采样点可能的观测输出值Zi,k|k-1=h(Xi,k|k-1).Step 3.3 观测输出合成.根据上一步计算的各采样点的观测输出,合成观测输出的预测值Step 3.4 滤波增益计算.根据Step 2.3、Step 3.1、Step 3.2和Step 3.3,计算滤波增益.主要计算过程分3步:第1步,根据各采样点的观测输出,计算观测误差协方差的平方根szz,k|k-1,其计算方法为szz,k|k-1=qr((Zk|k-1 sR,k));第2步,根据各个采样点的状态预估和观测预估情况,计算状态与观测输出误差的协方差pxz,k|k-1,其计算方式为第3步,根据观测误差协方差的平方根形式与互协方差,计算滤波增益Step 3.5 状态更新.即根据状态预估、观测误差和滤波增益,计算本周期的状态输出Step 3.6 对状态误差协方差的平方根形式的更新.获得用于下一周期递推的状态误差协方差矩阵的平方根形式sk|k=qr((xk|k-1-wkZk|k-1 wksR,k)).Step 4 返回Step 1.其中qr(·)表示矩阵的QR分解,sQ,k-1、sR,k分别表示为过程噪声Qk-1、量测噪声Rk的平方根形式,n为状态向量的维数,ω=1/(2n)为权值.3 仿真实验及结果分析为了验证算法性能,本文采用Matlab的Simulink建立了4旋翼无人机的控制系统模型,并通过该模型获得用于滤波计算的系统输入输出数据.控制系统结构框图如图3所示.4旋翼无人机模型见(1)式,其中无人机的参数:m=0.33 kg,g=9.81m·s2,l=0.33 m,JX,JY,JZ分别为0.008,0.008和0.02 kg·m2,KDX,KDY,KDZ均为1.565×10-6 N·s2.观测数据通过陀螺仪获得,陀螺仪模型见(2)式,控制器采用反步控制器,详细算法见文献[21].图3 4旋翼无人机的控制系统无人机3个姿态角设定值为θ=5sin t,φ=5sin t,ψ=5sin t.系统仿真时间20 s,采样周期T=0.01 s,系统过程噪声方差Q=diag[0.1 2.3 1 3 1 0.01]×10-5,陀螺仪测量噪声方差R=diag[0.1 0.01 10]×10-2.分别采用EKF、CKF、UKF和本文算法SCKF作为滤波器进行滤波实验,具体比较滤波精度和计算时间2个方面的性能.图4分别为4种方法的俯仰角估计及其误差情况比较;图5分别为4种方法的横滚角估计及其误差情况比较;图6分别为4种方法的偏航角估计及其误差情况比较.4种算法结果的均方误差对比见表1,4种方法的计算时间对比见表2.图4 俯仰角估计情况比较以及误差比较图5 横滚角估计情况比较以及误差比较图6 偏航角估计情况比较以及误差比较表1 4种方法的均方误差比较RMSE方法本文算法CKFUKFEKF俯仰角(pitch/deg)0.217 10.221 70.222 60.284 1横滚角(roll/deg)0.292 90.293 40.293 00.408 7偏航角(yaw/deg)0.089 70.108 10.107 90.144 4表2 4种算法的运算时间对比 ms方法本文算法CKFUKFEKF时间0.251 90.294 80.295 80.101 6从图4～图6中可以看出,无论俯仰角、横滚角还是偏航角,在4种方法中,本文算法的最大估计偏差最小,EKF的最大估计偏差最大,CKF与UKF的最大估计偏差差不多大,且介于本文算法与EKF之间.表1数据可以比较4种算法的平均估计效果.从表1数据来看,本文算法获得的俯仰角、横滚角、偏航角的均方误差最小,UKF估计得到的姿态角的均方误差与CKF相比,这2个方法的均方误差大体一样,EKF的最大.综合考虑最大估计偏差和平均估计偏差2个方面可以看出,对于4旋翼无人机的姿态估计问题,本文算法精度最高,EKF精度最差,CKF和UKF算法的估计精度则介于本文算法与EKF之间,且与本文算法较接近.表2可以比较4种方法的时间消耗.从表2可知,EKF滤波由于方法简单,所需的运算时间最短,CKF和UKF计算比较复杂,所需的时间较长,本文算法所花的时间虽然比EKF长,但是比CKF、UKF明显要短.综合起来可以看出,本文算法在保持CKF、UKF等非线性滤波方法相对传统EKF方法有良好的滤波精度的优势下,而且对比CKF、UKF算法,具有运算时间短、数值稳定性高等优点.4 结论本文将平方根容积卡尔曼滤波方法应用于4旋翼无人机的姿态角估计中,解决了EKF线性化引起的误差大的情况,以及UKF、CKF中误差协方差失去正定性并导致矩阵开方失败等问题,降低了运算的复杂程度,保证了数值的稳定性和提高了滤波精度.通过4旋翼无人机的姿态角估计仿真实验数据表明:在滤波时间上,本文算法与CKF、UKF相比有较大的优势;在滤波精度方面,本文算法比EKF、UKF、CKF更高.5 参考文献【相关文献】[1] 陶于金,李沛峰.无人机系统发展与关键技术综述 [J].航空制造技术,2014,464(20):34-39.[2] 彭孝东,张铁民,李继宇,等.基于传感器校正与融合的农用小型无人机姿态估计算法 [J].自动化学报,2015,41(4):854-860.[3] Valenti R G,Dryanovski I,Xiao J.Keeping a good attitude:a quaternion-based orientation filter for IMUs and MARGs [J].Sensors,2015,15(8):19302-19330.[4] 金舒灿,胡越黎,张贺.基于EKF的四旋翼姿态解算仿真与设计 [J].电子技术应用,2017,43(9):127-131.[5] Li Jian,Wei Xinguo,Zhang Guangjun.An extended Kalman filter-based attitude tracking algorithm for star sensors [J].Sensors,2017,17(8):1921.[6] Valenti R G,Dryanovski I,Xiao Jizhong.A linear Kalman filter for MARG orientation estimation using the algebraic Quaternion algorithm [J].IEEE Transactions on Instrumentation and Measurement,2016,65(2):467-481.[7] Goodarzi F A,Lee T.Global formulation of an extended Kalman filter on SE(3) for geometric control of a quadrotor UAV [J].Journal of Intelligent and RoboticSystems,2017,88(2/3/4):1-19.[8] Julier S J,Uhlmann J K.Unscented filtering and nonlinear estimation [J].Proceedings of the IEEE,2004,92(3):401-422.[9] Yuan Xuebin,Yu Shuai,Zhang Shengzhi,et al.Quaternion-based unscented Kalman filter for accurate indoor heading estimation using wearable multi-sensor system[J].Sensors,2015,15(5):10872-10890.[10] Garcia R V,Kuga H K,Silva W R,et al.Unscented Kalman filter and smoothing applied to attitude estimation of artificial satellites [J].Computational and AppliedMathematics,2018,37(4):1-10.[11] 鱼少少,裴军,胡超.非线性滤波在SINS中的应用 [J].天文研究与技术,2018,15(1):104-110.[12] Arasaratnam I,Haykin S.Cubature Kalman filters [J].IEEE Transactions on Automatic Control,2009,54(6):1254-1269.[13] 石章松,吴中红,刘健,等.微小型飞行器多传感器融合容积姿态估计 [J].现代防御技术,2017,45(3):34-40.[14] 魏喜庆,宋申民.基于容积卡尔曼滤波的卫星姿态估计 [J].宇航学报,2013,34(2):193-200.[15] 李洪阳,何潇.基于SCKF方法的非线性随机动态系统故障诊断方法 [J].山东大学学报:工学版,2017,47(5):130-135.[16] 刘华,缪晨,吴文.平方根嵌入式容积卡尔曼粒子滤波算法 [J].南京理工大学学报:自然科学版,2015,34(4):471-476.[17] 安军,杨振瑞,周毅博,等.基于平方根容积卡尔曼滤波的发电机动态状态估计 [J].电工技术学报,2017,32(12):234-240.[18] 熊志刚,黄树彩,赵炜,等.均方根嵌入式容积粒子PHD多目标跟踪方法 [J].自动化学报,2017,43(2):238-247.[19] 袁晓波,张超,詹银虎.强跟踪自适应SRCKF的卫星姿态确定算法 [J].测绘科学,2018(3):6-11.[20] 左宗玉.四旋翼无人飞行器自适应轨迹跟踪控制 [C].Proceedings of the 30th Chinese Control Conference,Yantai,2011:2435-2439.[21] 王仲伦.基于反步法的四旋翼无人机容错控制 [D].南昌:华东交通大学,2017.。
卡尔曼滤波在无人机姿态中的应用研究
Control Theory and Applications
《自动化技术与应用》 2019 年第 38 卷第 11 期
卡尔曼滤波在无人机姿态中的应用研究
邓洪明 , 贺 勇 , 於小杰 , 吴成通
（长沙理工ห้องสมุดไป่ตู้学 电气与信息工程学院 ，湖南 长沙 410114）
摘 要 : 无人机的发展，带动着研发无人机产品企业之间的日益激烈，低成本成为了大部分企业的首选。由于小型无人机中低成本 的传感器，相对于采用 MEMS 加速度计，陀螺仪精度高一些，但时间一长就会出现漂移影响，导致小型无人机出现浮动等不 良问题。针对 MEMS 加速度计和陀螺仪的问题，对最优估计、离散型卡尔曼进行了研究，并进行了相应参数分析，提出了利 用离散卡尔曼滤波器对芯片 MPU6050 陀螺仪和加速度计进行数据滤波。通过建模及其参数整定的重要程度，与未进行滤 波算法处理的结果和飞行效果相比较显示，该方法改善了漂移、抗噪声问题，该整定后的卡尔曼估计算法对小型无人机滤波 是可靠的。
Key words: small drone; Kalman filter; drift; parameter tuning
1 引言
小型四旋翼无人机外形结构简单并易于操作，它的 飞行控制技术成为无人机研究重点之一，无人机具有多 变量、非线性、强耦合和干扰敏感的特性。许多企业投资 开发成本低的小型无人机满足国内需求之外还出售国外 玩具市场，但其产品传感器陀螺仪容易受时间产生漂移， 对控制器性能产生影响。张承岫[1]等人针对四轴飞行器 惯性测量单元（IMU）易受干扰、存在漂移等问题，利用惯
收稿日期:2018-08-22
性传感器 MPU6050 采集实时数据，以互补滤波为基础， 提出一种可以自适应补偿系数的互补滤波算法。文献[2] 指出四旋翼模型可近似一个线性定常欠强耦合系统，系 统的不稳定可以利用二次型最优控制对系统进行镇定。 向少林 针 [3] 对目前大多数飞行控制系统姿态信息获取不 全、数据融合算法复杂等问题，对四轴飞行器的姿态数据 采集、传感器数据特性数据融合等方面进行了研究，设计 了一种基于互补滤波融合算法的四轴飞行控制系统，采 用了陀螺仪、加速度计和电子罗盘作为传感器组，将其用 于多种姿态信息的获取。
基于卡尔曼滤波的四旋翼飞行器悬停控制研究
基 于卡 尔曼 滤 波 的四旋 翼 飞行器 悬停 控 制 研 究
吴 友 强 ， 纪 浩 ， 邹力涵 。
（ １ ． 武 汉 大 学 动 力 与机 械 学院 ， 武汉 ４ ３ ０ ０ ７ ２ ； ２． 西 安 电子 科 技 大 学 电子 工 程 学 院 ， 西安 ７ １ ０ ０ ７ １ ）
Ｋｅ ｙ ｗｏｒ ｄ ｓ： ｑｕ ａ ｄ ｒ ｏ ｔ ｏ ｒ ； ｈ ｏ ｖ ｅ ｒ ｉ ｎｇ； Ｋ ｍ ａ ｎ ｆ ｉｌ ｔ ｅ ｉｎｇ； ｒ ＰＩ Ｄ
四旋 翼 飞 行 器 因其 结 构 简 单 、相 对 载 荷 能 力
强、 垂 直起 降 和 机 动 性 能 好 等 优 点 ， 被 当 做 了一 种
Ｗ Ｕ Ｙｏ ｕ — ｑ ｉ ａ ｎ ｇ ． Ｊ Ｉ Ｈａ ｏ １ ， Ｚ ＯＵ Ｌ ｉ — ｈ ａ ｎ
（ １ ． Ｓ ｃ ｈ ｏ ｏ ｌ ｏ ｆ Ｐ ｏ ｗｅ ｒ ａ ｎ ｄ Ｍｅ ｃ ｈ ａ ｎ ｉ ｃ ａ ｌ Ｅ ｎ ｇ ｉ ｎ ｅ ｅ ｉ ｒ ｎ ｇ ， Ｗｕ ｈ ａ ｎ Ｕｎ ｉ ｖ ｅ ｒ ｓ ｉ ｔ ｙ， Ｗｕ ｈ ａ ｎ ４ ３ ０ ０ ７ ２， Ｃ ｈ ｉ ｎ ａ： ２ ． Ｓ ｃ ｈ ｏ ｏ ｌ ｏ ｆ Ｅ ｌ ｅ ｃ ｔ ｒ ｏ ｎ ｉ ｃ Ｅｎ — ｇ ｉ ｎ ｅ ｅ ｒ ｉ ｎ ｇ ， Ｘｉ ｄ ｉ ａ ｎ Ｕｎ ｉ ｖ ｅ ｒ ｓ ｉ ｔ ｙ， Ｘｉ ’ ａ ｎ ７ １ ０ ０ ７ １ ， Ｃ ｈ ｉ ｎ ａ ）
Ａｂ ｓ ｔ ｒ ａ ｃ ｔ ： Ｂ ａ ｓ ｅ ｄ ｏ ｎ ａ ｓ ｉ ｎ ￣ｅ ｓ ｅ ｎ ｓ ｏ ｒ ｆ ｏ ｒ ａ ｉ ｒ ｃ ｒ ａ ｆ ｔ ｉ ｎ ｈ ｏ ｖ ｅ ｒ ａ ｔ ｔ ｉ ｔ ｕ ｄ ｅ ｍ ｅ ａ ｓ ｕ ｒ ｅ ｍｅ ｎ ｔ ｉ ｎ ａ ｃ ｃ ｕ ｒ ａ ｔ ｅ ｆ ａ ｕ ｌ ｔ ｓ ｏ ｆ ｔ ｈ ｅ ｑ ｕ ａ ｄ ｒ ｏ ｔ ｏ ｒ ，
TN07 - 卡尔曼滤波在姿态解算和位置估计中的应用
TN07 – 卡尔曼滤波在姿态解算和位置估计中的应用1. 卡尔曼滤波参考：ttp:///wiki/卡尔曼滤波 对象模型 1k k k k k kx A x B u w -=++ （1）k k k ky C x v =+（2）其中k x 为状态量，k y 为观测量，k w 和k v 分别为过程噪声和输出噪声，假定其为均值为0，协方差矩阵为k Q 和k R 的正态分布噪声。
()0,k k w N Q ()0,kk v N R滤波算法 预测 1ˆˆk k k k k xA xB u -=+(状态预估)（3） 1Tk k k k kP A P A Q -=+(状态协方差预估)（4）误差 ˆk k k k e y C x =-（测量误差）（5）T k k k k k S C P C R =+（测量误差协方差）（6） 1T k k k k L P C S -= （最优卡尔曼增益）（7）更新 ˆˆk k k k xx L e =+ （状态更新）（8）()k k k k P I L C P =-（状态协方差更新）（9）其中协方差准确的反映了估计的协方差 ()ˆcov k k k P x x =-()cov k k S e =2. 扩展卡尔曼滤波扩展卡尔曼滤波应用卡尔曼滤波的算法于非线性对象，对象模型为 ()1,,k k k k x f x u w -=（10）(),k k k y h x v =（11）其中变量的意义与卡尔曼滤波对象模型相同。
在卡尔曼滤波算法中，分别用非线性函数估计对象的状态和观测量，用非线性函数的Joccob 矩阵进行状态和协方差的更新运算，如下预测 ()1ˆˆ,,0k k k xf x u -=(状态预估)（12） 1T k k k k kP A P A Q -=+(状态协方差预估)（13）其中 ()1ˆ,,0k k k fA x u x -∂=∂(状态转换矩阵)（14）误差 ()ˆ,0k k k e y h x=-（测量误差）（15）Tk k k k k S C P C R =+（测量误差协方差）（16） 1T k k k k L P C S -= （最优卡尔曼增益）（17）其中 ()ˆ,0k k hC x x∂=∂(输出矩阵)（18）更新 ˆˆk k k k xx L e =+ （状态更新）（19） ()k k k k P I L C P =-（状态协方差更新）（20）3. 姿态解算姿态更新模型 12q M qω=- （21）其中000xy z xzy y zx zyxM ωωωωωωωωωωωωω⎡⎤⎢⎥--⎢⎥=⎢⎥--⎢⎥--⎢⎥⎣⎦（22）[]0123,,,Tq q q q q =，为姿态的四元素表示。
四翼飞行器姿态控制算法研究--叶树球
飞行器位置姿态图。
四翼飞行器主要硬件及电路组成一个基本的飞行器机体设计方案中,包括至少以下部分:主控制器、姿态传感器、电源电机以及通信接口,其系统结构图如下所示:由于四翼飞行器的传感器采样数据包含大量的噪声,使得控制系统对飞行器的姿态解算会出现很大的误差,为减小误差对控制系统的影响,需要对加速度、角加速度、地磁传感器的测量数据进行融合滤波处理才能得到更加有效的飞行器姿态信息。
本章首先介绍四翼飞行器姿态解算中常用的数据滤波算法包括,卡尔曼滤波和互补滤波,接着研究了利用四元数来对飞行器进行姿态解算的过程。
1.卡尔曼滤波卡尔曼滤波是一种递归(自回归)的滤波器,该滤波器的显著优点是高效率。
卡尔曼滤波的思想是釆用递归自更新的方法来估计出当前的状态,同时保证目标的真实状态与滤波估计出的状态的均方误差收敛值最小。
卡尔曼滤波器的过程由两个阶段组成,共由五个公式表示,它们分别是预测和更新,前阶段含两个公式,后阶段包含三个公式。
在前一阶段:滤波器需要对对象的当前状态作出评估,评估过程依照上一次对对象状态的估计;在后一阶段:滤波器需要对对象作出更加精确的评估,评估过程是通过优化当前状态的观测值来实现的。
如下式(3.1)和式(3.2)是预测阶段的公式公式中:Rk:对象测量噪声的协方差。
Kk:最优卡尔曼增益。
I:单位矩阵。
其中Zk是满足式(3.6)的对对象的一个测量值。
其中:Hk是对象的观测矩阵,Vk是均值为0的观测噪声矩阵。
公式(3.3)的作用是更新当前的最优卡尔曼增益;公式(3.4)的作用是根据当前卡尔曼增益对系统当前状态进行估计,也就是卡尔曼滤波的估计输出;公式(3.5)的作用是对滤波器当前误差的协方差进行更新。
这三个公式得出了当前的系统状态,并更新滤波器自身的状态,以便下一个卡尔曼滤波周期的正常进行。
2.互补滤波简单来说互补滤波就是把滤波器的两部分加起来成为一个滤波器,使其输出一个准确的线性估计在四翼飞行器中,由于加速度传感器的灵敏度高,其受外部的影响比较大,其测量值也容易被混入噪声,直接使用测量数据进行姿态解算会使得控制系统的输出误差非常大。
卡尔曼滤波在四轴飞行器导航中的应用
真初始条件如下： 建立以飞行器起飞地点为坐标 原点的空间直角坐标系， 四轴飞行器当前在空间 某位置 以 某 一 速 度 飞 行。 具 体 参 数 为 l x （ 0 ） = 50 m， l y （ 0 ） = 20 m， l z （ 0 ） = 10 m， v x （ 0 ） = 2 m / s， v y （ 0 ） = 3 m / s， v z （ 0 ） = 0 m / s。 系统初始误差方 P （ 1， 1 ） = P （ 2， 2 ） = P （ 3， 3） = 阵 P0 为对角阵， P（ 4 ， 4 ） = P （ 5， 5 ） = P （ 6， 6 ） = 1 2 。 GPS 观测 12 ，
Abstract： Navigation control accuracy of Quadrocopter is the main factor restricting its application． Quadrocopte navigation algorithm was put forward based on Kalman filter． The method mixes MIMU and GPS together． The simulation experiment and and has good error tolerpractical application indicate that the method can improve the accuracy of navigation and positioning， ance． Key words： quadrocopter； MIMU / GPS integrate navigation； Kalman filter XU Huazhong： Prof． ； School of Automation Engineering， WUT， Wuhan 430070 ， China．
基于扩展卡尔曼滤波的四旋翼无人机姿态估计方法
基于扩展卡尔曼滤波的四旋翼无人机姿态估计方法作者：段敏赵凌周莹来源：《现代信息科技》2022年第04期摘要：為提高四旋翼无人机姿态参数获取的准确性，确保后续姿态控制精度，采用STM32F407微控制器以及多传感器构成姿态测量系统。
对各传感器原始误差进行校准，应用扩展卡尔曼滤波（EKF）进行基于陀螺仪的状态预测和基于加速度计/磁力计的测量校正，融合信息并估计出3姿态角，与3自由度姿态算法验证系统测量出的姿态角真实值对比，3个角度的平均误差为0.7°，相对于基于单一陀螺仪积分和基于加速度计/磁力计的姿态解算，误差分别下降了3.034°和0.174°，该方法可有效提高EKF估计精度。
关键词：扩展卡尔曼滤波;四旋翼无人机;姿态估计中图分类号：TP368;V279 文献标识码：A文章编号：2096-4706（2022）04-0007-05Attitude Estimation Method for Quad-rotor UAV Based on Extended Kalman FilterDUAN Min， ZHAO Ling， ZHOU Ying（The College of Post and Telecommunication of WIT， Wuhan 430073， China）Abstract： In order to improve the accuracy of attitude parameters acquisition of quad-rotor UAV and ensure the subsequent attitude control accuracy， STM32F407 microcontroller and multi-sensor are used to form an attitude measurement system. The original error of each sensor is calibrated. The extended Kalman filter （EKF） is used for gyro-based state prediction andaccelerometer/magnetometer-based measurement correction. The information is fused and the three attitude angles are estimated. Compared with the real value of the attitude angle measured by the 3-DOF attitude algorithm verification system， the average error of the three angles is 0.7°， compared with the attitude solution based on single gyro integral and the attitude solution based on accelerometer/magnetometer， the errors are reduced by 3.034° and 0.174° respectively. This method can effectively improve the accuracy of EKF estimation.Keywords： extended Kalman filter; quad-rotor UAV; attitude estimation0 引言四旋翼无人机因其成本低廉、维护操作简单、可替代人力完成特殊任务等优点，近年来被广泛应用于军用和民用领域[1，2]。
卡尔曼滤波在四旋翼飞行器姿态测量中的应用
万方数据万方数据·８０·兵工自动化第３０卷则，但同时满足其它多条规则或某条规则持续满足，即可判定被测设备存在潜在故障。
如果满足第１条规则，也满足其它某条规则，则可判定发动机设备己发生功能故障，需要停机检查，如图７。
４结束语该系统可大幅降低报警系统的虚警率和误警率，还可及时发现导弹发动机的功能故障潜在故障，图６报警数据详细资料界面有效避免或降低故障发生所带来的损失。
隔ｉ丽．参考文献：匪基臣亟互］Ｈ厂＿叫竺笙！！竺型墨竺生量Ｉ【ｌ】张公绪，何国伟，郑慧英．新编质量管理学［Ｍ】．北京：臣量受到阴ｌ——高等教育出版社，２００３：７卜７９．厅疆历瓦了１一Ｉ毳ｌｒ◆Ｉ竺堡！！鲨型堡鱼垩查ｌ【２】王飞，李保华，冯伟，等．统计过程控制在无人机故障蕈刘篓ｎ匝酉巫要鬈舯啪叭刀·悃测黼杞２００７’３３０ｋ窖竺竺纠ｕｌ：二二二二二二二二＝［３１童国强．基于数据融合技术的多模型状态监测与故障Ｌ笠墨翌坚二＿ｊ－１Ｌ．叫竺笙！！垫型堡鱼奎兰丝丝竺兰ｌ预报［Ｄ】．南京：南京航空航天大学，２００５．Ｉ至噩互互型【４】崔吉俊．火箭导弹测试技术［Ｍ】．北京：国防工业出版图７故障逻辑判决示意图社．１９９９．通常，如果满足第１条规则，测量值略超出门限，［５】高明皓，王林斗，匡俊华，等·家庭报警信号处理系统而其他７条删未满足，可认定为检测系统虚警。
如【６】巍羹－雾姜芸囊差冀々篡缀翥等新方法＿绝对果不满足第１条规则，偶尔满足第２＂－８条规则中的～均值法及应用研究［Ｊ】．中国测试技术，２００６，３２（１）：某１条，可判定被测设备正常。
如果不满足第１条规４７—４９．（１－接第７４页广图３动态时由加速度计算的倾角７０６０５０４０｝吣３０２０ｌＯ０·…．…一｝…：－÷．＿喜。
０１００２００３００４００５００６００Ｎｉ耷图４动态时由卡尔曼滤波计算的倾角４结论该算法通过使用卡尔曼滤波算法对加速度传感器数据和陀螺仪数据进行融合，实时地改变测量噪声协方差的值，避免了动态噪声对加速度传感器的影响，提高了飞行器姿态测量的精度，为飞行器的自主飞行控制提供了可靠的姿态反馈，也为四旋翼飞行器系统姿态的进一步调整创造了条件，具有一定的应用价值。
自适应误差四元数无迹卡尔曼滤波四旋翼飞行器姿态解算方法
自适应误差四元数无迹卡尔曼滤波四旋翼飞行器姿态解算方法
自适应误差四元数无迹卡尔曼滤波四旋翼飞行器姿态解算方法崔培林;周翟和;吕品;胡斌【摘要】针对小型四旋翼飞行器在姿态解算时,使用的惯性器件在飞行器起飞、下降等过程中容易受到非重力加速度影响的问题,提出了一种自适应误差四元数无迹卡尔曼滤波的飞行器姿态解算方法.该方法在传统无迹卡尔曼滤波算法的基础上,结合陀螺仪漂移误差模型,使用误差四元数的方法构建无迹卡尔曼滤波的状态方程,选用加速度计和磁强计的输出作观测量,利用其构建无迹卡尔曼滤波的量测方程,并通过自适应调整量测噪声协方差矩阵,减小非重力加速度对姿态解算的影响.实验表明,当飞行器存在非重力加速度时,该方法可以有效减小非重力加速度影对姿态解算的影响.【期刊名称】《西安交通大学学报》【年(卷),期】2019(053)003【总页数】7页(P97-102,110)【关键词】四旋翼飞行器;非重力加速度;误差四元数;无迹卡尔曼滤波【作者】崔培林;周翟和;吕品;胡斌【作者单位】南京航空航天大学自动化学院,211106,南京;南京航空航天大学自动化学院,211106,南京;南京航空航天大学自动化学院,211106,南京;南京航空航天大学自动化学院,211106,南京【正文语种】中文【中图分类】TP274四旋翼飞行器(后文简称飞行器)是控制科学和无人机领域内备受关注的研究对象,飞行器姿态信息的测量精度直接影响了飞行器速度、位置的导航精度[1-2]。
在小型飞行器中,常使用陀螺仪、加速度计以及磁强计作为姿态测量器件,陀螺仪存在累积误差,而加速度计和磁强计受外界干扰较大,需要对其进行有效的数据融合以保证获得较为精确的姿态角[3-4]。
在姿态解算方法中,文献[5-6]使用卡尔曼滤波(Kalman Filter,KF)算法进行数据融合,该方法建模简单,实时性较好,但是忽略了非线性因素对滤波带来的影响。
文献[7-8]使用扩展卡尔曼滤波(Extended Kalman Filter,EKF)方法,这是一种关于噪声均值和协方差的线性化方法,用这种方法对状态方程和量测方程进行线性化处理,必然会引入误差。