TIER IV Tech Blog

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

Scan Matchingのスコア計算へ

復習になりますが、LiDAR点(ここではSourceと呼びます)とMap点(ここではTargetと呼びます)との点と点のScan Matchingをそのまま考えると、その整合度合いを計算するために、O(N) * O(M) [where N = Source点数, M = Target点数]の計算量が必要となります。ここで、Target点群をいくつかの格子に分割後、"格子中の点群の正規分布"、つまり雰囲気のようなものへ変換してしまい、Sourceの各点がその分布上その位置に存在する確率を「スコア値」として考えてしまう発想に至ります。もはや分布となってしまったTargetは(当然分割格子数は実計算の多寡に影響するとはいえ)、定数扱いとなり、計算量はぐっと減少し、O(N)となります。また、雰囲気との比較というのは、局所最適解に陥りやすいマッチング処理において、ロバスト性を高める点にも一役買っています。

このTarget点群の正規分布化(Normal Distribution Transform)を用いた、Scan Matching処理こそがNDT Scan Matchingであり、今日の自動運転の高精度自己位置推定において大活躍しています。

前編において解説した、近傍Voxelの取得によって、Source各点に対するTarget中近傍Voxel(の"雰囲気")が取得されました。本編では、その取得された近傍に対して、Source点がたしかにそこにあるという"たしからしさ" = スコア値計算の部分を見ていきます。

OpenCL accelerated NDT Scan Matcher

前編でもOpenCLを用いた再実装を行いましたが、今回は前編の成果物もまとめ、NDT Scan MatcherをOpenCLで実装し、他のデバイス(もちろんCPUもです！)へ容易にオフロードできるパッケージを作成しました。以降コードリーディングの際にも折に触れて登場します。

github.com

OpenCL版のREADMEはこちらをご覧ください。

前項で述べた通り、NDT Scan Matchingにおいては、その内部の処理は、Source各点に対して、各点並列でスコアを計算していきます。最終的にはそのSource各点のスコア値は合算されますが、この処理は並列化が比較的容易なアルゴリズムとなっており、AutowareではOpenMPによる並列化実装が採用されています。

OpenCLでの並列化も、基本的にはOpenMPと同じ戦法をとっており、さらに幅広いデバイス(Embedded GPU, GPGPU, CPU, Accelerator...)への適用を念頭にしたチューニングがなされています。

NDT Scan Matcher外観

それではソースコードを読みこんでいきますが、前編では省略した、NDT Scan Matcherの外観から読んでいきたいと思います。

NDT Scan Matcherは、LiDARからの点群を受けとった時点から処理がはじまります。NDT Scan Matcherに限ったことではありませんが、Autowareのソースコードリーディングにおいては、まずそのnodeが購読するtopicを知り、そのtopicを処理するcallbackを見つける所が最初の一歩です。

LiDAR点に対しては、void NDTScanMatcher::callbackSensorPoints がまさにそのcallbackにあたります。

さて、いきなり色々な処理があり面くらう所ですが、NDTにおいてその処理の大部分を占めるのは、俗に “align” 処理と呼ばれるものであり、一例として経過時間を以下に添付しますが、実際の所、計算機パフォーマンス的にはalign処理さえ抑えれば良いということがわかります。

align_time: 21.448ms (align処理が占めた時間、大体exe_time - 2.0ms程度)
exe_time: 23.242ms (callbackSensorPoints全体の時間)

そして、そのalign処理を呼び出しているのが ndt_scan_matcher_core.cpp L.371 の箇所です。

上記で登場する ndt_ptr_ という変数は、NDT Scan Matcherの各実装を示しており、node起動時のパラメーターによってどの実装が使用されるか決まります。今回のOpenCL実装は、NormalDistributionTransformOCLクラスによって実装がなされている…かと思いきや、その実装はさらに、別の ndt_ptr_ (class名としてはndt_ocl::NormalDistributionsTransform) のalign関数、実際には親クラス (pcl::Registration) のalign関数を呼び出しており、この中で、その子クラスで実装される関数である、ndt_ocl::NormalDistributionsTransform::computeTransformationを呼び出しています。ここまでの相関関係を図示しておくと以下の通りになりますが、これは実装の形式の話であり、最終的に重要なものは、自己位置計算の実装である、computeTransformationです。

