《The Balance Filter》互补滤波器

      鄙人在写另一篇博文时频频借鉴到这篇牛文（实际上是一个PPT），为能让更多人方便查阅，共同进步、探讨，遂翻译全文。鄙人才疏学浅，愿附上原文对照，以期指正。首发于CSDN：http://blog.csdn.net/qq_32666555。转载请注明作者及出处，谢谢！

The Balance Filter——A Simple Solution for Integrating Accelerometer and Gyroscope Measurements for a Balancing Platform

互补滤波器—— 一种用于以加速度计和陀螺仪坐整合测量的平衡平台的简单解决方案

作者：Shane Colton

译者：Wyman

Sensors

传感器

2-Axis Accelerometer:  

·Measures“acceleration,” but really force per unit mass. (F = ma, so a= F/m)                      

· Can be used to measure the force of gravity. Above, X-axis reads 0g, Y-axis reads-1g.   

· Can be used to measure tilt:                                                                                                         

 两轴加速度计：

· 测量“加速度”，但实际是每单位质量所受之力。(F = ma, 即 a= F/m)

· 被用于测量重力，上图中，X轴读作 0g， Y轴读作 -1g。                   

· 被用于测量倾角。                                                                                      

           

X now sees some gravity.   

X reads slightly positive.                Xreads slightly negative

Ysees slightly less gravity.   

Is Y useful information? Probably not:                                    

a) It is far less sensitive to small changes in angle than X.

b) It does not depend on direction of tilt.                                 

X轴现在有了重力分量。                          

 左图X轴分量为略小的正数，右图相反。

Y轴重力分量轻微减少。                          

Y轴是有用信息否？应该不是：              

a）对角度变化的灵敏度远不及X轴。    

b）不随倾角方向变化正负。                  

Gyroscope:   

·Measures angular rate (speed of rotation).   

·Reads “zero” when stationary.                         

·Reads positive or negative when rotating :   

陀螺仪：

· 测量角速度（旋转速度）。         

· 静止时为零。                                 

·  顺时针旋转为正，逆时针为反。

Reading Values from the Sensors

从传感器读值

The first step is to read in analog inputs  (through the analog-to-digital converter, ADC) for each sensor and get them into useful units. This requires adjustment foroffset andscale:

 第一步是读取每个传感器的模拟输入 （通过模数转换器，ADC）然后转换为有效单位。这需要补偿和比例的调节。

· The offset is easy to find: see what integer value the sensor reads when it is horizontal and/or stationary. If it flickers around, choose an average value. The offsetshould be a signedint-typevariable (or constant).

找补偿值so easy：读取平台水平且/或静止时的整数值。如果值上下跳变，则取平均值。补偿值应为整型变量（或常量）。

Tips：虽然ADC结果值及补偿值不可能为负，但会相减，所以作整形变量无伤大雅。

· The scale depends on the sensor. It is the factor by which to multiply to get to the desired units.This can be found in the sensor datasheet or by experiment. It is sometimes called the sensor constant, gain, or sensitivity. The scale should be afloat-type variable (or constant).

比例取决于传感器。这是为了达到期望单位的乘数。这可以通过传感器手册或实验得到。有时被称为传感器常数、增益或灵敏度。比例应为浮点型变量（或常量）。

Tips：单位可为度或弧度（陀螺仪每秒），一致就好。

A bit more about the accelerometer…

还与加速度计有关的那一丢丢...

If it was necessary to have an estimate of angle for 360º of rotation, having theY-axis measurement would be useful, but not necessary. With it, we could use trigonometry to find the inverse tangent of the two axis readings and calculatethe angle. Without it, we can still use sine or cosine and the X-axis alone to figure out angle, since we know the magnitude of gravity. But trig kills processor time and is non-linear, so if it can be avoided, it should.

若需对360º旋转进行角度测量估计，Y轴的测量是有用而不必要的。有它，我们可以使用三角函数来求出两轴的反正切值并计算出角度；没有它，我们仍可使用sin或者cos和X轴算出夹角，只要知道重力的量。但是三角函数的计算占用过多线程时间，且为非线性的，所以能免即免。

For abalancing platform, the most important angles to measure are near vertical. If the platform tilts more than 30º in either direction, there’s probably not much the controller can do other than drive full speed to try to catch it. Within this window, we can use small angle approximation andthe X-axis to save processor time and coding complexity:

对于平衡平台来说，最重要的测量角度是接近垂直时的。如果平台向任何方向倾斜超过30度，处理器在全速掰直它之外可能干不了什么。所以在这个范围内，我们可以采用小角度近似和计算X轴方向来减少线程时间和编程复杂度。

Platform is tilted forward by and angle θ, but stationary (not accelerating horizontally).

X-axis reads: (1g) × sin(θ) 

small angle approximation:sin(θ) ≈ θ, inradians

This works well (within 5%) up to θ = ±π/6 =±30º.

So in the following bit of code,

x_acc =(float)(x_acc_ADC – x_acc_offset) * x_acc_scale;

x_acc will be the angle in radians if x_acc_scale is set to scale the output to 1[g] when the X-axis is pointed straight downward.

To get the angle in degrees, x_acc_scale should be multiplied by 180/π.

平台向前倾斜角度为θ，但静止（没有水平加速度）

X轴方向： (1g) × sin(θ) 

小角度近似：sin(θ) ≈ θ，弧度值。

在θ = ±π/6 =±30º范围内效果良好（误差在5%内）。

遂有以下代码：

x_acc =(float)(x_acc_ADC – x_acc_offset) * x_acc_scale;

若x_acc_scale所设比例是使输出缩放到1g数量级，同时x轴方向为垂直向下，x_acc将是弧度制单位的角度值。

若需取角度制单位的角度，x_acc_scale 应乘180/π。

Desired Measurements

理想测量

In order to control the platform, it would be nice to know both the angle and the angular velocity of the base platform. This could be the basis for an angle PD(proportional/derivative) control algorithm, which has been proven to work well for this type of system. Something like this:

为了控制平台，知道其角度和角速度是极好的。这将是角度环PD（角度/微分）控制算法的基本参数。此算法被证实能有效应用于这类系统。举个栗子:-）

Motor Output = Kp ×(Angle) + Kd × (AngularVelocity)

What exactly MotorOutput does is another story. But the general idea is that this control setup can be tuned with Kp and Kd to give stability and smooth performance. It is less likely to overshoot the horizontal point than aproportional-only controller. (If angle is positive but angular velocity is negative, i.e. it is heading back toward being horizontal, the motors are slowed in advance.)

电机确切输出如何是另一回事了。但大致思路是控制程序可以通过调节 Kp 和 Kd使性能表现稳定、平滑，与纯比例控制器相比超调量更少。（若角度为正，而角速度为负，换言之，在返回水平位置的过程中，电机会提前减速。）

In effect, the PD control scheme is like adding an adjustable spring and damper to the Segway.

实际上，PD控制器的设计就像给平衡车加入了一个可调弹簧和阻尼器。