はじめに

「われ、よからぬことをたくらむ者なり」

この紙はただの紙じゃない。忍びの地図というものなんだ。
魔法をかけた相手の場所が文字通り手にとるようにわかる魔法の道具さ。

実際の使い方が知りたいって？下の画面を見てくれ！

ここに動画が表示されます

※カメラ映像は合成です
※右足の軌跡がプロットされています

1.構成

全体構成

IMUセンサにより取得した計測開始位置からの移動量をPCに送信します。
UI上で初期位置を指定することで、初期位置からの移動量を1歩ずつ表示することを可能にしています。

ハードウェア設計図

準備物

部品名	数量	金額（円）	役割
SPRESENSEメインボード[CXD5602PWBMAIN1]	1	6,050	制御ボード
BLE for Spresense【BLE1507】	1	3,850	PCとの通信用
BNO055使用9軸センサーモジュール	1	2,450	歩幅、角度などの算出用
SPRESENSE用Qwiic接続基板	1	770	IMUセンサ接続用
リチウムポリマー電池3.7V300mAh	1	900	バッテリー
インソール（厚さ3cm）	1	999	筐体埋め込み用

IMUユニット構成図

リチウムポリマー電池を電源として用いることで、対象者が自由に歩き回れるようにしています。

ハウス作成

配線しやすいように所々溝を切り、外れないように爪を作成しました。
上から体重がかかることを考え蓋部分はできる限り肉厚にし、Spresense本体とバッテリーを同一平面上に配置することで、全体の高さを抑えています。

組み立て

1.筐体に各部材を配置
各格納位置にセンサを配置した様子。
ほぼ隙間なく各センサ類が収まりました。

2.ふたをしめる
幅:55mm,奥行:65mm,高さ:25mmの直方体にすべてが収まります。

3.インソールと筐体を靴の中に挿入
直接踏むと足が痛い＆筐体に体重がかかってしまうため、インソールをいれることで踵とセンサ間にかかる力を軽減しています。

2.技術要素

歩行者自律航法(PDR:Pedestrian Dead Reckoning)

IMUのドリフト対策

MEMS式のIMUはゼロ点バイアスにより、ドリフトが起こることが知られています。
真値との誤差を補正する手法、特に環境に参照点を作らずにヒトに貼付したセンサのみで自己位置を推定する手法を歩行者自律航法(PDR)と呼び、過去に様々手法が提案されています。

今回は歩行が周期的な運動であることに着目し、足が静止している間の加速度・角速度の期間の積分を速度・角度・位置の算出から除外することで位置の推定精度を高めています。

具体的には下図に記載の通り下記の処理により歩行中の足の層を判定し、位置の算出を行っています。

1.足の接地の検出
足が接地する際には運動が床と接触して止まることにより加速度が大きく変化することが知られています。
今回は加速度の変化(微分値)をとらえることで、接地の瞬間を計測しています。
加速度の微分値は躍度(加加速度)と呼ばれる値であり、エレベーターの動きだしにガクンと体が下がる感覚を生み出すものとされています。
下図の「Initial Contact」を躍度を用いて検出することで、足の接地を検出しています。

2.足の静止の検出
足が接地した直後から体重が接地したほうの足に移ります。
踵を中心に足部は急激に回転し、足全体が床につきます。(下図中で「Toe On」と記されている時点)
歩行者の体重は急速に接地した足に移され、反対側の足が地面を離れても転ばないようになります。(下図の「Loading Response」)
ここからしばらく接地した足は体重を支えるため、静止します。
今回の手法では「Toe On」を加速度と角速度の大きさが一定未満になったときとして検出し、足の静止判定を行います。
次に足が動き始めるまでの間に計測した加速度・角速度を誤差により発生したものをみなし、位置算出のための積分値から除外します。

3.足の動き出しの検知
足が接地してから反対の足が接地する少し前まで、足の静止状態(下図の「Foot Rest」)は続きます。
加速度と角速度の大きさのどちらかもしくは両方が一定以上になったときに足が動き出したと判定し(下図の「Heel Off」)、再度、加速度・角度を位置算出のための積分値に含めるようにします。
この時の加速度・角速度の閾値は足の静止検出に用いた値と同値です。

4.足の離地の検出
加速度を積分し始め、足部の速度が一定以上になったところを「Toe Off」として検知し、それ以降、足が地面を離れた(下図の「Swing」)としています。
地面から離れている足が再度着地する際に「1.足の接地の検出」に出てきた躍度の大きくなる瞬間があるので、そこで1歩が完成します。上記の1から4までのイベントを経て、再度1の足の接地を検出した瞬間までの積分値を位置としてアプリに送信します。

3.ソースコード

Spresense側

メインコアでBLEコマンド処理、サブコアでIMUの計測および歩行軌跡の計算を行っています。


メインコア：BLEコマンド処理
// コア確認
#ifdef SUBCORE
#error "Core selection is wrong!!"
#endif

/* Includes ------------------------------------------------------------------*/
#include <MP.h>

/* Defines -------------------------------------------------------------------*/
// msg id
#define MSG_ID_CTRL   0

// msg
#define MSG_INIT   0
#define MSG_START  1
#define MSG_STOP   2

// BLE
#define CTRL_CMD_BUFF_SIZE 20  // BLEコマンド受信バッファ長

/* Variables -----------------------------------------------------------------*/
// BLE モジュールとの通信用
char receivedData[CTRL_CMD_BUFF_SIZE] = {0}; // 受信データ用の固定長バッファ
int bufferIndex = 0;                  // バッファの現在位置

// BLEコマンド
const String startReq = "START_REQ"; // 計測開始要求
const String startRsp = "START_RSP"; // 計測開始応答
const String stopReq = "STOP_REQ"; // 計測停止要求
const String stopRsp = "STOP_RSP"; // 計測停止応答
const String errorRsp = "ERROR_RSP"; // エラー応答

// コア間通信用
int subcore = 1;

