自動運転の地図、ベクターマップについて

こんにちは、ティアフォーでベクターマップの作成ツールの開発をしている浦本と申します。今回は、Autowareで使われている地図の中でも、特にベクターマップとその作成ツールについてお話しようと思います。

なお、ティアフォーでは、「自動運転の民主化」をともに実現していく様々なエンジニア・リサーチャーを募集しています。もしご興味があればカジュアル面談も可能ですので以下のページからコンタクトいただければと思います。

tier4.jp

自動運転の地図

点群地図とベクターマップについて

Autowareで使われる地図には、自己位置推定のための点群地図と、その地図上の走行可能領域や信号・標識などを定義したベクターマップがあります。

Autowareは、道路や周囲の環境の形状を持っている点群地図と、走行中に取得したスキャンデータが最もマッチする重ね合わせから、車両の位置や向きを推定しています。この仕組みをスキャンマッチングといいます。スキャンマッチングについての詳細はこちらをご覧ください。

tech.tier4.jp

ベクターマップには、走行可能領域や信号・標識以外にも、速度制限、優先道路などの交通ルールや注意が必要なエリアのような情報を入れることができ、これらの情報を適切に設定することで、車両がその場所に適した走行をできるようにしています。

Autowareは現在、このベクターマップのフォーマットとしてLanelet2を採用しており、このフォーマットをAutoware用に一部拡張して使っています(これ以降、この記事ではLanelet2はAutoware用に拡張したものを指します)。ティアフォーで開発中のベクターマップ作成ツール、Vector Map Builder (VMB)もこのAutoware拡張のLanelet2の地図作成が可能です。以下の画像はこのツール上で地図を表示している例です。白い点群地図と色がついているベクターマップを重ねています。

f:id:yuramoto:20210621153302p:plain

Lanelet2について

ここでは、Lanelet2の詳細について説明したいと思います。

Lanelet2はOpenStreetMap(OSM)形式を拡張したもので、拡張子は.osm、ファイルに出力する際にはXML形式となります。Lanelet2は以下の基本オブジェクトで構成されますが、出力時にはOSMに合わせてnode、way、relationの3種類に分類されます。各基本オブジェクトの説明とファイルに出力した際のサンプルは以下のとおりです。なお、画像はVMBでの表示例です。

Point
  • 一番基本となるオブジェクトで、座標や信号の色情報などを持ちます。
  • OSMのnodeに分類されます。z座標はeleというタグで表されます。
  • ローカル座標を使いたい場合は、local_x、local_yというタグを設定することもできます。
  • lat/lonは緯度経度、local_x、local_y、eleの単位はメートルです。
<node id="1" lat="35.0000" lon="139.0000">
  <tag k="ele" v="19.000"/>
</node>

 f:id:yuramoto:20210621153224p:plain

LineString
  • Pointのリストを順番に線で結んだオブジェクト。LineString2つでLaneletを構成したり、信号機や一時停止線に使われます。
  • OSMのwayに分類されます。
  • 構成要素のPointをndタグで持ちます。
  • 一時停止線はtypeをstop_line、点線はsubtypeをdashedにすることで表します。Laneletの境界線のLineStringをdashedにすると隣接レーンに車線変更が可能になるなど、タグの付け方を変えることで様々な設定が可能です。詳細はこちらをご覧ください。

    github.com

<way id="1">
 <nd ref="1"/>
 <nd ref="2"/>
 <nd ref="3"/>
 <nd ref="4"/>
 <tag k="type" v="line_thin"/>
 <tag k="subtype" v="solid"/>
</way>

 f:id:yuramoto:20210621153308p:plain

Polygon
  • Pointで囲まれた領域を表します。障害物検知領域を表すDetectionAreaなどで使われます。
  • OSMのwayに分類され、area=yesというタグをつけます。
<way id="1">
 <nd ref="1"/>
 <nd ref="2"/>
 <nd ref="3"/>
 <nd ref="4"/>
 <tag k="type" v="detection_area"/>
 <tag k="area" v="yes"/>
</way>

 f:id:yuramoto:20210621153247p:plain

Lanelet
  • 左右にLineStringを1つずつ持ち、その間の領域を表します。向きを持っており、車線や横断歩道などに使われます。
  • OSMのrelationに分類され、type=laneletというタグをつけます。
  • 左右のLineStringをmemberとして持ちます。
  • その車線で守る必要のある交通ルール(RegulatoryElement)がある場合は、それもmemberとして持ちます。
  • 制限速度や通行対象(車や歩行者など)、一方通行かどうかなどの情報を持てます。
<relation id="1">
 <member type="way" role="left" ref="1"/>
 <member type="way" role="right" ref="2"/>
 <member type="relation" role="regulatory_element" ref="2"/>
 <tag k="type" v="lanelet"/>
 <tag k="subtype" v="road"/>
 <tag k="speed_limit" v="50"/>
 <tag k="location" v="urban"/>
 <tag k="participant:vehicle" v="yes"/>
 <tag k="one_way" v="yes"/>
</relation>

 f:id:yuramoto:20210621153241p:plain

Area
  • LineStringで囲まれた領域を表します。車両や人の通行領域を表す場合によく使われますが、Laneletと違って向きは持ちません。
  • OSMのrelationに分類され、type=multipolygonというタグをつけます。
  • 構成要素のLineStringをmemberとして時計回りの順番に持ちます。
<relation id="1">
 <member type="way" role="outer" ref="1"/>
 <member type="way" role="outer" ref="2"/>
 <tag k="type" v="multipolygon"/>
</relation>

 f:id:yuramoto:20210621153227p:plain

※Polygonとの見た目の違いは、周囲がLineStringで囲まれているかどうかです。

RegulatoryElement
  • 一時停止、信号、優先道路などの種類があり、それらがどの車線や一時停止線、信号機と紐付いているのかという情報を持っています。オブジェクト間の紐付けとルール情報を持つだけなので地図上に表示されるものではなく、画像はありません。
  • OSMのrelationに分類されます。type=regulatory_elementというタグをつけます。
  • subtypeでルールの種類を指定します(例:traffic_lightは信号、right_of_wayは優先道路)。
<relation id="1">
 <member type="way" role="refers" ref="1"/>
 <member type="way" role="light_bulbs" ref="2"/>
 <member type="way" role="ref_line" ref="3"/>
 <tag k="type" v="regulatory_element"/>
 <tag k="subtype" v="traffic_light"/>
</relation>

RegulatoryElementはルールの種類によってmemberの数や種類も変わってきます。例としてサンプルにも記載している信号を説明します。信号のmemberは3つで、refers、ref_line、light_bulbsがあります。それぞれtypeがwayとなっているので、LineStringがPolygonを表すのですが、信号に紐付くのは全てLineStringです。

まずはrefersですが、これは信号の外形を表します。ファイルに出力すると以下のwayのようになるのですが、ndの2と3は画像の丸(Point)の部分で信号機の幅を表し、heightタグで高さを表します。

<way id="1">
 <nd ref="2"/>
 <nd ref="3"/>
 <tag k="type" v="traffic_light"/>
 <tag k="height" v="0.450000"/>
</way>

f:id:yuramoto:20210621153250p:plain

 次にlight_bulbsは、以下のwayのようになり、traffic_light_idは信号機の外形のLineStringのidを表しています。ndの4から6はPointで、以下のidが4のnodeのような形でそれぞれcolorタグで信号の色情報を持ちます(画像の信号の場合はred、yellow、greenの3つ)。

<way id="2">
 <nd ref="4"/>
 <nd ref="5"/>
 <nd ref="6"/>
 <tag k="type" v="light_bulbs"/>
 <tag k="traffic_light_id" v="1"/>
</way>
<node id="4" lat="35.0000" lon="139.0000">
 <tag k="ele" v="19.546"/>
 <tag k="color" v="red"/>
</node> 

最後にref_lineですが、信号のref_lineは信号が赤だった時に停止する停止線を表すため、以下のwayのように、stop_lineというtypeを設定します。

<way id="3">
 <nd ref="7"/>
 <nd ref="8"/>
 <tag k="type" v="stop_line"/>
</way>

このようなオブジェクトを組み合わせて、ベクターマップを作っています。

Vector Map Builderの紹介

これまで説明してきたようなベクターマップを作成するためのツールとして、ティアフォーではVMBを開発しています。ブラウザで動作するツールで、TypeScript、React、Three.jsを主に使っています。

f:id:yuramoto:20210621153254p:plain

リンクはこちらになりますが、最初にティアフォーアカウントを作成いただいてからログインが可能となります。

tools.tier4.jp

ここでは、地図を作りやすくするために実装しているVMBの機能を一部紹介します。

1つ目は、点群地図の表示範囲指定機能です。この機能を使うケースとして、例えば屋内のベクターマップを作成するときに、点群に屋根が入ってしまっていてそのままでは屋内の形状が見にくいというものがあります。そういう場合は、この機能で不要な屋根部分の点群をカットし、見たい範囲の点群だけを見えるようにすることで、地図作成がしやすくなります。他にも、下の画像のように木が邪魔で車線が見づらい場合にも使えたり、上下の高さを指定できるので、ビルのような複数階を含む点群があったときには、見たい階だけを残して作業することも可能です。

f:id:yuramoto:20210621153244p:plain   f:id:yuramoto:20210621153238p:plain
2つ目は、地図のリアルタイムチェック機能です。車線や交通ルールが増えて地図が複雑になると、作成・編集時にミスが起こりやすくなり、チェックも大変になります。ミスが残ってしまっていても、地図作成後はシミュレータで動作チェックを実施するため、想定外の挙動をする場合はそこでわかるのですが、複数箇所が間違っているとVMBに戻って該当箇所を修正→走行チェックという作業を何度か繰り返さなければいけない場合があります。

VMBではそのような作業を少しでも減らすため、フォーマットとして間違っている点に加え、ティアフォーのこれまでの地図作成経験からリストアップしたチェック項目を作成作業中にチェックするようにしています。問題のある設定をすると、その設定を行った直後に問題のあるオブジェクトとその詳細を表示し、修正を促します。また、修正をしないまま地図の保存を行った場合は、問題点が残っていることを表示し、問題点に気づかずに保存してしまうことをできるだけ避けられるようにしています。

画像は設定している制限速度を超えたLaneletが存在する場合の例です。何が問題なのかという説明とIDを表示しており、IDをクリックすることで該当オブジェクトの設定が確認できるようになっています。

 f:id:yuramoto:20210621153232p:plain

チェックすべき項目は、現在も実証実験等から問題を吸い上げたりしながら増やしており、今以上に充実させていく予定です。 

まとめ

今回は、Autowareで使われている地図の中でも、特にベクターマップについてお話しました。自動運転車がどのような地図を使って走行しているのか、イメージできたでしょうか。

ベクターマップは精度が求められるので作成時は細かな作業が多く、持たせる情報も多いことから設定とチェックに時間がかかります。これらの問題を解消し、より安全な地図をより早く作ることができるツール・仕組みの開発をこれからも進めていきます。

Autowareの開発を更に加速!Openなシナリオテストフレームワークを紹介!

f:id:masaya-kataoka:20210527135238p:plain

はじめに

こんにちは!ティアフォーの片岡と申します。
本日は、ティアフォーのSimulation Teamで開発してきたシナリオテストフレームワークであるscenario_simulatorをApache-2.0ライセンスでOSS(オープンソースソフトウェア)とした件についてお話します。
github.com

また、ティアフォーでは「自動運転の民主化」をともに実現すべく、ロボット技術、ゲームエンジンを使った開発等幅広い分野に知見を持ったSimulation Engineerを募集しております。
自動運転のSimulator開発は、バグの少ない高品質なコードを書くのはもちろんのこと、三次元幾何や統計、はてはプログラミング言語開発まで様々な知識が要求される分野であり、広く深く様々なことをやってみたい方には非常にマッチするポジションであると思います。
もし興味等ございましたら以下のページよりコンタクトいただければ大変幸いです。
カジュアルな面談等も大歓迎です!
tier4.jp

Autoware開発におけるSimulation Test

ティアフォーで行われているSimulation Testの種類

現在ティアフォーではSimulatorによってそれぞれ得意・不得意な領域があることを認識しており、テストケースごとに適切なSimulatorを選定し評価を行っています。
f:id:masaya-kataoka:20210316160300p:plain

AutowareをサポートしているSimulator

上の図で記されているのは大枠のカテゴリであり、実際にはAutoware対応のSimulatorには様々なものが存在しています。代表的なものには以下のようなものがあります。

SVL Simulator

youtu.be
github.com
LGエレクトロニクスのシリコンバレー研究所が開発しているUnityベースのSimulatorで、Pythonでテストシナリオを記述できます。
LiDARやCameraを多数取り付けたリッチな自動運転システムがSimulation可能です。

CARLA Simulator

youtu.be
github.com
IntelやToyota Research Instituteの出資のものとOpen Sourceで開発されているUnreal EngineベースのSimulatorで、OpenSCENARIO V1.0やPython等でテストシナリオを記述できます。
野心的な機能が多数盛り込まれており、非常に高機能なSimulatorです。

AutoCore Simulator

youtu.be
github.com
The Autoware Foundation (AWF) メンバーのAutoCoreが開発したUnityベースのSimulatorです。
非常に軽量でGTX750Tiでも動作可能になっています。

