【HC-SR04】ラズパイと超音波距離センサでレーダーを作る

今回は、超音波によって距離を測定するセンサ「HC-SR04」を使って周囲の障害物の状態を把握するレーダーを作成していきます。

今回使用するもの

HC-SR04の測定原理

センサーの挙動

① 片方のスピーカーから一瞬だけ超音波を発する
② 超音波が障害物に衝突し、再び戻ってくる
③ 戻ってきた超音波をもう片方のスピーカーで拾う
④ 発した超音波が返って来たことを認識する

センサーは上記の動作しか行っていません。

よってこれらをプログラムによって距離に変換する作業が必要です。

時間情報の取得

① 超音波を発する => 時刻を取得
② 戻ってきた超音波をもう片方のスピーカーで拾う => 時刻を取得
③ 時刻②-時刻①で超音波が移動した時間を求める

これで、「時間」を得ることができました。

「時間」から「距離」に変換

空気中を伝わる超音波は音速で移動します。

音速は環境によって若干変動しますが大体340m/sです。

音速と時間があれば距離に変換することができます。

ここで注意するのはHC-SR04で記録した時間は「往復時間」であることです。

センサーから障害物までの距離は1往復分の時間がかかっているので2で割ります。

距離(m) = 取得した時間(s) * 340(m/s) / 2

メートル(m)ではなく、センチメートル(cm)で出力したい場合は次のようにします。

距離(cm) = 取得した時間(s) * 34000(cm/s) / 2

以上の処理を行うことでHC-SR04で距離を測定することが可能になるわけです。

1.距離を測ってみる

HC-SR04の測定範囲は0.02m~4mなので、それ以外の値は範囲外として扱います。

import RPi.GPIO as GPIO
import time
import sys

#GPIOの番号
Trig = 14
Echo = 15

GPIO.setmode(GPIO.BCM)
GPIO.setup(Trig, GPIO.OUT)
GPIO.setup(Echo, GPIO.IN)

def read_distance():
    #一瞬超音波を発する
    GPIO.output(Trig, GPIO.HIGH)
    time.sleep(0.00001)
    GPIO.output(Trig, GPIO.LOW)
    
    #超音波を発した瞬間はLOW => 時間の記録
    while GPIO.input(Echo) == GPIO.LOW:
        sig_off = time.time()
    #超音波が帰ってきたらHIGH => 時間の記録
    while GPIO.input(Echo) == GPIO.HIGH:
        sig_on = time.time()
    
    #LOWからHIGHになるまでの時間を計算
    duration = sig_on - sig_off
    #距離を算出
    distance = duration * 340 / 2 #cm 340m/s 34000cm/s
    return distance

while True:
    try:
        dis = read_distance()
        
        #算出した距離が計測範囲内かどうか
        if dis > 0.02 and dis < 4: #0.02m~4m
            dis = round(dis, 3)
            print(dis, 'm')
        else:
            print('Out Of Range', round(dis, 3),'m')
        time.sleep(0.5)
    
    except KeyboardInterrupt:
        GPIO.cleanup()
        print('CleanUp')
        sys.exit()

実行結果

2.超音波レーダーの構想

距離が分かるということは障害物があるということです。

センサーを動かしながら取得した距離をグラフ上にプロットすれば、周囲の障害物情報を可視化することができそうです。

センサーの動かし方は次の2通りあります。

直前移動レーダー

この方法ではセンサーの移動方向をX軸、距離情報をY軸にプロットします。

180°回転レーダー

この方法ではセンサーの位置は固定(0,0)し、回転角度と距離情報を用いてプロットします。

三角関数を用いてプロットする(x,y)の値を計算します。

この方法では一方向のみに超音波を発するのではなくサーボモーターの回転角である180°の範囲を検出できます。

今回はサーボモーターを使って180°回転レーダーを作成していきます。

プロット位置の算出方法

例えばサーボモーターを1°ずつ回転させて距離を測っているとします。

15°の時に1.5mが得られたときxとyの計算はそれぞれ次のようになります。

x = 1.5 * cos15
y = 1.5 * sin15

このようにθが鋭角の時には次の式を用いることができます。

---

150°の時に2mが得られたときのxとyは次のようになります。

