こんにちは、モリカトロンのAIエンジニアの銭です。

ロボットトイ toio（トイオ）をベースに、弊社が４台のトイオキューブを生き物っぽく動かせるアプリ「ウロチョロス」を開発しました。その中に一つ大きな課題としては、どうやってキューブたちを互いに衝突せず且つ自然に動き回すかです。今回はこの集団行動の課題で用いた手法と改良方法を紹介して行きたいと思います。

集団行動の制御ーーNavigator

集団行動の制御ーーNavigator

Navigatorは複数のロボット（ウロチョロスの場合は「キューブ」）が存在する時、お互いのロボットの動きを考慮しながら上手く移動するために、作られたアルゴリズムであります。このアルゴリズムは主に「ヒューマンライク衝突回避」と「ボイド」二つのアルゴリズムに基づいてあります。

何ができる？

主に以下のようなことができます：

目的座標に向かう/目的座標から離れる
移動中に、指定した他ロボットを回避する
指定した他ロボットとボイド組んで行動する
回避とボイド同時作用する

とりあえずデモを見てみましょう！

回避のみ	ボイドのみ	ボイド+回避

注：シミュレーターで130msの遅延設定で行ったデモです。

以下の相違点があります：

パス――滑らかに回避する方向に向かうか否か
集団の間の距離
速度の変化
目標あたりでの動き

回避アルゴの効果は、自然に避けられることと目標辺りにサッと止まれることです。ボイドを用いることでロボットを群衆のように動かすことが出来ますが、上手く止まることが出来ず、綺麗に他のロボットを避けることも出来ません。しかしボイドと回避アルゴと組み合わせることで、ボイドの問題を解決した上で両方の効果を発揮できます。（速度の変化もボイドが帯びた特性ですが、調整可能です。）

全体構成

Navigator のクラスダイアグラムは以下のようになっています。

上図が示したように、キューブと独立した Navigator 本体が、インターフェイスを通じてリアルのキューブとやり取りするクラス Cube と繋がっています。

Navigator 本体：

Navigator は抽象的な「ロボット」を表すEntity変数を自分「1」：他者「多」の関係で持っています；
Wall は抽象的な壁です；
Navigator は HLAvoid と Boids 2つのアルゴリズムクラスを持っています；
アルゴリズムクラスは Entity と Wall を利用して計算しています；

Cube 用のインターフェイス：

CubeEntity は Entity を継承して、更に CubeHandle を取り込んで、Cube の情報を抽象的な Entity に落とし込みます；
CubeNavigator は Navigator を継承して、アルゴリズムの計算結果(ウェイポイント)を CubeHandle の運動制御アルゴリズムに通じて制御量を出力します。

注：CubeHandle は Cube の生情報の処理（予測）、基本の運動制御（指定座標に移動、指定角度に回転など）を担当しています。

Navigatorを独立させることで、Cube だけではなく、他のロボットにもゲームにも使えます。

ゲームオブジェクトとロボットの違い

衝突回避系のアルゴリズムとボイドはロボティクスとゲームと両方に良く使われていますが、この二つの環境（主には制御の対象となるゲームオブジェクトとロボット）においては大きな違いがあります。

ゲームオブジェクト	二輪ロボット
瞬間に回転できる	回転に時間かかる
瞬間に指定速度を出せる	加速・減速に時間かかる
指令に精確に従う	駆動システムのデッドゾーン、環境からの干渉によって完全に指令通りにはなれない
指令が即時に実行される	通信、駆動システムの特性による遅延がある
衝突しても透過させれば止まらない	衝突したら止まる可能性ある
よくあるカプセルのコライダーはカスっても止まらない	四角いロボットはカスると止まる可能性ある

こういった違いで、ロボットの場合はゲームオブジェクトより、上手く制御する難易度も万一衝突した代価もはるかに厳しいです。特にウロチョロスの場合、遅延が 100ms 以上になります。

これを対応するために：

Unity で遅延を設定できるシミュレーターを作った → テストの効率が大幅向上
CubeHandle で遅延後の予測状態を Navigator に与える → Navigator の入出力の時間を一致させた

ヒューマンライク(HL) 衝突回避

元アルゴリズム

このアルゴリズムをざっくり説明すると、こういうアイデアです：各方向に向かって最大速度で前進して、他のロボットは方向と速度を維持すると、どの程度の距離で衝突するかを計算し、衝突する前の領域において目標と一番近いポイントに目指します。

上図[2]が示したように、視野 2Φ と範囲 H 内の障害物（緑の円で囲まれた移動物体と灰色の壁）を方向と速度を維持するように想定し、自分が方向 α に向かって既定の速度で前進すると、距離 s(α) のところに衝突することを計算します。f(α) は s(α) と同一です。各方向の衝突ポイントが薄赤いラインとなり、ラインの内側は可動域になります。可動域の中に、目標 O に一番近いポイントがこのフレームのウェイポイントとされます。その角度が α_des と記されています。（注意：ウェイポイントは可動域のボーダーにあるには限らない）

