《The Balance Filter》互補濾波器--MIT著名牛文翻譯（上）

      鄙人在寫另一篇博文時頻頻借鑑到這篇牛文（實際上是一個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控制器的設計就像給平衡車加入了一個可調彈簧和阻尼器。