toppan_sawatome が 2025年01月30日11時54分21秒 に編集
コメント無し
本文の変更
# はじめに  「われ、よからぬことをたくらむ者なり」 この紙はただの紙じゃない。忍びの地図というものなんだ。 魔法をかけた相手の場所が文字通り手にとるようにわかる魔法の道具さ。 実際の使い方が知りたいって？下の画面を見てくれ！ [動作の動画] # 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.インソールとIMUモジュールを靴の中に挿入 直接踏むと足が痛い＆モジュールに体重がかかってしまうため、インソールをいれることで踵とセンサ間にかかる力を軽減しています。  # 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側 ```arduino:歩幅推定 #define LED_PIN 13 void setup() { pinMode(LED_PIN, OUTPUT); } void loop() { digitalWrite(LED_PIN, HIGH); delay(1000); digitalWrite(LED_PIN, LOW); delay(1000); } ``` ## UI側
```arduino:UIのコード(大きくなるかも) #define LED_PIN 13
void setup() { pinMode(LED_PIN, OUTPUT); }
```python # -*- 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
void loop() { digitalWrite(LED_PIN, HIGH); delay(1000); digitalWrite(LED_PIN, LOW); delay(1000); }
global communicator DEV_MAC_ADDRESS = "XX:XX:XX:XX:XX:XX" # 接続先のBLEのMACアドレス UUID_NOTIFY = "XXXXXXXX-XXXX-XXXX-XXXX-XXXXXXXXXXXX" #接続先のUUID UUID_WRITE = "XXXXXXXX-XXXX-XXXX-XXXX-XXXXXXXXXXXX" #接続先のUUID(基本上と同じ) 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_prints = {} #ウィンドウの位置とサイズの変更 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送信 #TODO: タイミングはボタン押下時に変更する # 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 #TODO: 抽出結果をもとに描画を更新 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を用いることでより自由な移動経路の算出や異なる補正の仕方を用いてより頑健な位置推定を行っていきたいと思っています。 # おわりに  どうだった？ これがあれば、オフィスで迷子になる心配も、誰かを探し回るムダな時間も、全部なくなっちゃう！ 誰かに見つからないようにすることもできるし、最高だろ？ おっと、使ってることはばれないほうがいいからな他言は無用だぜ、使い終わりの呪文はこうだ 「いたずら完了！」