eBPFやLD_PRELOADを利用した共有ライブラリの関数フック

こんにちは、ティアフォーでパートタイムエンジニアをしている石川です。

本記事では、楽に「動的ライブラリ(及び実行バイナリ)の特定の関数をフックして何かしらの処理をする」手法について紹介していきます。

この記事は、同じくパートタイムエンジニアの西村さんによる作業の成果を元にして、石川が執筆したものです。ソースコードや図のいくつかも西村さんによる貢献です。

また、ティアフォーでは「自動運転の民主化」をともに実現していく、学生パートタイムエンジニアを常時募集しています。自動運転を実現するためには、Softwareに関してはOSからMiddlewareそしてApplicationに至るまで、Hardwareに関してはSensorからECUそして車両に至るまで異なるスキルを持つ様々な人々が不可欠です。もしご興味があれば以下のページからコンタクトいただければと思います。 tier4.jp

この記事の主題

この記事の主題は、楽に「動的ライブラリ(及び実行バイナリ)の特定の関数をフックして何かしらの処理をする」( ≒ ソースコードを改変したりビルドしなおしたりしない) ことです。

ティアフォーを中心として開発されているAutowareは、ROS (Robot Operating System) という通信ミドルウェアに依存しています。今回はAutowareの性能解析という業務を進める上で、ROS1内部の特定区間の処理時間(実時間)を計測したくなり、上記目的を達成する手段を用意することになりました。

手段としては、eBPF(extended Berkeley Packet Filter) を利用した方法と、LD_PRELOADを利用した方法の2種類を紹介します。

eBPFを利用した方法

まず、eBPFとは何かについて説明します。 eBPFとは、Linux kernel内部で実行される独自の命令セットを持つ仮想マシンにおいて、ユーザ空間から実行プログラムを送り込み、指定したevent (下図におけるkprobes, uprobes, tracepoints, perf_events に対応)にフックするなどして実行することができる機能のことです。

eBPFのユーザは、BPF bytecodeをbpf(2)によってkernelに送りこみ、安全なコードであるかのverifyとコンパイル/実行をkernelが行います。eBPFのユーザがBPF bytecodeを直接書くのは稀で、通常はC言語likeな高級言語で記述することができます。eBPFのユーザプログラムとのデータのやりとりは、“maps” と呼ばれるデータ構造を通して容易に行うようになっています。

