昨天，有同学在公众号提出一个问题。从问题来看，他应该是在准备今年的节能平衡自行车组别的智能车竞赛。

卓大大，请问您可以针对卡尔曼滤波、互补滤波、清华滤波的原理与异同做一个推送吗？最近在查阅相关的资料学习滤波算法，感觉好难理解呀/::<

^{▲ 平衡双轮自行车}

由于近期相关四旋翼飞机、平衡车、活动机器人等领域都需要对控制对象的姿态进行感知，因此 卡尔曼(Kalman)滤波器 、 互补滤波器 的基本原理，以及它们之间的 实验比较 等相关的资料在网路上早已广泛存在了。下面的内容希望能够帮助最初接触这块内容的同学掌握理解这方面的内容。

同学们之所以提到对这三种算法进行比较，是因为对智能车竞赛中平衡车模控制算法中对于车模姿态（实际上只有车模前倾角以及角速度）进行测量中，需要融合惯性测量传感器（陀螺仪和加速度计）的输出数据，在以往比赛中很多同学使用了这三种算法，它们之间有着很深的联系。

谈到滤波器，它的一个作用就是将信号中的噪声去除。线性滤波器是一种广泛使用的方法，前面提到的三种方法都是属于线性滤波器，这样就可以通过它们的 传递函数 、频率特性等多方面进行对比。

为了分析这三种算法在控制平衡车模中的联系，就需要先从车模姿态传感器存在的问题分析出发，讨论解决的方案，逐步扩展到三种滤波算法。

01问题来源

用于测量平衡车模姿的惯性传感器主要包括两种：陀螺仪和加速度计。

1.陀螺仪的特性

陀螺仪输出的信号VG(t)V_G \left( t \right)VG(t)反映角速度θ˙(t)\dot \theta \left( t \right)θ˙(t)。通过对角速度的积分，便可以得到相应的角度。通常，在平衡车模直立控制中，需要角度、角速度两个量，通过一定的加权系数作为反馈量来控制电机输出力矩，从而维持车模的基本平衡。

下图给出了一个模拟陀螺仪输出的电压信号与转动之间的关系。

^{▲ 陀螺仪输出信号}

在陀螺仪静止的时候，输出一个电压常量VG0V_{G0}VG0，随着它的转动输出电压便上下浮动。

将陀螺仪输出的电压VG(t)V_G \left( t \right)VG(t)，减去静态VG0V_{G0}VG0，然后进行积分，便可以得到一个与角度成正比的量：K⋅θ(t)=∫−∞tVG(τ)−VG0dτK \cdot \theta \left( t \right) = \int_{ - \infty }^t {V_G \left( \tau \right) - V_{G0} d\tau }K⋅θ(t)=∫−∞tVG(τ)−VG0dτ

下图给出单电源运放组成的积分电路。调整电位器PV，使得它的电压等于陀螺仪静止时的常量VG0V_{G0}VG0，这样输出电压便与传感器倾角成正比。

^{▲ 对陀螺仪信号进行积分}

下图反映了该信号经过积分之后的信号。随着传感器在不同的角度，输出电压在处在不同的电压范围之内。

^{▲ 陀螺仪的积分信号反映了角度}

作为积分电路，本身它是一个不稳定系统。由于这种原因（陀螺仪静态电压漂移、运放偏置电压的漂移等）会使的积分输出电压会随着时间发生积分偏移。不仅使得输出角度出现误差，而且也会使得运发进入饱和而失去作用。

下图显示了陀螺仪在静止状态下，上面的积分电路输出逐步发生积分偏移，最终达到饱和的过程。

^{▲ 陀螺仪积分信号不稳定性}

因此，只使用陀螺仪则无法获得稳定准确传感器的姿态角度。

为了消除信号中的直流偏移，可以通过隔直电容来完成。下图显示了RC电路完成信号的直流隔离。为了保护运放不受直流偏移量的影响，将RC隔直电路移到积分电路的输入端。

^{▲ 在陀螺仪积分电路上增加RC高通滤波}

在下图中显示了增加隔直电容后的积分电路。其中R3并联在原来的积分电容C1两端，是为了抑制运放偏置电压的漂移对输出的影响。R3,C1R_3 ,C_1R3,C1组成的时间常数大约是10秒钟。

C3是对输出电压中点参考电压进行滤波。

^{▲ 带有RC隔直陀螺仪积分电路}

在积分电路的输入端增加了隔直电容C2，又在积分电容C1上并联了R3，这就抑制了积分电路的积分漂移。

