読者です 読者をやめる 読者になる 読者になる

MyEnigma

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

Dynamic Window ApproachのMATLAB サンプルプログラム

MATLAB Robot


はじめに

以前、自律移動ロボットのPath Planningアルゴリズムとして、

ダイクストラ法や

ダイクストラ法による最短経路探索MATLABプログラム - MY ENIGMA

A*アルゴリズム

A*による最短経路探索MATLABプログラム - MY ENIGMA

を紹介しましたが、これらのアルゴリズムは一般的に、

Global Path Planinngというアルゴリズムに分類されます。


このGlobal Path Planningは、

スタート地点からゴールまでのパスを計算する方法ですが、

いくつか問題点があります。


一つ目は、

ロボットのダイナミクス(運動モデル)を考慮していないということです。

標準的なA*アルゴリズムでは、現在のロボットの速度や運動モデルは考慮せず、

単純に隣接グリッド間の移動が最小になるようなパスを計算します。

しかし実際には、ステアリング型のロボットなど、

横方法に簡単に旋回できないロボットの場合、

折角、安全が担保されたパスを生成したのも関わらず、

そのパスに追従できずに、障害物に衝突してしまうということが発生します。


二つ目は、計算コストが大きいということです。

パスを引くスタート地点とゴール地点が遠い場合、

グリッドの解像度にもよりますが、計算コストは莫大になります。

従って、毎回の制御ループ時に

パスを再計算(Replanning)をすることが難しいという問題があります。

これは移動物体などが存在する動的環境での自律移動の際に、

非常に大きな問題になります。

Replanningが間に合わないままのパスで走行すると、

動的物体を考慮しないまま走行してしまうので、

最悪の場合、動的物体に衝突してしまうのです。


以上のようなGlobal Path Planningの問題を解決する方法が

Local Path Planningと呼ばれるアルゴリズムです。


Local Path Planningは、ロボットのダイナミクスを考慮した上で、

非常に近い距離までのパスプランニングを高速に実施するための

アルゴリズムです。

Global Path Planningでは、計算結果としてパス(x-yの位置情報)が

出力されますが、

Local Path Planningでは、車体のダイナミクスを考慮しているため、

最終的な制御入力(前進速度やステアリング角度)まで計算することが多いです。


しかし、Local Path Planningは計算コストの問題で、

非常に近い距離までしかパスを引くことができないので、

局所解(行き止まりなど)でパスが引けなくなるという問題があります。

よって、一般的にはGlobal Path Planningと

Local Path Planningを組み合わせることが多いようです。

(A*で引いたパスを追従するようにLocal Path Planningで制御入力を計算するなど)


今回はLocal Path Planningの代表的なアルゴリズムである

Dynamic Window Approachの簡単な説明と、

MALTAB サンプルプログラムを公開したいと思います。

Dynamic Window Approachとは、

DWAは1997年に有名なロボティクスの教科書である

『確率ロボティクス』の著者の人達が提案した

Local Path Planningアルゴリズムです。


論文リンク:Dynamic Window Approach to Collision Avoidance

確率ロボティクス (Mynavi Advanced Library)

確率ロボティクス (Mynavi Advanced Library)

Probabilistic Robotics (Intelligent Robotics and Autonomous Agents series)

Probabilistic Robotics (Intelligent Robotics and Autonomous Agents series)

  • 作者: Sebastian Thrun,Wolfram Burgard,Dieter Fox
  • 出版社/メーカー: The MIT Press
  • 発売日: 2005/08/19
  • メディア: ハードカバー
  • 購入: 1人 クリック: 6回
  • この商品を含むブログを見る



非常に単純なアルゴリズムでありながらも、

ロボットの運動モデルを考慮したパスを

高速に生成し、最終的なな制御入力まで計算できることから、

非常に多くのロボットで使用されています。


特に、動的環境を走行する屋内ロボットにおいては、

広く利用されており、ROSのデフォルトパッケージである、

Navigationのbase_local_plannerも

DWAを元にしたソフトになっています。

base_local_planner - ROS Wiki



DWAの詳しいアルゴリズムは、

上記の論文とMATLABサンプルプログラムを参照して頂きたいのですが、

簡単にDWAのアルゴリズムの流れを説明したいと思います。


DWAは、現在の位置情報と速度情報、障害物情報、

そしてゴールの位置情報から、

