目次
はじめに
以前、自律ロボットの位置計測のサンプルプログラムとして、
拡張カルマンフィルタを使用したものと、
Unscented Kalman Filterを使用したもの、
Histogram Filterを利用したものを
紹介しましたが、
今回はもう一つ非常に有名なベイズフィルタの一つである
Particle Filterを使って自己位置推定の概要と、
MATLABとPythonのサンプルプログラムを紹介したいと思います。
パーティクルフィルタとは
パーティクルフィルタの詳細に関しては、
下記のwikiや
『確率ロボティクス』を読んで頂きたいのですが、
簡単に概要と、利点&欠点を説明したいと思います。
まずはじめににParticle Filterは
ノンパラメトリックなベイズフィルタです。
カルマンフィルタは、ロボットの状態量の誤差分布を
正規分布だと仮定し、その正規分布のパラメータ(平均と標準偏差)
を推定するので、パラメトリックなベイズフィルタと言われます。
(ベイズの定理についてはこちら:
待ち合わせに遅れる彼女、ベイズの定理、そして例題 - MY ENIGMA)
一方、パーティクルフィルタは、そのように誤差分布を仮定せず、
パーティクルと呼ばれるロボットの位置の仮説を複数生成し、
その仮説の分布を元に、ロボットの位置を計算する方法です。
このパーティクルフィルタは、カルマンフィルタと比較すると
いくつか利点と欠点があります。
利点1 確率分布の近似誤差が少ない
EKFやUKFは確率分布をガウス分布で近似しています。
しかし、実際の確率分布はガウス分布で表せない場合が多いため、
おのずと推定誤差が生じてしまいます。
一方、パーティクルフィルタは
複数のサンプリング点とその重みによって
確率分布を表現するため、
複雑な形状の確率分布における推定精度が高いという利点があります。
しかし、注意しなくてはならないことは、
使用するパーティクルの数が無限個あれば、
理論的には確率分布の近似誤差は非常に小さくなりますが、
パーティクルは有限個なので、
少なからず確率分布の近似誤差は発生します。
しかし、十分な数のパーティクルを使用すれば、
多くの場合、それは無視できるぐらい小さくなります。
利点2 複雑なモデルでも利用できる
カルマンフィルタは、
一般的に観測モデルと呼ばれる、
推定する状態ベクトルから、
センサの観測値を計算する線形モデルが必要です。
しかし実際には、
そのような単純な観測モデルは利用できないこともあります。
(グリッドマップを使用したSLAMなど)
そんな時でも、Particle Filterは非常に便利です。
それぞれのパーティクルの尤度さえ計算できれば、
実装できるからです。
利点3 実装が簡単
パーティクルフィルタはカルマンフィルタと比べると
実装が簡単だと言われます。
カルマンフィルタでは
行列の逆行列などを計算しなければなりませんし、
アルゴリズムの内容を理解することは難しい人も多いと思います。
一方、パーティクルフィルタは非常に簡単にコーディングできますし、
アルゴリズムも直感的にわかりやすいものになっています。
欠点1 計算コストが大きい
Particle Filterの一番の欠点は、計算コストが大きいことです。
安定した推定を実現するためには、
多くのパーティクルが必要になります。
従って、カルマンフィルタと比べるとかなり計算コストが大きくなります。
またそれぞれのパーティクルに対して、
Predict, Updateの処理をする必要があるので、
それぞれの処理が複雑な場合、
計算コストが大きくなってしまいます。
例えば、AMCLなどのアルゴリズムでは、
レーザの大量のScanデータに対して、
尤度の計算をしなくてはらないため、
Updateの処理は複雑になります。
しかし、ICPなどのアルゴリズムを組み合わせて、
できるだけ必要なパーティクルの数を減らす手法も提案されています。
欠点 2 乱数を使っているため毎回同じ結果がでない
パーティクルフィルタは乱数を使っているため、
なんとなく信頼性に不安があるという意見もあります。
また、同じ観測値のデータを使ってシミュレーションをした場合も、
毎回若干結果が変わるのも問題点としてしばしば指摘されます。
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リポジトリでも公開しています。
今回のサンプルプログラムは、GPSの観測値ではなく、
設置位置が既知のRFIDからの距離データを元に
位置計測を行うシミュレーションになっています。
RFIDの数や位置、観測範囲は、
プログラムの冒頭で設定されているので、
色々変えてみると楽しいかもしれません。
Pythonサンプルプログラム
下記は前述とほぼ同じ、RFIDによる距離情報を使った
Particle filterによる自己位置推定推定Pythonサンプルプログラムです。
""" Particle Filter localization sample author: Atsushi Sakai (@Atsushi_twi) """ import numpy as np import math import matplotlib.pyplot as plt # Estimation parameter of PF Q = np.diag([0.1])**2 # range error R = np.diag([1.0, math.radians(40.0)])**2 # input error # Simulation parameter Qsim = np.diag([0.2])**2 Rsim = np.diag([1.0, math.radians(30.0)])**2 DT = 0.1 # time tick [s] SIM_TIME = 50.0 # simulation time [s] MAX_RANGE = 20.0 # maximum observation range # Particle filter parameter NP = 100 # Number of Particle NTh = NP / 2.0 # Number of particle for re-sampling show_animation = True def calc_input(): v = 1.0 # [m/s] yawrate = 0.1 # [rad/s] u = np.matrix([v, yawrate]).T return u def observation(xTrue, xd, u, RFID): xTrue = motion_model(xTrue, u) # add noise to gps x-y z = np.matrix(np.zeros((0, 3))) for i in range(len(RFID[:, 0])): dx = xTrue[0, 0] - RFID[i, 0] dy = xTrue[1, 0] - RFID[i, 1] d = math.sqrt(dx**2 + dy**2) if d <= MAX_RANGE: dn = d + np.random.randn() * Qsim[0, 0] # add noise zi = np.matrix([dn, RFID[i, 0], RFID[i, 1]]) z = np.vstack((z, zi)) # add noise to input ud1 = u[0, 0] + np.random.randn() * Rsim[0, 0] ud2 = u[1, 0] + np.random.randn() * Rsim[1, 1] ud = np.matrix([ud1, ud2]).T xd = motion_model(xd, ud) return xTrue, z, xd, ud def motion_model(x, u): F = np.matrix([[1.0, 0, 0, 0], [0, 1.0, 0, 0], [0, 0, 1.0, 0], [0, 0, 0, 0]]) B = np.matrix([[DT * math.cos(x[2, 0]), 0], [DT * math.sin(x[2, 0]), 0], [0.0, DT], [1.0, 0.0]]) x = F * x + B * u return x def gauss_likelihood(x, sigma): p = 1.0 / math.sqrt(2.0 * math.pi * sigma ** 2) * \ math.exp(-x ** 2 / (2 * sigma ** 2)) return p def calc_covariance(xEst, px, pw): cov = np.matrix(np.zeros((3, 3))) for i in range(px.shape[1]): dx = (px[:, i] - xEst)[0:3] cov += pw[0, i] * dx * dx.T return cov def pf_localization(px, pw, xEst, PEst, z, u): """ Localization with Particle filter """ for ip in range(NP): x = px[:, ip] w = pw[0, ip] # Predict with ramdom input sampling ud1 = u[0, 0] + np.random.randn() * Rsim[0, 0] ud2 = u[1, 0] + np.random.randn() * Rsim[1, 1] ud = np.matrix([ud1, ud2]).T x = motion_model(x, ud) # Calc Inportance Weight for i in range(len(z[:, 0])): dx = x[0, 0] - z[i, 1] dy = x[1, 0] - z[i, 2] prez = math.sqrt(dx**2 + dy**2) dz = prez - z[i, 0] w = w * gauss_likelihood(dz, math.sqrt(Q[0, 0])) px[:, ip] = x pw[0, ip] = w pw = pw / pw.sum() # normalize xEst = px * pw.T PEst = calc_covariance(xEst, px, pw) px, pw = resampling(px, pw) return xEst, PEst, px, pw def resampling(px, pw): """ low variance re-sampling """ Neff = 1.0 / (pw * pw.T)[0, 0] # Effective particle number if Neff < NTh: wcum = np.cumsum(pw) base = np.cumsum(pw * 0.0 + 1 / NP) - 1 / NP resampleid = base + np.random.rand(base.shape[1]) / NP inds = [] ind = 0 for ip in range(NP): while resampleid[0, ip] > wcum[0, ind]: ind += 1 inds.append(ind) px = px[:, inds] pw = np.matrix(np.zeros((1, NP))) + 1.0 / NP # init weight return px, pw def plot_covariance_ellipse(xEst, PEst): Pxy = PEst[0:2, 0:2] eigval, eigvec = np.linalg.eig(Pxy) if eigval[0] >= eigval[1]: bigind = 0 smallind = 1 else: bigind = 1 smallind = 0 t = np.arange(0, 2 * math.pi + 0.1, 0.1) #eigval[bigind] or eiqval[smallind] were occassionally negative numbers extremely #close to 0 (~10^-20), catch these cases and set the respective variable to 0 try: a = math.sqrt(eigval[bigind]) except ValueError: a = 0 try: b = math.sqrt(eigval[smallind]) except ValueError: b = 0 x = [a * math.cos(it) for it in t] y = [b * math.sin(it) for it in t] angle = math.atan2(eigvec[bigind, 1], eigvec[bigind, 0]) R = np.matrix([[math.cos(angle), math.sin(angle)], [-math.sin(angle), math.cos(angle)]]) fx = R * np.matrix([x, y]) px = np.array(fx[0, :] + xEst[0, 0]).flatten() py = np.array(fx[1, :] + xEst[1, 0]).flatten() plt.plot(px, py, "--r") def main(): print(__file__ + " start!!") time = 0.0 # RFID positions [x, y] RFID = np.array([[10.0, 0.0], [10.0, 10.0], [0.0, 15.0], [-5.0, 20.0]]) # State Vector [x y yaw v]' xEst = np.matrix(np.zeros((4, 1))) xTrue = np.matrix(np.zeros((4, 1))) PEst = np.eye(4) px = np.matrix(np.zeros((4, NP))) # Particle store pw = np.matrix(np.zeros((1, NP))) + 1.0 / NP # Particle weight xDR = np.matrix(np.zeros((4, 1))) # Dead reckoning # history hxEst = xEst hxTrue = xTrue hxDR = xTrue while SIM_TIME >= time: time += DT u = calc_input() xTrue, z, xDR, ud = observation(xTrue, xDR, u, RFID) xEst, PEst, px, pw = pf_localization(px, pw, xEst, PEst, z, ud) # store data history hxEst = np.hstack((hxEst, xEst)) hxDR = np.hstack((hxDR, xDR)) hxTrue = np.hstack((hxTrue, xTrue)) if show_animation: plt.cla() for i in range(len(z[:, 0])): plt.plot([xTrue[0, 0], z[i, 1]], [xTrue[1, 0], z[i, 2]], "-k") plt.plot(RFID[:, 0], RFID[:, 1], "*k") plt.plot(px[0, :], px[1, :], ".r") plt.plot(np.array(hxTrue[0, :]).flatten(), np.array(hxTrue[1, :]).flatten(), "-b") plt.plot(np.array(hxDR[0, :]).flatten(), np.array(hxDR[1, :]).flatten(), "-k") plt.plot(np.array(hxEst[0, :]).flatten(), np.array(hxEst[1, :]).flatten(), "-r") plot_covariance_ellipse(xEst, PEst) plt.axis("equal") plt.grid(True) plt.pause(0.001) if __name__ == '__main__': main()
下記のGitHubリポジトリでも公開しています。
上記のコードを実行すると、
記事冒頭のようなシミュレーションが実行されるはずです。
参考資料
- [ymcl/src at master · sugarsweetrobotics/ymcl](https://github.com/sugarsweetrobotics/ymcl/tree/master/src)
myenigma.hatenablog.com
myenigma.hatenablog.com
myenigma.hatenablog.com
myenigma.hatenablog.com
myenigma.hatenablog.com
myenigma.hatenablog.com
myenigma.hatenablog.com
myenigma.hatenablog.com
myenigma.hatenablog.com
myenigma.hatenablog.com
myenigma.hatenablog.com
myenigma.hatenablog.com
myenigma.hatenablog.com
myenigma.hatenablog.com
# MyEnigma Supporters
もしこの記事が参考になり、
ブログをサポートしたいと思われた方は、
こちらからよろしくお願いします。