/*----------------------------------------------------------------------------*/
void setup() {

  int ret = 0;
  int8_t msgid = MSG_ID_CTRL;
  digitalWrite(LED0, LOW);

  /////////////////////////////////////////////
  // シリアル通信開始
  Serial.begin(115200);
  Serial2.begin(115200);
  Serial.println("Boot up.");

  /////////////////////////////////////////////
  // サブコア起動
  Serial.println("Start subcore");
  ret = MP.begin(subcore);
  if (ret < 0) {
    // エラー処理
    printf("Error: MP.begin, ret = %d\n", ret);
    digitalWrite(LED0, HIGH);
    while(1){}
  }

  /////////////////////////////////////////////
  // 加速度センサ初期化
  uint32_t msg;
  msg = MSG_INIT;

  // サブコアにIMU初期化　メッセージ送信
  MP.Send(msgid, msg, subcore);

  // サブコアからの加速度センサ　初期化完了メッセージ待ち
  MP.Recv(&msgid, &msg, subcore);

  // メッセージ受信モード設定変更：ポーリング
  MP.RecvTimeout(MP_RECV_POLLING);
}

void loop() {

  int8_t msgid = MSG_ID_CTRL;
  uint32_t msg;
  int ret;

  /////////////////////////////////////////////
  // BLEモジュールからのコマンド確認
  if (Serial2.available() > 0) {
    char receivedChar = (char)Serial2.read(); // 1文字受信

    if (receivedChar == '$') {            // 終端文字 '$' を検出
      if (bufferIndex > 0) {              // バッファにデータがある場合
        receivedData[bufferIndex] = '\0'; // 終端文字を追加
        String command(receivedData);     // 文字配列をStringに変換

        Serial.println("Received command: " + command); // デバッグ出力

        // コマンドが一致するか確認
        if (command.compareTo(startReq) == 0) {
          Serial.println("Command received 1: " + command);
          // コマンド応答
          Serial2.println(startRsp + "$");

          // 測定開始をサブコアに指示
          msg = MSG_START;
          MP.Send(msgid, msg, subcore);

        } else if (command.compareTo(stopReq) == 0) {
          Serial.println("Command received 2: " + command);
          // コマンド応答
          Serial2.println(stopRsp + "$");

          // 測定停止をサブコアに指示
          msg = MSG_STOP;
          MP.Send(msgid, msg, subcore);

        } else {
          Serial.println("Unknown command: " + command);
          // コマンド応答
          Serial2.println(errorRsp + "$");
        }

        // バッファクリア
        memset(receivedData, 0, CTRL_CMD_BUFF_SIZE);
        bufferIndex = 0;
      }
    } else if ((receivedChar != '\r') && (receivedChar != '\n')) {
      // 改行コード以外の文字をバッファに格納
      if (bufferIndex < CTRL_CMD_BUFF_SIZE - 1) {
        receivedData[bufferIndex++] = receivedChar;
        Serial.println(receivedChar);
      } else {
        Serial.println("Error: Buffer overflow");
        memset(receivedData, 0, CTRL_CMD_BUFF_SIZE); // バッファをクリア
        bufferIndex = 0;
      }
    }
  }

  /////////////////////////////////////////////
  // サブコアからの計算結果確認
  char *buffer; // 受信バッファ
  ret = MP.Recv(&msgid, &buffer, subcore);

  // メッセージを受信していればBLE送信する
  if (ret == msgid) {
    for(uint8_t i = 0; i < 128; i++) {
      if (buffer[i] == '$') {
        break;
      }
      Serial2.print(buffer[i]);
    }
    Serial2.println("$");
  }

  delay(1);
}


サブコア：IMU処理
// コア確認
#if (SUBCORE != 1)
#error "Core selection is wrong!!"
#endif

/* Includes ------------------------------------------------------------------*/
#include <MP.h>
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
#include <math.h>

/* Defines -------------------------------------------------------------------*/
// #define AUTO_START_EN // デバッグ用フラグ: STARTコマンド無しで計測開始

#define BNO055_SAMPLERATE_DELAY_MS 10 // 100Hzサンプリング

#define IMU_SAMPLE      10000 // 10sec
#define DATA_POS_X      0
#define DATA_POS_Y      1
#define DATA_POS_Z      2

// msg id
#define MSG_ID_CTRL   0
#define MSG_ID_DATA   1

// msg
#define MSG_INIT   0
#define MSG_START  1
#define MSG_STOP   2

// 歩行状態の定義
typedef enum {
  LOADING_RESPONSE = 1,
  FOOT_REST,
  PRE_SWING,
  SWING,
} state_t;
state_t walk_state = FOOT_REST; // 初期状態

Adafruit_BNO055 bno = Adafruit_BNO055(-1, 0x28, &Wire);

/* Variables -----------------------------------------------------------------*/
uint64_t measure_time_ms = 0; // センサデータ取得間隔制御用変数
uint8_t measurement_cnt = 0; // 計測1回目, 2回目判定用カウント
bool measurement_flag = false; // IMU計測開始・停止制御用フラグ

int writeIndex = 0; // バッファ配列　書き込み番号
char buffer[128]; // 歩行軌跡　結果通知用バッファ

// 前回の計算値格納用
float acc_r_xyz_prev[3] = {0};
float velocity_xyz_prev[3] = {0};
float position_xyz_prev[3] = {0};

// 初期回転行列 (単位行列)
float R_prev[3][3] = {
    {1, 0, 0},
    {0, 1, 0},
    {0, 0, 1}
};

// 歩行状態判定　しきい値
const float a_ths = 2.0;
const float omega_ths = 85.9437; // 1.5 rad/sec
const float v_ths = 2.5;
const float j_ths = 3750;

/*----------------------------------------------------------------------------*/
void setup() {
  float *addr;
  int8_t msgid = MSG_ID_CTRL;

  /////////////////////////////////////////////
  // サブコア起動をメインコアに通知
  MP.begin();
  Serial.begin(115200);

  /////////////////////////////////////////////
  // 加速度取得開始
  // メインコアから開始メッセージを待つ
  uint32_t msg;
  MP.Recv(&msgid, &msg);

  // 加速度センサ初期化
  accel_init();

  // 加速度センサ初期化完了を　メインコアに通知
  msg = MSG_INIT;
  MP.Send(msgid, msg);

  // メッセージ受信モード設定変更：ポーリング
  MP.RecvTimeout(MP_RECV_POLLING);
}