Simple Planning Simulator

youtu.be
github.com
AutowareArchitectureProposalに同梱されているSimulatorです。
ROSノードとして実装されており、OpenSCENARIO V0.9.2をベースとしたティアフォーの独自フォーマットでのシナリオ記述が可能で、軽量であるため非常に高速に動作します。

複数のSimulatorを使う場合に起こる問題点

複数のSimulatorを使ってシナリオテストを行っていく過程でティアフォーは以下のような問題に直面しました。

シナリオフォーマットがSimulatorに依存している

汎用なシナリオフォーマットはOpenSCENARIO等様々なものが提案されていますが、Simulator固有の拡張が入っていたり、SVL Simulator等独自シナリオフォーマットを中心としているようなSimulatorも多いため、一度書いたシナリオを使い回すのは簡単ではありません。
シナリオを異なるSimulatorの間でやり取りするには人力で動作確認をしながらポーティングを行う必要があり、シナリオ変更履歴のトレーサビリティの観点からも大きな問題があります。

NPCロジックがSimulatorごとに異なる

完全に等価なシナリオを人力でポーティングできたとしても、それを処理する処理系が異なるのであれば処理結果は異なってしまいます。
具体的な例として、「車の前に自転車が飛び出す」という状況を考えてみましょう。
f:id:masaya-kataoka:20210316212415p:plain
これをシナリオに起こす時、以下のような処理が記述されることになります。

  1. ある地点から N(m) 以内に自車両が入った時
  2. 自転車のNPCをSpawnさせ、Waypointを与える。

ここで、自転車NPCの行動ロジックがSimulator Aでは「直線の点列をカクカクなぞる」、Simulator Bでは「スプライン補間したあと単純追跡コントローラでその経路をできるだけなぞる」のように異なっていた場合、これらは等価なテストシナリオとは言えません。
こういったNPC行動ロジックは厳密に一致している必要があります。
これをCARLAやSVL Simulatorに備わっているテストランナーの枠組み上で解決しようとすると、彼らのソースコードに含まれる何千行ものNPC行動ロジック記述部分に手を入れて異なる言語やプラットフォームを超えて完全に等価なロジックを実装し、メンテナンスし続けなくてはなりません。
これはほとんど不可能に近いと言えます。
こういった問題を解決するために仕様策定されたのがOpenSCENARIOですが、例えば以下のような基本的な箇所ですら未定義動作が非常に多く、OpenSCENARIOに対応したからと言って一度書いたシナリオがすべてのSimulatorで簡単に動くということは担保できません。

OpenSCENARIO V1.0の未定義動作の例

  • 道路端点や世界の端に到達したNPCはどう振る舞うべきか
  • AssignRouteActionで経路指定した結果ゴールに到達したNPCはそのまま走っていくべきか、それともその場で停止するべきか
  • レーンチェンジができない場所でレーンチェンジ指示が出た場合はどのような挙動をすべきか
SimulatorごとにIntegrationの仕方が違う

例えばCARLAにおいては独自ブリッジでROSおよびAutowareとのIntegrationを行っており、ブリッジ内部でPID制御器が動いていたりと他のSimulatorと全く異なるロジックで制御がなされています。
youtu.be
他にも、SVL SimulatorはAutowareとrosbridgeで接続しており、ROSの標準的なツールを用いてインターフェースが設計されています。
https://www.lgsvlsimulator.com/docs/autoware-instructions/www.lgsvlsimulator.com
このように、Simulatorごとに根本的に異なるインターフェースが設計されており、これはクラウド上にテストパイプラインを構築、メンテナンスしていく上で大きな障害になります。
また、明示的な標準仕様がないため、新しいSimulatorをIntegrationする際には毎回設計を1からやり直す必要があり莫大な工数が発生することになります。

シナリオフォーマットは現在も技術開発が盛んな領域

現在シナリオフォーマットには様々なものが提案されています。
例えば、ASAMという標準化団体が提唱しているOpenSCENARIO V1.0が有名です。
www.asam.net
その他にも、現在策定が進められているOpenSCENARIO V2.0およびそのベースとなっているM-SDL
youtu.be
といった様々なフォーマットが現在も開発されています。
産業界だけでなく、2019年ので発表されたGeoSCENARIO等アカデミア方面からも研究開発が進められています。
geoscenario.readthedocs.io
どのシナリオフォーマットをどう使うのがAutowareの評価を最も効率化できるのかというのは未だ結論が出ていない領域の1つです。
そのため、同時に複数のシナリオフォーマットをサポートできるようなシナリオテストフレームワークが強く求められます。

ティアフォーにおいてはこれらの問題点を以下のアプローチで解決しました。

ティアフォーのシナリオテストフレームワークのアプローチ

Co-Simulation Modelの採用

すべてのSimulatorにおいてNPCロジックを共通化するため、NPCロジックをシナリオテストフレームワーク中の交通流Simulatorに対して実装しました。
センサSimulatorが交通流Simulatorとプロセス間通信を行い、歩調を合わせながらCo-SimulationをすることでNPCロジック共通化を実現しています。
これにより、交通流SimulatorはセンサSimulatorに対するOpenSCENARIOの「翻訳機」かのように振る舞い、OpenSCENARIOの未定義動作によるSimulatorごとの差異を吸収します。
プロセス間通信はすべて同期呼び出しで実装されており、シナリオテストフレームワークのうちROS 2に依存しない部分に関しては決定性を担保しています。

シナリオテストフレームワークとシナリオ解釈器のコンポーネント分離

シナリオテストフレームワークにC++ APIを用意し、シナリオフォーマットの解釈器がそれを叩くアーキテクチャ構成にすることで同時に複数のシナリオフォーマットをサポートしています。
完全にコンポーネントが分離されているので、スッキリとしたアーキテクチャに仕上がっています。

Simulatorを抽象化

SimulatorとAutowareとのインターフェースを統一し、複数のSimulatorを同時にサポートできるシナリオテストフレームワークを開発しました。
Autowareとの結合部分や、NPCロジックで使われている微分幾何アルゴリズムやHD MapとのインテグレーションをSimulatorで独自実装する必要はありません。
また、Autowareは非常に高い頻度で更新が行われているソフトウェアであり、常に結合確認を取りながらインテグレーションを行う必要があり、メンテナンスコストが非常に高いです。
これらの複雑で開発を難航させる原因になりがちなロジックはすべてシナリオテストフレームワークに含まれていますし、メンテナンスもティアフォーのSimulation Team主体で行っているため、Simulator開発者は短期間でSimulatorとAutowareを結合し、その結合を維持することが非常に簡単になりSimulatorとしてより本質的なセンサSimulationの高精度化や車両の運動モデルといった開発に時間を使うことができるようになります。

Open Source

更に、ティアフォーではこのシナリオテストフレームワークをオープンにすることでAutowareを様々なSimulator上で動かしてAutowareの評価を更に高速化していく仕組みを担保しようと考えています。
ライセンスはApache-2.0で統一されているため非常に使いやすいツール群となっています。

ティアフォーのシナリオテストフレームワークのアーキテクチャ

アーキテクチャ全体の紹介

f:id:masaya-kataoka:20210614182842p:plain
シナリオテストフレームワークを構成するツールチェイン

本シナリオテストフレームワークのAWFの公式シナリオフォーマットであるTier IV Scenario Format version 2.0フォーマットあるいはテストシーケンスを記述したworkflowファイルの読み込みは、テストを起動するScenario Test Runner、OpenSCENARIO 1.0フォーマットで記述されたシナリオを解釈するOpenSCENARIO Interpreter、交通流をSimulationするTraffic Simulatorおよび本フレームワークにセンサSimulatorを統合する際の簡易なセンサSimulatorのリファレンス実装となるSimple Sensor Simulatorが含まれています。
OpenSCENARIO InterpreterおよびTraffic Simulatorは下のような実装になっており、シングルプロセスなROS 2 Nodeとして実装されています。

f:id:masaya-kataoka:20210316221411p:plain
シナリオテストフレームワークアーキテクチャ

ティアフォーのシナリオテストフレームワークはAutowareとSimulatorの間をつなぐ役割を果たします。
シナリオテストフレームワークはROS 2との通信部分以外がシングルスレッドで動作する単独のROS 2 nodeとして設計されており、ROS 2 topicをPub / Subする部分を除いて完全に決定性があります。
シナリオテストフレームワークと通信してCo-Simulationを行うSimulatorはZeroMQで同期的に通信を行います。
zeromq.org
ZeroMQは様々な言語をサポートしており、Unity (C#)やUnreal Engine (C++)等様々な言語、プラットフォームで作られているSimulatorと簡単に通信することができます。
また、ZeroMQで送信されるデータはProtocol Buffersを使ってシリアライズすることで、スキーマを明確に定義してSimulator開発者がインターフェースが変わったことをすぐに把握できるよう工夫しています。
developers.google.com
SimulatorとROS 2はAutoware APIというAutowareを操作するために定義されたROS 2 topic群を利用して通信します。シナリオテストフレームワークは内部にBehavior Treeを用いたNPCロジックやHD Map管理機構を内包しており、それらを操作できるC++のインターフェースを備えています。
まず第一弾としてSimulatorに依存しないシナリオフォーマットの代表格の一つであるOpenSCENARIO V1.0をシナリオフォーマットとして採用し、シナリオテストフレームワークのC++ Interfaceを叩いてOpenSCENARIO規格に基づいてSimulationを行えるツールを開発しました。
その過程で動作確認用に副次的に生まれたC++ Scenario(ROS 2ノードからシナリオテストフレームワークのインターフェースをダイレクトに操作して実現)の誕生により、はからずも複数シナリオフォーマットをサポートすることが可能なアーキテクチャであることが立証されました。
下のC++ Scenarioのサンプルコードに示すような非常に簡易な指示をscenario_simulator::APIクラス(シナリオテストフレームワークとのC++ Interface)を経由して出すだけでSimulator内部のNPCの行動を制御することが可能です。

// Copyright 2021 Tier IV, Inc. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <quaternion_operation/quaternion_operation.h>

#include <ament_index_cpp/get_package_share_directory.hpp>
#include <rclcpp/rclcpp.hpp>
#include <traffic_simulator/api/api.hpp>
#include <traffic_simulator/metrics/metrics.hpp>

// headers in STL
#include <memory>
#include <string>
#include <utility>
#include <vector>

// headers in pugixml
#include "pugixml.hpp"

class ScenarioRunnerMoc : public rclcpp::Node
{
public:
  explicit ScenarioRunnerMoc(const rclcpp::NodeOptions & option)
  : Node("scenario_runner", option),
    api_(
      this, __FILE__,
      ament_index_cpp::get_package_share_directory("kashiwanoha_map") + "/map/lanelet2_map.osm", 5)
  {
    api_.setVerbose(true);
    api_.initialize(1.0, 0.05);
    pugi::xml_document catalog_xml_doc;
    catalog_xml_doc.load_string(catalog_xml.c_str());
    auto vehicle_params = traffic_simulator::entity::VehicleParameters(catalog_xml_doc).toRosMsg();
    vehicle_params.name = "ego";
    api_.spawn(false, "ego", vehicle_params);
    api_.setEntityStatus(
      "ego", traffic_simulator::helper::constructLaneletPose(120545, 0),
      traffic_simulator::helper::constructActionStatus(10));
    api_.setTargetSpeed("ego", 15, true);
    pugi::xml_document pedestrian_xml_doc;
    pedestrian_xml_doc.load_string(pedestrian_xml.c_str());
    const auto pedestrian_params =
      traffic_simulator::entity::PedestrianParameters(pedestrian_xml_doc).toRosMsg();
    api_.spawn(false, "tom", pedestrian_params);
    api_.setEntityStatus(
      "tom", "ego", traffic_simulator::helper::constructPose(10, 3, 0, 0, 0, -1.57),
      traffic_simulator::helper::constructActionStatus());
    api_.requestWalkStraight("tom");
    api_.setTargetSpeed("tom", 3, true);
    api_.spawn(
      false, "bob", pedestrian_params, traffic_simulator::helper::constructLaneletPose(34378, 0.0),
      traffic_simulator::helper::constructActionStatus(1));
    api_.setTargetSpeed("bob", 1, true);
    vehicle_params.name = "npc1";
    api_.spawn(
      false, "npc1", vehicle_params, traffic_simulator::helper::constructLaneletPose(34579, 20.0),
      traffic_simulator::helper::constructActionStatus(5));
    api_.setTargetSpeed("npc1", 5, true);
    lanechange_executed_ = false;
    vehicle_params.name = "npc2";
    api_.spawn(
      false, "npc2", vehicle_params, traffic_simulator::helper::constructLaneletPose(34606, 20.0),
      traffic_simulator::helper::constructActionStatus(5));
    api_.setTargetSpeed("npc2", 0, true);
    api_.requestAssignRoute(
      "ego", std::vector<openscenario_msgs::msg::LaneletPose>{
               traffic_simulator::helper::constructLaneletPose(34675, 0.0),
               traffic_simulator::helper::constructLaneletPose(34690, 0.0)});
    api_.requestAcquirePosition(
      "npc1", traffic_simulator::helper::constructLaneletPose(34675, 0.0));
    api_.spawn(false, "npc3", vehicle_params);
    api_.setEntityStatus(
      "npc3", traffic_simulator::helper::constructLaneletPose(34468, 0),
      traffic_simulator::helper::constructActionStatus(10));
    /*
    api_.addMetric<metrics::TraveledDistanceMetric>("ego_traveled_distance", "ego");
    api_.addMetric<metrics::MomentaryStopMetric>(
      "ego_momentary_stop0", "ego",
      -10, 10, 34805, metrics::MomentaryStopMetric::StopTargetLaneletType::STOP_LINE,
      30, 1, 0.05);
    api_.addMetric<metrics::MomentaryStopMetric>(
      "ego_momentary_stop1", "ego",
      -10, 10, 120635, metrics::MomentaryStopMetric::StopTargetLaneletType::STOP_LINE,
      30, 1, 0.05);
    api_.addMetric<metrics::MomentaryStopMetric>(
      "ego_momentary_stop_crosswalk", "ego",
      -10, 10, 34378, metrics::MomentaryStopMetric::StopTargetLaneletType::CROSSWALK,
      30, 1, 0.05);
    */
    std::vector<std::pair<double, traffic_simulator::TrafficLightColor>> phase;
    phase = {
      {10, traffic_simulator::TrafficLightColor::GREEN},
      {10, traffic_simulator::TrafficLightColor::YELLOW},
      {10, traffic_simulator::TrafficLightColor::RED}};
    api_.setTrafficLightColorPhase(34802, phase);
    using namespace std::chrono_literals;
    update_timer_ = this->create_wall_timer(50ms, std::bind(&ScenarioRunnerMoc::update, this));
  }

private:
  void update()
  {
    if (api_.getCurrentTime() >= 4 && api_.entityExists("tom")) {
      api_.despawn("tom");
    }
    /*
    if (api_.getLinearJerk("ego")) {
      std::cout << "ego linear jerk :" << api_.getLinearJerk("ego").get() << std::endl;
    }
    */
    if (api_.reachPosition(
          "ego", traffic_simulator::helper::constructLaneletPose(34615, 10.0), 5)) {
      api_.requestAcquirePosition(
        "ego", traffic_simulator::helper::constructLaneletPose(35026, 0.0));
      if (api_.entityExists("npc2")) {
        api_.setTargetSpeed("npc2", 13, true);
      }
    }
    if (api_.reachPosition("ego", traffic_simulator::helper::constructLaneletPose(34579, 0.0), 5)) {
      api_.requestAcquirePosition(
        "ego", traffic_simulator::helper::constructLaneletPose(34675, 0.0));
      if (api_.entityExists("npc2")) {
        api_.setTargetSpeed("npc2", 3, true);
      }
    }
    if (api_.reachPosition(
          "npc2", traffic_simulator::helper::constructLaneletPose(34513, 0.0), 5)) {
      api_.requestAcquirePosition(
        "npc2", traffic_simulator::helper::constructLaneletPose(34630, 0.0));
      api_.setTargetSpeed("npc2", 13, true);
    }
    /*
    if (api_.checkCollision("ego", "npc1")) {
      std::cout << "npc1 collision!" << std::endl;
    }
    if (api_.checkCollision("ego", "npc2")) {
      std::cout << "npc2 collision!" << std::endl;
    }
    */
    if (api_.getCurrentTime() > 10.0 && api_.entityExists("bob")) {
      api_.despawn("bob");
    }
    api_.updateFrame();
  }
  bool lanechange_executed_;
  bool target_speed_set_;
  int port_;
  traffic_simulator::API api_;
  rclcpp::TimerBase::SharedPtr update_timer_;

  std::string catalog_xml =
    R"(<Vehicle name= 'vehicle.volkswagen.t2' vehicleCategory='car'>
         <ParameterDeclarations/>
         <Performance maxSpeed='69.444' maxAcceleration='200' maxDeceleration='10.0'/>
         <BoundingBox>
           <Center x='1.5' y='0.0' z='0.9'/>
           <Dimensions width='2.1' length='4.5' height='1.8'/>
         </BoundingBox>
         <Axles>
           <FrontAxle maxSteering='0.5' wheelDiameter='0.6' trackWidth='1.8' positionX='3.1' positionZ='0.3'/>
           <RearAxle maxSteering='0.0' wheelDiameter='0.6' trackWidth='1.8' positionX='0.0' positionZ='0.3'/>
         </Axles>
         <Properties>
           <Property name='type' value='ego_vehicle'/>
         </Properties>
       </Vehicle>)";

  std::string pedestrian_xml =
    R"(<Pedestrian model='bob' mass='0.0' name='Bob' pedestrianCategory='pedestrian'>
         <BoundingBox>
           <Center x='0.0' y='0.0' z='0.5'/>
           <Dimensions width='1.0' length='1.0' height='2.0'/>
         </BoundingBox>
         <Properties/>
       </Pedestrian>)";
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::NodeOptions options;
  const auto component = std::make_shared<ScenarioRunnerMoc>(options);
  rclcpp::spin(component);
  rclcpp::shutdown();
  return 0;
}

