【番外編】SPIKE-RTでもジャイロセンサで正確な90度ターンがしたい!!【SPIKE-RTでロボコンに出よう!!】

本シリーズの目次

SPIKE-RTについて

SPIKE-RTとは、LEGO Education のロボットキット 「SPIKE Prime」用のOSです。

ベースには、国産組み込みOSであるTOPPERS/ASP3が採用されているリアルタイムOSです。
2022年末に最初のリリースが行われており、今年の夏にかけて何度かのアップデートがなされました。

開発者様の記事👇

しかし、まだ開発途中のため、まだロボットプログラミングで不自由な点があります。
その一つが、今回の課題となります。

ハブブロック内蔵のIMU(ジャイロセンサ)について

SPIKE Prime の核となるハブブロック(上の画像の真ん中にあるケーブルが繋がっているブロック)には、IMU(Inertial Measurement Unit) すなわち 慣性計測装置 が内蔵されています。
平たく言えば 「ジャイロセンサ」 が内蔵されているので、このハブブロックだけで角度を測ることが出来ます。
先代の EV3 では、ジャイロセンサは外付けのセンサだったため、ハブブロック内に内蔵されたのは画期的でした。

ところが、SPIKE-RTのAPIを確認してみると…

あれ、角速度しか取得出来ないの…??

LEGO標準のMicroPython環境では普通に角度が取得出来ていたため、角速度しか取れないことに少し驚きました。
開発者に確認したところ、SPIKE-RTへドライバを移植する時に、角速度を取得するものしか無かったため、それしか追加していなかったとのことでした。確かにそれは仕方ありません。

ただ、せっかくジャイロセンサがあるので、ロボットが向いている方向を取得し、正確な90度ターン実現したいところです。
そこで、SPIKE-RTのベースであるTOPPERS/ASP3の強みを生かし、角速度を積分することで角度を得ようという試みです。

軸の向き

ハブブロック内蔵のIMUの軸の向きを示しておきます。

方針

基本部分

まず、先述のとおり、IMUから角速度を取得する関数はあります。

hub_imu_get_angular_velocity(float angv[3]);

引数に「角速度を格納する配列」を指定することで、その時点での角速度が得られます。
これを出来るだけ短い間隔で取得し続け、以下のように「角度の配列」に加算し続ければ、角度が得られるという算段です。

float ang_v[3];
float ang_raw[3];

while(1){
  hub_imu_get_angular_velocity(ang_raw);

  ang_v[0] += ang_raw[0] / (1秒間の取得回数);
  ang_v[1] += ang_raw[1] / (1秒間の取得回数);
  ang_v[2] += ang_raw[2] / (1秒間の取得回数);
}

ただし、得られる角速度の単位は $^{\circ}/\mathrm{s}$ (1秒当たりの角速度)であるため、正確には1秒間の取得回数で割ったものを加算する必要があります。

調べたところによると、ハブブロックのIMUのサンプリングレート(値の更新頻度)は833Hz、すなわち1秒間に833回値を取得しているということでした。

参照したページはこちら👇

ここでは少し多めに見積もって、$1 \mathrm{ms}$ ごとに値を取得する、すなわち1秒間に1000回値を取得して、角度を算出したいと思います。

加速度の連続的な取得

さて、加速度を$1 \mathrm{ms}$ ごとに取得し加算・積分すれば良いという方針は立ちました。
次に考えなければならないのは、どのようにして連続的に値を取得するかです。

大抵の場合、ロボットというのは何かしら作業をしています。
その傍ら、ジャイロセンサの角速度を取得し、角度を算出し続けなければならないわけですが、これをシングルタスクの中でロボットのメインの作業と混在して実行するのは、事実上不可能です。
(ある一定の部分だけ角度を取得したい場合は別です。)

そこで、リアルタイムOS TOPPERS/ASP3の強みである、周期ハンドラの機能を用いたいと思います。

周期ハンドラとは、ある一定周期でタスクを自動的に実行してくれる機能で、これによりマルチタスクを実現することが出来ます。