下图显示了积分电路输出电压将会稳定在一个固定电压的过程。

^{▲ 增加隔直后电容消除零点漂移}

上述电路虽然抑制了角度速度积分的漂移。但对于角度信号中的低频分量也有很大的抑制。

将模块固定在一个确定的倾角，输出电压开始变化到相应的输出电压滞后，便逐渐向静态工作点进行衰减。

^{▲ 对姿态低频信息的丢失}

测图测量了陀螺仪经过积分后的输出信号。可以看到积分信号能够快速跟踪角度的变化。但是当陀螺仪一旦静止，输出电压值便向输出稳定值衰减。

^{▲ 陀螺仪输出及其隔直积分输出}

通过截取角度衰减曲线中7.5秒之后的数据，可以测量衰减的时间常数。对上面数据中陀螺仪输出信号的衰减曲线数据进行指数建模，可以得到拟合公式为：

y(t)=5.14⋅e−t8.04+0.6347y\left( t \right) = 5.14 \cdot e^{ - {t \over {8.04}}} + 0.6347y(t)=5.14⋅e−8.04t+0.6347

时间常数为8秒钟。反映了陀螺仪积分输出的稳态值，每经过8秒钟，输出值便衰减到原来的1/e=0.3671/e = 0.3671/e=0.367。

^{▲ 输出曲线的7.5秒~27.5秒的电压曲线以及指数拟合曲线}

y(t)=5.14⋅e−t8.04+0.6347y\left( t \right) = 5.14 \cdot e^{ - {t \over {8.04}}} + 0.6347y(t)=5.14⋅e−8.04t+0.6347

2. 加速度计特性

加速度计可以输出一个与传感器加速度成正比的电压量。由于加速度是一个矢量，输出的电压量与加速度绝对值AAA以及加速度与传感器敏感方向之间的夹角ϕ\phiϕ都有关系。具体关系如下：VA=k⋅A⋅cos⁡(ϕ)V_A = k \cdot A \cdot \cos \left( \phi \right)VA=k⋅A⋅cos(ϕ)

使用加速度来测量倾角主要是利用地球重力场加速度矢量gˉ\bar ggˉ，近似一个恒定加速度。此时将加速度的敏感方向与gˉ\bar ggˉ保持垂直。此时传感器的倾角θ(t)\theta \left( t \right)θ(t)与传感器输出电压VA(t)V_A \left( t \right)VA(t)之间就是正弦关系：VA(t)=k⋅A⋅sin⁡[θ(t)]V_A \left( t \right) = k \cdot A \cdot \sin \left[ {\theta \left( t \right)} \right]VA(t)=k⋅A⋅sin[θ(t)]

当角度θ(t)\theta \left( t \right)θ(t)小的时候，它就可以近似与加速度传感器输出成正比：VA(t)=Ka⋅θ(t)V_A \left( t \right) = K_a \cdot \theta \left( t \right)VA(t)=Ka⋅θ(t)。

下图显示了加速度传感器输出与传感器在不同位置之间的关系。

^{▲ 加速度计输出特性}

由于传感器随着车模运动会产生额外的加速度a(t)a\left( t \right)a(t)，它也会在加速度传感器上产生相应的电压波动。这就干扰了作为姿态倾角信号的输出。

下图显示了手拿着姿态传感器水平晃动，保持传感器倾角不变，加速度传感器输出电压的波动情况。

^{▲ 加速度计输出受到运动加速度的干扰}

为了减少运动所引起加速度输出信号的波动，可以通过低通滤波器对它进行平滑。

下图是一个RC一阶低通滤波器，可以对信号的波动进行平滑。根据图中R10,C10R_{10} ,C_{10}R10,C10的参数，滤波器的时间常数大约是τ=R10⋅C10=7.5s\tau = R_{10} \cdot C_{10} = 7.5sτ=R10⋅C10=7.5s。

^{▲ 使用RC低通滤波器平滑加速度传感器输出信号}

下图给出了加速度信号（青色）经过上面低通滤波器之后的输出信号（黄色），前面由于晃动引起的输出信号波动，经过滤波之后基本上都消失了。

但是，这种滤波也带来了另外一个问题，就是输出信号不能够及时反映角度的变化了。

^{▲ 加速度计经过RC滤波后的波形}

通过对测量数据中角度变化曲线（黄色曲线）进行建模分析，可以得到实际信号延迟时间常数。

