1軸 姿勢制御モジュール がやっと立った ーリアクションホイールへの道7ー
お父ちゃん
2020.8.9
1
0
さて、前回の1軸 姿勢制御モジュール SHISEIGYO-1 (シセーギョーワン) 倒立に向けた下準備から随分日が経ちました。
実はずーーーーっと倒立させるべく試行錯誤しておりました。
この度無事になんとか倒立させることができましたのでご報告いたします。
目次
倒立した！
まずは SHISEIGYO-1 が倒立した勇ましい姿をご覧ください。
やっとたった。。。#リアクションホイールへの道
物理無視で以下パラメータ総当りで
5日以上かかった。。
・傾き係数
・傾き角速度係数
・モータ角速度係数
・傾きオフセット補正
・モータ初速
・ブレーキ範囲もうちょい調整したら
物理的意味のあるパラメータに絞る！
一生立たないかとおもた pic.twitter.com/Qq8Xr3l1Va— HomeMadeGarbage (@H0meMadeGarbage) August 9, 2020
マジで何度も心が折れそうになりましたが遂に倒立を確認することができました。
ちなみにモータ制御なしで倒立することはありません。
以下で今回実施した制御方法を記載いたします。
制御方法
姿勢制御モジュールは作用反作用の法則を用いて倒立を維持しています。
倒立地点 (傾き0°) から傾いた方向にフライホイールを回して反作用で起き上がります。
傾きを検出し、その状況に応じたモータ回転に制御して倒立を維持します。
2020年6月号のトラ技の3軸姿勢制御モジュールの記事にモータのトルク$T_m$を以下の式に基づいて制御していると記載がありました。
$$T_m = -K_{d1}・θ_b-K_{d2}・\dotθ_b-K_{d3}・\dotθ_w (1)$$
$$θ_bはモジュールの傾き、\dotθ_wはホイールの角速度、K_{dx}はそれぞれの係数$$
こちらを参考にSHISEIGYO-1も制御することにいたしました。
モータ電流特性
SHISEIGYO-1はリアクションホイールとしてフライホイール付きのブラシレスモータ ID-549XWを使用しています。
モータのトルクはモータに流れる電流に比例します。
SHISEIGYO-1のモータはPWM入力によって回転速度を制御しますので、PWMのOFFデューティーとモータに流れる電流の関係を測定しました。
モータに入力するPWM信号のOFFデューティーを上げていくとブラシレスモータ ID-549XWの回転速度も上昇し負荷電流も増えます。
上の測定結果のグラフからデューティーとモータ負荷電流Iはほぼリニアに比例していることがわかりました。
したがって上式(1)より
$$T_m ∝ I ∝Duty ∝-K_{d1}・θ_b-K_{d2}・\dotθ_b-K_{d3}・\dotθ_w$$
となり、モジュールの姿勢角($θ_b$) とその角速度($\dotθ_b$) とモータの回転速度($\dotθ_w$) からモータに入力するべき信号のデューティー比が導き出せると言えそうです。
また、SHISEIGYO-1がこの理論式だけで立つと感覚的に確証が持てなかったので更に以下のパラメータも追加して、手動で各パラメータを調整して倒立を目指しました。
- 傾きオフセット補正
SHISEIGYO-1の物理的偏りやセンサのオフセットを数値で補正します。 - モータ初速
モータの電気的抵抗や物理的摩擦による抵抗を打ち消すためのオフセット値 - ブレーキ範囲
倒立する姿勢角でモータがきっちり止まるためにブレーキをかけるデューティー値の範囲を指定します。
構成
前回と変更はありません。
PWM入力線とエンコーダ出力、ブレーキ端子をATOM Matrixに接続し制御します。
部品
- M5Stack ATOM Matrix
- フライホイール付きブラシレスモータ ID-549XW
Arduinoコード
|
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 |
#include "M5Atom.h" #include <Kalman.h> #define ENC_A 22 #define ENC_B 19 #define brake 23 #define rote_pin 32 #define PWM_pin 26 #define button 39 unsigned long oldTime = 0, loopTime, nowTime; float dt; volatile byte pos; volatile int enc_count = 0; int DutyIni = 490, pwmDuty; float Kp = 4.2; float Kd = -0.19; float Kw = 0.1; float D0 = 0.31; int brakeRange = 0; float accX = 0, accY = 0, accZ = 0; float gyroX = 0, gyroY = 0, gyroZ = 0; float temp = 0; float theta_acc = 0.0; float theta_dot = 0.0; //オフセット float accXoffset = 0, accYoffset = 0, accZoffset = 0; float gyroXoffset = 0, gyroYoffset = 0, gyroZoffset = 0; Kalman kalmanY; float kalAngleY, kalAngleDotY; //センサオフセット算出 void offset_cal(){ delay(1000); accXoffset = 0; accYoffset = 0; accZoffset = 0; gyroXoffset = 0; gyroYoffset = 0; gyroZoffset = 0; for(int i=0; i<10; i++) { M5.IMU.getAccelData(&accX,&accY,&accZ); M5.IMU.getGyroData(&gyroX,&gyroY,&gyroZ); delay(10); accXoffset += accX; accYoffset += accY; accZoffset += accZ; gyroXoffset += gyroX; gyroYoffset += gyroY; gyroZoffset += gyroZ; } if(accXoffset < 0){ accXoffset = accXoffset / 10 + 1.0 / sqrt(2.0); }else{ accXoffset = accXoffset / 10 - 1.0 / sqrt(2.0); } accYoffset /= 10; accZoffset = accZoffset / 10 + 1.0 / sqrt(2.0); gyroXoffset /= 10; gyroYoffset /= 10; gyroZoffset /= 10; } //加速度センサから傾きデータ取得 [deg] float get_theta_acc() { M5.IMU.getAccelData(&accX,&accY,&accZ); //傾斜角導出 単位はdeg theta_acc = atan(-1.0 * (accX - accXoffset) / (accZ - accZoffset)) * 57.29578f; return theta_acc; } //Y軸 角速度取得 float get_gyro_data() { M5.IMU.getGyroData(&gyroX,&gyroY,&gyroZ); theta_dot = gyroY - gyroYoffset; return theta_dot; } void setup() { M5.begin(true, false, false); //SerialEnable ,I2CEnable , LEDEnable pinMode(ENC_A, INPUT); pinMode(ENC_B, INPUT); pinMode(brake, OUTPUT); pinMode(button, INPUT); digitalWrite(brake, HIGH); attachInterrupt(ENC_A, ENC_READ, CHANGE); attachInterrupt(ENC_B, ENC_READ, CHANGE); M5.IMU.Init(); //センサオフセット算出 offset_cal(); //フルスケールレンジ M5.IMU.SetGyroFsr(M5.IMU.GFS_500DPS); M5.IMU.SetAccelFsr(M5.IMU.AFS_4G); kalmanY.setAngle(get_theta_acc()); ledcSetup(0, 20000, 9); ledcAttachPin(PWM_pin, 0); pinMode(rote_pin, OUTPUT); } void loop() { //オフセット再計算 if (digitalRead(button) == 0){ Serial.println("オフセット再計算"); offset_cal(); } nowTime = micros(); loopTime = nowTime - oldTime; oldTime = nowTime; dt = (float)loopTime / 1000000.0; //sec //モータの角速度算出 float theta_dotWheel = -1.0 * float(enc_count) * 3.6 / dt; enc_count = 0; //カルマンフィルタ 姿勢 傾き kalAngleY = kalmanY.getAngle(get_theta_acc(), get_gyro_data(), dt); //カルマンフィルタ 姿勢 角速度 kalAngleDotY = kalmanY.getRate(); /* Serial.print("kalAngleY: "); Serial.print(kalAngleY); Serial.print(", kalAngleDotY: "); Serial.print(kalAngleDotY); Serial.print(", theta_dotWheel: "); Serial.print(theta_dotWheel); */ //ブレーキ if(abs(kalAngleY) > 20.0){ digitalWrite(brake, LOW); }else{ digitalWrite(brake, HIGH); } //シリアル入力 char key; if ( Serial.available() ) { key = Serial.read(); switch (key) { case 'q': Kp += 0.1; break; case 'a': Kp -= 0.1; break; case 'w': Kd += 0.01; break; case 's': Kd -= 0.01; break; case 'e': Kw += 0.01; break; case 'd': Kw -= 0.01; break; case 'r': D0 += 0.01; break; case 'f': D0 -= 0.01; break; case 't': DutyIni++; break; case 'g': DutyIni--; break; case 'y': brakeRange++; break; case 'h': brakeRange--; break; } } Serial.print("Kp: "); Serial.print(Kp); Serial.print(", Kd: "); Serial.print(Kd,3); Serial.print(", Kw: "); Serial.print(Kw, 3); Serial.print(", D0: "); Serial.print(D0, 3); Serial.print(", DutyIni: "); Serial.print(DutyIni); Serial.print(", brakeRange: "); Serial.print(brakeRange); Serial.print(", kalAngleY: "); Serial.print(kalAngleY); //モータ回転 float M = Kp * (kalAngleY + D0) / 90.0 + Kd * kalAngleDotY / 500.0 + Kw * theta_dotWheel / 10000.0; M = max(-1.0f, min(1.0f, M)); pwmDuty = DutyIni * (1.0 - fabs(M)); /* Serial.print(", M: "); Serial.print(M); Serial.print(", pwmDuty: "); Serial.print(pwmDuty); */ //回転方向 if(pwmDuty > (DutyIni - brakeRange)){ digitalWrite(brake, LOW); ledcWrite(0, 511); }else if(M > 0.0f){ digitalWrite(rote_pin, LOW); ledcWrite(0, pwmDuty); }else{ digitalWrite(rote_pin, HIGH); ledcWrite(0, pwmDuty); } /* if(M > 0){ digitalWrite(rote_pin, LOW); ledcWrite(0, pwmDuty); }else{ digitalWrite(rote_pin, HIGH); ledcWrite(0, pwmDuty); } */ /* Serial.print("theta: "); Serial.print(kalAngleY); Serial.print(", PWM: "); Serial.print(pwmDuty); Serial.print(", loopTime: "); Serial.print((float)loopTime / 1000.0); Serial.print("msec, theta_dotWheel: "); Serial.println(theta_dotWheel); */ delay(20); Serial.println(""); //digitalWrite(brake, LOW); } void ENC_READ() { byte cur = (!digitalRead(ENC_B) << 1) + !digitalRead(ENC_A); byte old = pos & B00000011; byte dir = (pos & B00110000) >> 4; if (cur == 3) cur = 2; else if (cur == 2) cur = 3; if (cur != old) { if (dir == 0) { if (cur == 1 || cur == 3) dir = cur; } else { if (cur == 0) { if (dir == 1 && old == 3) enc_count--; else if (dir == 3 && old == 1) enc_count++; dir = 0; } } bool rote = 0; if (cur == 3 && old == 0) rote = 0; else if (cur == 0 && old == 3) rote = 1; else if (cur > old) rote = 1; pos = (dir << 4) + (old << 2) + cur; } } |
- L. 135～139
M5 ATOM Matrix内蔵の6軸慣性センサ MPU6886による加速度センサとジャイロからカルマン・フィルタライブラリを用いてモジュールの姿勢角 (kalAngleY )とその角速度 (kalAngleDotY )を算出します。 - L. 132
モータの回転速度 (theta_dotWheel ) はエンコーダのステップ数を割り込みでカウントして算出します。 - L. 218～221
モータに入力するPWM信号のデューティを算出します。
PWMの分解能は9ビット(0～511)としました(L. 111)。モジュールの姿勢角と角速度、モータの回転速度に関する係数 (Kp, Kd, Kw) に加えて
傾きオフセット補正値 (D0) とモータ初速 (DutyIni) も絡めて算出しています。 - L. 230～240
算出されたデューティ値の正負からモータ回転方向を判定しています。
またブレーキ範囲 (brakeRange) を設けてバランスが取れた姿勢角付近でモータが止まるようにします。 - L. 157～201
M5 ATOM MatrixにUSBケーブルをつなぎながらシリアル入力で各パラメータを手動で調整しました。
動作
以下のパラメータ値で取りあえず倒立が実現できました。
-
姿勢角係数 Kp = 4.2
-
姿勢角速度係数 Kd = –0.19
-
モータ回転速度係数 Kw = 0.1
-
姿勢角オフセット補正 D0 = 0.31
-
ブレーキ範囲 brakeRange = 0
- モータ初速デューティ DutyIni = 490
上記パラメータの妥当性や物理的理解はまだできておらず、実際外乱には弱い状況です。
しかし、パラメータの大体のオーダーを把握することができ、なにより実際に倒立することが確認できたことは非常に有意義であったと言えます。
おわりに
やっと立ちました！
もうこの道を驀進するしかないですね。
次回はM5 ATOM MatrixにUSBケーブルを除去して無線でパラメータ調整できるようにする予定です。
またパラメータを物理的意味のあるものに絞って外乱に強い倒立を目指したいです！！
次の記事
関連記事
1軸 姿勢制御モジュール の倒立に向けた準備 ーリアクションホイールへの道6ー
1軸 姿勢制御モジュール の改良 ーリアクションホイールへの道8ー
1軸 姿勢制御モジュール 起き上がり実験 ーリアクションホイールへの道9ー
1軸 姿勢制御モジュール 起き上がりからの倒立 ーリアクションホイールへの道10ー
1軸 姿勢制御モジュール 倒立精度の向上 ーリアクションホイールへの道11ー
SHISEIGYO-1 作例と代替部品の紹介
3軸 姿勢制御モジュール SHISEIGYO-3 への挑戦そして挫折 ーリアクションホイールへの道16ー
3軸 姿勢制御モジュール SHISEIGYO-3 安定点倒立メソッドの確立 ーリアクションホイールへの道23ー
素晴らしい記事ありがとうございます。
自分も倒立振子に興味がありまして参考にさせていただいております。
制作レシピも購入させていただきましたが、L219の部分についてご指導いただきたいです。
各パラメータを定数で割っておられるのはなぜでしょうか?(本体角度/90の90の部分etc)
宜しくお願い致します。
レシピのご購入をいただき誠にありがとうございます。
各項を定数で割っている理由は、それぞれのパラメータ値の大きさに極端な差が出ないように
するためです。
パラメータ値をだいだい０～10におさめるためにそれぞれ定数で割っていますが
物理的に意味は全くございません。
ご返信ありがとうございます。
パラメータ値の大きさを合わせるための定数であること理解しました。
この部分は定数として扱うように致します。
ご教授ありがとうございます。