メインタスクではロボットの基本動作を行い、サブタスクとしてIMUの加速度を取得する関数を定義、これを周期ハンドラを用いて$1 \mathrm{ms}$ ごとに実行したいと思います。

これにより、メインタスクの動作に支障を与えることなく、高精度に角度を取得し続けることが出来ます。

では、これらの方針のもと、実際に角度を取得するプログラムを作成していきます。

角度取得プログラム 初期版

まずは、一番初めに作成したプログラムを示します。

尚、プログラム開発の方法については本稿では記しません。
開発の方法が知りたい方は、私の記事【SPIKE-RTでロボコンに出よう!!】シリーズをご覧ください。

又、値の確認にはUSBシリアル通信を用い、PC側のTeraTermを用いて監視することとします。

gyro_first.c

#include <stdlib.h>
#include <kernel.h>

#include <spike/hub/system.h>

#include <gyro_first.h>

#include "spike/pup/motor.h"
#include "spike/pup/colorsensor.h"
#include "spike/pup/forcesensor.h"
#include "spike/pup/ultrasonicsensor.h"

#include "spike/hub/battery.h"
#include "spike/hub/button.h"
#include "spike/hub/display.h"
#include "spike/hub/imu.h"
#include "spike/hub/light.h"
#include "spike/hub/speaker.h"

#include <pbio/color.h>

#include "kernel_cfg.h"
#include "syssvc/serial.h"

// IMU用の配列 0:ロール(x) 1:ピッチ(y) 2:ヨー(z)
float ang_v[3] = {0}; // 角度(角位置)

void Main(intptr_t exinf)
{
  // ここからプログラムを書く
  char msg[128] = "Program Start\n";
  char str[256] = {' '};

  serial_opn_por(SIO_USB_PORTID); // USBシリアル通信の開通
  serial_wri_dat(SIO_USB_PORTID, msg, sizeof(msg)); // "Program Start"を送信

  hub_imu_init(); // IMUの初期化

  // 左ボタンが押されるまで待機 (LED: 緑)
  hub_button_t pressed;
  while(!(pressed&HUB_BUTTON_LEFT)){
    hub_button_is_pressed(&pressed);
    hub_light_on_color(PBIO_COLOR_GREEN);
  }

  sta_cyc(CYC_HDR); // ジャイロセンサ監視開始

  int count = 0; //カウンタ変数宣言・初期化

  while(1){
    sprintf(str,"%d, X:%f, Y:%f, Z:%f\n", count, ang_v[0], ang_v[1], ang_v[2]); // 角度情報を文字列に
    serial_wri_dat(SIO_USB_PORTID, str, sizeof(str)); // シリアル通信で角度を表示

    // 真ん中ボタンが押された時にIMU初期化
    hub_button_is_pressed(&pressed);
    if(pressed&HUB_BUTTON_CENTER){
      hub_imu_init();
      ang_v[0] = 0;
      ang_v[1] = 0;
      ang_v[2] = 0;
      dly_tsk(500*1000);
    }

    dly_tsk(1*1000*1000); // 1s待機
    count++; // カウントアップ
  }

  stp_cyc(CYC_HDR); // ジャイロセンサ監視終了

  exit(0);
}

void gyro_monitor(intptr_t exinf)
{
  // IMU角加速度 格納用配列
  float ang_raw[3];
  hub_imu_get_angular_velocity(ang_raw);

  ang_v[0] += ang_raw[0] * 0.001;
  ang_v[1] += ang_raw[1] * 0.001;
  ang_v[2] += ang_raw[2] * 0.001;
}

gyro_first.h