对黄色曲线进行指数拟合，可以得到以下表达式：
y(t)=−1.263⋅e−t9.008+2.354y\left( t \right) = - 1.263 \cdot e^{ - {t \over {9.008}}} + 2.354y(t)=−1.263⋅e−9.008t+2.354

对应的时间常数τRC=9.008s\tau _{RC} = 9.008sτRC=9.008s
比RC低通滤波器的时间常数（7.5秒）大了近1.5秒。这说明加速度传感器输出内阻大约为15kΩ左右。

^{▲ 对滤波后的信号进行拟合结果}

02初步解决方案

^{▲ 陀螺仪（黄色）加速度计（青色）滤波后的输出波形}

^{▲ 陀螺仪和加速度输出信号融合}
^{▲ Gyro比例过大的情况}

^{▲ Gyro比例过小}

^{▲ Gyro比例基本合适}

^{▲ 互补滤波后的结果}

卡尔曼滤波，互补滤波，“Tsinghua”滤波相关推荐

卡尔曼滤波04_2 Sage-Husa自适应滤波
Sage-Husa自适应滤波自适应滤波的状态空间模型为: 自适应滤波适用的系统为噪声和噪声的方差未知,且不是零均值.由于噪声未知,噪声参数的不准确可能会影响系统输出,此时自适应滤波采用一边进行参数估 ...
图像降噪算法——中值滤波/均值滤波/高斯滤波/双边滤波
图像降噪算法--中值滤波/均值滤波/高斯滤波/双边滤波图像降噪算法--中值滤波/均值滤波/高斯滤波/双边滤波 1. 基本原理 2. C++代码实现 3. 结论图像降噪算法--中值滤波/均值滤波/高 ...
envi中的sg滤波_ENVI滤波
ENVI 支持一种滤波类型: Convolution . Morphological . Texture . Adaptive 和 FFT 滤波, 它们都可以经过 ENVI 主菜单的 Filters ...
基于TIA博途的一阶滞后滤波+中位值滤波+堆栈平均值滤波算法SCL语言程序（封装FB库）
基于TIA博途的一阶滞后滤波+中位值滤波+堆栈平均值滤波算法SCL语言程序(封装FB库) 如下图所示,新建一个新项目,添加一个1215CPU,在程序块中添加一个FB,命名为:一阶滞后滤波法, 在该FB ...
转载之-中值滤波均值滤波
转自:openCV之中值滤波&均值滤波(及代码实现):https://blog.csdn.net/weixin_37720172/article/details/72627543 在开始我们 ...
opencv-12-高斯滤波-双边滤波(附C++代码实现)
开始之前这几天由于自己的原因没有写, 一个是因为自己懒了, 一个是感觉这里遇到点问题不想往下写了, 我们先努力结束这个章节吧, 之前介绍了比较常用而且比较好理解的均值和中值滤波, 但是呢,在例程Sm ...
利用openCV实现中值滤波自定义滤波窗口尺寸的功能（类似halcon中的median_rect函数功能）
利用openCV实现中值滤波自定义滤波窗口尺寸的功能(类似halcon中的median_rect函数功能) 在halcon图像库中有一个常用的中值滤波算子median_rect: median_rec ...
C++手敲灰度图均值滤波中值滤波高斯滤波
一.均值滤波(Meaning Filtering)概念均值滤波是典型的线性滤波算法,它是指在图像上对目标像素给一个模板,该模板包括了其周围的临近像素(以目标像素为中心的周围8个像素,构成一个滤波模板 ...
opencv 滤波方框滤波均值滤波高斯滤波中值滤波双边滤波
文章目录一. 线性滤波 1.1. 方框滤波 demo 1.2. 均值滤波 demo 1.3. 高斯滤波 demo 二. 非线性滤波 2.1. 中值滤波 demo 2.2. 双边滤波 demo 结构体 ...
Opencv学习笔记各向异性滤波/均值滤波/高斯滤波/中值滤波/加权中值滤波/高斯低通滤波
各向异性滤波各向异性扩散滤波主要是用来平滑图像的,克服了高斯模糊的缺陷,各向异性扩散在平滑图像时是保留图像边缘的,和双边滤波很像. public static void ImageA ...

卡尔曼滤波，互补滤波，“Tsinghua”滤波

01问题来源

1.陀螺仪的特性

2. 加速度计特性

02初步解决方案

卡尔曼滤波，互补滤波，“Tsinghua”滤波相关推荐

最新文章

热门文章