|
|
回復 17# 幸福羔羊
不必謙虛,我覺得你蠻會的,
要對感測器先做一些處理的話,可以採用信號放大器,
至於 陀螺儀及加速器的互補濾波器,
[ 互補濾波器就是根據傳感器特性不同，通過不同的濾波器（高通或低通，互補的），然後再相加得到整個頻帶的信號。
例如，加速度計測傾角，其動態響應較慢，在高頻時信號不可用，所以可通過低通抑制高頻；
陀螺響應快，積分後可測傾角，不過由於零漂等，在低頻段信號不好。通過高通濾波可抑制低頻噪聲。
將兩者結合，就將陀螺和加表的優點融合起來，得到在高頻和低頻都較好的信號。
互補濾波需要選擇切換的頻率點，即高通和低通的頻率。]
如下圖:
互補濾波器就是一串數學方程式:
We design a 2nd order complementary filter.
 = \\frac{s^2}{(s+0.2)^2}) - The low pass filter is the complementary:
 = 1 - Hp_2(s) = \\frac{0.4s + 0.02}{(s+0.2)^2}) - As for the 1st order complementary filter, the high pass filter can combine with the integration of the rate gyro. We do not take into account the dynamic of the rate gyro:
^2} = \\frac{s}{(s+0.2)^2})
This 2nd order complementary filter compensates for the rate gyro bias with no steady state error.
-------------------------
把數學方程式轉換成自己用程式寫,
成為code, 一連串的函數計算,如下例:
#define LPWM 9 // left motor PWM
#define RPWM 10 // right motor PWM
#define LDIR 11 // left motor direction
#define RDIR 12 // right motor direction
#define A_PIN 0 // accelerometer analog input
#define G_PIN 4 // gyro analog input
#define S_PIN 6 // steering analog input
#define A_ZERO 341 // approx. 1.5[V] * 1024[LSB/V]
#define G_ZERO 253 // approx. 1.23[V] * 1024[LSB/V]
#define S_ZERO 766 // approx. 2.5[V] * 1024[LSB/V]
#define A_GAIN 0.932 // [deg/LSB]
#define G_GAIN 1.466 // [deg/s/LSB]
#define S_GAIN 0.25 // [LSB/LSB] (AAAHHHHHHH WHAT?)
#define DT 0.02 // [s/loop] loop period
#define A 0.962 // complementary filter constant
#define KP 0.5 // proportional controller gain [LSB/deg/loop]
#define KD 0.5 // derivative controller gain [LSB/deg/loop]
float angle = 0.0; // [deg]
float rate = 0.0; // [deg/s]
float output = 0.0; // [LSB] (100% voltage to motor is 255LSB)
void setup()
{
// Make sure all motor controller pins start low.
digitalWrite(LPWM, LOW);
digitalWrite(RPWM, LOW);
digitalWrite(LDIR, LOW);
digitalWrite(RDIR, LOW);
// Set all motor control pins to outputs.
pinMode(LPWM, OUTPUT);
pinMode(RPWM, OUTPUT);
pinMode(LDIR, OUTPUT);
pinMode(RDIR, OUTPUT);
pinMode(13, OUTPUT);
// switch to 15.625kHz PWM
TCCR1B &= ~0x07;
TCCR1B |= 0x01; // no prescale
TCCR1A &= ~0x03; // 9-bit PWM
TCCR1A |= 0x02;
Serial.begin(9600);
}
void loop()
{
signed int accel_raw = 0;
signed int gyro_raw = 0;
signed int output_left = 0;
signed int output_right = 0;
signed int steer_raw = 0;
// Loop speed test.
digitalWrite(13, HIGH);
// Read in the raw accelerometer, gyro, and steering singals.
// Offset for zero angle/rate.
accel_raw = (signed int) analogRead(A_PIN) - A_ZERO;
gyro_raw = G_ZERO - (signed int) analogRead(G_PIN);
steer_raw = (signed int) analogRead(S_PIN) - S_ZERO;
// Scale the gyro to [deg/s].
rate = (float) gyro_raw * G_GAIN;
// Complementarty filter.
angle = A * (angle + rate * DT) + (1 - A) * (float) accel_raw * A_GAIN;
// PD controller.
output += angle * KP + rate * KD;
// Clip as float (to prevent wind-up).
if(output < -255.0) { output = -255.0; }
if(output > 255.0) { output = 255.0; }
// Add/subtract steering and integerize.
output_left = (signed int) (output + (float) steer_raw * S_GAIN );
output_right = (signed int) (output - (float) steer_raw * S_GAIN);
// Clip as integer.
if(output_left < -255) { output_left = -255; }
if(output_left > 255) { output_left = 255; }
if(output_right < -255) { output_right = -255; }
if(output_right > 255) { output_right = 255; }
// Choose directions and set PWM outputs.
if(output_left >= 0)
{
digitalWrite(LDIR, HIGH);
analogWrite(LPWM, output_left);
}
else
{
digitalWrite(LDIR, LOW);
analogWrite(LPWM, -output_left);
}
if(output_right >= 0)
{
digitalWrite(RDIR, HIGH);
analogWrite(RPWM, output_right);
}
else
{
digitalWrite(RDIR, LOW);
analogWrite(RPWM, -output_right);
}
// Loop speed test.
digitalWrite(13, LOW);
// Debug.
Serial.println(output_right);
// Delay for consistent loop rate.
delay(20);
你目前只要知道互補濾波器在上述code裡面,就行了. |
|
發表於 2011-10-8 21:05:08
