制御バリア関数(Control Barrier Function，以下"CBF"とする)とは，制御における制約関数とそれをスマートに守るための制御メソッドです．論文としてはControl Barrier Functions: Theory and Applicationsなどがあります．本記事では、CBFについて少し紹介したあとに、作ったゲームについて説明します。
今回は，CBFを用いてp5.jsでゲームを作ってみました．2D平面上にプレイヤーと数個の敵があります．ユーザーはマウス操作でプレイヤーの移動速度や方向を指定することができますが，その裏で常にCBFの制御メソッドが働いています．CBFは，プレイヤーが敵にぶつかりそうになると，ユーザーによる操作を拒否し，敵にぶつからないようにうま〜くよしなにやってくれます．
ゲームは以下から遊べます!
自分(オレンジ玉)が敵(青玉)に近づこうとすると...
それ以上近づこうとしなくなります(オレンジ玉から出ているオレンジの線は，速度とその向きを表しています．)．自分から敵にぶつかりに行くことができません！
ただ，敵もふよふよ動いていますので，不本意にも敵にハサミ撃ちにあうと，Infeasible(制御不能)が宣告されます．
これは実際に動かすまで分からなかったのですが，自分が敵と敵の間に入ろうとすると，減速します．なんか，狭い路地に入る時の車の振る舞いと似ていますね．もしかすると、徐行や減速という振る舞いもCBFの理論から説明できるのかもしれません．
CBFとは？
ここでは，CBFの理論を少し紹介します．詳しくはControl Barrier Functions: Theory and Applicationsを見て下さい．既にご存じの方は次のセクションまですっ飛ばして下さい．
準備
制御では，制約条件というものを定義することがあります．制約条件とは，例えば今回の例では「敵にぶつからないようにして」などという，プラントに守って欲しい事柄のことをいいます．
CBFでは，そのような制約条件を関数$h$で表し，
- $h\ge0$なら制約条件を守っている状態(安全)
- $h<0$なら制約条件を守っていない状態(安全じゃない)
- $h=0$なら制約条件を守れているか守れていないかの境目の状態
とみなすこととします．例えば「敵にぶつからないようにして」という制約は以下のような式で表せばいいです．
h=(x-x_e)^2+(y-y_e)^2-(r_m+r_e)^2
ただし，プレイヤーの位置を$(x,y)$，半径を$r_m$，敵の位置を$(x_e,y_e)$，半径を$r_e$とします．
問題点
制御では，$h$が負に転じないようにやっていかなければなりません．そのためには，$h$が正であっても自分の行動に色々と注意を払う必要があるのです．
例えば，「敵にぶつからないようにして」という制約について．この制約は，敵にぶつかってさえいなければOKです．では，制約を満たしているのなら，敵のことは全く気にせずに行動していいのか？というと，ちょっと違うと思います．
敵との距離が1mしかない状況を考えます．もうちょっとでぶつかってしまいそうです．もしプレイヤーが僕だったら，多分，否応なく敵から逃げる方向に移動しますね．誰もこの状況で敵に近づこうとはしないはず．触らぬ神に祟りなし．
でも，数学的には，まだぶつかってはいないので，制約を満たして($h>0$)います．だから，何かの工夫がないと，敵から逃げろ！などという意思決定をするのが難しいのです．
このように，制約条件$h>0$を守るため，制約条件$h$が正であっても，必要に応じてスマートに特別な行動(敵と距離を取るなど)を取れるようにしたいです．
CBFが提案すること
CBFの主となるアイデアは，$h$が負に転じること・転じそうになることを予防しようというものです．具体的には，$h$を以下のようなダイナミクスにしちゃいます．
\dot h\ge-\alpha(h)
ただし，$\alpha$は原点を通る単純増加関数．
ダイナミクスというと，状態$x$や入力$u$に組み込まれているものですね．これは普通の制御．CBFでは，それに加えて$h$もダイナミクスにしてしまうんです．お前もダイナミクスになるんだよ！！！！！！
制御システムでは，上の式を解くと入力$u$に関する不等式になります．入力$u$にこの不等式を守らせることで，幸せになれます．
CBFの物理的意味と振る舞い
まず，$\dot h$の符号について確認します．
- $\dot h<0$の時は，制約条件を違反する方向に突き進んでいることを，
- $\dot h>0$の時は，制約条件を守り，かつ，より安全な方向に進んでいること
を示していると考えられます．
次に，以上の式の振る舞いを，敵との距離の例も交えながら考察します．
- $h>>0$の時
制約条件を大いに満たしていて，余裕がある．だから，$\dot h$がある程度負の方向に大きくても大丈夫．
敵から十分に離れているので，近づく方向に動いても大丈夫！ - $h>0$だが$h=0$に近い時
制約条件を満たしているかもしれないが，もう余裕がない．だから，$\dot h$が負になることはできればやめてほしい．
敵との距離があまりないので，これ以上はあまり近づきたくないなあ． - $h<0$の時
制約条件を違反している！制約条件を満たす状態に移行せよ！！
敵とぶつかって重なり合っちゃった❤️❤️❤️！！今すぐ逃げなきゃ！！
何に応用できるの？
まずは，制約条件をスマートに守る安全なロボットを作れるというものがあります．僕の作ったゲームみたいに．また，元々あった制約条件をCBFの考えに基づいて変換することで，より簡単な形にすることもできるそうです．
あとは逆に，ロボットや人間が危険な振る舞いをしていないかどうかを監視するシステムを作れたりするそうです(参考:Trust-based Rate-Tunable Control Barrier Functions for Non-Cooperative Multi-Agent Systems)！
ゲームを作ろう
ということで，CBFを使って，絶対に敵にぶつからないロボットをつくりまーす！！！
以下，自分の位置，半径を$(x,y)$，$r_m$とし，敵$i$の位置，半径を$(x_e,y_e)$，$r_e$とします．
自分の希望速度は，自分とマウスの相対座標で決めることとします．具体的には，$v=(x_{\rm mouse}-x,y_{\rm mouse}-y)$．ここにCBFを適用し，必要に応じて速度にフィルターをかけ，フィルター後の速度$v_f$を自分に適用することにします．フィルターをかけることで，敵にぶつかりそうな場合などにうまくよしなにやってもらおうという算段です．
フィルターは速度の角度ではなく，大きさだけにかけるものとします．具体的には，CBFに基づいたフィルター関数$Filter$を使って，以下のような処理を行ないます．
- 先程の希望速度のベクトル$v$を大きさ$|v|$と角度$\theta$に分ける．
- $|v|_f\leftarrow Filter(|v|)$
- $v_f\leftarrow|v|_f(\cos(\theta), \sin(\theta))$
- フィルター済み速度$v_f$を使って自分の位置を更新する．
では，そのCBFとそれに基づいたフィルター関数$Filter$を定義していきます．今回は「自分と敵がぶつからないように」という制約をかけていくことにします．具体的には，敵$i$に対する制約関数$h_i$を以下のように定義します．
h_i:=(x-x_e^{(i)})^2+(y-y_e^{(i)})^2-(r_m+r_e)^2
今回は以下のようなダイナミクスを考えます．
\dot h_i\ge-\alpha h_i
なお，$\alpha$は任意の正の定数で，ゲーム内でも色々調整できまーす！
ちなみに，$h_i$の時間微分は
\dot h_i=2((x-x_e^{(i)})\cos\theta+(y-y_e^{(i)})\sin\theta)|v|
とします．以上の式をまとめると，速度の大きさ$|v|$は
|v| \ge\{-\alpha (x-x_e^{(i)})^2+(y-y_e^{(i)})^2-(r_m+r_e)^2 \} / \{2((x-x_e^{(i)})\cos\theta+(y-y_e^{(i)})\sin\theta)\}
っていうような制約条件を，敵の数だけ守る必要があります．
よって，CBFに基づいたフィルター関数は，以下のような処理で実装します．
- 希望速度の大きさ$|v|$を受け取る．
- 敵の数だけある制約条件を全て守るように，希望速度$|v|$を動かす．この時，動かすズレ分は最小限にとどめ，なるべく希望にそえるようにする．
- もしも全ての制約条件を守ることができなかった場合，Infeasible(制御不能)を宣告する．
- 動かした後の速度をフィルター済みの速度の大きさ$|v|_f$とする．
そうして動いたのが，冒頭で説明したやつです．簡単な制約条件をCBFの考えに基づいて扱うだけで，敵とぶつからないよう，うまくやってくれるロボットができあがったのが分かります．
まとめ
CBFというものを紹介し，それを使ったゲームを制作しました．CBFは，制約条件を違反する前から，違反しそうになるのを予防するように働くスマートな制御テクニックです．ゲームでは，そんな制御テクニックのおかげで，敵にぶつからないようにスマートに動くロボットを実装することができました．