二輪倒立振子ロボットについて、
以前、概要のみ紹介しました。
倒立振子ロボットについてもアクセス数が多いようなので、
今回は倒立振子ロボットで使用しているカルマンフィルタについてご紹介します。
ジャイロセンサにノイズがなく、確実な角速度のみ得られる場合には
角速度情報を積分して角度を得ます。
しかしながら実際にはピュアな角速度を得ることは難しく、
ノイズ等で積分の誤差が蓄積し、
積分した結果の角度が実際の角度と違ってくるという問題が生じます。
倒立振子ロボットが時間経過につれてバランスを崩す1つの原因にもなります。
このため、角速度を積分せずに角度を角速度センサだけで得ると、
倒立振子ロボット自体の並進運動の加速度も混じってしまい、
これもまたピュアな角度が得られない問題が生じます。
角速度センサと加速度センサのそれぞれの特徴を上手く合わせて
補正してほしい値を得る方法があります。
フィルタを通してほしい値を得るということです。
・角速度センサはハイパスフィルタ(HPF)、加速度センサはローパスフィルタ(LPF)でフィルタして値を得る方法
・カルマンフィルタを用いて値を得る方法
NASAの人工衛星の姿勢制御にも使用している？との
カルマンフィルタで角度を得る方法を説明します。
カルマンフィルタの細かい説明は割愛して、
実装のみ説明します。
2輪倒立振子ロボットで使用しているセンサは500円程度なので
安く性能も良いのでおススメです。
角速度を検出するジャイロセンサに加えて加速度センサもセットで
入っている優れモノです。
加速度/ジャイロセンサMPU6050
以下プログラムです。
float Q_angle = 0.001;
float Q_gyro = 0.003;
float R_angle = 0.03;
float x_angle;
float x_bias = 0;
float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0;
float y, S;
float K_0, K_1;
float dt=0.005;
float kal_deg;
float Q_gyro = 0.003;
float R_angle = 0.03;
float x_angle;
float x_bias = 0;
float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0;
float y, S;
float K_0, K_1;
float dt=0.005;
float kal_deg;
float kalmanCalculate(float newAngle, float newRate){
x_angle += dt * (newRate - x_bias);
P_00 += dt * (dt * P_11 - P_01 - P_10 + Q_angle);
P_01 -= dt * P_11;
P_10 -= dt * P_11;
P_11 += Q_gyro * dt;
y = newAngle - x_angle;
S = P_00 + R_angle;
K_0 = P_00 / S;
K_1 = P_10 / S;
x_angle += K_0 * y;
x_bias += K_1 * y;
P_00 -= K_0 * P_00;
P_01 -= K_0 * P_01;
P_10 -= K_1 * P_00;
P_11 -= K_1 * P_01;
return x_angle;
}
newAngleに加速度センサで得られた角度を入れます。
newRateには角速度を積分して得られた角度を入れます。
x_angleとしてフィルタリングされた角度が得られます。
加速度センサで得られた角度と角速度を積分して得られた角度で
基準となる原点と正負を一致させる必要があるため、
必要に応じて下記のようなオフセットや正負をそろえる必要があります。
kal_deg=kalmanCalculate(acc_angle+90.0, -gy1_angle);
必要に応じてQ_angle、Q_gyro、R_angleの係数を変更する
必要があるかもしれません。
ただ様々な資料を見ても大体この値なようです。
プログラムに関する責任は全く負いませんが、
簡単にまずは実装してみたいという場合には使ってみてください。
********************広告**********************
15年12月16日から16年2月12日まで
ローコストレーザープロジェクタ、
プロジェクションボールのクラウドファンディング資金調達を開始しました！
ぜひサイトをご覧ください。
*********************************************
