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

MyEnigma

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

Particle Filterを使用した自己位置推定MATLABサンプルプログラム

Robot


はじめに

以前、自律ロボットの位置計測のサンプルプログラムとして、

拡張カルマンフィルタを使用したものと、

拡張カルマンフィルタを使用した自己位置推定MATLABサンプルプログラム - MY ENIGMA

Unscented Kalman Filterを使用したものを

Unscentedカルマンフィルタを使用した自己位置推定MATLABサンプルプログラム - MY ENIGMA

紹介しましたが、

今回はもう一つ非常に有名なParticle Filterによる

自己位置推定MATLABサンプルプログラムを公開したいと思います。

パーティクルフィルタとは

パーティクルフィルタの説明の詳細に関しては、

下記のwikiや

粒子フィルタ - Wikipedia

『確率ロボティクス』を読んで頂きたいのですが、

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

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

Probabilistic Robotics (Intelligent Robotics and Autonomous Agents series)

Probabilistic Robotics (Intelligent Robotics and Autonomous Agents series)

簡単に概要と、利点&欠点を説明したいと思います。


まずはじめににParticle Filterは

ノンパラメトリックなベイズフィルタです。

カルマンフィルタは、ロボットの状態量の誤差分布を

正規分布だと仮定し、その正規分布のパラメータ(平均と標準偏差)

を推定するので、パラメトリックなベイズフィルタと言われます。

(ベイズの定理についてはこちら:

待ち合わせに遅れる彼女、ベイズの定理、そして例題 - MY ENIGMA)


一方、パーティクルフィルタは、そのように誤差分布を仮定せず、

パーティクルと呼ばれるロボットの位置の仮説を複数生成し、

その仮説の分布を元に、ロボットの位置を計算する方法です。


このパーティクルフィルタは、カルマンフィルタと比較すると

いくつか利点と欠点があります。


利点1 確率分布の近似誤差が少ない

EKFやUKFは確率分布をガウス分布で近似しています。

しかし、実際の確率分布はガウス分布で表せない場合が多いため、

自ずと推定誤差が生じてしまいます。


一方、パーティクルフィルタは

複数のサンプリング点とその重みによって

確率分布を表現するため、

複雑な形状の確率分布における推定精度が高いという利点があります。


しかし、注意しなくてはならないことは、

使用するパーティクルの数が無限個あれば、

理論的には確率分布の近似誤差は無くなりますが、

パーティクルは有限個なので、

少なからず確率分布の近似誤差は発生します。

しかし、十分な数のパーティクルを使用すれば、

多くの場合、それは無視できるぐらいになります。

利点2 複雑なモデルでも利用できる

カルマンフィルタは、一般的に観測モデルと呼ばれる、

推定する状態ベクトルから、

センサの観測値を計算するモデルが必要です。


しかし実際には、

そのような単純な観測モデルが利用できないこともあります。

(グリッドマップを使用したSLAMなど)


そんな時、Particle Filterは非常に便利です。

それぞれのパーティクルの尤度さえ計算できれば、

実装できるからです。

利点3 実装が簡単

パーティクルフィルタはカルマンフィルタと比べると

実装が簡単だと言われます。


カルマンフィルタでは行列の逆行列などを駆使しなければ行けませんし、

アルゴリズムの内容を理解することは難しい人も多いと思います。


一方、パーティクルフィルタは非常に簡単にコーディングできますし、

アルゴリズムも直感的にわかりやすいものになっています。


欠点1 計算コストが大きい

Particle Filterの一番の欠点は、計算コストが大きいことです。

安定した推定を実現するためには、

多くのパーティクルが必要になります。

従って、カルマンフィルタと比べるとかなり計算コストが大きくなります。


またそれぞれのパーティクルに対して、

Predict, Updateをする必要があるので、

Predict, Updateの処理が複雑な場合、

より一層計算コストが大きくなってしまいます。

(例えば、AMCLなどのアルゴリズムでは、

レーザの大量のScanデータに対して、

尤度の計算をしなくてはらないため、

Updateの処理は非常に複雑になります。)

しかし、ICPなどのアルゴリズムを組み合わせて、

