【Raspberry Pi】超音波センサーで距離を測る【pigpio】

こんにちは！　けい(Twitter)です．

今回は，ラズパイで超音波センサーを使って，距離を測る方法についてまとめていきたいと思います．また，フィルターを使って，安定したデータの取得の方法についてもまとめていきます．

必要なもの

ラズパイ本体

【国内正規代理店品】Raspberry Pi4 ModelB 4GB ラズベリーパイ4 技適対応品【RS・OKdo版】

created by Rinker

Raspberry Pi

¥9,930 (2024/04/26 16:21:30時点 Amazon調べ-詳細)

超音波センサー

ELEGOO Arduino用 HC-SR04超音波距離センサー5PCS

ELEGOO

超音波センサーは距離センサーの中でも安いのが特徴です．

1個当たり200円程度で入手できます．

回路図

抵抗値は上が1kΩ，下が2kΩ

ラズパイのピン	センサーのピン
5V	VCC
GPIO23	Trig
*GPIO24	Echo
GND	GND

*ラズベリーパイと超音波センサーをつなげるにあたって，超音波センサーが5V駆動なので，ラズパイの電圧である3.3Vにレベルシフトする必要があります．
5Vの電圧をラズパイに入力してしまうと，壊れる可能性があるので推奨されません．

なので，抵抗を用いて5Vを分圧することで，3.3Vになるように調整します．

オームの法則より，分圧回路の方程式は次の式で表されます．

$$V_{out} = \frac{R_2}{R_1 + R_2}V_{in}$$

ここで分かっている変数は，$$V_{in}=5 , V_{out}=3.3$$

なので，式を変形すると

$$\frac{3.3}{5} = \frac{R_2}{R_1 + R_2}$$

$$\frac{2}{3} = \frac{R_2}{R_1 + R_2}$$

つまり，R2:(R1+R2) = 2:3になればいいので，R1:R2=1:2とすると，上手くいきます．

よって，R2がR1の2倍になるようにすればいいということが分かりました．

また，実際に抵抗値を決めるときに低すぎると，ラズパイから大電流が抵抗に流れてしまい，危ないので抵抗は高めに設定し，R1=1kΩとします．

従って，今回用いる抵抗は1kΩと2kΩのものになります．

プログラム

では，実際に超音波センサーを動作させ，距離を測定した後，画面に結果を出力するプログラムを動かしてみましょう．

import pigpio
import time
 
#set GPIO Pins
TRIG = 23
ECHO = 24 

pi = pigpio.pi()
 
#set GPIO direction (IN / OUT)
pi.set_mode(TRIG, pigpio.OUTPUT)
pi.set_mode(ECHO, pigpio.INPUT)
 
def distance():
    # set Trigger to HIGH
    pi.write(TRIG, 1)
 
    # set Trigger after 0.01ms to LOW
    time.sleep(0.00001)
    pi.write(TRIG, 0)
 
    StartTime = time.time()
    StopTime = time.time()
 
    # save StartTime
    while pi.read(ECHO) == 0:
        StartTime = time.time()
 
    # save time of arrival
    while pi.read(ECHO) == 1:
        StopTime = time.time()
 
    # time difference between start and arrival
    TimeElapsed = StopTime - StartTime
    # sonic speed (34300 cm/s)
    distance = (TimeElapsed * 34300) / 2
 
    return distance
 
if __name__ == '__main__':
    try:
        while True:
            dist = distance()
            print ("Measured Distance = %.1f cm" % dist)
            time.sleep(0.2)

    except KeyboardInterrupt:
        print("Measurement stopped by User")
        pi.stop()

実行方法

pigpioライブラリを用いているので，上のプログラムを実行する前に，ターミナルで以下のコマンドを入力します．

$ sudo pigpiod

上のpythonプログラムを「ultrasonic_pigpio.py」というファイル名で保存したら，以下のコマンドで実行することができます．もちろんThonnyなどのPythonエディタでも実行できます．

$ python3 ultrasonic_pigpio.py

プログラム解説

TRIGとECHOが超音波センサーのピンと対応しています．

トリガーピンを10μ秒の間Highにして，物体に当たって超音波が返ってきたものをエコーピンでキャッチします．

センサー自体で計測できるのは時間だけです．なので，距離を出すためには音速を乗算する必要があります．(距離 = 速さ × 時間)
最後に，往復分の時間を測定しているので2で割ると実際の距離を計算できます．

測定精度が悪い?

上のプログラムを実行すると分かると思いますが，同じ距離を測定しているのに結果がバラバラだったりします．

画像

平気で3cm前後のばらつきが生じています．

そこで，測定精度を改善するためにフィルターを用意してみたいと思います．
フィルターといっても簡単なもので中央値フィルターというものです．こちらのフィルターはよく画像処理の分野で使われているそうですが，センサーの値にも適応させてみたいと思います．

プログラム(フィルター利用ver)

中央値フィルターの概要

今回は1次元の中央値フィルターを用います．

下の画像のように10個の数字を入れる箱を用意します．

センサーの値は順にロケットえんぴつのように，10個の箱に入っていき，古いデータからはじきだされていきます．

30cmの距離を測定したいときの10回分の取得したデータが上の画像のような場合，結構なブレ幅であることが分かります．
また，青色で示した部分が外れ値となっていて，このまま結果として出力すると誤った結果となってしまいます．

しかし，中央値フィルターを用いれば，10個のデータの中央値を1つ結果としてはきだすので，外れ値が結果として出力されません．

上の10個のデータを大きい順から並べると，(25,26,28,30,30,30,32,34,34,150)となっていて，中央値は30となります．

プログラム

import pigpio
import time
import collections
import numpy

TRIG = 23
ECHO = 24 

pi = pigpio.pi()
 
#set GPIO direction (IN / OUT)
pi.set_mode(TRIG, pigpio.OUTPUT)
pi.set_mode(ECHO, pigpio.INPUT)

# 中央値フィルター用の箱10個
history = collections.deque(maxlen=10)

def distance():
    # set Trigger to HIGH
    pi.write(TRIG, 1)
 
    # set Trigger after 0.01ms to LOW
    time.sleep(0.00001)
    pi.write(TRIG, 0)
 
    StartTime = time.time()
    StopTime = time.time()
 
    # save StartTime
    while pi.read(ECHO) == 0:
        StartTime = time.time()
 
    # save time of arrival
    while pi.read(ECHO) == 1:
        StopTime = time.time()
 
    # time difference between start and arrival
    TimeElapsed = StopTime - StartTime
    # sonic speed (34300 cm/s)
    distance = (TimeElapsed * 34300) / 2
 
    return distance

def distance_filtered():
    history.append(distance())
    return numpy.median(history)
 
if __name__ == '__main__':
    try:
        while True:
            dist = distance_filtered()
            print ("Measured Distance = %.1f cm" % dist)
            time.sleep(0.2)
 
        # Reset by pressing CTRL + C
    except KeyboardInterrupt:
        print("Measurement stopped by User")
        pi.stop()