DCモーターにはエンコーダーがついていないのでモーターから角度を知ることはできない。また、エンコーダーをつけたとしても、プーリーベルトがしょっちゅう滑りを起こすので、正確に測定することはできない。

そこでカメラ画像から角度を推定することを考える。

カメラの設定

まずは、できるかぎり高速でカメラモニターできる設定を考える。カメラは、ラズパイ専用カメラ、Camera Module V2 を使用した。このカメラは、640 x 480 の解像度ならば 200FPSで撮影できると言われている。

プログラムの環境は、
python 2.7.13
opencv 3.3.1　インストールはここを参考にしました（以下抜粋）。

wget https://github.com/mt08xx/files/raw/master/opencv-rpi/libopencv3_3.3.1-20171126.2_armhf.deb
sudo apt install -y ./libopencv3_3.3.1-20171126.2_armhf.deb
sudo ldconfig

で、picamera というCamera Module の python interface を使用。
公式ページは、こちら。

できるだけ高速にカメラ画像を取得したいと、いろいろ試した末に、最終的に採用した方法が以下のスレッドを使う方法（それでも50Hz。もっと良い方法があればだれか教えてほしいです。。）。

サンプルプログラムの使い方は、プログラム名をsample_picam_thread.py として、
>>> python sample_picam_thread.py xxx

xxx には fast, middle, middle2, large のどれかを入れる。
それぞれ以下のモードでカメラが起動する。

fast: 320 x 240, 50-58Hz (画角がなぜか狭い）
middle: 320 x 240, 30-35Hz (なぜかframerateを抑えると画角が戻る)
middle2: 640 x 480, 18-20Hz (VGAサイズ)
large: 1920 x 1080, 4Hz 

# coding: utf-8
from picamera.array import PiRGBArray
from picamera import PiCamera
from threading import Thread
import cv2
import time

class PiVideoStream:
    def __init__(self, resolution=(320, 240),
                 shutter_speed=20000,
                 iso=400,
                 framerate=70):
	self.camera = PiCamera()
	self.camera.resolution = resolution
	self.camera.framerate = framerate
        self.camera.shutter_speed=shutter_speed
        self.camera.iso = iso
	self.rawCapture = PiRGBArray(self.camera, size=resolution)
	self.stream = self.camera.capture_continuous(self.rawCapture,
		format="bgr", use_video_port=True)
	self.frame = None
	self.stopped = False
        self.c = 0
        self.prev_c = 0

    def start(self):
	self.thread=Thread(target=self.update, args=())
        self.thread.daemon=True
        self.thread.start()
	return self
 
    def update(self):
	for f in self.stream:
            self.c = self.c + 1
            if self.c > 10000:
                self.c = 0
	    self.frame = f.array
	    self.rawCapture.truncate(0)
  	    if self.stopped:
		self.stream.close()
		self.rawCapture.close()
		self.camera.close()
  	        return
            
    def read(self):
        while self.c == self.prev_c:
            time.sleep(0.001)
        self.prev_c = self.c
        return self.frame

    def stop(self):
	self.stopped = True


if __name__=='__main__':
    import sys

    argvs = sys.argv
    
    mode = 'fast'
    if len(argvs) > 1:
        mode = argvs[1]

    if mode == 'fast': # 50-58Hz
        vs = PiVideoStream(resolution=(320, 240),
                           shutter_speed=20000,
                           iso=100,
                           framerate=60).start()

    elif mode == 'middle': # 30-35Hz
        vs = PiVideoStream(resolution=(320, 240),
                           shutter_speed=20000,
                           iso=100,
                           framerate=40).start()
        
    elif mode == 'VGA': # 18-20 Hz
        vs = PiVideoStream(resolution=(640, 480),
                           shutter_speed=20000,
                           iso=100,
                           framerate=40).start()
        
    elif mode == 'large': # 4Hz
        vs = PiVideoStream(resolution=(1920, 1080),
                           shutter_speed=20000,
                           iso=100,
                           framerate=30).start()
    
    time.sleep(2.0)
    
    # for ftp
    prev_t = 0
    current_t = 0
    cnt = 0    
    while True:
        # fps
        current_t = time.time()
        cnt += 1
        if current_t - prev_t > 1.0: 
            dt = current_t - prev_t
            prev_t = current_t
            fps = cnt / dt 
            cnt = 0
            print('fps = %.2f' % fps)
            
        # capture
	frame = vs.read()
        cv2.imshow("Frame", frame)
        
	INPUT = cv2.waitKey(1) & 0xFF
        if INPUT == ord('s'):
            cv2.imwrite('img.png', frame)
            print('img is saved.')
        if INPUT == ord('q'):
            cv2.destroyAllWindows()
            vs.stop()
            break

作成したサンプルプログラムは、以下を参考にしたのだが、空回り現象でftp が盛って見える現象を修正したら、50Hz 程度になった。

Increasing Raspberry Pi FPS with Python and OpenCV

振り子には、上記の’fast’ の設定で使うことにした。しかし、なぜかframerate だけが遅い middle に比べて画角が狭くなってしまうのだ。

middle の画角は、以下。

fast の画角は、以下。

謎だ。

ちなみに、上記の方法だとVGA (640 x 480)の解像度では20fps 程度しか出ない。これだったら、以下のように、普通のusb camera として opencv を使って取得した方が簡単だし速い。30fps出る。

# coding: utf-8
import cv2
import numpy as np
import time
 
cap = cv2.VideoCapture(0)
 
prev_t = 0
current_t = 0
cnt = 0
 
while True:
    # fps の計算
    current_t = time.time()
    cnt += 1
    if current_t - prev_t > 1.0: 
        dt = current_t - prev_t
        prev_t = current_t
        fps = cnt / dt 
        cnt = 0
        print('fps = %.2f' % fps)
 
    # カメラ画像表示
    ret, img = cap.read()
    cv2.imshow('img', img)
 
    # ’q’ で終了
    INPUT = cv2.waitKey(10) & 0xFF
    if INPUT == ord('q'):
        cv2.destroyAllWindows()
        break

角度の推定

振り子の下側から見上げるようにカメラを机上に配置し、振り子に張り付けた緑色のフェルトの色を検出、その重心をOpenCVを使って算出する。

振り子が回転するとき、その重心点の軌跡は楕円形となる。その楕円の位置を前もって記録しておけば（uとvの最大値と最小値でよい）、現時点での振り子の位置から、楕円内での振り子の角度が近似的に計算できる。

マーカーの重心位置から角度を算出するプログラムはこんな感じになる。

import numpy as np

def cal_theta(u, v):
    nu = 1.0 * (u - center_u) / (max_u - min_u)
    nv = 1.0 * (v - center_v) / (max_v - min_v)
    return np.arctan2(-nu, -nv)

max_u, min_u … 楕円上でのu の最大値と最小値
max_v, min_v … 楕円上でのv の最大値と最小値
center_u, center_v … 楕円の中心

Post Views: 752