はじめに

nRF52840を使用してジャイロセンサーから値を取得する方法の解説．

必要なもの

nRF52840
USB-Cで接続できるケーブル

01.環境構築

Arduino IDEをインストールする．
Arduino IDEを起動し，File > Preferences に移動．"Additional Boards Manager URLs"に https://files.seeedstudio.com/arduino/package_seeeduino_boards_index.json を追加する．
次に，Tools > Board > Boards Manager に移動し，seeed nrf52を検索し，出てきた2つのボードを選択してインストール．
ここで，デバイスとPCをUSB-Cで接続する．
Tools > Board > Seeed nRF52 Boards > Seeed XIAO nRF52840 Sense を選択する．
Tools > Port で COM??(Seeed Studio XIAO nRF52840 Sense) と書いてあるものを選択する．
File > Examples > 01.Basic > Blink を選択する．
左上の右矢印ボタンを押して，正しく書き込まれるかを確認する．
1. もし書き込まれない場合は，リセットボタンを2回早く連続で押してから，再度書き込み．(リセットボタンはUSB-Cケーブルを指すところの右側にある小さいRSTと書かれたところにあるボタン)
2. それでも書き込まれないときは，右矢印の右にあるポート選択を変えて再度書き込み．
3. それでも書き込まれないときは，Tools > Programmer > JLink もしくは Bootloader を選択して書き込み．
書き込みが完了し赤いLEDが点滅すれば成功．
これ以降何か新たに書き込むときは，一度USB-Cケーブルを抜いてから，再度差し込んでから書き込むか，リセットボタンを2回早く連続で押してから，再度書き込み．

02.ジャイロセンサーから値を取得する

まず，Seeed XIAO nRF52840 Senseには3軸加速度センサと3軸ジャイロセンサが搭載されている．これをまとめて6軸慣性測定ユニット(IMU)と呼ぶ．何か検索する際はIMUと検索すると良い．なお，搭載されているIMUはLSM6DS3なため，IMUに関して詳細な情報を取得する必要がある場合は，LSM6DS3について調べるとよい．

02-1. ライブラリのインストール

このデバイスのセンサを使用する場合，ライブラリのインストールが必要である．

https://github.com/Seeed-Studio/Seeed_Arduino_LSM6DS3 にアクセスし，緑色のCodeボタンからDownload ZIPを選択してダウンロードする．
Arduino IDEのSketch > Include Library > Add .ZIP Library... を選択し，ダウンロードしたZIPファイルを選択する．
以下のプログラムを書き込む．

#include "LSM6DS3.h"
#include "Wire.h"

//create a instance
LSM6DS3 myIMU(I2C_MODE, 0x6A);    //I2C device address 0x6A

void setup() {
    // start serial com
    Serial.begin(9600);
    while (!Serial);
    // call IMU.begin()
    // if IMU.begin() return non-zero ,it means imu is not ok
    // if IMU.begin() return 0 ,it means imu is ok

    if (myIMU.begin() != 0) {
        Serial.println("Device error");
    } else {
        Serial.println("Device OK!");
    }
    pinMode(LED_BUILTIN, OUTPUT);
}

void loop() {
    // get accelerometer data
    digitalWrite(LED_BUILTIN, HIGH);   
    Serial.print("\nAccelerometer:\n");
    Serial.print(" X1 = ");
    Serial.println(myIMU.readFloatAccelX(), 4);
    Serial.print(" Y1 = ");
    Serial.println(myIMU.readFloatAccelY(), 4);
    Serial.print(" Z1 = ");
    Serial.println(myIMU.readFloatAccelZ(), 4);

    // get gyroscope data
    Serial.print("\nGyroscope:\n");
    Serial.print(" X1 = ");
    Serial.println(myIMU.readFloatGyroX(), 4);
    Serial.print(" Y1 = ");
    Serial.println(myIMU.readFloatGyroY(), 4);
    Serial.print(" Z1 = ");
    Serial.println(myIMU.readFloatGyroZ(), 4);

    // get thermometer data
    Serial.print("\nThermometer:\n");
    Serial.print(" Degrees C1 = ");
    Serial.println(myIMU.readTempC(), 4);
    Serial.print(" Degrees F1 = ");
    Serial.println(myIMU.readTempF(), 4);

    // get data / 1000ms
    digitalWrite(LED_BUILTIN, LOW);
    delay(1000);
}

