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

 

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

この記事では、Raspberry Piを使って超音波センサとサーボモータを組み合わせた物体の追跡方法を解説していく。超音波センサはHC-SR04、サーボモータはSM-S2309Sを使用した。

Kuman 5個 超音波 センサーモジュール DC 5V 2cm-450cm HC-SR04
Kuman 5個 超音波 センサーモジュール DC 5V 2cm-450cm HC-SR04
Amazon

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

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

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

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

超音波で距離がわかる仕組み

やまびこのイメージ
やまびこのイメージ

まずは超音波で距離が測定できる仕組みを説明しよう。

超音波で物体の距離を測れる仕組みは「やまびこ」のイメージだ。音を発してから測定物に当たって、跳ね返ってくるまでの時間を測ることで距離がわかるのだ。

実は、1気圧の空気での音速は気温によって決まっていて、たとえば、気温20°のとき約343m/sになることが知られている。速度と時間がわかれば中学生で習った、速度 x 時間で距離が導き出せるはずだ。今回は音速を343m/sとして計算していく。

超音波から距離を計算する方法

こちらが音波の速度から距離を計算する式となる。

$$ Distance = \frac{T}{2} \times 343 $$

「音を発信してから受信するまでの時間」をT秒としたので、片道の時間はそれを2で割った値、つまりT/2秒である。

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

それでは実際にHC-SR04を使って、距離を測定してみよう。

Kuman 5個 超音波 センサーモジュール DC 5V 2cm-450cm HC-SR04
Kuman 5個 超音波 センサーモジュール DC 5V 2cm-450cm HC-SR04
Amazon

HC-SR04とラズパイの配線

ラズパイとHC-SR04の配線図
ラズパイとHC-SR04の配線図

HC-SR04ラズパイ
Vcc5V
TrigGPIO20
Echo電圧レベル変換後GPIO21⭐︎
GNDGND

Echoピンの電圧レベル変換⭐︎

HC-SR04のEchoからは5Vの信号が出力される。しかし、ラズパイのGPIOの入力電圧は3.3Vまで。そのため直接繋ぐことはできない。今回は、3.3Vのツェナーダイオードを使って5Vを3.3Vへ変換させることにした。

ツェナーダイオードで5V電圧を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の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()

サーボモータの動かし方

サーボモータと超音波センサを合体させたロボット
サーボモータと超音波センサを合体させたロボット

物体の動きに合わせて首を振るようにしたいので、ここではサーボモータの使い方を説明する。何かのキットに付いていたサーボモータSM-S2309Sを使ったが、他のサーボモータでも問題ないと思う。

Miuzei サーボモーター マイクロサーボ 9g 10個セット デジタル・サーボ
Miuzei サーボモーター マイクロサーボ 9g 10個セット デジタル・サーボ
Amazon

サーボーモータの動かし方のポイントは次の2つだ。

  • PWM信号(50Hz程度)を、SIGピンへ送信する。
  • PWM信号のデューティー比によって、角度が決められる。

サーボモータとラズパイの配線

サーボモータとラズパイの配線図
サーボモータとラズパイの配線図

ラズパイサーボモータ
GNDGND
GPIO26Signal
ラズパイ以外の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つの超音波センサモジュールだけで、物体の追跡をさせる方法は次の通り。

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

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

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

動画の紹介

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

Kuman 5個 超音波 センサーモジュール DC 5V 2cm-450cm HC-SR04
Kuman 5個 超音波 センサーモジュール DC 5V 2cm-450cm HC-SR04
Amazon
Miuzei サーボモーター マイクロサーボ 9g 10個セット デジタル・サーボ
Miuzei サーボモーター マイクロサーボ 9g 10個セット デジタル・サーボ
Amazon

超音波センサーを使ったロボット製品

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

Freenove Raspberry Pi 4 B 3 B + B A +用のロボットドッグキット
Freenove Raspberry Pi 4 B 3 B + B A +用のロボットドッグキット
Amazon
Makeblock プログラミングロボット mBot 日本語版
Makeblock プログラミングロボット mBot 日本語版
Amazon
micro: Maqueen micro:bit 教育プログラミングロボットプラットフォーム
micro: Maqueen micro:bit 教育プログラミングロボットプラットフォーム
Amazon
タミヤ プログラミング工作シリーズ No.01 マイコンロボット工作セット クローラータイプ
タミヤ プログラミング工作シリーズ No.01 マイコンロボット工作セット クローラータイプ
Amazon

人気のラズパイ

Raspberry Pi 4 Model B 8GB 技適マーク入 正規品!ラズベリーパイ4 モデルB
Raspberry Pi 4 Model B 8GB 技適マーク入 正規品!ラズベリーパイ4 モデルB
Amazon
TRASKIT Raspberry Pi 4 Model B Starter Kit
TRASKIT Raspberry Pi 4 Model B Starter Kit
Amazon
Raspberry Pi Zero W - ヘッダー ハンダ付け済み
Raspberry Pi Zero W - ヘッダー ハンダ付け済み
Amazon

人気のラズパイ周辺機器

10 インチRaspberry Pi用タッチモニター EleDuino HDMI モバイルディスプレイ
10 インチRaspberry Pi用タッチモニター EleDuino HDMI モバイルディスプレイ
Amazon
Raspberry Pi4 Model B /アルミニウム金属ケース/ファンレス/放熱シート付き
Raspberry Pi4 Model B /アルミニウム金属ケース/ファンレス/放熱シート付き
Amazon
Freenove Raspberry Pi 4 B 3 B+ 400用の究極のスターターキット
Freenove Raspberry Pi 4 B 3 B+ 400用の究極のスターターキット
Amazon
KEYESTUDIO DC 5V 4チャンネル リレーシールドモジュール 拡張ボード for Raspberry Pi
KEYESTUDIO DC 5V 4チャンネル リレーシールドモジュール 拡張ボード for Raspberry Pi
Amazon

Raspberry Piのオススメ入門書

Raspberry Piクックブック 第3版 (Make:PROJECTS)
Raspberry Piクックブック 第3版 (Make:PROJECTS)
Amazon
これ1冊でできる! ラズベリー・パイ 超入門 改訂第6版 Raspberry Pi 4/Zero W対応
これ1冊でできる! ラズベリー・パイ 超入門 改訂第6版 Raspberry Pi 4/Zero W対応
KindleAmazon
写真や図解でよくわかる ラズパイZeroを使い倒す本 Raspberry Pi Zero W対応
写真や図解でよくわかる ラズパイZeroを使い倒す本 Raspberry Pi Zero W対応
KindleAmazon
記事に関するご質問などがあれば、ぜひTwitterへお返事ください。