できるだけ必要なパーティクルの数を減らす手法も提案されています。

ICPアルゴリズムを利用したSLAM用MATLABサンプルプログラム - MY ENIGMA

欠点 2 乱数を使っているため毎回同じ結果がでない

パーティクルフィルタは乱数を使っているため、

なんとなく信頼性に不安があるという意見もあります。


また、同じ観測値のデータを使ってシミュレーションをした場合も、

毎回若干結果が変わるため、

真の意味での性能というものに不安が残るという人もいます。

Particle Filter LocaliationのMATLABサンプルプログラム

下記がMATLABサンプルプログラムです。

% -------------------------------------------------------------------------
%
% File : ParticleFilterLocalization.m
%
% Discription : Mobible robot localization sample code with
%               ParticleFilterLocalization (PF)
%               The Robot can get a range data from RFID that its position
%               known.
%
% Environment : Matlab
%
% Author : Atsushi Sakai
%
% Copyright (c): 2014 Atsushi Sakai
%
% License : Modified BSD Software License Agreement
% -------------------------------------------------------------------------
 
function [] = ParticleFilterLocalization()
 
close all;
clear all;
 
disp('Particle Filter (PF) sample program start!!')
 
time = 0;
endtime = 60; % [sec]
global dt;
dt = 0.1; % [sec]
 
nSteps = ceil((endtime - time)/dt);
 
result.time=[];
result.xTrue=[];
result.xd=[];
result.xEst=[];
result.z=[];
result.PEst=[];
result.u=[];

% State Vector [x y yaw]'
xEst=[0 0 0]';
 
% True State
xTrue=xEst;
 
% Dead Reckoning Result
xd=xTrue;
 
% Covariance Matrix for predict
Q=diag([0.1 0.1 toRadian(3)]).^2;
 