void loop() {

  // メインコアからのコマンド受信処理
  int8_t msgid = MSG_ID_CTRL;
#ifndef AUTO_START_EN
  uint32_t msg;
  int ret;
  ret = MP.Recv(&msgid, &msg);

  // メッセージを受信していれば処理する
  if (ret == msgid) {
    if (msg == MSG_START) {
      // 計測開始 -> 計測フラグオン
      measurement_flag = true;
      measurement_cnt = 0;
      walk_state = FOOT_REST;
    } else if (msg == MSG_STOP) {
      // 計測停止 -> 計測フラグオフ
      measurement_flag = false;
    }
  }
#else // AUTO_START_EN
  measurement_flag = true; // 常に計測
#endif

  // サンプリング周期ごとにIMUからデータ取得
  if ((measurement_flag) && (millis() - measure_time_ms > BNO055_SAMPLERATE_DELAY_MS)) {

    // 計測時刻の更新
    uint64_t now_time_ms = millis(); // 測定開始の時刻を格納

    imu::Vector<6> imu_vector;
    while(1) {
      imu_vector = bno.getAccGyro();

      // エラーチェック -> 加速度3軸が全て0以外の場合は正常とする
      if ((imu_vector[0] != 0) && (imu_vector[1] != 0) && (imu_vector[2] != 0)) {
        break;
      }
      // センサとの通信失敗時はリトライする
    }

    float elapsed_time_sec = (float)((now_time_ms - measure_time_ms) * 0.001); // 前回測定のタイミングからの経過時間を計算
    measure_time_ms = now_time_ms; // 測定タイミングの時刻を更新

    float acc_r_xyz[3] = {0}, gyro_xyz[3] = {0}, angle_xyz[3] = {0}, acc_w_xyz[3] = {0}, velocity_xyz[3] = {0}, position_xyz[3] = {0}, acc_delta_xyz[3] = {0};
    float acc_slope_sum, acc_sum, gyro_sum, velocity_sum;

    // 生の加速度
    acc_r_xyz[DATA_POS_X] = (float)imu_vector[0];
    acc_r_xyz[DATA_POS_Y] = (float)imu_vector[1];
    acc_r_xyz[DATA_POS_Z] = (float)imu_vector[2];

    // 角速度 (deg/sec)
    gyro_xyz[DATA_POS_X] = (float)imu_vector[3];
    gyro_xyz[DATA_POS_Y] = (float)imu_vector[4];
    gyro_xyz[DATA_POS_Z] = (float)imu_vector[5];

    // 加速度（3軸合計）の算出
    acc_sum = sqrt((acc_r_xyz[DATA_POS_X] * acc_r_xyz[DATA_POS_X]) +
                  (acc_r_xyz[DATA_POS_Y] * acc_r_xyz[DATA_POS_Y]) +
                  (acc_r_xyz[DATA_POS_Z] * acc_r_xyz[DATA_POS_Z]))
                  - 9.8; // z軸からは重力加速度を引く

    // 角速度（3軸合計）の算出
    gyro_sum = sqrt((gyro_xyz[DATA_POS_X] * gyro_xyz[DATA_POS_X]) +
                    (gyro_xyz[DATA_POS_Y] * gyro_xyz[DATA_POS_Y]) +
                    (gyro_xyz[DATA_POS_Z] * gyro_xyz[DATA_POS_Z]));

    // FOOTREST判定確認
    bool f_footrest = (acc_sum < a_ths) && (gyro_sum < omega_ths);

    // FOOTREST判定となる場合、各速度をゼロにする
    if (f_footrest) {
      gyro_xyz[DATA_POS_X] = 0;
      gyro_xyz[DATA_POS_Y] = 0;
      gyro_xyz[DATA_POS_Z] = 0;
    }

    // 角度の算出
    // 底背屈角度 = 角速度の積分
    if (measurement_cnt == 0) { // 初回計測時はゼロ
      angle_xyz[DATA_POS_X] = 0;
      angle_xyz[DATA_POS_Y] = 0;
      angle_xyz[DATA_POS_Z] = 0;
    }
    else {
      // 現在の回転行列
      float R_current[3][3];

      // 角速度変換（deg/sec to rad/sec）
      float gyro_rad_x = gyro_xyz[DATA_POS_X] * (M_PI / 180.0);
      float gyro_rad_y = gyro_xyz[DATA_POS_Y] * (M_PI / 180.0);
      float gyro_rad_z = gyro_xyz[DATA_POS_Z] * (M_PI / 180.0);

      // 回転の近似回転行列 ΔR
      float deltaR[3][3] = {
        {1, -gyro_rad_z * elapsed_time_sec, gyro_rad_y * elapsed_time_sec},
        {gyro_rad_z * elapsed_time_sec, 1, -gyro_rad_x * elapsed_time_sec},
        {-gyro_rad_y * elapsed_time_sec, gyro_rad_x * elapsed_time_sec, 1}
      };

      // ΔR * R_prev の計算 (行列の積)
      for (int i = 0; i < 3; i++) {
          for (int j = 0; j < 3; j++) {
              R_current[i][j] = 0;
              for (int k = 0; k < 3; k++) {
                  R_current[i][j] += deltaR[i][k] * R_prev[k][j];
              }
          }
      }

      // 方位角 (azimuth) -> 角度z
      angle_xyz[DATA_POS_Z] = atan2(R_current[1][0], R_current[0][0]) * 180.0 / M_PI;

      // 仰角 (elevation) -> 角度y
      angle_xyz[DATA_POS_Y] = atan2(-R_current[2][0], sqrt(R_current[2][1] * R_current[2][1] + R_current[2][2] * R_current[2][2])) * 180.0 / M_PI;

      // ロール角 (roll) -> 角度x
      angle_xyz[DATA_POS_X] = atan2(R_current[2][1], R_current[2][2]) * 180.0 / M_PI;

      // R_prev を R_current に更新
      for (int i = 0; i < 3; i++) {
          for (int j = 0; j < 3; j++) {
              R_prev[i][j] = R_current[i][j];
          }
      }
    }

    // 進行方向の加速度の算出
    // y: 進行方向、x: 左右方向、z: 鉛直方向
    transform_acceleration(acc_r_xyz[DATA_POS_X], acc_r_xyz[DATA_POS_Y], acc_r_xyz[DATA_POS_Z],
                            angle_xyz[DATA_POS_X], angle_xyz[DATA_POS_Y], angle_xyz[DATA_POS_Z],
                            &acc_w_xyz[DATA_POS_X], &acc_w_xyz[DATA_POS_Y], &acc_w_xyz[DATA_POS_Z]);

    // 重力加速度の補正
    acc_w_xyz[DATA_POS_Z] = acc_w_xyz[DATA_POS_Z] - 9.8;

    // 速度の算出
    // 速度 = 進行方向の加速度の積分
    if (measurement_cnt == 0) {
      // 初回計測時はゼロ
      velocity_xyz[DATA_POS_X] = 0;
      velocity_xyz[DATA_POS_Y] = 0;
      velocity_xyz[DATA_POS_Z] = 0;
    }
    else {
      velocity_xyz[DATA_POS_X] = velocity_xyz_prev[DATA_POS_X] + acc_w_xyz[DATA_POS_X] * elapsed_time_sec;
      velocity_xyz[DATA_POS_Y] = velocity_xyz_prev[DATA_POS_Y] + acc_w_xyz[DATA_POS_Y] * elapsed_time_sec;
      velocity_xyz[DATA_POS_Z] = velocity_xyz_prev[DATA_POS_Z] + acc_w_xyz[DATA_POS_Z] * elapsed_time_sec;
    }

    // FOOTREST判定となる場合、速度をゼロにする
    if (f_footrest) {
      velocity_xyz[DATA_POS_X] = 0;
      velocity_xyz[DATA_POS_Y] = 0;
      velocity_xyz[DATA_POS_Z] = 0;
    }

    // 位置算出
    // 位置 = 速度の積分
    if (measurement_cnt < 2) {
      // 2回目計測まではゼロ
      position_xyz[DATA_POS_X] = 0;
      position_xyz[DATA_POS_Y] = 0;
      position_xyz[DATA_POS_Z] = 0;
    }
    else {
      position_xyz[DATA_POS_X] = position_xyz_prev[DATA_POS_X] + velocity_xyz_prev[DATA_POS_X] * elapsed_time_sec;
      position_xyz[DATA_POS_Y] = position_xyz_prev[DATA_POS_Y] + velocity_xyz_prev[DATA_POS_Y] * elapsed_time_sec;
      position_xyz[DATA_POS_Z] = position_xyz_prev[DATA_POS_Z] + velocity_xyz_prev[DATA_POS_Z] * elapsed_time_sec;
    }

    // 加速度の微分値の算出
    acc_delta_xyz[DATA_POS_X] = abs(acc_r_xyz[DATA_POS_X] - acc_r_xyz_prev[DATA_POS_X]) / (elapsed_time_sec);
    acc_delta_xyz[DATA_POS_Y] = abs(acc_r_xyz[DATA_POS_Y] - acc_r_xyz_prev[DATA_POS_Y]) / (elapsed_time_sec);
    acc_delta_xyz[DATA_POS_Z] = abs(acc_r_xyz[DATA_POS_Z] - acc_r_xyz_prev[DATA_POS_Z]) / (elapsed_time_sec);

    // 加速度微分値（3軸合計）の算出
    acc_slope_sum = sqrt((acc_delta_xyz[DATA_POS_X] * acc_delta_xyz[DATA_POS_X]) +
                        (acc_delta_xyz[DATA_POS_Y] * acc_delta_xyz[DATA_POS_Y]) +
                        (acc_delta_xyz[DATA_POS_Z] * acc_delta_xyz[DATA_POS_Z]));

    // 速度（3軸合計）の算出
    velocity_sum = sqrt((velocity_xyz[DATA_POS_X] * velocity_xyz[DATA_POS_X]) +
                        (velocity_xyz[DATA_POS_Y] * velocity_xyz[DATA_POS_Y]) +
                        (velocity_xyz[DATA_POS_Z] * velocity_xyz[DATA_POS_Z]));

    // 状態の判定
    // foot restに戻る判定
    if ((acc_sum < a_ths) && (gyro_sum < omega_ths)) {

      // 前状態がSwingなら、1歩終わりとして計算結果を通知
      if (walk_state == SWING) {
        // 位置
        Serial.print("DATA_NTF,");
        Serial.print(position_xyz[DATA_POS_X]);
        Serial.print(",");
        Serial.print(position_xyz[DATA_POS_Y]);
        Serial.print(",");
        Serial.print(position_xyz[DATA_POS_Z]);
        Serial.print(",");
        // 角度
        Serial.print(angle_xyz[DATA_POS_X]);
        Serial.print(",");
        Serial.print(angle_xyz[DATA_POS_Y]);
        Serial.print(",");
        Serial.print(angle_xyz[DATA_POS_Z]);
        Serial.println("$");

        // メインコアに送信データを通知
        String msg = "";
        msg += "DATA_NTF,"
          + String(position_xyz[DATA_POS_X]) + ","
          + String(position_xyz[DATA_POS_Y]) + ","
          + String(position_xyz[DATA_POS_Z]) + ","
          + String(angle_xyz[DATA_POS_X]) + ","
          + String(angle_xyz[DATA_POS_Y]) + ","
          + String(angle_xyz[DATA_POS_Z]) + "$\r\n";

        size_t length = msg.length() + 1;
        msg.toCharArray(buffer, length);
        int8_t msgid = MSG_ID_DATA;
        MP.Send(msgid, &buffer);
      }

      // 状態更新
      walk_state = FOOT_REST;
    }
    else if (walk_state == FOOT_REST) {
      walk_state = PRE_SWING;
    }
    else if ((velocity_sum >= v_ths) && (walk_state == PRE_SWING)) {
      walk_state = SWING;
    }
    else if ((acc_slope_sum >= j_ths) && (walk_state == SWING)) {

      // 前状態がSwingなら、1歩終わりとして計算結果を通知
      // 位置
      Serial.print("DATA_NTF,");
      Serial.print(position_xyz[DATA_POS_X]);
      Serial.print(",");
      Serial.print(position_xyz[DATA_POS_Y]);
      Serial.print(",");
      Serial.print(position_xyz[DATA_POS_Z]);
      Serial.print(",");
      // 角度
      Serial.print(angle_xyz[DATA_POS_X]);
      Serial.print(",");
      Serial.print(angle_xyz[DATA_POS_Y]);
      Serial.print(",");
      Serial.print(angle_xyz[DATA_POS_Z]);
      Serial.println("$");

      // メインコアに送信データを通知
      String msg = "";
      msg += "DATA_NTF,"
        + String(position_xyz[DATA_POS_X]) + ","
        + String(position_xyz[DATA_POS_Y]) + ","
        + String(position_xyz[DATA_POS_Z]) + ","
        + String(angle_xyz[DATA_POS_X]) + ","
        + String(angle_xyz[DATA_POS_Y]) + ","
        + String(angle_xyz[DATA_POS_Z]) + "$\r\n";

      size_t length = msg.length() + 1;
      msg.toCharArray(buffer, length);
      int8_t msgid = MSG_ID_DATA;
      MP.Send(msgid, &buffer);

      // 状態更新
      walk_state = LOADING_RESPONSE;
    }

    // 前回値を更新
    for (int i = 0; i < 3; i++) {
      acc_r_xyz_prev[i] = acc_r_xyz[i];
      velocity_xyz_prev[i] = velocity_xyz[i];
      position_xyz_prev[i] = position_xyz[i];
    }

    // デバッグプリント用
    // printf("x:%f, y:%f, z:%f\n", angle_xyz[DATA_POS_X], angle_xyz[DATA_POS_Y], angle_xyz[DATA_POS_Z]);
    // printf("walk state: %d\n", (uint8_t)walk_state);

    // 1回目・2回目判定用
    if (measurement_cnt < 2) {
      measurement_cnt++;
    }
  }

  delay(1);
}