f:id:james-murakami:20210715144909p:plain — クラス/関数相関図

このcomputeTransformation関数の中では、マッチングスコアがよくなる方向へ移動、スコア計算の流れを何度かイテレーションし、スコア(正確にはスコアを元に計算した値)が事前設定した数値条件 (transformation_epsilon_) を満足したら、その時点での変換行列を最終的な結果として終了します。このようにalign処理は、移動とスコア計算のイテレーションという一連の流れのことであるとソースコードからわかります。

これ以降の処理追跡では、重要な部分を動的な観測から"あたり"をつけつつ、行っていきます。

さらに深くへ

ソースコード上では様々な処理が存在しますが、複雑な記述だからといって重要度が高い、あるいは処理負荷が高いといったのはよくある誤謬です。

今回はperfの結果可視化ツールのFlameGraphと、ValgrindのツールであるCallgrindの2つの結果を、ソースコードリーディングの座右に置きたいと思います。

github.com

f:id:james-murakami:20210713195833p:plain — ndt scan matcher FlameGraph

f:id:james-murakami:20210713195924p:plain — ndt scan matcher Call Graph

Call Graphにおいて上から3番目の緑のBOXが callbackSensorPoints であり、先程のコードリーディングで登場しました。ここを起点に下に目を移していきます。最初の枝分かれで、 init の方は 72:1 で呼び出しの頻度が低いため、左に降りていきましょう。すると computeDerivatives があり、これがFlameGraphにも登場している、処理割合が大きい関数です。

最終的に2つの関数にたどりつきます。それは updateDerivatives と radiusSearch でありますが、後者は前編で解説をした部分ですので、ここで updateDerivatives をもう少し掘り下げて見てみましょう。

f:id:james-murakami:20210713200307p:plain — ndt scan matcher Call Graph (detailed)

computeDerivatives は updateDerivatives および computePointDerivatives から成ることがわかります。

以上をまとめると、NDT align処理の主関数は下記のたった3つであり、これにより重点的に見ていく場所が明らかになりました。

radiusSearch
updateDerivatives
computePointDerivatives

computeDerivativesの中では、まずSource各点に対するTarget内の近傍探索、点の雰囲気とのマッチングスコア計算、そしてスコアがよくなる方向を上記の関数を用いて計算しています。

ここからはOpenCL実装の話ですが、上記の通り"Source各点に対する並列化"が容易であるため、1, 2, 3をSource各点に対して呼び出すカーネル関数 computeDerivatives を用意しています。

github.com

なお、Autowareが採用している OpenMP版でも同様の並列化が、omp_parallelを用いて実装されています。

(補足) 地図のVoxel分割と地図KD木構築実装

OpenCL版で大きく異なるのは、むしろ地図のVoxel分割(およびそのKD木の構築)の部分であり、概念としては前編の通りですが、今一度package化に際しての実装を説明しておきます。

OpenCL版NDT Scan Matcherではndt_ocl::NormalDistributionsTransform::setInputTarget関数内のinitから、target_cells_.filter()関数(なお、target_cells_の型は、ndt_ocl::VoxelGridCovariance<PointTarget>) 、さらにPCLの関数を経由した後、最終的に voxel_grid_covariance_ocl_impl のこの箇所で木を構築しています。

OpenMPとの違いとしては、FLANN Libraryを経由せず、そのメモリ配置を voxel_grid_covariance_ocl_impl 内で任意に制御できる点にあります。この点は組み込み環境の制限されたメモリ環境においては必須となります。

そして最終的に作成された木メモリは、SVM (Shared Virtual Memory) argumentとしてkernelに渡され、デバイス側の近傍探索に使用されます。

スコア計算部の"命令実行的性質"

計算量といってしまうと、それはランダウ表記で示される、入力数に対するチューリングマシン上での操作手数の関係(スケーラビリティ)という抽象性を持つものになってしまいますが、ここではより具体的に、特定の命令セット命令を持つ(load/store型)計算機上における、命令の実行性質をみたいと思います。

computePointDerivatives

2つの行列演算といくつかの行列のエレメント生成から成ります。

j_ang * x4 (<8, 4> * <4, 1>)
h_ang * x4 (<16, 4> * <4, 1>)

