1.目的
- 屋内のためGPSの使用が難しい食堂や工場内の搬送台車の位置をpreMSMを用いて推定するための方法検討として、プラレールを使って、曲がった角度と方向を計算する検討を行った（列車はどこ? プラレールで9軸センサ（準備編））。準備編では、ファイルに落とした9軸センサのデータをもとに曲がった角度の計算を行った。
- 今回は、リアルタイムに9軸センサのデータから曲がった角度と方向を計算し、コースのどこにいるのかを求め、画面に表示する環境を作成する。
2.プラレールを使った位置推定の検討(ポジションの推定)
- 下記 図1に示すプラレールのコースにpreMSMを搭載した貨車を走らせ、9軸センサーの出力からリアルタイムに位置推定を実施する環境を作った。
- 図2に、推定を実施した結果を示す。図1の番号の位置を順番に走っているのが分かる。
- 位置推定のアルゴリズムは、過去の角を曲がった情報を記憶しておき、どのパターンに当てはまるかで、場所を特定している。一致しない状況が発生した場合は、”0”を出力し、再度位置特定を開始する。表1参照。
※ BMX160: 50ms周期で検出（10msだと、データが飛ぶことがあり）
図1.位置推定に使用するプラレールのコース
表1で、1つ前の角の情報が、CW180であった場合に、現在地が①か③かの区別がつかない。CW90であった場合には現在地は②と確定でき、ACW90であった場合には現在は④と推定できる。もう一つ角を曲がると現在地が②か、④で確定できるので、その前が、①か③かが分かる。
表1. 位置推定のための表
3.プラレールを使った位置推定の検討(位置のリアルタイム表示)
- P3の位置推定アルゴリズム(Python3)の位置情報を、MQTTでprocessingに渡し、描画をさせた。
- 図3に、ソフトウェアの構成とデータの流れをまとめる。
- 図4に、列車の位置表示の画面と位置推定プログラムの出力の対比を載せる。プログラムからポジション②が選ばれ、それが表示画面に表示されている。
- 図5にプラレールの貨車とPCの位置情報の動画を載せる。角を曲がると直線部のどこに貨車がいるかがリアルタイムで表示されていることが分かる。
- 図6は、姿勢表示と同時に表示させた場合である。
図4. 位置推定プログラムの結果と位置表示画面の対比
Appendix プログラムのリスト
A-1. 位置推定プログラム MQTT対応版(Python3)
# -*- 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
# 2022/01/28 ver 0.03 position detection trial no.1 Pla-rail 1
#
import sys
import csv
import time
import paho.mqtt.client as mqtt
import paho.mqtt.publish as publish
SamplingTime = 50 # 50ms
Aw = 30 # 角と認識するための角度の幅 90-Aw < 角 < 90+Aw
# サンプリング時間[s]
t = SamplingTime * 10**-3
# 角速度のしきい値：この値以上で角処理を行う。
thr_gyrz = 5.0
# 変数定義
i = 0 # 読み出し行数
# 角度
aglx = 0.0
agly = 0.0
aglz = 0.0
init_aglz = 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
j = 0
agl_dim = "zzzz"
agl_num = 0
# メイン処理
def main():
# MQTT の接続
client = mqtt.Client()
client.on_connect = on_connect # 接続時のコールバック関数を登録
client.on_disconnect = on_disconnect # 切断時のコールバックを登録
client.on_message = on_message # メッセージ到着時のコールバック
client.connect("localhost", 1883, 60)
client.loop_forever() # 永久ループして待ち続ける
# ブローカーに接続できたときの処理
def on_connect(client, userdata, flags, rc):
print("Connected with result code "+str(rc))
client.subscribe("/position")
# ブローカーが切断したときの処理
def on_disconnect(client, userdata, flag, rc):
if rc != 0:
print("Unexpected disconnection.")
# メッセージが届いたときの処理
def on_message(client, userdata, msg):
# 変数定義
#グローバル変数
global j
global i # 読み出し行数
global aglx # 角度
global agly # 角度
global aglz # 角度
global init_aglz # 角速度上昇開始時の角速度
global gyrz_lst # 移動平均用配列
global gyrz_ave # 移動平均
global isSwing # 角速度変動か
global agl_dim
global agl_num
#ローカル変数
gyrz = 0.0 # ジャイロ
diff_aglz = 0.0 # 角速度減少時と上昇開始時の差分値
agl_size = 0.0 # 曲がった角の大きさ
isTurn = False # 角か?
# 行の読み込み
lines = str(msg.payload).replace('\'','')
lines = lines.replace('b','')
l = lines.split(',')
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:
pos, agl_dim = pos_det(agl_size, agl_dim, agl_num)
agl_num = agl_num + 1 if agl_num < 4 else agl_num
print(f'### Turn!! angle= {agl_size:4}deg t={t*i:.2f}s \
Position is {pos} ###')
publish.single("/position2", str(pos), hostname="localhost")
if pos == 0: # 位置を見失ったときの初期化処理
agl_dim = "zzzz"
agl_num = 0
isTurn = False
i += 1
# publishが完了したときの処理
def on_publish(client, userdata, mid):
print("publish: {0}".format(mid))
# 位置検出
# cw90=a, acw90=b, cw180=c, acw180=d, cw270=e, acw270=f, cw360=g, acw360=h
def pos_det(size, dim, num):
n = 4
ch = "z"
pos = 0 # 0:発見不可、位置番号は、1から開始
route = ["cbca","acbc","cacb","bcac"] # 位置情報
# 角度を記号に変える
ch = agl2chr(size)
# 位置情報をシフト
dim = shift_dim(dim, ch, n)
# 位置情報照合
pos = chk_route(route,dim,num,n)
return pos, dim
# 位置情報照合
def chk_route(route, dim, num, n):
if num < n-1:
for i in range(n):
if route[i].startswith(dim[0:num+1]):
return i+1 # 見つかった
return 0 # 見つからなかった
else:
for i in range(n):
if route[i].startswith(dim):
return i+1 # 見つかった
return 0 # 見つからなかった
# 位置情報をシフト
def shift_dim(dim, ch, n):
dim=ch+dim[0:n-1]
return dim
# 角度を記号に変える
# cw90=a, acw90=b, cw180=c, acw180=d, cw270=e, acw270=f, cw360=g, acw360=h
def agl2chr(size):
ch = "z"
if size == -90: # cw90
ch = "a"
elif size == 90: # acw90
ch = "b"
elif size == -180: # cw180
ch = "c"
elif size == 180: # acw180
ch = "d"
elif size == -270: # cw270
ch = "e"
elif size == 270: # acw270
ch = "f"
elif size == -360: # cw360
ch = "g"
elif size == 360: # acw360
ch = "h"
return ch
# 移動平均
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()
A-2. 位置表示プログラム pra_rail.pde (processing)
import mqtt.*; // added for MQTT
int x,y,s,i;
MQTTClient client; // added for MQTT
void setup(){
client = new MQTTClient(this); // added for MQTT
client.connect("mqtt://192.168.11.43:1883"); // added for MQTT
size(600,600);
x = y = 50;
s = 100;
i = 0;
background(200);
}
void draw(){
standby();
if (i==1){
position1();
} else if (i==2){
position2();
} else if (i==3){
position3();
} else if (i==4){
position4();
}
}
// added for MQTT
void clientConnected() {
println("client connected");
client.subscribe("/position2");
}
// added for MQTT
void messageReceived(String topic, byte[] payload) {
String myString = new String( payload );
if (myString != null) {
myString = trim(myString);//trimで空白消去
println(myString);
i=int(myString);
//String sensors[] = split(myString, ',');// ,cut output
//if (sensors[0] == "Pos=") {
// i = int(sensors[1]);
// println("i=",i);
//}
}
}
// added for MQTT
void connectionLost() {
println("connection lost");
}
void standby(){
background(200);
fill(200);
stroke(56,93,138);
strokeWeight(20);
line(x+s,y+5*s,x+4*s,y+5*s); // line (1)
line(x,y+s,x,y+4*s); // line (2)
line(x+2*s,y+1*s,x+2*s,y+2*s); // line (3)
line(x+3*s,y+3*s,x+4*s,y+3*s); // line (4)
arc(x+s,y+s,2*s,2*s,PI,2*PI);
arc(x+s,y+4*s,2*s,2*s,PI/2,PI);
arc(x+3*s,y+2*s,2*s,2*s,PI/2,PI);
arc(x+4*s,y+4*s,2*s,2*s,-1*PI/2,PI/2);
}
void position1(){
background(200);
fill(200);
strokeWeight(20);
stroke(56,93,138);
line(x+s,y+5*s,x+4*s,y+5*s); // line (1)
line(x,y+s,x,y+4*s); // line (2)
line(x+2*s,y+1*s,x+2*s,y+2*s); // line (3)
line(x+3*s,y+3*s,x+4*s,y+3*s); // line (4)
arc(x+s,y+s,2*s,2*s,PI,2*PI);
arc(x+s,y+4*s,2*s,2*s,PI/2,PI);
arc(x+3*s,y+2*s,2*s,2*s,PI/2,PI);
arc(x+4*s,y+4*s,2*s,2*s,-1*PI/2,PI/2);
stroke(192,80,77);
line(x+s,y+5*s,x+4*s,y+5*s); // line (1)
}
void position2(){
background(200);
fill(200);
strokeWeight(20);
stroke(56,93,138);
line(x+s,y+5*s,x+4*s,y+5*s); // line (1)
line(x,y+s,x,y+4*s); // line (2)
line(x+2*s,y+1*s,x+2*s,y+2*s); // line (3)
line(x+3*s,y+3*s,x+4*s,y+3*s); // line (4)
arc(x+s,y+s,2*s,2*s,PI,2*PI);
arc(x+s,y+4*s,2*s,2*s,PI/2,PI);
arc(x+3*s,y+2*s,2*s,2*s,PI/2,PI);
arc(x+4*s,y+4*s,2*s,2*s,-1*PI/2,PI/2);
stroke(192,80,77);
line(x,y+s,x,y+4*s); // line (2)
}
void position3(){
background(200);
fill(200);
strokeWeight(20);
stroke(56,93,138);
line(x+s,y+5*s,x+4*s,y+5*s); // line (1)
line(x,y+s,x,y+4*s); // line (2)
line(x+2*s,y+1*s,x+2*s,y+2*s); // line (3)
line(x+3*s,y+3*s,x+4*s,y+3*s); // line (4)
arc(x+s,y+s,2*s,2*s,PI,2*PI);
arc(x+s,y+4*s,2*s,2*s,PI/2,PI);
arc(x+3*s,y+2*s,2*s,2*s,PI/2,PI);
arc(x+4*s,y+4*s,2*s,2*s,-1*PI/2,PI/2);
stroke(192,80,77);
line(x+2*s,y+1*s,x+2*s,y+2*s); // line (3)
}
void position4(){
background(200);
fill(200);
strokeWeight(20);
stroke(56,93,138);
line(x+s,y+5*s,x+4*s,y+5*s); // line (1)
line(x,y+s,x,y+4*s); // line (2)
line(x+2*s,y+1*s,x+2*s,y+2*s); // line (3)
line(x+3*s,y+3*s,x+4*s,y+3*s); // line (4)
arc(x+s,y+s,2*s,2*s,PI,2*PI);
arc(x+s,y+4*s,2*s,2*s,PI/2,PI);
arc(x+3*s,y+2*s,2*s,2*s,PI/2,PI);
arc(x+4*s,y+4*s,2*s,2*s,-1*PI/2,PI/2);
stroke(192,80,77);
line(x+3*s,y+3*s,x+4*s,y+3*s); // line (4)
}
