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

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

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

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

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

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

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

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

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

Freenove Raspberry Pi 4 B 3 B + B A +用のロボットドッグキット、ウォーキング、セルフバランシング、ボールトレース、顔認識、ライブビデオ、超音波測距、カメラサーボワイヤレスRC
Freenove Raspberry Pi 4 B 3 B + B A +用のロボットドッグキット、ウォーキング、セルフバランシング、ボールトレース、顔認識、ライブビデオ、超音波測距、カメラサーボワイヤレスRC

Amazon
Makeblock プログラミングロボット mBot 日本語版
Makeblock プログラミングロボット mBot 日本語版

約20種類のパーツがあり、ロボットの構造やセンサーの仕組みを学びながら、約30分で組み立てができます。 ライントレースセンサー・超音波センサー・光センサーを搭載しており、黒線の上を走らせたり、障害物を回避したり、好きな色に光らせたり応用次第で遊び方は無限大です。

Amazon
micro: Maqueen micro:bit 教育プログラミングロボットプラットフォーム (micro:bit ボード付き)
micro: Maqueen micro:bit 教育プログラミングロボットプラットフォーム (micro:bit ボード付き)

これは、創造教育のために生まれたミニロボットである。makecodes、Mind+グラフィックスプログラミング(Scratch3.0に基づく)をサポート。 使いやすい、煩雑なインストールが不要:ねじ一つも要らなく、車輪、電池とmicro:bitボードを付けるだけで、すぐにプログラミングの学習を開始できる。

Amazon
タミヤ プログラミング工作シリーズ No.01 マイコンロボット工作セット クローラータイプ 71201
タミヤ プログラミング工作シリーズ No.01 マイコンロボット工作セット クローラータイプ 71201

プログラミング工作シリーズ 超音波センサー、駆動回路を搭載したクローラー走行のロボット工作セット インストールされた走行プログラムを元に2つのモーターをコントロール、障害物を避けて自動走行 別売りパーツを組み合わせれば、前後進・左右旋回の無線操縦が可能 お手持ちのパソコンでBBCマイクロビットを自分でプログラミングすれば、自由に動きをコントロールすることも可能

Amazon

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

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

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

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

実は、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+20ピンオスーメスデュポン線+超音波ステント Raspberry Pi B+Robot ロボットに交換 K18
Kuman 5個 超音波 センサーモジュール DC 5V 2cm-450cm 超音波センサHC-SR04+20ピンオスーメスデュポン線+超音波ステント Raspberry Pi B+Robot ロボットに交換 K18

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個セット デジタル・サーボ

電圧範囲:4.8V〜6.0V、制限角度:200°±1°、重量:9±1 g

AmazonRakuten

サーボーモータの動かし方のポイントは次の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()

動画の紹介

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

この記事で使用した関連製品

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

電圧範囲:4.8V〜6.0V、制限角度:200°±1°、重量:9±1 g

AmazonRakuten
Kuman 5個 超音波 センサーモジュール DC 5V 2cm-450cm 超音波センサHC-SR04+20ピンオスーメスデュポン線+超音波ステント Raspberry Pi B+Robot ロボットに交換 K18
Kuman 5個 超音波 センサーモジュール DC 5V 2cm-450cm 超音波センサHC-SR04+20ピンオスーメスデュポン線+超音波ステント Raspberry Pi B+Robot ロボットに交換 K18

Amazon
Raspberry Pi Zero W - ヘッダー ハンダ付け済み - ラズベリー・パイ ゼロ W ワイヤレス
Raspberry Pi Zero W - ヘッダー ハンダ付け済み - ラズベリー・パイ ゼロ W ワイヤレス

AmazonRakuten

こんな商品も人気です!