このように、ROS 2と密にIntegrationされ非常に簡易に使用できるインターフェースとなっていますので、今後他のシナリオフォーマットを採用する必要が出た場合は、その解釈器さえ書いてしまえば簡単にAutoware、Simulatorと接続を行うことができます。

開発時に注力した要素

シナリオテストフレームワーク開発においては、以下の要素に対して開発時に大きく注力しました。

レーン座標系のサポート

レーン座標系とは、レーンに沿って歪んだ座標系のことです(いわゆるフレネ座標系です)。

f:id:masaya-kataoka:20210316231351p:plain
https://releases.asam.net/OpenSCENARIO/1.0.0/ASAM_OpenSCENARIO_BS-1-2_User-Guide_V1-0-0.html#_cut_in

みなさんが公道で車を運転する時、「前30mにトラックがいる」というとそれはデカルト座標系で距離を計算したら30m先にトラックがいるという意味ではなくレーンに沿ってこのまま進んだら30m先にトラックがいるという意味になるかと思います。
レーン座標系でシナリオが記述できないと、直感的な記述ができず非常にシナリオ記述が煩雑なものとなります。
また、レーン座標系を計算する時にはスプライン曲線等のパラメトリック曲線で点列を補間する処理が入るのですが、どんな式の形のパラメトリック曲線を使用するかで補間の結果は全く異なります。
そのため、シナリオテストフレームワークでは内部にレーン座標系、ワールド座標系、物体間の相対座標系を相互に座標変換するAPIを備えており、Simulatorとプロセス間通信する際に渡される情報ではすべての物体位置はワールド座標系で記述されています。
これらの座標変換はtfを介さずにフルスクラッチで実装されていますので、完全に決定性があります。
この仕組みを導入することによってSimulatorはレーン座標系をサポートするためにHD Mapを解釈する必要がなくなり、AutowareとIntegrationする際の工数を大幅に削減することができる上、どのSimulatorを使っても完全に同じロジックで座標変換が行われるのでクオリティの担保が非常にやりやすくなります。

強化されたNPCロジック

Autowareに同梱されているSimple Planning SimulatorのNPCロジックは非常にシンプルなものでした。

  • レーンに沿って動く
  • NPCの前方にBounding Boxを設け、その中に他のNPCやAutowareの車両が入った場合急ブレーキを踏む

実装されていたロジックはこの2点のみです。
これではシナリオライターはNPCの一挙手一投足を指定しなければならず、シナリオ作成に大きな負荷が発生していました。
そこでBehavior Treeを使用してNPCロジックを大幅に強化し、以下のような挙動を明示的に指示しなくても自動的にやってくれるようにしました。

  • レーンに沿ってNPCが移動する。
  • 赤、黄色信号の前で停止する。
  • HD Mapに埋め込まれた法定速度を遵守して走行する。
  • 明示的に速度指示があった場合にはそちらを目標速度として走行する。
  • 一時停止線で一時停止後、発車する。
  • 非優先道路において優先道路に他の車両がいる場合道を譲る。
  • レーン座標系でみて自分の前方に車両がいる場合、速度を合わせて追従する。
  • レーンチェンジの指示が出た場合、レーンチェンジ可能な地点であればレーンチェンジを行う。
  • レーンチェンジが不可なエリアであった場合、レーンチェンジは行わない。
自動テスト

自動運転のテストパイプラインを運用していく上で「Simulatorにバグがあった」というケースは非常に厄介です。徹底的にバグは潰していかないといけませんし、早期発見しなくてはなりません。
Simulatorにバグがあった場合、テスト担当の方はかなり深くSimulatorのソースコードやシナリオを追って問題の根源を考えないといけません。このコストは非常に高いですし、致命的な不具合が見つかった場合過去のテスト結果が信用できなくなる可能性もありえます。
大きなコード規模(2021年5月21日現在、リポジトリ全体で36481行)を誇るROS 2パッケージ群であるシナリオテストフレームワークはなにか致命的な問題が見つかったからといって簡単に大規模なリファクタや修正をすることはできませんし、機能追加やバグフィックスによってデグレが発生しうることは容易に想像できます。

f:id:masaya-kataoka:20210520184802p:plain
シナリオテストフレームワークのコード規模

そこで、シナリオテストフレームワークの開発においてはGitHub Actionsを使用して以下のテストケースをシナリオテストフレームワーク本体のCIとして継続的に回しています。

f:id:masaya-kataoka:20210520181818p:plain
GitHub ActionsでCIを実行している様子

テスト項目 テスト内容 トリガー
BuildTest リポジトリに含まれる各ROS 2パッケージが独立にビルドできること
各種linterチェックが通ること
単体テストの実施
毎朝9時
PRへの更新
手動
ScenarioTest リポジトリに含まれるパッケージ全体をビルド
結合テストの実施
テストの分岐網羅率チェック
毎朝9時
PRへの更新
手動
Docker (PRへの更新時)DockerImageのビルド
(master更新時)DockerImageのビルドとDocker Hubへのpush
毎朝9時
PRへの更新
master更新
手動
Documentation (PRへの更新時)ドキュメントのビルド
(その他)ドキュメントのビルドとGitHub Pagesへのデプロイ
毎朝9時
PRへの更新
master更新
手動
Release CHANGELOG発行とバージョン付与 手動
SpellCheck ドキュメント、ソースコードに対してスペルチェック PRへの更新

単にテストケースを回すだけでなくGitHub Pagesに常に最新版ドキュメントがデプロイされるようになっています。
これにより簡単に最新版のドキュメントにアクセスすることができます。

tier4.github.io

今後の開発計画

今後ティアフォーのSimulation Teamでは以下のような改良をシナリオテストフレームワークに加えることを計画しています。

NPCロジックの更なる性能向上

現状のNPCロジックはある程度賢く動いてシナリオライターの作業負荷を大きく軽減してくれるものの、左折時の割り込みや信号無視、速度制限の超過といったより抽象的な指示を簡単に出せるようにするにはNPCロジックを更に賢くしていく必要があります。
現在、ICRA2020で発表されたこちらのNPCロジックを改良し、Autowareの更なるシナリオテスト効率化を推し進めていこうと考えています。

www.youtube.com
arxiv.org

公道走行ログからのシナリオ自動起こし

公道走行中にあったヒヤリハットの状況をSimulatorの中で再現し、安全であることを検証する取り組みは非常に重要です。
以前紹介したRSSは公道において発生した危険な状況を数値化することが可能となります。
tech.tier4.jp
Simulation TeamではRSS等のメトリクスを用いて検出した危険な状況のデータを取り出し、周囲の交通状況を自動的にシナリオ化する技術開発にもトライしていこうと考えています。


最後に改めてですが、興味等ございましたら以下のページよりコンタクトいただければ大変ありがたいです。
カジュアルな面談も大歓迎です!
tier4.jp

よろしくお願いいたします。

読み解くNDT Scan Matchingの計算 [前編]

こんにちは、ティアフォーパートタイムエンジニアの上野と、ティアフォーエンジニアの村上です。

今回読み解いたのは、自動運転位置推定のターニングポイントとなった、Scan Matchingによる高精度自己位置推定技術の華、NDT Scan Matchingです。点と点のMatchingから、点と分布のMatchingにすることで、計算負荷が現実的な程度まで削減されました。方法としては博士論文 M. Magnusson, “The three-dimensional normal-distributions transform : an efficient representation for registration, surface analysis, and loop detection,” PhD dissertation, Örebro universitet, Örebro, 2009. で説かれており、また論文中の方程式は、Autowareの実装コードの中でも度々言及されています。

しかしながら、LiDARを使用した高精度な位置推定に今や必須となったこの方法は、なおも計算負荷が比較的高いままです。計算負荷の観点でも目にする機会が多いこのアルゴリズムを、定量的/実装的な観点からあらためて見てみましょう。

NDT Scan Matcherについて、Autowareが用いている実装としては、全体的には ndt_scan_matcher_core.cpp、部分的には ndt_omp_impl.hpp に主要な処理が記述されています。

主要な処理は、その負荷の大きさや計算の姿から大きくは、1. 移動がある自動運転におけるMatchingスコア値収束性能向上のための「近傍地図点群の取得処理」と、2. LiDAR点群と近傍地図点群のDistributionを用いた「Matchingスコア値計算処理」との2つの処理に大別されます。

以下にndt scan matcherのflame-graph画像を上げます。初期位置/姿勢の推定処理である alignUsingMonteCarlo 部分を除外し callbackSensorPoints 直下に注目します。

