画像認識した白線に沿って自走するラズマウス５号機の製作記録
ラズマウスに取り付けたカメラ画像を、OpenCVを使って白線を認識して、左右白線の中央線に沿って自走するラズマウス５号機の製作記録。
左右白線のセンターとラズマウスの進行方向のズレ量を最小にするようにPID制御して自走する。
●白線を認識するラズマウスの構成部品
＜WEBカメラの位置と撮影範囲＞
●白線を認識するプログラム構成
白線の認識は、OpenCVでカメラ画像を二値化し、輪郭から白線候補を選択し、左右白線の条件で絞り込みを行った。そのプログラム例は次のモジュールで構成。
rasPosition：白線を認識して、左右白線の中心線からラズマウスの進行方向のズレ量(pixel)を算出
├（１）detectWhiteLines：白線候補を抽出
│ ├①cv2.cvtColor：入力画像をグレースケール化
│ ├②cv2.threshold：グレースケール画像を閾値で抜き出し
│ ├③cv2.findContours：RETR_TREEで全輪郭を検出し全階層情報を得て、その中から、
│ │ hierarchyの親が無く(-1)、親parentが0の輪郭のみ抽出。
│ └④isThisWhiteLine：抽出した輪郭が白線かどうかを評価
│ 評価条件：最小長さ>50pixel、最小太さ(Width/Length)>0.05、最大太さ(W/L)<0.3
│ 傾き>0.3(撮影方向に対して横向きの輪郭は除く)
├（２）selectVerticalLines：白線候補の中から進行方向に沿う縦方向の２本の白線を選択
├（３）drawWhiteLines：選択した白線を画面に描画
└（４）calcCenterPos：左右白線の中心線からラズマウスの進行方向のズレ量(pixel)を算出
＜白線認識のプログラム事例＞
# -*- coding: utf-8 -*-
import numpy as np
import cv2
import time
COL, ROW = 640, 480
CV_FONT = cv2.FONT_HERSHEY_SIMPLEX
BLUE = (255, 0, 0)
GREEN = (0, 255, 0)
RED = (0, 0, 255)
YELLOW = (0, 255, 255)
# 白線選択の条件
LINE_MIN_LEN = 50 # 最小長さ(pixel)
LINE_MIN_RATIO = 0.05 # 最小太さ W/L > 0.05
LINE_MAX_RATIO = 0.3 # 最大太さ W/L < 0.3
class CONT_LINE:
def __init__(self, contNo, direct, center, boxNP, lineC, lineL):
self.contNo = contNo # 輪郭番号
self.direct = direct # 縦線(0)/横線(1)
self.center = center # 重心位置[x,y]:pixel
self.boxNP = boxNP # 白線外形矩形[[x0,y0],[x1,y1],[x2,y2],[x3,y3]]:pixel
self.lineC = lineC # センターライン[[xs,ys],[xe,ye]]:pixel
self.lineLength = lineL # 白線長さ（boxの長辺)
class WHITE_LINES:
def __init__(self, thresh):
self.frame = 0
self.thresh = thresh
self.prevPosCol = int(COL / 2)
self.prevOffsetL = 0
self.prevOffsetR = 0
self.prevL = 0
self.prevR = 0
def calcCenterPos(self, whiteLines, indexL, indexR):
if indexL > -1 and indexR > -1: # 2本の白線がある場合
xsL = whiteLines[indexL].lineC[0][0] # 左白線のxs
xsR = whiteLines[indexR].lineC[0][0] # 右白線のxs
posCol = int((xsL + xsR) / 2)
self.prevOffsetL = posCol - xsL
self.prevOffsetR = posCol - xsR
elif indexL > -1: # 左の白線しかない場合
xsL = whiteLines[indexL].lineC[0][0] # 左白線のxs
posCol = xsL + self.prevOffsetL # 左白線から右へoffset
xsR = posCol - self.prevOffsetR
else: # indexR > -1 右の白線しかない場合
xsR = whiteLines[indexR].lineC[0][0] # 右白線のxs
posCol = xsR + self.prevOffsetR # 右白線から左へoffset
xsL = posCol - self.prevOffsetL
return posCol, xsL, xsR
def drawWhiteLines(self, whiteLines, indexL, indexR):
# 選択された白線(indexL, indexR)を描画
lines = [indexL, indexR]
for i in range(2):
if lines[i] != -1:
boxNP = whiteLines[lines[i]].boxNP
[[x0,y0],[x1,y1]] = whiteLines[lines[i]].lineC
cv2.line(self.frame, (x0, y0), (x1, y1), GREEN, 2) # センターライン
cv2.drawContours(self.frame, [boxNP], 0, RED, 2) # 認識白線を赤で囲む
c1x, c1y = int(COL/2), 0
c2x, c2y = int(COL/2), ROW
cv2.line(self.frame, (c1x, c1y), (c2x, c2y), YELLOW, 2) # ラズマウス進行方向
return
def isThisWhiteLine(self, cnt, box, rectR, minLen, minRatio, maxRatio):
# 白線候補を抽出、lineStat=True、中心線：cy = a * x + b
if rectR[1][0] > rectR[1][1]:
boxL, boxW = rectR[1][0], rectR[1][1]
else:
boxL, boxW = rectR[1][1], rectR[1][0]
if boxL < 0.001:
boxRatio = 10000
else:
boxRatio = boxW / boxL
if boxL >= minLen and (boxRatio > minRatio and boxRatio < maxRatio):
lineStat = True
M = cv2.moments(cnt)
cx = int(M['m10']/M['m00']) # col
cy = int(M['m01']/M['m00']) # row
boxLen0 = (box[1][0] - box[0][0]) ** 2 + (box[1][1] - box[0][1]) ** 2
boxLen1 = (box[2][0] - box[1][0]) ** 2 + (box[2][1] - box[1][1]) ** 2
if boxLen0 < boxLen1:
bp = [[0, 1], [2, 3]]
else:
bp = [[1, 2], [3, 0]]
bpCx, bpCy = [0,0], [0,0]
for k in range(2):
bpCx[k] = int(box[bp[k][0]][0] + (box[bp[k][1]][0] - box[bp[k][0]][0]) / 2.)
bpCy[k] = int(box[bp[k][0]][1] + (box[bp[k][1]][1] - box[bp[k][0]][1]) / 2.)
vx = bpCx[1] - bpCx[0]
vy = bpCy[1] - bpCy[0]
# 白線の傾きvy/vx、点(bpCx[0],bpCy[0])を通る直線, cy = a * x + b
if abs(vx) > 0.0: # 垂直でない場合
a, c = vy/vx, 1
b = bpCy[0] - a * bpCx[0]
else: # 垂直(vx=0)の場合, x=-b
a, b, c = 1, -bpCx[0], 0
else: # 基準を満たさないbox
a, b, c, cx, cy = 0, 0, 0, 0, 0
lineStat = False
return lineStat, (cx, cy), a, b, c, boxL
def selectVerticalLines(self, whiteLines):
# 白線候補の縦線の中から２本選ぶ
vLines = []
for k in range(len(whiteLines)):
if whiteLines[k].direct == 0:# 縦線だけを抽出
vLines.append([whiteLines[k].lineLength, k])
if len(vLines) >= 2:# 縦線が２本以上あった場合、長い白線から２本選択
l0 = vLines.index(max(vLines))
vLines[l0][0] = 0
l1 = vLines.index(max(vLines))
k0, k1 = vLines[l0][1], vLines[l1][1]
xe0, xe1 = whiteLines[k0].lineC[1][0], whiteLines[k1].lineC[1][0]
if xe1 < xe0:
indexL, indexR = k1, k0
if xe0 < COL/2 and xe1 < COL/2:
indexL, indexR = k0, -1
elif xe0 > COL/2 and xe1 > COL/2:
indexL, indexR = -1, k1
else:
indexL, indexR = k0, k1
if xe0 < COL/2 and xe1 < COL/2:
indexL, indexR = k1, -1
elif xe0 > COL/2 and xe1 > COL/2:
indexL, indexR = -1, k0
lineStat = True
elif len(vLines) == 1: # 縦白線が１本しか検出できない時
k0 = vLines[0][1]
if whiteLines[k0].lineC[1][0] >= COL/2:
indexR, indexL = k0, -1
else:
indexR, indexL = -1, k0
lineStat = True
else: # 縦白線が認識されなかった時
lineStat, indexL, indexR = False, -1, -1
return lineStat, indexL, indexR
def detectWhiteLines(self):
# frameの中から、白線の輪郭を検出して、白線候補を検出。
imgray = cv2.cvtColor(self.frame, cv2.COLOR_BGR2GRAY) # 入力画像をグレースケール化
retVal, imgThresh = cv2.threshold(imgray, self.thresh, 255, 0) # グレースケール画像を閾値で抜き出し
imgThresh, contours, hierarchy = cv2.findContours(imgThresh, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
whiteLines=[]
for i in range(len(contours)):
if hierarchy[0][i][3] > 0: # hierarchyの親がなし(-1)、親parentが０の輪郭のみ抽出
continue
cnt = contours[i]
rectR = cv2.minAreaRect(cnt) # 回転を考慮した外接矩形 rectR : ((x,y), (w,h), angle)
box = cv2.boxPoints(rectR) # 4角のコーナーの座標
lineStat, (cx, cy), a, b, c, lineLen = self.isThisWhiteLine(cnt, box, rectR, LINE_MIN_LEN, LINE_MIN_RATIO, LINE_MAX_RATIO)
# 白線の場合lineStat = True、白線の中心線cy = a * x + b
if not lineStat: # 白線でない場合
continue
boxNP = np.int0(box) # 整数に
if ( abs(a) > 0.3 ): # 縦方向の白線
direct = 0
x0, y0, x1, y1 = int(-b / a), 0, int((c * ROW - b) / a), ROW
else: # 横方向の白線
direct = 1
x0, y0, x1, y1 = 0, int(b), COL, int(a * COL + b)
whiteLines.append(CONT_LINE(i, direct, [cx,cy], boxNP, [[x0,y0],[x1,y1]], lineLen))
return whiteLines
def rasPosition(self, frame):
self.frame = frame
whiteLines = self.detectWhiteLines() # 白線認識
lineStat, indexL, indexR = self.selectVerticalLines(whiteLines) #２本の白線を選択
if lineStat: # 1本 or ２本選択できた時
self.drawWhiteLines(whiteLines, indexL, indexR) # 左右白線の赤枠、センターラインなどを描画
posCol, xsL, xsR = self.calcCenterPos(whiteLines, indexL, indexR) # 白線センター位置の計算
else:
posCol = self.prevPosCol
xsL, xsR = self.prevL, self.prevR
cv2.circle(self.frame,(posCol, 20), 4, RED, -1) # 白線中央位置の描画
strData = "{:5d}".format(int(posCol - COL/2))
cv2.putText(self.frame, strData, (posCol, 50), CV_FONT, 0.5, GREEN, 1, cv2.LINE_AA, False)
self.prevPosCol = posCol
self.prevL = xsL
self.prevR = xsR
return posCol, self.frame
"""メイン関数"""
if __name__ == '__main__':
cap = cv2.VideoCapture(0)
iDetect = False
wLines = WHITE_LINES(thresh=160) # 白線検出の閾値=160
while(True):
ret, frame = cap.read() # WEBカメラからの画像をキャプチャして表示
if iDetect:
posCol, frame = wLines.rasPosition(frame) # ２本の白線を認識し、軌道のズレ量posCol(pixel)を返す
cv2.imshow('WEB CAMERA', frame) # 画像情報を描画
k = cv2.waitKey(1) & 0xFF
if k == ord('q'): # 終了
print("QUIT")
break
elif k == ord('d'): # ライン検出
iDetect = True if not iDetect else False
cap.release()
cv2.destroyAllWindows()
●２本の白線の中央に沿って自走するラズマウスの制御方法
ラズマウスの進行方向と、白線中央からのズレ量は、ラズマウスの車軸から前方\(400mm\)の位置で計測した値。速度\(v\)で前進するラズマウスが、\(400mm\)前方のズレ量\(\Delta d\)を\(0\)（ゼロ）になるように、左車輪と右車輪の速度差をそれぞれ、\(v(1\ -\ \alpha)、v(1\ +\ \alpha)\)とすることで旋回させる。
PID制御量を\(\Delta u\)とすると、\(\alpha = k \Delta u\)でコントロールすることになる。
\(r\theta\ =\ v(1\ -\ \alpha)\Delta t\) ・・・・・ (1)
\((r\ +\ W)\theta\ =\ v(1\ +\ \alpha)\Delta t\) ・・・・・(2)
(1)と(2)より
\(\displaystyle r\ =\ \frac{W}{2}(\frac{1}{\alpha}\ -\ 1)\)
\(\displaystyle \theta\ =\ \frac{2\alpha v\Delta t}{W}\)
\(\Delta d=20mm、L=400mmの時、\Delta t後に\Delta d=0\)とすると、
\(\displaystyle\theta=\tan^{-1}(\frac{20}{400})=0.05\)
\(\displaystyle\alpha = \frac{\theta}{2}\frac{W}{v\Delta t}\simeq \frac{0.05}{2}\frac{135}{400}= 0.008\)
\(\displaystyle\alpha\ =\ k\Delta u\) とすると、\(\displaystyle\Delta u=20mm\)の時、\(\displaystyle k=\frac{0.008}{20}=0.0004\)
左右車輪が指定速度になるまで時間ロスがあるので、感度を10倍に上げて、\(k=0.004\)とした。
＜ラズマウスの定常速度＞
ラズマウスの車輪径：\(\phi70mm\)
ステッピングモーター(SM-42BYG011)の1回転のステップ数：\(200steps\)
ドライバー(L6470)の定速前進速度：\(10,240=10 \times 1,024\)、分解能\(=0.015step/sec\)
走行速度\(v=70\pi \times 10,240 \times 0.015 \div 200 = 168.8mm/sec\)
\(\alpha：0～0.2\)
●白線認識ラズマウスの走行テスト
左右白線の間隔を約180mmとして、全長約3500mmのテストコースを設定。テストコースの前半は直線で、途中で右カーブしている。
●PID制御のログ
PID制御のパラメータは、Kp=0.5、Ki=0.5、Kd=0.08とした。以下のグラフは、0.12sec毎に記録した、軌道からのズレ量Δdと、PID制御量(u)、計算上の修正後のズレ予測量(d)、ラズマウスの走行軌跡を表している。まだパラメータのチューニングが必要だが、大きく蛇行せず自走している。
右カーブに入る時、ラズマウスの軌道が左側に大きくズレ、それを補正する制御が働き始めるが、ズレ量(Δd)を計測しているのは、前方400mmの位置なので、その位置に到達するまでに少しずつズレ量を補正している。
●画像認識した白線に沿って自走するラズマウス(YouTube)
ラズマウスに取り付けたWEBカメラでキャプチャした画像から白線を選択して、その左右白線の中線とラズマウスの走行方向のズレ量を最小にするようにPID制御して自走するラズマウス５号機。白線の認識と選択はOpenCVを使った。