実際、α は連続的ではなく離散的に一定の数でサンプリングされますので、f(α) は離散的関数或いはテーブルになります。（２Dライダーセンサーと同じですね。）またコーディングの便利のため、α 毎に複数の障害物との計算を行うではなく、障害物毎に f(α) を計算して最小値を取るのであります。

ベースになるアルゴリズムは以下の擬似コードで表されています。

	Algorithm 1 HL Avoiding
0	while true do
1	方向セットをサンプリングする A = { 現在方向 α0 - Φ + 2 Φ * i / (サンプル数 N - 1) \| i = 0,1,...,N-1 }
2	計算対象となる障害物セット E = { 障害物 e \| e が視野(Φ, H)内にある }
3	for e in E
4	for α_i in A
5	f(α_i \| e) を計算する
6	end for
7	f(α \| e) を得る
8	end for
9	f(α) = MIN{ f(α \| e) \| e ∈ E }
10	for α in A
11	方向 α、距離 f(α) の範囲内、目標と一番近いポイントを計算　→　ポイント p(α), 目標との距離 d(α)
12	end for
13	ウェイポイント wp = p( ARGMIN{ d(α) \| α} )
14	end while

付加方向

元のアルゴリズムの問題：

サンプリングした方向が有限なため、目標への方向は二つのサンプルの間にある場合、ロボットは真っ直ぐに向かえない。
視野が180°以下かつ目標がロボットの後ろにある場合、ウェイポイントが自身の位置となり、ロボットは動けない。

解決策：ロボットから目標への方向をサンプルセットに加えれば解決です。

注：この改良は、[3]にも提案されています。

回転のペナルティ

下図の場合、人間ならどっちにいくのでしょう。

上と思いますが、今の回避アルゴリズムは下を選びますね――Algorithm 1 の 11 行の d(α) は下の方が小さいから。

解決策：サンプル方向と現在方向の差によって d(α) にペナルティを加えます。

解決策の問題：後ろの近くに目標がある時、ロボットが動かない。

提案：このペナルティが必要になる根本的な原因は、回避アルゴリズムは d(α) つまり距離でウェイポイントを評価するが、人間はざっくり言うと「手間」ですね。距離ではなく見込み時間で評価すればこういう副作用もないでしょう。

速度制限

Navigator をロボットから独立させるため、出力をウェイポイントと速度上限にしています。そして動きの自然さと滑らかさなどのため、回転してウェイポイントに向いてから直進するという動きはさせたくない――つまり回転しながら前進することもあります。その特性が f(α) の計算の前提――ロボットは方向 α に直進することに合わないことから生じた問題は、想定外のパスを走って衝突する可能性があります。

解決策：周囲を考慮して、角速度によって、速度上限をかけます。

具体的には、

まず可動域のボーダー（灰色）を一定距離遠くへ拡張して、自身の方向からウェイポイントへの方向まで（水色）の範囲にある部分を切り抜きます　→　黒い破線。
黒い破線と接する円形軌跡の半径を求めます（黒い破線にある各点を通る円形の半径の最小値）　→　安全な円形軌跡の半径の最大値になります。
最大半径と、現在角速度或いは出力する予定の回転速度に等価する角速度によって、最大前進速度を計算します　→　速度上限になります。

なぜステップ１でボーダーを拡張するか？

ウェイポイントがボーダーにあるのは常にあること、つまりロボットがボーダーに近い、更に少し越えるのもよくある状態です。そういう状態で、元のボーダーで速度上限を計算すると、結果が０になる事が頻繁で、動きがカクカクします。

理想的な対策としては、ロボットのマージンを２つに分けます。大きい方は回避のウェイポイント計算用、小さい方は衝突範囲を意味し速度上限の計算に使います。するとスキャンが二回になり、計算が二倍重いですので、近似的にボーダーを拡張しています。

解決策の問題

やはりカクカクします。安全性と滑らかさのトレードオフになっている感じですね。

衝突処理

HL 衝突回避アルゴリズムは字面通り衝突を回避する手法であり、ロボットの通信不安定やタイヤの滑りなどによって衝突状態（マージンが重なる状態）になる場合は別で処理する必要があります。

ロボットがある障害物と衝突状態にあると、全ての方向においての f(α) が 0 になって、動けなくなります。

最初の解決策は、次で紹介するボイドのような、衝突物から離れる力を与える――ウェイポイントを衝突物の反対方向に設定するのです。しかし問題は動きが不自然のです。例えば並列走行中の二つのロボットが偶然で近くなって衝突状態になったら、少し方向を修正して元の並列「車線」に戻せばいいのに、大袈裟に横に曲がって離れるようになります。

根本的原因といえば、衝突した障害物がロボットの視線を塞げるのであります。なので衝突していない障害物と衝突した障害物と分けて処理すればいいです。具体的に、

