1.目的
- preMSMに搭載されている9軸センサBMX160のZ軸角速度を積分することで、曲がった方向と角度が分かることが、「列車はどこ!?プラレールで9軸センサ（準備編）」で確認できた。
- 最終的には、屋内のためGPSの使用が難しい食堂や工場内の搬送台車の位置をpreMSMを用いて推定したい。そこで、preMSMを手押しの台車に搭載して、動かしその時の9軸センサの出力から、台車の動きを推定する実験を行い、位置推定の準備実験とする。
2. 移動推定のための検討内容
- 移動推定のために、どちら方向に角を何度曲がったかを検出する。そして、曲がった角の方向と回数から場所を推定する。
- そのためには、今の状態を知る必要がある。直線運動中か、回転運動中なのか。
- 直線運動中: |角加速度|≦5、回転運動中: |角加速度|＞5 ※ 値は要検討
- 直線運動中から、回転運動中になり、直線運動に戻った時点での、角度の変化をみて、どちらの方向に何度曲がったのかを判定する。
- preMSMを上向きに置いた場合のZ軸の角速度は、時計回りがマイナスの値を示す。
- preMSMの9軸センサの出力周期は、100msとする。
- 上記のアルゴリズムを角判定プログラムとしてPython3作成する(whereAmI_100ms.py：Appendix参照)。
3. 実験結果
以下に、3パターンの手押し台車の移動を行い、どちらの方向に何度曲がったのかを上記で作成したプログラムで判定し、その結果から、移動を推定した。
推定結果は、台車走行イメージとして示す。
3パターンとも、移動推定がうまくいき、移動のイメージを作成することができた。短距離移動では、360°の回転を判定することができている。また、長距離移動(2)では、初めの複雑な移動もうまく判定できた。
3-1. 短距離移動
- プログラムでの判定結果
### Turn!! angle= 180 deg t= 18.6 s ###
### Turn!! angle= -360 deg t= 44.5 s ###
### Turn!! angle= -360 deg t= 67.7 s ###
図1. 短距離移動の判定結果
3-2. 長距離移動(1)
- プログラムでの判定結果
### Turn!! angle= 90 deg t= 8.5 s ###
### Turn!! angle= -180 deg t= 65.5 s ###
### Turn!! angle= 90 deg t= 111.8 s ###
### Turn!! angle= -90 deg t= 124.9 s ###
図2. 長距離移動(1)の判定結果
3-3. 長距離移動(2)
- プログラムでの判定結果
### Turn!! angle= 180 deg t= 32.2 s ###
### Turn!! angle= 90 deg t= 37.6 s ###
### Turn!! angle= -90 deg t= 49.6 s ###
### Turn!! angle= -180 deg t= 59.4 s ###
### Turn!! angle= 90 deg t= 66.6 s ###
### Turn!! angle= -180 deg t= 103.4 s ###
### Turn!! angle= 90 deg t= 127.8 s ###
図3. 長距離移動(2)の判定結果
Appendix：角判定プログラム
# -*- coding: utf-8 -*-
#
# 9 axis motion sensor data read and position calculation program
# 2021/12/24 ver 0.01
# 2022/01/28 ver 0.02 calculate turn angle and direction
#
import sys
import csv
import time
SamplingTime = 100 # 100ms
Aw = 30 # 角と認識するための角度の幅 90-Aw < 角 < 90+Aw
# メイン処理
def main():
# サンプリング時間[s]
t = SamplingTime * 10**-3
# 角速度のしきい値：この値以上で角処理を行う。
thr_gyrz = 5.0
# ファイルのオープン：ファイル名は引数で与える
csv_file = open(sys.argv[1], "r", encoding="utf-8")
# 変数定義
i = 0 # 読み出し行数
# ジャイロ
gyrz = 0.0
# 角度
aglx = 0.0
agly = 0.0
aglz = 0.0
init_aglz = 0.0 # 角速度上昇開始時の角速度
diff_aglz = 0.0 # 角速度減少時と上昇開始時の差分値
agl_size = 0.0 # 曲がった角の大きさ
gyrz_lst = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
gyrz_ave = 0.0
# 角速度変動か
isSwing = False
# 角か?
isTurn = False
# 行の読み込み
f = csv.reader(csv_file)
for l in f:
if l[0] == '1':
gyrz = float(l[3])
gyrz_ave, gyrz_lst = move_ave(gyrz_lst, gyrz) # 移動平均
# 角速度の積分実施
aglx = float(l[1]) * t + aglx
agly = float(l[2]) * t + agly
aglz = gyrz * t + aglz
# 角速度の変動検出
if abs(gyrz) >= thr_gyrz and not isSwing: # 角速度上昇
init_aglz = aglz
isSwing = True
if abs(gyrz_ave) < thr_gyrz and \
abs(gyrz) < thr_gyrz and isSwing: # 角速度減少
diff_aglz = aglz - init_aglz
isSwing = False
isTurn, agl_size = IsTurn(diff_aglz*2) # 角の判定
if isTurn:
print("### Turn!! angle=",agl_size,"deg t=",t*i,"s ###")
isTurn = False
i += 1
time.sleep(t/100)
# 終了処理
csv_file.close()
# 移動平均
def move_ave(lst, dt):
ttl = dt
num = 10
for i in range(1,num):
lst[num-i] = lst[num-i-1]
ttl = ttl + lst[num-i-1]
lst[0] = dt
return ttl/num, lst
# 角の判定
def IsTurn(diff_aglz):
isTurn = False
agl_size = 0.0
if abs(diff_aglz) > 90-Aw:
isTurn = True
agl_size = AngleSize(diff_aglz)
return isTurn, agl_size
# 回転角度の判定
def AngleSize(angle):
sign = 1 if angle > 0 else -1
angle = abs(angle)
if angle < 90+Aw:
angle = 90 * sign
elif angle > 180-Aw and angle < 180+Aw:
angle = 90 * 2 * sign
elif angle > 270-Aw and angle < 270+Aw:
angle = 90 * 3 * sign
elif angle > 360-Aw and angle < 360+Aw:
angle = 90 * 4 * sign
else:
angle = angle * sign
return angle
if __name__ == "__main__":
main()
