ラズパイと超音波センサで物体の追跡

はじめに

この記事でやることは「超音波センサモジュール1つだけで物体を追跡させる」ことである。

超音波センサモジュール1つで、物体を追跡させるには少し工夫しなければならない。なぜなら、人間にたとえると「片耳だけ」で方向を探知するようなものだからだ。

超音波センサモジュールを見ると、2つの目があるように見えるが実は「1つ目」である。というか「口と耳」なのだ。1つは超音波を発する「口」、そしてもう1つは自分が発した超音波を受信する「耳」である。

ちなみに、Arduinoで超音波センサHC-SR04を使いたい場合は、こちらの記事を参考に。

ところで、超音波センサを使った、かわいらしいロボット達がお手頃な値段でAmazonに売られている。センサモジュールの配線などは、慣れるまでなかなか敷居が高いかもしれない。そこでとにかく動かしてみたい方や、プログラミングに集中したい方は、プログラミングロボット製品を使うと良いと思う。この手のロボットで十分遊んだ後なら、自分のオリジナルロボットを作れるようになるだろう。

HC-SR04で距離を測定してみよう

それでは実際に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へ変換させることにした。

ツェナーダイオードで5V電圧を3.3Vへ変換

他にも抵抗で分圧する方法がある。

分圧抵抗で5V電圧を3.3Vへ変換

HC-SR04で往復時間を計測するには？

今回使う超音波センサHC-SR04は、Triggerピンを一瞬Highにすれば超音波の発信から受信までをセンサが自動でやってくれて、その間だけEchoがHighの状態になる仕組みだ。つまりEchoがHighの状態の時間を測ることで、往復にかかった時間を知ることができる。

HC-SR04のTrigger、ECHOのタイミング図

参照:

距離測定のプログラム

実際に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()

# -*- 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つの超音波センサモジュールだけで、物体の追跡をさせる方法は次の通り。

1. 物体を見つけたら常にその右端または左端を狙うようにする
2. ごくわずかの角度だけ常に首を振って「見つけた」「見つけていない」を高速で繰り返す

超音波で常に物体の端を狙う

超音波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()

動画の紹介

最後に、物体の動きに追従する様子を動画にしたのでご覧いただきたい。改善点としては、動きが速すぎると物体を見失ってしまうので、首振りの部分を工夫する必要がある。