10 インチRaspberry Pi用タッチモニター EleDuino HDMI モバイルディスプレイ 2021最新版 ゲームモニターIPS液晶パネル 1366x768薄い 軽量、自立,USB Type-C標準 HDMI/スピーカー搭載
10 インチRaspberry Pi用タッチモニター EleDuino HDMI モバイルディスプレイ 2021最新版 ゲームモニターIPS液晶パネル 1366x768薄い 軽量、自立,USB Type-C標準 HDMI/スピーカー搭載

Amazon
Raspberry Pi4 Model B DIYメタルケース/ラズベリーパイ4 モデルB ケース/アルミニウム金属ケース/ファンレス/放熱シート付き/ブラック
Raspberry Pi4 Model B DIYメタルケース/ラズベリーパイ4 モデルB ケース/アルミニウム金属ケース/ファンレス/放熱シート付き/ブラック

ラズベリーパイ4用メタルケース スタイリッシュで堅牢なメタルボディ ケーズ全体がヒートシンクの高い放熱性能 アルミ製のファンレスケーズボディー シンプルでコンパクトなモノクローム

AmazonRakuten
Miuzei 最新Raspberry Pi 4 ケース ラスベリー パイ 4 ケース+ 5V 3A 電源Micro USB-C アダプター 冷却ファン+ヒートシンク Raspberry Pi 4 Model B対応
Miuzei 最新Raspberry Pi 4 ケース ラスベリー パイ 4 ケース+ 5V 3A 電源Micro USB-C アダプター 冷却ファン+ヒートシンク Raspberry Pi 4 Model B対応

AmazonRakuten
Freenove Raspberry Pi 4 B 3 B+ 400用の究極のスターターキット、434ページの詳細なチュートリアル、Python C Javaコード、223アイテム、57プロジェクト、無はんだブレッドボード
Freenove Raspberry Pi 4 B 3 B+ 400用の究極のスターターキット、434ページの詳細なチュートリアル、Python C Javaコード、223アイテム、57プロジェクト、無はんだブレッドボード

Amazon

Raspberry Piの参考書

Raspberry Piクックブック 第3版 (Make:PROJECTS)
Raspberry Piクックブック 第3版 (Make:PROJECTS)

本書は、登場以来多くのユーザーの支持を集め続けているマイコンボード「Raspberry Pi」を使いこなすための267本のレシピ集です。 ハードウェアの基本、オペレーティングシステムの使い方、ネットワーク接続、Pythonプログラミングの基本から、高度なPythonプログラミング、GPIO(汎用入出力)、モーター、センサー、ディスプレイ、コンピュータービジョン、Arduinoとの連携まで、幅広いニーズに応えます。

AmazonRakuten
写真や図解でよくわかる ラズパイZeroを使い倒す本 Raspberry Pi Zero/Zero W対応
写真や図解でよくわかる ラズパイZeroを使い倒す本 Raspberry Pi Zero/Zero W対応

本書ではRaspberry Pi Zero / Zero Wの概要から必要な周辺機器の説明、OSの導入やセットアップなどといった準備、そしてLinuxに初めて触れる人に向けてLinuxの基礎やシェルの操作などを解説しています。また、準備が整ったら実際に電子部品をRaspberry Pi Zero / Zero Wで制御する方法も解説しました。

KindleAmazonRakuten
Raspberry Pi ZeroによるIoT入門- Zero W 対応
Raspberry Pi ZeroによるIoT入門- Zero W 対応

本書は、大人から子供まで、初心者の方でも、ラズパイZeroとラズパイZero WをIoTのデバイスとして使いこなせるようになることを目的とした入門書です。また、ラズパイZeroとラズパイZero WをUSBケーブル1本でパソコンに接続できる便利な「Zero over USB」について、日本で初めて詳しく解説しました。

AmazonRakuten

Amazonでお得に購入するなら、Amazonギフト券がオススメ!

\Amazonギフトがお得/

コンビニ・ATM・ネットバンキングで¥5,000以上チャージすると、プライム会員は最大2.5%ポイント、通常会員は最大2%ポイントがもらえます!
Amazonギフト券

\この記事をシェアする/