初心者がAIチャレンジやってみた（11）：障害物回避モジュールの改良①

# 概要
「初心者がAI Challengeやってみた」シリーズの第11弾です。
第10弾では障害物回避をするモジュールを作成しました。
今回はその時にあった２つの課題を解決していきます。

本シリーズでは[Japan Automotive AI Challenge 2023](https://www.jsae.or.jp/jaaic/)にautoware初心者の筆者が試行錯誤しながら挑戦する記録を公開しています。自動運転に興味があるけどプログラミングに自信が無い方などの参考になれば幸いです。

前の記事はこちら：
* 第1弾：[初心者がAIチャレンジやってみた(1)](https://qiita.com/h_bog/items/86fba5b94b2148c4d9da)：Autowareを動かしてみる
* 第2弾：[初心者がAIチャレンジやってみた(2)](https://qiita.com/h_bog/items/a36df3042a342bd2a0c5)：1つ目の障害物の回避成功
* 第3弾：[初心者がAIチャレンジやってみた(3)](https://qiita.com/h_bog/items/849a4241cdef257f3b3a)：2つ目の障害物の回避成功
* 第4弾：[初心者がAIチャレンジやってみた(4)](https://qiita.com/h_bog/items/8bce776fef0a39676c46)：全障害物回避達成！（全パラメータ公開）
* 第5弾：[初心者がAIチャレンジやってみた(5)](https://qiita.com/h_bog/items/4ed94ef6256ac5ae9c03)：開発環境の再構築（番外編）
* 第6弾：[初心者がAIチャレンジやってみた(6)](https://qiita.com/h_bog/items/075543f30f9b3432fa1d)：Autoware-Miniを使ってみる
* 第7弾：[初心者がAIチャレンジやってみた(7)](https://qiita.com/h_bog/items/bcfd5648afa8b6dee5b3)：外部モジュール導入（失敗）
* 第8弾：[初心者がAIチャレンジやってみた(8)](https://qiita.com/h_bog/items/368543a4b10b356db0e5)：自作パッケージの導入
* 第9弾：[初心者がAIチャレンジやってみた(9)](https://qiita.com/h_bog/items/c84c4886c0ba0e6c1724)：障害物回避モジュールの作成①
* 第10弾：[初心者がAIチャレンジやってみた(10)](https://qiita.com/h_bog/items/a2c2cbdcec6a852bfcd5)：障害物回避モジュールの作成②（成功？）

:::note warn
筆者はautoware初心者です。
説明等が正確でない可能性があるので本記事だけではなく他の記事や[autowareのドキュメント](https://autowarefoundation.github.io/autoware-documentation/main/)も確認するようにしてください。
:::

# 目標
前回遭遇したいくつかの課題を解決するのが今回の目標です。
具体的には以下の課題の解決を目指します：
- [ ] 走行可能エリア外に経路が生成されてしまう
- [ ] 障害物に近づくとOccupancy Grid Mapから消えて衝突してしまう

# 課題１：走行可能エリア外に経路を生成してしまう
この問題は主に以下の２パターンあります：

1. 障害物を大きく迂回してしまうパターン
![無題のプレゼンテーション (2).png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3483805/2bd5e48f-7632-92e0-0736-c8840913f53f.png)

1. L字カーブを一直線で進もうとしてしまうパターン
![無題のプレゼンテーション (3).png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3483805/542729a2-6f34-c537-e35d-7866269ae3bb.png)

少し走行可能エリアから外れている程度であれば`ObstacleAvoidancePlanner`が`Path`を`Trajectory`に変換する時に修正してくれますが、上記の例のように大きく外れてしまうと対応しきれないようで、車両が停止してしまいます。

## 対策：走行可能エリアを「壁」で仕切る

走行可能エリアは`BehaviorPathPlanner`が配信している `/planning/scenario_planning/lane_driving/behavior_planning/path` というトピックに含まれている走行レーンの左端と右端の情報から得ることができます（[autoware_auto_planning_msgs/Path](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_planning_msgs/msg/Path.idl)の`left_bound`と`right_bound`）。

走行レーンの両端を「壁」（＝障害物）で表すことができれば、走行可能エリア内を走行するようなポテンシャル場を計算できそうです。

レーンの端の情報はいくつかの点（[Point](http://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/Point.html)）として表されているので、点同士を直線で結び、それら直線上の座標に障害物があるというふうにプログラムします。
以下のページの画像を見るとイメージがしやすいかもしれません。

https://tier4.github.io/AWSIM/Components/Environment/LaneletBoundsVisualizer/

## 結果
レーンの「壁」は作れたが、壁からの反発力によってレーンを外れる経路が生成されてしまう。
![無題のプレゼンテーション (4).png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3483805/47dab9ca-0c11-1d40-7259-d38d2bd98331.png)

目的地からの引き寄せる力を10倍にしたらいい感じの経路が生成できた。
![Screenshot from 2023-08-21 12-15-01.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3483805/09944c27-0699-8c81-ebc4-1f6eabcca80c.png)


# 課題２：近づくと障害物が消えてしまう

以下の動画からも分かるように、車両が障害物に近づくとその障害物がOccupancy Grid Mapから消えてしまう問題があります（原因は不明です。LiDARからのレーザーが車両自体に遮られて近くの障害物が検知できないとか？）。

Occupancy Grid Mapに写っていないと障害物を無視した経路が生成されてしまうため、障害物にぶつかりながら車両が進んでしまいます。
<iframe width="560" height="315" src="https://www.youtube.com/embed/z8zZ01CzkjM" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture; web-share" allowfullscreen></iframe>

こちらに関しては対策案が２つあります：
1. 一定範囲内に写った障害物の位置情報を保持する
1. 写った障害物の位置情報を一定時間保持する

①のメリットは障害物の位置さえ把握できれば（比較的）簡単に実装できること。デメリットは障害物が移動した場合でも位置情報を保持し続けてしまい、動けなくなってしまう可能性があるということ。
②のメリットは障害物が移動した場合、一定時間経過すればその情報を忘れることができること。デメリットは各障害物を観測した時間を記録しておかないといけない点や、静止している障害物に近づいて一定時間経過してしまえばその障害物も忘れてしまうこと。

今回はとりあえず実装がより簡単そうな対策①を実装してみます。

## 対策：一定範囲内の障害物情報を保持する
車両の周囲○m以内に写った障害物の情報を保持し続け、車両が移動して障害物が範囲外になったらその障害物を忘れる、という風にしてみる。

レーンの壁に対しても同じ処理を行った。

## 結果
課題①と課題②の改良を組み合わせて動かした結果がこんな感じ。
中心のプロットは車両が考慮している障害物と経路を表しています（色が同じで分かりにくいですが中心部から伸びている点線が経路、それ以外の点が考慮している障害物です）。
<iframe width="560" height="315" src="https://www.youtube.com/embed/hOoG3hWpc14" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture; web-share" allowfullscreen></iframe>

良い点：
- 障害物の位置情報が保持されている
- 基本的に走行エリア内に経路が生成されている
→後半のL字カーブをまぁまぁいい感じに曲がれている
- 車両がクネクネ動かない（上記２点による副作用？）

悪い点：
- 引き寄せる力を強くした影響で障害物回避がおろそかになってしまっている
→パラメータのチューニングが必要かも

コードが汚いので今回は載せないつもりですが、もし需要があれば載せます。

## 考察
引き寄せる力の強さに関するチューニングをすると生成される経路が結構変わります。

まだ実験の途中ですが今のところ以下のように途中で引き寄せる力を弱くするとそこそこの動作をしてくれます。
- 最初は500:1くらいの比率→前進する
- 障害物エリアは50:1くらいの比率→前進しつつも障害物回避する（回避しすぎて次の障害物が回避できない？）
<iframe width="560" height="315" src="https://www.youtube.com/embed/6itbyBoKC20" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture; web-share" allowfullscreen></iframe>

もしかしたら最初の部分は車両の後ろに壁を作ることで、わざわざ引き寄せる力を調整しなくても同じ効果が得られるかもしれません。
また、連続して障害物を回避するためには目的地を分割して設定してみたりする工夫が必要かもしれません。

# 今後の予定
以下のジレンマが発生している：
- 引き寄せる力を下げる→走行可能エリアから出ようとしてしまう
- 引き寄せる力を上げる→障害物を無視してほぼ直進するような経路になってしまう

解決案①：車両の後ろに「壁」を作る→走行可能エリア内に車両を「閉じ込めて」前進するように促す
解決案②：レーンの壁と障害物で違う重みを設定する→走行可能エリア内にとどまりつつ、障害物の回避を優先する（実際にどうなるかは不明）
解決案③：他にも目的地を複数個用意してあげることでレーンに沿った経路が生成されやすくなるかも？

その他の改善点もいくつかあるが、まずは上記の問題の解決に注力したい。
改善案①：走行可能エリア外の障害物は無視する→現状は走行可能エリア外にあるコーンや電柱なども障害物として考慮してしまっている。もしかしたら経路生成に悪影響を与えているかもしれない。
改善案②：バック走行も可能にする。特に後半部分では切り返しなどが必要な箇所がある。ただし現状のポテンシャル法では車両の動きに関する特性などを一切考慮していないのでこれはなかなか難しそう。