ロボットの人工ポテンシャル法をざっくり解説する

下書きのまま約1年経ってしまいましたが......

「ロボットやろうぜ！- toio & Unity 作品動画コンテスト -」というのに，人工ポテンシャル法を使ってロボットを制御するシミュレーターアプリを作って出してました．

コンテストに出したアプリについての話はこちら： mu-777.hatenablog.com

人工ポテンシャル法は大学の学生実験か何かでやった記憶を掘り起こして実装したのですが，案外解説みたいな記事がネット上に見当たらなかったので，せっかくですし簡単にまとめてみたいなと思います．

ホントは学生実験のテキストが出てくればよかったのですが......今は手元にありません（たぶん実家のどこかに眠ってるはず）

ということで，上述の通り記憶を掘り起こして書くので間違いも含まれる可能性があります．というか，ちゃんと文献を参照して書いてはいません（面倒なので......）．なので必ずしも正確性を保証するものではありません．ロボット制御ガチ勢からするとツッコミ祭りだと思います......　ご了承ください（免責）

こちらのtoioシミュレータを動かしつつ読むとわかりやすいかなぁと思います：

mu-777.github.io

人工ポテンシャル法は何をするものか
対象をゴールへ誘導するために
仮想的な山/谷はどう表現するのか（数式編）
仮想的な山/谷はどう表現するのか（実装編）
実際にロボットを動かす
まとめ
問題点
さいごに

人工ポテンシャル法は何をするものか

機械工学辞典を見ると

　障害物に正の強い電荷，ゴールに負の強い電荷，さらに，ロボットに正の弱い電荷を人工的に与え，障害物からの斥力とゴールへの引力によりロボットを制御し，障害物を回避しながらロボットをゴールへ誘導させる経路計画法の一種．

とあります．

https://www.jsme.or.jp/jsme-medwiki/14:1006105www.jsme.or.jp

経路計画法というと強そうですが，雑に言うと，ロボットをゴールへ誘導するためにロボットをどう動かすかを決める方法ぐらいの認識でよいかと思います．

対象をゴールへ誘導するために

上述の機械工学辞典は電場的な考え方で書かれていますが，個人的には位置エネルギーのイメージで山/谷を考えるほうがわかりやすいかなぁと思います．

山が複数ある中，1つ谷のように凹んでいるところがある環境を考えてみます．そこにボールを置くと谷の底に向かってコロコロと転がっていくのが想像できるのではないでしょうか？

これを仮想的に実現するのが人工ポテンシャル法という理解でよいかと思います．

つまり，障害物のところに山，ゴールに谷があると仮想的に考えます．その環境においてボールが転がる方向にロボットを動かし続けると，まるでボールが谷間を転がるように，山（＝障害物）を避けつつ谷底（＝ゴール）にロボットを向かわせることができます．

自分のtoioシミュレータでは，赤Cubeを障害物，緑Cubeをゴールとして，仮想の山と谷をメッシュで可視化しています．左上「Start」ボタンを押すと，青Cubeが谷間を縫って緑Cubeの方向に向かいます．

仮想的な山/谷はどう表現するのか（数式編）

前節で「障害物のところに山，ゴールに谷があると仮想的に考えます」と述べましたが，これは，平面上の座標 $\boldsymbol{x}=[x, y$ ] を与えると，その場所の高さを返してくれる関数を用意すればよい，と考えます（以下，2次元環境で考えます）．

この関数は，物理的に言うと「場」というやつです．場 - Wikipediaより参照すると，

物理学において、場は時空の各点に関連する物理量である。場では、座標および時間を指定すれば、（スカラー量、ベクトル量、テンソル量などの）ある一つの物理量が定まる。

とあります．この座標を与えて定まる「物理量」は今回では「高さ」(＝「ポテンシャル」)になります．つまり，「ポテンシャル場」を用意することを考えます．

人工ポテンシャル法において，このポテンシャル場をどう定義するかは様々バリエーションがあるものと思います．今回のtoioシミュレータで採用した，ガウス関数を用いるものを例に説明します．

皆さんご存知かと思いますが，ガウス関数はこんな感じです．

数式で表すと，以下のような感じです（ただし， $A$ ， $\sigma$ は定数）

$f(\boldsymbol{x}) = A \exp \left( \cfrac{\boldsymbol{x} - \boldsymbol{\mu}}{2\sigma^2} \right)$

障害物のある場所とゴールの場所にこれがある（ただしゴールのガウス関数は負方向＝谷です）ような場を定義し，平面上の座標 $\boldsymbol{x}=[x, y$ ] を与えると，その場所の高さ（＝ポテンシャル）を返してくれる関数を作ります．

数式で表すと，以下のような関数を考えます．

