1.目的
- 屋内のためGPSの使用が難しい食堂や工場内の搬送台車の位置をpreMSMを用いて推定したい。そのため、preMSMを搬送台車に取り付け、preMSMに搭載している9軸センサBMX160のデータを取得することで搬送台車が角を曲がったことを検知し、あらかじめ決まったコースを周回する搬送台車の位置推定が出来ないかを今後検討する。
- そのための基礎実験として、プラレールを使って、角速度とオイラー角(ヨー角)の変化を調べてみて、角速度とヨー角から角を曲がったことがわかるかどうかを調べてみる。
2. 実験内容
- 図1に示すようなコースをプラレールで組み立て、preMSMを搭載した列車を走らせる。
- 9軸センサからのデータをもとに、オイラー角を計算する。オイラー角の計算に関しては、下記、記事を参照。
「ロール、ピッチ、ヨー!! 9軸センサを使ったオイラー角の算出と表示」 - 角速度とオイラー角(ヨー角)は、ともにZ軸の情報を使用する。角速度は、時間で積分し角度にしてヨー角と比較する。ヨー角は、スタート地点が０°となるようにスタート地点のデータで補正をかける。
図1. プラレールのコース
3. 結果
- プラレールの列車に取り付けたpreMSMから送られる角速度から算出した角度を使ってどこを走っているかの推定は可能と考える。ただし、あらかじめコースのデータを取得しておく必要がある。そのデータと実際のデータを比較することで位置を推定する。
- 図1は、プラレールのコース図。①から④の4つのコーナーを曲がって列車は走る。図2は、Z軸の角速度を積分した角度とヨー角のプロット図。
- preMSMのデータは10msサンプリング。
- 図2の角速度の積分値(青線)から、コーナーで列車が曲がるときに角度が変化し、直進の時には角度が変化しないことが分かる。ヨー角(橙線)も、同じように変化するが、直進時にフラットにならず反転するような値を取る。角度はなぜか半分の値になっている。
- 時計回りの時には角度が減り、反時計回りの時には角度が増える（preMSMの取り付け方で変わる）
図2 Z軸角度データのプロット図(4周分)
（青:角速度の積分値、橙:ヨー角）
Appendix-1 角度計算プログラム
図2を作成するために角速度から角度を計算したプログラムを以下に示す。
ファイルに記録した角速度を積分しただけのプログラム。ヨー角は初期値を0にするために、スタート時点の数値を引き算している。
# -*- coding: utf-8 -*-
#
# 9 axis motion sensor data read and position calculation program
# 2021/12/24 ver 0.01
#
#
import sys
import csv
import time
# メイン処理
def main():
csv_file = open(sys.argv[1], "r", encoding="utf-8")
i = 0 # 読み出し回数
# 角度
aglx = 0.0
agly = 0.0
aglz = 0.0
# 地軸
magz = 0.0
magz0 = 0.0
# Yaw
yaw = 0.0
yaw0 = 0.0
# サンプリング時間[s]
t = 10 * 10**-3 # 10ms
f = csv.reader(csv_file)
for l in f:
if l[0] == '1':
agly = float(l[2]) * t + agly
aglz = float(l[3]) * t + aglz
if l[0] == '3':
if i == 0:
magz0 = float(l[3])
else :
magz = float(l[3]) - magz0
if l[0] == '4':
if i == 0:
yaw = float(l[3])
else :
yaw = float(l[3]) - yaw0
print("aglz=,", aglz, ",magz=,", magz, ",agly=,", agly,",yaw=,",yaw)
i += 1
# 終了処理
csv_file.close()
if __name__ == "__main__":
main()
