Raspberry Pi 2 ＋赤外線距離センサー”GP2Y0A710K”でレーダーっぽいもの作ってみた

「右舷前方 艦影多数！！距離 5000！」

などと中二病が捗るアイテムといえばやはりレーダー！

昨年末に作ったRaspberry Pi＋シャープ製の赤外線距離センサー”GP2Y0A710K”をもう少し使える形にしようと年明け早々ごちゃごちゃやって、レーダーっぽいものを表示できるように改良しました。

先の記事でも書きましたが、このRaspberry Pi＋センサーの狙いは自動車の死角用センサーに使うこと。

本当はRaspberry Piに”GP2Y0A710K”を2つつけて、左右の死角をカバーしようかと考えたんですが、使ってるピンの数を考えると、Raspberry Piにこのセンサーを2つつけるのはどう見ても無理そう。

ならばレーダーっぽく回転させて1つのセンサーで広範囲をカバーすればいけるんじゃないかと思った次第です。

180度カバーできればいいので、回転はサーボにやらせることにしました。

サーボは一つしか使わないんですが、せっかく買ったし、またサーボの安定動作させることも考慮し、こいつでサーボを制御することに。

まだテストしてなかったんですよね。これの電源ケーブル用に買った携帯電話の充電ケーブルも引っ張り出してやっと活用です。

Raspberry Piで10個のサーボモーター動かしてみたの記事を参考にしつつ、まずはサーボの動作チェック。

何とか動きましたが、なかなかあっさりとは行きませんでした。

というのも”Raspberry PiのI2Cの有効化”が以前の記事で書いた方法とはちょっと変わってました。

Raspberry Pi の I2C を有効化する方法 (2015年版) - 意識低い開発者のBlog

”/etc/modules”の一番下に”i2c-dev ”を書き加えるだけ。これでリブートするとI2Cが有効になりました。

あとは以前の記事と同じように配線し

sudo python Servo_Example.py

というところまでは同じ。

一度やってるだけに、以前ほどは時間がかかりませんでしたが、若干配線間違えてたりして2時間ほどかけてようやく動作。

さて、今回はサーボの制御が目的じゃありません。

あくまで距離センサーの広範囲化が最終目的。

こんな感じに”GP2Y0A710K”をサーボの頭に取り付けて、くるくる回転させるようにしました。

PCA9685 PWMサーボ基板はこの通りむき出しのままビニール紐で固定するという超原始的な固定方法。

サーボは1度刻みで動かして、そのたびに距離センサーの値を取得。

問題は、この数値をどうやって可視化するか？

散々悩んだ末に、”apache2”と”OpenCV”を使うことにしました。

ということで、この2つをインストール。

sudo apt-get install libopencv-dev sudo apt-get install python-opencv
sudo apt-get install apache2

そして600×480のサイズの以下のような画像をあらかじめ作っておきます。これは真っ黒な画像の上に、OpenCVで線を引かせて作りました。

角度方向は15度おき、円は1mごとに線を引いてます。

で、サーボの角度と距離計の数値を使って上の画像に短い線を描画し、これを”/var/www/html/result.jpg” というファイルにアウトプットするという方法で、距離計の値を可視化します。

で、apache2を使ってPCなどの端末にこの画像(result.jpg)を移す、というわけです。

サーボと距離計制御用のコード(IR_radar.py)は以下。

----------------------------------------------------------------

#!/usr/bin/python

from Adafruit_PWM_Servo_Driver import PWM
import math
import spidev
import time
import RPi.GPIO as GPIO
import subprocess

import numpy as np
import cv2

# open SPI device 0.0
spi = spidev.SpiDev()
spi.open(0, 0)

# LED GPIO
PIN = 11
GPIO.setmode(GPIO.BOARD)
GPIO.setup(PIN,GPIO.OUT)

# Initialise the PWM device using the default address
pwm = PWM(0x40)

def setServoPulse(channel, pulse):
  pulseLength = 1000000                   # 1,000,000 us per second
  pulseLength /= 60                       # 60 Hz
  print "%d us per period" % pulseLength
  pulseLength /= 4096                     # 12 bits of resolution
  print "%d us per bit" % pulseLength
  pulse *= 1000
  pulse /= pulseLength
  pwm.setPWM(channel, 0, pulse)