f:id:james-murakami:20210430102443j:plain

ndt scan matcher; flame graph

KdTreeFLANN::radiusSearchが1. 近傍地図点群の取得の部分で、これが大体40%近い処理量となります。その他の computeDerivatives がおおよそ2. Matchingスコア値の計算をしている、演算中心の部分です。

前半の今回は、まず入力として入ってきたLiDAR点それぞれに対し、物理的に近傍な地図点群を取得する方法を詳しく見ていきたいと思います。

Radius Search on KD-Tree

近傍地図点群(のvoxel)を取得する方法には、木探索による方法と、位置をkeyとしたhashからvoxel(とその上下左右前後にあるvoxel)を直ちに求める方法の、大きく2種類があります。

ここで取得される近傍voxelの数は、続く演算処理量を線形に増加させることに注意します。代表的な関数だけを抽出すると、以下の図のような関係となります。

f:id:james-murakami:20210430102547p:plain

木探索は近傍判定を精密にし、それ自体の処理量は重いですが、結果として近傍数が減少傾向となります。対してhash引きは、近傍判定を等閑にするため軽く、結果として近傍数が増加傾向となり、Scan Matching全体の処理量は増加する可能性があります。下のグラフにもこのことは表れています。

f:id:TierIV:20210504213000p:plain

Radius Search(木探索)の場合のLiDAR InputsとNeighbor Voxelsの関係

(縦: 個数, 横: LiDAR frame)

f:id:TierIV:20210504213051p:plain

HASH引き(周辺26voxel)の場合のLiDAR InputsとNeighbor Voxelsの関係

(縦: 個数, 横: LiDAR frame)

近傍の多寡による処理量のトレードオフとともに、”近さ”の正確さは、位置推定精度にも大きく関わるため、判定方法の選択にはこれらを総合的に勘案して採用することになります。Autowareを使用する際のTuning対象として、様々な近傍探索の実装例や実証実験での採用例があります。

ティアフォーでは表題の通りKD木(K-Dimensions tree)上でのRadius Searchを使用しており、今回はこの探索を見ていきたいと思います。

地図のKD木の構築方法

KD木とは多次元(K-Dimensions)空間を分割して作る二分木です。KD木の構築は、多次元空間を各座標軸に垂直に分割していくことで行われます。KD木の各ノードは多次元空間の部分空間に対応しており、あるノードの子ノードに対応する空間は、親ノードの空間に包含されるという関係にあります。

以下、地図点群からKD木を構築する方法について、具体的に説明します。なお、簡単にするために2次元を例として用いますが、実際にAutowareで用いられている地図は3次元であることは留意してください。また、空間の分割はX軸 → Y軸 → X軸 → … と交互に行われることとします。

{(2, 3), (5, 4), (9, 6), (4, 7), (8, 1), (7, 2)}という6つの点から成る点群から構築する手順を以下のGIFで示します。

f:id:james-murakami:20210430102818g:plain

まず最初に、全体の空間をX軸で2分割するにあたって、点群全体をX軸の値でソートし中央の点 (7, 2)を中心とし、全体を分割します。この時の中心が木のルートノードとなります。

続いて、ルートノードから見て左側の子ノードを構築します。2段目ではY軸の値でソートし、中央となる(5, 4)を中心に2分割します。ルートノードから見て左側の構築が葉まで完了したら、最後に右側の子ノードを構築していきます。

このようにKD木を構築すると、この2次元空間は下図のような領域に分割されることになります。

f:id:james-murakami:20210430102931p:plain

Radius Search

次に、上で構築した木構造に対してクエリ点(入力点)を与え、Radius Searchを行う方法について説明します。例として、点(6, 1)から半径2以内の距離にあるノードを、Radius Searchで取得します。

f:id:james-murakami:20210430103024g:plain

アニメーションの通り、深さ優先探索となります。近傍点の判定は、木を降りていく途中で各ノードに対応する分割点と与えられた点との距離を比較し、所与の半径以内であるか否かで行います。

まず最初に、ルートノードの分割点とクエリ点のX軸の値を比較します。今回は、クエリ点のX軸の値の方が小さいため、左側の子ノードに降りていきます。また、ルートノードの分割点 (7, 2) とクエリ点 (6, 1) の距離は2以下であるので、このノード(7, 2)自身も近傍として判定します。

次に、分割点とクエリ点のY軸の値を比べ、先と同様に、左右どちらの子ノードに降りるかを決定します。今回は(2, 3) のノードに降ります。さて、(5, 4)のノードとの距離は2より大なので、これは近傍点ではありません。また、(2, 3)のノードは葉ノードとなり、これ以上は探索しません。そして(2, 3)のノードもまた近傍点とは判定されません。

正確に近傍点を探すためには、木を降りるだけではなく「登る」処理が必要となります。葉ノードに到達した後、一度1つ上の(5, 4)のノードに登ります。そして、最初に探索しなかった側の子ノード以下に近傍点が存在するかどうかの判定を行います。

判定には、分割点 (5, 4) とクエリ点 (6, 1) のY軸の値の差の絶対値と探索半径との比較を用います。この時、探索半径の方が小さくなり、このことは (5, 4) のノードから見て右側の子ノード以下には近傍点が存在し得ないことを意味しますので、右側の子ノードに降りる必要はなく、探索は打切となります。

上の手順で降りた方向とは逆の枝を判定しつつ、木を順に登っていき、ルートノードまで登ってくることになります。ルートノードでも、最初に降りた方向とは逆、つまり例の場合右側の子ノード以下に近傍点が存在するか確かめましょう。ルートノードにおいて、分割点とクエリ点のX軸の値の差は探索半径よりも小さいので、右側の子ノード以下に近傍点が存在する可能性があります。よって、右側の子ノード側へも改めて降りていき、探索を行っていきます。

後は同様の手順を行い、結果として (7, 2) と (8, 1) の2点がクエリ点 (6, 1) の近傍として取得されます。

ただし、近傍探索の精度よりも高速性を重視する場合、葉ノードに到達した時点で近傍探索を打ち切る、すなわち「登る」処理を行わないことも考えられます。Autowareにおけるこの近傍探索はFLANN (Fast Library for Approximate Nearest Neighbors) というライブラリを用いていますが、このライブラリにおいても、実際「登る」側の処理は行われていません。

以下、OpenCLとして再実装を行いますが、同様の方針を取ることにします。

木構築とRadius Searchの再実装

この節ではKD木の構築を純粋なCとして再実装し、KD木を用いたRadius SearchをLiDAR点に対して並列化するためにOpenCLを用いて実装します。

LiDARの各点の処理や探索は、各点独立して並列に行うことができ、並列化のうまみが大きい部分です。また、KD木の構築も移植性やメモリ配置の透明性をもたせるため、Cとして再実装を行っています。

探索コード: https://github.com/yuteno/kdtree_radius_search_opencl

なお、KD木の構築はhttps://github.com/fj-th/kd-tree-C-language- のコードを参考にし、kdtree_construction.hに実装しています(元のコードは2次元平面上の点群の分割を行なっているため、これを3次元に拡張する形で変更しています)。