/**
 * @brief BNO055初期化
 *
 */
void accel_init()
{
  if(!bno.begin(OPERATION_MODE_ACCGYRO))
  {
    /* There was a problem detecting the BNO055 ... check your connections */
    Serial.print("Sub: Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
    while(1);
  }

  delay(1000);

  // 計測までに温度を1回読み出しておく
  // 初回の計測値がゼロになるのを回避するおまじない
  int8_t temp = bno.getTemp();

  // 必要であれば、ここで補正用の加速度を取得しておく
  // 現状、z軸から9.8を引いてざっくりと補正している

  bno.setExtCrystalUse(true);
}

/**
 * @brief センサの加速度データをワールド座標系に変換する
 *
 * @param ax x軸加速度
 * @param ay y軸加速度
 * @param az z軸加速度
 * @param theta_x 方位角
 * @param theta_y 仰角
 * @param theta_z ロール角
 * @param[out] ax_world 変換後のワールド座標系 x軸加速度
 * @param[out] ay_world 変換後のワールド座標系 y軸加速度
 * @param[out] az_world 変換後のワールド座標系 z軸加速度
 */
void transform_acceleration(float ax, float ay, float az,
                            float theta_x, float theta_y, float theta_z,
                            float *ax_world, float *ay_world, float *az_world) {
    // 角度を用いた三角関数の計算
    float cx = cos(theta_x * M_PI / 180), sx = sin(theta_x * M_PI / 180); // cos, sin for roll (X-axis rotation)
    float cy = cos(theta_y * M_PI / 180), sy = sin(theta_y * M_PI / 180); // cos, sin for pitch (Y-axis rotation)
    float cz = cos(theta_z * M_PI / 180), sz = sin(theta_z * M_PI / 180); // cos, sin for yaw (Z-axis rotation)

    // 回転行列 R を用いた変換式
    *ax_world = cy * cz * ax + (cz * sx * sy - cx * sz) * ay + (cx * cz * sy + sx * sz) * az;
    *ay_world = cy * sz * ax + (cx * cz + sx * sy * sz) * ay + (-cz * sx + cx * sy * sz) * az;
    *az_world = -sy * ax + cy * sx * ay + cx * cy * az;
}

BLEモジュール

BLEモジュールにはNordic UART Service(NUS)のFWを書き込んであります。
BLE1507の製品ページからダウンロードできます。

UI側

呪文入力画面を表示し、呪文の確認を行っています。
地図表示後に1回目のクリックで初期地点設定、2回目のクリックで計測開始のシグナルを送信しています。
その後は位置情報の受信をトリガーにして現在地の描画を行い、過去4歩までを表示しています。


BLEから取得した位置表示アプリ
# -*- encoding: utf-8 -*-
import PySide6
from PySide6.QtCore import (Signal,
                            QObject,
                            QPropertyAnimation,
                            Qt,
                            QEventLoop,
                            QTimer)
from PySide6.QtGui import (QPixmap,
                            QFont,
                            QAction,
                            QPainter
                            )
from PySide6.QtWidgets import (QApplication,
                                QLabel,
                                QPushButton,
                                QWidget,
                                QVBoxLayout,
                                QGraphicsOpacityEffect,
                                QMenu,
                                QLineEdit,
                                QGraphicsBlurEffect                                
                                )
import os
import sys
import asyncio
import threading
from bleak import BleakClient
import queue
import math

global communicator
DEV_MAC_ADDRESS = "XX:XX:XX:XX:XX:XX" # 接続先のBLEのMACアドレス
# Service UUID (NUS)
UUID_NOTIFY = "6E400003-B5A3-F393-E0A9-E50E24DCCA9E"
UUID_WRITE = "6E400002-B5A3-F393-E0A9-E50E24DCCA9E"

START_CMD = bytearray("START_REQ$","utf-8") #計測開始
STOP_CMD = bytearray("STOP_REQ$","utf-8") #計測停止

# シグナルを作成するクラス
class Communicator(QObject):
    async def __aenter__(self):
        # 非同期の初期化処理
        print("Entering Communicator")
        return self  # 自身のインスタンスを返す

    async def __aexit__(self, exc_type, exc_value, traceback):
        # 非同期の終了処理（例: リソースのクリーンアップ）
        print("Exiting Communicator")
    update_label_signal = Signal(bytearray)  # 引数に文字列を渡せるシグナル

# 表示する最上位ウィンドウ
class MainWindow(QWidget):
    def __init__(self, parent=None):
        super().__init__(parent)
        #ウィンドウタイトル
        self.setWindowTitle("忍びの地図")
        #表示位置
        xPos = 0 # x座標(横)
        yPos = 0 # y座標(縦)
        windowwidth = 288 #Let's note CF-QVの解像度
        windowheight = 192 #Let's note CF-QVの解像度

        # 全画面表示
        self.showFullScreen()

        self.foot_print_counter = 0
        self.foot_size = 50

        #ウィンドウの位置とサイズの変更
        self.path_to_script = os.path.abspath(__file__)
        self.secondary_window = None
        self.start_app() # 描画開始

        # BLEデータ受信処理用
        self.data_buffer = bytearray()  # データを保持するバッファ

    def start_app(self):
        self.set_top() #ふた絵表示
        self.open_secondary_window() #ポップアップ表示
        self.update() 

    def set_top(self):
        #画像の読み込み
        map_path = os.path.dirname(self.path_to_script)+"\\top_logo.png"
        #map_path = os.path.dirname(self.path_to_script)+"\\map_bg_resize.png"
        bg_image = QPixmap(map_path)
        self.bg_label = QLabel(self)
        #label.setText("テストラベルです。")
        self.bg_label.setPixmap(bg_image)
        window_width = self.width()
        window_height = self.height()
        self.bg_label.setGeometry(0,0, window_width, window_height)
        self.bg_label.setAlignment(Qt.AlignCenter)
        self.bg_label.setScaledContents(True)
        self.bg_label.show()

    def open_secondary_window(self):
        # 別ウィンドウを初めて開く際にインスタンスを作成
        if self.secondary_window is None:
            self.secondary_window = PopupWindow()
            self.secondary_window.eventTriggered.connect(self.set_map)
        self.secondary_window.show()

    def set_map(self):
        self.set_bg()

    def set_bg(self):
        #画像の読み込み
        map_path = os.path.dirname(self.path_to_script)+"\\bg_resize.png"
        #map_path = os.path.dirname(self.path_to_script)+"\\map_bg_resize.png"
        bg_image = QPixmap(map_path)
        self.bg_label = MainLabel(self)
        #label.setText("テストラベルです。")
        self.bg_label.setPixmap(bg_image)
        window_width = self.width()
        window_height = self.height()
        self.bg_label.setGeometry(0,0, window_width, window_height)
        self.bg_label.setAlignment(Qt.AlignCenter)
        self.bg_label.setScaledContents(True)
        self.bg_label.show()
        self.bg_label.map_appear.connect(self.on_map_show)

    def on_map_show(self):
        self.prepare_measuring()

    def prepare_measuring(self):
        self.bg_label.initial_click.connect(self.set_initial_position) # 1回目のクリックで初期点指定
        self.bg_label.second_click.connect(self.start_measuring) # 2回目のクリックで計測スタート

    def set_initial_position(self,pos_tuple):
        self.initial_position = pos_tuple

    def start_measuring(self):
        #BLE接続待ち
        while True:
            if BLE_evt_que.empty() == False:
                if BLE_evt_que.get() == True:
                    break
        BLE_ctrl_que.put(True) # START_CMD送信

    # BLE Notification受信時の処理
    def catchBLE(self,data):
        # 受け取ったデータを文字列に変換
        data_str = data.decode('utf-8')
        # print(f"Received data: {data}")

        # バッファが空でない場合、データをバッファに追加
        if self.data_buffer:
            self.data_buffer.extend(data)
        else:
            # 新しいデータが「DATA_NTF」から始まっている場合、バッファに追加
            if data_str.startswith("DATA_NTF"):
                # print("Valid data received, adding to buffer.")
                self.data_buffer.extend(data)
            else:
                print("Invalid data received, ignoring.")
                return

        # 終端文字「$」が見つかるまで、バッファを処理
        while b'$' in self.data_buffer:
            # 終端文字の前の部分を抽出
            end_index = self.data_buffer.index(b'$') + 1  # 終端文字の位置までを含める
            complete_data = self.data_buffer[:end_index]  # 完全なデータ（ヘッダ + 値 + 終端文字）

            # データを処理
            result = self.process_data(complete_data)

            if result:
                position_x, position_y, position_z, angle_x, angle_y, angle_z = result
                # 抽出結果をもとに描画を更新
                self.draw_footprint((position_x, position_y, position_z))
                self.update()

            else:
                print("Failed to process data.")

            # 使用したデータをバッファから削除
            self.data_buffer = self.data_buffer[end_index:]


    def process_data(self, data: bytearray):
        # バイト列を文字列に変換
        data_str = data.decode('utf-8').strip()
        # print(f"Received complete data: {data_str}")

        # 「DATA_NTF」ヘッダを削除し、残りの値を取り出す
        if data_str.startswith("DATA_NTF"):
            # 「DATA_NTF」を削除し、カンマで分割
            data_values = data_str[len("DATA_NTF"):].strip('$').split(',')

            # 空の要素を除去する
            data_values = [value for value in data_values if value.strip()]

            # それぞれの値をfloatに変換して変数に代入
            try:
                # リストが正しい長さかを確認
                if len(data_values) == 6:
                    position_x = float(data_values[0])
                    position_y = float(data_values[1])
                    position_z = float(data_values[2])
                    angle_x = float(data_values[3])
                    angle_y = float(data_values[4])
                    angle_z = float(data_values[5])

                    # 変換した値を出力
                    print(f"Position: ({position_x}, {position_y}, {position_z}), Angle: ({angle_x}, {angle_y}, {angle_z})")

                    # 呼び出し元の関数で使用するために、変数を返す
                    return position_x, position_y, position_z, angle_x, angle_y, angle_z
                else:
                    print(f"Unexpected number of data values: {len(data_values)}")
                    return None

            except ValueError as e:
                print(f"Error converting data to float: {e}")
                return None
        else:
            print("Invalid data format.")
            return None
    
    #足跡を描画
    def draw_footprint(self,position):
        if self.foot_print_counter == 0:
            self.mod_rad = math.atan2(position[0],position[1])-0.08
            self.mod_direction_angle = math.degrees(self.mod_rad)
        diff = math.sqrt(position[0]*position[0]+position[1]*position[1])
        rotate_diff = (0,diff)
        self.op_diff = (rotate_diff[0]*(-1.0),rotate_diff[1]*2)
        self.foot_number = self.foot_print_counter % 4 + 1
        self.foot_print_counter += 1
        current_dir = os.path.abspath(__file__)
        foot_print_img_path = os.path.dirname(current_dir)+"\\footprint.png"
        self.foot_print_img = QPixmap(foot_print_img_path)
        mtopix  = 20. #mからpixelへ変換
        foot_print_position = (self.initial_position[0]+rotate_diff[0]*mtopix,self.initial_position[1]+rotate_diff[1]*mtopix)

        # 足跡表示(4ストライド)
        if self.foot_number == 1:
            try:
                self.fpa.move(foot_print_position[0],foot_print_position[1])
            except:
                self.fpa = QLabel(self)
                self.fpa.setPixmap(self.foot_print_img)
                self.fpa.setGeometry(foot_print_position[0],foot_print_position[1], self.foot_size, self.foot_size)
                self.fpa.setAlignment(Qt.AlignCenter)
                self.fpa.setScaledContents(True)
                self.fpa.show()
        elif self.foot_number == 2:
            try:
                self.fpb.move(foot_print_position[0],foot_print_position[1])
            except:
                self.fpb = QLabel(self)
                self.fpb.setPixmap(self.foot_print_img)
                self.fpb.setGeometry(foot_print_position[0],foot_print_position[1], self.foot_size, self.foot_size)
                self.fpb.setAlignment(Qt.AlignCenter)
                self.fpb.setScaledContents(True)
                self.fpb.show()
        elif self.foot_number == 3:
            try:
                self.fpc.move(foot_print_position[0],foot_print_position[1])
            except:
                self.fpc = QLabel(self)
                self.fpc.setPixmap(self.foot_print_img)
                self.fpc.setGeometry(foot_print_position[0],foot_print_position[1], self.foot_size, self.foot_size)
                self.fpc.setAlignment(Qt.AlignCenter)
                self.fpc.setScaledContents(True)
                self.fpc.show()
        elif self.foot_number == 4:
            try:
                self.fpd.move(foot_print_position[0],foot_print_position[1])
            except:
                self.fpd = QLabel(self)
                self.fpd.setPixmap(self.foot_print_img)
                self.fpd.setGeometry(foot_print_position[0],foot_print_position[1], self.foot_size, self.foot_size)
                self.fpd.setAlignment(Qt.AlignCenter)
                self.fpd.setScaledContents(True)
                self.fpd.show()


class MainLabel(QLabel): #地図全体用ラベル
    map_appear = Signal()
    initial_click = Signal(tuple)
    mouse_move = Signal(tuple)
    second_click = Signal(tuple)
    def __init__(self,parent=None):
        super().__init__(parent)
        self.setMouseTracking(True)#マウスをトラッキング
        self.setAttribute(Qt.WA_Hover,True)
        self.timer = QTimer(self)
        self.timer.timeout.connect(self.update)  # タイマーがタイムアウトしたときに再描画
        self.timer.start(10)
        self.opacity = 0.0
        self.start_position = (0.0,0.0)
        self.current_position = (0.0,0.0)
        self.stop_position = (0.0,0.0)
        self.state = -1
        
    def paintEvent(self,event): #
        super().paintEvent(event)
        path_to_script = os.path.abspath(__file__)
        map_path = os.path.dirname(path_to_script)+"\\ink_map.png"
        image = QPixmap(map_path)
        painter = QPainter(self)
        painter.setCompositionMode(QPainter.CompositionMode_SourceOver)
        scaled_image = image.scaled(self.width(), self.height(),Qt.KeepAspectRatio,Qt.SmoothTransformation)
        painter.setOpacity(float(self.opacity)/100.)
        painter.drawPixmap((self.width()-scaled_image.size().width())/2, 0, scaled_image)
        self.opacity += 1.0
        painter.end()
        if self.opacity >= 101.0:
            if self.timer.isActive() == False:
                return
            self.timer.stop()
            self.map_appear.emit()
            self.state = 0

    def mousePressEvent(self,event):
        if self.state == 0:
            mouse_pos = event.pos()
            self.start_position = (mouse_pos.x(),mouse_pos.y())
            self.state = 1
            self.initial_click.emit(self.start_position)
            #print("First Click")
        elif self.state == 1:
            mouse_pos = event.pos()
            self.end_position = (mouse_pos.x(),mouse_pos.y())
            self.state = 2
            self.second_click.emit(self.end_position)
            #print("Second Click")

    def mouseMoveEvent(self,event):
        #print("mousemove")
        if self.state != 1:
            return False
        if self.state == 1:
            mouse_pos = event.position()
            self.current_position = (mouse_pos.x(),mouse_pos.y())
            #print(self.current_position)
            self.mouse_move.emit(self.current_position)

class PopupWindow(QWidget): #呪文入力ポップアップ
    eventTriggered = Signal()

    def __init__(self, parent=None):
        super().__init__(parent)
        self.setWindowTitle("Spell?")
        screen = QApplication.primaryScreen()
        screen_geometry = screen.geometry()
        center_point = screen_geometry.center()
        self.setGeometry(100, 100, 400, 80)
        window_geometry = self.frameGeometry()
        # ウィンドウを画面中央に移動
        #print(center_point)
        window_geometry.moveCenter(center_point)
        self.move(window_geometry.topLeft().x(),window_geometry.topLeft().y()+200)

        # 常に最前面に表示するフラグを設定
        self.setWindowFlag(Qt.WindowType.WindowStaysOnTopHint, True)

        #テキストフィールド
        self.text_input = QLineEdit(self)
        self.text_input.setAlignment(Qt.AlignCenter)
        self.text_input.setGeometry(10,10,380,25)

        #ボタン
        self.input_button = QPushButton("send",self)

        self.input_button.clicked.connect(self.check_label) #入力チェックへ
        self.input_button.setGeometry(10,45,380,25)

    def check_label(self): #入力チェック
        if self.text_input.text() == "われ、よからぬことをたくらむ者なり":
            self.eventTriggered.emit()
            self.hide()
        elif self.text_input.text() == "I solemnly swear that I am up to no good":
            self.eventTriggered.emit()
            self.hide()
        #いたずら完了
        #Mischief managed!

# 通知を受け取るハンドラ
def notification_handler(sender, data: bytearray):
    communicator.update_label_signal.emit(data)  # シグナルを発火

# メイン処理
async def BLE_loop(BLE_ctrl_que, BLE_evt_ques):
    while True:
        try:
            async with BleakClient(DEV_MAC_ADDRESS) as client:
                # 接続確認
                connected = await client.is_connected()
                print("Connected:", connected)
                if not connected:
                    print("Failed to connect to the device.")
                    break

                # 通知を有効化
                try:
                    await client.start_notify(UUID_NOTIFY, notification_handler)
                    print("Notification started.")
                except Exception as e:
                    print(f"Failed to start notification: {e}")
                    break

                BLE_evt_que.put(True) # BLE接続完了イベントをメインスレッドに通知

                # BLEイベントループ
                while True:
                    await asyncio.sleep(0.1) # BLE受信から描画までの、レイテンシに関わる部分

                    # 接続状態の確認
                    connected = await client.is_connected()
                    if not connected:
                        print("disconnected.")
                        BLE_evt_que.put(False) # 切断イベントをメインスレッドに通知
                        break

                    # メインスレッドからのコマンド送信指示確認
                    if BLE_ctrl_que.empty() == False:
                        if BLE_ctrl_que.get() == True:
                            print("send START_REQ")
                            ###ここでwriteしたい：START_CMD
                            await client.write_gatt_char(UUID_WRITE, START_CMD)  # START_REQコマンドを送信
                            print("START_REQ sent.")
                        else:
                            print("send STOP_REQ")
                            ###ここでwriteしたい：STOP_CMD
                            await client.write_gatt_char(UUID_WRITE, STOP_CMD)  # STOP_REQコマンドを送信
                            print("STOP_REQ sent.")

                # 通知を停止
                await client.stop_notify(UUID_NOTIFY)
                print("Notification stopped.")
        except Exception as e:
            print(f"An error occurred: {e}")

def Proc_BLE_thread(BLE_ctrl_que, BLE_evt_que):
    asyncio.run(BLE_loop(BLE_ctrl_que, BLE_evt_que)) #別スレッドでBLE処理ループを回す

if __name__ == "__main__":
    # 環境変数にPySide6を登録
    dirname = os.path.dirname(PySide6.__file__)
    plugin_path = os.path.join(dirname, 'plugins', 'platforms')
    os.environ['QT_QPA_PLATFORM_PLUGIN_PATH'] = plugin_path
    app = QApplication(sys.argv)    # PySide6の実行
    window = MainWindow()           # ユーザがコーディングしたクラス
    window.show()         # PySide6のウィンドウを表示
    #window.showFullScreen()         # PySide6のウィンドウを表示

    # シグナルを作成
    communicator = Communicator()
    communicator.update_label_signal.connect(window.catchBLE)

    # 非同期処理用スレッドの作成
    BLE_ctrl_que = queue.Queue() # BLE制御用キュー（メインスレッド -> BLEスレッド）
    BLE_evt_que = queue.Queue()  # BLEイベント用キュー（BLEスレッド -> メインスレッド）
    BLE_thread = threading.Thread(target=Proc_BLE_thread, args=(BLE_ctrl_que, BLE_evt_que), daemon=True)
    BLE_thread.start()

    sys.exit(app.exec())            # PySide6の終了

4.ライセンス

本アプリはPysideを用いているため、ライセンスはLGPL2.1です。

5.改善点

ジャイロのドリフト誤差について

今回実際に計測を行った結果、位置よりも角度の方がドリフト誤差が大きく乗るような印象を受けました。
今回は実際の進行方向との角度差分が大きくなってしまったため、やむなく進行方向を限定することとなってしまいましたが、
まもなく発売されるSpresense用のIMU add onボードでリベンジしたいと考えています。
https://www.switch-science.com/products/10181?srsltid=AfmBOopYV03zSuhVSbN7J9IQwqx6kJ3xyr8SIO8syQQH8-zlL6qof_yV

IMUセンサを用いることによって限定的ではあるが忍びの地図の機能を再現することができました。
ジャイロのドリフトの補正がしきれなかったのは残念ですが、接地の検出は1歩も漏らさずに検出できたため、実際にヒトが歩いている臨場感のある製品にできたと思っています。
将来的にはAHRSなどの回転方向に強いIMUを用いることでより自由な移動経路の算出や異なる補正の仕方を用いてより頑健な位置推定を行っていきたいと思っています。

おわりに

どうだった？

これがあれば、オフィスで迷子になる心配も、誰かを探し回るムダな時間も、全部なくなっちゃう！
誰かに見つからないようにすることもできるし、最高だろ？

おっと、使ってることはばれないほうがいいからな他言は無用だぜ、使い終わりの呪文はこうだ

「いたずら完了！」

コメント無し