Raspberry Pi3 B+と超音波距離センサーHC-SR04で自動運転ロボットカーを作る

はじめに

現実世界の自動運転車を構成する要素は自車位置推定、物体認識、行動判断、車体制御です。これらの4要素全てをラジコンをベースに実装することも可能ですが、自車位置推定を簡易的に実現するのは容易ではなく、また今回は低速走行が目標のため車体制御もほとんど必要ありません。そこで今回は超音波距離センサーで前方に存在する物体を認識し、それを回避するための行動判断をラジコンをベースとした自動運転のロボットカーで簡易的に実現したいと思います。

環境

• Raspbian GNU/Linux 9.4
• Python 3.5.3
• GNU Make 4.1
• gcc version 6.3.0
• lircd 0.9.4c
• ServoBlaster(2nd Oct 2019)

ロボットカー全体構成

市販品のラジコンにPWM信号をラズパイから送信しアクセルとステアリングを制御します。前方の3つの超音波距離センサーを用いて前方にある物体を認識し、左右にステアリングを切るように行動判断します。

使用したパーツ

- ラジコン : タミヤ 1/10 XBシリーズ No.199 NSX (TT-02シャーシ)
本体は何でもOKなのですが、やはりボディを付けて諸々のハードをキレイに収めたいので大きめのものがおすすめすです。また後述致しますが、今回は低速走行をやりたかたったので7.2V系のものでないほうが速度コントロールがやりやすかったと思われます。

- 本体バッテリー : タミヤ 7.2Vカスタムパック
本体の7.2Vからリニアレギュレータで電圧を揃えてラズパイの電源と共通化してもよかったのですが、ラズパイ側の電源がモバイルバッテリーを使用するほうが手軽だったので今回はラズパイ側とラジコン本体電源を独立させました。

- 速度コントローラ : HiLetgo 5Aミニ DC モータ PWM 速度コントローラー 3V-35V
今回一番悩まされたものです。7.2VのPWMでモーターをコントロールしますが、今回のコントローラーを介した方法ではDuty比だけでは低速側である一定の比率でカットオフされるようで、低速を得ることができないため使用しました。またこの回路内で整流されているため逆回転が入りません。2つ使用して正負両側に対応すれば可能かと思われますが今回は正回転のみです。

- シングルボードコンピュータ : Raspberry Pi3 B+
特に注意点はありません。そのまま使用可能です。OS ver.

- ラズパイ用バッテリー : Poweradd Ombar ijuice 3350mAhモバイルバッテリー
種類は何でも安いものでOKです。容量は多ければ多いほど良いですが、レイアウトを考慮してこのサイズにしました。

- ラズパイ用スイッチ : オーディオファン USB TypeA
ラズパイを起動するスイッチ用です。これをONしたらスタンバイ状態に入るようにします。

- 赤外線リモコンモジュール : IR HX1838
スタンバイから走行をスタートするためのトリガに使用します。

- 超音波距離センサー : HC-SR04 ＋ アクリル マウント アングル
前方左右の障害物を認識するためのセンサーです。

- ハード固定用治具 : MOLATE ストレートプレート フラット修理プレート96*19mm厚さ 2.6mm
このロボットカーを作製する上で、ハードウェア類をどうレイアウトするかが最も苦労します。本体に元からついているネジの間にこのプレートをかましたり、ドリルでネジ穴を開けたりして取り付けました。

ハードウェア実装

- 配線
全体構成は同型のラジコンをベースに作成されているこちらのsiteを参考にさせて頂きました。最終的な全体配線は最後の図で記載。

- 超音波センサ
超音波の基本的な使用方法はこちらのsiteを参考にさせて頂きました。超音波センサを3個使用するため5Vと3.3Vを両方使用しております。5Vの出力を使用するときは抵抗を使用して３．３Vに降圧する必要があると記載されておりますが、直でそのまま使用しております（5Vをそもまま使用しても問題ないという記事も見かけましたが、とりあえず動作は可能でした）。また3つ全く同じものを入手したと思っていたら1つだけ異なることが事後に発覚いたしました。

- 赤外線リモコン
ロボットカーの動き出しのトリガのためにこちらのsiteを参考にさせて頂きながら赤外線リモコンを使用しました。ここにかける時間がなかったため、リモコンのどのボタンを押しても動作開始としました。詳細は後述のソースコードを参照。
ハードウェア配線の全体構成は下記の図となりました。

Raspberry Piセットアップ

- RaspberryPiとPCのNetwork設定
ラズパイ側に必要な設定はまずはインターネットに接続し必要なソフトウェアをダウンロードできる環境にすることと、SSH接続のためのサーバー設定が必要となります。開発はもちろんサーバーとなるラズパイとクライアントのPCがWi-fiが使用出来る環境下で行いますが、動作テストのために広いWif-fi環境が無いような場所でデバッグが必要になる際はラズパイとPCを無線で直接接続するためにアドホック接続が必要になります。アドホック接続はこちらのsiteを参考にさせて頂きました。
- RaspberryPiの起動時の設定
ラジコンとラズパイの電源を入れ、そのままラズパイが起動するとpython3で/home/pi/nsx.pyというプログラムが動作するように設定するため、/etc/rc.localに下記の記述を追記します。注意点としまして自分の環境ではsudo実行する必要があるため、password（ここではhogehoge）を引き渡すことと、フルパスが必要になります。

