はじめに
今回のネタもM5Stack公式のtweetがきっかけです。
GiraffeVC okay. pic.twitter.com/VTBCliNbJI
— M5Stack (@M5Stack) January 10, 2020
この動画では、二本線で表示された「道路」を上手に走っています。UnitVは高い位置に下向きにセットされ、ライントレースを行っているのが判ります。
首が長いのでGiraffeRoverCVとなっていますね。
M5Stickさん、今回は開発中の様子も見せてくれています。
GiraffeRoverCV ing... pic.twitter.com/icAfmu0Ijr
— M5Stack (@M5Stack) January 10, 2020
映ってるモニターから多分こんな風に処理してると予測。
①画像を取得して2値化
②2本のラインを認識
③何かゴニョゴニョ画像処理してセンターラインに変換
④センターラインの傾きθと幅方向の変位xを取得してStickCに送信
⑤取得したデータをもとにStickCでステアリング制御
やりたいこと
今回は、やり方が良くわからないので画像処理の練習がしたい訳では無いので、2本線→1本線への変換は割愛して、1本線のライントレースを行います。
手抜きする代わりに信号機を作り、灯火を検出させてstop/goの制御を追加します。
その方が絵になりそうですしね。
できたもの
おおよそやりたかった動きは実現できました。
line trace + 信号機検出も出来た。 pic.twitter.com/2mg7ivHA5s
— AIRPOCKET＠rastaman vibration (@AirpocketRobot) January 19, 2020
ちなみにはこの動きはベストエフォートです。
自分の影をラインと誤検知して、コースを外れる事が良くあります。対策としてはroverにヘッドライトを着けて取得画像を安定させるか、2値化させて識別精度を上げるかなどが考えられますが、
あと、夜しか実験出来ていないので、昼や異なる照明下では認識率が落ちると思われます。
画像処理の方法とUnitVの配置
このキリンさんの特徴はカメラで路面を見てライン検出を行っているところです。今回は前方上方にある信号機の灯火も検出しないといけないため、下方と前方の両方を視野に納めなくてはいけません。
解決策としては二通りの方法を考えましたが、コーディングが難しそうなので画像処理の負荷が大きそうなので、①の物理的な改造で対処しました。
UnitV取付け案
①採用案
・カメラは下向きに取付け、レンズの前に設置した鏡で視野の半分を前方確認に用いる。
メリット：ライン認識とオブジェクト認識を画像エリアごとに実行するだけでよい
デメリット：鏡を取り付ける治具の工作が必要
②不採用
・カメラは前向きに取付け、広角レンズでラインと信号の両方を視野に収める。
メリット：ソフト面だけで対処可能。
デメリット：コーディングが難しそう画像処理負荷が大きそう（地面の画像を変形させて上から俯瞰した画像図に書き換えてから2値化してノイズ取ってライン認識させて・・・）
実際の設置状況
全体像はこんな感じ。
前方確認用の鏡は自作。名付けてM5stick Mirror Hat!!
3㎜厚のプラバンをUnitVの幅に合わせて切り出しコの字型に組んだ後、同じく3㎜厚のプラバンに貼り付けた鏡を適当な角度を持たせて接着しています。水平面と鏡のなす角が45°だと真正面を向くので、若干上を向く様40°位のイメージで適当に位置合わせしています。後で調整出来るので適当でＯＫ。
UnitVへは両面テープで固定しているだけです。テープ止めする際にスペーサーを入れれば鏡の角度は調整可能です。
鏡の加工を行う際には、カット面を鋭角に加工するのがコツです。ピントが合わない近距離に設置しますので、鋭角な線状に加工してもボケて大きな死角になります。カット面が粗かったり90°付近だと、断面が大きく映り込んで可視領域が大幅に減ります。
鏡はなんでもいいのですが、これを使いました。
Cloud ElevenⓇ スマホ本体やスマホケース等に貼って使える ミラーシール 小型鏡シール
スクエア型を注文したつもりが間違えて雲型注文しちゃったので変な形の鏡になってます。100円ショップでも売ってそうです。
UnitVのコーディング
信号検出は色認識で
信号の灯火の認識は使い慣れたimage.find_blobs()を用います。
今回は手抜き信号のため、基板にLEDを載せただけのむき出しです。LED出力とblobsの閾値調整はしたのですが明るすぎて色飛びし、黄色と赤の識別が困難（実はできていない）です。もっと細かな制御を行うのであれば、LEDの前に拡散板をつけるなどして色差を大きくする必要があるでしょう。
また、信号は青でGo、黄で注意、赤でStopとしたいところですが、冗長なので赤のStop、それ以外はGoで済ましています。よって、信号の色の認識は赤が点灯しているか否かのみを検出しています。
ライン認識は最小二乗法で行う
当初、ライン認識は画像内の線分を検出するimage.find_lines()を使えばいいやと思って今しがたが、これでは曲線の（接線の）検出が出来ません。どうしたもんかと思ったらjimmy_olafさんにヒントを頂きました。有難う御座いますmm
この辺でしょうか。https://t.co/Jrv4kao8zN
— jimmy_olaf (@OlafJimmy) January 13, 2020
unitVに落とし込んでないので、不確かな情報です、すいません。
リンク先をまとめると、画像認識でライントレースするときは閾値でドット検出して、ドットの座標を最小二乗法で線形近似しなさいよ、との事でした。あとステアリングはPID制御しろよとか。
と言う事で、ライン認識はimage.get_regression()を用いて実装します。
UnitVのコード
#UnitV用ライントレース
#画像からblobsで信号読み取り、赤信号の見かけの大きさmaxredareaを送る
#画像の下1/3でregressionでlineを読み取りラインの重心cx、cy、ラインの傾きtheataを送る。
import sensor
import image
import lcd
from machine import UART
from Maix import GPIO
from fpioa_manager import *
lcd.init(freq = 15000000)
fm.register(34,fm.fpioa.UART1_TX)
fm.register(35,fm.fpioa.UART1_RX)
uart_out = UART(UART.UART1, 115200, 8, None, 1, timeout=1000, read_buf_len=4096)
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQVGA)
sensor.set_vflip(True)
sensor.set_hmirror(True)
#sensor.set_auto_gain(False)
#sensor.set_auto_whitebal(False)
sensor.run(1)
RED_lab_threshold =(93, 100, -10, 12, -8, 3)#赤信号認識の為の閾値rgb565タプル
while False:
uart_out.write('TEST\n')
utime.sleep_ms(100)
while True:
img = sensor.snapshot()
redsignal = img.find_blobs([RED_lab_threshold], x_stride = 2, y_stride = 2, pixels_threshold = 100, merge = True, margin = 20) #赤信号を検出
maxredarea = 0
if redsignal: #赤信号と同じ色を複数検出した場合は、最大面積のblobを残す
for i in redsignal:
area = i.area()
if area > maxredarea:
maxred = i
maxredarea = area
img = img.draw_rectangle(maxred.rect(),(255,0,0))
img2 = img.copy(); #ライン認識用の画像としてコピー
img2 = img2.draw_rectangle(0,0,160,80,fill = True) #ライン認識用の画像の上1/3をマスク
linep = img2.get_regression([(0, 23, -19, 33, -45, 17)], pixels_threshold = 100,robust=True) #最小二乗法で認識した線分の線形近似を取得
print(linep)
if linep:
print(linep.line()) #読み取ったラインのパラメータをシリアルモニタで確認
img.draw_line(linep.line()) #読み取ったラインをシリアルモニタに表意ⓙ
cx = int((linep.x1()+linep.x2())/2 - 80) #ラインの横方向のセンター値
cy = int((linep.y1()+linep.y2())/2 - 60) #ラインの縦方向のセンター値
hexlist = [(cx >> 8) & 0xFF, cx & 0xFF, (cy >> 8) & 0xFF, cy & 0xFF ,(int(linep.theta()) >> 8) & 0xFF, int(linep.theta()) & 0xFF, (maxredarea >> 8) & 0xFF, maxredarea & 0xFF]
uart_out.write(bytes(hexlist))
print(cx,cy,linep.theta(),maxredarea) #シリアルモニタで送信データ確認
ポイントは、ライン認識させるために、不要な画像が映っている上半分と、鏡のエッジ領域が映り込んでいる部分をマスクして下1/3だけでライン認識させているところです。
StickCのコーディング
//use UnitV
#include <M5StickC.h>
HardwareSerial VSerial(1);
typedef struct
{
int16_t cx;
int16_t cy;
int16_t theta;
int16_t redarea;
}v_response_t;
uint8_t I2CWrite1Byte(uint8_t Addr, uint8_t Data)
{
Wire.beginTransmission(0x38);
Wire.write(Addr);
Wire.write(Data);
return Wire.endTransmission();
}
uint8_t I2CWritebuff(uint8_t Addr, uint8_t *Data, uint16_t Length)
{
Wire.beginTransmission(0x38);
Wire.write(Addr);
for (int i = 0; i < Length; i++)
{
Wire.write(Data[i]);
}
return Wire.endTransmission();
}
uint8_t Setspeed(int16_t Vtx, int16_t Vty, int16_t Wt)
{
int16_t speed_buff[4] = {0};
int8_t speed_sendbuff[4] = {0};
Wt = (Wt > 100) ? 100 : Wt;
Wt = (Wt < -100) ? -100 : Wt;
Vtx = (Vtx > 100) ? 100 : Vtx;
Vtx = (Vtx < -100) ? -100 : Vtx;
Vty = (Vty > 100) ? 100 : Vty;
Vty = (Vty < -100) ? -100 : Vty;
Vtx = (Wt != 0) ? Vtx * (100 - abs(Wt)) / 100 : Vtx;
Vty = (Wt != 0) ? Vty * (100 - abs(Wt)) / 100 : Vty;
speed_buff[0] = (Vty - Vtx - Wt) * 1.0; //left front モーターの出力を補正するために*1.0
speed_buff[1] = (Vty + Vtx + Wt) * 1.15; //right front 同じく*1.5
speed_buff[3] = (Vty - Vtx + Wt) * 1.15; //right back 同じく*1.5
speed_buff[2] = (Vty + Vtx - Wt) * 1.0; //left back 同じく*1.0 ※この補正値はroverCの機体毎に要調整
for (int i = 0; i < 4; i++)
{
speed_buff[i] = (speed_buff[i] > 100) ? 100 : speed_buff[i];
speed_buff[i] = (speed_buff[i] < -100) ? -100 : speed_buff[i];
speed_sendbuff[i] = speed_buff[i];
}
return I2CWritebuff(0x00, (uint8_t *)speed_sendbuff, 4);
}
void setup()
{
M5.begin();
M5.Lcd.setRotation(3);
M5.Lcd.fillScreen(RED);
VSerial.begin(115200, SERIAL_8N1, 33, 32);
Wire.begin(0, 26);
}
enum
{
kNoTarget = 0,
kLeft,
kRight,
kStraight,
kTooClose
};
v_response_t v_data; // Data read back from V
int vx = 0;
int vy = 0;
int vrotate = 0;
double Protate = -0.1; //回転速度制御の比例成分係数
double Pvx = -0.1; //平行移動速度制御の比例成分係数
void loop()
{
VSerial.write(0xAF);
if(VSerial.available())
{
uint8_t buffer[7];
VSerial.readBytes(buffer, 8);
v_data.cx = (buffer[0] << 8) | buffer[1];
v_data.cy = (buffer[2] << 8) | buffer[3];
v_data.theta = (buffer[4] << 8) | buffer[5];
v_data.redarea = (buffer[6] << 8) | buffer[7];
M5.Lcd.fillScreen(BLUE);
if(v_data.theta > 90) //検出したラインの角度を-90~+89に変換
{
v_data.theta = v_data.theta -180;
}
//UnitVから得たパラメータをLCDに表示
M5.Lcd.drawString("cx=",0,10);
M5.Lcd.drawNumber(v_data.cx,50,10);
M5.Lcd.drawString("cy=",0,20);
M5.Lcd.drawNumber(v_data.cy,50,20);
M5.Lcd.drawString("theta=",0,30);
M5.Lcd.drawNumber(v_data.theta,50,30);
M5.Lcd.drawString("red=",0,40);
M5.Lcd.drawNumber(v_data.redarea,50,40);
vx = static_cast<int>(v_data.cx * Pvx); //横移動速度を計算
vy = 15; //前進速度定義、redareaの面積が100ピクセル以上で停止
if(v_data.redarea > 100)
{
vy = 0;
}
vrotate = static_cast<int>(v_data.theta * Protate); //回転速度を計算
Setspeed(vx, vy, vrotate); //Duty ratio ±100 //得られた移動速度をroverCへ送信
}
}
速度のPID制御
StickCでは、UnitVから送られてきたデータをもとにUnitVの前進速度Vy、横移動速度Vx、回転速度Vrotateを計算し、RoverCへ送ります。
今回は前進速度Vyは定数とし、赤信号検出時のみ0にしています。
横移動速度と回転速度はPID制御するのが望ましいのですが、今回はVyを小さくとっているので比例成分Pのみで制御出来ています。速度を上げたい場合はI,D成分も含めた方が良いのでしょう。
停止位置の調整
赤信号認識時の停止位置は、blobsの大きさで決まります。信号までの距離が近づくと大きく見えるため、閾値を越えると接近したとみなして停止する、というロジックになっています。しかし、blobsの面積はばらつくため、鏡の取付け角度を調整して視野に入るタイミングを見た方が再現性の高い位置に止まれそうです。
モーター出力の補正
Roverへのモーター出力はSetspeed()関数で送っています。出力は0-100の整数で指定しますが、モーターにより出力のバラツキが有ります。今回はそれぞれのモーター出力のバランスを取るため、事前に調べた係数をかけて出力のキャリブレーションをしています。お好みで適当に。。。
まとめ
長々と書きましたが、前回のコードをベースにしているので、キリンさん本体はそれほど手間をかけずに作る事が出来ました。まとめるとこんな感じでしょうか。
・画像認識によるライントレースは最小二乗法を使おう。
・RoverCは組立不要で、コード開発に専念出来て超便利。
・もっとオリジナルなこともやらないといけない気がする。
・信号とコース作りが面倒。
・なぜ黄信号を点滅させてしまったのだろう？
・まとめがまとめになってない。
おまけ１ なぜか転載されてた
Combined #M5Atom on #RoverC, controlled by #M5Stack #JoyC. #MakerFaireBangkok2020 #M5StickC #MakerFaire pic.twitter.com/2s7BsbS4iI
— M5Stack (@M5Stack) January 20, 2020
削除されました。どうも別の動画と間違えてアップしていたみたいですね。
おまけ２ ゾウさん？
ToF Servo for RoverC. pic.twitter.com/Hw0RGE6m22
— M5Stack (@M5Stack) January 15, 2020
ServoとToF（距離計）ユニットを使ったマシンもアップされてましたね。迷路探索なんかが出来るのかな？M5Stack動物園シリーズ、次は何がでてくるのか楽しみです。