右矢印のボタンを押す
右上の虫眼鏡のボタンを押す
デバイスを動かして値が動くかを見てみる
1. なお，赤いLEDが点滅しているときは動作しています

02-1-1. プログラムの解説

LSM6DS3.h → 加速度やジャイロ，温度のデータを取得することができるライブラリ． Wire.h → I2C通信を行うためのライブラリ．

#include "LSM6DS3.h"
#include "Wire.h"

myIMUという名前のLSM6DS3操作用インスタンスを作成する．インスタンスとは，クラスから作成されるオブジェクトのこと．今回I2Cを使用して通信するため，I2C_MODEと0x6Aを設定する．

LSM6DS3 myIMU(I2C_MODE, 0x6A);    //I2C device address 0x6A

シリアル通信を9600bpsで開始

Serial.begin(9600);
while (!Serial);

IMUを初期化するできなかった場合エラーを出す

if (myIMU.begin() != 0) {
    Serial.println("Device error");
} else {
    Serial.println("Device OK!");
}

加速度データを取得この加速度とは，直線的な動くの早さを測定するものである．取得される値は$[g]$(重力加速度)である．

Serial.print("\nAccelerometer:\n");
Serial.print(" X1 = ");
Serial.println(myIMU.readFloatAccelX(), 4);
Serial.print(" Y1 = ");
Serial.println(myIMU.readFloatAccelY(), 4);
Serial.print(" Z1 = ");
Serial.println(myIMU.readFloatAccelZ(), 4);

ジャイロデータを取得このジャイロとは，デバイスに対しての回転速度(角加速度)を算出するものである．取得される値は$[dps]$(degress per second)である．

Serial.print("\nGyroscope:\n");
Serial.print(" X1 = ");
Serial.println(myIMU.readFloatGyroX(), 4);
Serial.print(" Y1 = ");
Serial.println(myIMU.readFloatGyroY(), 4);
Serial.print(" Z1 = ");
Serial.println(myIMU.readFloatGyroZ(), 4);

温度データを取得(*参考) 取得される値は摂氏/華氏である．

Serial.print("\nThermometer:\n");
Serial.print(" Degrees C1 = ");
Serial.println(myIMU.readTempC(), 4);
Serial.print(" Degrees F1 = ");
Serial.println(myIMU.readTempF(), 4);

02-2. IMUの特性について

IMUを低周波成分と高周波成分で考えたとき，低周波成分では加速度を，高周波成分では角速度を採用すると精度が上がる．低周波成分(ゆっくりとした動き/姿勢推定)に関しては，重力方向が安定しており，誤差が蓄積しないため，加速度を利用すべきである．高周波成分(急激な回転/瞬間的な姿勢変化)に関しては，回転運動に対する応答が速く，バイアス誤差の影響が相対的に小さい角速度を利用すべきである．低周波と高周波で割合をかけて相補フィルタをかけるとよい．

姿勢推定

本節では，6軸IMU（3軸ジャイロセンサ＋3軸加速度センサ）を用いた姿勢推定手法について説明する．
姿勢はクォータニオンで表現し，ジャイロセンサによる高周波成分と，加速度センサによる低周波成分を相補フィルタで融合することで推定する．

姿勢表現方法の選択

姿勢表現にはオイラー角，回転行列，クォータニオンなどが存在するが，本手法ではクォータニオンを採用する．
その理由は以下の通りである．

ジンバルロックが発生しない
数値的に安定である
姿勢の補間や積分計算に適している