f:id:sykwer:20210303121516p:plain
eBPF internal (http://www.brendangregg.com/ebpf.html より引用)

今回は、「動的ライブラリ(及び実行バイナリ)の特定の関数をフックして何かしらの処理をする」ことを実現したかったので、フックイベントとしてuprobesを使用します。uprobesとは、eBPFとは独立した概念であり、ユーザアプリケーションにbreakpointを仕込んで、kernelでハンドリングするための仕組みです。

uprobesにフックしてeBPFのコードを実行するためのAPIなどについては、iovisor/bccリポジトリにおいてある bcc Reference Guide を見ればよいです。今回は、ユーザコードの関数の開始地点にフックを仕込む attach_uprobe() を利用します。

例として、ROS1内部の ros::Publication::publish(ros::SerializedMessage const&) をフックして文字列をprintするだけのコードを示します。

from __future__ import print_function
from bcc import BPF

libroscpp_path = "/path/to/libroscpp.so"

bpf_text = """
#include <uapi/linux/ptrace.h>
#include <linux/sched.h>

int publish(struct pt_regs *ctx) {
    uint64_t pid = bpf_get_current_pid_tgid();
    bpf_trace_printk("publish() called: msg addr - %p :pid/tid %d", (void *)ctx->di, pid);
    return 0;
}
"""

b = BPF(text=bpf_text)
b.attach_uprobe(name=libroscpp_path,
  sym="_ZN3ros11Publication7publishERNS_17SerializedMessageE",
  fn_name="publish")

while 1:
    try:
        (task, pid, cpu, flags, ts, msg) = b.trace_fields()
    except ValueError:
        continue
    print("%s" % msg)

以上のスクリプトを実行することにより、libroscpp.so を動的リンクして実行されている全てのROS1プログラムにおける、指定した関数をフックすることができます。

ところで、libroscpp.soはC++によって書かれたライブラリなので、関数名がマングリングされており (上のコードにおけるattach_uprobe()の第二引数に渡している “_ZN3ros11Publication7publishERNS_17SerializedMessageE”)、面倒なことにビルド済みのバイナリからマングリングされた関数名を調べなくてはなりません。

# Get offset address of the specified function
$ nm -D  /path/to/binary_file -t | c++filt | grep <function_name>
# Get the mangled function name
$ nm -D /path/to/binary_file | grep <offset>

さて、このようにすることで実行バイナリや動的リンクされるsoファイルに変更を加えることなく、Pythonスクリプトを実行するのみでユーザ空間の関数をフックすることに成功しましたが、計測のオーバーヘッドの面でデメリットがあります。

関数にフックしてeBPFのコードを実行する際には、毎回ユーザ空間からカーネル空間へのコンテキストスイッチが生じるため、高い頻度で呼ばれる関数のフックに対して無視できないオーバーヘッドが生じてしまうという点です(eBPFはkprobesに対するフックやkernel eventなどの、kernel空間で発生するイベントに対するフックに適している選択肢と言えます)。

これを解決するため、次はユーザ空間のみで完結するLD_PRELOADを用いた方法を検討します。

LD_PRELOADを利用した方法

LD_PRELOADとは、/lib/ld-linux.so.2 の動作を制御する環境変数の一つであり、環境変数LD_PRELOADに共有オブジェクトを指定して実行することによって先にLD_PRELOADで指定した共有オブジェクトがリンクされます。

したがって、その共有オブジェクトに同名の関数を定義しておくことによって、元の共有ライブラリに定義されている関数の代わりにLD_PRELOADで指定した共有オブジェクトの関数を実行させることが可能になります。

さて、今回実現したいのは「特定の関数の開始時にフックして、ある処理を挟む」ことなので、LD_PRELOADで置き換えた関数の実行後に元の関数に処理を戻します。これを実現する方法はいくつかありますが、一番簡単な方法はRTLD_NEXTハンドルを引数に指定して、dlsym(3)を呼びだすことです (LD_PRELOADで置き換えた後の関数内でその関数を呼び出しても、置き換えた後の関数を再帰的に呼びだすだけなので無限ループしてしまいます)。

RTLD_NEXTハンドルとはGNU拡張による特殊なハンドルであり、現在の共有オブジェクトの次の共有オブジェクト以降で発見されるシンボルの値を取ってきます。つまり今回の場合では、LD_PRELOADで置き換える前の本来の共有ライブラリにて定義されている関数のシンボルを取ってくることができます。ちなみにRTLD_NEXTハンドルを利用するためには、GNU拡張を有効にするためにdlfcn.h をincludeする前に #define _GNU_SOURCE する必要があります。

さて、eBPFを使用したときと同様にros::Publication::publish(ros::SerializedMessage const&) が呼び出された際にフックして特定の処理を挟むことをしてみます。以下のコードを共有ライブラリとしてコンパイルし、LD_PRELOADに指定しつつROS1のプログラムを実行します。

#define _GNU_SOURCE
#include<dlfcn.h>
#include<iostream>

using publish_type = bool (*)(void* ,void*);

extern "C" bool _ZN3ros11Publication7publishERNS_17SerializedMessageE(void *p, void *q) {
  void *orig_pub = dlsym(RTLD_NEXT, "_ZN3ros11Publication7publishERNS_17SerializedMessageE");
  std::cout << "call detect: publish 1st arg: " << p << " 2nd arg: " << q << std::endl;
  return ((publish_type)orig_pub)(p,q);
}

マングリングされたそれぞれの関数を再定義し (マングリングされた関数名を調べる方法は前章のとおり)、再定義した関数の内部から dlsym(3)にRTLD_NEXTハンドルを渡して元の関数のシンボルを取得し、その元の関数を呼びだしています。

まとめ

本記事では、楽に「動的ライブラリ(及び実行バイナリ)の特定の関数をフックして何かしらの処理をする」ことを目的とし、eBPFによる方法とLD_PRELOADによる方法を紹介しました。本記事において紹介した手法は、動的ライブラリをリンクして実行するアプリケーション全般(やアプリケーションの実行バイナリ自体)に使用できるものですので、幅広く応用先があると思います。

参照

github.com

ebpf.io

www.brendangregg.com

www.amazon.co.jp

www.amazon.co.jp

Autoware開発におけるrvizの活用ついて 

こんにちは、ティアフォーで自動運転システムを開発している高木です。今回はROS開発の心強い味方、「rviz」についてのちょっとしたお話を紹介していきたいと思います。主に開発者向けのノウハウについて書いていくので実装に踏み込んだ話が多くなりますが、そうでない方も画像だけ見れば概要は伝わると思いますので、こういった機能があることを知っていただき、ぜひ率直な感想をお聞かせください。

なお、ティアフォーでは「自動運転の民主化」をともに実現していく仲間を様々な分野から募集しています。下記ページから募集職種のリストをご覧いただき、興味を持った方はぜひコンタクトをお願いします。

tier4.jp

可視化について

Autowareは多数のプロセスが組み合わさって動作する複雑なシステムであり、最終的な出力となる車両の挙動のみを見て動作状況を把握するのは容易なことではありません。特にAutowareはオープンソースであるため、第三者が作成したプログラムとの連携が前提となります。そのため、それぞれの処理でどのような判断が行われており、どういった出力がなされているかを把握することは開発の効率や品質に大きな影響を与えます。

プログラムの動作を把握する方法としてはログ出力やデバッガなどが汎用的ですが、AutowareはROS上で動作するためrqtやrvizといった強力な周辺ツールを利用することができます。私達人間にとって視覚情報は極めて相性が高く、適切な可視化は、詳細な説明をせずとも直感的に動作を理解させるほどの効果を発揮します。というわけで、今回は可視化を行う際に大活躍するrvizについて語っていこうと思います。

rvizとは

ROSが提供しているデータの可視化を行うツールがrvizで、センサーから取得した映像や点群、各プログラムの出力結果を簡単に表示することができます。ティアフォーのデモ映像でもよく登場していて、例えばこの動画でもrvizを使っています。

www.youtube.com

それでは、ここから先はrvizの機能の細かいところについてお話していきます。

Marker/MarkerArray

恐らくrvizで何らかの表示を行いたいと考えた時、真っ先に思い浮かぶのがこれでしょう。ROS標準のメッセージ型(visualization_msg)の中で定義されており、typeを指定して様々な図形を描画することができます。

poseとpoints

知ってる方にとっては当たり前かもしれませんが、POINTSやLINE_LISTなどpointsを使用する図形ではposeとpointsが併用できます。挙動はpointsの全座標をpose.positionだけ平行移動してから、pose.positionを中心にしてpose.orientationの回転を行います。私はなぜかposeが無視されてpointsだけ使われると勘違いしていたのでpointsの時点で座標変換をしていました。同じ作業に煩わされている方がいるかもしれないのでメモとして残しておきます。

LINE_LISTとLINE_STRIP

Markerで線を描くにはLINE_LISTかLINE_STRIPを使います。LINE_LISTは頂点を2個ごとに使用して独立した直線を描き、LINE_STRIPは頂点を順番に繋いだ連続する直線を描きます。単に頂点の指定方法が違うだけかと思いきや、実は連続した直線を描く場合に微妙に挙動が異なります。下の画像は星形をそれぞれの方法で描いた結果なのですが、右のLINE_STRIPで描いた方は線の幅が均等になっていません。これは線と言いつつも内部的には四角形になっているからで、頂点で折り曲げるときに線の幅が縦に配置されてしまうためです。なので見た目を気にする場合はLINE_LISTを使ったほうが綺麗になります。

LINE_LISTとLINE_STRIPで描いた星形

透過度の罠

まずはこちらの画像をご覧ください。これは手前(Z=+1.0)に緑の三角形を、奥(Z=-1.0)に赤の三角形を配置したときの描画結果です。両方の図形で透明度は1.000に設定しています。

 続いて透過度を0.999にした結果がこちらです。再表示するたびにランダムで左右のどちらかの結果になります。赤の三角形が奥に配置されていて、Z座標は緑の三角形と2.0も離れているのにもかかわらず、手前に赤の三角形が表示されてしまう場合があります。

原因はMarker表示時に透過度に応じて以下のような設定が行われるためで、透過度を0.9998以上に設定した時にしか前後関係が正しく処理されません。

透過度 深度バッファ アルファブレンディング
0.9998以上 有効 (前後関係が正しく表示される) 無効 (上書きする)
0.9998未満 無効 (前後関係が正しく表示されない) 有効 (合成する)

両方有効にすれば良いのではないかと思ったかもしれませんが、そう簡単にはいかない事情があります。深度バッファは描画の順序によらず前後関係を正しく表示する方法であるのに対し、透過度を考慮するには背景色が必要なので奥の物体から処理しなければならないためです。残念なことに順序はMarkerで制御できないので、透過度をどちらが上になっても判別できるように調整するのが最も簡単な対応となるでしょう。

Display Plugin

Displayは主にrviz中央の3D画面に何かを表示する目的で使用します。特殊な用途でなければMarkerで十分表現可能だと思いますが、より高度な処理をしたければ自作のpluginを作成することになります。Markerに似た図形を描画するならogre_helpersにあるこれらのクラスが役立ちます。

  • Arrow
  • Axes
  • BillboardLine
  • Line
  • PointCloud
  • Shape

それでも満足できないという方はOgreという3D描画ライブラリを調べてみてください。ここから先はrvizというよりもOgreの領域になります。詳細は省きますが、大まかには次の手順で自由に図形を描画できるようになり、TRIANGLE_STRIPといったMarkerにはない描画方法の指定も行えるようになります。先ほどのogre_helpers内にあるline.cppの実装が分かりやすいです。

  1. 親となるSceneNodeから新しいSceneNodeを作成する
  2. ManualObjectを作成して新しいSceneNodeに関連付ける
  3. Materialを作成する
  4. Materialを使用してManualObjectの描画関数一式を実行する

 Panel Plugin

Panelはrvizの上下左右にGUIを追加できます。Qtというライブラリで実装されており、純粋なQtアプリケーションと似たような感覚で開発できるでしょう。固有の機能としてrvizが内部で管理しているデータにアクセスできるほか、設定をrviz本体と一緒に管理するための機能が用意されています。

個人的にはrvizと同じウィンドウでまとめて情報を表示できるのがメリットだと思っています。特段rviz固有の機能を使うわけではないのですが、使用頻度の高いデータを表示しておけば別途端末を立ち上げてrostopic echoする手間が省けます(rqtにもtopicを表示するpluginがありますが、自作する場合は最小限の情報を適切に配置できるのが強み)。このような感じでQGridLayoutとQLabelを組み合わせ、簡単に情報表示を行うpluginを作ってみたのですが、これだけでもかなり印象が変わってくるのではないでしょうか。

f:id:isamu-takagi:20210226030837p:plain

特に情報が常に視界に入っているというのが大きいです。能動的にデータを見に行く場合、気にすべきデータがどれなのかを把握し、適切なタイミングで確認しなければなりません。意外とこの作業の負担は大きく、確認し忘れなどヒューマンエラーが起こる頻度も高いです。しかし、このような表示があれば「何かエラーの部分が赤くなっているんだけど大丈夫なのだろうか」といった具合で疑問を抱かせ、迅速な対応を取ることが可能となります。

また、現地で実験を行っているスタッフを開発者が遠隔からサポートする際も役立ち、確認用のコマンドを細かく指示せずとも画面の表示を読み上げてもらうだけで大まかに状況を理解することができます。実際、スクリーンショットを一枚送ってもらうだけで問題を解決できたこともありました。今後は自動運転のサービス化を目指して車両の台数を増やしていかなければなりませんから、現地で活動するスタッフの規模も拡大していくと予想されます。そのとき、個人のスキルレベルに大きく依存せず、技術者と容易にコミュニケーションがとれるかどうかは、プログラムの重要な要素となっているはずです。

Tool Plugin

Toolはrviz上部に配置され、主に中央の3D画面を使った操作を作成できます。その性質上マウスとキーボードのイベントを処理するためのコールバック関数が提供されていて、素晴らしいことにマウスの座標と任意の平面の交点を計算する関数(getPointOnPlaneFromWindowXY)まで用意されています。もちろん、rvizにはInteractive Markerが用意されているので基本的にこちらを使えば良いのですが、例えば点群を任意の場所に出して障害物検知のテストを行いたい場合に便利だったりします。 

本来の使い方ではないような気もしますが(Tool内で処理を完結させた方が良いと思っています)、画面上でクリックされた座標をtopicとしてpublishするToolを作って利用しています。マウスがクリックされた時だけ処理を行ったり、ドラッグの始点と終点を取得したりと大抵は同じような操作が欲しくなるので該当部分をToolで実装しておき、利用したいプログラムでは結果をsubscribeすればお手軽にrvizとの連携が可能となるわけです。あくまでデバッグの範囲に限りますが、作っておくとそこそこ便利に使えます。

youtu.be

この動画では歩行者に見立てた点群をマウスで動かせるようにしてデバッグをしています。任意のタイミングで急に飛び出したり、車両に合わせて速度を調整するといった動きを用意するのはなかなか難しいのですが、マウスで歩行者の位置を操作すれば複雑なケースも自由自在にテストできます。突き詰めていくとやはり専用のツールには敵いませんが、基本的にrvizにはデバッグ用の情報を色々と出しているはずですので、同じ画面から空間を共有して操作できるというメリットは非常に大きいです。

まとめ

普段の開発では可視化まわりの処理にそこまで力を入れることはないと思います。しかし、ちょっとした工夫で効率が大きく変わったり、気づきにくいミスを防止できたりと大きな可能性を秘めている部分なので、余裕があればひと手間かけてみると良いかもしれません。

Motion Planning Under Various Constraints - Autonomous Parking Case

Introduction to the Motion Planning

Autonomous vehicles (robots, agents) are requested to move around in an environment, executing complex, safe maneuvers without human interference and to make thousands of decisions in a split second. How can we endow autonomous vehicles with these properties?  

By the way, at Tier Ⅳ, we are looking for various engineers and researchers to ensure the safety and security of autonomous driving and to realise sharing technology for safe intelligent vehicles. Anyone who is interested in joining us, please contact us from the web page below. 

tier4.jp

Then, the key to answering this question is “Motion Planning”. Autonomous vehicles should be able to quickly generate motion plans and choose the best among many or the optimal plan defined by some optimization criteria. 

Motion planning modules for autonomous driving might serve for three different objectives [1]:

  • Producing a trajectory to track,
  • Producing a path to follow,
  • Producing a path and trajectory with a definite final configuration. 

In a trajectory tracking task, agents execute time-stamped commands to satisfy the spatial coordinates with the prescribed velocities. In the path tracking, only positions are tracked. 


Vehicle motion is formally defined using a couple of differential equations. These differential equations propagate the state variables such as velocities (Longitudinal and lateral velocities; Vx, Vy and Heading Rate ) in the chosen coordinate system. The resultant integral curves define the configuration space of the robots (see Figure 1.). If we are controlling robots by velocities, the motion equations are called kinematic model equations.

 

f:id:boyali:20210222170034p:plain

Figure 1. Configuration space (x, y and heading rate) for a car-like robot motion

The most difficult task in the objectives list is the last one: to generate a trajectory or a path while satisfying the endpoint configuration. We are familiar with this kind of planning tasks from our daily life. You can probably remember how frustrating it was to park a vehicle within a parking slot the first time and execute a couple of maneuvers without hitting the cars or walls around (Figure 2).

 

f:id:boyali:20210222170443p:plain

Figure 2. Parallel parking

 Why is planning with the endpoint configuration a difficult task even for cases without any obstacle in the environment?

When considering the parking maneuver, notice that a car can only move forward-backward directions. If we also turn the steering wheel, the vehicle follows a circular path. Cars cannot move in their sideway direction as it is impossible due to the fixed rear wheel orientation. On the other hand, while executing a parking maneuver, we need to be able to move the car in the sideway directions to properly park the vehicle. That is why we drive the car, backward-forward and steer to move the car’s position in parallel to the starting position (Figure 3).

 

f:id:boyali:20210222170651p:plain

Figure 3. To move the car in the sideway direction parallel to the starting configuration, we need at least two maneuvers.

 

In physics, the prohibited sideway directions mean zero velocity in these directions. Although there are no environmental obstacles or physical constraints in these directions, the velocity is constrained to be zero. We can not integrate these velocities to convert them to path constraints which are relatively simpler to deal with. These velocity constraints are called "non-holonomic" motion constraints.

Since we have three motion direction (x, y and heading angle) and two controls (Speed V and steering δ), these kinds of systems are called under-actuated. Non-holonomic systems are in general under-actuated. Finding a path given two configuration positions respecting the velocity constraint is difficult due to the missing control direction.

Almost five decades of research effort has been put into finding a solution for non-holonomic motion planning. There has been no general solution proposed so far to the problem. Although it is possible to move a car position in parallel with a couple of maneuvers, such as zigzagging (Figure 3), the resultant paths and trajectories are discontinuous and generally have some cusp and breakpoints.  This makes the solutions more difficult to find smooth trajectories mathematically. The nonholonomic motion planning problem becomes even more difficult when there are constraints in the environment (such as parking a vehicle in between the other vehicles).  What is more, the motion equations are usually nonlinear and nonlinear differential equations do not have a direct solution except in a few cases.

In the robotics literature, approximate solutions have been proposed. These solutions are based on the notion of local controllability and the geometry of the configuration space. In principle, a global motion between two endpoints is divided into small motion trajectories. The small motion intervals are then connected using various algorithms.

When there are no obstacles in the environment, we can use Reed & Shepp Curves (R&S Curves) to generate a direct path given two endpoints [2]. The R&S curves fit a path for the given endpoint configurations using some motion primitives; Straight (Backward-Forward), Left - Right Turns (Forward-Backward). 

 

A couple of examples of R&S curves are shown in the following figures [3]. 

f:id:boyali:20210222182247p:plain

f:id:boyali:20210222182307p:plain

Figure 4. Reeds and Shepp curves for two different endpoint conditions.

Even though the R&S curves produce time-optimal and pleasing curves, they cannot provide a solution when there are environmental constraints. Their use is limited to unconstrained cases. 

The most recent literature focuses on two-staged nonlinear optimization-based solutions for constrained environments.   Nonlinear Optimization (Nonlinear Programming techniques - NLP) can deal with nonlinear motion equations and constraints. However, the nonlinear optimization methods require a good feasible solution to start with. This difficulty is what diversifies the proposals in the literature. We can list these methods under four categories. These are;

  • Optimization-based (bi-level or multi-staged optimization)
  • Heuristic algorithms (particle swarm optimization, genetic algorithm)
  • Sampling-based algorithms (RRT, Hybrid-A*) and, 
  • Machine learning. 

The proposed algorithms have advantages and disadvantages relative to each other. The common theme of these algorithms is having a two-stage solution that contributes to the implementation complexity.

At Tier IV, we have proposed a new algorithm for autonomous vehicle parking maneuvers inspired by an optimization method trending in the aerospace industry for designing spacecraft trajectories. The Successive Convexification Algorithm (SCvx) is capable of returning a feasible trajectory under a wide range of initial conditions, removing the need for a two stage approach, and has been proven to converge to the solution of the original nonlinear problem without compromising accuracy. In the next section, we explain what kind of problems we can encounter in the numerical solution of optimization problems, and how the successive convexification methods offer a solution to constrained nonholonomic motion planning.

Solving Nonlinear Problems by Successive Convexification Algorithms

Apart from a very demanding feasible initial solution in nonlinear programs, a design engineer can experience infeasible solution error messages raised by the solvers used in the numerical problem although the original problem is feasible. This kind of infeasibility is called artificial infeasibility. Solvers in numerical optimization have a mechanism to check beforehand whether the problem is feasible or not. The cause of infeasibility can be attributed to the results of implicit or explicit unrealistic linearization. 

Furthermore, linear functions might not have a lower bound in the search space. This could be another reason for the solvers to halt. The lack of a lower bound in a minimization problem is called “artificial unboundedness” [see 3, 4, 5 and references therein]. 

The SCvx algorithm is a kind of sequential quadratic optimization. In the sequential solutions, the nonlinear motion and constraint equations are linearized starting from an initial trajectory. Almost in all mathematical problems, one of the best strategies for the solution is to convert complex problems into a series of linear problems.

The solution of a linear problem yields a feasible trajectory which is then used as an initial trajectory for the next solution (Figure 5).

f:id:boyali:20210222182708p:plain

Figure 5. A nonlinear trajectory can be approximated by linear sub-problems which can be solved iteratively.

The SCvx algorithm operates in the same manner with two additional mechanisms to prevent artificial infeasibility and artificial unboundedness error messages. 

The artificial unboundedness problem is addressed by introducing a trust region that restricts the search space (Figure 6). The radius of the thrust region is adjusted by checking if our linearization reasonably approximates the nonlinear cost. If the linearization approximates the nonlinearity well enough, the radius of the trust region is increased, and vice versa.

 

f:id:boyali:20210222182925p:plain

Figure 6. Trust regions are used to restrict the search space of the optimization algorithm. The minimum or maximum is sought within the circles specified by the trust region radius.

The artificial infeasibility problem is addressed by adding a virtual force pushing all the infeasible states into a feasible region. However, the amount of virtual force we apply is penalized in the cost function with a very high weighting factor (i.e 1e5). One of the objectives is to make the contribution of the virtual force to the cost zero in the optimization. 

These two additional mechanisms to prevent artificial infeasibility and unboundedness prevent the solvers from stopping due to the numerical problems and allow the solvers to seek the solution. The SCvx algorithms have been proved to converge to the original nonlinear solution. When a solution of the linear counterpart of a nonlinear problem is equivalent, the solution is called lossless (no feasible region is removed from the problem, [5]).

How Does the Successive Convexification Algorithm (SCvx) Help in Solving the Autonomous Parking (Nonholonomic Motion Planning) Problem?

So far, we alluded that the SCvx algorithm could give us a solution for a nonlinear optimization problem by solving sequential linear sub-problems. We also emphasised a virtual force that pushes infeasible states into the feasible state-space regions, and later on, this force is driven to zero. These two properties eliminate the need for a feasible initial trajectory, therefore reducing the two-stage solutions into one. The SCvx algorithm does not require an initial feasible trajectory. This is a highly desirable property since it is generally a hard problem to come with an initial trajectory in nonlinear optimization problems.  

Although we are now closer to the solution for an autonomous parking problem, we need to overcome the other continuity issues afflicted.  When we have a look at the R&S curves, point-to-point trajectories might require forward-backward - full stop motion. The vehicle moves a bit forward, stops fully, makes a full steering turn and moves back. There are both input and output discontinuities. The solvers and optimization algorithms operate on the continuity assumption. The second issue is the discontinuities in the environmental constraints. 

Inspired by the shape of R&S curves, we divide a whole parking maneuver into a minimum of three curve sections. Most of the trajectories that the R&S method produces consists of a maximum of four or five connected curves. Dividing a whole trajectory into three sections captures most of the solution families. In this proposal, we can imagine three separate vehicles trying to reach each other similar to the situation we see in a flag race (Figure 7).

 

f:id:boyali:20210222183507p:plain

Figure 7. Three separate curve sections on which each vehicle travels.

 

The three separate-curve-approach  can be formulated with boundary conditions for each curve. The start and endpoint of the whole trajectory must be fixed. The curve connections (light blue circles) are the free boundary conditions for each of the curves that are needed to be connected by the optimization algorithm. In this way, we can obtain any arbitrary trajectory with cusps and breakpoints 

A solution to a nonholonomic motion planning problem without environmental constraints using the SCvx algorithm is shown in Figure 8.

 

f:id:boyali:20210222183538p:plain

Figure 8. A trajectory with two cusps solved by SCvx for an unconstrained case. The arrows with red-hats show the direction of the vehicle.

For the second issue; discontinuities in the environmental constraints, we used a state-triggered constraint formulation offered by the same authors of SCvx algorithms [5]. The authors formulate the keep-out zone for the spacecraft landing regions using the state-triggered constraints. The keep-out zone formulation resembles parking situations. In some optimization problems, we need to define the regions where  some constraints could be active or inactive. This requires logical constraints or binary decisions such that, if some conditions occur, then some constraints are activated [if the triggering “condition < 0” ⇒ constraints < 0]. In mathematical optimization theory, these kinds of logical constraints are solved by mixed integer programming which is equally as hard as nonlinear optimization. We show an example of triggering and constraint conditions in Figure 9.

 

f:id:boyali:20210222183628p:plain

Figure 9. State-triggered constraint condition for a parking lot. Here the triggering conditions are xw < - b OR xw > b. When these conditions are triggered, the vehicle’s position value in yw direction must be yw>2 meters.

 

As you may notice, the solutions obtained at each iteration becomes an initial trajectory for the subsequent problem and hints us the regions where the logical constraints become active or inactive.  This allows us to inject discontinuous environmental constraints when the iterated trajectory triggers the condition. 

Using the state-triggers eliminates the use of complicated collision checking algorithms and alleviates the load of the solvers.

Complete Solution for Parking Maneuvers

We applied the strategies we have mentioned to a couple of tight parking scenarios: parallel and perpendicular parking, which are shown in figures 10 and 11 respectively and with an animation in Figure 12. In these figures, we compare the SCvx solutions to the R&S curve solutions that are not aware of any constraints in the environment.

f:id:boyali:20210222183816p:plain

Figure 10. A parallel parking case. The vehicle starts in a random position and direction in the green shaded area and is requested to park in between two obstacles parallel to the X-axis.

f:id:boyali:20210222183835p:plain

Figure 11. A reverse parking case. The vehicle is requested to park perpendicular to the X-axis in between two obstacles.

 

f:id:boyali:20210222183853g:plain

Figure 12. An animation for a reverse parking maneuver.

 

Final Remarks

In this blog article, we summarized one of our publications presented at ITSC 2020; “Autonomous Parking by Successive Convexification and Compound State Triggers”. The detailed algorithms, equations and procedures and the related literature review and references can be found in our paper. We are planning to put the application of SCvx solutions into Autoware.

 

References:
  1. Latombe, J.C., 2012. Robot motion planning (Vol. 124). Springer Science & Business Media.
  2. LaValle, S.M., 2006. Planning algorithms. Cambridge university press.
  3. Boyali, A. and Thompson, S., 2020, September. Autonomous Parking by Successive Convexification and Compound State Triggers. In 2020 IEEE 23rd International Conference on Intelligent Transportation Systems (ITSC) (pp. 1-8). IEEE.
  4. Mao, Y., Szmuk, M. and Açıkmeşe, B., 2016, December. Successive convexification of non-convex optimal control problems and its convergence properties. In 2016 IEEE 55th Conference on Decision and Control (CDC) (pp. 3636-3641). IEEE.
  5. Szmuk, M., 2019. Successive Convexification & High-Performance Feedback Control for Agile Flight (Doctoral dissertation).

http://larsblackmore.com/losslessconvexification.htm

 

大規模非構造データ(Point Cloud)のリアルタイム通信

こんにちは、ティアフォーでフロントエンド開発を担当している田上です。

ティアフォーでは先日ご紹介したSREの信頼性への取り組みの一つとして、少人数でも信頼性が高く、効率的な開発ができるように技術選定会を実施しています。

今回は、最近技術選定会で取り上げた「Point cloud data(PCD)のように大きなサイズのデータを含んだROS TopicをAWS CloudからWebブラウザに転送する」仕組みと技術選定会の様子についてお話したいと思います。

なお、今回ご紹介するようにティアフォーには新しい技術や技術的なチャレンジを良しとする雰囲気、エンジニアの興味や好奇心を満たせる環境があります。ぜひ以下のページから募集職種のリストを見ていただき、興味を持った方は応募をしていただければと思います。

herp.careers

背景

Autowareを使った自動運転車両は車両に搭載したLiDARを用いて、周囲の物体の形状を点群データ(PCD: Point Cloud Data)の形式で取得します。PCDは、自動運転を行う上で非常に重要なデータです。例えば、自動運転車両が地図上のどこを走行しているかの推定(自己位置推定)に用いられたり、障害物の物体認識に用いられたりします。自動運転開発者は、自動運転時に発生した問題の原因を特定するために、PCDが適切に取得されたかどうか、あるいはPCDをもとに障害物が適切に認識されたかどうかを確認します。そのため、PCD自体やPCDをもとに認識した結果を可視化する仕組みは、自動運転開発においてとても重要です。

youtu.be

AutowareはROSをベースに構築されていますが、ROSにはRosbagと呼ばれるROS Topicをファイル(bagファイル)に保存する仕組みがあります。Autowareを使った自動運転車両はPCDや物体の認識結果、その他さまざまな自動運転時に用いたデータをRosbagを用いてbagファイルに保存しています。保存されたbagファイルは自動運転走行後に、クラウドにアップロードされます。自動運転開発者はクラウド上のbagファイルをダウンロードし、手元のPCでbagファイルの中身を可視化して自動運転時のセンサーのデータやAutowareの動作などを確認することができます。

通常、bagファイルの可視化にはRVizというソフトウェアが用いられます。RVizは優れた可視化ツールであり、ROSの開発者にとって一般的なソフトウェアです。ティアフォーでもRVizは当然のように使われていますが、問題点もあります。

RVizを使う上での問題点の一つは、可視化ツールとしてクラウド上の大量のデータを扱うのに適さないという点です。一台の自動運転車両は実証実験の際に一日あたり数百GByteを超えるbagデータを生成し、そのデータをクラウドに保存します。クラウド上のデータストアに存在する大量のデータのうち、どのデータが自分の興味のあるデータか探索したり、大きなサイズのファイルをダウンロードして手元のツールで表示するのは開発者にとってストレスのかかる作業です。

youtu.be

そのような問題に対処するため、現在ティアフォーではウェブブラウザ上でPCDを含んだbagファイルを可視化する仕組み(Webアプリ)を開発中です。可視化ツールをWebアプリにすることで複数のOS上でインストールレスで使用することができます。また車両の運行管理システムやbagファイルの管理システムと連携することで、開発者が可視化したいデータをより容易に見つけられるようになると期待できます。

課題

bagファイルの可視化をWebアプリで実現する際に気をつけるべき点があります。それは表示するデータ(PCDを含んだROS Topic)のサイズが大きく、クラウドからウェブブラウザに対してデータを転送する際にそれなりのネットワーク帯域を必要とすることです。1台のLiDARが取得する点群データの数は数万ポイントに及び、その点群の位置は常に変化します。そのためPCDのサイズは動画像データに匹敵する大きさになります。

ROSにはROS Bridgeと呼ばれる仕組みがあり、WebSocketを用いてROSのTopicをウェブブラウザに送信することができます。ROS Bridgeは便利な仕組みではありますが、WebSocket上でJSONフォーマット(テキスト形式)を用いてROS Topicを転送しているため、PCDのような大きなデータを転送する場合には必要以上に転送量が大きくなってしまいます。

そのため、帯域が制限された環境で使用しづらかったり、クラウドからのデータ転送量にかかる料金が増大することが懸念されます。

そこで、ROS Bridgeではない別のデータ転送方法によって、可視化に必要なデータだけをバイナリ形式でウェブブラウザに転送することを検討しました。
(また、今回は未検討ではありますが、PCDの転送には圧縮技術を適用することも求められると思います。)

実験システムの構築

ROS Bridgeに代わるデータの転送方法としてgRPC-webを使用できないか考えました。

gRPC-webを選んだ理由は

  • バイナリ形式でデータを転送できること
  • 転送するデータの型定義ができること
  • サーバを開発する際に選択できるプログラミング言語が多いこと

などがあげられます。(またバックエンドのサービス間で使われる仕組みをフロントエンドに適用できるのは、単純に面白そうだと思ったのもあります)

実験のためAWS上に構築したシステム構成の概要が下の図です。

f:id:naokit00:20210215103453p:plain

 

クラウド側は構築するのが容易であるという理由でALBとECS(FARGATE)を組み合わせた構成にしました。Fargateのタスクとして二つのコンテナが動作しています。一つはEnvoyで、これはプロトコル変換(gRPC-webとgRPCの変換)を行っています。もう一つはBag serverと呼ばれるサーバで、これはbagファイルのデータをクライアントに転送するgRPCサーバです。

クライアント側はWebブラウザ上にThree.jsを使ったROS Topicの可視化アプリが動作しており、gRPC-webクライアントとしてBag serverと通信します。

実験システムの大まかな動作は次のようなものです。

  • Webブラウザ上の可視化アプリがgRPCのリクエストをALB/Envoyを経由してBag serverに送信する
  • Bag serverはクラウド上のマイクロサービス(図中のAPI Gateway)を呼び出して、S3上のbagファイルのkeyを取得する
  • Bag server はS3上のbagファイル中のROS Topicを読み込んで、可視化に必要なデータをgRPCのServer streaming でWebブラウザに送信する
  • Webブラウザ上の可視化アプリがgRPCのServer streamingのデータを受信して3次元的に可視化する

うまく動くとS3上のbagファイルのデータをWebブラウザ上で動画プレイヤーのような見せ方で表示できるというものです。

実際に試したところ構築した実験システムは期待通り動作することがわかりました。そこでこの仕組みを利用して良いかどうかを社内の技術選定会で問うことにしました。

技術選定会とその準備

ティアフォーでは技術選定会を行うことで、課題解決のためにどの技術を選ぶべきか決定しています。まず、起案者はある課題を解決するための複数の手法について次のことを準備します。

  • あらかじめ分類されたさまざまな観点(開発生産性、技術信頼性、コスト、セキュリティなど)の比較表を作成する
  • 議論すべき重要なトピックをリストアップする

その上で社内の専門性の高いメンバーを集めて議論をすることで妥当な技術を選択する、といったやり方をしています。

今回はROS Bridgeの代替技術として、「gRPC-webを用いた仕組み」と「WebSocketとprotobufを用いた仕組み」の二つについて比較表を作成しました。

また重要なトピックとして次のような項目をリストアップしました。

  • PCDの転送効率
  • 開発生産性
  • クラウド上のシステム構築の容易さ

重要なトピックとして挙げた項目について、gRPC-webは比較対象の技術と比べて概ね同等か、あるいは優位なものもあると私は考えました。

会議はバックエンドエンジニア、フロントエンドエンジニア、SREエンジニアを含む6名程度のメンバーを集めてオンラインで行われました。

1時間に満たない会議ではありましたが参加したメンバーから技術的な懸念や課題が示されて、追加で検討すべき項目がリストアップされました。

技術選定会後のFeedback

技術選定会で示された追加の検討項目は次のようなものでした。いくつかの項目は私が想定していなかったものもあり、技術的な検討を深める助けになりました。

  • gRPC-web自体は未成熟な技術であるから、やっぱり使えないとなる可能性は十分ある。その場合、代替技術に移行できるか?
  • データのアップロードに使用可能か?(Client streaming/Bi-directional streamingが可能か?)
  • 想定される使用規模はどの程度か?ユースケースを明確にするべき
  • 回線品質が悪い時の動作検証が必要。帯域を絞るなどして試すべき
  • 運用コストの見積もりが必要

追加の検討項目のうち帯域制限された環境での動作検証を進めたところ、適切に動作させるためにはなんらかのフローコントロールの仕組みを導入する必要があることがわかりました。実験システムに単純なフローコントロールの仕組みを導入することで、帯域制限された環境下でも通信エラーを起こすことなく動作することが確認されました。

また、運用コスト見積もりの結果、転送量にかかる料金が支配的であることがわかりました。PCDの表示に限って言えば、点群の浮動小数点の精度を落としても問題ない(元データが単精度(4byte)であるのを半精度(2byte)に落としても表示上は問題ない)と考え、転送時にデータの精度を落とすことで転送量を削減するといった試みも行いました。

その他の追加の検討項目についても検討を進めたのち、後日再び技術選定会を開催しました。その会議においても新たに課題や懸念が示されましたが、技術選定としては承認されました。また、社内のシステムで使用されることと、3ヶ月後に技術的な振り返りをすることが決定されました。

まとめ

「Point cloud data(PCD)のように大きなサイズのデータを含んだROS TopicをAWS CloudからWebブラウザに転送する」仕組みについて検討したことと、技術選定会の様子についてお話ししました。

今回示した仕組みは、まずは社内の限定的な用途で使用される予定ですし、今後見直され使われなくなるかもしれません。ただし、検討や技術選定会をスピード感を持って進められたこと(他の業務を行いながら1ヶ月程度でできたこと)や、技術選定のプロセスを経験できたことは良かったと思います。

技術選定の資料作成に多くの時間は取れなかったので、技術選定会のメンバーは少ない情報の中で判断をすることになりましたが、前向きに検討してくれたと思います。

また、参加メンバーは技術的な懸念や課題を示しつつも、新しい技術や技術的なチャレンジを許容する姿勢を示していたと感じました。
今後、今回のような内容をテックブログだけでなく、以下の勉強会でも発表していきますのでこちらもぜひご覧ください。

tier4.connpass.com

LinuxのSCHED_DEADLINEスケジューラについての話

こんにちは、ティアフォーでパートタイムエンジニアをしている佐々木です。

今回はLinuxに搭載されているスケジューラの一つ、SCHED_DEADLINEについて紹介していきたいと思います。自動運転には多数のクリティカルタスクがあり、自動運転の安心・安全をしっかりと確保するためにはこのスケジューラを上手に設定することでこれらのクリティカルタスクが効率的にまた互いにコンフリクトすることなくリアルタイムに処理されることを担保する必要があります。なお、この記事で紹介するコードはLinuxカーネル5.4.0 (Ubuntu 20.04 LTSのベースカーネル) を元としています。

また、ティアフォーでは「自動運転の民主化」をともに実現していく、学生パートタイムエンジニアを常時募集しています。自動運転を実現するためには、Softwareに関してはOSからMiddlewareそしてApplicationに至るまで、Hardwareに関してはSensorからECUそして車両に至るまで異なるスキルを持つ様々な人々が不可欠です。もしご興味があれば以下のページからコンタクトいただければと思います。 tier4.jp

Linuxのスケジューラについて

まず、スケジューラとはなんぞや?という話から始めると、CPU上で実行するタスク(Linuxではスレッドがスケジュール単位)が複数あるとき、どのタスクから実行するかを決めるものです。 スケジュールの仕方のことをスケジューリングポリシーと呼び、Linuxには現在6つのスケジューリングポリシーが搭載されています。

搭載されているポリシーはkernel/sched/sched.hで確認することができます。

#define SCHED_NORMAL 0
#define SCHED_FIFO 1
#define SCHED_RR 2
#define SCHED_BATCH 3
#define SCHED_IDLE 5
#define SCHED_DEADLINE 6

SCHED_NORMAL (SCHED_OTHER)は通常のタスクに使われるスケジューラで、現在LinuxだとCompletely Fair Scheduler (CFS)という赤黒木を使用するできるだけ平等にタスクに実行時間を割り当てるスケジューラです。

SCHED_FIFOとSCHED_RRは静的優先度を使うスケジューリングポリシーです。

優先度が高いタスクが低いタスクより優先的にスケジュールされ、高優先度のタスクでコアが占有されている時、低優先度のタスクは実行されません。 逆に、低優先度のタスクが実行開始後に高優先度のタスクが来たら低優先度のタスクは直ちに中断し、高優先度のタスクが実行開始します。

SCHED_FIFOとSCHED_RRは0~99の優先度を持ち、双方この優先度に従ってスケジューリングを行いますが、挙動の違いは同じ優先度を持つタスクが複数存在する時、SCHED_FIFOはFirst-In-First-Out (FIFO)キューを用いて逐次実行し、SCHED_RRはRound-Robin (RR)を用いて設定したtime slice事に実行タスクを切り替えます。

SCHED_NORMALのタスクは優先度が100以上なので、SCHED_FIFOとSCHED_RRは任意のSCHED_NORMALタスクより優先され、SCHED_NORMALのタスクと比較してSCHED_FIFO/SCHED_RR/SCHED_DEADLINEのタスクをリアルタイムタスクと呼んだりします。

また、SCHED_FIFO/SCHED_RRはPOSIX.1-2001で規程されているので、pthread_setschedparam(3)を使って設定することが可能です。

SCHED_BATCH, SCHED_IDLEについては本記事では省略します。

Earliest Deadline First (EDF)

さて、本題のSCHED_DEADLINEについて説明していきます。

SCHED_DEADLINEはLinux 3.14以降に標準搭載されたEarliest Deadline First (EDF)というスケジューリングポリシーをベースとしたスケジューラで、設定されたデッドラインが一番近いものから実行していきます。

EDFの例を見てみましょう。

f:id:twertho:20210209113837p:plain

  • まずt=0にタスク1がリリースされ、他のタスクはないのでそのままスケジュールされます。
  • 次にt=1にタスク2がリリースされますが、デッドラインがタスク1のデッドラインより後ろなのでタスク1が終わるまで待機キューに入れられます。
  • t=2になりタスク1が実行終了するので、タスク2が実行を開始します。
  • t=3ではタスク3がリリースされ、今度はタスク3のデッドラインの方が近いので、タスク2は中断(プリエンプション)されます。
  • t=4でタスク3が終了するので、中断されていたタスク2が実行を再開します。

よって実行結果は図の最下部のようになります。 例はシングルコアでのスケジューリング例ですが、マルチコアでも同様のスケジューリングポリシーが使えます。

SCHED_DEADLINEの使用方法

ここではSCHED_DEADLINEの使い方を簡単に説明していきます。 基本は公式ドキュメントDeadline Task Schedulingを参照してください。

SCHED_DEADLINEポリシーはPOSIX規格ではないのでpthread_setschedparam(3)ではなくsched_setattr(2)を使います。(sched_setscheduler(2)sched_setparam(2)の組み合わせでも同様の設定ができます)

sched_setattr(2)の引数を説明していきます。

int sched_setattr(pid_t pid, const struct sched_attr *attr, unsigned int flags);
  • pid:ポリシーを設定する対象のpid/0の時は呼び出しスレッドが設定対象になる。
  • attr:スケジューリング属性(attribute)後述
  • flags:現状では何も意味がないので0を設定(ドキュメントには将来の機能拡張のために確保となっている)

さてスケジューリング属性attrの構造体であるstruct sched_attrは以下のようになっています。

struct sched_attr {
    __u32 size;

    __u32 sched_policy;
    __u64 sched_flags;

    /* SCHED_NORMAL, SCHED_BATCH */
    __s32 sched_nice;

    /* SCHED_FIFO, SCHED_RR */
    __u32 sched_priority;

    /* SCHED_DEADLINE (nsec) */
    __u64 sched_runtime;
    __u64 sched_deadline;
    __u64 sched_period;
};

コメントにも書かれている通りsched_nicesched_priorityはSCHED_DEADLINEでは使いません。

sched_priorityは0を設定するものの、内部ではSCHED_DEADLINEタスクは優先度が-1に設定されており、任意のSCHED_FIFO,SCHED_RRタスクよりも優先されます。

SCHED_DEADLINEでは実行時間sched_runtime、デッドラインsched_deadline、周期sched_periodを使います。

実際にSCHED_DEADLINEを使って実行時間10ms/デッドライン30ms/周期40msのタスクの設定してみると以下のようになります。

sched_attr attr;
attr.size = sizeof(attr);
attr.sched_flags = 0;
attr.sched_nice = 0;
attr.sched_priority = 0;

attr.sched_policy = SCHED_DEADLINE;
attr.sched_runtime = 10 * 1000 * 1000;
attr.sched_deadline = 30 * 1000 * 1000;
attr.sched_period = 40 * 1000 * 1000;
unsigned int flags = 0;
sched_setattr(0, &attr, flags);

なお、sched_periodは必須ではなく、設定しない場合sched_deadlineが周期として使われます。

SCHED_DEADLINEを使用する際の注意点

SCHED_DEADLINEを使うにはsched.hをインクルードして前セクションの通りに設定すれば基本的に動きます。(環境によってはlinux/sched.h)

が、環境によっていくつかハマりポイントがあるので、いくつか紹介していきます。

sched.hがない/インクルードしてもsched_setattrが見つからない

環境によってsched.hが見つからない/インクルードしてもsched_setattrコンパイルされない場合があります。(Ubuntu 18.04 LTS等)

自分の環境では以下のようにシステムコールを直打ち/構造体を直接宣言することで使用できました。

#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>
#include <linux/unistd.h>
#include <linux/kernel.h>
#include <linux/types.h>
#include <sys/syscall.h>
#include <pthread.h>
#define SCHED_FLAG_DL_OVERRUN       0x04
#define gettid() syscall(__NR_gettid)

#define SCHED_DEADLINE  6

#ifdef __x86_64__
#define __NR_sched_setattr      314
#endif

#ifdef __i386__
#define __NR_sched_setattr      351
#endif

#ifdef __arm__
#define __NR_sched_setattr      380
#endif

#ifdef __aarch64__
#define __NR_sched_setattr      274
#endif

struct sched_attr {
    __u32 size;

    __u32 sched_policy;
    __u64 sched_flags;

    /* SCHED_NORMAL, SCHED_BATCH */
    __s32 sched_nice;

    /* SCHED_FIFO, SCHED_RR */
    __u32 sched_priority;

    /* SCHED_DEADLINE (nsec) */
    __u64 sched_runtime;
    __u64 sched_deadline;
    __u64 sched_period;
};

int sched_setattr(pid_t pid, const struct sched_attr *attr, unsigned int flags){
    return syscall(__NR_sched_setattr, pid, attr, flags);
}

SCHED_DEADLINEが設定されない

sched_setattrを使っているのにSCHED_DEADLINEポリシーが適用されていないという場合はsched_setattrの返り値をみてみます。 正しく設定されている場合は0、失敗した場合は-1を返します。

sched_setattrにはいくつかのerrnoが設定されてますがSCHED_DEADLINEを使う場合、以下の3つがよく遭遇します。

  • EPERM:root権限で実行していない。SCHED_DEADLINEはユーザー権限では動きません。
  • EINVAL:パラメータが正しく設定されてない、または周期等の値が210~263の間にない時にこのエラーがでます。
  • EBUSY:sched_setattrでSCHED_DEADLINEを設定する際、使用率=実行時間/周期の計算が行われます。この使用率をスケジューラが受け入れるだけの余裕があるかを計算し、無理な場合EBUSYを返します。これにより最低限のデッドライン保証をチェックしています。

特に複数のタスクをSCHED_DEADLINEポリシーで動かす場合EBUSYエラーが起こり易いので、可能ならデッドラインを長めに見積もると良いでしょう。

詳しくここのvalidationを知りたい方はここに実装があります。 まだRC版である最新の5.11系ではこのvalidationが高速化されるみたいです(参照)。

tasksetでCPU制限した場合に失敗する

SCHED_DEADLINEとtasksetを併用するとエラーを起こすケースがあります。

これはSCHED_DEADLINEのCPUハンドリングがtasksetによるマスキングに対応していないためと思われます。

実行するコアを限定したい場合はcgroupcpusetで設定することで上手く動きます。

Constant Bandwidth Server (CBS) について

さて、SCHED_DEADLINEはEDFポリシーに従ってスケジューリングしてくれますが、適切なdeadlineとruntimeを設定するのはユーザーの責任となっています。

設定が適切でなければ、仮にプロセスが無限loopなどで実行し続けてdeadlineを超過したとしても、何も起こらずCPUリソースが占有され続けることになります。

これを防ぐためにLinux4.16以降ではConstant Bandwidth Server (CBS)という機能がついています。

CBSは実行時間を制限するもので、SCHED_DEADLINEでは設定されたスレッドの実行時間がsched_runtimeを超えた場合、シグナルを送るように設定することができます。

attr.sched_flags=SCHED_FLAG_DL_OVERRUNと設定することで超過時にSIGXCPUというシグナルをスレッドに送ります。 このシグナルをハンドリングすることで超過対策を行うことができます。

またCBSにはreplenishというもう一つの機能があります。

例えばタスクAの実行中にデッドラインが近いタスクBがリリースされたことによりAは中断(プリエンプション)され、Bが終了しAを再開しようとしたときにAのデッドラインを過ぎていて本来ならばAを実行できないといったケースがあります。

このような中断はユーザが関知することができない以上、Linuxの実装ではタスク自体は実行時間が設定されたruntimeを超えない限り、デッドラインを超過していても最後まで実行させるというポリシーを取っています。

この最後まで実行させるために追加の実行時間を確保する仕組みがreplenishになります。

内容としては残りの実行時間(runtime-中断するまでに実行した時間)をruntimeとし1周期後のデッドラインをデッドラインと変更し、再度スケジューリングされます。

このCBSがあるため厳密なEDF通りの実行にならないことを考慮した上でSCHED_DEADLINEを使う必要があります。

SCHED_DEADLINEの実装

最後にSCHED_DEADLINEの実装を簡単にみてみましょう。

実装は主にkernel/sched/deadline.cに記述されています。

まず、SCHED_DEADLINEのランキューをみてみます。

struct dl_rq {
    struct rb_root_cached   root;
    unsigned long       dl_nr_running;
    struct dl_bw        dl_bw;
    u64         running_bw;
    u64         this_bw;
    u64         extra_bw;
    u64         bw_ratio;
};
  • root:ランキューを管理する構造体
  • dl_nr_running:デッドラインクラスのスレッドで実行可能な数
  • *_bw:このbwはbandwidthの略で使用率とほぼ同義で、先ほどのスケジューリング可能性のチェック等で使われます。

次にデッドラインクラスをみてみます。 デッドラインクラスはRTクラス(SCHED_FIFO/SCHED_RR)より上、最上位クラスのSTOPクラスより下になっています。

const struct sched_class dl_sched_class = {
    .next           = &rt_sched_class,
    .enqueue_task       = enqueue_task_dl,
    .dequeue_task       = dequeue_task_dl,
    .yield_task     = yield_task_dl,

    .check_preempt_curr = check_preempt_curr_dl,

    .pick_next_task     = pick_next_task_dl,
    .put_prev_task      = put_prev_task_dl,
    .set_next_task      = set_next_task_dl,

    .task_tick      = task_tick_dl,
    .task_fork              = task_fork_dl,

    .prio_changed           = prio_changed_dl,
    .switched_from      = switched_from_dl,
    .switched_to        = switched_to_dl,

    .update_curr        = update_curr_dl,
};

ここで重要なメンバを3つほどみていきます。

  • enqueue_task:タスク追加時の処理
  • dequeue_task:タスク削除時の処理
  • pick_next_task:次に実行するタスクの選択処理

タスク追加時の処理

static void
enqueue_dl_entity(struct sched_dl_entity *dl_se,
          struct sched_dl_entity *pi_se, int flags)
{
    BUG_ON(on_dl_rq(dl_se));

    //省略

    __enqueue_dl_entity(dl_se);
}
static void enqueue_task_dl(struct rq *rq, struct task_struct *p, int flags)
{
    struct task_struct *pi_task = rt_mutex_get_top_task(p);
    struct sched_dl_entity *pi_se = &p->dl;

    //省略

    enqueue_dl_entity(&p->dl, pi_se, flags);

    if (!task_current(rq, p) && p->nr_cpus_allowed > 1)
        enqueue_pushable_dl_task(rq, p);
}

DEADLINEタスク追加時の処理は、優先度継承、CBSに関連する例外処理等がありますが、重要な追加時の関数は__enqueue_dl_entityであることがわかります。

static void __enqueue_dl_entity(struct sched_dl_entity *dl_se)
{
    struct dl_rq *dl_rq = dl_rq_of_se(dl_se);
    struct rb_node **link = &dl_rq->root.rb_root.rb_node;
    struct rb_node *parent = NULL;
    struct sched_dl_entity *entry;
    int leftmost = 1;

    BUG_ON(!RB_EMPTY_NODE(&dl_se->rb_node));

    while (*link) {
        parent = *link;
        entry = rb_entry(parent, struct sched_dl_entity, rb_node);
        if (dl_time_before(dl_se->deadline, entry->deadline))
            link = &parent->rb_left;
        else {
            link = &parent->rb_right;
            leftmost = 0;
        }
    }

    rb_link_node(&dl_se->rb_node, parent, link);
    rb_insert_color_cached(&dl_se->rb_node, &dl_rq->root, leftmost);

    inc_dl_tasks(dl_se, dl_rq);
}

この実装からデッドラインクラスのランキューには通常タスクのCFSでも使われている赤黒木が使われていて、タスク構造体をデッドラインを基準に挿入していることがわかります。 赤黒木は安定した二分木なので、これによりデッドラインが一番近い(最小値)のタスクを選択しスケジュールすることが可能になっています。

タスク削除時の処理

削除に関する実装も同様に__dequeue_dl_entityを見ると赤黒木から終了したタスクの削除を行っていることがわかります。

static void __dequeue_dl_entity(struct sched_dl_entity *dl_se)
{
    struct dl_rq *dl_rq = dl_rq_of_se(dl_se);

    if (RB_EMPTY_NODE(&dl_se->rb_node))
        return;

    rb_erase_cached(&dl_se->rb_node, &dl_rq->root);
    RB_CLEAR_NODE(&dl_se->rb_node);

    dec_dl_tasks(dl_se, dl_rq);
}

次に実行するタスクの選択処理

最後に次に実行するタスクの選択処理をみてみましょう。

static void set_next_task_dl(struct rq *rq, struct task_struct *p)
{
    p->se.exec_start = rq_clock_task(rq);

    dequeue_pushable_dl_task(rq, p);

    if (hrtick_enabled(rq))
        start_hrtick_dl(rq, p);

    if (rq->curr->sched_class != &dl_sched_class)
        update_dl_rq_load_avg(rq_clock_pelt(rq), rq, 0);

    deadline_queue_push_tasks(rq);
}
static struct sched_dl_entity *pick_next_dl_entity(struct rq *rq,
                           struct dl_rq *dl_rq)
{
    struct rb_node *left = rb_first_cached(&dl_rq->root);

    if (!left)
        return NULL;

    return rb_entry(left, struct sched_dl_entity, rb_node);
}

static struct task_struct *
pick_next_task_dl(struct rq *rq, struct task_struct *prev, struct rq_flags *rf)
{
    struct sched_dl_entity *dl_se;
    struct dl_rq *dl_rq = &rq->dl;
    struct task_struct *p;

    WARN_ON_ONCE(prev || rf);

    if (!sched_dl_runnable(rq))
        return NULL;

    dl_se = pick_next_dl_entity(rq, dl_rq);
    BUG_ON(!dl_se);
    p = dl_task_of(dl_se);
    set_next_task_dl(rq, p);
    return p;
}

実装によると選択処理は5ステップから構成されています。

  • sched_dl_runnableでランキューを確認し、タスクが実行する可能か確認
  • pick_next_dl_entityで赤黒木からデッドラインが一番近いものを選択
  • dl_task_ofで選択したスケジュールから実行するタスク構造体ポインタを抽出
  • set_next_task_dlで実行開始時間を記録
  • 次に実行するタスク構造体ポインタを返す

CBSの処理が絡むとややこしいですが、それ以外の基本的な処理は赤黒木を使ったわかりやすい実装になっていることがわかります。

まとめ

今回はLinuxに搭載されているEDFスケジューラのSCHED_DEADLINEを簡単にみてみました。 なお今回は簡単にまとめる関係上Symmetric Multi-Processing(SMP)の処理部分は省いています。

リアルタイムに対応するためにRT_PREEMPT_PATCHを導入する手法も知られてはいますが、SCHED_DEADLINEを使う分には何も変更することなく値を設定するだけで簡単にEDFスケジューラがLinuxで使えるので、是非興味ある方はいじって見ると面白いと思います。

参照

www.kernel.org

linuxjm.osdn.jp

linuxjm.osdn.jp

西新宿における自動運転実証実験の振り返り

こんにちは、ティアフォーで自動運転システムを開発している木村と斉藤です。今回は2020年11月から12月にかけて行った西新宿における自動運転の実証実験の概要と振り返りをご紹介します。

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

tier4.jp

ティアフォーにおける自動運転の実証実験について

私たちが行う実証実験の目的は、お客様へのデモンストレーションから、新しい自動運転技術への挑戦、新たなオペレーション手法の検証などなど多岐にわたりますが、いずれも実証実験毎にそれぞれ具体的な目的を定めています。

今回の実証実験はビジネス視点ではサービスとしての成り立ちを見るための観点から行われましたが、私たち自動運転エンジニアとしての視点では3つの目的を持って取り組みました。1つ目は都心での実運用されている5Gを活用した遠隔自動運転の性能評価、2つ目はMobility Technologies様のアプリケーションと弊社運行管理システムとの連携、3つ目は新しいアーキテクチャとなったAutowareの公道検証となります。今回は、私たちが主に担当していた新しいアーキテクチャのAutowareを扱った自動運転システムを中心に振り返っていきたいと思います。ここでの新しいアーキテクチャのAutowareというものはAutoware Architecture Proposalとして私たちがAutoware Foundationに提案しているもので、このAutowareを用いた自動運転システムによって走行する車両を社外の方に公道でお見せするのは今回行われた西新宿における実証が初めてとなります。 

f:id:tkimura4:20210128143949p:plain

西新宿における実証実験について

①11月の実証実験

11月に西新宿での自動運転実証実験を行いました。実験は2020年11月5日(木)から2020年11月8日(日)までの計4日間行われました。11月5日(木)は関係者試乗、他3日間は一般の試乗者を募集して乗車していただきました。

11月の実証実験ではジャパンタクシー車両をベースにした自動運転車両を1台用いました。初日以外の3日間は京王プラザホテルから新宿中央公園までの1km程度のコースを走行しましたが、初日は京王プラザホテルからKDDI新宿ビル内を経由し、新宿中央公園の前を通って京王プラザホテルに戻る2km程度のコースを走行しました。

f:id:tkimura4:20210128143952p:plain

11月の実証実験で使用した走行ルート

実験中は安全のためドライバーとオペレーター(助手席)*1が乗り込んで非遠隔監視での自動運転走行を実施しました。それに加え、初日には運転席は無人で遠隔のドライバーが映像を通して監視しながら走行を行う遠隔型自動走行も行いました。

実証実験では東京都の宮坂副都知事にもご乗車いただき、「違和感のない乗り心地だった」という大変ありがたいコメントもいただきました。多くの関係者・一般試乗者の方に乗車いただきながら、西新宿という交通量の多い環境の中での実証実験を無事終えることが出来ました。

なお、11月の実証実験に関するより詳しい情報はこちらのプレスリリースをご覧ください。

tier4.jp

また、実証実験の様子は以下の動画をご覧ください。

www.youtube.com

www.youtube.com

②12月の実証実験

こちらの実証実験は西新宿にて2020年12月8日(火)から2020年12月23日(水)までの平日計12日間で行われ、そのうち7日間は関係者試乗、5日間は一般者試乗という形で試乗を乗せての自動運行を行いました。

11月の実証実験と同様に西新宿で実施しましたが、12月の実証実験では最大3台のジャパンタクシー車両を同時稼働させ、試乗者が京王プラザホテル/東京都庁第二庁舎ロータリー/新宿中央公園の3か所から出発地・目的地を選択可能という形での運行を行いました。そのため11月に行った実証実験と比較して、多数台の稼働や複数ルートの走行などの要素が加わり、更に難易度を上げての実施となりました。

f:id:tkimura4:20210128144000p:plain

12月の実証実験で使用した走行ルート

f:id:tkimura4:20210128143955p:plain

三台のジャパンタクシー

本実証実験には12日間で計100名を超える試乗者の方に参加いただき、多くの方に自動運転を体験していただく機会となりました。西新宿という難しい環境かつ難しい条件での実験を通し、ありがたい事に多くの課題も見つかりました。そうしたことを踏まえながら実証実験の振り返りをしていきたいと思います。

なお、12月の実証実験に関するより詳しい情報はこちらのプレスリリースをご覧ください。

tier4.jp

また、実証実験の様子は以下の動画をご覧ください。 

www.youtube.com

振り返り

実証実験では、西新宿という交通量も多く難しい環境の中で、多くの右左折を含むコースの走行、遠隔監視型自動走行、同時の複数台運行などの難しい条件が複合した実験となり、私たち開発者としても不安の尽きない日々でした。そのような中でも、自動運転をする上で特に難しかった/大変だったケースをいくつか紹介いたします。

障害物の誤検知

今回の実証実験でかなり悩まされたのが障害物を適切に検知することです。その中でも特に悩まされたのが歩道の段差や落ち葉です。

Autowareは、センサ情報から自動車・歩行者といった検出したいものを認識して結果を出力する「ホワイトリスト形式」、地面や雨・落ち葉といった検出したくないものを検出対象から除いて結果を出力する「ブラックリスト形式」の両方のアプローチで動作しています。この様に「ホワイトリスト形式」と「ブラックリスト形式」の両方を採用している理由は、例えば前方のトラックから急に荷物が落ちてくる、といったイレギュラーな事象にも対応するためです。

例えば今回の実証実験では、車道から新宿公園に入る際には20~30cmほどの段差がある歩道を通過する必要がありますが、その様な大きい段差がある場所では適切に地面除去できず、段差を障害物として認識してしまうことが起きていました。また、今回は秋の実験ということもあり落ち葉が非常に落ちやすい環境だったので、大量の落ち葉がまとまって落ちて来たり落ち葉が舞い上がったりすると大きな障害物としてみなされ、適切に除去出来ず障害物として認識されてしまうことが起きていました。今回の実証実験で明らかとなったこれらの課題を踏まえ、認識技術をより高精度な手法へと発展させていく取組を現在行っています。

交差点での素早い判断とスピーディな加速

西新宿で自動運転を行う上で、最も難しいことと言えばやはり交通量の多さが挙げられると思います。このような交通量が多い環境の中、今回の実証実験のコースには多くの右折が含まれていました。右折の中でも、特に難しい場所は右折信号のない交差点での右折です。右折信号がある場所では対向車両の流れが完全に途切れてから右折をすることが可能ですが、そうでない場所ではかなりシビアなタイミング判断が求められます。

ティアフォーの自動運転システムでは、安全のために前方車との衝突余裕時間(Time to Collision)を計算し、前方車との衝突がまず起きないコンサバティブな見積もりをして右折をします。これに加え今回の実証実験では、快適な乗り心地を実現するため車両の発進時には紳士的な加速を行う様に設定していました。しかしながら、このコンサバティブな見積もりに加えてこのような紳士的な加速を意識していては、交通量の多い西新宿の一部の交差点で時間帯によっては曲がるタイミングがなかなか得られないことがあり、信号の変わり目では乗り心地よりも素早い加速が求められるシーンがありました。このことから、西新宿のような交通量の多い交差点でもより自動運転サービスの可用性を高めることができるよう、人間の様にシーンに応じてコンサバティブさやアグレッシブさの加減を切り替えられるように今後取り組んでいきます。

複数の走行経路

ティアフォーが行ったこれまでの実験では走行経路が固定された1つに限られていましたしかし、今回の12月の実証実験ではより実サービスに近い設計となっており、3か所から出発地・目的地を選択可能となっていたことから、当日選ばれた組み合わせに応じて計6つの経路を走るという大変チャレンジングな取組を行いました。

Autowareでは信号や交差点、横断歩道、道路の優先関係などが紐付いた地図情報を用いて交通ルールを認識・判断しているため、実験で使用するコースを実際に走行してみて、問題なく走れるかどうか、地図情報が正しく現実と合っているかどうかなど様々なことを確認する必要があります。そのため、これまでの実証実験と同様の手法で全てを確認していたら、単純計算で6倍の時間がかかってしまいます。

そこで、今回は今まで以上にシミュレータをフルに活用することで、可能な限り現地に行かずに地図の検証等を行うことで時間を短縮し実証実験を乗り切りました。今後シミュレータの活用範囲をさらに広げていき、開発スピードもより効率化していきます。

車両個体差

最後に、大きな技術課題とまでは言えないものの地味に悩まされたのが車両個体差です。同じ種類の車両とはいえ、同じアクセルペダル、ブレーキペダルを踏んでも個体によって加減速の具合は全然異なりますし、同じハンドル操作をしても個体によってカーブの具合も全く異なります。また、タイヤ空気圧やブレーキパッドの経年劣化といった事象によっても車両の制御性は変わります。そういった個体差を吸収するためにそれぞれパラメタチューニングする必要があるのですが、どうしても実際に車両を動かす必要があるため、必要以上に時間が取られます。少し加減速の具合が違うだけでも思った以上に運転の滑らかさに影響するため、精細なチューニングが求められるのも中々難しいところでした。

今回の実証実験では台数も少なかったのでマンパワーで乗り切ることが出来ましたが、これが100台、1000台となってくると流石にワークしません。こちらの実証実験を通して、走っているうちに段々走行具合が滑らかになっていく様な自動チューニングの必要性を実感し、ティアフォーでも自動チューニングの仕組みを取り入れ始めています。

まとめ 

今回は西新宿の実証実験の概要と振り返りをご紹介しました。今回ご紹介したように、私たちの開発している自動運転ソフトウェアにはまだまだ課題が多くあります。これからも引き続きより良い自動運転のために精進していこうと思います。

謝辞

今回の自動運転実証実験は株式会社Mobility Technologies、株式会社ティアフォー、損害保険ジャパン株式会社、KDDI株式会社アイサンテクノロジー株式会社の5社の協業体制にて行いました。私たちが自動運転システムの開発に集中して取り組めたのも上記企業様の多大なご協力があってこそのことです。また、11月の実証実験は一般社団法人新宿副都心エリア環境改善委員会、12月の実証実験は東京都がそれぞれの実証実験の主催者として舞台を提供いただき実現しました。この場をお借りして深く感謝いたします。

*1:オペレーターというのは、自動運転システムで対応が困難かつ緊急性が求められる際に、自動運転システムに対する緊急停止などのソフトウェア的な介入や、ドライバーに対するオーバーライド指示を出す役割を指します

Visual SLAMの可能性と技術的チャレンジ

こんにちは、ティアフォーでVisual SLAMの研究をしている石田です。今回は、自動運転における自己位置推定に関してVisual SLAMの重要性と課題についてお話ししたいと思います。

www.youtube.com

Visual SLAMの要素技術であるVisual Odometryの例

Reference: Alismail et al., 2016
Dataset: Computer Vision Group - Dataset Download

自動運転における自己位置推定

車両が地図上のどこを走っているかを推定する技術(自己位置推定)は自動運転において欠かせない要素のひとつです。自分がどこを走っているかを把握できなければ迷子になってしまいますし、自分が走っている場所の先に何があるかを把握することも難しくなってしまいます。

現状のAutowareではレーザー光を用いたLiDARという巨大なセンサーを車両に搭載して自己位置推定を行っています。あらかじめ作っておいた地図の構造と、LiDARで得られた車両周囲の3次元構造を照らし合わせて自分がどこにいるのかを把握するというわけです。ジグソーパズルをはめ込むようにして位置推定をしていると考えるとイメージが湧きやすいかもしれません。

しかしながらLiDARベースの位置推定手法には大きく2つの欠点があります。

ひとつはLiDARそのものが非常に高価であることです。性能のいいものでは数千万円もするものがあります。これを世の中のクルマひとつひとつに取り付けて走らせることは現実的ではないため、なんらかの方法でコストを下げる必要があります。

もうひとつは構造特徴に乏しい場所では性能を発揮できないことです。絵柄のないジグソーパズルをイメージしてみてください。ピースひとつひとつが異なる形をしていたら、それぞれのピースは特定の場所にはまります。しかし、もし全てのピースが正方形だったら... これはもはやジグソーパズルの意味を成しませんよね。全てのピースがどこにでもはまってしまいます。同じように、構造的な特徴に乏しい場所(平原やトンネルなど)では、LiDARはどこにでも”はまって”しまうため、位置を一意に定めることができません。

これらの問題を解決するために現在私が取り組んでいるのがVisual SLAMという技術です。

Visual SLAMとは、視覚情報を用いて地図を作成し、同時に地図内での自己位置を推定する技術です。 また、あらかじめ作成しておいた地図に対して、その地図内でどこにいるのかを推定する relocalization という技術も活発に研究されています。

Visual SLAMの最大の利点は設備の安さです。Visual SLAMは基本的にカメラやIMU(物体の加速度や回転速度を測るセンサー)からの入力に基づいて動作します。これらのセンサーは安価に手に入るため、数万円で位置推定を始めることができます。また、Visual SLAMは視覚情報をもとに動作するため、構造特徴に乏しい場所であっても車の位置を推定することができます。さらに、カメラやIMUだけでなく、LiDARなどの他のセンサーと組み合わせることによってロバスト性を向上させ、さまざまな場所を走ることができるようになります。

自動運転ではあらかじめ作成しておいた地図との照合による位置推定が基本となるため、通常走行の範囲では relocalization が主体となります。しかし、LiDARがうまく動かない場所ではLiDARで地図を作ることが難しいため、Visual SLAMによって車両の位置推定をしつつLiDARによって地図を作ることも想定されています。

 

Visual SLAMの技術的チャレンジ

自動運転におけるVisual SLAMの最大の課題は、なんといってもそのロバスト性です。自動車はあらゆる環境のあらゆる場所を走ります。街中、ハイウェイ、峠、住宅街、晴れ、雨、雪、夜…

現在はODD(運行設計領域)対象外の環境もあるとはいえ、トンネルや逆光など、日常生活でよく目にするシーンの中にもVisual SLAMにとっては難しいものがあります。こういった場所で安定して動作する手法を設計し実装するのは至難の業ですが、技術者の腕の見せどころでもあります。

Visual SLAMにとって難しい環境は大きく分けて2つあります。ひとつは暗い場所、もうひとつは動く物体が多い場所です。

Visual SLAMは画像の情報から地図の3次元構造を計算し、その地図に対して自分がどの程度動いたかを推定することによって位置を把握します。画像からどのような情報を抽出するのかは手法によって大きく異なりますが、画像の特徴がよく現れている点(特徴点)を用いるものが一般的です。カメラから得られた画像1枚1枚から特徴点を抽出し、それらの幾何関係を利用して3次元地図を作ったり、位置推定を行ったりします。しかし、暗い場所ではそもそも画像から得られる情報が大きく減ってしまいますし、画像から有用な特徴点を抽出することも難しくなります。露光時間が長くなってブレが大きくなってしまいますし、カメラの感度が高くなって明るいものは極端に明るく映ってしまうためです。暗い場所でVisual SLAMを安定して動作させるためにはカメラ本体も含めた包括的な設計が必要になるため、これは非常にチャレンジングな問題だと言えます。

動く物体の扱いも大きな課題です。既存のVisual SLAMのほとんどは周囲の環境が動かないことを仮定しており、動く物体の扱いをあまり考慮できていません。このため、周囲に移動物体がたくさんあると、自分が動いているのか、それとも環境が動いているのかを判別できなくなってしまいます。自動車は周囲に動く物体がたくさんある場所をよく走るため、Visual SLAM単体で位置推定を行うことはせず、他のセンサーと組み合わせることが必須となります。

 

トンネル内で得られた画像の例
カメラの感度が上がっている一方で対向車のライトは非常に明るいため
カメラが扱える明るさを大きく超えてしまっている

自動運転車にとって最も難しい環境のひとつがトンネルです。トンネル内部にはGNSS(GPSなどの衛星測位システム)の信号が届きませんし、構造的な特徴に乏しいためLiDARの信頼性も低くなります。また、トンネルはVisual SLAMにとってもやはり課題のひとつです。

トンネルの内部は暗いため有用な視覚情報を得ることが難しいですし、全体が暗いのに対して周囲の車のライトは明るいため、ひとつの環境の中で非常に激しい光量変化が起きてしまいます。また、トンネル出入口では視野全体の明るさが一気に変わるため、やはり極端な光量変化に対応できる機材およびアルゴリズムが求められます。それに加えて、周囲の全ての車が移動物体なので、これらもうまく扱わなければなりません。

こういった難しい環境でも安心して走れるよう、我々はハードウェアからソフトウェア、さらにはインフラまでも対象として、日々研究開発を行っています。

 

Visual SLAMの面白さ

Visual SLAMはコンピュータビジョンの総合格闘技です。画像処理に対する基礎的な知識はもちろんとして、エピポーラ幾何や数理最適化に対する深い理解が要求されますし、IMUなど他のセンサと組み合わせる場合はカルマンフィルタや制御理論の知識も必要になります。また、Visual SLAMはやや大規模なシステムになることが多いため、コードをきれいに管理したり、綿密にテストしたりする能力も求められます。特に自動運転では非常に高い安全性が求められるため、他のVisual SLAMのアプリケーションと比べると徹底したテストが必要となります。

自動運転では、Visual SLAMは省電力デバイス上で高速に動作することが要求されるため、コンピュータの能力をフルに発揮するための実装力も必要になります。車に数千ワットの巨大なコンピュータを載せるわけにはいきませんし、かといってゆっくり動作すると車の運行性能や安全性が犠牲になってしまいます。先ほども述べたように、Visual SLAMの多くは画像から特徴点を抽出し、それらの幾何関係を利用して地図作成と位置推定を行います。カメラは1秒間に何十枚もの画像を出力しますし、1画像から得られる特徴点の数は数百から数千にもなります。自動運転では、省電力デバイスの上で、大量の特徴点から信頼できるものを選び出し、地図作成と位置推定をできるだけ高速に行う必要があるというわけです。また、地図を作っていくと誤差が蓄積してどんどん歪んでいってしまうため、この歪みを補正する処理も必要になります。小さなコンピュータで大量の情報を正確かつ高速に処理することがVisual SLAMの最大のチャレンジであり、これを徹底した安全管理と品質管理に基づいて自動車に載せていくことが私にとっての自動運転の面白さです。

Visual SLAMを自動運転に活用するためには、情報工学の理論から実装までありとあらゆる知識が必要とされます。自分がいままで学んできたことを全てつぎ込んでものをつくることが、この仕事の最大の面白さです。

もしあなたが自動運転車に乗る日が来たら、車のまわりを細かくチェックしてみてください。もしかしたらカメラがたくさんついていて、そのうしろでは私が作ったシステムが動いているかもしれません。

ティアフォーでは、自己位置推定に関するリサーチャーとエンジニアを絶賛募集しています!もしご興味ある方がいましたら、以下のURLからご応募ください!!

tier4.jp