$\begin{eqnarray} P(\boldsymbol{x}) &=& A_{obs_1} \exp \left( \cfrac{\boldsymbol{x} - \boldsymbol{\mu_{obs_1}}}{2\sigma_{obs_1}^2} \right) + A_{obs_2} \exp \left( \cfrac{\boldsymbol{x} - \boldsymbol{\mu_{obs_2}}}{2\sigma_{obs_2}^2} \right) \\ &+& \cdots + A_{obs_N} \exp \left( \cfrac{\boldsymbol{x} - \boldsymbol{\mu_{obs_N}}}{2\sigma_{obs_N}^2} \right) + A_{goal} \exp \left( \cfrac{\boldsymbol{x} - \boldsymbol{\mu_{goal}}}{2\sigma_{goal}^2} \right) \end{eqnarray}$

ここで， $\boldsymbol{\mu}_{obs_i}$ は $i$ 番目の障害物の位置座標， $\boldsymbol{\mu}_{goal}$ はゴールの位置座標です．また， $A_{obs_i}$ ， $A_{goal}$ ， $\sigma_{obs_i}$ ， $\sigma_{goal}$ は適当な定数です．（ただし， $A_{obs_i} > 0$ ， $A_{goal} ＜ 0$ ） $A$ は山の高さ・谷の深さを調整でき， $\sigma$ は裾野の広さを調整するパラメータだと思えばよいと思います．

また，この式を見ればわかるように，障害物やゴールが複数あったとしてもそれぞれが作る山/谷の高さを足し合わせればよいだけです．

仮想的な山/谷はどう表現するのか（実装編）

ということで，数式で表現した山/谷を実装に落とし込みます．今回はUnityでの実装ですので，以下に挙げるコードはC#になっています．

今回のtoioシミュレータでは，以下のような感じで実装しています．このGetPotentialFieldValue関数は，引数に位置 $[x, y$ ]（実装ではVector2 uv）を与えると，その場所の高さ（＝ポテンシャル）を計算して返してくれます．

つまり，上記数式の，障害物とゴールの高さを足し合わせる計算をしている部分になります．

    public float GetPotentialFieldValue(Vector2 uv)
    {
        var potential = 0.0f;

        // ゴールの高さを足し込む
        foreach(var attraction in _attractionCubes)
        {
            potential += PotentialMethods.GetPotential(
                            uv, attraction.GetPosInMatUV(), _attractionParam);
        }
        // 山の高さを足し込む
        foreach(var repulsion in _repulsionCubes)
        {
            potential += PotentialMethods.GetPotential(
                            uv, repulsion.GetPosInMatUV(), _repulsionParam);
        }
        return potential;
    }

（実際のアプリ実装から編集してます．詳細はこちら： ToioPotentialBasedControl/PotentialControlManager.cs · GitHub）

_attractionCubesというのがゴールを表す緑Cubeのインスタンスを保持するListで，_repulsionCubesは障害物を表す赤Cubeを保持するListです．

また，肝心の高さ（＝ポテンシャル）を計算しているPotentialMethods.GetPotential関数の実装は以下です．

    public struct GaussianParam
    {
        public float sigma;
        public float scale;
        public GaussianParam(float inSigma, float inScale)
        {
            sigma = inSigma;
            scale = inScale;
        }
    }

    private class Gaussian: PotentialMethod<GaussianParam>
    {
        public float GetPotential(Vector2 targetPos, Vector2 potentialOrigin, GaussianParam param)
        {
            var sigma2 = param.sigma * param.sigma;
            return param.scale * Mathf.Exp(-(targetPos - potentialOrigin).sqrMagnitude / (2.0f * sigma2));
        }
    }

（詳細はこちら：ToioPotentialBasedControl/PotentialMethods.cs · GitHub）

引数のtargetPosは高さがほしいところの位置座標，potentialOriginは障害物 or ゴールの位置座標です．paramはガウス関数の定数を保持します．

これで山/谷の高さを表現し，ある位置における高さ（＝ポテンシャル）を取得できるようになりました．

実際にロボットを動かす

これまでで，平面上の座標 $\boldsymbol{x}=[x, y$ ] を与えるとその場所の高さを返してくれる関数は用意できました．

「で，これでどうロボット動かすの？」となりますが，その方法はポテンシャル場の作り方と同様にバリエーションがあると思います．

今回のtoioシミュレータでも採用した一般的な（と思われる）方法は，上述の通りボールが転がる方向にロボットを動かす，つまり，ポテンシャル場の勾配が底に向かっている方向にロボットを進めるとします．
（勾配の負が力になる，これすなわちポテンシャルです．詳しくはこちら：ポテンシャル - Wikipedia）

そしてポテンシャル場の勾配とは，ポテンシャル場の $grad$ を取る，すなわち偏微分を取ることで算出できます．