クォータニオンは以下のように表される．

$$ q = (w, x, y, z) $$

ここで $w$ は実部，$(x, y, z)$ は虚部であり，単位クォータニオンとして正規化されている．

ジャイロセンサによる姿勢更新（高周波成分）

ジャイロセンサは，各軸周りの角速度ベクトル

$$ \vec{\omega} = (\omega_x, \omega_y, \omega_z) $$

を出力する．この角速度を時間積分することで，姿勢の時間変化を推定できる．

サンプリング周期を $dt$ とすると，微小回転角は次式で求められる．

$$ \Delta \vec{\theta} = \vec{\omega} \cdot dt $$

回転角の大きさ $\theta$ および回転軸 $\vec{u}$ は以下で与えられる．

$$ \theta = \sqrt{\Delta\theta_x^2 + \Delta\theta_y^2 + \Delta\theta_z^2} $$

$$ \vec{u} = \frac{1}{\theta} \begin{pmatrix} \Delta\theta_x \ \Delta\theta_y \ \Delta\theta_z \end{pmatrix} $$

この回転を表す微小回転クォータニオン $dq$ は，

$$ dq = \left( \cos\frac{\theta}{2}, u_x \sin\frac{\theta}{2}, u_y \sin\frac{\theta}{2}, u_z \sin\frac{\theta}{2} \right) $$

として表される．現在の姿勢クォータニオン $q_{current}$ に対し，

$$ q_{gyro} = \mathrm{normalize}(q_{current} \otimes dq) $$

とすることで姿勢を更新する．

この手法は短時間では高精度であるが，ジャイロバイアスやノイズによりドリフト誤差が蓄積するという問題を持つ．

加速度センサによる姿勢推定（低周波成分）

加速度センサは，

$$ \vec{a} = (a_x, a_y, a_z) $$

を出力する．静止状態または等速直線運動時には，加速度ベクトルは重力方向を示すため，これを用いて姿勢を推定できる．

Roll角およびPitch角は次式で求められる．

$$ \mathrm{Roll} = \arctan2(a_y, a_z) $$

$$ \mathrm{Pitch} = \arctan2(-a_x, \sqrt{a_y^2 + a_z^2}) $$

Yaw角は重力情報のみからは求められないため，ジャイロ由来のYaw角をそのまま使用する．

加速度センサによる姿勢推定はドリフトを生じないが，動的な運動中には並進加速度の影響を受けやすいという欠点がある．

相補フィルタによる融合

ジャイロセンサは高周波成分に強く，加速度センサは低周波成分に強いという特性を持つ．
そこで両者を相補的に組み合わせるため，相補フィルタを適用する．

ジャイロ由来のクォータニオン $q_{gyro}$ と，加速度由来のクォータニオン $q_{accel}$ を，正規化線形補間（nlerp）により融合する．

$$ q = \mathrm{nlerp}(q_{gyro}, q_{accel}, 1 - \alpha) $$

ここで $\alpha$ は重み係数であり，本手法では $\alpha = 0.98$ とする．

加速度信頼性判定

加速度センサは動的運動時に誤差を含むため，加速度ノルム

$$ |\vec{a}| = \sqrt{a_x^2 + a_y^2 + a_z^2} $$

を用いて信頼性判定を行う．

$$ 0.5 < |\vec{a}| < 1.5 ; [g] $$

の範囲内にある場合のみ，加速度による補正を行う．
この条件を外れた場合は，加速度データを無視し，ジャイロ積分のみで姿勢更新を行う．

まとめ（本節）

姿勢表現にはクォータニオンを採用
ジャイロ積分により高速な姿勢変化を追従
加速度により長期的なドリフトを補正
相補フィルタにより安定した姿勢推定を実現

この手法により，nRF52840に搭載された6軸IMUを用いて，高周波・低周波の両特性を活かした実用的な姿勢推定が可能となる．

nRF52840でジャイロセンサーから値を取得する