MyEnigma

とあるエンジニアのブログです。#Robotics #Programing #C++ #Python #MATLAB #Vim #Mathematics #Book #Movie #Traveling #Mac #iPhone

Pure pursuit アルゴリズムによる経路追従とPythonサンプルプログラム

Fractional Order Differentiation and Robust Control Design: CRONE, H-infinity and Motion Control (Intelligent Systems, Control and Automation: Science and Engineering)

Fractional Order Differentiation and Robust Control Design: CRONE, H-infinity and Motion Control (Intelligent Systems, Control and Automation: Science and Engineering)

目次

はじめに

経路追従は、

経路生成アルゴリズムによって生成されたコースに対して、

できるだけずれ量が小さくなるように

リアルタイム制御する技術です。

myenigma.hatenablog.com

 

経路追従アルゴリズムには、

いくつかありますが、

その中でも最も基本的なものが、

pure pursuitと呼ばれるアルゴリズムで、

今でも広くロボティクスの世界で使われれています。

 

今回はこのpure pursuitアルゴリズムの概要と、

Pythonサンプルプログラムについて

説明したいと思います。

 

Pure pursuitアルゴリズム

Pure pursuitアルゴリズムは、

経路追従アルゴリズムの中で

最も基礎的ですが、非常に広く使われているアルゴリズムです。

 

Pure pursuitアルゴリズムは、

下記の図のようにあるリファレンスコースに対して、

一定距離(Look ahead distance)先の点を目標点(Target point)にし、

その点に到達するような旋回制御を実現します。

pure_pursuit_lookahead1.png (579×189)

 

そして車両が前進すると、

またより先の目標点に辿り着くように制御するため、

結果的にリファレンスパスに追従する形になります。

 

このようなアルゴリズムであることから、

Pure pursuitアルゴリズムは、

Carrot Seek アルゴリズムとも呼ばれます。

 

あるTarget point に対して、

現在の車両との方位誤差αは、

下記の式で計算できます。

f:id:meison_amsl:20170605105424p:plain

 

そこで、Target pointと現在の車両の中心を

円で結んだ場合、その円の旋回半径Rは下記の図と式のように計算できます。

f:id:meison_amsl:20170605105611p:plain

 

あとは、毎回の制御の際に上記の旋回半径になるように、

旋回制御すれば良いということになります。

 

差動二輪式のロボットの場合は、

前進速度vrを使って、

下記のような旋回速度ωになるように、

左右輪を制御すればOKです。

f:id:meison_amsl:20170605105824p:plain

 

ステアリング型のロボットの場合は、

下記の記事で紹介した通り、

myenigma.hatenablog.com

旋回Rで走行したい場合は、車両のトレッドをWとすると、

ステアリング切り角δは、

で計算されるため、

先程の式を代入して、下記の式で目標ステアリング角を計算できます。

 

Pure pursuitアルゴリズムの注意点としては、

車両がバックする際には、

後ろ方向のTarget pointを参照するようにすることと、

車速が上がると、Lの長さが短いと制御安定性が低下するため、

車速に応じてLを変更する必要があるということです。

加えて、旋回コースではアルゴリズムの特性上、

かならず制御偏差が発生してしまいます。

 

詳しくは、下記の論文を参照下さい。

 

Pythonサンプルコード

下記にPythonによるPure pursuitアルゴリズムによる

経路追従のシミュレーションコードを公開しています。

github.com

車両のモデルとしては、

Kinematic Modelを使用し、

myenigma.hatenablog.com

速度の制御にはPIDのP制御、

ステアリングの制御には、上記のPure pursuitアルゴリズムを使用しています。

 

上記のコードで、初期車両位置や速度を設定すると、

下記のようにコースを追従してくれるシミュレーションを実行します。

f:id:meison_amsl:20170605023412p:plain

f:id:meison_amsl:20170605023425p:plain

f:id:meison_amsl:20170605023439p:plain

 

初期状態で後進している時や、

初期状態でまったく別の方向を向いていた時でも

追従できていることがわかります。

 

参考資料

myenigma.hatenablog.com

myenigma.hatenablog.com

myenigma.hatenablog.com

myenigma.hatenablog.com

myenigma.hatenablog.com

Fractional Order Differentiation and Robust Control Design: CRONE, H-infinity and Motion Control (Intelligent Systems, Control and Automation: Science and Engineering)

Fractional Order Differentiation and Robust Control Design: CRONE, H-infinity and Motion Control (Intelligent Systems, Control and Automation: Science and Engineering)