倒立振子ロボットの姿勢制御
「倒立振子」ロボットを作るにあたってのポイントは、姿勢制御にあると思っています。
そしてそもそも、それがやりたくてこの形のロボットを選んだのです。
こちとら、クローラ（キャタピラ）式で、しかも段差乗り越え用にクローラベルトとアームを独立駆動できるアームクローラまで備えたロボットを持っているんですよ。
単なるホームロボットを作りたいのなら、それにGoogleHomeとスマートリモコンを載せれば良いけど、それをしなかったのは、動かすだけで面白いものが作りたかったからです。
というわけで、前回の基本設計でも「倒立振子型台車」という要求機能を実現するための機構要素として出てきた、姿勢制御の部分を考えていきます。
機構ではないような気もしますが。。。
姿勢制御の方法
息巻いたものの、実際にやることというのはそれほど多くありません。
姿勢制御はIMU（Inertial Measurement Unit : 慣性計測装置、センサに印加される加速度や回転等の運動情報を計測する装置）を使うのが定番で、私自身も使用したことがありますので、そちらを使っていきます。
姿勢制御のための装置
arduinoなどのマイコンと、mpu-9250というIMUを使ったのが私のIMU経験の全てですが、arduinoとIMUを配線するのが結構面倒だった記憶があります。
マイコンとIMUの通信は理論上は4本の線があればできますが、実際はIMU側で、モード選択のためにこのピンは5Vに繋ぐ、このピンはGNDに繋ぐ。。。と煩雑になるのです。
今回は、arduino UNO互換のマイコンで予めIMUが搭載されているLambda Plusという商品を使うことにしました。
![[商品価格に関しましては、リンクが作成された時点と現時点で情報が変更されている場合がございます。]](https://hbb.afl.rakuten.co.jp/hgb/18fcc2f7.c5ae071b.18fcc2f8.ad85d157/?me_id=1255046&item_id=10012978&m=https%3A%2F%2Fthumbnail.image.rakuten.co.jp%2F%400_mall%2Ftsukumorobo%2Fcabinet%2Fimgrc0110406037.jpg%3F_ex%3D80x80&pc=https%3A%2F%2Fthumbnail.image.rakuten.co.jp%2F%400_mall%2Ftsukumorobo%2Fcabinet%2Fimgrc0110406037.jpg%3F_ex%3D240x240&s=240x240&t=picttext "[商品価格に関しましては、リンクが作成された時点と現時点で情報が変更されている場合がございます。]")
姿勢制御のためのアルゴリズム
先述のとおり、IMUは加速度（加速度計）、回転速度（ジャイロ）、地磁気（磁気コンパス）が搭載されています。全部とは限りませんが。
ジャイロだけあれば、その値を積分することで理論上は回転角度（倒立振子の傾き）が算出できますが、長期的にはドリフトによる誤差が大きい。
一方で加速度を見ることで、突き飛ばされてその方向に強く加速している時はともかく、長期的には安定して重力加速度方向（=鉛直下向き方向）を検出することができます。
短期的観測に強いジャイロと、長期的観測に強い加速度計、それぞれに基づく姿勢計測結果の重み付け平均（のようなもの）をとることで、ちょうどよく姿勢計測が可能になります。
相補フィルタと呼ぶらしいです。
参考：ジャイロのドリフト補正と比較（カルマン、相補フィルター） – 自作のいろいろ
ジャイロの積分は、ソースコード上は区分求積的に（というのかな？）求めます。
その時点のジャイロの値と、制御周期を掛け算することで、その制御周期の間に回転した角度を求めています。
詳しいコードはこちら。
github.com
コードのメイン部分だけ下に載せておきます。先述の通り、Arduino UNO向けのコードとして動きます。
一部の関数をArduinoのタブ機能を使って別ファイルに実装しており、ここには現れていませんが、同じファイルとして機能しています。
制御周期が一定になるように、タイマー割込を使って周期を１００Hzにしています。
コメントにも書きましたが、タイマー割込で割り込んだ関数の中でセンサデータ処理をすると上手く行かなくなるため、（Arduino-PCの通信すら上手く行かず、スケッチの書き込みすらできなくなる）
タイマー割込でフラグを立て、フラグが立った時のみ姿勢計算を行うことで、100Hzでの計算を可能にしています。
また、ジャイロにデフォルトで結構ノイズというかオフセットが載っているようなので、
処理開始直後の２００サンプルの平均値を常に計測値から引くことで、全く動いていない時に確実に角速度ゼロが出力されるようにしました。
加速度にはこのオフセットは載っていなかったようです。
取得したデータは、ROSの別のノードから送信トリガが送られるごとにPCにシリアル通信で送られるようにしています。
（ROSじゃなくても、Arduinoに接続されたPCからトリガがあれば良いので、ArduinoIDEのシリアルプロッタから何か送信すればOK）
それだとデバッグが面倒なので、ROSノード使用のフラグを０にすれば、シリアルモニタで垂れ流しのデータが見られます。
// code for Lambda Plus : compatible to Arduino UNO #include <Wire.h> #include <SPI.h> #include <SparkFunLSM9DS1.h> #include <TimerOne.h> LSM9DS1 imu; #define LSM9DS1_M 0x1C // Would be 0x1C if SDO_M is LOW #define LSM9DS1_AG 0x6A // Would be 0x6A if SDO_AG is LOW // after start processing, sample sensor data for "NUM_OF_SAMPLES_FOR_INIT" times to cancel native noise of the sensor #define NUM_OF_SAMPLES_FOR_INIT 200 // parameters to contain native noise of the sensor to be canceled float offset_gx = 0; float offset_gy = 0; float offset_gz = 0; // define sampling rate (Hz) #define SAMPLING_RATE 100 // flag parameter to activate timer interruption volatile int interrupt_flag = 1; int ros_flag = 1; //flag==1 then use for ROS, flag==0 then use for debug int noise_filtering_flag = 0; //ジャイロのスパイクノイズをフィルタする場合は１にする。 // parameter to contain time to measure spent time of a control loop int previous_time = 0; int present_time = 0; int passed_time = 0; // parameter to contain sensor value float accelX, accelY, accelZ, gyroX, gyroY, gyroZ, magX, magY, magZ, roll, pitch, heading, ACCroll, ACCpitch; float last_gyroX = 0, last_gyroY = 0, last_gyroZ = 0, filtered_gyroX = 0, filtered_gyroY = 0, filtered_gyroZ = 0; void setup() { // start serial communication Serial.begin(115200); // start sensor LSM9DS1 processing (refer sample code of the library) imu.settings.device.commInterface = IMU_MODE_I2C; imu.settings.device.mAddress = LSM9DS1_M; imu.settings.device.agAddress = LSM9DS1_AG; // set up sensor LSM9DS1(refer sample code of the library) setupGyro(); setupAccel(); setupMag(); // error message when beggining was failed if (!imu.begin()) { Serial.println("Failed to communicate with LSM9DS1."); Serial.println("Double-check wiring."); Serial.println("Default settings in this sketch will " \ "work for an out of the box LSM9DS1 " \ "Breakout, but may need to be modified " \ "if the board jumpers are."); while (1) ; } // initial process to subtract gyro offset from measured data init_gyro_process(); // flush serial input buffer while (Serial.available() > 0) { Serial.read(); } Timer1.initialize(1000000 / SAMPLING_RATE); //interrupt per 10000 micro seconds(10 msec) Timer1.attachInterrupt(interrupt_function); } void loop() { // execute reading sensor process only if the interrupt flag is on if (interrupt_flag == 1) { // get sensor data get_IMU_data(); // get posture data from complementary filter get_posture_complementary_filter(); // turn off the interruption flag interrupt_flag = 0; // print_time(); } // (for debugging) if it is not connected to ROS node if (flag == 0) { // Serial.print(roll); // Serial.print(','); // Serial.print(pitch); // Serial.print(','); // Serial.println(heading); // Serial.print(accelX); // Serial.print(','); // Serial.print(accelY); // Serial.print(','); // Serial.println(accelZ); // Serial.print(gyroX); // Serial.print(','); // Serial.print(gyroY); // Serial.print(','); // Serial.println(gyroZ); // // Serial.print(filtered_gyroX); // Serial.print(','); // Serial.print(filtered_gyroY); // Serial.print(','); // Serial.println(filtered_gyroZ); // print_time(); } // if it is connected to ROS node to send IMU data to inverted pendulum if (ros_flag == 1) { // receive message trigger from the connected ROS node if (Serial.available() > 0) { while (Serial.available() > 0) { // flush message trigger Serial.read(); } // send IMU data Serial.println(roll); Serial.println(pitch); Serial.println(heading); Serial.println(accelX); Serial.println(accelY); Serial.println(accelZ); Serial.println(gyroX); Serial.println(gyroY); Serial.println(gyroZ); // print_time(); Serial.flush(); } } } // function when timer interrupt has occured // just turn on the interruption flag, so that the IMU data read process will be executed // it could be implemented as read & process IMU data in this function, but it wouldn't work. // timer for interruption and I2C communication may be in conflict...? void interrupt_function() { interrupt_flag = 1; } // print the passed time per loop if you need void print_time() { present_time = micros(); passed_time = present_time - previous_time; Serial.println(passed_time); previous_time = present_time; }
ここからがArduinoIDEのタブ機能で実装した別の関数（の一部）です。
相補フィルタで加速度由来姿勢角とジャイロ由来姿勢角の重みをつける値であるｋについて、
以下のWEBサイトを参考にして、加速度が強くかかっている時（＝加速度由来姿勢角の信頼性が下がる時）には、
ｋが小さくなるように調整しています。（ジャイロを重視し、加速度の影響度を低く見積もる）
参考：ジャイロのドリフト補正を改良 – 自作のいろいろ
デフォルトは k = 0.1（加速度の影響度：ジャイロの影響度を１：９とする）です。
float k, g; float a = 0.7; #define C 1 // function to get sensor data (according to the sample program of the library) void get_IMU_data() { imu.readGyro(); delayMicroseconds(100); imu.readAccel(); delayMicroseconds(100); gyroX = imu.calcGyro(imu.gx - offset_gx); gyroY = imu.calcGyro(imu.gy - offset_gy); gyroZ = imu.calcGyro(imu.gz - offset_gz); accelX = imu.calcAccel(imu.ax); accelY = imu.calcAccel(imu.ay); accelZ = imu.calcAccel(imu.az); magX = imu.calcMag(imu.mx); magY = imu.calcMag(imu.my); magZ = imu.calcMag(imu.mz); //ジャイロの値にスパイクノイズが入るため、フィルタする。 if (noise_filtering_flag == 1) { filtered_gyroX = a * last_gyroX + (1 - a) * gyroX; filtered_gyroY = a * last_gyroY + (1 - a) * gyroY; filtered_gyroZ = a * last_gyroZ + (1 - a) * gyroZ; } last_gyroX = filtered_gyroX; last_gyroY = filtered_gyroY; last_gyroZ = filtered_gyroZ; } // function to calculate posture angle by complementary filter void get_posture_complementary_filter() { // calculate k : coefficient that indicates impact of acceleration // default k = 0.1 (when no acceleration is loaded except gravity) g = sqrt(pow(accelX, 2) + pow(accelY, 2) + pow(accelZ, 2)); k = 0.1 * pow(65536, -1 * (pow((1 - g), 2) / C)); // calculate posture angle by accelerometer ACCroll = 0.9 * ACCroll + 0.1 * (atan2(accelY, accelZ) * 180 / M_PI); ACCpitch = 0.9 * ACCpitch + 0.1 * (atan2(accelX, sqrt(pow(accelY, 2) + pow(accelZ, 2))) * 180 / M_PI); //calculate pitch/roll/yaw by gyrosensor (About pitch & roll, using Complementary Filter) //(とりあえずピッチのみ）角速度を相補フィルタリング後の値から再計算する。 roll = (1 - k) * (roll + gyroX / SAMPLING_RATE) + k * ACCroll; pitch = (1 - k) * (pitch + (-1 * gyroY) / SAMPLING_RATE) + k * ACCpitch; //ピッチの角速度データが正負逆で入っているようだ。 // calculate yaw angle (ignore when the value is small enough : smaller than 0.05deg/sec if (abs(gyroZ) < 0.05) { heading = heading; } else heading = heading + gyroZ / SAMPLING_RATE; // shift heading range from -180deg to 180deg if (heading > 180) { heading = -360 + heading; } else if (heading < -180) { heading = 360 + heading; } }
こんな感じで、ROSノードとの接続フラグを０にセットし、Lambda Plusにアップロードしてシリアルプロッタを見れば、姿勢角がグラフ表示されます。
（上に書いたコードはROSノードとの接続フラグが１になっており、シリアルプロッタへの出力部分もコメントアウトされているので注意）
まとめ
肝心なところの説明が雑だった気がしなくもないですが、とにかくロボットの姿勢角をセンサから取得できるようになりました。
次回はこのセンサを取り付ける車体の設計を考えていこうと思います。
ktd-prototype.hatenablog.com