% Covariance Matrix for observation
R=diag([1]).^2;%[range [m]

% Simulation parameter
global Qsigma
Qsigma=diag([0.1 toRadian(5)]).^2;
 
global Rsigma
Rsigma=diag([0.1]).^2;

%RFIFタグの位置 [x, y]
RFID=[10 0;
      10 10;
      0  15
      -5 20];
  
MAX_RANGE=20;%最大観測距離
NP=100;%パーティクル数
NTh=NP/2.0;%リサンプリングを実施する有効パーティクル数

px=repmat(xEst,1,NP);%パーティクル格納変数
pw=zeros(1,NP)+1/NP;%重み変数
 
tic;
%movcount=0;
% Main loop
for i=1 : nSteps
    time = time + dt;
    % Input
    u=doControl(time);
    % Observation
    [z,xTrue,xd,u]=Observation(xTrue, xd, u, RFID, MAX_RANGE);
    
    % ------ Particle Filter --------
    for ip=1:NP
        x=px(:,ip);
        w=pw(ip);
        
        % Dead Reckoning and random sampling
        x=f(x, u)+sqrt(Q)*randn(3,1);
    
        % Calc Inportance Weight
        for iz=1:length(z(:,1))
            pz=norm(x(1:2)'-z(iz,2:3));
            dz=pz-z(iz,1);
            w=w*Gauss(dz,0,sqrt(R));
        end
        px(:,ip)=x;%格納
        pw(ip)=w;
    end
    
    pw=Normalize(pw,NP);%正規化
    [px,pw]=Resampling(px,pw,NTh,NP);%リサンプリング
    xEst=px*pw';%最終推定値は期待値
    
    % Simulation Result
    result.time=[result.time; time];
    result.xTrue=[result.xTrue; xTrue'];
    result.xd=[result.xd; xd'];
    result.xEst=[result.xEst;xEst'];
    result.u=[result.u; u'];
    
    %Animation (remove some flames)
    if rem(i,5)==0 
        hold off;
        arrow=0.5;
        %パーティクル表示
        for ip=1:NP
            quiver(px(1,ip),px(2,ip),arrow*cos(px(3,ip)),arrow*sin(px(3,ip)),'ok');hold on;
        end
        plot(result.xTrue(:,1),result.xTrue(:,2),'.b');hold on;
        plot(RFID(:,1),RFID(:,2),'pk','MarkerSize',10);hold on;
        %観測線の表示
        if~isempty(z)
            for iz=1:length(z(:,1))
                ray=[xTrue(1:2)';z(iz,2:3)];
                plot(ray(:,1),ray(:,2),'-r');hold on;
            end
        end
        plot(result.xd(:,1),result.xd(:,2),'.k');hold on;
        plot(result.xEst(:,1),result.xEst(:,2),'.r');hold on;
        axis equal;
        grid on;
        drawnow;
        %動画を保存する場合
        %movcount=movcount+1;
        %mov(movcount) = getframe(gcf);% アニメーションのフレームをゲットする
    end
   
end
toc

%アニメーション保存
%movie2avi(mov,'movie.avi');

DrawGraph(result);

function [px,pw]=Resampling(px,pw,NTh,NP)
%リサンプリングを実施する関数
%アルゴリズムはLow Variance Sampling
Neff=1.0/(pw*pw');
if Neff<NTh %リサンプリング
    wcum=cumsum(pw);
    base=cumsum(pw*0+1/NP)-1/NP;%乱数を加える前のbase
    resampleID=base+rand/NP;%ルーレットを乱数分増やす
    ppx=px;%データ格納用
    ind=1;%新しいID
    for ip=1:NP
        while(resampleID(ip)>wcum(ind))
            ind=ind+1;
        end
        px(:,ip)=ppx(:,ind);%LVSで選ばれたパーティクルに置き換え
        pw(ip)=1/NP;%尤度は初期化
    end
end

function pw=Normalize(pw,NP)
%重みベクトルを正規化する関数
sumw=sum(pw);
if sumw~=0
    pw=pw/sum(pw);%正規化
else
    pw=zeros(1,NP)+1/NP;
end
    

function p=Gauss(x,u,sigma)
%ガウス分布の確率密度を計算する関数
p=1/sqrt(2*pi*sigma^2)*exp(-(x-u)^2/(2*sigma^2));

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

function u = doControl(time)
%Calc Input Parameter
T=10; % [sec]
 
% [V yawrate]
V=1.0; % [m/s]
yawrate = 5; % [deg/s]
 
u =[ V*(1-exp(-time/T)) toRadian(yawrate)*(1-exp(-time/T))]';

%Calc Observation from noise prameter
function [z, x, xd, u] = Observation(x, xd, u, RFID,MAX_RANGE)
global Qsigma;
global Rsigma;
 
x=f(x, u);% Ground Truth
u=u+sqrt(Qsigma)*randn(2,1);%add Process Noise
xd=f(xd, u);% Dead Reckoning
%Simulate Observation
z=[];
for iz=1:length(RFID(:,1))
    d=norm(RFID(iz,:)-x(1:2)');
    if d<MAX_RANGE %観測範囲内
        z=[z;[d+sqrt(Rsigma)*randn(1,1) RFID(iz,:)]];
    end
end

function []=DrawGraph(result)
%Plot Result
 
figure(1);
hold off;
x=[ result.xTrue(:,1:2) result.xEst(:,1:2)];
set(gca, 'fontsize', 16, 'fontname', 'times');
plot(x(:,1), x(:,2),'-.b','linewidth', 4); hold on;
plot(x(:,3), x(:,4),'r','linewidth', 4); hold on;
plot(result.xd(:,1), result.xd(:,2),'--k','linewidth', 4); hold on;
 
title('PF Localization Result', 'fontsize', 16, 'fontname', 'times');
xlabel('X (m)', 'fontsize', 16, 'fontname', 'times');
ylabel('Y (m)', 'fontsize', 16, 'fontname', 'times');
legend('Ground Truth','PF','Dead Reckoning');
grid on;
axis equal;
 
function radian = toRadian(degree)
% degree to radian
radian = degree/180*pi;

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

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

github.com

 

今回のサンプルプログラムは、GPSの観測値ではなく、

設置位置が既知のRFIDからの距離データを元に

位置計測を行うシミュレーションになっています。

RFIDの数や位置、観測範囲は、

プログラムの冒頭で設定されているので、

色々変えてみると楽しいかもしれません。


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

こちらを参照下さい。

AtsushiSakai/MATLABRobotics