x = -(2 * cos(180-150))
y = 2 * sin150

このようにθが鈍角の時は次の式を用いることができます。

---

最後にθ = 0, 90, 180の時のxとyの値についてです。

それぞれ次のようになります。

distance : 取得した距離

θ=0
    x = distance
    y = 0
θ=90
    x = 0
    y = distance
θ=180
    x = -distance
    y = 0

以上より、回転角度θの値によって条件分岐し、使う式を切り替えることでプロットする値を計算することができます。

作成したプログラム

ここまでの内容をもとに作成したプログラムです。

距離を取得するたびにサーボモーターを1°回転させます。

サーボモーターの回転角は180°ですが、プログラム上では+90°～-90°として扱う必要があります。

「theta(θ)」は0～180°、「angle」は-90～+90°となっています。

import RPi.GPIO as GPIO
import time
import sys
import RPi.GPIO as GPIO
import math
import csv

#GPIOの設定
Trig = 14
Echo = 15
Servo_pin = 13
GPIO.setmode(GPIO.BCM)
GPIO.setup(Servo_pin, GPIO.OUT)
GPIO.setup(Trig, GPIO.OUT)
GPIO.setup(Echo, GPIO.IN)
Servo = GPIO.PWM(Servo_pin, 50)
Servo.start(0)

#距離の算出
def read_distance():
    GPIO.output(Trig, GPIO.HIGH)
    time.sleep(0.00001)
    GPIO.output(Trig, GPIO.LOW)
    
    while GPIO.input(Echo) == GPIO.LOW:
        sig_off = time.time()
    while GPIO.input(Echo) == GPIO.HIGH:
        sig_on = time.time()
    
    duration = sig_on - sig_off
    distance = duration * 340 / 2 #cm 340m/s 34000cm/s
    return distance

#サーボモーターを回転させる
def servo_angle(angle):
    duty = 2.5 + (12.0 - 2.5) * (angle + 90) / 180
    Servo.ChangeDutyCycle(duty)
    time.sleep(0.3)

while True:
    try:
        angle = -90
        theta = 0
        while True:
            print('\ntheta', theta)
            print('angle', angle)
            servo_angle(angle)
            
            dis = read_distance()
            
            if dis > 0.02 and dis < 4: #2cm~4m
                #dis = round(dis, 3)
                print(dis, 'm')
                #print(theta)
            else:
                print('Out Of Range')
                #print(theta)
                theta = theta + 1000

            if theta == 0:
                x = dis
                y = 0
            elif theta > 0 and theta < 90:
                x = dis * round(math.cos(math.radians(theta)), 5)
                y = dis * round(math.sin(math.radians(theta)), 5)
            elif theta == 90:
                x = 0
                y = dis
            elif theta > 90 and theta < 180:
                x = dis * -round(math.cos(math.radians(180-theta)), 5)
                y = dis * round(math.sin(math.radians(theta)), 5)
            elif theta == 180:
                x = -dis
                y = 0
                #print('finish')
            elif theta >= 1000:
                x = 0
                y = 0
                theta -= 1000
            else:
                print('finish')
                break

            data = [[x, y]]
            with open('radar_180.csv', 'a') as f:
                writer = csv.writer(f)
                writer.writerows(data)
            
            theta += 1
            angle += 1
            
            if angle > 90:
                servo_angle(-90)
                Servo.stop()
                GPIO.cleanup()
                sys.exit()
            else:
                #print(angle)
                time.sleep(1)
        
    except KeyboardInterrupt:
        servo_angle(-90)
        Servo.stop()
        GPIO.cleanup()
        sys.exit()

実行結果：グラフ化(レーダーの結果)

部屋の角(直角)にセンサーを置いて実行した結果です。

かなりなまったグラフになりました。

なまった原因

以上の2つが考えられます。

原因の多くはサーボモーターの回転角の制御が不安定だったことにあると考えられます。

サーボモーターはPWM制御を行うことで指定した角度まで回転しますが、今どこまで回っているのかをプログラム上で読み取ることは原理的に不可能です。

「1°回転させる」場合はPWMのパルス幅もかなり短くなっているはずです。

誤差が発生していてもおかしくないです。

回転子の位置を把握しながら制御するモーターが使用できれば回転角の誤差はなくなります。