では以下で実装を見てみます．今回は以下のように中心差分法による数値微分を採用しました．（中心差分法にした明示的な理由は特にないです）

    public Vector2 GetPotentialDifferential(Vector2 uv)
    {
        var deltaScale = 0.001f;
        var deltaX = new Vector2(deltaScale, 0.0f);
        var deltaY = new Vector2(0.0f, deltaScale);
        var diffX = GetPotentialFieldValue(uv + deltaX) - GetPotentialFieldValue(uv - deltaX);
        var diffY = GetPotentialFieldValue(uv + deltaY) - GetPotentialFieldValue(uv - deltaY);
        return new Vector2(diffX, diffY) / (2.0f * deltaScale);
    }

（詳細はこちら：ToioPotentialBasedControl/PotentialControlManager.cs · GitHub）

解析的に偏微分を計算するのは~~めんどくさかった~~ポテンシャルの定義の方法にバリエーションを持たせる実装にうまく合わせられなかったので，数値微分を採用しました．

この関数で勾配のベクトルを求められたので，これを使ってロボットを制御します．今回は「まぁいけるっしょ」と思って，この勾配ベクトルをロボットに入力する速度ベクトルとしました（ホントは良くないです，たぶん．詳細は以下「問題点」の章で）

具体的な実装は以下です．勾配ベクトルが速度ベクトルになるようにするためにはロボットの車輪速度がどれぐらいになればよいかを計算し，それをロボットに入力します

    IEnumerator ControlLoop()
    {
        var yielder = new WaitForSeconds(_controlLoopMs * 0.001f);
        var center2controlPoint = -0.0038f;
        var wheelRadius = 0.00625f;
        var tread = 0.0266f;
        while(true)
        {
            var diff = -GetPotentialDifferential(targetCube.GetPosInMatUV());
            var theta = targetCube.angle * Mathf.Deg2Rad;
            var v = Mathf.Cos(theta) * diff.x + Mathf.Sin(theta) * diff.y;
            var w = (-Mathf.Sin(theta) * diff.x + Mathf.Cos(theta) * diff.y) / center2controlPoint;
            var w_l = (v + 0.5f * tread) / wheelRadius;
            var w_r = (v - 0.5f * tread) / wheelRadius;
            targetCube.MoveWithRadPerSec(w_l, w_r, (int)(_controlLoopMs * 0.2f), Cube.ORDER_TYPE.Strong);

            yield return yielder;
        }
    }

（詳細はこちら：ToioPotentialBasedControl/PotentialControlManager.cs · GitHub）

車輪速度に変換するために，制御点(center2controlPoint)，車輪半径(wheelRadius)，車輪感距離(tread)を使ってますが，一応こちらの値を参考にしています：

toio.github.io

勾配ベクトルからロボットの車輪速度に変換するところはちょっと本題から逸れるので説明は省きます．（また今後ちゃんと整理して記事にするかも）

また，MoveWithRadPerSec関数については，以下の記事をご覧ください：

mu-777.hatenablog.com

ということで，これでロボットを動かす指令を周期的に送るので，すなわちロボットが動き出します．

まとめ

ホントにこれで動くの？というところですが，一応それっぽく動いているように見えます．

一通りここまでの説明をビジュアライズしてるのが下の動画です．

問題点

ここまでつらつらと書いてきましたが，内容にはいろいろ問題点があるかと思ってます．

ローカルミニマムに簡単に落ちる

動画でも簡単に落ちてますね．局所的な解，今回の例でいうと，山/谷を重ね合わせた結果，ゴールではない場所にも谷のようなスペースができてしまい，ロボットがそこに落ちて身動きとれなくなる感じになります．

回避する方法はさんざん研究されてると思いますが，論文読んだりしていないのでわかんないです．すみません🙏

ポテンシャルの決め方

機械工学辞典で書かれている通りもともとは，ポテンシャルは電荷・電場と同じモデルにするのが正統派なのではないかと勝手に思っています．つまり，障害物とロボットの間の距離の反比例になるようなポテンシャルを定義するということです．

ただ，これだと障害物・ゴールの位置で無限になるので可視化がややこしいのですよね......そういう理由で今回はガウス関数でポテンシャルを表すようにしました．

速度制御になっている

そもそも速度制御にもなってないですけど．．

ポテンシャルの元来の意味からも，ポテンシャル場の勾配ベクトルだけ力をうける，すなわち勾配ベクトルがロボットに付与される加速度だとして制御するべきなのだろうな，と思います．

まぁでもとりあえず勾配ベクトルの負方向に向かわせとけば，それなりには動くというのも「まとめ」項の動画を見てみればわかるかなと思います．

さいごに

ふんわり解説だったので，ロボットガチ勢のガチ解説を期待します！

ひつじTips

技術系いろいろつまみ食います。