ラズパイと超音波センサで物体の追跡
この記事では、Raspberry Piを使って超音波センサとサーボモータを組み合わせた物体の追跡方法を解説していく。超音波センサはHC-SR04、サーボモータはSM-S2309Sを使用した。
はじめに
この記事でやることは「超音波センサモジュール1つだけで物体を追跡させる」ことである。
超音波センサモジュール1つで、物体を追跡させるには少し工夫しなければならない。なぜなら、人間にたとえると「片耳だけ」で方向を探知するようなものだからだ。
超音波センサモジュールを見ると、2つの目があるように見えるが実は「1つ目」である。というか「口と耳」なのだ。1つは超音波を発する「口」、そしてもう1つは自分が発した超音波を受信する「耳」である。
ちなみに、Arduinoで超音波センサHC-SR04を使いたい場合は、こちらの記事を参考に。
ところで、超音波センサーを使った、かわいらしいロボット達がお手頃な値段でAmazonに売られている。センサーモジュールの配線などは、慣れるまでなかなか敷居が高いかもしれない。そこでとにかく動かしてみたい方や、プログラミングに集中したい方は、プログラミングロボット製品を使うと良いと思う。この手のロボットで十分遊んだ後なら、自分のオリジナルロボットを作れるようになるだろう。
超音波で距離がわかる仕組み
まずは超音波で距離が測定できる仕組みを説明しよう。
超音波で物体の距離を測れる仕組みは「やまびこ」のイメージだ。音を発してから測定物に当たって、跳ね返ってくるまでの時間を測ることで距離がわかるのだ。
実は、1気圧の空気での音速は気温によって決まっていて、たとえば、気温20°のとき約343m/sになることが知られている。速度と時間がわかれば中学生で習った、速度 x 時間で距離が導き出せるはずだ。今回は音速を343m/sとして計算していく。
超音波から距離を計算する方法
こちらが音波の速度から距離を計算する式となる。
$$ Distance = \frac{T}{2} \times 343 $$
「音を発信してから受信するまでの時間」をT秒としたので、片道の時間はそれを2で割った値、つまりT/2秒である。
HC-SR04で距離を測定してみよう
それでは実際にHC-SR04を使って、距離を測定してみよう。
HC-SR04とラズパイの配線
| HC-SR04 | ラズパイ |
|---|---|
| Vcc | 5V |
| Trig | GPIO20 |
| Echo | 電圧レベル変換後GPIO21⭐︎ |
| GND | GND |
Echoピンの電圧レベル変換⭐︎
HC-SR04のEchoからは5Vの信号が出力される。しかし、ラズパイのGPIOの入力電圧は3.3Vまで。そのため直接繋ぐことはできない。今回は、3.3Vのツェナーダイオードを使って5Vを3.3Vへ変換させることにした。
他にも抵抗で分圧する方法がある。
HC-SR04で往復時間を計測するには？
今回使う超音波センサHC-SR04は、Triggerピンを一瞬Highにすれば超音波の発信から受信までをセンサが自動でやってくれて、その間だけEchoがHighの状態になる仕組みだ。つまりEchoがHighの状態の時間を測ることで、往復にかかった時間を知ることができる。
参照:
距離測定のプログラム
実際にHC-SR04で距離の測定を行ったプログラムがこちら。
# -*- coding: utf-8 -*-
# Created by Toshihiko Arai.
# https://101010.fun/iot/raspberry-pi-sonic-radar.html
# python2で実行すること
import RPi.GPIO as GPIO
import time
import os
import signal
GPIO.setmode(GPIO.BCM)
TRIG = 20
ECHO = 21
C = 343 # 気温20度の時の音速(m/s)
GPIO.setup(TRIG, GPIO.OUT)
GPIO.setup(ECHO, GPIO.IN)
GPIO.output(TRIG, 0)
time.sleep(0.3)
def readDistance():
GPIO.output(TRIG, 1)
time.sleep(0.00001) # 10μs
GPIO.output(TRIG, 0)
while GPIO.input(ECHO) == 0:
signaloff = time.time()
while GPIO.input(ECHO) == 1:
signalon = time.time()
t = signalon - signaloff
distance = t * C * 100 / 2
return distance
def cleanup():
print('cleanup')
GPIO.cleanup()
try:
while True:
dist = readDistance()
print(dist)
time.sleep(0.1)
except KeyboardInterrupt:
# SIGINTを監視していれば不要
print('KeyboardInterrupt')
except:
print('other')
finally:
# 終了処理
cleanup()
サーボモータの動かし方
物体の動きに合わせて首を振るようにしたいので、ここではサーボモータの使い方を説明する。何かのキットに付いていたサーボモータSM-S2309Sを使ったが、他のサーボモータでも問題ないと思う。
サーボーモータの動かし方のポイントは次の2つだ。
- PWM信号（50Hz程度）を、SIGピンへ送信する。
- PWM信号のデューティー比によって、角度が決められる。
サーボモータとラズパイの配線
| ラズパイ | サーボモータ |
|---|---|
| GND | GND |
| GPIO26 | Signal |
| ラズパイ以外の5V電源 | 5V |
サーボモータの電源はラズパイと別にしよう
サーボモータの電源はラズパイから拾うのではなく、別電源を用意する。なぜならモータの消費電力は大きいので、動作が不安定になるからだ。
サーボモータについて詳しく解説したのでこちらも参考に
サーボモータの動作テストプログラム
これを元に、サーボモータの動作テストプログラムをPythonで書いてみた。
# -*- coding: utf-8 -*-
# Created by Toshihiko Arai.
# https://101010.fun/iot/raspberry-pi-sonic-radar.html
# python2で実行すること
import RPi.GPIO as GPIO
import time
import os
import signal
GPIO.setmode(GPIO.BCM)
SIG = 26
GPIO.setup(SIG, GPIO.OUT)
# PWMサイクル:20ms(=50Hz)
servo = GPIO.PWM(SIG, 50)
time.sleep(0.3)
servo.start(0)
servo.ChangeDutyCycle(6.3) # 0°
time.sleep(1.0)
for i in range(5):
servo.ChangeDutyCycle(2.2) # 0°
time.sleep(1.0)
servo.ChangeDutyCycle(10.8) # 180°
time.sleep(1.0)
servo.ChangeDutyCycle(6.3) # 90°
time.sleep(1.0)
servo.stop()
GPIO.cleanup()
超音波センサ1つで物体を追跡する
それでは本題の「超音波センサ1つで物体を追跡」をやってみよう。
1つの超音波センサモジュールだけで、物体の追跡をさせる方法は次の通り。
- 物体を見つけたら常にその右端または左端を狙うようにする
- ごくわずかの角度だけ常に首を振って「見つけた」「見つけていない」を高速で繰り返す
もし物体が移動して見失ってしまったら、すぐに見つけられるよう首を振るスピードを速める。つまりサーボモータの角速度を大きくする。こうすることにより、物体の端を捉えることができるはずだ。
物体を追跡するプログラム
これらの考えを元に、組んだPythonプログラムがこちらである。条件分岐が多く読みにくいプログラムとなってしまったが悪しからず。
# -*- coding: utf-8 -*-
# Created by Toshihiko Arai.
# https://101010.fun/iot/raspberry-pi-sonic-radar.html
# このプログラムはpython2で実行する
import RPi.GPIO as GPIO
import time
import os
import signal
GPIO.setmode(GPIO.BCM) # 役割ピン番号で命名
TRIG = 20
ECHO = 21
SIG = 26
SERVO_PWM_MIN = 2.2 # 0° 時計の針で9時を0°とする
SERVO_PWM_MAX = 10.8 # 180°
SERVO_PWM_1DEGREE = (SERVO_PWM_MAX - SERVO_PWM_MIN) / 180
MONITORING_DIST = 20 # 最短距離
C = 343 # 気温20度での音速(m/s)
GPIO.setup(TRIG, GPIO.OUT)
GPIO.setup(ECHO, GPIO.IN)
GPIO.setup(SIG, GPIO.OUT)
GPIO.output(TRIG, 0)
servo = GPIO.PWM(SIG, 50)
time.sleep(0.3)
servo.start(0)
def readDistance():
GPIO.output(TRIG, 1)
time.sleep(0.00001) # 10μs
GPIO.output(TRIG, 0)
while GPIO.input(ECHO) == 0:
signaloff = time.time() # 秒
while GPIO.input(ECHO) == 1:
signalon = time.time() # 秒
t = signalon - signaloff # 秒
distance = t * C * 100 / 2
return distance
def rotate(angle): # 0°から180°
pwm = angle * SERVO_PWM_1DEGREE + SERVO_PWM_MIN
if pwm > SERVO_PWM_MAX:
pwm = SERVO_PWM_MAX
elif pwm < SERVO_PWM_MIN:
pwm = SERVO_PWM_MIN
servo.ChangeDutyCycle(pwm)
return
def cleanup():
print('cleanup')
rotate(90)
time.sleep(1)
servo.stop()
GPIO.cleanup()
initAngle = 0
currentAngle = 0
clockwise = True
isCatched = False
isMissing = False # 物体を見失う
isWondering = False # 不安フラグ
missingCount = 0
try:
rotate(initAngle)
time.sleep(1)
while True:
dist = readDistance()
# print('角度: {0}, 距離: {1}'.format(currentAngle, dist))
isExistObject = dist < MONITORING_DIST
if isExistObject:
isMissing = False
missingCount = 0
isWondering = False
if isCatched == False: # 新規発見！
isCatched = True
clockwise = False if clockwise == True else True
else:
if isCatched:
isCatched = False
clockwise = False if clockwise == True else True
# print("物体を見逃しますた！")
isMissing = True
if isMissing:
missingCount += 1
if missingCount > 100: # 物体が存在しなかったのでリセットする
isCatched = False
isWondering = False
isMissing = False
elif missingCount > 10: # 不安フラグを立てる
isWondering = True
if isCatched:
gain = 0.7
elif isWondering:
gain = 4
else:
gain = 1
if clockwise:
gain *= 1
else:
gain *= -1
currentAngle += 1 * gain
if currentAngle > 180:
clockwise = False
elif currentAngle < 0:
clockwise = True
rotate(currentAngle)
time.sleep(0.005)
except KeyboardInterrupt:
print('KeyboardInterrupt')
except:
print('other')
finally:
cleanup()
動画の紹介
最後に、物体の動きに追従する様子を動画にしたのでご覧いただきたい。改善点としては、動きが速すぎると物体を見失ってしまうので、首振りの部分を工夫する必要がある。
