目次
はじめに
経路追従は、
経路生成アルゴリズムによって生成されたコースに対して、
できるだけずれ量が小さくなるように
リアルタイム制御する技術です。
経路追従アルゴリズムには、
いくつかありますが、
その中でも最も基本的なものが、
pure pursuitと呼ばれるアルゴリズムで、
今でも広くロボティクスの世界で使われれています。
今回はこのpure pursuitアルゴリズムの概要と、
Pythonサンプルプログラムについて
説明したいと思います。
Pure pursuitアルゴリズム
Pure pursuitアルゴリズムは、
経路追従アルゴリズムの中で
最も基礎的ですが、非常に広く使われているアルゴリズムです。
Pure pursuitアルゴリズムは、
下記の図のようにあるリファレンスコースに対して、
一定距離(Look ahead distance)先の点を目標点(Target point)にし、
その点に到達するような旋回制御を実現します。
そして車両が前進すると、
またより先の目標点に辿り着くように制御するため、
結果的にリファレンスパスに追従する形になります。
このようなアルゴリズムであることから、
Pure pursuitアルゴリズムは、
Carrot Seek アルゴリズムとも呼ばれます。
あるTarget point に対して、
現在の車両との方位誤差αは、
下記の式で計算できます。
そこで、Target pointと現在の車両の中心を
円で結んだ場合、その円の旋回半径Rは下記の図と式のように計算できます。
あとは、毎回の制御の際に上記の旋回半径になるように、
旋回制御すれば良いということになります。
差動二輪式のロボットの場合は、
前進速度vrを使って、
下記のような旋回速度ωになるように、
左右輪を制御すればOKです。
ステアリング型のロボットの場合は、
下記の記事で紹介した通り、
旋回Rで走行したい場合は、車両のトレッドをWとすると、
ステアリング切り角δは、
で計算されるため、
先程の式を代入して、下記の式で目標ステアリング角を計算できます。
Pure pursuitアルゴリズムの注意点としては、
車両がバックする際には、
後ろ方向のTarget pointを参照するようにすることと、
車速が上がると、Lの長さが短いと制御安定性が低下するため、
車速に応じてLを変更する必要があるということです。
加えて、旋回コースではアルゴリズムの特性上、
かならず制御偏差が発生してしまいます。
また、Lが小さいとコースの誤差をできるだけ小さくなるように制御されますが、
制御が振動しやすくなります。
一方、Lが大きいと滑らかで、制御は振動しづらくなりますが、
コースの追従性能は低くなります。
詳しくは、下記の論文を参照下さい。
Pythonサンプルコード
下記にPythonによるPure pursuitアルゴリズムによる
経路追従のシミュレーションコードを公開しています。
車両のモデルとしては、
Kinematic Modelを使用し、
速度の制御にはPIDのP制御、
ステアリングの制御には、上記のPure pursuitアルゴリズムを使用しています。
上記のコードで、初期車両位置や速度を設定すると、
冒頭の動画や、
下記の図のようにコースを追従してくれるシミュレーションを実行します。
初期状態で後進している時や、
初期状態でまったく別の方向を向いていた時でも
追従できていることがわかります。
参考資料
MyEnigma Supporters
もしこの記事が参考になり、
ブログをサポートしたいと思われた方は、
こちらからよろしくお願いします。