:/etc/rc.local

echo "hogehoge" | sudo -u pi /user/bin/python3 /home/pi/nsx.py

ソフトウェア実装

- ServoBrasterによるサーボ制御
PWM信号による速度コントローラーとステアリング用サーボに指令値を送るためにServoBlasterを使用させて頂きました。PWM信号のDuty比によって、150をニュートラルの中心に100~200の値を送信して開度を調整します。サーボによってこの値は異なるので実装しながら制御に使用する値を決定する必要があります。また150前後のギリギリの値はなぜか細かい司令を受け取ってくれず、140~150の間や160~150の間は不感帯となっていたため、アクセル開度による低速走行に限界があります。そのため電流値に絞りを入れるために追加の速度コントローラHiLetgoを使用しております。

- 超音波距離センサーによる物体との距離計測
距離精度は最高で数センチ程度出るようですが、注意点としまして超音波の反射が正しく計測できない場合、延々と計測が完了せずWhileループから抜けれなくなるためAbortタイマーを導入しております。Abortした場合は前回計測した値をそのまま使用するようにしました。数ステップの平均値を取るなど様々な方法を試しましたが、単純な方法が最も安定した動作が得られるため値の精度は妥協しました。

- 赤外線リモコンによる起動スイッチ
ラズパイの赤外線送受信にはlircを使用しました。インストール手順等は参考にさせて頂いたこちらのsiteの通りですが、注意点は下記のconfigファイルの記述です。dtoverlayの指定をgpio-ir,gpio_pinとしてGPIOピンの指定をしてください。ここではGPIOピンを20（＝ボードピンNo.38）を使用。

:/boot/config.txt

# Uncomment this to enable the lirc-rpi module
#dtoverlay=lirc-rpi
#dtparam=gpio_in_pin=20
#dtparam=gpio_in_pull=up
dtoverlay=gpio-ir,gpio_pin=20
dtoverlay=gpio-irtx,gpio_pin=20

上記を踏まえまして、ソースコードの全体は下記となりました。

:~/nsx.py

import subprocess as sp # Servo Blasterをsudoで実行用
import os
import sys
from time import sleep
import time 
import RPi.GPIO as GPIO # ラズパイGPIO用 

# 実行用の値定義
dirServo = "PiBits/ServoBlaster/user"
dirHome = "/home/pi"
passwd = ("hogehoge").encode() # sudo実行用パスワード

# 超音波距離センサーのパラメータ
trigCenter = 19
echoCenter = 18
trigLeft = 15
echoLeft = 16
trigRight = 7
echoRight = 8

# ServoBlasterの実行（初期化）
def resetSystem(dirServo, passwd, dirHome):
    os.chdir(dirHome)
    os.chdir(dirServo)
    sp.run(("sudo", "./servod"), input=passwd)
    os.chdir(dirHome)
    print("Reset System")

# ステアリングとアクセル（モーター指令値）を同時にニュートラル化
def setNeutral():
    stNeutral = "echo 1=150 > /dev/servoblaster"
    accNeutral = "echo 2=150 > /dev/servoblaster"    
    os.system(stNeutral)
    os.system(accNeutral)
    print("Set Neutral")

# ステアリング単体のニュートラル化
def setStNeutral():
    stNeutral = "echo 1=150 > /dev/servoblaster"
    os.system(stNeutral)
    print("Set Steer Neutral")

# アクセル単体のニュートラル化
def setAccNeutral():
    accNeutral = "echo 2=150 > /dev/servoblaster"    
    os.system(accNeutral)
    #print("Set Accel Neutral")

# 前進アクセルMax指令値
def setFwdMax():
    fwdMax = "echo 2=100 > /dev/servoblaster"
    os.system(fwdMax)
    #print("Set fwdMax")

# 左ステアリングMax指令値
def setLeftMax():
    leftMax = "echo 1=100 > /dev/servoblaster"
    os.system(leftMax)
    print("Set leftMax")

# 左ステアリングMin指令値（今回は未使用）
def setLeftMin():
    leftMin = "echo 1=140 > /dev/servoblaster"
    os.system(leftMin)
    print("Set leftMin")

# 右ステアリングMax指令値
def setRightMax():
    rightMax = "echo 1=200 > /dev/servoblaster"
    os.system(rightMax)
    print("Set rightMax")

# 右ステアリングMin指令値（今回は未使用）
def setRightMin():
    rightMin = "echo 1=160 > /dev/servoblaster"
    os.system(rightMin)
    print("Set rightMin")