衝突した障害物はサンプル方向に対して更に衝突が深まらないよう禁止範囲（下図の赤い範囲）を与えます；
衝突していない障害物は今まで通りに計算して可動域（緑の部分）を取得します；
可動域から禁止範囲を除けて（緑の下の部分）、ウェイポイント評価の部分に戻ります。

改良版 HL 衝突回避

以上の改良を合わせた HL 衝突回避の pseudocode は以下のようです。

	Algorithm 2 HL Avoiding 改良版
0	while true do
1	方向セットをサンプリングする A = { 現在方向 α0 - Φ + 2 Φ * i / (サンプル数 N - 1) \| i = 0,1,...,N-1 }
2	計算対象となる障害物セット E = { 障害物 e \| e が視野(Φ, H)内にある }
3	for e in E
4	if e と衝突状態にある
5	安全性 g(α \| e) を計算する
6	else
7	for α_i in A
8	f(α_i \| e) を計算する
9	end for
10	f(α \| e) を得る
11	end if
12	end for
13	f(α) = MIN{ f(α \| e) \| e ∈ E, e と衝突していない }
14	g(α) = MIN{ g(α \| e) \| e ∈ E, e と衝突状態にある }
15	ウェイポイント初期化 wp = 現在位置, 評価基 j_min = inf
16	for α in A
17	if g(α) > 0
18	方向 α、距離 f(α) の範囲内、目標と一番近いポイントを計算 → ポイント p(α), 目標との距離 d(α)
19	評価 j(α) = d(α) - 回転ペナルティ
20	if j(α) < j_min
21	j_min = j(α)
22	wp = p(α)
23	end if
24	endif
25	end for
26	速度上限を計算する
27	ウェイポイント wp　と　速度上限　を出力
28	end while

ボイド(boids)

ボイドは鳥の集団行動を擬似するアルゴリズムであります。下図[5]のように、三つの簡単なルールで実現されています。

他個体から離れる
周囲の個体の平均方向に向かう
周囲の個体の平均位置に移動する

この３つのベクトルを合成した結果を、ゲームの場合だと加速度に、本 Navigator の場合は目標座標と目標速度(の大きさ)に設定します。

問題

ボイドの個体は互いに影響しあう、観測と制御が実際に実行されるのとの間にラグが存在する、というのは勿論、ベクトルを加速度か目標座標かにするのが制御システムにおいて積分要素取り入れるのが、振動の原因になります。

そもそも、同じベクトルで偏位(Cohesion)と回転(Alignment)との二種の動きを表すのは、個体の向きは速度ベクトルの方向に決められているという、つまり回転の時間が完全に無視されるのがボイドの前提になっているからであります。

なのでロボット群をゲームの中のボイドのように綺麗に動かすのはかなり難しいです。

HL 衝突回避 + ボイド

モチベーション：

ボイドの群がる特性がほしい
でもボイドの動きの不安定さを除けたい
HL衝突回避の綺麗な動きがほしい

手法:

タイムスケールの角度から考えてみれば、衝突回避はリアルタイムの制御で、群がるのはもっと長い時間レベルの行動なので、ボイド（上）ー衝突回避（下）の二階層構造を思いつきました。

ボイドの出力したベクトルを、一定の比例で目標の座標に足し算し、仮の目標を計算する；
同時に、そのベクトルの長さで、速度係数を計算する；
仮の目標で HL 衝突回避を行う；
ナビゲーターが　ウェイポイントと衝突回避の速度上限とボイドの速度係数を出力する。

下図を例として説明します。ボイドがないと、青いロボットは灰色の障害物見えないので、暫く目標に向けて直進するのです。ボイドが作用する場合、便利なために Alignment を主な部分として考えてみると、青いロボットは灰色のロボットを見えたので、灰色ロボットの速度に合わせようとするベクトルが生じるのです。そして、仮の目標を設定し、衝突回避を行うと、ウェイポイントがボイドなしの場合より上になっています。つまり、青いロボットは障害物を見えないけど、それを見える灰色のロボットの動きを観察しフォローし、障害物を事前に回避することもできます。

まとめ

Navigator のベイスになっていた「ヒューマンライク衝突回避」と「ボイド」は、パスプランニングではなく、各々のロボットが局所的な情報で動いてここまで自然に動けるのは、やはりすごく良いアルゴリズムだなぁと感じています。

しかし、局所的な情報なので、やはり行き止まり、デッドロックなどが発生するのもやむを得ないことだと思います。

衝突回避アルゴリズムに対する各改良においても、良くなるところがある同時に、問題になるところも出て来ます。こういうトレードオフは、実際の応用ケースに応じて調整していくことが大事だと思います。

ウロチョロスは以上の手法を採用しましたが、スケージュールの事情やそもそも台数が少ないなどのため、詳しい実験やチューニングなどを行っていなかったのです。これをベースとして、アルゴリズムの優位性の検証やパラメータの最適化を徹底することで、もっとスムーズで違和感のない制御に改良する余地があるかもしれません（今後の課題）。