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

【ラズパイ】超音波センサーの使い方

こんにちは! けい(Twitter)です.

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

必要なもの

ラズパイ本体

超音波センサー

created by Rinker
ELEGOO
¥980 (2021/07/26 16:41:01時点 Amazon調べ-詳細)

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

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

回路図

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

ラズパイのピンセンサーのピン
5VVCC
GPIO23Trig
*GPIO24Echo
GNDGND

*ラズベリーパイと超音波センサーをつなげるにあたって,超音波センサーが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()

プログラム解説

1個目のプログラムから変更した点はハイライトしている部分です.

まず,16行目に中央値フィルターのための10個の箱を用意します.
この際リストで作るのではなく,collectionsのdequeを使うことで,ロケットえんぴつ型のリストができます.

44行目にdistance_filtered()という関数を作りました.
numpyのmedian関数を用いることで中央値を求めます.

まとめ

今回は,ラズパイで超音波センサーを使って,距離を測定し,測定精度を上げるために中央値フィルターを用いたプログラムをまとめました.

距離を測定するセンサーの中では超音波センサーが安価で手に入るので,ぜひ使ってみてください.

では!