基于互补滤波算法的四旋翼飞行器姿态和高度解算_肖宇1
需积分: 0 98 浏览量
2022-08-03
14:07:45
上传
评论 1
收藏 282KB PDF 举报
姿态和高度的解算
是四旋翼飞行器控制的基础! 也是四旋
翼飞行器研究的一个核心问题! 四旋翼飞行器的姿态和高度主
要由陀螺仪"加速度计和气压计来进行测量!但由于陀螺仪数据
随着时间的积分会产生静态误差!而加速度计存在着高频误差!
气压计所测得的高度也在真实高度附近变动! 因此本文根据互
补滤波原理!设计了两个互补滤波器!分别对陀螺仪数据和加速
度计数据以及加速度计数据和气压计数据进行互补滤波#
1
旋翼飞行器姿态的描述
1．1
坐
标系的定义
为了描述飞行器的运动状态!定义了两个坐标系$地理坐标
系和机体坐标系!如图
1
所
示#
Earth
%
X
!
Y
!
Z
&表示飞行器的地理
坐
标系!简称为
n
系!原
点
O
位于飞行器所在的点!
X
轴
沿着纬
线指东!
Y
轴沿着子午线指北!
Z
轴沿着地理垂线并与
X
"
Y
轴
构
成右手直角坐标系’
Body
%
x
!
y
!
z
& 表示机体坐标系! 简
称为
b
系! 机
体支架中
1
!
3
号电机所在的轴与
x
轴
重合! 机体支架中
2
!
4
号电机所在的轴与
y
轴
重合!
z
轴
垂直于机体平面!并与
x
"
y
轴构成右手直角坐标系(
飞行器坐标系以
x
轴为转轴所旋
转的角度为滚转角
α
! 以
y
轴为转轴所旋转的角度为俯仰角
β
!绕
着
z
轴所旋转的角度为偏
航
角
γ
!飞行器的高度为飞行器在地理坐标系下
Z
轴
的偏移量(
图
1
地理坐标系与机体坐标系
1．2
飞行器姿态的表示
本文在姿态融合过程中!使用四元数表示飞行器姿态( 四
元
数可定义为$
Q
＝q
0
＋q
1
i
＋q
2
j
＋q
3
k
! 其在物理上的意义为一个转
轴!一个坐标系到另一个
坐标系的旋转!可以用唯一的四元数进
行表示! 机体坐标系到地理坐标系的转换矩阵可用四元数的方
法表示为$
!
"
#
$
%
&
’
(%
&
)
*%
&
&
*%
&
+
&
%
%
)
%
&
*%
’
%
+
&
&
%
%
)
%
+
,%
’
%
&
&
&
%
%
)
%
&
(%
’
%
+
&
%
&
’
-%
&
)
(%
&
&
-%
&
+
&
%
%
&
%
+
-%
’
%
)
&
&
%
%
)
%
+
-%
’
%
&
&
&
%
%
&
%
+
(%
’
%
)
&
%
&
’
-%
&
)
-%
&
&
(%
&
+
!
"
"
"
#
$
%
%
%
&
%
1
&
计算出飞行器每个
控制周期的四元数! 即可根据飞行器的
姿态角!四元数的更新可由一阶龙格库塔法解算得到$
.
%
/,’/
&
$.
%
/
&
,
’/
&
-ω
0
%
)
-ω
1
%
&
-ω
2
%
+
ω
0
%
’
*ω
1
%
+
,ω
2
%
&
ω
0
%
+
,ω
1
%
’
*ω
2
%
)
*ω
0
%
&
,ω
1
%
)
,ω
2
%
’
(
)
)
)
*
+
,
,
,
-
%
2
&
2
互补滤波算法原理及设计
2．1
互补滤波算法的原理
在飞行器俯仰角和滚转角的测量上!加
速度计的静态响应
好!但由于飞行器震动等原因会存在高频的噪声信号!动态性能
较差!因此可设计一个低通滤波器来保留长时的有效信号!去除
短时快速变化的干扰信号( 而对于陀螺仪! 其动态响应性能良
好!但由于需要对陀螺仪数据进行积分!随着时间的积累会存在
一定的累积误差! 因此对陀螺仪的数据需要采用高通滤波的方
法来进行处理( 根据这两个传感器的特性!可以设计一个互补滤
波器!来求解飞行器的姿态( 由于加速度计只能测量飞行器的滚
转角和俯仰角! 因此本文所设计的互补滤波器只能矫正飞行器
的俯仰角和滚转角! 对飞行器偏航角的矫正可加入电子罗盘来
进行!本文不做研究(
在飞行器高度的测量上! 可通过坐标转换求出飞行器在地
理坐标系下
Z
轴的加速度分量! 并对其进行二
次积分可得飞行
器的飞行高度!但积分会产生累积误差’也可通过气压计测得高
度!但由于气压计的精度问题!会导致气压计所得的飞行器高度
会在真实值附近浮动( 从上述分析!可以发现!加速度积分会有
累积误差! 但加速度计的高频特性较好! 而气压计所测得的高
度!在长时间来看其值非常具有参考性!低频特性较好!因此可
以将加速度计的值进行积分与气压计的值进行互补滤波! 这样
就能取长补短!获得较好的效果(
2．2
基于加速度计和陀螺仪的互补滤波算法设计
为了充分发挥加速
度计和陀螺仪在各自频率范围内的动态
基于互补滤波算法的四旋翼飞行器姿态和高度解算
At
titude and Height Estimation of Quad－rotor Aircraft Based on Complemen tary Filter
肖
宇
（东南大学自动化学院，江
苏 南京
2
10096
）
摘
要：四旋翼飞行器的姿态和高度主要由陀螺仪、加速度计和气压计来进行测量，针对这些传感器存在温度漂移和噪
声干扰等问题，根据各个传感器的特点，设计了两个互补滤波器，分别对陀螺仪和加速度计数据进行互补滤波，以及加速度
计数据和气压计数据进行互补滤波。 在由
STM32
、陀
螺仪、气压计搭建的飞行器实验平台上进行了算法的实现，将互补滤波
算法得到的姿态和高度与仅由单个传感器数据得到的姿态和高度进行对比，验证了算法的有效性。
关键词：四旋翼飞行器，互补滤波器，姿态解算，高度解算
Abstract:B
ased on the characteristics of each sensor,this paper designs two complementary filter,one for gyroscope and
acceleration,the other for acceleration and barometer． Then,using the STM32,barometer,gyroscope and accelerometer built an
experiment platform and realize the algorithm in the exper imental platform．Finally,In order to verify the validity of the algo-
rithm,compared the attitude and height calculated by Complementary filter algorithm and the single sensor data．
Keywords:quadrotor aircraft,complementary filter,attitude estimation,height estimation
基于互补滤波算法的四旋翼飞行器姿态和高度解算
94
评论0