カルマン・フィルタで M5stickC 傾斜計 ー倒立振子への道 1ー
お父ちゃん
2019.6.21
0
0
今月のトランジスタ技術が非常に興味深い内容でした。
確率統計コンピューティング特集ということでカルマン・フィルタの歴史とカルマン倒立振子の作り方を非常に丁寧に紹介しています。
必要な数学や力学の基礎も記載されておりロボット制御の入門として最適なのではと思います。DVDでのセミナーも易しくありがたい限りです。
はっきり言ってお父ちゃん
はカルマン・フィルタとは何ぞやというレベルで全く知識のない状況です。そこで倒立振子への道と題してM5StickCを用いた倒立振子の実現を目指して一歩ずつお勉強していこうと思います。
ここではトラ技のコードを参考にカルマン・フィルタを用いた傾斜計を作ってみたいと思います。カルマン・フィルタの素晴らしさをまずは体験してみようと思います。
目次
M5stickC
M5stickCには6軸の慣性センサ(加速度、ジャイロ) SH200Q が内蔵されていますので、このセンサ値を用いてM5stickC自身の傾きを算出してみます。
6軸センサ SH200Q
SH200Q データシート
https://github.com/m5stack/M5-Schematic/blob/master/Core/SH200Q.pdf
SH200QのArduino IDEサンプルコードは以下にございます。
https://github.com/m5stack/M5StickC/blob/master/examples/Basics/SH200I/SH200I.ino
M5StickCのXYZ軸方向は以下の通りです。
センサのデフォルトの設定は以下の通りです。
- 加速度：16bit ±8 g
- ジャイロ(角速度)：16bit ±2000 deg/sec
上の写真のようにM5stickCを置いてSH200Q のサンプルコードを動かすとSH200Qから出力されるデータは以下のようになるはずです。
- 加速度：AccX = 0, AccY = 0, AccZ = 4096（+1g）
- 角速度 : GyroX = 0, GyroY = 0, GyroZ = 0
しかしセンサのオフセットのため実際に出力される値にはずれがありましたので、今回はあらかじめオフセットを測って引いて使用します。
カルマン・フィルタを用いた傾斜計に関してはセンサのデフォルトオフセットの影響はないかもしれませんが今回は生データとの比較も行いたいのでオフセット引いております。
カルマン・フィルタによる傾斜導出の概要
ここではカルマン・フィルタの詳細理解はすっ飛ばして とにかく使ってみてご利益を実感するのを目的としています。ですので詳細はトラ技や他に譲るとしてざっくりとした概要です。
カルマン・フィルタは測定値と数理モデルによる予測値をうまいこと利用してもっともらしい傾斜角を導出します。
加速度センサによる角度
重力加速度のY軸、Z軸の成分よりM5stickCの傾斜角θは以下の式で求められます。
$$θ = tan^{-1}\left( \dfrac{g_z}{-g_y} \right)$$
しかし加速度センサは振動などの重力加速度以外の加速度も検出してしまう問題があります。
ジャイロセンサによる角度
ジャイロセンサのx軸の角速度$ω_x$を時間積分することでM5stickCの角度を求めることができます。
しかしジャイロは初期姿勢の角度は検出できません。さらにセンサ測定値にはオフセット誤差が含まれるので誤差も積分されて増幅していきます。
モデルによる予測
上記のそれぞれ短所をもった測定値とそのバラつき誤差を加味した数理モデルをつくって傾斜角度の予測値を算出します。
測定バラつきとして予めセンサを複数回測定(ここでは100回)して分散を求めます。
モデルの詳細はまだ理解できてないので割愛します。いつか理解できるその日まで。。。
カルマン・フィルタのアルゴリズム
上記の測定値とその分散そして数理モデルによる予測値をうまいこと用いて角度の推定値を得ます。
内容は全く分からないので後に回します。トラ技紙面に譲ります。。今は分からない。。。今は。。。
傾斜計コード
Arduino IDEをもちいでカルマン・フィルタM5stickC 傾斜計を作製しました。比較のために加速度センサによる傾斜角とカルマン・フィルタによる推定値を出力します。
トラ技のソースコードInclinometer.cppをほぼ丸々参考にしています(p. 49 – 52)。
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383 384 385 386 387 388 389 390 391 392 393 394 |
#include <M5StickC.h> #include <Ticker.h> int16_t accX = 0; int16_t accY = 0; int16_t accZ = 0; int16_t gyroX = 0; int16_t gyroY = 0; int16_t gyroZ = 0; //加速度センサ オフセット int16_t accXoffset = 1020; int16_t accYoffset = 450; int16_t accZoffset = 958; //ジャイロセンサ オフセット int16_t gyroXoffset = -60; int16_t gyroYoffset = -15; int16_t gyroZoffset = 370; Ticker tickerUpdate; //センサ バラつき取得用変数 int sample_num = 100; int meas_interval = 10; float theta_deg; float theta_mean; float theta_variance; float theta_dot_mean; float theta_dot_variance; //カルマン・フィルタ用変数 float theta_update_freq = 400; //Hz float theta_update_interval = 1.0/double(theta_update_freq); //State vector //[[theta(degree)], [offset of theta_dot(degree/sec)]] float theta_data_predict[2][1]; float theta_data[2][1]; //Covariance matrix float P_theta_predict[2][2]; float P_theta[2][2]; //"A" of the state equation float A_theta[2][2] = {{1, -theta_update_interval}, {0, 1}}; //"B" of the state equation float B_theta[2][1] = {{theta_update_interval}, {0}}; //"C" of the state equation float C_theta[1][2] = {{1, 0}}; //========================================================= // 行列演算関数 //========================================================= //行列の和 void mat_add(float *m1, float *m2, float *sol, int row, int column) { for(int i=0; i<row; i++) { for(int j=0; j<column; j++) { sol[i*column + j] = m1[i*column + j] + m2[i*column + j]; } } return; } //行列の差 void mat_sub(float *m1, float *m2, float *sol, int row, int column) { for(int i=0; i<row; i++) { for(int j=0; j<column; j++) { sol[i*column + j] = m1[i*column + j] - m2[i*column + j]; } } return; } //行列の積 void mat_mul(float *m1, float *m2, float *sol, int row1, int column1, int row2, int column2) { for(int i=0; i<row1; i++) { for(int j=0; j<column2; j++) { sol[i*column2 + j] = 0; for(int k=0; k<column1; k++) { sol[i*column2 + j] += m1[i*column1 + k]*m2[k*column2 + j]; } } } return; } //転置行列算出 void mat_tran(float *m1, float *sol, int row_original, int column_original) { for(int i=0; i<row_original; i++) { for(int j=0; j<column_original; j++) { sol[j*row_original + i] = m1[i*column_original + j]; } } return; } //行列の定数倍算出 void mat_mul_const(float *m1,float c, float *sol, int row, int column) { for(int i=0; i<row; i++) { for(int j=0; j<column; j++) { sol[i*column + j] = c * m1[i*column + j]; } } return; } //逆行列算出 void mat_inv(float *m, float *sol, int column, int row) { //allocate memory for a temporary matrix float* temp = (float *)malloc( column*2*row*sizeof(float) ); //make the augmented matrix for(int i=0; i<column; i++) { //copy original matrix for(int j=0; j<row; j++) { temp[i*(2*row) + j] = m[i*row + j]; } //make identity matrix for(int j=row; j<row*2; j++) { if(j-row == i) { temp[i*(2*row) + j] = 1; } else { temp[i*(2*row) + j] = 0; } } } //Sweep (down) for(int i=0; i<column; i++) { //pivot selection float pivot = temp[i*(2*row) + i]; int pivot_index = i; float pivot_temp; for(int j=i; j<column;j++) { if( temp[j*(2*row)+i] > pivot ) { pivot = temp[j*(2*row) + i]; pivot_index = j; } } if(pivot_index != i) { for(int j=0; j<2*row; j++) { pivot_temp = temp[ pivot_index * (2*row) + j ]; temp[pivot_index * (2*row) + j] = temp[i*(2*row) + j]; temp[i*(2*row) + j] = pivot_temp; } } //division for(int j=0; j<2*row; j++) { temp[i*(2*row) + j] /= pivot; } //sweep for(int j=i+1; j<column; j++) { float temp2 = temp[j*(2*row) + i]; //sweep each row for(int k=0; k<row*2; k++) { temp[j*(2*row) + k] -= temp2 * temp[ i*(2*row) + k ]; } } } //Sweep (up) for(int i=0; i<column-1; i++) { for(int j=i+1; j<column; j++) { float pivot = temp[ (column-1-j)*(2*row) + (row-1-i)]; for(int k=0; k<2*row; k++) { temp[(column-1-j)*(2*row)+k] -= pivot * temp[(column-1-i)*(2*row)+k]; } } } //copy result for(int i=0; i<column; i++) { for(int j=0; j<row; j++) { sol[i*row + j] = temp[i*(2*row) + (j+row)]; } } free(temp); return; } //加速度センサから傾きデータ取得 [deg] float get_acc_data() { M5.IMU.getAccelData(&accX,&accY,&accZ); //得られたセンサ値はオフセット引いて使用 //傾斜角導出 単位はdeg theta_deg = atan( (float)(accZ - accZoffset) / (float)(-1 * accY - accYoffset) ) * 57.29578f; return theta_deg; } //加速度センサによる傾きのばらつき測定 void acc_init(){ float theta_array[sample_num]; for(int i=0; i<sample_num; i++) { theta_array[i] = get_acc_data(); delay(meas_interval); } //平均値 theta_mean = 0; for(int i=0; i<sample_num; i++) { theta_mean += theta_array[i]; } theta_mean /= sample_num; //分散 float temp; theta_variance = 0; for(int i=0; i<sample_num; i++) { temp = theta_array[i] - theta_mean; theta_variance += temp*temp; } theta_variance /= sample_num; } //x軸 角速度取得 float get_gyro_data() { M5.IMU.getGyroData(&gyroX,&gyroY,&gyroZ); //得られたセンサ値はオフセット引いて使用 float x_data = ((float) (gyroX - gyroXoffset)) * M5.IMU.gRes; return x_data; } //ジャイロセンサばらつき測定 void gyro_init() { float theta_dot_array[sample_num]; for(int i=0;i<sample_num;i++) { theta_dot_array[i] = get_gyro_data(); delay(meas_interval); } //平均値 theta_dot_mean = 0; for(int i=0;i<sample_num;i++) { theta_dot_mean += theta_dot_array[i]; } theta_dot_mean /= sample_num; //分散 float temp; theta_dot_variance = 0; for(int i=0; i<sample_num; i++) { temp = theta_dot_array[i] - theta_dot_mean; theta_dot_variance += temp*temp; } theta_dot_variance /= sample_num; } //========================================================= //カルマン・フィルタアルゴリズム処理 //========================================================= void update_theta() { //measurement data float y = get_acc_data(); //degree //input data float theta_dot_gyro = get_gyro_data(); //degree/sec //calculate Kalman gain: G = P'C^T(W+CP'C^T)^-1 float P_CT[2][1] = {}; float tran_C_theta[2][1] = {}; mat_tran(C_theta[0], tran_C_theta[0], 1, 2);//C^T mat_mul(P_theta_predict[0], tran_C_theta[0], P_CT[0], 2, 2, 2, 1);//P'C^T float G_temp1[1][1] = {}; mat_mul(C_theta[0], P_CT[0], G_temp1[0], 1,2, 2,1);//CP'C^T float G_temp2 = 1.0f / (G_temp1[0][0] + theta_variance);//(W+CP'C^T)^-1 float G1[2][1] = {}; mat_mul_const(P_CT[0], G_temp2, G1[0], 2, 1);//P'C^T(W+CP'C^T)^-1 //theta_data estimation: theta = theta'+G(y-Ctheta') float C_theta_theta[1][1] = {}; mat_mul(C_theta[0], theta_data_predict[0], C_theta_theta[0], 1, 2, 2, 1);//Ctheta' float delta_y = y - C_theta_theta[0][0];//y-Ctheta' float delta_theta[2][1] = {}; mat_mul_const(G1[0], delta_y, delta_theta[0], 2, 1); mat_add(theta_data_predict[0], delta_theta[0], theta_data[0], 2, 1); //calculate covariance matrix: P=(I-GC)P' float GC[2][2] = {}; float I2[2][2] = {{1,0},{0,1}}; mat_mul(G1[0], C_theta[0], GC[0], 2, 1, 1, 2);//GC float I2_GC[2][2] = {}; mat_sub(I2[0], GC[0], I2_GC[0], 2, 2);//I-GC mat_mul(I2_GC[0], P_theta_predict[0], P_theta[0], 2, 2, 2, 2);//(I-GC)P' //predict the next step data: theta'=Atheta+Bu float A_theta_theta[2][1] = {}; float B_theta_dot[2][1] = {}; mat_mul(A_theta[0], theta_data[0], A_theta_theta[0], 2, 2, 2, 1);//Atheta mat_mul_const(B_theta[0], theta_dot_gyro, B_theta_dot[0], 2, 1);//Bu mat_add(A_theta_theta[0], B_theta_dot[0], theta_data_predict[0], 2, 1);//Atheta+Bu //predict covariance matrix: P'=APA^T + BUB^T float AP[2][2] = {}; float APAT[2][2] = {}; float tran_A_theta[2][2] = {}; mat_tran(A_theta[0], tran_A_theta[0], 2, 2);//A^T mat_mul(A_theta[0], P_theta[0], AP[0], 2, 2, 2, 2);//AP mat_mul(AP[0], tran_A_theta[0], APAT[0], 2, 2, 2, 2);//APA^T float BBT[2][2]; float tran_B_theta[1][2] = {}; mat_tran(B_theta[0], tran_B_theta[0], 2, 1);//B^T mat_mul(B_theta[0], tran_B_theta[0], BBT[0], 2, 1, 1, 2);//BB^T float BUBT[2][2] = {}; mat_mul_const(BBT[0], theta_dot_variance, BUBT[0], 2, 2);//BUB^T mat_add(APAT[0], BUBT[0], P_theta_predict[0], 2, 2);//APA^T+BUB^T } void setup() { // put your setup code here, to run once: Serial.begin(115200); M5.begin(); M5.Axp.ScreenBreath(8); M5.IMU.Init(); M5.Lcd.setRotation(1); M5.Lcd.fillScreen(BLACK); //初期設定開始 LED ON pinMode(10, OUTPUT); digitalWrite(10, LOW); //センサばらつき取得 100回測って分散導出 acc_init(); gyro_init(); //カルマンフィルタの初期設定 初期姿勢は0°(直立)を想定 theta_data_predict[0][0] = 0; theta_data_predict[1][0] = theta_dot_mean; P_theta_predict[0][0] = 1; P_theta_predict[0][1] = 0; P_theta_predict[1][0] = 0; P_theta_predict[1][1] = theta_dot_variance; //Timer //2.5msec(400 Hz)ごとにカルマン・フィルタで角度導出 tickerUpdate.attach(theta_update_interval, update_theta); //初期設定終了 LED OFF digitalWrite(10, HIGH); } void loop() { //加速度センサによる傾斜角 Serial.print(theta_deg); Serial.print(", "); //カルマン・フィルタによる角度の推定値 Serial.println(theta_data[0][0]); delay(50); } |
やってることの概要は以下の通りです。
- センサばらつき算出：加速度とジャイロセンサを100回測って予め分散を導出
- カルマン・フィルタの初期設定：使用する行列パラメータの初期値設定
- タイマ割り込み(2.5msecごと)でカルマン・フィルタアルゴリズム発動させて傾斜角の推定値を算出
- 50msecごとに加速度センサによる傾斜角とカルマン・フィルタによる角度の推定値をシリアル出力
カルマン・フィルタアルゴリズムについての詳細はしつこいですがトラ技紙面か以下の動画を参照ください！
動作
青線：M5stickCの加速度センサから算出した傾斜角で
赤線：カルマン・フィルタによる角度の推定値
トラ技7月号のコードを参考にカルマン・フィルタの傾斜計を試してみた。
青がM5stickCの加速度センサから算出した角度で
赤が加速度センサとジャイロセンサから推察されたカルマン・フィルタ越しの角度。
ノイズに強いどころか外乱にも負けずに傾き算出してる。#M5stickC #トラ技 #倒立振子への道 pic.twitter.com/rlyN7eGuQr— HomeMadeGarbage (@H0meMadeGarbage) 2019年6月20日
最初の感想は”凄い！”でした。振ると当然 加速度の外乱が入るので、加速度センサによる角度（青線）は乱れますがカルマン・フィルタによる角度の推定値（赤線）に大きな乱れはありません。
しかも加算平均とかではなくあくまでリアルタイムのセンサ値と前回算出した予測値で算出されているようなのです。
なんかすごいね。。理解できないけど。
カルマン・フィルタのアルゴリズムは全く理解出来てないが ご利益は体感できた。ゆっくり勉強しよう。
最終目標はもちろん。。。#解析力学とか何年ぶりだよ #トラ技 pic.twitter.com/Y9ZLQvsNt7— HomeMadeGarbage (@H0meMadeGarbage) 2019年6月20日
今回スタートした 倒立振子への道 。どうなるものか。たどりつけるのか。
全くの未知ですが 今回カルマン・フィルタのご利益を全身で堪能出来て興奮気味なので頑張りたいです！！
初心者なのでソースを見てもなかなか理解できませんがいつも興味深く拝見しています。
ところで、公開されている上記をコンパイルすると以下のエラーが出ます。
no matching function for call to ‘IMU::getAccelData(int16_t*, int16_t*, int16_t*)’
int16_tとfloatの違いもわかっていませんが、int16_tだとgetAccelADCになると思いますが。
ご教示頂けると助かります。
コメントありがとうございます。
私のM5stickCは6軸センサがSH200Qの初期のもので記事のコードも古いライブラリで動作させております。
お使いのM5stickCの6軸センサの型式をご確認いただいて、適宜floatに変更かgetAccelADCを使用かご検討ください。
宜しくお願い致します。
返信ありがとうございます。
搭載デバイスがいくつかあってそれぞれ対応するコードが違うんですね。中身わからないままですが色々試してみます。
M5StickCの裏のシールに各部品の型番が記載されていますのでご確認よろしくお願い致します。
進捗楽しみにしております。