Autowareの紹介と課題（2/2）

はじめに

2016 ROS Advent Calendar 23日目は前回の続きです。

Autowareの動作

グローバルパスプランニング

グローバルパスプランニングとは、スタート地点からゴールまでのルート探索です。
つまるところカーナビで探索するような経路です。
逆に言えば、カーナビですでに実現できているので、技術的に難しいところではないはず。

Autowareでは、経路はwaypointという一定間隔の離散的な点の集合で表しています。
このwaypointは位置(x,y,z)以外に方向・速度の情報を持っています。
下のスクショの赤色の矢印がwaypointです。

Autowareには、waypointを生成する機能が用意されています。
ちなみに、Githubのデータの中にwaypointの経路情報ファイルも入っています。
https://github.com/CPFL/Autoware#sample-data

waypointの経路情報生成手法

大きく別けて3つの生成手法があります。

１つは、waypointはベクターマップの車線情報とインクリメントP社のナビアプリMapFanを組み合わせることで、自動的にwaypointを生成する手法。
２つ目は、MapFan無しで、ベクターマップのみで自動生成する手法。
３つ目は、走行したパスからwaypointを生成する手法。

私は、３つ目の走行したパスからwaypointを生成する手法しか分かっていないので、それについて説明します。

走行したパスからwaypointを生成

自己位置情報と速度・向き情報あれば経路を生成することが出来ます。
自己位置情報は自己位置推定で取ることが出来ます。
速度・向き情報は、車体からCANで取り出してもいいですし、自己位置情報から生成することも出来ます。自己位置情報から生成する場合は、Computingタブのvel_pose_muxを起動してください。

あとは、Computingタブ右側のwaypoint_saverのappをクリックし、保存場所の指定後、waypoint_saverにチェックを入れれば完了です。
チェックを外したら、保存完了となります。

waypointの読み込み

読み込み方はレクチャー動画を作成したので、動画にそってください。
waypoint_loaderでファイルを指定してチェックするだけです。

ローカルパスプランニング

ローカルパスプランニングとは、その時の状況によって生成する動的な経路計画です。
例えば、障害物を避けたり、赤信号を見て止まったりなどといった感じです。

自動運転は、認知・判断・操作の三段階がありますが、ローカルパスプランニングがこの判断にあたり、かなり大きなウェイトを占めています。
ヒューマノイドなどのロボットと最も異なる部分は、このローカルパスプランニングだと思っています。車線上しか走れないという制約や、パッシングを利用した相手のコミュニケーションなど、自動運転特有の判断が必要です。例えば障害物があった時、追い越し車線を使うのか、避けるのか、停止するのかの判断をどうするのかとても難しい問題です。
つまり、自動運転のローカルパスプランニングは、今までのロボット分野では培ってきていない技術が必要とされており、今後も試行錯誤な状態が続いていくと予想されます。

ここでは、2016/12/21現在のAutowareのローカルパスプランニング構成について説明します。
ローカルパスプランニングは大まかに2種類にわけられます。

• 停止・減速のプランニング
• 回避のプランニング

これらと、判断の方法、ノード構成と利用方法について述べます。

停止・減速のプランニング

現在の停止・減速をする条件として、

• 歩道橋に歩行者がいる場合
• 赤信号の場合
• 経路上・付近に障害物がある場合

になります。
停止・減速のプランニングは、新たな経路を生成する必要はなく、速度の計画になります。
下の動画でwaypointの右側の速度がローカルパスプランニングした際の速度になります。速度の表示が緑色だったら、信号が青の時であり、赤色だったら、赤信号のときの目標速度になるようになっています。

また、経路付近に障害物（歩行者とか）が近くにいる場合、いつでも停止できるよう一定速度まで減速する必要があります。
Autowareでは経路上にいる障害物がある場合は停止、経路付近にいる障害物がある場合は減速という2段階のエリアを設けています。

回避のプランニング

回避のプランニングは開発中です。
私の予想を書きますｗ。
回避時には、障害物情報などから、ポテンシャルフィールドを生成し安全なパスを決定する必要があります。
また、車両の運動状態によって走行可能なパスというのは決まっているため、これらを組み合わせることでコストが最小となる回避経路を生成することができます。
現状では簡単なコストマップとstate latticeで走行可能なパスのはじき出しは出来ています（動画はstate lattice planning）。

ただ、コストマップとはじき出されたパスとの組み合わせが出来ていません。

また、今後、ポテンシャルフィールドなどにも取り組んでいく必要があるかもしれません。
※ポテンシャルフィールドは、危険度マップ・運転手が感じる危なさポテンシャルをマップとして表現したもの
理想的なポテンシャルフィールドには、障害物の移動予測なども含めたポテンシャルを生成する必要がありおそらく導入には時間がかかります。
私の中のイメージこんな感じ？

（ポテンシャルフィールドを使ったパスプランニング参考動画１）

（ポテンシャルフィールドを使ったパスプランニング参考動画２）

※追記　Autowareに追加:動画(2017/1/12)

ちなみに、走行可能なパスのはじき出しはROSデフォルトではstate latticeではなくDWA（Dynamic Window Approach）が利用されることが多いと思いますが利用されますが、Autowareには移植できていません。
http://myenigma.hatenablog.com/entry/20140624/1403618922

別の手法で、Hybrid State A*を利用したものも開発中です。

また、分かり次第追記します。
ごちゃごちゃとした書き方をしてすいません。

停止・回避の判断