# 超音波距離センサーによる距離計測
def getDistance(TRIG, ECHO, abortTime):
    GPIO.setwarnings(False)
    GPIO.setmode(GPIO.BOARD)
    GPIO.setup(TRIG, GPIO.OUT)
    GPIO.setup(ECHO, GPIO.IN)
    GPIO.output(TRIG, GPIO.LOW)
    time.sleep(0.01)

    GPIO.output(TRIG, True)
    time.sleep(0.00001)
    GPIO.output(TRIG, False)
    
    startTime = time.time()
    while GPIO.input(ECHO) == 0:
        signaloff = time.time()
        
        if ((signaloff - startTime) > abortTime):
            distance = -1
            return distance

    while GPIO.input(ECHO) == 1:
        signalon = time.time()

    timepassed = signalon - signaloff
    distance = timepassed * 17000
    
    return distance
    
    GPIO.cleanup()

# 赤外線リモコンの受信
def getRemoteSignal(flagState):
    boardNo = 38
    GPIO.setmode(GPIO.BOARD)
    GPIO.setup(boardNo, GPIO.IN)
  
    try: 
        if (GPIO.LOW  ==  GPIO.input(boardNo)):
            
            if (flagState == False):
                flagState = True
                print("Switch ON !!!!!!!!")

            elif (flagState == True):
                flagState = False
                print("Switch OFF !!!!!!!!")

            time.sleep(0.5)

    except:
        GPIO.cleanup()
    
    return flagState

# アクセル制御（前方距離計測による単純な前進及び停止制御のみ）
def getAccControl(currDist, setDist):

    flagStop = 0
    
    if (currDist < setDist) and (currDist > -1):
        
        setNeutral()
        flagStop = 1

    else:
        setFwdMax()
    
    return flagStop

# ステアリング制御（距離計測による単純制御のみ）
def getStControl(currDistRight, setDistRight, currDistLeft, setDistLeft, flagStop):
    statusRight = (currDistRight < setDistRight) and (currDistRight > -1)
    statusLeft =  (currDistLeft < setDistLeft) and (currDistLeft > -1)

    if (flagStop == 1):

        setStNeutral()

    elif (statusRight and statusLeft):

        setStNeutral()
    
    elif (statusRight):

        setLeftMin()

    elif (statusLeft):

        setRightMin()

    else:
        setStNeutral()

# 超音波距離センサーのエラー値回避
def getTakeOverValue(distLeft, distRight):

    global prevDistLeft
    global prevDistRight

    if (distLeft == -1):
        distLeft = prevDistLeft

    prevDistLeft = distLeft

    if (distRight == -1):
        distRight = prevDistRight

    prevDistRight = distRight

    return (distLeft, distRight)


def main():

    # 赤外線リモコン受信状態フラグの初期化
    global flagState
    flagState = False
    
    # 左右の物体との距離の前回値の初期化
    global prevDistRight
    global prevDistLeft
    prevDistRight = 0
    prevDistLeft = 0

    # 超音波距離センサーのキャリブ値
    centerThreshold = 30 # 前方は停止のために短めに設定
    rightThreshold = 70　# 右のセンサーのみ種類（ロット？？）が異なっていたため左右の値は異なる
    leftThreshold = 60

    # スタンバイ状態にしてパラメータをニュートラルに設定
    resetSystem(dirServo, passwd, dirHome) 
    setNeutral()
    
    while True:
        
        # 赤外線リモコンの受信状態の確認（動作開始時）
        flagState = getRemoteSignal(flagState)
        
        if (flagState == True):        

            # 全超音波距離センサーによる距離計測
            distCenter = getDistance(trigCenter, echoCenter, 0.1)
            distLeft = getDistance(trigLeft, echoLeft, 0.1)
            distRight = getDistance(trigRight, echoRight, 0.3)
           
            # 超音波距離センサーがエラーを返した時の代替値生成 
            distLeft, distRight = getTakeOverValue(distLeft, distRight)
            
            # 前方の物体との距離に基づく停止指令値生成
            flagStop = getAccControl(distCenter, centerThreshold)

            # 左右の物体との距離に基づくステアリング指令値生成
            getStControl(distRight, rightThreshold, distLeft, leftThreshold, flagStop)
            
            # 赤外線リモコンの受信状態の確認
            flagState = getRemoteSignal(flagState)
            
        else:
            setNeutral()

if __name__ == "__main__":
    main()

終わりに

本ラジコンを何に使用したかといいますと、結婚式での指輪を運ぶリングピロー運搬ロボットカーとして使用しました笑。たぶん世界初です。

##Link

1. Raspberry Pi 2とタミヤの1/10RCでインターネットラジコンを作ってみた

2. 超音波距離センサ(HC-SR04)を使う

3. Raspberry Pi 3 Model B+とlircを使ってリモコンをコントロールする 1

4. RPIのAd-Hocネットワーク作成方法

5. ServoBlaster