ロボットの運動モデルに則した制御入力を計算してくれます。


今回のMATLABプログラムでは、

一般的な差動二輪運動モデルを元に、

制御入力を前進速度v、回頭速度ωの2つとして

次の時刻に入力すべき最適な制御入力を計算します。


一言でDWAのアルゴリズムを説明すると、

“現在のロボットの速度とロボットの運動モデルから

次の時刻までに取りうるロボットの制御入力の範囲

(Dynamic Window)を計算し、

その範囲内の制御入力をサンプリングして、

事前に決めた評価関数が最大になる制御入力を計算する”

アルゴリズムだと言えます。


もう少し詳細を説明しましょう。

DWAは大きくわけて下記の2つのプロセスから、

安全で最適な制御入力を計算します。

1.Dynamic Windowの計算

2.評価関数の最適化

1. Dynamic WIndowの計算

前述のように、Dynamic Windowとは、

次の時刻にロボットが実施できる

制御入力の範囲のことを言います。


Dynamic Windowは

下記の3種類のWindowのANDを取ったものになります。

下記の論文の図を元に説明したいと思います。



・Window 1: 制御可能範囲

これは、上図でいう所のVsに対応し、

ロボットが取りうる制御入力の最大値と最小値

の範囲を意味します。


そもそもロボットにおいては、モータのスペックなどで

最高速や最高回頭速度が決っているので、

この値を使用することになります。


・Window 2: 動的制御可能範囲

これは上図で言う所のVrに対応し、

現在の速度と、スペック上での最大加減速度を元に計算された

次の時刻までに取りうる最大最小の制御入力のことです。


このように最大加減速を考慮することで、

ロボットのスペックに応じた制御入力を探索することができます。


・Window 3: 障害物による許容制御入力

これは上図で言う所のVaに対応し、

障害物センサによる観測結果と、

ロボットの最大減速度の値から計算される、

安全に走行できる制御入力の範囲になります。


このようなWindowを設定することで、

モデルが正確であれば、障害物にはぶつからない

制御入力のみを探索することができます。


以上の3つの制御入力の範囲(Window)の

重なる部分の制御入力の範囲のみ

を考えることにより、

安全で、かつ、ロボットの運動モデルを考慮した

制御入力のみを利用することができるのです。

2. 評価関数の最適化

続いて、先ほどのDynamic Windowの制御範囲を

ある一定値毎にサンプリングします。

このサンプリング値は、ロボットが制御可能な制御解像度を与えます。

しかし、この解像度を小さくし過ぎると、

計算コストが増大するので注意が必要です。


続いて、それぞれのサンプリング制御値に関して

下記の評価関数を計算します。

まず一つ目の項、heading(v,ω)は、

その制御入力の時のロボットの方位とゴール方向の差の角度を180度から引いた値です。

ゴールに真っ直ぐ向かっている場合は、評価値が大きくなります。


二つ目の項dist(v,ω)は、

その制御入力の時の、最近傍の障害物までの距離を表します。

障害物から遠い制御入力の評価値が大きくなります。


三つ目の項velocity(v,ω)は、

単純に制御入力のvの値を使用します。

速度が早い制御入力の評価値が大きくなります。


α、β、γはそれぞれの項の線形足しあわせの重みパラメータです。

これらのパラメータを調整することにより、

ゴールに向かいつつ、障害物を避けながら、高速に走ることができる

制御入力が逐次的に計算されるようになるのです。


また最後のσはスムージング関数です。

制御入力が近いサンプリング値の評価値の平均などをとって、

滑らかにさせるための関数です。

今回のMATLABのサンプルコードでは、このスムージングは実施していません。


最後にそれぞれの制御サンプリング値において

この評価値を計算し、その中で一番評価値が大きい制御入力を

最適値として採用します。


以上のような方法でDWAでは、

高速に最適な制御入力を計算するのです。


DWAのMATLABサンプルプログラム

% -------------------------------------------------------------------------
%
% File : DynamicWindowApproachSample.m
%
% Discription : Mobile Robot Motion Planning with Dynamic Window Approach
%
% Environment : Matlab
%
% Author : Atsushi Sakai
%
% Copyright (c): 2014 Atsushi Sakai
%
% License : Modified BSD Software License Agreement
% -------------------------------------------------------------------------

function [] = DynamicWindowApproachSample()
 
close all;
clear all;
 
