0から始める自動運転 for FPT2018 FPGA Design Competition 〜ライントレース編〜

# はじめに
FPT2018 FPGA Design Competition というコンペに参加してなんとか優勝しました。
開発記はチームメイトが書いてくれたので、興味のある方はこちらをご覧ください。[FPGAデザインコンテスト@FPT2018 開発記](http://lp6m.hatenablog.com/entry/2018/12/16/184054)
実のところ、ロボット, 自動運転, 画像処理に触れたことは全くなかったのですが、そんな私がロボットを動かすためにやったことや考えたことを書いていきます。
もし、今後似たコンペに参加される方の参考になれば幸いです。どちらかというと技術紹介というより、開発の雰囲気を掴んでいただければと思います。

詳しいルールはこちら（[英語](https://wwp.shizuoka.ac.jp/fpt-design-contest/contents-and-conditions/)）、もしくはこちら（[日本語](https://wwp.shizuoka.ac.jp/design-contest/contest_detail/)）をご覧下さい。
読むのが面倒な人のために簡単に説明すると、

- センサは普通のカメラだけ
- CPUはFPGAボード上のもの（私たちが使っていたZyboはだいたいラズパイ3の半分程度の処理能力）および、センサからの情報を送受信するためだけのArduino等のマイコン限定
- FPGAアクセラレータを何らかに利用している

といった要素を兼ね備えたロボットでコースを走るというルールです。
私たちの製作したZytleBotは、人形検知以外の課題をクリアできるように開発し、

* ライントレースによる走行
* 交差点
*  障害物回避
*  信号検知

が実装されています。
またROSを導入しており、ROBOTISさんのTurtleBot3を改造して足回りはOpenCRに任せています。
信号検知は機械学習を用いており、計算の一部にFPGAを用いてCPUの負担を軽減&高速化を行なっています。


さて、自動運転の話に戻ります。いきなり自動運転やろーって言ってもわりかしチンプンカンプンでした。
真下の線を読み取ってラインに沿って走る話や、逆にハイレベルすぎるLiDARを活用して障害物を避けるなどという話はよくありますが、今回のような「割とまっすぐなコースだよ〜、交差点あるよ〜、エッジデバイスだけで処理してね」という競技に参加してこうこうこうした！という話はあまりなかったです。ので、結局は一から全部考えてました。それほど難しいことをしている訳ではありませんが、一回理解すれば実装は簡単ということが多かったので、せっかくなので重要な部分を中心的にアウトプットしておきます（もし需要があれば連載ということで）


### 前提
言語はC++を使います。

また今回の競技は平坦な地面のため、地面を俯瞰画像にして白色を2値化すると非常に考えやすくなるのでそのように処理しています。(座標変換するだけでできますが、調整必須で長くなるので詳しいコードは別の機会に...)

![fig1.png](https://qiita-image-store.s3.amazonaws.com/0/276457/ece8e9f2-7923-0522-d956-b46b6266b8e9.png)

(右が元画像で左上が変換後の画像)


さて、まず最初はライントレース（車線検出）をやって行きます。色々やり方がありますが、私は勝手に0~3次元に分けて考えていました。


# 0次元
0次元、つまり点です。画像のように、ある点をみて、その点が理想の位置からどちらにどれだけ離れているかで進路を決定します。下のgifでは赤が検出点、青が走行したい理想のラインです。

![linetrace1.gif](https://qiita-image-store.s3.amazonaws.com/0/276457/ff093183-03cb-825a-1c07-40d49381f2be.gif)


#### メリット
処理が軽い、実装が楽、ぐにゃぐにゃした道にも対応可能

#### デメリット
外乱に非常に弱い（光が一筋ピッと差し込むだけで変な方向に行く）

#### サンプルコード
ちょっとだけノイズ耐性入れてます

```cpp:sample1.cpp
/** あらかじめセットされている変数
* cv::Mat road_img : 2値化画像
* double DETECT_HEIGHT = 0.7 :検出するy座標の高さの比
* int detected_line_x : (前回)検出されたx座標
*/
int width = road_img.size().width
int height = road_img.size().height

cv::Mat road_hough;

// Canny画像に変換して輪郭を抽出することで、直線が二本の線に見える！
// 二本の線の幅が一定以下のものを直線とし、その中点を検出点とする。
cv::Canny(road_img, road_hough, 50, 200, 3);


int temp_detected_line = detected_line_x; // とりあえず前回の検出結果を値として入れておく

// 複数候補が存在する場合、直前のライントレースとの差がもっとも少ない部分を利用する
int temp_dif = width; // 差：初期値はとりあえず最大に


for (int i = 0; i < width; i++) { // 1ピクセルずつ見ていく
    int p = road_hough.at<uchar>(height * DETECT_HEIGHT  - 1, i); // 0以外の値を探す(この画素アクセスの方法が早いらしい)
    if (p) {
        for (int j = i + 1; j < i + width / 10; j++) { // 白を見つけたらそこからwidht/10ピクセル以内の白の輪郭を探索する
            int q = road_hough.at<uchar>(height * DETECT_HEIGHT - 1, j);
            if (q) { // 白の輪郭が発見された場合の処理、これを仮の白線とする
                int this_dif = std::abs((i + j) / 2 - detected_line_x); // 今検出された仮の白線と前回との差を計測
                if (this_dif < temp_dif) {
                    temp_dif = this_dif; // もし差が最小（＝前回検出された値にもっとも近い）ならばこれを白線として更新
                    temp_detected_line = (i + j) / 2;
                }
            }
        }
    }
}
detected_line_x = temp_detected_line; // detected_line_xを更新
// デバッグ出力
// cv::circle(road_img, cv::Point(detected_line_x, height * detectHeight), 3, Scalar(0,0,255), 3, 4);
```

# 1次元
ハフ変換を行い、直線を検出します。（私は確率的ハフ変換を利用していました）

![linetrace2.gif](https://qiita-image-store.s3.amazonaws.com/0/276457/b9cfaa97-d911-3467-ff75-cb07e16bad49.gif)


#### メリット
実装はわりかし簡単
車線の傾きが取得できる！（直線コースにおいては車線の傾き=車体の傾きとなるので、これを二次利用すると色々捗る）

#### デメリット
外乱にまだ弱い
カーブやぐねぐねした道に対応できない

#### コード
```cpp:sample2.cpp
/*
 * road_img:路面画像
 * HEIGHT: 縦幅
 */
cv::Mat temp_dst, temp_color_dst;
cv::Canny(road_img, temp_dst, 50, 200, 3);
std::vector <cv::Vec4i> lines;
cv::HoughLinesP(temp_dst, lines, 1, CV_PI / 180, 20, HEIGHT*0.4, 5); // 要調整 詳しくはopenCVのドキュメント

/* デバッグ出力
for (size_t i = 0; i < lines.size(); i++) {
    cv::line(road_img, cv::Point(lines[i][0], lines[i][1]),
             cv::Point(lines[i][2], lines[i][3]), cv::Scalar(0, 0, 255),
             3, 8);
}
*/

// 以下、検出されたlinesを処理する
```

# 2次元
一定領域に対してテンプレートマッチングやルールベースでの処理など色々することによってなんか得る。。。
まあここからは完全に画像処理の世界に入るので、各自論文読んだりアルゴリズム考えたりして頑張りましょうということで…

####メリット
画像から抽出できる情報を全て活かせる

####デメリット
簡単にできたら苦労しないんじゃーー

ちなみに2次元処理が極まってくると立命館大学が開発中のこちらのSLAMみたいになるらしいです（なお現在はもっと進化しているとのこと）

<blockquote class="twitter-tweet" data-lang="en"><p lang="en" dir="ltr">Our Zybot-R2Z2 for the design competition now recognizes indicators and construct the map with the labels. All the processing is done within a Zynq7020 alone (except for debugging functions). <a href="https://t.co/0TG5y3SnBx">pic.twitter.com/0TG5y3SnBx</a></p>&mdash; 春巻 (@izumitomonori) <a href="https://twitter.com/izumitomonori/status/1065099751366901761?ref_src=twsrc%5Etfw">November 21, 2018</a></blockquote>
<script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>


# 3次元
ここからは今回のコンテストに関係ないですが。
2次元までの処理はあくまでコンテスト向けの技術で、これだけで実際の道路を走るのは難しいです。(さっきのSLAMくらいまでできると話は別ですが)
道路の凹凸、昼夜の照度の変化、多様な障害物やリアルタイム性の要求など、対応しなければならないことは山ほどあります。
LiDARや他センサから得られた様々なデータから必要な情報を抽出し、、、
とここまで書いてこんな文章は他に星の数ほどあることに気づきました。適当に検索してそっちをご覧くだしゃ。


# おわりに
どうでしょうか、100人いたら99人くらいがライントレース簡単じゃんという感想になってる気がします。
**まあ実際にはデメリットの「外乱に弱い」が本番で大きく効いてきて、軽視してると現地であぼーんします**

今後も似たようなコンテストがあるらしい？ので、参加したいけど何から始めればいいか分からないといった方の参考になれば幸いです。

gifみたいに動画出力して確認したいゾ！という方はもう少しお待ちください、~~来週締め切りの論文書き上げたら~~もう少ししたらデバッグ環境含めZytleBot公開に向けて整備していきます。