何故、経路平滑化が必要なのか？
先日紹介したダイクストラ法やA*を使用すれば、
スタート地点からゴールまでの最短経路を計算することができます。
ダイクストラ法による最短経路探索MATLABプログラム - MY ENIGMA
A*による最短経路探索MATLABプログラム - MY ENIGMA
しかし、上記の記事を見るとわかるとおり、
これらのアルゴリズムで生成された経路は、
最短経路を出力しているだけなので、
経路がカクカクしています。
ロボットによっては、
このようなパスを追従することは難しいという問題があります。
(例えば、ステアリング駆動のロボットの場合、
滑らかなコースしか走行することが難しいです)
そこで、このようなパスを平滑化(Smoothing)する技術が重要になります。
入力された経路を滑らかに修正するアルゴリズムです。
今回はこの経路平滑化(Path Smoothing)の技術の一例を
紹介し、そのMATLABサンプルコードを公開したいと思います。
経路平滑化アルゴリズム
では、経路平準化アルゴリズムの中で
最もシンプルなアルゴリズムの一つを紹介したいと思います。
まず初めに、平準化前のパスの座標値を
とし、平準化後のパスの座標値を
とします。
ここでiはパスの座標値のインデックスです。
ここで、パスの最初と最後の座標値を固定した状態で、
パスを平準化するためには、
平準化後のパスの各座標値間の各距離が小さくなれば、
パスは平準化されると考えます。
つまり、
座標P_iとその前後の座標P_i-1, P_i+1との距離を考えると、
となれば良いということです。
しかし、この評価式通り最適化を実施すると、
平滑化前のパスの始点と終点を繋いだ
下記のようなパスにになってしまいます。
座標間の各距離を最小になるようにすると
直線になるのは当然ですね。
続いて、このように考えます。
先ほどの評価式をできるだけ満たしながらも、
平滑化前のパスからできるだけ離れないパスが、
平準化されたパスではないかと。
つまり、αとβを定数とすると
をできるだけ満たすようなP'_iを
計算すれば良いということです。
αとβはそれぞれの式の最適化の
重みパラメータになります。
では、実際に最適化のアルゴリズムを導出したいと思います。
今回は最適化のアルゴリズムの中でも最もシンプルなものの一つである、
勾配法を利用します。
まず初めにαの項に関しては、
ベクトル P'_i -> P_iが小さくなる方向なので、
と逐次的に更新することで、
ベクトル P'_i -> P_iをαの割合分だけ短くすることができます。
続いて、βの項に関しても同様に、
ベクトルP'_i -> P'_i-1とベクトルP'_i -> P'_i+1
が小さくなる方向なので、
となります。
以上の２つの式を順番に使用して
各パスの点を更新することで、
パスの平準化を実現することができます。
もちろん上記の２つの式を一つにまとめてもOKです。
MATLABよる経路平滑化(Path Smoothing)プログラム
プログラムは下記のGitHubにアップされています。
中身はコードを見るとわかると思いますが、
path変数に平滑化前のx-y座標リストを格納し、
前述の平準化パラメータα、βを
PathSmoothing関数内で指定すると
平準化したパスを出力してくれます。