disp('Dynamic Window Approach sample program start!!')

x=[0 0 pi/2 0 0]';%ロボットの初期状態[x(m),y(m),yaw(Rad),v(m/s),ω(rad/s)]
goal=[10,10];%ゴールの位置 [x(m),y(m)]
%障害物リスト [x(m) y(m)]
obstacle=[0 2;
          4 2;
          4 4;
          5 4;
          5 5;
          5 6;
          5 9
          8 8
          8 9
          7 9];
      
obstacleR=0.5;%衝突判定用の障害物の半径
global dt; dt=0.1;%刻み時間[s]

%ロボットの力学モデル
%[最高速度[m/s],最高回頭速度[rad/s],最高加減速度[m/ss],最高加減回頭速度[rad/ss],
% 速度解像度[m/s],回頭速度解像度[rad/s]]
Kinematic=[1.0,toRadian(20.0),0.2,toRadian(50.0),0.01,toRadian(1)];

%評価関数のパラメータ [heading,dist,velocity,predictDT]
evalParam=[0.1,0.2,0.1,3.0];
area=[-1 11 -1 11];%シミュレーションエリアの広さ [xmin xmax ymin ymax]

%シミュレーション結果
result.x=[];
tic;
%movcount=0;
% Main loop
for i=1:5000
    %DWAによる入力値の計算
    [u,traj]=DynamicWindowApproach(x,Kinematic,goal,evalParam,obstacle,obstacleR);
    x=f(x,u);%運動モデルによる移動
    
    %シミュレーション結果の保存
    result.x=[result.x; x'];
    
    %ゴール判定
    if norm(x(1:2)-goal')<0.5
        disp('Arrive Goal!!');break;
    end
    
    %====Animation====
    hold off;
    ArrowLength=0.5;%矢印の長さ
    %ロボット
    quiver(x(1),x(2),ArrowLength*cos(x(3)),ArrowLength*sin(x(3)),'ok');hold on;
    plot(result.x(:,1),result.x(:,2),'-b');hold on;
    plot(goal(1),goal(2),'*r');hold on;
    plot(obstacle(:,1),obstacle(:,2),'*k');hold on;
    %探索軌跡表示
    if ~isempty(traj)
        for it=1:length(traj(:,1))/5
            ind=1+(it-1)*5;
            plot(traj(ind,:),traj(ind+1,:),'-g');hold on;
        end
    end
    axis(area);
    grid on;
    drawnow;
    %movcount=movcount+1;
    %mov(movcount) = getframe(gcf);% アニメーションのフレームをゲットする
end
figure(2)
plot(result.x(:,4));
toc
%movie2avi(mov,'movie.avi');
 

function [u,trajDB]=DynamicWindowApproach(x,model,goal,evalParam,ob,R)
%DWAによる入力値の計算をする関数

%Dynamic Window[vmin,vmax,ωmin,ωmax]の作成
Vr=CalcDynamicWindow(x,model);
%評価関数の計算
[evalDB,trajDB]=Evaluation(x,Vr,goal,ob,R,model,evalParam);

if isempty(evalDB)
    disp('no path to goal!!');
    u=[0;0];return;
end

%各評価関数の正規化
evalDB=NormalizeEval(evalDB);

%最終評価値の計算
feval=[];
for id=1:length(evalDB(:,1))
    feval=[feval;evalParam(1:3)*evalDB(id,3:5)'];
end
evalDB=[evalDB feval];

[maxv,ind]=max(feval);%最も評価値が大きい入力値のインデックスを計算
u=evalDB(ind,1:2)';%評価値が高い入力値を返す

function [evalDB,trajDB]=Evaluation(x,Vr,goal,ob,R,model,evalParam)
%各パスに対して評価値を計算する関数
evalDB=[];
trajDB=[];

for vt=Vr(1):model(5):Vr(2)
    for ot=Vr(3):model(6):Vr(4)
        %軌跡の推定
        [xt,traj]=GenerateTrajectory(x,vt,ot,evalParam(4),model);
        %各評価関数の計算
        heading=CalcHeadingEval(xt,goal);
        dist=CalcDistEval(xt,ob,R);
        vel=abs(vt);
        
        evalDB=[evalDB;[vt ot heading dist vel]];
        trajDB=[trajDB;traj];     
    end
end

function EvalDB=NormalizeEval(EvalDB)
%評価値を正規化する関数
if sum(EvalDB(:,3))~=0
    EvalDB(:,3)=EvalDB(:,3)/sum(EvalDB(:,3));
end
if sum(EvalDB(:,4))~=0
    EvalDB(:,4)=EvalDB(:,4)/sum(EvalDB(:,4));
end
if sum(EvalDB(:,5))~=0
    EvalDB(:,5)=EvalDB(:,5)/sum(EvalDB(:,5));
end

function [x,traj]=GenerateTrajectory(x,vt,ot,evaldt,model)
%軌跡データを作成する関数
global dt;
time=0;
u=[vt;ot];%入力値
traj=x;%軌跡データ
while time<=evaldt
    time=time+dt;%シミュレーション時間の更新
    x=f(x,u);%運動モデルによる推移
    traj=[traj x];
end

function stopDist=CalcBreakingDist(vel,model)
%現在の速度から力学モデルに従って制動距離を計算する関数
global dt;
stopDist=0;
while vel>0
    stopDist=stopDist+vel*dt;%制動距離の計算
    vel=vel-model(3)*dt;%最高原則
end

function dist=CalcDistEval(x,ob,R)
%障害物との距離評価値を計算する関数

dist=2;
for io=1:length(ob(:,1))
    disttmp=norm(ob(io,:)-x(1:2)')-R;%パスの位置と障害物とのノルム誤差を計算
    if dist>disttmp%最小値を見つける
        dist=disttmp;
    end
end

function heading=CalcHeadingEval(x,goal)
%headingの評価関数を計算する関数

theta=toDegree(x(3));%ロボットの方位
goalTheta=toDegree(atan2(goal(2)-x(2),goal(1)-x(1)));%ゴールの方位

if goalTheta>theta
    targetTheta=goalTheta-theta;%ゴールまでの方位差分[deg]
else
    targetTheta=theta-goalTheta;%ゴールまでの方位差分[deg]
end

heading=180-targetTheta;

function Vr=CalcDynamicWindow(x,model)
%モデルと現在の状態からDyamicWindowを計算
global dt;
%車両モデルによるWindow
Vs=[0 model(1) -model(2) model(2)];

%運動モデルによるWindow
Vd=[x(4)-model(3)*dt x(4)+model(3)*dt x(5)-model(4)*dt x(5)+model(4)*dt];

%最終的なDynamic Windowの計算
Vtmp=[Vs;Vd];
Vr=[max(Vtmp(:,1)) min(Vtmp(:,2)) max(Vtmp(:,3)) min(Vtmp(:,4))];
%[vmin,vmax,ωmin,ωmax]

function x = f(x, u)
% Motion Model
global dt;
 
F = [1 0 0 0 0
     0 1 0 0 0
     0 0 1 0 0
     0 0 0 0 0
     0 0 0 0 0];
 
B = [dt*cos(x(3)) 0
    dt*sin(x(3)) 0
    0 dt
    1 0
    0 1];

x= F*x+B*u;

function radian = toRadian(degree)
% degree to radian
radian = degree/180*pi;

function degree = toDegree(radian)
% radian to degree
degree = radian/pi*180;

下記のGitHubリポジトリにて公開しています。

MATLABRobotics/PathPlanning/DynamicWindowApproach/DynamicWindowApproachSample.m at master · AtsushiSakai/MATLABRobotics


使い方はコードをちょっと見てもらえればわかると思いますが、

プログラムの冒頭のパラメータを設定すれば、

障害物を避けるようにDWAで走行するはずです。

ただ、あまりにも障害物を置き過ぎると

すぐに局所解に陥ってしまうので注意しましょう。

その他のロボティクスアルゴリズムのサンプルコード

こちらを参照下さい。

AtsushiSakai/MATLABRobotics


参考資料

確率ロボティクス (Mynavi Advanced Library)

確率ロボティクス (Mynavi Advanced Library)

Probabilistic Robotics (Intelligent Robotics and Autonomous Agents series)

Probabilistic Robotics (Intelligent Robotics and Autonomous Agents series)

  • 作者: Sebastian Thrun,Wolfram Burgard,Dieter Fox
  • 出版社/メーカー: The MIT Press
  • 発売日: 2005/08/19
  • メディア: ハードカバー
  • 購入: 1人 クリック: 6回
  • この商品を含むブログを見る


Dynamic Window Approach to Collision Avoidance