北京模板网站开发全包,昆明网站开发正规培训,阿里云服 WordPress,建立网站滤波器设计实验一 一. 无人机滤波器简介
无人机在飞行时会使用滤波器来处理传感器数据、控制飞行和稳定飞行#xff0c;以及实现导航和定位等功能。卡尔曼滤波器是无人机领域中常见滤波器类型之一#xff0c;也称为线性二次型估计#xff0c;能够从一系列不完全且包含噪声不… 滤波器设计实验一 一. 无人机滤波器简介
无人机在飞行时会使用滤波器来处理传感器数据、控制飞行和稳定飞行以及实现导航和定位等功能。卡尔曼滤波器是无人机领域中常见滤波器类型之一也称为线性二次型估计能够从一系列不完全且包含噪声不确定性的观测量中估计系统的未知状态其估计精度往往比单纯地基于单一观测量的方法更高。这种滤波器以它的发明者鲁道夫·卡尔曼(Rudolf E. Kalman)命名。
卡尔曼滤波器要求噪声误差满足高斯分布假设在这个特定情况下能够产生精确的条件概率估计,卡尔曼滤波器算法主要分为两步处理。在预测步骤中卡尔曼滤波器产生当前状态变量的预测估计这些估计量包含不确定性,一旦出现下一个观测量伴随着一定的误差以及随机噪声之前的估计量会以加权平均的方式更新其权重值会随着估计的确定性而变化确定性越大其权重值越大。卡尔曼滤波器是一种实时的递归滤波器仅仅利用当前观测量和先验估计量及不确定性矩阵而不需要增加多余的历史信息。
二. 测量原理
三轴加速度计固连在多旋翼机体其三轴与机体坐标系一致。因此低频的俯仰角和滚转角观测量可以由加速度计测量值近似得到: 其中 表示加速度计测量值。
同时还有两点需要注意
1为了得到更加精确的角度信息需要消除加速度计的慢时变漂移。
2如果机体振动很大则 αxbmαybm将被噪声严重污染这样将进一步影响角度θmφm 的估计。因此机体的减振很重要。另外姿态变化率 θ·,φ·,ψ·和角速度bω有如下关系 由此可知俯仰角和横滚角可以由加速度计测量得到漂移小但噪声大。另一方面姿态角也可以通过角速度积分得到噪声小但是漂移大。 三、线性互补滤波器设计
下面我们着重以俯仰角为例详细推导下线性互补滤波俯仰角的拉氏变换可以表示为 为了估计俯仰角以上式子的 需要用传感器信息替代。
同时加速度计测量的俯仰角无漂移但噪声大我们可以将测量到的俯仰角建模为θm θnθ其中nθ 表示高频噪声θ表示俯仰角真值。 陀螺仪的角速度测量会有漂移但噪声小我们可以建模为 线性互补滤波的标准形式以传递函数方式表示为 为了计算机算法实现需要对其进行离散化 通过一阶向后差分法将s表示为 进一步表示为 再把上式化为差分方程可以得到 具体推导过程请见文献[1]第8章和文献[2]第8章内容。
互补滤波器的MATLAB程序可见如下。
其中“theta_am”和“phi_am”分别代表由加速度计计算出的俯仰角和滚转角theta_cf”和“phi_cf”分别代表由互补滤波计算出来的俯仰角和滚转角。 function [ phi_cf, theta_cf ] Attitude_cf(dt, z, phi_cf_k, theta_cf_k, tao) %函数描述: % 互补滤波姿态结算。 %输入 % dt: 时间间隔,单位s % z: 三轴角陀螺仪和三轴加速度计测量值[gx, gy, gz, ax, ay, az] % 单位rad/s, m/s2 % phi_cf_k, theta_cf_k: 上一时刻的角度值单位rad % tao: 滤波器系数 %输出: % phi_cf, theta_cf: 解算的姿态角单位rad gx z(1); gy z(2); ax z(4); ay z(5); az z(6); %使用加速度计测量值计算姿态角 g sqrt(ax*ax ay*ay az*az); theta_am asin(ax/g); phi_am -asin(ay/(g*cos(theta_am))); %互补滤波 theta_cf tao/(tao dt)*(theta_cf_k gy*dt) dt/(tao dt)*theta_am; phi_cf tao/(tao dt)*(phi_cf_k gx*dt) dt/(tao dt)*phi_am; end 可以得到结论
1直接对陀螺仪测量的角速度进行积分得到的姿态角有很大的累积误差并且还有可能发散2根据来自加速度计的原始数据计算得到的姿态角不会发散但噪声最大且有明显的尖峰尤其是使用实际飞行中的数据时3使用互补滤波器估计的姿态角是平滑的并且没有累积误差。 滤波器设计实验2 四、线性互补滤波器参数分析
基于上一讲中得出的滤波器公式 对其参数т值进行改变对所给的数据进行滤波即可分析该参数对滤波效果的影响。可在MATLAB中编写如下程序 %参数tao对滤波效果的影响 clear; load logdata n length(ax); %采集数据个数 Ts zeros(1,n); %时间间隔 Ts(1) 0.004; for k 1:n-1 Ts(k1) (timestamp(k 1) - timestamp(k))*0.000001; end theta_cf zeros(1, n); %互补滤波得到的滚转角对应theta phi_cf zeros(1, n); %互补滤波得到的俯仰角对应phi tao 0.001; for i 1 : 3 tao tao*10; for k 2 : n g sqrt(ax(k)*ax(k) ay(k)*ay(k) az(k)*az(k)); theta_am asin(ax(k)/g); phi_am -asin(ay(k)/(g*cos(theta_am))); theta_cf(i, k) tao/(tao Ts(k))*(theta_cf(i, k - 1) gy(k)*Ts(k)) Ts(k)/(tao Ts(k))*theta_am; phi_cf(i,k) tao/(tao Ts(k))*(phi_cf(i, k- 1) gx(k)*Ts(k)) Ts(k)/(tao Ts(k))*phi_am; end end 改变该程序中的 т值分别为0.010.11时的滤波效果。 可以看到参数т越大对高频噪声的滤波作用越明显。当т很大时 互补滤波器变为 相当于加速度计不起作用只使用陀螺仪的积分值。因此要合理选择参数т的值。
注本实验对应demo文件对于RflySim v3.0以下版本地址为*\PX4PSP\RflySimAPIs\Exp02_FlightControl\e4-FilterDesign\e4.1
对于RflySim v3.0及以上版本地址为*\PX4PSP\RflySimAPIs\5.RflySimFlyCtrl\1.BasicExps\e4-FilterDesign\e4.1 参考文献 [1] 全权,杜光勋,赵峙尧,戴训华,任锦瑞,邓恒译.多旋翼飞行器设计与控制[M],电子工业出版社,2018. [2] 全权,戴训华,王帅.多旋翼飞行器设计与控制实践[M],电子工业出版社,2020.