kdtree_node * kdtree(kdtree_node * root_node,
                     unsigned * alloc_pointer,
                     struct point * pointlist,
                     int left, int right, unsigned depth,
                     int * node_indexes) {
  if (right < 0) {
    // not allocate
    return NULL;
  }
  // allocate
  kdtree_node * new_node = &root_node[*alloc_pointer];
  *alloc_pointer = *alloc_pointer + 1; // count up

  // index list included in this node
  new_node->left_index = left;
  new_node->right_index = left + right + 1;
  // tree's depth
  new_node->depth = depth;
  int median = right / 2;

  if (right == 0 || depth > MAX_DEPTH) {
    // new_node is leaf
    new_node->location = pointlist[median];
    // no child node
    new_node->child1 = NULL;
    new_node->child2 = NULL;

    qsort(pointlist, right + 1, sizeof(struct point), comparex);
    new_node->leftmost = pointlist[0].x;
    new_node->rightmost = pointlist[right].x;

    qsort(pointlist, right + 1, sizeof(struct point), comparey);
    new_node->downmost = pointlist[0].y;
    new_node->upmost = pointlist[right].y;

    qsort(pointlist, right + 1, sizeof(struct point), comparez);
    new_node->zlowmost = pointlist[0].z;
    new_node->zupmost = pointlist[right].z;

    for (int i = 0; i < right+1; i++) {
      node_indexes[new_node->left_index+i] = pointlist[i].id;
    }

    return new_node;
  }

  // change the dividing direction
  new_node->axis = depth % 3;

  // sorting by space (1 direction) for binary division
  if (new_node->axis == XAxis) {
    // ascending sorting by point.x
    qsort(pointlist, right + 1, sizeof(struct point), comparex);
    new_node->leftmost = pointlist[0].x;
    new_node->rightmost = pointlist[right].x;
  } else if (new_node->axis == YAxis) {
    // by point.y
    qsort(pointlist, right + 1, sizeof(struct point), comparey);
    new_node->downmost = pointlist[0].y;
    new_node->upmost = pointlist[right].y;
  } else if (new_node->axis == ZAxis) {
    // by point.z
    qsort(pointlist, right + 1, sizeof(struct point), comparez);
    new_node->zlowmost = pointlist[0].z;
    new_node->zupmost = pointlist[right].z;
  }


  node_indexes[new_node->left_index+median] = pointlist[median].id;
  //printf("node->left_index+median: %d\n", new_node->left_index+median);
  new_node->location = pointlist[median];
  new_node->child2 = kdtree(root_node, alloc_pointer,
                            pointlist + median + 1,
                            /*left  =*/ left + median + 1,
                            /*right =*/ right - (median + 1),
                            /*depth =*/ depth + 1, node_indexes);

  new_node->child1 = kdtree(root_node, alloc_pointer,
                            pointlist,
                            /*left  =*/ left,
                            /*right =*/ median - 1,
                            /*depth =*/ depth + 1, node_indexes);

上記コードは、KD木を構築したい点群の配列を pointlist として与え、再帰的に木構造を構築するコードです。L. 51 ~ L. 66 では、X, Y, Zのいずれかの軸で空間を分割します。pointlistqsort でソートし、その中央を分割点としてノードを構築する部分は、前節で説明した通りです。ソートしたpointlistのうち、分割点よりもindexが小さい点群を左の子ノード(child1)に渡し、分割点よりもindexの大きい点群が右の子ノード(child2)に渡されます。また、node_indexesには点群のインデックスとノードのインデックスを対応させるのに必要な情報を格納します。

新たなノードを作る際に受け取った点群数が1であるか、または木の深さがMAX_DEPTHよりも大きくなれば、そのノードを葉ノードとして木の構築をそこで打ち切ります。この時、葉ノードであることは、child1およびchild2NULL とすることで表現します。

また、root_nodeポインタにはあらかじめ木のノード数分の領域を確保しておき、alloc_pointerを用いて、木のルートノードを先頭としてノードが構築された順にメモリ上に配置されるようにすることで、配置の透明性を上げています。

Radius SearchをOpenCLで実装した際のカーネルコードkernel_radius_search.clを以下に示します。

__kernel void radiusSearchCL(const global float * lidar_points_x,
                        const global float * lidar_points_y,
                        const global float * lidar_points_z,
                        const global float * map_points_x,
                        const global float * map_points_y,
                        const global float * map_points_z,
                        const global int * node_indexes,
                        __constant kdtree_node * root_node,
                        const int n,
                        const int limit,
                        const float radius,
                        global int * neighbor_candidate_indexes,
                        global float * neighbor_candidate_dists) {

  // 1d range kernel for the point cloud
  int item_index = get_global_id(0);
  if (item_index >= n)
    return;

  float4 reference_point = (float4)(lidar_points_x[item_index],
                                    lidar_points_y[item_index],
                                    lidar_points_z[item_index],
                                    0);

  if (item_index == 0)
    printf("ref point (%f, %f, %d), radius %f, limit %d\n", lidar_points_x[item_index], lidar_points_y[item_index], lidar_points_z[item_index], radius, limit);

  __constant kdtree_node  *current_node = root_node;
  int neighbors_count = 0;

  int level = 0;
  float dist;
  float4 dist_square;
  // radius search
  while (true) {
    if ((current_node->child1 == NULL) && (current_node->child2 == NULL)) {
      //reached to leaf node

      for (int i = current_node->left_index; i < current_node->right_index; ++i) {
        int index = node_indexes[i];
        float4 map_point = (float4)(map_points_x[index],
                                    map_points_y[index],
                                    map_points_z[index],
                                    0);
        for (int d = 0; d < 4; d++) {
          dist_square[d] = (map_point[d] - reference_point[d]) * (map_point[d] - reference_point[d]);
        }
        //float4 dist_square = (map_point - reference_point) * (map_point - reference_point);
        dist = sqrt(dist_square[0] + dist_square[1] + dist_square[2]);
        if (dist < radius) {
          neighbor_candidate_indexes[limit*item_index + neighbors_count] = index;
          neighbor_candidate_dists[limit*item_index + neighbors_count] = dist;
          neighbors_count++;
        }
        if (neighbors_count >= limit){
          break;
        }
      }
      break;
    } else {
      int index = node_indexes[current_node->left_index + (current_node->right_index - current_node->left_index-1) / 2];
      float4 map_point = (float4)(map_points_x[index],
                                  map_points_y[index],
                                  map_points_z[index],
                                  0.0f);
      //dist_square = (map_point - reference_point) * (map_point - reference_point);

      for (int d = 0; d < 4; d++) {
        dist_square[d] = (map_point[d] - reference_point[d]) * (map_point[d] - reference_point[d]);
      }
      dist = sqrt(dist_square.x + dist_square.y + dist_square.z);
      if (dist < radius) {
        neighbor_candidate_indexes[limit*item_index + neighbors_count] = index;
        neighbor_candidate_dists[limit*item_index + neighbors_count] = dist;
        neighbors_count++;
      }
      if (neighbors_count >= limit){
        break;
      }
      float val = reference_point[current_node->axis];
      float diff = val - current_node->axis_val;

      __constant kdtree_node * next;
      if (diff < 0 && current_node->child1) {
        next = current_node->child1;
        // printf("%d in (%d), select child1\n", item_index, n);
      } else {
        next = current_node->child2;
        // printf("%d in (%d), select child2\n", item_index, n);
      }
      current_node = next;
    }
  }
}

OpenCLの各スレッドはitem_indexを用いて入力点群のうちの1つを取得してクエリ点とし、構築したKD木に対して近傍探索を行います。

L. 35 からのwhile文で近傍探索を行ないます。

葉ノードに到達するまでは L. 60 からのelse節が実行され、ノードの分割点とクエリ点の距離を計算し、近傍点かどうかの判定を行いつつ適切な子ノードに向かって木を降りていきます。

葉ノードに到達すると L. 36 のif節が実行されます。葉ノードには分割点以外にも複数の点が含まれる場合があるので、それら全てとクエリ点の距離を計算し、近傍判定を行います。

また、前述の通り木を「登る」処理は行わないので、葉ノードに到達するか、あるいは取得した近傍点の数がlimitを超えた場合にwhile文からのbreakとなり、処理が終了します。

Host-Device間メモリについて

KD木を用いたRadius Searchの処理を各点で並列化するためには、ホスト側で構築したKD木を各スレッドが参照できるメモリに配置する必要があります。そこで、__constant修飾子を用いてコンスタントメモリに木構造を配置しますが、そのまま配置するとホスト側で、特に子ノードをポインタを使って表現されている木構造が、コンスタントメモリ上でも同様の構造を保つ保証がなくなってしまいます。

f:id:james-murakami:20210430103111p:plainOpenCL 2.0から実装されたShared Virtual Memory (SVM) という機能を用いて上の問題を解決できます。名前の通り、ホストとデバイスで仮想メモリ空間を共有できる機能であり、今回の木構造のメモリのデバイス間共有に最適です。SVMについてはFixstars社のTech blogでも詳しく紹介されています。また、OpenCLにおいてメモリ構造の記事は、こちらの記事が大変参考になります。

AutowareのTutorial ROSBAGとMapを用いた動作

OpenCL実装したRadius Searchの動作確認を行うために、Autowareの動作確認に用いられているMapとLiDARのデータを使用します。しかし、LiDARのデータは、Autoware用のサンプルデータにおいてrosbagの中のtopic群として提供されていますし、何より座標変換の処理をしたり、複数のセンサーデータの統合もしなければなりません。動作確認だけのために、本実装にこの処理を追加するのは面倒なので、今回はAutowareを一部改変し、Scan Matching処理直前の点群を、外部ファイルに保存するという手段を取りましょう。

再びAutowareでの実装である、localization/pose_estimator/ndt_scan_matcher/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp を見ていきます。

LiDAR点群が到着するたびに、コールバック関数 callbackSensorPoints が呼ばれ、それを入力としてScan Matching処理が行われます。そこで、この関数内で入力として使われている点群を外部ファイルに保存します。

先に座標変換の話に触れましたが、rosbagの中に存在するLiDAR点群の座標は、あくまでLiDARセンサから見たものであり、Map点群とのMatching処理入力として有効であるためには、車体およびMapに対して座標変換を行っておく必要があります。

ndt_scan_matcher_core.cpp の L.310setInputSource 関数を用いて sensor_points_baselinkTF_ptr という点群が入力として設定されていますが、この点群はまだ車体位置・姿勢に対しての座標変換が行われていません。車体位置・姿勢に対しての座標変換行列は、L. 346align 関数の引数として使われているinitial_pose_matrix です。そこで、PCL (Point Cloud Libary) の pcl::transformPointCloud 関数を用いて、sensor_points_baselikTF_ptrinitial_pose_matrx で座標変換を行います。このようにして変換した点群を pcl::io::savePCDFile 関数でpcdファイルに保存することで、Autoware用サンプルデータ内の点群を、今回の実装の入力として適当な形に変換することができます。

ぜひティアフォーへ!

今回の調査や再実装の動機は、採用できる計算システムの選択肢を広げたいところにあります。実証実験のソフトウェアは実証実験のシステムや環境からの影響を多大に受け、さらに使用しているライブラリ類の制限を多分に受けます。計算機側の立場は高速な開発/実証実験とは対極ではありますが、HW/SWの双方に高水準な性能が要求される自動運転開発の両軸です。

移植性についても、あらゆるシステムに移植できることは幻想です。ただ、特定のシステムに依存することは、本質的に効率の悪い選択肢をとらざるを得なければならない危険性を抱えてしまいます。

このようにティアフォーでは、自動運転技術の精度性能のチューニングはもちろん、計算システムでの性能のチューニングも行っております。計算機およびアルゴリズムは自信あり、でも自動運転は興味があるだけ...という、そこのお方!ぜひ一緒に完全自動運転を実現する計算機を探索しましょう!下記リンクからお問い合わせお待ちしています! 

tier4.jp

筑西市における小型自動搬送ロボットの実証実験

こんにちは、ティアフォーのフィールドインテグレーションチームに所属している帯津です。今回は2021年3月29日から4月13日の期間で、茨城県筑西市にある道の駅「グランテラス筑西」と周辺地域で実施しました、小型自動搬送ロボットの実証実験の概要と小型自動搬送ロボットならではの実証ポイントについてご紹介します。

なお、ティアフォーでは、自動運転の安心・安全を確保し「自動運転の民主化」をともに実現していく、様々なエンジニア・リサーチャーを募集しています。もしご興味があればカジュアル面談も可能ですので以下のページからコンタクトいただければと思います。

tier4.jp

実証実験概要

小型自動搬送ロボットの実証実験を行う背景には大きく分けて2つの理由があります。1つ目は、eコマースの普及にともなう物流量の増大や施設管理の大規模化が進み、対応する人手不足が深刻化しているため、2つ目は新型コロナウイルス感染症拡大にともない、新しい生活様式が広まる中、非接触型の配送ニーズが高まっているためです。そして将来的には自動配送ロボットを活用した新たな配送サービスの実現が期待されています。

科学都市であるつくば市を擁する茨城県は、Society 5.0地域社会実装を強く推進、その茨城県配下の筑西市が手をあげ、本実証実験の支援を頂くこととなりました。

概要

本実証実験では、仕様が異なる2台の小型自動搬送ロボット「Logiee」(以下、Logiee)で、下記2つの自動配送を実施しました。

  1. 道の駅「グランテラス筑西」と周辺農家・民家を結ぶ公道の遠隔監視型配送
  2. 「グランテラス筑西」施設敷地内における複数地点を結ぶ近接監視型配送

上記2のルートについては、株式会社オプティマインドの配送ルートを最適化する自動配車システム「Loogia」(以下、Loogia)と、ティアフォーの自動運転車両の運行管理サービスFMS(以下、FMS)との連携によるスケジュール配信の実験を行いました。

近接監視型と遠隔監視型

小型自動搬送ロボットの運用では、自動運転システムの監視者はロボット近傍で常時監視するか、ロボットに搭載したカメラの映像を遠隔監視室で常時監視するかによって2つの監視型に分けられ、前者を近接監視型、後者を遠隔監視型としています。

近接監視型は、近接監視者が車両近傍において、車両周囲の状況を目視で随時確認を行うとともに、自動運転OS Autoware(以下、Autoware)の稼働状況も随時確認します。また、システムの異常時や危険を感じた場合は、車両の緊急停止ボタンを押下、手動走行に切り替えての危険回避などを実施し、安全性を確保します。

一方、遠隔監視型は、遠隔監視者が遠隔監視室よりモニターに映し出される車両のカメラ映像および各種情報より車両周囲の状況を確認するとともに、Autowareの稼働状況を監視し、異常が発生していないか監視します。また、必要に応じて緊急停止やシステムへの介入を行い、遠隔コントローラーを操作して危険回避などを実施し、安全性を確保します。

車両コンセプト

前述のとおり、私有地と公道をそれぞれが連携して走行させるため、異なる仕様のLogieeを使用しました。

1つは、公道走行が可能な高スペック自動搬送ロボット『LogieeS-TC』(以下、LogieeS-TC)、もう1つは、私有地での走行に限定した廉価版の自動搬送ロボット『LogieeSS』(以下、LogieeSS)です。

LogieeS-TCは一般消費者向けを想定しており、丸みを帯びたデザインとなっております。また、前後左右に搭載したカメラの映像が遠隔地からオペレータが監視可能であるほか、LogieeS-TCと遠隔室のオペレータ間に内蔵されたマイク、スピーカーを使うことで、通行する歩行者の方への呼びかけ、通話も可能となっています。さらに、商品の受け渡しの際に、オペレータと通話することで、初めて配送サービスを利用する方でも安心して利用することが可能です。事実、実証実験期間中に、地域の子どもたちとLogieeS-TC(このイベントでは「ろーじー」と称していました)が会話し、子どもたちの疑問を「ろーじー」が答えるなど、イメージキャラクターとしての役割も担いました。

一方、LogieeSSはビジネス向け用途を想定しており、今回の実証実験の場となったグランテラス筑西内を走行させるというよりも、一般には目にすることがないバックヤードでの走行を想定しているため、LogieeS-TCと比べて、LogieeSSでは遠隔監視機能、通話機能などはなく、デザインもシンプルです。

ただし、自動走行にフォーカスを当てると、LogieeS-TCとLogieeSSはコンセプト・外観は異なるものの、同一のパワーユニットを使用し、どちらもAutowareをコアアーキテクチャとした自動搬送ロボットです。なお、LogieeS-TCならびにLogieeSSの操舵方式は、後輪2輪の独立制御による差動2輪方式です。差動2輪方式は構造がシンプルである一方、2輪の駆動状況がパス追従性に大きく影響を及ぼします。例えば、2輪のタイヤエアー圧のバランスが異なると、タイヤ中心(回転軸)から接地面(地面)までの距離(タイヤ径)が変化し、直進指示を与えているにも関わらず、エアー圧が低いとタイヤ径が小さくなり、その結果、エアー圧が低い方へ曲がっていく挙動を示します。

そのため、実証実験期間中は、日々の車両点検で、外観チェック、カメラ・センサーの動作確認を実施するとともに、タイヤエアー圧の確認を実施し、安全な運行に努めました。

f:id:TierIV:20210504003334p:plain

走行ルート

走行ルートは、道の駅「グランテラス筑西」施設内と周辺の公道になります。今回、(A)施設内テナントの商品を地域住民へ配送する、(B)周辺農家から農産物をピックアップし、施設内テナントへ配送する、という2つのシナリオについて実証実験を実施しました。

(A)のルートは、以下の画像の緑のラインで示す道の駅「グランテラス筑西」施設敷地内のルートと、施設内から公道へ出て、周辺民家を周回するピンクのラインを走行するルートに分かれます。

一方、(B)のルートは、最初に公道を走行して農産物をピックアップし、施設へ戻り、その後、施設内テナントへ農産物を配送するルートです。

それぞれの走行ルートでLogieeが停車する箇所を便宜上「バス停」とし、FMSより次に向かうバス停までのルートを配信して各シナリオに即したルート指示を行いました。

f:id:majestickou:20210419093925p:plain

ODDアセスメント

Operational Design Domain(以下、ODD)とは、自動運転システムが安全に作動するための前提条件です。あらかじめ定められたODDの範囲内でのみ自動運転システムが作動するように設計することで、走行時の安全性を担保します。

Logieeが安全に走行するためには、上記よりODD設定が必要不可欠で、走行環境や運用方法を制限することで、事故などを未然に防ぎます。

ここで、設定した条件が欠けた場合などODD範囲外となった場合、安全な運行停止措置を行うか、手動運転への切り替えを行い、ODD範囲外となった原因を排除し、ODD範囲内での走行とします。

以下は、公道走行におけるODDです。道路条件では、決められたルートのみの走行であることや信号・横断歩道がないため、ODD範囲外となるケースでは、車両(自動車、自転車など)、人の接近がリスクとして挙げられます。

f:id:majestickou:20210419095231p:plain

このように、ODD範囲外となるケースを評価していく手法をODDアセスメントとして、事前に評価を実施しました。

走行ルートとODD道路条件が同一のルート区間(レーンのない道路、歩道、交差点など)をロードセクションと称し、ロードセクションごとにODD範囲外となるケースを割り出し、ODD範囲外となる確率が高いロードセクションに対し、Autowareの機能を維持・制限した状態でAutowareの稼働を継続させるフォールバックや、車両を一時停止させるミニマル・リスク・マヌーバー、手動運転への切り替えを行うなど、運行上の安全を担保する施策を行いました。

以下の図は、公道走行ルートの各ロードセクションにおけるODD範囲外ユースケースと全体に占める割合の評価結果です。(ロードセクションNo.1〜No.11は施設内のため、本稿では割愛致します。)評価結果として、車両接近によるODD外れが大きな割合を占めており、保安要員による監視が安全を担保する上で重要な役割を担うことになります。

そのため、監視・操作者と保安要員とのコミュニケーションプロトコルを定め、監視・操作者からは車両の状況を、保安要員からは接近する人・車情報を伝達することで、ODD範囲外を排除することができました。

f:id:majestickou:20210419095424p:plain

オペレーションイメージ

前述のシナリオ(A)、すなわち商品を地域住民へ配送するオペレーションイメージは以下の図のとおりです。

まずは、地域住民から電話orメールでカスタマーセンターへ注文が入ります(①)。カスタマーセンターのオペレータは、Loogiaへ情報を入力すると(②)、Loogiaは最適なルートを割り出し、画面に表示します(③)。オペレータは注文があった住民へ電話orメールで配達日時を連絡します(④)。また、該当商品を取り扱う店舗に対しては電話orメールにて集荷日時を連絡します(⑤)。

集荷先、配送先への連絡後、カスタマーセンターのオペレータは、Loogiaの画面よりルート配信を実行すると、Loogiaは、ティアフォーが開発している配車管理サービスのFMSへ、データインターフェース(API)を通してスケジュール登録を行います(⑥)。その後、FMSはLogieeへルート配信を行います(⑦)。ルート配信を受けたLogiee(今回、施設内走行はLogieeSS)は店舗前まで自動走行し、店舗スタッフより商品を受け取ります(⑧)。その後、商品を公道走行可能のLogieeS-TCへ載せ替えるため、施設内に設定したパーキングロットへ向かいます。向かった先にはLogieeS-TCが待機しているので、LogieeS-TCの隣に駐車し、そこにいる人が商品をLogieeS-TCへ載せ替えます(⑨)。商品を載せたLogieeS-TCは注文のあった住民宅まで公道を自動走行し、商品をお届けします(⑩)。

f:id:TierIV:20210504002951p:plain

上記は地域住民へ商品を配送するオペレーションイメージですが、周辺農家から農産物を受け取り、施設内テナントへお届けするオペレーションも同様のイメージです。

なお、FMSの詳細につきましては、以下の記事で紹介しています。
https://tech.tier4.jp/entry/2019/04/05/165349

tech.tier4.jp

小型自動搬送ロボットならではの実証ポイント

本実証実験では、2台の仕様が異なる小型自動搬送ロボットで行いましたが、自動運転システムとしてフォーカスすると、自動運転システムのコアアーキテクチャは、公道走行している車両のシステムと同様のAutowareです。このことは、市街地における公道走行が可能なアーキテクチャがLogieeにも採用されていることを指しており、高度な安全性、信頼性をもつ小型自動搬送ロボットとなっています。

一方、運用面のユースケースにフォーカスすると、2台の小型搬送ロボットが同じ場所に到着し、荷物の受け渡しをしやすいようにする必要があったり、公道を走行中に路肩に障害物があれば少し避けたり、遠隔監視走行から近接監視走行へ速やかに切り替え、近接操作者(ロボットの傍で監視を行っているオペレータ)によるマニュアル操作で障害物を回避したい、といった小型自動搬送システムとしての要求が発生します。

これら運用上の要求に対し、Autowareの機能となるパーキング、障害物回避を実証ポイントとして実験を行いましたので、以下にご紹介します。

パーキング

パーキングは、いわゆる駐車することを目的としているほか、LogieeSSが集荷してきた荷物をLogieeS-TCへ積み替えるためのポジショニングを主目的としています。物流業界において、トラックを荷積みのために出荷場所へ駐めるイメージです。

自動走行は地図情報にあるレーン上を走行しますが、パーキングは、レーン上で停止した位置(スタート位置)からパーキングロット(駐車エリア)上の目標位置(ゴール位置)まで移動することができるモード(シナリオ)です。

Autowareではレーン走行(lane driving)、パーキング(parking)の各シナリオを、Planningモジュール内のScenario selectorで、条件判定し切り替えを行います。

Scenario selectorは、レーン上からパーキングロットへ移動しようとした場合にパーキングへ切り替えを行います。

以下の図は、実際に施設内に設定したパーキングロット上まで移動するパス(赤色の線)です。スタートからゴールまでのパスは、Hybrid A*アルゴリズムによるパスプランニングを行うことで生成しています。

f:id:majestickou:20210419100116p:plain

パスプランニングでは、スタート、ゴールの位置、姿勢から横列、縦列の判断、さらに右折、左折、スイッチバック、方向転換などの動作を算出します。

以下の図は、シミュレーターを使用して、さまざまなスタート、ゴールの位置、姿勢に対するパスプランニングを行った結果です。なお、Vehicle model付近にある白い矢印は車両の向き、赤線は算出されたパス、ピンクの矢印は走行方向、青の点がゴール位置で、赤の線がゴール時の車両の向きを示します。

f:id:majestickou:20210419100213p:plain

下の写真は、パーキングロットにパーキングさせたLogieeSSとLogieeS-TCで、スタッフが農産物が入った箱の積み替えを行っているデモ風景です。

f:id:majestickou:20210419100340p:plain

障害物回避

小型自動搬送ロボットLogieeが障害物回避するユースケースとして、ロボットが走行するレーンに侵入している”もの”の回避があります。ここでいう”もの”とは、駐輪場から少しはみ出した自転車の車輪、置き去りにされたスーパーのカートなど、生活空間における無作為にレーンにはみ出しているものを差します。

これらの”もの”がレーン上にある場合、Logieeは障害物として認識し停止し、近接監視者、あるいは遠隔監視者がLogieeSを手動介入して障害物を回避しますが、Logieeがレーンをはみ出すことなく、ほんの少し避けるだけで”もの”を回避しても走行可能領域内にあると判断できる場合、Autowareは回避動作にともなうパスプランニングを行い、その結果、Logieeは障害物を自動回避します。

このことは、複数台の運行を鑑みた場合、ほんの少し避けるだけで自動走行が可能なケースではLogieeは自動で回避し、レーンを逸脱しなければ回避できない場合のみ手動介入して回避することで手動介入率を下げ、効率的な運行が可能となります。

今回、公道でのレーン幅は1.5mとしており、車両幅が0.75mであるため、左右のマージンは0.375m、理論上では30cmほどレーンに侵入している”もの”を回避できることになります。それ以上の侵入がある場合は、障害物停止となります。

また、物体認識にはLiDARの点群からクラスタリングを行う”Euclidean Clustering”を使用していますが、この手法では、物体全体の形状推定は行っていないことから、1つの物体を複数と認識したり、複数の物体を1つの物体と認識してしまいます。

そのため、走行する場のユースケースにあったパラメータチューニングが必要ですが、今回は、障害物回避の有効性について検証を進めました。

以下の動画は、県道に隣接した歩道で行った障害物(しゃがみこんだ人)の回避の様子です。

youtu.be

 実証実験の様子

3/29(月)から現地での実証実験準備を進め、4/2(金)が近接監視走行の警察審査、4/9(金)が遠隔監視走行の警察審査と進めていき、4/12(月)、4/13(火)の2日間がお本番というスケジュールで進めていく中、期間中に多くの方々にお越し頂きました。

以下は実証実験の様子の動画です。 

www.youtube.com

リハーサル、ならびに実証実験本番で、LogieeS-TCは、およそ30回、距離にして30kmの集荷・搬送を、LogieeSSでは、およそ30回、距離にして10kmの配送を実施し、実用化へ向けた課題などを洗い出すことができました。

今後の小型自動搬送ロボットによる実証実験について

冒頭でも触れましたが、昨今の新型コロナウィルス感染症拡大により、宅配サービスが急増し、それにともない、非接触型の配送ニーズが高まっています。

また、政府の成長戦略実行計画(令和2年7月)における低速・小型自動配送ロボットの社会実装に向けて「遠隔監視・操作」型の公道走行実証を実施するとの方針により、宅配ロボット事業へ参入する企業が増加し、実証実験も各地域でさかんに実施されています。

小型自動搬送ロボットにおける配送サービスの実用化には、法整備、インフラ整備など課題は山積みですが、その課題を解決すべく、国が動き出しています。

自動走行では、私たちティアフォー以外にも、大手メーカーが限定的ではあるものの公道での自動運転レベル3、あるいはレベル4の実用化に向け開発を進めていますが、法規制の敷居が高いのが現状です。

一方、小型自動搬送ロボットによる自動走行では、小型、低速(歩行レベル)で、歩道をメインに走行することから、リスクレベルが低く、国内における自動走行の実用化、ビジネスとして収益を得る土壌ができつつあります。

2021年度は、普及へ向けたさらなる拡大が予想されており、私たちティアフォーもパイロット企業となるべく技術改革を進めていきます。

 実証実験を終えて・・・

仕様が異なる2種類の小型自動搬送ロボットを私有地、公道を連携して走行させる今回の実証実験は国内初であることから、非常に多くの方々が訪問、見学に来られました。これは、自動配送ロボットを活用した配送サービスに対する地域住民の方々の期待の表れと感じとることができました。

また、地域住民の方々の声を直接聞くことで、自動配送サービスに対する気づきを得ることができました。

実証実験を通して得られた新たな課題は、社内へフィードバックし、より良いサービスの提供、ならびに小型自動搬送ロボットにおける自動運転レベル4の実用化にむけ、さらなる検証・評価を進めていく所存です。

自動運転データの検索システムを爆速で作った話

はじめに

こんにちは、機械学習の活用を加速させるために学習インフラの開発やワークフロー自動化に取り組むチームに所属している澁井です。今回は自動運転のためのデータ検索基盤を自作した話を書きます。

なお、ティアフォーでは「自動運転の民主化」をともに実現していくエンジニアを募集しています。今回ご紹介する機械学習だけではなく、様々なバックグラウンドをお持ちの方と開発を進めていく必要があります。下記ページから募集職種のリストをご覧いただき、興味を持った方はぜひお気軽にご連絡ください!

さて、ティアフォーでは自動運転に機械学習を活用しています。主な機械学習の用途は画像や動画データを扱った認識技術への応用です。

機械学習では意味づけ(アノテーション)されたデータが大量に必要となります。アノテーションとはネコの画像には「ネコ」、イヌの画像には「イヌ」と画像に意味を付与することです。

自動運転で扱うデータは主に実証実験で各地を走ったデータになり、機械学習の学習データも実証実験で得たデータになります。しかし実証実験のデータはアノテーションされておらず、自動運転を動かしているROS(Robot Operating System)特有のrosbagというデータ形式になっているため、人間が生データを見て理解することはできません。データを分析して学習データを作るためにはrosbagを画像や動画形式に変換し、人間に読めるようにする必要があります。

f:id:cvusk:20210412082703p:plain

自動運転からROS経由でデータを得る

実証実験で収集するデータ量はビッグデータと呼ぶに相応しく、一日に数TBを超える非構造化データが集まります。実証実験の価値を機械学習にフィードバックするためには収録したデータの中から有益なデータを探索し、学習データに加えられるようにする必要がありました。

f:id:cvusk:20210412082956p:plain

自動運転のデータ規模

社内のデータ活用の状況を確認するため、社内コミュニケーションツールで下記のような質問をしてみました。

質問:rosbagのデータを活用してる方々にオープンに質問なのですが、必要なデータはどうやって検索していますか? たとえば緯度経度や地名による検索とか、年月日、時間帯とか、どういう条件で検索できると便利でしょうか?

 

回答:ほしい!

f:id:cvusk:20210412083253p:plain

社内取引はネコ画像

その結果、みんなデータを探せなくて困っているものの、データを整備する工数が取れない、ということがわかりました。課題は見つけた人が解決する精神で、私が自分で検索システムを作ることにしました。

課題

まず、ティアフォー社内のデータ活用には3つの課題がありました。

  1. データを探せない:実証実験で得たデータは社内に大量に貯まっているが、開発者が目的のデータを簡単に見つけられない。
  2. 保管先がまばら:データは社内ストレージにアップロードされている一方、「どこ」に「いつ」の「どの」データが保管されているのか、統一して整理されていない。
  3. クラウドを使わない:データがクラウドに貯まっているのにローカルの開発端末にダウンロードして使おうとする。

他にも細かい課題は多々ありますが、データ活用を加速して機械学習開発を効率化するためにはデータが探しづらい状態です。ほしいデータを得るためには社内ドキュメントに書かれた実験記録(自動運転で走った場所、日時、天気など)を元に、社内ストレージからrosbagファイルを探してダウンロードし、開発者の端末でrosbagを解凍して必要なデータを探さなければいけませんでした。rosbagは1ファイルで1GB以上のサイズの中に、自動運転中の5、6分程度を記録しています。機械学習に必要なバラエティのあるデータを得るためには5、6分程度では足りません。漏れのないデータセットを作るためには、年間通して場所や明るさ、天気、被写体の位置、大きさ、カメラの方向、自動運転の速度などを組み合わせた多様なデータパターンを用意する必要があります。rosbagファイルをいちいちダウンロードして確認するのはあまりにも非効率です。

そこで、rosbagで収集したデータを検索できるシステムをクラウド上に作ることにしました。検索システムのゴールは社内に散らばったデータを同じインターフェイスで検索し、すぐ利用できるようにすることです。

検索システムをどのように作るか

検索システム開発で立てた目標の一つは、最速で作ってすぐ使える状態にすることです。データは貯まれば貯まるほど整理し使えるようにすることが難しくなります。データベースに登録するだけでも長時間かかりますし、バラエティが増えれば属性の整理やテーブル設計が困難になります。日々数TBの非構造化データが貯まる状況で放っておくことは良策ではありません。もちろんこれまでティアフォーでは検索システムがなくても開発できていたので、自動運転に検索システムは必要ないのかもしれません。しかし、ツールを導入することで仕事が楽になるかどうかは導入してみないとわかりません。検索の有用性(または無用性)を証明するため、スピーディに作ってPoCを進めることも目標としました。

スピード優先で開発するため、検索システムのソフトウェアスタックには下記のすぐ簡単に使えるものを選びました。 

全体像は以下のようになります。

f:id:cvusk:20210412083517p:plain

検索システム(α版)

ソフトウェアスタックと全体像からわかるとおり、主な開発言語はPythonです。検索のレスポンス速度を上げるならばJavaGolangを使ったほうが性能が改善されますが、短い期間で作り切ることを優先してPythonを選びました。ディープラーニングでデータに意味づけするためにはPythonがより扱いやすいという理由もあります。

インフラは以前構築したAmazon EKSのKubernetesクラスターを使っています。個人的にKubernetesが最も楽に使えるインフラになっているからです。データベースは私の好みでAurora PostgreSQLを使っています。

バックエンドAPIはFastAPIとSQL Alchemyで作っています。SQL (Relational) Databases - FastAPIで書かれているデータインターフェイス設計をそのまま参考にしています。Python側のデータスキーマはPydanticで定義することで、関数間のデータ受け渡しで型がわからなくなって実装に迷うことを避けました。型が定まっているとコーディングの手戻りが減ります。

バックエンドAPIPython+FastAPI+SQL Alchemyで手早く済ませたコンポーネントです。機能とデータが増えてパフォーマンス劣化してきたら、他の言語かgRPCで作り変えてORMも排除すると思います。

画面はStreamlitというPythonをベースにWeb画面を作ってデータを可視化するライブラリを使います。通常はHTML + JavaScriptで作る部分ですが、JavaScriptを書きたくないという個人的な理由でStreamlitを選びました。次はFlutterで作りたいと考えています。

以下の公式の動画のように、コンポーネントPythonコードで書くことで画面表示を定義できてとても便利です。

youtu.be

StreamlitであればPythonのグラフ作成ライブラリのMatplotlibやSeabornと組み合わせてデータのグラフを簡単に画面に表示できるため、良い選択だったと思います。 

データパイプラインとデータの意味づけではrosbagから画像を抽出し、データベースとストレージに登録します。抽出するデータは主に一般道を走る自動運転車のカメラで撮影する画像になります。

意味づけのために物体検知を使い、映っている被写体を判定しています。自動運転の機械学習では、認識のためにカメラに映っている歩行者や対向車、バイク、信号機等を検知します。学習済みの物体検知(EfficientDet)で簡易的に被写体を検知し、検索時に被写体の数で絞り込みできるようにしています。データの意味づけには物体検知以外にも、画像の明るさや逆光、緯度経度と住所、日時と場所をベースにした天気、気温による検索を可能にしています。

データパイプラインはKubernetes Jobで定期的にデータを収集、解凍し、意味づけしてデータベースに登録します。

意味づけのおかげで、たとえば以下のように「信号機」が映っている逆光画像を検索できるようにしています。

f:id:cvusk:20210412083656p:plain

うおっまぶしっ

別の例として、天気は「雪」で「自転車」が映っている画像です。

f:id:cvusk:20210412083737p:plain

雪の日の自転車

どのくらいの期間で作ったか

上記の構成と機能で検索システムを作りました。

ところどころ再開発するつもりで、さっさと使えるようにすることを優先して開発しています。以下に各コンポーネントの開発所要時間をまとめます。

  • Kubernetesは構築済みのクラスターにノードグループを追加し、各コンポーネントを動かすマニフェストを書きました。所要時間は1時間以内です。
  • バックエンドAPIとテーブル設計は最低限の機能と正規化にとどめ、2週間程度で開発しています。
  • 画面はStreamlitのおかげで3日くらいで作れました。マークダウンやテーブル、チェックボックス等Web画面に必要なコンポーネントPythonのコード数行で書くことができるため、とても楽に作れました。
  • データパイプラインはrosbagを解凍しデータベースとストレージに登録するコードをシングルプロセスで書いています。今後収集するデータ量が増えたら並列処理を検討します。データの意味づけは学習済みの物体検知モデルや簡易的な明るさ評価を活用しています。データの意味づけは開発者の要望を聞きながらオンデマンドで追加していったため所要時間はまちまちですが、1機能1週間程度です。

作ったものを適宜リリースしていく方式で、最初はバックエンドAPIと画面だけを用意し、オリジナルのrosbagデータを取得した日時や場所だけで検索できる状態で社内公開しました。ここまで大体3週間で第1弾をリリースしています。

そこから毎週1機能追加を目標としました。1週間ごとに物体検知、天気や気温による検索、画像の明るさや逆光検索を追加していきました。

他の作業をこなしながら検索システムを作っていたのですが、1ヶ月強で作ったわりには便利なものが作れたと思います。

f:id:cvusk:20210412083828p:plain

大体3週間ちょっとで作る

機能追加時は最初にモック画面と動画を作り、機能のイメージを利用者に共有してから実物を作る流れを取りました。検索の仕様を言葉で要件定義する作業を減らし、見えて使えるものを作ることにこだわっています("Working software over comprehensive documentation"または"Done is better than perfect")。これもスピード優先で作るための工夫です。

もっと検索できるようにしたいデータがあった

ここまで2021年3月中に開発とリリースを進めてきました。社内でデータ検索システムを作っていることを宣伝したおかげで、当初は存在を知らなかったデータや、課題が不明確だったデータ活用のシーンが判明しました。

その一例がアノテーションされたデータの検索です。機械学習で使うデータは外注してアノテーションしているのですが、アノテーションされたデータも画像とJSON形式でストレージに貯まったままで、データベース管理や検索可能化はされていません。アノテーションされたデータが貯まれば貯まるほど、学習条件に合うデータを探すのに人手と時間を要するようになります。そこで、アノテーションされたデータを元データの意味とアノテーションを組み合わせて検索する機能を開発しました。

信号機のアノテーションされた画像(以下は「黄色信号」の含まれる画像)を検索できるようにしています。

f:id:cvusk:20210412083926p:plain

小さくて見えないけど黄信号が写っています

この過程でアノテーションされたデータを整理し数量をまとめることになりました。そこで判明したことは、アノテーションは特定のカテゴリに偏っており、本来考慮すべきだが、アノテーションが少ないデータカテゴリが存在することです。日々扱っているデータでも、整理、集計しないと実態を知ることはできないということです。

今回は画像データをターゲットに検索データを作りましたが、その他にもポイントクラウドのデータ等、自動運転には欠かせないデータが残っています。また、社内ストレージから発掘された未知のデータを検索できるようにしたいという要望が出てきています。

個人的には「類似した状況」を条件に検索できるようにしたいと思っています。自動運転では運転中の周囲の状況が運転の判断を左右することがあります。左を並走している自動車が車線変更して先方に入ってきたり、先行している自動車が右折したり、道路幅が狭くなっていたり、周囲に建物があったり、林の中を走っていたり、自転車が並走していたり・・・。多種多様な状況を認識して自動運転するため、こうした「状況」を検索できるようにすることで、状況に応じた運転ロジックの開発を効率化することができると思います。画像や動画を入力にして似ている状況を検索する類似状況検索のようなシステムを作りたいです。

まとめ

データは貯めるだけでもシステム開発とコストが必要となりますが、貯めるだけでなく使える状態にするためにはデータの意味づけと検索が必須になります。データは容量が増えれば増えるほど整理し管理する難しさが増します。加えてデータを使えていない状態では、データを使えない不便さに気づけない事態が頻繁に発生します。データ検索のあり方はデータの使い方や目的次第で千差万別です。今回の開発では素早く作って直しながら、本当に必要なものを目指していく方針がうまくハマりました。

最後に1つだけご紹介させてください。

4月21日にティアフォー初の【Tier IV SUMMIT 2021】をオンラインで開催します!

参加は無料で自動運転の最前線を語ります!ぜひご登録ください!

tier4summit.com

自動運転の実証実験を「安全」に進めるためのリスクアセスメントの活用について

こんにちは、ティアフォーで安全に関して担当している新宅と申します。今回は、全国で進めている実証実験を「安全」に進めるためのリスクアセスメントの活用についてご紹介します。実証実験に限らず、安全を担保することは自動運転車両を開発する上で必要不可欠であり、社会受容性を高めていくための最も重要な要素の1つとなります。

なお、ティアフォーでは「自動運転の民主化」をともに実現していくエンジニアを募集しています。今回ご紹介する安全だけではなく、様々なバックグラウンドをお持ちの方と開発を進めていく必要があります。下記ページから募集職種のリストをご覧いただき、興味を持った方はぜひお気軽にご連絡ください!

安全に関わる最近のデータや動き

リスクアセスメントについてお話する前に、安全に関わる最近のデータや動きについてご紹介します。

年間の交通事故死者数

2021年1月4日、警察庁が2020年の交通事故死者数は前年よりも376人少ない2,839人だったことを発表しました。この数字は、4年連続で戦後最小を更新し、かつ初めて3,000人を下回ったとのことです。

f:id:yst4:20210322103308p:plain

出典:警察庁ウェブサイト

この数字を見て、本記事を読んでいただいている皆さまは多いと思うでしょうか?それとも戦後最小を更新し続けているのだから少ないと思うでしょうか?おそらく皆さまそれぞれの感覚があるかと思います。しかし、1つはっきりしていることは、交通事故の数が限りなくゼロに近づけば同時に交通事故死者数も減っていきます。自動運転が普及することの意義の1つには、交通事故の数が限りなくゼロに近づいていくことにあります。一方で、仮に全て自動運転車になれば事故の数はゼロになるのでしょうか?不慮の事態を考慮すると、残念ながら自動運転が実現しても事故の数がゼロになるとは言えません。それでも、事故の数を限りなくゼロにすることを目指すべく、自動運転車の開発を進めています。これをエンジニアリング観点で言えば、許容可能なリスクの発生確率や損害の程度を定量的に定め、総合的に許容可能な範囲を決めながら安全な設計を行っています。

自動走行の実現に向けた国の直近の動き

 3月はじめに、「自動走行ビジネス検討会」から「自動走行の実現及び普及に向けた
取組報告と方針」Version5.0の概要案が示されたのでこちらも簡単にご紹介します。

f:id:yst4:20210322162229p:plain

出典:経済産業省・自動走行ビジネス検討会ウェブサイト

特に(1)では、本記事の主題である安全に関する記載があります。ここでは、米国NHTSA(運輸省道路交通安全局)の項目を参照しながら日本版セーフティレポート(仮称)と称して情報発信することの重要性を示し、さらに「走行環境・運行条件で想定されるリスクを網羅的に評価し、その安全対策をあらかじめ十分に行う、セーフティアセスメント(仮称)がきわめて重要」*1と判断しており、今年度を目途にセーフティアセスメント(仮称)に係るガイドラインが作成される予定となっています。また本概要案では、2025年度までに40か所以上で自動運転レベル4の無人自動運転サービスを実現する旨の記載もあり、今後5年以内に自動運転レベル4の普及が進んでいくことが考えられます。

米国NHTSAの項目について興味のある方は、https://www.nhtsa.gov/document/automated-driving-systems-20-voluntary-guidanceをご参照ください。また、エンジニアリング観点での「安全」については、安全への取り組み:②自動運転の安心・安全について - Tier IV Tech Blogでご紹介しているので是非ご覧ください。

ティアフォーの実証実験の安全をリスクアセスメントで担保

さて、昨年の交通事故死者数から自動運転車開発の意義の1つを、そして国でも安全に自動運転車を普及・実現させていくために様々な動きがあることを見てきました。さらに「自動走行ビジネス検討会」では、セーフティアセスメント(仮称)に係るガイドライン作成が予定されている旨お話しましたが、ここからは全国で進めている実証実験を「安全」に進めるためにティアフォーが既に活用している「リスクアセスメント」についてご紹介します。現在ティアフォーでは、異なる観点から「ODDアセスメント」と「走行ルートサーベイ」という2種類の「リスクアセスメント」を行っています。

※ODD:「Operational Design Domain」の略で、「運行設計領域」と呼ばれる自動運転システムが作動する前提となる走行環境条件のことです。

ODDアセスメントとは?

まず、なぜODDアセスメントが必要なのでしょうか?ティアフォーでは下記のような理由からODDアセスメントを実施しています。

  • 走行ルートを決める際、車両を走行させることなく自動走行の可否を事前に把握するため
  • 走行限界を超えている箇所について、事前の対策を行うことが出来るため
  • アセスメントの結果をドライバーやオペレータに周知することで、より安心・安全な走行を実現するため
  • アセスメントの結果を活用し、運用面でのリスクを可視化するため

実施にあたっては地図・走行経路、ODDユースケース、Pilot.Auto性能仕様、シミュレータ実施結果を使用します。それでは、ここからODDアセスメントの実施方法をご説明します。

ODDアセスメントの実施方法
  1. コースの細分化
    下図のように、コースをODDカテゴリに分割してロードセクション番号を付加し、ロードセクション単位で評価を実施していきます。

    f:id:yst4:20210323160102p:plain

  2. 各ロードセクションごとにユースケース定義を割当
    次に、各ロードセクションにおけるユースケース分析を行います。例えば、下図のように大枠として発車、同一車線内走行、右折、左折などとし、そこから同一車線内走行-通常走行(対向車線なし)や、左折-T字路交差点(信号なし)などのユースケースに当てはめていきます。

    f:id:yst4:20210324104932p:plain

  3. シナリオによる検証確認
    ロードセクションごとにユースケース定義を割り当てたら、ロードセクションのユースケースにおけるシナリオのシミュレーション・実車での検証結果の確認を行います。
  4. 各ロードセクションの評価
    そして各ロードセクションの評価をしていきます。下図のように、これまで行ってきたシナリオ総数とそのシナリオを検証した結果のNG数の割合である「性能限界率」を算出します。さらにそれだけでなく、「ODD範囲(=人的介入が必要なのか、もしくは誘導員を配置するなど運用面の対策やそもそもソフトウェアの調整が必要なのかといったもの)」、「限界要因(=性能限界に達している要因が自車両によるものなのか、もしくはカットインしてきた車両や工事中の場所を走行する場合など、外部のものなのかといった要因をより細かく見ていくもの)」をそれぞれ4段階に分けた点数付けを行い、合計点数を算出します。

    f:id:yst4:20210323162643p:plain
    合計点数を算出したら、下表にしたがってリスク評価を行います。点数が高ければよりリスクが高いことを示しており、運行計画の変更を検討していきます。

    f:id:yst4:20210323172456p:plain

  5. 評価結果からリスク軽減策の検討・実施

    最後に、4.で出てきた結果から安全余裕を確保するよう自動運転計画を修正し、リスクの軽減を図ります。リスク軽減策には、例えば車速の低減やより車間距離を取るなど、性能限界内での運用可能な手段を講じる場合もあれば、オーバーライド(=自動運転から手動運転に切り替えること)により、ルートの一部は必ず手動運転とすることで安全を担保するといった対策があります。下図がその一例となります。

    f:id:yst4:20210324104424p:plain

このような一連の流れを経て、ODDアセスメント行っています。リスク軽減策を実施した場合には、改めて評価を行うことでより安全に実証実験が進められるようにしています。

走行ルートサーベイとは?

次に、走行ルートサーベイについてご紹介します。既にご紹介したODDアセスメントと似たようなものではありますが、アプローチの仕方でいくつか異なる点が存在します。異なる2点を中心に走行ルートサーベイについてお話します。

走行環境条件の設定のパターン化参照モデルの活用

これは、「自動走行に係る官民協議会」で日本経済再生総合事務局が策定したもので、「無人自動運転移動サービスの導入を検討している企業・団体等向けに、導入の検討段階において参考となる導入地域の環境や条件についてパターンを整理した」*2ものになっています。無人自動運転移動サービスの実施に向けて「共通言語」として使用することで、より導入の検討が進むことを意図しています。

f:id:yst4:20210322172341p:plain

出典:https://www.kantei.go.jp/jp/singi/keizaisaisei/jidousoukou/index.html

このモデルを参照し、順番に実証実験実施予定地域の走行ルートサーベイを行います。例えば、1番目の時間については、実施予定地域・場所の日の出・日の入り時刻を調べることで、実証実験を安全に行うための開始・終了時間設定の参考にします。また8番目の交通量については、自治体から情報提供いただき、その道路の交通量を可視化(例:〇〇台/24時間など)することで、安全に実証実験を進めていくルートを策定する参考にします。自動運転の技術力を上げていくため、時にはチャレンジングなルートを策定することもありますが、先ほどご紹介したODDアセスメントと合わせて確認していくことで、チャレンジングでありながらも、安全を最優先して実証実験を行っています。

保険会社の事故データの活用

さらに、保険会社の事故データを活用して「事故多発地点」と「うっかり運転地点」を本サーベイの中に含めています。「過去5年間で3回以上事故が起こった地点」を「事故多発地点」、「直近半年間で一時不停止、踏切不停止、速度超過、一方通行違反が起こった地点」を「うっかり運転地点」と定義して「リスクの高い地点」を割り出しています。ご参考までに、「うっかり運転地点」の例を下図に示しています。一時不停止や一方通行違反があることが見て取れます。

f:id:yst4:20210324103510p:plain

実際に起こった事故だけでなく、「うっかり運転地点」まで加えている理由ですが、「ハインリッヒの法則」では「1つの重大事故の背後には29の軽微な事故があり、その背景には300の異常(ヒヤリ・ハット)が存在する」*3と言われており、ヒヤリハットをきちんと分析し、対策を立てることが安全を担保するのに重要であると言われています。これを今回の「うっかり運転地点」に置き換えると、「うっかり運転地点」が発生している場所は「重大事故が起こる可能性がある」ことを示していると言えるので、これらの情報も本サーベイに活用しています。なお、事故データの活用にあたり、ティアフォーがともに「自動運転の民主化」を実現するために連携している損害保険ジャパン株式会社様から提供いただいています。このように、ティアフォーだけでは提供出来ない情報を含めてご案内することで、より安全に対する優先度が高いことをおわかりいただけるのではないでしょうか。

まとめ

今回は、「安全」というキーワードをもとに直近のデータや国の動きをお話し、安全を実現するための「リスクアセスメント」についてご紹介しました。ティアフォーでは、安全を担保しながら自動運転の社会受容性を高めていけるよう引き続き開発を進めていきます。

Autowareにおける狭路走行のための走行経路計画

こんにちは。ティアフォーで自動運転システムを開発している渡邉です。
今回は、Autowareで使われている車両の走行経路の計画について簡単にご紹介したいと思います。

なお、ティアフォーでは「自動運転の民主化」をともに実現していくエンジニアを募集しております。今回ご紹介するような計画・制御に関わる分野だけでなく、様々なバックグラウンドをお持ちの方と開発を進めていく必要があります。下記ページから募集職種のリストをご覧いただき、興味を持った方はぜひお気軽にご連絡ください!

tier4.jp

走行経路計画とは

走行経路計画とは、読んで字のごとく、車両が走行してほしい経路を計画することです。走行経路を計画するときには、道路の形状や道路上の障害物といったものを考慮しながら、車両が実際に走行できるように目標経路を計画する必要があります。走行経路の計画で必要な要件としては、次のようなものが考えられます。

  • 走行可能なエリアからはみ出さないこと
  • 障害物に衝突しないこと
  • 必要以上の蛇行といった不自然な挙動をしないこと

人間が運転している場合、無意識のうちに走行したい経路を考えながらステアを切るといった操作をすると思います。しかし、自動運転では走行する経路を明示的に算出する必要があり、どのような曲線を描けば車両は安全に走行できるのか、を考える必要があります。また、車両が通れるかどうかの狭い道を通るようなシーンでは、車両の挙動を考えながら通過できる経路を考える必要があります。このようなシーンは人間でも難しい場合があると思います。

今回は、狭い道を通るシーンを例に取り上げながら、Autowareの走行経路の計画方法についてご紹介したいと思います。

狭路走行の難しさ

狭い道を走行するシーンでは、単純に道路の中央を走行するような経路を計画すれば良いわけではありません。なぜならば、車両の大きさによってはうまく曲がりきれずに走行可能なエリアをはみ出してしまうからです。狭い道を走行するために考慮しなければならないことは、大きく分けて

  • 道路の道幅、走行可能なエリア
  • 車両の形状
  • 車両の運動

の3つになります。道路や車両の形状を考慮する必要があることは明らかだと思いますが、走行経路を決める上では車両の運動の特性も考える必要があります。例えば一般的な4輪の自動車の場合、その場で旋回して姿勢を変えるといった運動はできません。自動車の向きを変えたいときにはステアを切って前後に走行することで徐々に自動車の向きを変える必要があります。このように、一般に自動車は空間を自由に動くことはできず、自動車が動くことが可能な範囲は限定されています。そのため、自動車の運動の制限を考慮しながら走行経路の計画を行う必要があります。

車両運動を考慮した走行経路計画

車両運動を考慮した走行計画を考えるためには、車両の運動がどのように記述されるかを事前に求めておく必要があります。Autowareでは、ある速度で走行しながらステアを切ったときに、そのステア角に対する車両の位置・姿勢の変化量を数式ベースで取り扱っています。このように、ある入力に対する車両挙動の変化を表す数式を車両運動の数理モデルと呼びます。車両運動の数理モデルを用いると、与えられたステア角に対して車両が走行する経路をシミュレートすることができるようになります。これにより、車両が走行可能な範囲を考慮することができるようになります。

走行経路を計画するとき、多くの場合通過可能なパターンは多数存在します。走行経路をひとつ決めるためには、何かの評価基準を与えて一番良いものを選ぶということを考えます。今回は、必要以上の蛇行や不自然な挙動を抑えたいため、ステアの角度や角速度、角加速度といったものをできる限り小さくするような走行経路を選択すると良さそうです。

ここまでをまとめると、走行経路を計画するための目標と条件は次のようになります。 

目標:

  1. ステアを切る角度・角速度・角加速度をできる限り小さくする
  2. 道路の中央からできる限り離れないようにする

条件:

  1. 車両は数理モデルで記述された運動の法則に従って動く
  2. 車両は走行可能なエリアからはみ出さない 

これらの目標や条件が数式として定式化できれば、定量的に走行経路の計画を行うことができるようになります。このように、目標や条件を数式として定式化して何かを計画する問題を数理最適化問題と呼びます。数理最適化問題は工学の分野だけでなく金融・経済など広い分野で登場するもので、その解き方は広く研究・開発されています。数理最適化問題の形で定式化することができれば、適切なソルバを用いることで条件を満たしつつできるだけ目標に近い解を得ることができます。上述した目標1にあるステア角度・角速度・角加速度や目標2にある道路中央からの距離はその数値そのものを扱えば良く、条件1の車両運動については数理モデルとして定式化されています。したがって、走行経路計画を数理最適化問題として扱うためには、条件2の車両が走行可能なエリアからはみ出さないことを数式的に表現する必要があります。

車両形状と走行可能領域

車両が走行可能エリア内にいるかどうかの判定は様々な表現方法がありますが、表現の仕方によっては数理最適化問題の形式が難しくなってしまう場合があります。 そのため、Autowareではできる限り解きやすい形式で走行可能領域に関する条件を与えられるように工夫しています。

Autowareでは、車両の中心線(図1の破線)と道路の中心線(図1の赤線)との距離を計算し、その距離と車両幅の半分の和が道路中心線から道路の端までの距離より小さいときに車両が走行可能エリア内にいると判定します。この判定を車両の前端・中央・後端に対して行うことで、車両全体が走行可能領域の中にいるかどうかを判定しています。

この判定方法を用いることで、走行経路の計画問題を二次計画問題と呼ばれるクラスの数理最適化問題へ帰着することができます。二次計画問題は数理最適化のなかでも頻出の形式で、オープンソースのソルバもあります。Autowareでは二次計画問題を解くためにOSQP*1というソルバを利用しています。

f:id:fwatanabe-t4:20210323154240p:plain

図1:走行可能領域の表現方法

Autowareでの適用例

では、実際に狭い道を走行している例をご紹介します。ここでは、S字カーブを走行するシーンを想定して走行経路を計画してみましょう。計画した結果を図2に示します。図中の赤線が道路中心線、白線が実際に計画された(後方車輪軸中心が通る)走行経路、黄色の四角い枠が白線に沿って走行したときの車両の外形(footprint)になります。まず、カーブ入口で中心に沿って走行すると走行可能エリアを逸脱してしまうため、カーブ内側を走行するような経路が引かれていることがわかります。また、S字カーブ内では車両が走行可能エリアを逸脱しないように計画されているだけでなく、中心線よりやや内側を走行して走行経路の曲率をできるだけ小さくしており、人間が見ても自然な経路が計画されていると思います。この結果から、走行可能エリアの逸脱を避けつつ、必要以上に大きくステアを動かさないような走行経路の計画が達成できていることがわかります。

f:id:fwatanabe-t4:20210326131049p:plain

図2:S字カーブの走行経路計画結果

まとめ

今回は、狭路走行を例に取り上げて走行経路の計画方法についてご紹介しました。Autowareでは、このアルゴリズムの導入により走行できるようになったシーンが増えました。しかし、経路計画にはまだ様々な課題が残っており、引き続き有効な手法の研究および開発が必要だと考えております。経路計画に限らず自動運転には難しい課題が多数ありますが、問題解決したときの喜びは大きいです。自動運転技術の発展に興味のある方は、ぜひ本記事冒頭のリンクからコンタクトしていただければと思います!