#ifdef __cplusplus
extern "C" {
#endif

#include <kernel.h>

/*
 *  タスクの優先度の定義
 */
#define MAIN_PRIORITY	5		/* メインタスクの優先度 */

// 以下を追加
#define HIGH_PRIORITY	9		/* 並行実行されるタスクの優先度 */
#define MID_PRIORITY	10
#define LOW_PRIORITY	11

/*
 * Definitions of Port ID for Serial Adapter
 */
#define SIO_USART_F_PORTID    1
#define SIO_USB_PORTID        2
#define SIO_BLUETOOTH_PORTID  3
#define SIO_TEST_PORTID       4

#ifndef STACK_SIZE
#define	STACK_SIZE		4096		/* タスクのスタックサイズ */
#endif /* STACK_SIZE */

/*
 *  関数のプロトタイプ宣言
 */
#ifndef TOPPERS_MACRO_ONLY

extern void Main(intptr_t exinf);
extern void gyro_monitor(intptr_t exinf);

#endif /* TOPPERS_MACRO_ONLY */

gyro_first.cfg

INCLUDE("tecsgen.cfg");

#include "gyro_first.h"
CRE_TSK(MAIN_TASK, { TA_ACT, 0, Main, MAIN_PRIORITY + 1, STACK_SIZE, NULL });

CRE_TSK(CYCHDR_TASK, { TA_NULL, 0, gyro_monitor, MAIN_PRIORITY, STACK_SIZE, NULL });
CRE_CYC(CYC_HDR, { TA_NULL, { TNFY_ACTTSK, CYCHDR_TASK }, 1*1000, 0U });

これを実行し、TeraTermにて角度を監視したところ、以下のようになりました。

あれ、おかしいな…

何がおかしいかというと、この時ハブブロックは机に置いてある状態で、一切動いていませんでした。
ところが、上の画像のように、値がどんどん下がっているのです。
これでは正確に角度を測ることが出来ません。

ただ、同時に次のことにも気づきました。
それは、 「1秒あたりの下がり幅も概ね一定である」 ということです。

X軸(ロール)では約-0.1、Y軸(ピッチ)では約-1.3、Z軸(ヨー)では約-0.05といった具合です。
ここから言えることとして、IMUの特性上、常にこれくらいの値が出てしまう「定常的な誤差」があると考えられます。
逆に言えば、この「定常的な誤差」を推定し、それに対する補正値(オフセット)を定めておけば、誤差を相殺して正確な角度を割り出せるというわけです。

角度取得プログラム オフセット同定版

前章で示した改善点を盛り込んだコードを以下に示します。

gyro_test.c

#include <stdlib.h>
#include <kernel.h>

#include <spike/hub/system.h>

#include <gyro_test.h>

#include "spike/pup/motor.h"
#include "spike/pup/colorsensor.h"
#include "spike/pup/forcesensor.h"
#include "spike/pup/ultrasonicsensor.h"

#include "spike/hub/battery.h"
#include "spike/hub/button.h"
#include "spike/hub/display.h"
#include "spike/hub/imu.h"
#include "spike/hub/light.h"
#include "spike/hub/speaker.h"

#include <pbio/color.h>

#include "kernel_cfg.h"
#include "syssvc/serial.h"

// IMU用の配列 0:ロール(x) 1:ピッチ(y) 2:ヨー(z)
float ang_v[3] = {0}; // 角度(角位置)
float imu_offset[3] = {0}; // 角度オフセット

float ang_debug[3] = {0}; // 角度(角位置) (デバッグ用)

void Main(intptr_t exinf)
{
  // ここからプログラムを書く
  char msg[128] = "Program Start\n";
  char str[256] = {' '};

  serial_opn_por(SIO_USB_PORTID); // USBシリアル通信の開通
  serial_wri_dat(SIO_USB_PORTID, msg, sizeof(msg)); // "Program Start"を送信

  imu_setup(imu_offset); // IMU初期化・オフセット同定
  // オフセット算出結果表示
  sprintf(msg,"off_X: %f off_Y: %f off_Z: %f\n", imu_offset[0], imu_offset[1], imu_offset[2]);
  serial_wri_dat(SIO_USB_PORTID, msg, sizeof(msg));

  // 左ボタンが押されるまで待機 (LED: 緑)
  hub_button_t pressed;
  while(!(pressed&HUB_BUTTON_LEFT)){
    hub_button_is_pressed(&pressed);
    hub_light_on_color(PBIO_COLOR_GREEN);
  }

  sta_cyc(CYC_HDR); // ジャイロセンサ監視開始

  int count = 0; //カウンタ変数宣言・初期化

  while(1){
    // 角度情報を文字列に
    sprintf(str,"%d, OFF_X:%f, OFF_Y:%f, OFF_Z:%f, X:%f, Y:%f, Z:%f\n", count, ang_v[0], ang_v[1], ang_v[2], ang_debug[0], ang_debug[1], ang_debug[2]);
    serial_wri_dat(SIO_USB_PORTID, str, sizeof(str)); // シリアル通信で角度を表示

    // 真ん中ボタンが押された時にIMU初期化
    hub_button_is_pressed(&pressed);
    if(pressed==HUB_BUTTON_CENTER){
      hub_imu_init();
      ang_v[0] = 0;
      ang_v[1] = 0;
      ang_v[2] = 0;
      dly_tsk(500*1000);
    }

    dly_tsk(1*1000*1000); // 1s待機
    count++; // カウントアップ
  }

  stp_cyc(CYC_HDR); // ジャイロセンサ監視終了

  exit(0);
}

void gyro_monitor(intptr_t exinf)
{
  // IMU角加速度 格納用配列
  float ang_raw[3];
  hub_imu_get_angular_velocity(ang_raw);

  // オフセット無し
  ang_debug[0] += ang_raw[0] * 0.001;
  ang_debug[1] += ang_raw[1] * 0.001;
  ang_debug[2] += ang_raw[2] * 0.001;

  // オフセット有り
  ang_v[0] += (ang_raw[0] - imu_offset[0]) * 0.001;
  ang_v[1] += (ang_raw[1] - imu_offset[1]) * 0.001;
  ang_v[2] += (ang_raw[2] - imu_offset[2]) * 0.001;
}

void imu_setup(float offset[3]){
  dly_tsk(3*1000*1000); // 3s待機
  
  hub_light_on_color(PBIO_COLOR_ORANGE); // LED: オレンジ

  hub_imu_init(); // IMUの初期化

  float ang_raw[3]; // IMU角加速度 raw値 格納用配列

  // オフセット同定 (1秒間で1000回測定して平均取る)
  for(int i=0; i<1000; i++){
    hub_imu_get_angular_velocity(ang_raw); //角加速度取得
    offset[0] += ang_raw[0];
    offset[1] += ang_raw[1];
    offset[2] += ang_raw[2];
    dly_tsk(1*1000); // 1ms待機
  }

  // オフセットをサンプル取得回数で割る
  offset[0] /= 1000;
  offset[1] /= 1000;
  offset[2] /= 1000;
}

gyro_test.h

#ifdef __cplusplus
extern "C" {
#endif

#include <kernel.h>

/*
 *  タスクの優先度の定義
 */
#define MAIN_PRIORITY	5		/* メインタスクの優先度 */

// 以下を追加
#define HIGH_PRIORITY	9		/* 並行実行されるタスクの優先度 */
#define MID_PRIORITY	10
#define LOW_PRIORITY	11

/*
 * Definitions of Port ID for Serial Adapter
 */
#define SIO_USART_F_PORTID    1
#define SIO_USB_PORTID        2
#define SIO_BLUETOOTH_PORTID  3
#define SIO_TEST_PORTID       4

#ifndef STACK_SIZE
#define	STACK_SIZE		4096		/* タスクのスタックサイズ */
#endif /* STACK_SIZE */

/*
 *  関数のプロトタイプ宣言
 */
#ifndef TOPPERS_MACRO_ONLY

extern void Main(intptr_t exinf);
extern void gyro_monitor(intptr_t exinf);
void imu_setup();

#endif /* TOPPERS_MACRO_ONLY */

(gyro_test.cfgはgyro_first.cfgと3行目を除き全く同じ。)

新たに関数imu_setup()を定義し、IMUの初期化、ならびにジャイロセンサのオフセット同定を行っています。
方法としては、$1 \mathrm{ms}$ ごとに1000回加速度を計測し、それらの平均を取ることで「定常的な誤差」を推定しています。
当たり前のことですが、$1 \mathrm{ms}$ ごとに1000回の計測なので、ぴったり1秒かかります。
ただこれを正確に実行できるのも、リアルタイムOSだからこそなせる業です。

さて、このコードに変更して、先ほどと同様にTeraTermで角度を関したところ、以下のようになりました。

オフセットで補正した値はOFF_軸: XXXXのように表示しています。
又、補正していない値は各行後ろの方に軸: XXXXのように表示しています。

明らかに、補正している値の方が変動が少ないことが見て取れます。

このプログラムで120秒間計測し、解析してみました。
120秒というのは、WROというロボコンの競技時間が2分であるため、競技に耐えうる時間の基準として採用しています。
TeraTermに出力したテキストをExcelに取り込み、各軸ごとのグラフに起こしてみました。
以下がその結果です。

X軸まわり(ロール)

Y軸まわり(ピッチ)

Z軸まわり(ヨー)

どの軸にしても、補正無しの角度を表すオレンジの線に対し、補正ありの角度を表す青の線の方が明らかに0付近で推移していることが分かります。

又、補正した3軸の角度を並べたグラフも示します。

オフセットあり角度の時間推移

大きく変動しているように見えるかもしれませんが、縦軸は1目盛り0.1°なので、それほど大きな変動ではありません。
ただし、X軸まわり(ロール)とZ軸まわり(ヨー)は誤差の範囲が0.3°以内で済んでいるのに対し、Y軸まわり(ピッチ)では誤差の範囲が0.6°に及んでいます。
120秒動かして0.6°なので十分小さい誤差だとは思いますが、使用する際には注意が必要かもしれません。

いずれにせよ、これでハブブロック内蔵のIMUから角度を取得することが可能になりました。

正確に90度回るロボットを作る

では、タイトルにもある通り、ハブブロック内蔵のIMU(ジャイロセンサ)により、正確に90度回転するロボットを作成したいと思います。

私はハード屋ではありませんので、最低限実験できるロボットを作成しました。👇

ソフトについては、先ほど作成した角度取得コードを流用します。
制御方法について、ヨーの角度の絶対値が90度以内の時はモータを回転、90度以上になったらモータを停止する、といった制御を行います。

gyro_turn.c

#include <stdlib.h>
#include <kernel.h>

#include <spike/hub/system.h>

#include <gyro_turn.h>

#include "spike/pup/motor.h"
#include "spike/pup/colorsensor.h"
#include "spike/pup/forcesensor.h"
#include "spike/pup/ultrasonicsensor.h"

#include "spike/hub/battery.h"
#include "spike/hub/button.h"
#include "spike/hub/display.h"
#include "spike/hub/imu.h"
#include "spike/hub/light.h"
#include "spike/hub/speaker.h"

#include <pbio/color.h>

#include "kernel_cfg.h"
#include "syssvc/serial.h"

#include "math.h"

// IMU用の配列 0:ロール(x) 1:ピッチ(y) 2:ヨー(z)
float ang_v[3] = {0}; // 角度(角位置)
float imu_offset[3] = {0}; // 角度オフセット

// モータ用ポインタ
pup_motor_t *motorA;
pup_motor_t *motorB;

void Main(intptr_t exinf)
{
  // ここからプログラムを書く
  motorA = pup_motor_get_device(PBIO_PORT_ID_A);
  motorB = pup_motor_get_device(PBIO_PORT_ID_B);

  // モータのセットアップ
  pup_motor_setup(motorA, PUP_DIRECTION_CLOCKWISE, true);
  pup_motor_setup(motorB, PUP_DIRECTION_COUNTERCLOCKWISE, true);

  imu_setup(imu_offset); // IMU初期化・オフセット同定

  // 左ボタンが押されるまで待機 (LED: 緑)
  hub_button_t pressed;
  while(!(pressed&HUB_BUTTON_LEFT)){
    hub_button_is_pressed(&pressed);
    hub_light_on_color(PBIO_COLOR_GREEN);
  }

  dly_tsk(500*1000); // 500ms待機

  sta_cyc(CYC_HDR); // ジャイロセンサ監視開始

  // 右90度回転
  while(fabs(ang_v[2])<90){
    pup_motor_set_power(motorA, -30);
    pup_motor_set_power(motorB, 30);
  }

  // モータ停止
  pup_motor_stop(motorA);
  pup_motor_stop(motorB);

  dly_tsk(500*1000); // 500ms待機

  // IMU初期化
  hub_imu_init();
  ang_v[0] = 0;
  ang_v[1] = 0;
  ang_v[2] = 0;

  // 左90度回転
  while(fabs(ang_v[2])<90){
    pup_motor_set_power(motorA, 30);
    pup_motor_set_power(motorB, -30);
  }

  // モータ停止
  pup_motor_stop(motorA);
  pup_motor_stop(motorB);

  stp_cyc(CYC_HDR); // ジャイロセンサ監視終了

  exit(0);
}

void gyro_monitor(intptr_t exinf)
{
  // IMU角加速度 格納用配列
  float ang_raw[3];
  hub_imu_get_angular_velocity(ang_raw);

  // オフセット補正
  ang_v[0] += (ang_raw[0] - imu_offset[0]) * 0.001;
  ang_v[1] += (ang_raw[1] - imu_offset[1]) * 0.001;
  ang_v[2] += (ang_raw[2] - imu_offset[2]) * 0.001;
}

void imu_setup(float offset[3]){
  dly_tsk(3*1000*1000); // 3s待機
  
  hub_light_on_color(PBIO_COLOR_ORANGE); // LED: オレンジ

  hub_imu_init(); // IMUの初期化

  float ang_raw[3]; // IMU角加速度 raw値 格納用配列

  // オフセット同定 (1秒間で1000回測定して平均取る)
  for(int i=0; i<1000; i++){
    hub_imu_get_angular_velocity(ang_raw); //角加速度取得
    offset[0] += ang_raw[0];
    offset[1] += ang_raw[1];
    offset[2] += ang_raw[2];
    dly_tsk(1*1000); // 1ms待機
  }

  // オフセットをサンプル取得回数で割る
  offset[0] /= 1000;
  offset[1] /= 1000;
  offset[2] /= 1000;
}

(gyro_turn.h及びgyro_turn.cfgはそれぞれgyro_test.h、gyro_test.cfgとほぼ同じ)

これを実行した結果が以下の動画になります。

ご覧の通りある程度90度で回れてはいますが、慣性により少し行き過ぎていることが分かります。
そこで、90度付近に近づいたら減速するように、比例制御を導入したいと思います。

先ほどのモータを動かしている部分について

// 右90度回転
while(fabs(ang_v[2])<90){
  pup_motor_set_power(motorA, -30);
  pup_motor_set_power(motorB, 30);
}

これを以下のように変更します。

// 右90度回転 (比例制御)
while(fabs(ang_v[2])<90){
  int power = (90 - abs((int)ang_v[2]))*0.5;
  int power_A = - 15 - power;
  int power_B = 15 + power;
  pup_motor_set_power(motorA, power_A);
  pup_motor_set_power(motorB, power_B);
}

ベースパワーは15とし、最低でもこのパワーは出るようにします。
又、指定角度である90度から現在の角度の差分を算出し、比例ゲインをかけます。
これらの計算を行い、回転パワーを設定します。

この比例制御を導入した結果が以下の動画になります。

モータパワー一定値の時より、かなり正確に90度回せています。

以上で、SPIKE-RTでもハブブロックのIMU(ジャイロセンサ)を使って、正確な回転を行うことが出来ました。

まとめ

今回は、SPIKE-RTのIMUが加速度しか取れない問題を解決し、角度を取得できるようにソフトを設計。
その角度取得システムを利用して、正確にロボットを90度回転させるプログラムを作成しました。

EV3よりポート数が少ないSPIKE Primeを使っている以上、ハブブロック内蔵のIMUを使わない手はないので、是非参考にしていただければと思います。

宣伝

現在、SPIKE-RTの使い方を記した記事群 【SPIKE-RTでロボコンに出よう!!】 を執筆中です。
今回の記事でSPIKE PrimeやSPIKE-RTに興味を持たれた方、是非この記事シリーズを読んでいただき、SPIKE-RTを使ってみてください!!