いずれもかなり小規模な行列であるため、行列演算自体のカーネル化やSIMD packedな演算化は行っていません。実際にカーネルコードを、RISC-Vのコンパイラ(RV32if)でコンパイルした結果における、命令ヒストグラムを見てみます。

大体がflw(FPRメモリロード) → fmadd.s(乗算加算複合) → fsw(FPRメモリストア)の系統であり、またspill outも極端でないため、fsw << flwとなっていることがわかります。ちなみにfmv.w.xはGPR → FPR移動命令だったり、retは(仮想命令)関数リターン(jalr x0)だったりしますが、本筋とは関係ありません。ちなみに乗算加算複合命令はgccでは-O3でないと出現しないようです。

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

updateDerivatives

多数の小規模行列演算と、スカラー値演算、およびexponent演算が使用されているのが特徴的です(スカラー値演算も多数ありますが、以下では割愛しています)。

e_x_cov_x<scalar> = exp(-gauss_d2<scalar> * x_trans4<1, 4>.dot(x_trans4<1, 4> * c_inv4<4, 4>) * 0.5f
c_inv4_x_point_gradient4<4, 6> = c_inv4<4, 4> * point_gradient4<4, 6>
x_trans4_dot_c_inv4_x_point_gradient4<6, 1> = x_trans4<1, 4> * c_inv4_x_point_gradient4<4, 6>
x_trans4_x_c_inv4<1, 4> = x_trans4<1, 4> * c_inv4<4, 4>
point_gradient4_colj_dot_c_inv4_x_point_gradient4_col_i<6, 6> = point_gradient4.transpose()<6, 4> * c_inv4_x_point_gradient4<4, 6>
for i in 0 ... 5

x_trans4_dot_c_inv4_x_ext_point_hessian_4ij.noalias()<6, 1> = x_trans4_x_c_inv4<1, 4> * point_hessian_.block<4, 6>(i * 4, 0)

最終的に、このupdateDerivativesの返却値がscoreとなり、これをinput点群数で割り正規化したものが、Transform Probabilityとして、推定のもっともらしさを表す数値として使用されることになります。

こちらも、RISC-Vコンパイラでのコンパイル結果とその命令分布を見てみましょう。

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

こちらはソースコードでも依存関係のある行列演算であるため、flwやfswよりもかなりfmul.s, fmadd.sの比重が大きいこととちょうど符合しています。

OpenCL実装について

紹介したOpenCL実装とOpenMP実装について、実際に(README記載の)サンプルを動作させて比較してみます。今回は、OpenCLもCPUにoffloadする形となっています。

Align TimeとTransform Probabilityの時間推移

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

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

結論としては、平均的にOpenCLの方がAlign Timeの時間は大きく、分散が小さいです。これは、OpenMPでは処理のロードバランシング等の最適化が行われており、応答性に関してはより優れた結果になっていると思われます。例えば、GPGPUやAccelerator等のCPU外のdeviceにもoffloadすることが可能な点がOpenCL版の魅力ではありますが、ナイーブな実装もあってCPUは少し苦手といった所のようです。

Transform Probabilityとしては同程度の推移です。

最後にFlameGraphを取得してみると、libgomp (OpenMP) libraryの代わりに、intel::internal::arena (Threading Building Block)が使用されていることがわかります。

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

ndt_ocl packageのREADMEでは、Intel環境におけるOpenCL 2.x環境の構築の例も紹介していますので、実行する際にぜひご参照ください。

リソース管理厳格化

OpenCLの実装にあたっては、使用する各種のリソースにも制限を意図的に加えています(いずれも設定が可能です)。

実際問題として車載環境のような組み込みによった環境では、動的で潤沢なメモリ確保ができない場合の方が多いですし、何より実行毎に予想できない挙動であることは、一つのリスクとなります。

重要なことは、平均でも最小でもなく、最大値を予め設定できるということです。

具体的には、NDT Scan Matchingにおいて、その処理量に影響を与える要素は3つしかなく、以下の3要素の最大値を予め定めさえすれば、処理時間を予測できるということになります。

LiDAR点数
発見近傍Voxel数
スコア値計算の最大繰り返し数

OpenMP実装では、3のmax_iterationのみの制限でありますが、OpenCL実装では全てに以下のような制限をかけることができます。

LIMIT_NUM (MAX_NEIGHBOR_VOXELS)
- Radius Searchが発見する近傍Voxcelの限界数、これを超過すると単純に近傍として追加されず、Scan Matchingが不安定となる恐れがあります。
MAX_PCL_INPUT_NUM (MAX_NUM_OF_LIDAR_POINTS)
- LiDARからの入力点群数の最大値を設定する。

おわりに

前編/後編と2回にわたり、NDT Scan Matchingの計算面の調査を行い、その結果を一緒に眺めてきましたが、いかがでしたでしょうか。

途中から解析や、再実装の話に始終してしまいましたが、腰をすえてソースコードを追ってみる時、ただやみくもに読むよりも定量的に姿を捉えながら全体を理解していく、そんな一例をご紹介できたかと思います。

魔法のような自動運転のアルゴリズムも、実装となってしまえばただの計算です。Autowareはオープンなソフトウェアです。実際に読み、触れ、動作させることで理解ができます。臆することなく挑戦していきましょう！

2021-07-08

安全への取り組み：③自動運転車のサイバーセキュリティについて

　こんにちは、ティアフォーのSafety Engineerの須永です。

ティアフォーの安全への取り組みを数回にわたって紹介していくシリーズ。時間がたってしまいましたが今回は第3回目になります。

　1回目は法律・ガイドラインについてでした。

　2回目は安心・安全についてでした。

　3回目の今回は自動車のサイバーセキュリティについての取り組みを説明していこうと思います。

　なお、ティアフォーでは、「自動運転の民主化」をともに実現していくサイバーセキュリティエンジニアを絶賛大募集しています。自動車および自動運転のサイバーセキュリティは最先端、かつ広域にわたる知識が必要になってきます。我こそは、という方が居ましたら、気軽にカジュアル面談からでも対応しますので是非応募ください。

自動車のサイバーセキュリティと安全
自動車へのサイバー攻撃の現状
自動車サイバーセキュリティのルール化
自動車のサイバーセキュリティルールの概要
自動車のサイバーセキュリティ開発プロセス
安全分析・サイバーセキュリティ分析およびリスクアセスメントの差分
おわりに

自動車のサイバーセキュリティと安全

　自動車のサイバーセキュリティと安全にはどのような関係があるのでしょうか？

2015年に行われたBlack Hat USA 2015という大規模なセキュリティカンファレンスで、ホワイトハッカーであるCharlie Miller氏とChris Valasek氏がFiat Chrysler Automobiles（FCA）社のJeep Cherokeeという車両のハッキングに成功したことを報告し、140万台がリコールという話になりました。このハッキングは、はじめはWi-Fi経由でオーディオ音量を調整したりGPSで車両をトラッキングするなど、いわゆる車両内のマルチメディアシステムを掌握していました。さらにハッキングを進めると、マルチメディアシステムとは独立した車両の運転制御用のCANバスにアクセスできるまでになり、結果として外部から車両の運転制御までできるようになりました。つまり、車両を遠隔操作できることが分かったのです。ホワイトハッカーが実験的に行ったハッキングではあったものの、もし悪意のあるハッカーが遠隔操作で車両をコントロールすれば、故意に事故を起こすこともできるわけです。

youtu.be

自動車へのサイバー攻撃の現状

　2010年から2019年の10年間に公開された世界の車両サイバー攻撃インシデントの総件数は把握されている件数のみでも400件余り、2019年は単年で150件相当とのデータがあり、把握できていないものも含めるとその数倍はある可能性があります。このように、車両のサイバー攻撃は急増してきています。(参照：自動車へのサイバー攻撃の新常識と対応の考察)
　サイバー攻撃対象は、キーエントリシステムやテレマティクスや自動車メーカーのサーバ、カーシェアリングや自動車警報サービスなどのモバイルアプリケーションサーバといった自動車窃盗目的が多く、ホワイトハッカーの研究目的の攻撃から、悪意あるハッカーの攻撃に移行しているのは確実なようです。もし悪意のあるハッカーがJeep Cherokeeの事例のように遠隔操作で車両をコントロールすれば、故意に事故を起こすこともできるわけです。自動車のハッキングは最悪人命に関わることから、車両のサイバーセキュリティ侵害は安全侵害の一つになりえるとし、世界中が事態を重大にとらえている状況です。

自動車サイバーセキュリティのルール化

　Jeep Cherokeeのハッキングと時を同じくして、自動車業界はCASE(Connected・Autonomous・Shared & Services・Electric)時代といわれる100年に一度の変革期を迎えました。これらの背景を受けて、自動車業界や自動車のサイバーセキュリティルールを定めようとする国際的な動きが本格化したのです。

　主に国際間での自動車のルールを取り決める国連のワーキンググループWP29(自動車基準調和世界フォーラム)および国際規格を制定するISOでルール化が動きはじめます。これらの活動は国際協調するために整合するのに時間がかかり、WP29では2020年6月にUN-R-155(車両型式・サイバーセキュリティマネジメントシステム)・UN-R156(車両型式・ソフトウェアマネジメントシステム)として、またISOでは2021年の5月にFDIS としてISO/SAE 21434が正式な承認を迎える一歩手前まで来ている状況です。

　日本は自動車のサイバーセキュリティに関して各国を先導するように、WP29では共同議長または副議長を務めてきました。その活動の中で国土交通省は、2020年8月に自動車の特定改造等に関わる許可制度を発表しており対応が必要になっています。

　自動運転もまだ完全に完成されたソフトウェアが存在しないことから、適宜ソフトウェアをアップデートして機能をどんどん追加・改良していく段階です。市販車に搭載された場合でもアップデートしていけるようにOTA(Over-The-Air)が行われることが想定されています。また、自動運転システムになると車両だけでなくそれを取り巻くサブシステムとしてクラウドを利用します。また、先の説明でも行ったようにサイバーセキュリティ侵害がそのまま安全侵害に直結する可能性も出てきています。

　自動運転を見据えた規格では、SaFAD(Safety First for Automated Driving)が標準規格化されたISO/TR 4804や民間規格であるUL 4600もあります。これらの中でも、システムのライフサイクルの中でサイバーセキュリティの確保および適宜見直しを行ってアップデートしていくことが求められています。

自動車のサイバーセキュリティルールの概要

今まで説明してきたルールでは概ね考え方の連携ができており、サイバーセキュリティの守り方は2種類に大別されます。

1.　サイバーセキュリティ・ソフトウェアアップデートを運用できる組織体制

2.　サイバーセキュリティ・ソフトウェアアップデートを安全に行うためのリスクアセスメント

　1の運用できる組織体制とは、いわゆるCSMS(Cyber Security Management System)のことで、サイバーセキュリティに関わるリスクマネジメントを効果的・効率的に実施するための仕組みになります。サイバーセキュリティを完全にすることはできないため、一般的にはスイスチーズモデルのように何層かの対策を打つのが一般的ですが、深刻な脆弱性問題が出た時にすぐに対処できる組織体制が必要、かつ準備しておくことが必要ということになります。

　2のリスクアセスメントとは、人はミスをすることを前提に、設計手法(プロセス)の中でミスを見つけてつぶし、設計の変更があった場合はその影響を見つけて設計に反映させていくものです。この設計プロセスの中でミスを抑える手法は、安全規格であるISO 26262のV字プロセスと同じ手法になるため、V字を使用した開発プロセスという意味で兄弟的存在になります。また、製品の企画段階から廃棄までの製品ライフサイクルの中で活動していく部分に関しては同じく安全規格のISO/PAS 21448 (SOTIF) と兄弟的な存在になってきます。これからの自動車・自動運転車のルール・規格としてはこの3つの規格を総合・統合した形で見ていき、サイバーセキュリティを含んだ安全を確保していくことが業界の暗黙のルールになっています。

f:id:sunagaya:20210708122806p:plain — 自動車・自動運転システムのサイバーセキュリティ規格と安全規格

自動車のサイバーセキュリティ開発プロセス

　安全とサイバーセキュリティは、同じような開発プロセスでルール化されていますが、当然全く同じというわけではないので、その違いについて簡単に説明していきます。

　まず、自動車の安全設計プロセスとして有名なISO 26262は、自動車でも特に車両の電気電子機器の機能安全(壊れても安全・壊れたら分かる)設計を行うもので、V字プロセスとして開発(設計・検証)を進めるものになります。対象のシステムの構成をアイテム定義し、定義したアイテムの潜在的・危険因子を特定するためのハザード分析を行い、壊れたらどのような危害を加えてしまうか危険度や被害度・回避可能性などのリスク評価を行います。そのリスク評価に対して安全目標を立ててリスクを許容可能なレベルまで落とせるように安全要件を作成し、設計・実装していきます。設計・実装したものが意図したとおり、効果あるものか確認・検証していきます。　

　併せて、重要な安全設計プロセスとしてのISO/PAS 21448では、システムとして未知で不安全である事象を減らし、既知で安全である事象を増やすことを続けていきます。これは車両・システムが破棄されるまで継続していき、安全性をどんどん上げていく取り組みになります。

f:id:sunagaya:20210706141601p:plain — 自動車の安全開発プロセス

　同じように、自動車のサイバーセキュリティ設計プロセスについて、ISO/SAE 21434の概要をなぞる形で説明します。この規格は車両の電気電子機器の開発、生産、運用、保守を含めたサイバーセキュリティのリスク管理のためのエンジニアリング要件になっています。対象となるシステムの構成でアイテムおよび守るべき資産を定義し、定義したアイテム・資産のサイバーセキュリティ侵害による影響を脅威分析として実施し、攻撃の経路および影響からリスク評価を行います。そのリスク評価に対してサイバーセキュリティ目標を立てて許容可能なリスクレベルまで落とせるように要件を作成し、設計・実装していきます。設計・実装したものが意図したとおり、効果あるものか確認・検証していきます。さらにサイバーセキュリティポリシーを設定し、その製品に脆弱性があった場合は対策・対応およびインシデントの管理を車両が廃棄されるまで継続していきます。これらのサイバーセキュリティインシデントは、PSIRT(Product Security Incident Response Team)という組織横断的なチームによって管理していきます。

f:id:sunagaya:20210706141636p:plain — 自動車のサイバーセキュリティ開発プロセス

安全分析・サイバーセキュリティ分析およびリスクアセスメントの差分

　開発領域では似たようなプロセスであるものの、安全・サイバーセキュリティ設計では大きく違うところがあります。

1.　サイバーセキュリティでは資産の特定がある

　サイバーセキュリティで侵害があると、影響があるステークホルダーを明らかにし、資産をCIA (秘匿性、完全性、可用性：Confidentiality、Integrity、Availability)の3要素に分類します。サイバーセキュリティ侵害による影響は、無視できるものから深刻なものまで測定され、SFOP(安全性、財務、運用、プライバシー：Safety、Financial、Operational、Privacy)に分類します。安全は万人に影響がありますが、サイバーセキュリティは非常に範囲が広いので守るべき人や物に対して領域の線引きをする必要があるということを示しています。

2.　分析方法

　安全分析ではHARA(Hazard Analysis and Risk Assessment)を実施し安全性の評価をしていきます。この分析にはFTA(Fault Tree Analysis)・FMEA(Failure Mode and Effects Analysis)・FMEDA(Failure Modes, Effects, and Diagnostics Analysis)・HAZOP(HAZard and OPerability studies)・STAMP(Systems-Theoretic Accident Model and Processes) /STPA(STAMP based Process Analysis)などの多くの安全分析手法が使用されます。

　一方、サイバーセキュリティ分析ではTARA(Threat Analysis and Risk Assessment)を実施しサイバーセキュリティ評価をしていきます。ハザードが脅威(Threat)に置き換わったものになります。この分析手法にも様々な種類がありますが、DFD(Data Flow Diagram)によってデータの流れを可視化し、STRIDE法や5W法およびAttack Treeでサイバーセキュリティリスクを洗い出し、CVSS(Common Vulnerability Scoring System)で脆弱性を評価、対策を検討し、要件化を通して設計に反映していきます。

f:id:sunagaya:20210707213933p:plain — 安全分析・リスクアセス・要求プロセス

f:id:sunagaya:20210708123119p:plain — サイバーセキュリティ分析・リスクアセス・要求プロセス

　車両のサイバーセキュリティは一般のコンピュータに比べると分析結果や知見など、まだまだなところがあります。また、自動運転システム自体巨大なシステムになっているのでサイバーセキュリティは安全と同様、膨大な分析と対策検討が必要になってきます。

　ティアフォーでは自動運転システムを搭載したPoC車両を開発しています。車両を改造してのシステムになるので、できる範囲からサイバーセキュリティ分析およびその対策を入れてはいますが、まだまだ発展途上という状況です。

f:id:sunagaya:20210707215845p:plain — サイバーセキュリティ分析抜粋

おわりに

　以上、簡単ではありますが、自動車のサイバーセキュリティに関する内容を説明させて頂きました。簡単に表現しすぎているところもあるのでルールの詳細はご確認ください。自動運転のサイバーセキュリティへの対応はソフトウェアに関する部分だけでなく、ハードウェアやシステム、さらに組織体制まで見る必要があり、安全と同様、もしくはそれ以上に大変なものです。したがって自動運転車での対応はそれだけ必要かつチャレンジングになっています。

　最後に、改めてになりますが、ティアフォーで自動運転システムのサイバーセキュリティ構築にチャレンジしたい方がおりましたら、以下のページよりコンタクトいただければと思います。カジュアルな面談も大歓迎ですので気軽にアクセスください！

2021-06-23

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

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

自動運転の地図
- 点群地図とベクターマップについて
- Lanelet2について
  - Point
  - LineString
  - Polygon
  - Lanelet
  - Area
  - RegulatoryElement
Vector Map Builderの紹介
まとめ

自動運転の地図

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

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

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

https://tier4.github.io/scenario_simulator_v2/tier4.github.io

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

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の３つ）。

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

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

2021-06-17

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

はじめに

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

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

はじめに
Autoware開発におけるSimulation Test
ティアフォーのシナリオテストフレームワークのアーキテクチャ
- アーキテクチャ全体の紹介
- 開発時に注力した要素
今後の開発計画
- NPCロジックの更なる性能向上
- 公道走行ログからのシナリオ自動起こし

Autoware開発におけるSimulation Test

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

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

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ごとに異なる

完全に等価なシナリオを人力でポーティングできたとしても、それを処理する処理系が異なるのであれば処理結果は異なってしまいます。
具体的な例として、「車の前に自転車が飛び出す」という状況を考えてみましょう。

これをシナリオに起こす時、以下のような処理が記述されることになります。

ある地点から N(m) 以内に自車両が入った時
自転車の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で統一されているため非常に使いやすいツール群となっています。

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

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

本シナリオテストフレームワークの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として実装されています。

ティアフォーのシナリオテストフレームワークは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と接続を行うことができます。

開発時に注力した要素

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

レーン座標系のサポート

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

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パッケージ群であるシナリオテストフレームワークはなにか致命的な問題が見つかったからといって簡単に大規模なリファクタや修正をすることはできませんし、機能追加やバグフィックスによってデグレが発生しうることは容易に想像できます。

そこで、シナリオテストフレームワークの開発においては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に常に最新版ドキュメントがデプロイされるようになっています。
これにより簡単に最新版のドキュメントにアクセスすることができます。

今後の開発計画

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

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

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

www.youtube.com
arxiv.org

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

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

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

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

2021-05-06

読み解く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のいずれかの軸で空間を分割します。pointlistを qsort でソートし、その中央を分割点としてノードを構築する部分は、前節で説明した通りです。ソートしたpointlistのうち、分割点よりもindexが小さい点群を左の子ノード(child1)に渡し、分割点よりもindexの大きい点群が右の子ノード(child2)に渡されます。また、node_indexesには点群のインデックスとノードのインデックスを対応させるのに必要な情報を格納します。

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

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

ぜひティアフォーへ！

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

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

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

2021-04-28

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

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

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

実証実験概要

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

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

概要

本実証実験では、仕様が異なる2台の小型自動搬送ロボット「Logiee」（以下、Logiee）で、下記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輪のタイヤエアー圧のバランスが異なると、タイヤ中心（回転軸）から接地面（地面）までの距離（タイヤ径）が変化し、直進指示を与えているにも関わらず、エアー圧が低いとタイヤ径が小さくなり、その結果、エアー圧が低い方へ曲がっていく挙動を示します。

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

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