pwm.setPWMFreq(60)                        # Set frequency to 60 Hz
while (True):
  # Change speed of continuous servo on channel O
  img = cv2.imread('black.png')
  for i in range(80):
        servoPls = int(5 * i + 175)
        servoRad = i * 2 + 10
        pwm.setPWM(0, 0, servoPls)

        resp = spi.xfer2([0x68, 0x00])
        value = (resp[0] * 256 + resp[1]) & 0x3ff
        volt = value * 5.0 / 1023
        distance = ( 18.679 / volt ) -4.774

        x = 300 + int(80 * distance * math.cos((180-servoRad) * math.pi/180))
        y = 480 - int(80 * distance * math.sin((180-servoRad) * math.pi/180))

        a = int(distance * math.sin((180-servoRad) * math.pi/180))
        b = int(distance * -math.cos((180-servoRad) * math.pi/180))

        cv2.line(img,(x-a,y+b),(x+a,y-b),(255,255,255),3)
        time.sleep(0.02)

  cv2.imwrite('/var/www/html/result.jpg', img)

  img = cv2.imread('black.png')
  for j in range(80):
        servoPls = int(575 - 5 * j)
        servoRad = 170 - j * 2
        pwm.setPWM(0, 0, servoPls)

        resp = spi.xfer2([0x68, 0x00])
        value = (resp[0] * 256 + resp[1]) & 0x3ff
        volt = value * 5.0 / 1023
        distance = ( 18.679 / volt ) -4.774

        x = 300 + int(80 * distance * math.cos((180-servoRad) * math.pi/180))
        y = 480 - int(80 * distance * math.sin((180-servoRad) * math.pi/180))

        a = int(distance * math.sin((180-servoRad) * math.pi/180))
        b = int(distance * -math.cos((180-servoRad) * math.pi/180))

        cv2.line(img,(x-a,y+b),(x+a,y-b),(255,255,255),3)

        time.sleep(0.02)

  cv2.imwrite('/var/www/html/result.jpg', img)

----------------------------------------------------------------

サーボは2度おきに10～170度まで回転させるようにしました。

10→170度回転し、その後170→10度に戻るという動作。ウェイトは0.02秒。あまり短いとサーボがうまく回りません。

注意点としては、上のコード(IR_radar.py)とPCA9685のドライバコードであるAdafruit_I2C.py、Adafruit_I2C.pyc、Adafruit_PWM_Servo_Driver.py、Adafruit_PWM_Servo_Driver.pycの４つのファイルは必ず同じディレクトリに入れておいてください。

apache2のホームページ(index.html)は以下のように記述。

----------------------------------------------------------------

<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN" "http://www.w3.org/TR/html4/loose.dtd">
<html>
<head>
<meta http-equiv="Content-Type" content="text/html; charset=UTF-8">
<title></title>
<meta http-equiv="Refresh" content="3">
<body>
<img width="600" src="result.jpg">
</body>

----------------------------------------------------------------

result.jpgを3秒おきに表示させてます。

で、まずサーボ・距離計のコードを実行。

sudo python IR_radar.py

PCなどの端末でブラウザを開き、アドレスに”http://(Raspberry Pi 2のIPアドレス)”を入れると、画像が表示されます。

こんな感じに表示されてます。

ブラウザ上の画像は以下の通り。

すごくレーダーっぽいですねぇ。試行錯誤してなんとかこの表示にできました。

真ん中だけ妙に飛んでます。これは、うちの2階にある2つの6畳間は扉で仕切られていて、この扉が空いた状態だったため、向こうの部屋の壁までの距離を測ってます。

画面左右はこのセンサーのある6畳の部屋の角部を表示してますが、やっぱり測定誤差が大きいのか、がたがたしてますね。いかにもレーダーって感じです。

片道約2秒ほどで回るため、上のレーダー画像は約2秒おきに更新されてます。一方、HTMLの更新時間も3秒としているため、時々HTMLの更新タイミングと画像タイミングが合わず同じ絵を出し続けることも。まあ、その辺はご了承ください。

ところで、動作中にデジカメで赤外線発光部を撮影してみました。人間の目には見えませんが、この通りデジカメを介してみると向かって右側だけ紫っぽい色で見えますね。こいつが測距用赤外線の正体です。左が受光用ということになります。

・・・とまあ、なんとか作ってみたいものが一応形にはなりました。

細かいことを言うと、先回記事で作った近似式がいまいちなようで若干 測定距離が不正確。もう一度校正しなおしが必要ですね。

また、ずっと向こうに何もない状態で測定すると、明らかに5m以内に何もないところでも～0.2m程度の距離を出すことがあります。

まだまだ課題は多いのが現状。

しかし、Python使ってるわりには思ったより早く処理できているのは驚き。

正直、もう少し遅いかと思ったんですが、さすがはRaspberry Pi 2。

ただ、だんだん欲が出てきて、やっぱり5m以上の距離も測りたいなぁと。

しかし、このセンサーでは5mが限界。

強力なレーザー光を使った”レーザー距離計”あたりを使ったらもっと長距離測れるんでしょうが、さすがにレーザービームを当たりかまわず振り回すのはまずいでしょう。

なんとか20～30mを測定するいい手段がないか、検討中です。

余談ですが、数百m程度の測定が可能なレーダーなら一応市販品はあります。

GARMIN　バードビューレーダーGMR18XHD | ■300mサイドビュー,トランサム取付 | | GPS魚探　魚群探知機専門店　ボトムハウスレーダー

船舶用のレーダーですが、魚群探知ではなく”バードビュー”レーダー。

これは鳥の群れているところには魚がいるということで、鳥の群れを探知するためのものだそうです。

免許不要なものの、基地局申請は出さないといけない上に、車載用に使えるかどうかは不明。おまけに値段が高い・・・まあ、さすがにここまで強力なものはいりませんね。

[GPG] PCA9685 16Channel 12bit PWM サーボドライバー Arduino等の電子工作用 (Droidfun型)