障害物があった時、停止・回避の判断には先程のポテンシャルフィールドで避けられそうかどうかの判断で決定することも出来ます。
また、別の手法で、流行りのDeep learningを用いて判断する手法も試しています。
この辺は、まだまだ研究段階です。

ノード構成と利用方法

現段階では、停止・減速のプランニングのみとなります。
まず、構成とノードの対応関係を説明します。

構成として、まず初めに、waypointsから青信号と赤信号の場合の2種類を生成します。
そして信号認識の結果からどちらを利用するかを判断します。
（信号認識：http://myenigma.hatenablog.com/entry/20140624/1403618922）
その後、追い越し車線を走るかどうかの選択（現在手動）し障害物情報から速度計画を行う流れになります。

これらのノードはComputingタブの右側にあります。

ノード構成との対応関係として

• 信号状態によるwaypoints最適化：lane_rules
• 信号状態によるwaypointsの決定：lane_stop
• 車線選択：lane_select

車線選択は手動で行う必要があります。

• 速度計画：velocity_set

になってます。
※現在、その他にpath_selectの起動も必要です。path_selectはvelocity_setのトピックをスルーしているだけなので意味をなしていないです。

経路追従

ローカルパスプランニングで生成された経路を追従し、制御信号を出す必要があります。
Autowareの制御信号としてはgeometry_msgs/TwistStamped形式でパブリッシュされます。目標速度と目標角速度が出ることになるので、車両に制御を与えるときには別途PID制御などをする必要があります。

経路追従の手法として、Pure Pursuitを用いています。
https://www.ri.cmu.edu/pub_files/pub3/coulter_r_craig_1992_1/coulter_r_craig_1992_1.pdf
Pure Pursuitはミサイルの追撃にも利用されるアルゴリズムで、目の前のものを追いかけようとするアルゴリズムです。
別名、"Hound-and-Hare Course"（猟犬が野ウサギを追いかけるコース）らしいです。
http://kubota01.my.coocan.jp/navigation.htm

ただし、自動運転では問題があります。
Autowareではwaypoint上にターゲット（ミサイル）が走っていると仮定し、それを追撃しようとします。
しかし、ターゲットが右に曲がった際、距離が離れていると自分はショートカットしようとします。
逆に距離が短い場合、強い制御量が働き蛇行します。また、自己位置推定誤差の影響も強くなります。

引用：https://jp.mathworks.com/help/robotics/ug/pure-pursuit-controller.html?requestedDomain=jp.mathworks.com

これらの問題を解決するために、自分の速度によってターゲットとの距離（Looka head distance）を動的に変更するようにしています。

下記の論文に詳しく書かれています。
論文：Pure Pursuit Revisited: Field Testing of Autonomous Vehicles in Urban Areas
著者：Hiroki Ohta, Naoki Akai, Eijiro Takeuchi, Shinpei Kato and Masato Edahiro.

Computingタブの右側のpure_pursuitにチェックを入れ、速度・角速度制限をかけるtwist_filterにもチェックを入れれば完了です。

※State lattice planningやDWAでは車両運動状態を考慮し経路生成+制御信号を出すことができるので経路追従ノードがいらない場合もあります。

その他

完全自動運転になった時のエンタメ要素としてVRとも連携しています。（結構面白いです）
すこし、古い動画ですが・・・。

地図を名大のサーバからの動的読み込みすることで、メモリ・ストレージ容量不足を解決しています。また、個々の車で作られる地図を名大のサーバで統合し、リアルタイム更新する機能も開発中です。

ダイナミックマップの開発も行っています

組込みシステムへの移植も始まりつつあります。
https://www.esol.co.jp/news/news_167.html

今後の課題

私の主観で課題だと思うところを書きます

• 組込みシステムへの移植
• リアルタイム性の保証
• 障害物回避のパスプランニング
• ポテンシャルフィールド
• レーンチェンジ・回避・停止の判断
• DWAの移植
• SLAMのLoop closure実装
• 自己位置推定の信頼性向上
  • SLAMの位置推定でIMUの利用
  • Visual SLAM、NDT SLAMとの協調動作
  • その他複数センサ（IMU、GPS）やSLAMとの協調動作
• 障害物検知の精度
• 障害物移動予測
• 3次元トラッキング
• 並列処理・GPGPUによる高速化
• 各種パタメータのチューニング
• 経路追従に関する制御全般!!(たぶんモデル予測制御みたいな話かな)
• エッジコンピューティングによる情報共有
• ダイナミックマップ
• Gazeboとの連携
• Ubuntu 16.04+ROS kinetic対応

宣伝

Autowareを使った自動運転の構築塾を2017年3月に大阪・東京にて開催します。
http://www.nikkeibp.co.jp/lab/atsys/index.html
演習で利用した資料一部もあるので、参考にしてください。
公開資料

• 障害物検知
• 自己位置推定
• ROS 2.0の動向
• Autoware概要と操作

公開資料と本ページの内容が一部異なる可能性がありますが、常に開発中であり変化することもあるのでご了承ください。

さいごに

Autowareではリアルタイム性確保のために独自のコーディング規約を設けています。
https://github.com/CPFL/Autoware/wiki/Coding-Style(JP)
また、コードが汚いこともありますが、アルゴリズム自体次々と新しいものに変わっていく中で、（パッケージ内でコードが完結している場合は）めちゃくちゃキレイなコードを書くのに時間をかけるより、スピードの方が重要だと思います。
ある程度固定して利用できるアルゴリズムが見つかった際には、リファクタリングをすれば良いです。
つまり、コードの綺麗なことに越したことはないですが、多少汚くても大丈夫なのでコミットしてくれるディベロッパー募集してます！