จากครั้งที่แล้วหามุมการเอียงของหุ่นย์โดยใช้วิธี complementary filter พบว่าเมื่อหุ่นยนต์ run ไปนานๆแล้วมุมที่อ่านได้จาก sensor มีความผิดพลาดเกิดขึ้นเนื่องจากสัญญาณที่ได้เกิดการ drift เกิดขึ้นที่ gyro sensor ดังนั้นผมเลยเปลี่ยนวิธีในการหามุมเอียงใหม่โดยใช้วิธี sensor fusion kalman filter โดยมีขั้นตอนดังนี้ เริ่มจากการเขียน state-space ของ Predict model เสียก่อน เรานิยามว่ามุมที่ได้จาก gyro ก็คือผลของการ Integrate สัญญาณจาก gyro นั้นเองและเรายังสามารถนิยาม bias ของสัญญาณขึ้นมาอีกตัวหนึ่งเพื่อที่จะทำการ tracking ค่า bias ให้มีค่าคงที่
หาสมการ Kalman Predict
Kalman predict equation
x_dot=Ax+Bu
P=APA'+Q;
//***********************************************************************************//
theta_dot=(gyro-bias) ...........................................................................(1)
bias_dot=0
จากสมการ (1) เรานิยาม state variable ดังนี้ x=[theta;bias]
จากนั้นให้เราหา jacobian matrix ก็คือการทำ partial derivative สมการ (1) เทียบกับ [theta;bias] จะได้ Matrix A,B ดังนี้
A=[0 -1;0 0], B=[1;0]
เมื่อนำมาเขียนให้อยู่ในรูปของ state-space เราจะได้ดังนี้ จาก
x_dot=A*x+B*u
x_dot=[0 -1;0 0]*x+[1;0]*gyro ...............................(2)
จะเห็นได้ว่า (2) ยังเป็นสมการที่อยู่ในรูปของระบบแบบ continuous อยู่แต่ในทางปฎิบัติจริงส่วนมากแล้วเราจะใช้พวก MCU ดังนั้น เราจึง Transform ไปสู่ Discrete model เราจะได้
x_dot=(x(n+1)-x(n))/dt=[0 -1;0 0]*x+[1;0]*gyro;
x1_(n+1)=x1(n)+dt*(gyro-x2(n)) .............................................(3)
x2(n+1)=x2(n)
Ad=[1 -dt;0 1];
Bd=[dt;0];
gyro ก็คือค่า ที่อ่านจาก sensor
x1 คือ theta
x2 คือ bias
dt คือ sampling time
จากนั้นจะคำนวนหา Covariance matrix P จากสมการ
P(n+1)=Ad*P(n)*Ad'+Q ..........................................(4)
โดยที่
Q=[Q_accelerometer 0;0 Q_gyro];
เมื่อคูณ Matrix แล้วได้
P(n+1)=[P00 - dt P10 - (P01 - dt P11) dt + Q_accelerometer , P01 - dt P11;
P10 -dt P11, P11 + Q_gyro];
P00(n+1)=P00 - dt P10 - (P01 - dt P11) dt + Q_accelerometer ; .......................(5)
P01(n+1)=P01 - dt P11;
P10(n+1)=P10 -dt P11;
P11(n+1)= P11 + Q_gyro;
จากนั้นพิจรณา Measurement Update
Measurement Update equation:
K=PH'inv(HPH'+R).........................................(6)
P=(I-KH)P......................................................(7)
x=x+K(y-Hx)...................................................(8)
H คือ measure output matrix
y คือ measure sensor
R measurement noise matrix
H=[1 0]; เนื่องจากเราวัดมุมได้อย่างเดียวนั้นคือ accelerometer
P=[P00 P01;P10 P11]
คูณ (6) ได้
K1=P00/(P00+R);
K2=P10/(P00+R);
คูณ (8) ได้
x1=x1+K1*(y-x1);
x2=x2+K2*(y-x2);
คูณ (7) ได้
P00=(1-K1)*P00;
P01=(1-K1)*P01;
P10=-K2*P00+P10;
P11=-K2*P01+P11;
เป็นอันเรียบร้อยในการทำ Kalman filter sensor fusion
จากนั้นเราก็จะนำสมการด้านบนทั้งหมดมาเขียนโปรมแกรมและโหลดลงใน Micro-conroller (CCS Compiler)
*********************************** CODE ****************************************
#include <18f458.h>
#device adc=10
#fuses H4, NOWDT, NOPROTECT, BROWNOUT, PUT, NOLVP
#use delay(clock=40000000)
#define TxD PIN_C6
#define RxD PIN_C7
#use rs232(baud=115200, xmit=TxD,rcv=RxD)
#include
#include
#include
#include
#use rtos(timer=0,minor_cycle=10ms)
float x[2][1]={{0.47},{0.0}};
float u[1][1];
float P[2][2]={{.01*.01*0.00000135 0 },{0}};
float K[2][1];
float x1=0.4656;
float Q_roll=0.00000135,Q_roll_bias=.183;
const float R=0.028;
float dt=0.010;
float fc=1.765;
float roll();
float qm();
float gyro;
float qm(){
set_ADC_channel(0);
delay_cycles(60);
gyro=(float)read_adc()*1/1023;
return(gyro);
}
float ax;
float roll(){
set_ADC_channel(2);
delay_cycles(60);
ax=atan((float)read_adc()*1/1023);
return(ax);
}
// ****************************** LPF ****************************************//
void LPF(){
x1+=dt*(-2*pi*fc*x1+2*pi*fc*ax);
}
float fhc=0.01;
float gyro1_old=0.34,tou,a,y=0;
void HPF(){
tou=1/(2*pi*fhc);
a=tou/(tou+dt);
y=a*y+a*(gyro-gyro1_old);
gyro1_old=gyro;
}
// ************************* KALMAN PREDICT **********************************//
void Kalman_Predict(float qm){
HPF();
u[0][0]=y;
x[0][0]=x[0][0]+dt*u[0][0]-dt*x[1][0];
x[1][0]=x[1][0];
/* P=A*P*P'+Q */
P[0][0]=P[0][0]-dt*P[1][0]-(P[0][1]-dt*P[1][1])*dt+Q_roll;
P[0][1]=P[0][1]-dt*P[1][1];
P[1][0]=P[1][0]-dt*P[1][1];
P[1][1]=P[1][1]+Q_roll_bias;
}
//************************ KALMAN MEASURE ************************************//
void Kalman_Measurement(float roll){
LPF();
K[0][0]=P[0][0]/(P[0][0]+R);
K[1][0]=P[1][0]/(P[0][0]+R);
x[0][0]+=K[0][0]*(x1-x[0][0]);
x[1][0]+=K[1][0]*(x1-x[0][0]);
P[0][0]=(1-K[0][0])*P[0][0];
P[0][1]=(1-K[0][0])*P[0][1];
P[1][0]=-K[1][0]*P[0][0]+P[1][0];
P[1][1]=-K[1][0]*P[0][1]+P[1][1];
}
//****************************************************************************//
#task(rate=10ms,max=10ms)
void MAIN_LOOP(){
Kalman_Predict(qm());
Kalman_measurement(roll());
}
#task(rate=10ms,max=10ms)
void print(){
printf("%.4f %.4f %.4f %.4f\r\n",ax,x[0][0],K[0][0],K[1][0]);
}
void main(){
setup_port_a(0x07);
setup_adc(adc_clock_internal);
enable_interrupts(GLOBAL);
while(TRUE)
{
rtos_run();
}
}
สนใจติดต่อ : sarayuthassa@gmail.com
083-4391356
