今回，著作権的にグレーな感じがするので問題があればご一報ください．
専門ではないですが，ロボットの自己位置推定に興味を持ったのでMATLAB/simulinkでシミュレーションしてみました．
ところどころ用語が間違っていると思いますがご容赦ください．
手を付けたきっかけは，“グリッドマップのマッチングに基づく未知障害物にロバストな自己位置推定”という論文の発表を聞いたことです．
面白そうだったので今回は，これを実装したいと思います．
また，今回の記事の大部分はMyEnigmaさんの記事を参考にしています．
パーティクルフィルタやグリッドマップそれぞれについては，こちらがとてもに詳しいのですが，これらを組み合わせた実装について記載がなかったため，本記事で述べていきます．自己位置推定は，ロボットが現在，どこに居るのかを知るための手法です．
見晴らしの良い屋外であればGPSで事足りることが多いかと思いますが，そうでない場合にはロボット自身が現在の位置を知ることは難しくなります．
自己位置推定はよく知るものだとカーナビがありますが，これは普段はGPSで位置を測定していて，トンネル内などのGPSの電波が入らないところでは，タイヤの回転数（回転角）や加速度センサの2回積分（位置）を使って，自己位置を推定しています（多分）．
カーナビでは，トンネル内であまり自己位置の精度が要求されない（トンネル内での分岐は少ない）ことと，道路自体がカーナビ内部にあるので推定された位置が多少ズレていても道路に引き寄せられること，最終的な運転は人間が行うことから，あまり自己位置推定は難しくない/重要でないのでは，と以前は思っていました．ロボットの場合だと，精度の高い自己位置の推定が要求されるため，いろいろな手法が開発されてきたとのことです．あまり詳しくないのでこの辺りは省略します．
今回はその中でもパーティクルフィルタによるグリッドマップマッチングについてシミュレーションをしてみました．
パーティクルフィルタはベイズ推定を利用した手法で，ロボットの仮想的な位置をパーティクルの数だけ生成し，それぞれのモデルからロボットの位置の尤度（ゆうど/もっともらしさ）を推定します．
ぼんやりとした表現では，次のような感じで進むのではないかと思っています（曖昧）．
1. パーティクルの数だけ，ランダムにロボットの仮想位置を作成．（大体の初期位置はわかっていると仮定）
2. ロボットが動いたとき，タイヤの回転角度，車体のyaw方向の角速度などの測定値から，パーティクルごとの現在の位置と向きを推定．
3. ランドマーク（事前に位置を知っている物体）とロボットの距離などの外界の情報と，各パーティクルの推定位置から尤度を算出．
4. 算出された尤度からもっともらしい位置を推定．
ざっくりと言うとこんな感じなのではないかな．
このあたりの理解についてはMyEnigmaさんのこちらの記事を参考にしました．
次にグリッドマップですが，こちらは地図の情報を”障害物がある”/”障害物がない”の2値で表現したものになります．
図では，濃紺の部分が壁（障害物あり），黄色の部分が自由な空間（障害物なし）となっており，500 x 500のグリッドマップとなっています．
こちらも詳細はMyEnigmaさんを参考に．
ここからが論文の本題で，上記のパーティクルフィルタの説明ではランドマークからの距離を直接測定していましたが，実際の現場では特定されているランドマークを測定し続けることは非常に困難です．
MyEnigmaさんでは，RFIDを使い，そのRFIDの番号とそこからの距離が直接測定できると仮定しています．
これは例えば，ロボットを動かしたい地域の周辺に小型のGPSを設置する（LPS:Local positioning systemとでも言うのでしょうか）ことと同義です．これでは，動作範囲全体にわたってLPSを設置するコストもありますし，壁などを直接検知していないという危険性も考えられ，使い勝手が悪そうです．
というわけで，レーザーセンサ（Laser range finderというものがよく使われるらしい）でロボット周囲の地図（障害物）状況だけ把握して，それを事前に得ている地図情報と見比べることで，現在ロボットがどこに居るのか推定するというグリッドマップマッチングが出てくるわけです．
前置きが非常に長くなりました．
このグリッドマップマッチングの手法も様々ですが，今回は上述の“グリッドマップのマッチングに基づく未知障害物にロバストな自己位置推定”を実装します．
この手法の特徴としては，従来手法では壁に埋まっている推定位置が出た場合にはペナルティが課せられるものの，壁から遠ざかる方向ではペナルティが課せられないため，片側にのみ障害物がある場合には精度が低下するのに対して，壁から遠ざかる方向にもペナルティを課すことでそのような条件でも精度が高い，ということのようです．
MATLAB/simulinkでの実装
MATLAB/simulink (R2015a)で実装した内容が以下です．
config.mを実行することで全体のconfigurationと実行が行われます．
図では，Truthが実際の位置，Dead reckoningがデッドレコニングによる推定値，Estがパーティクルフィルタによる推定値です．
また，最適化が一切されていないため，非常に重いです．
20秒のシミュレーションに自分の環境では60秒かかりますがご容赦（笑）
config.m
close all;
clear all;
% サンプリング周期
dSamplingPeriod = 0.1;
%----------------------------%
% グローバルマップの生成 %
% pbm画像からグリッドマップに変換 %
%----------------------------%
fileID = fopen('gridmap.pbm', 'r');
fgets(fileID);
fgets(fileID);
mapSize = fscanf(fileID, '%d %d ', 2);
map.width = mapSize(1);
map.height = mapSize(2);
% create map (test)
map.grid = zeros(map.width, map.height);
for i = 1:map.height
buff = fscanf(fileID, '%c ', map.width);
for j = 1:map.width
map.grid(j, i) = ~str2num(buff(j));
end
end
fclose(fileID);
% 生成したグローバルマップの表示
hoge = pcolor(map.grid');
axis ij square;
set(hoge, 'EdgeColor','none');
% ロボットの観測範囲の設定(100 x 100 grid)
body.width = 100;
body.height = 100;
body.grid = zeros(body.width, body.height);
% 論文の設計パラメータ，貫通率と侵入率
body.Sp = 10; %variance of penetration rate
body.Si = 130; %variance of intrusion rate
% ロボットの運動同定式（状態方程式）
Tt = 1;
Tth = 1;
Kt = 2;
Kth = 1;
A = [-1/Tt 0 0;
0 0 1;
0 0 -1/Tth];
B = [Kt/Tt 0;
0 0;
0 Kth/Tth];
C = [1 0 0;0 1 0];
D = zeros(2,2);
% パーティクル数
NP = 100;
% リサンプリングを実行する数
Nth = NP / 2.0;
% ランダムに配置するパーティクルの分散
Q = diag([0.2 0.2 10/180*pi]).^2;
R = diag([1]).^2;
% 状態方程式の離散化
sysd = c2d(ss(A, B, C, D), dSamplingPeriod);
Ad = sysd.a;
Bd = sysd.b;
Cd = sysd.c;
Dd = sysd.d;
% シミュレーションの実行と実行時間の計測
tic;
sim('./ParticleFilterGridmap.slx');
tocParticleFilterGridmap.slx (simulinkファイル/R2015a)
MATLAB Function (Particle)の中身
function xEst = particle(body, map, u, Q, NP, Nth, dSamplingPeriod, trueState)
persistent px;
% 実際の位置から周囲100 x 100 gridをローカルマップとしてサンプリング
% 実機ではこの部分がLaser range finderなどによる計測値となる
% ロボットの初期位置を(x, y) = (250, 300)としている
% trueState = [x, y, yaw]'
transX = trueState(1)+250;
transY = trueState(2)+300;
rad = trueState(3);
Sth = sin(rad);
Cth = cos(rad);
for i = 1:body.width
for j = 1:body.height
x = fix(transX + (i - body.width/2) * Cth - (j - body.height/2) * Sth);
y = fix(transY + (i - body.width/2) * Sth + (j - body.height/2) * Cth);
if ((x >= 1) && (x <= map.width) && (y >= 1) && (y <= map.height))
body.grid(i, j) = map.grid(x, y);
else
body.grid(i, j) = 0;
end
end
end
% パーティクル格納変数 [x, y, yaw]'がパーティクルの数だけある
if isempty(px)
px = zeros(3, NP);
end
% 重み変数の初期化
pw = ones(1, NP) / NP;
for np = 1:NP
% 1ステップ前の状態xと重みwの取り出し
state = px(:, np);
w = pw(np);
% x = [x;y;yaw]
% fcn(): 次の時刻のロボット位置の予測，randn(): 正規分布の乱数
state = fcn(state, u, dSamplingPeriod) + Q * randn(length(Q),1);
% 運動方程式から予測した状態 state = [x, y, yaw]'
transX = state(1)+250;
transY = state(2)+300;
rad = state(3);
Sth = sin(rad);
Cth = cos(rad);
% 貫通率，侵入率の初期化
penetrationNum = 0;
intrusionNum = 0;
penetrationDen = 0;
for i = 1:body.width
for j = 1:body.height
% 予測されたロボットの状態[x, y, yaw]'のとき，ローカルマップの座標(i, j)がグローバルマップではどの座標(x, y)にあるかの計算
% 単純な同次変換
x = fix(transX + (i - body.width/2) * Cth - (j - body.height/2) * Sth);
y = fix(transY + (i - body.width/2) * Sth + (j - body.height/2) * Cth);
% 同次変換後の座標がグローバルマップ内に存在する場合
if ((x >= 1) && (x <= map.width) && (y >= 1) && (y <= map.height))
% 貫通数と侵入数を計算
penetrationNum = penetrationNum + bitand(bitxor(map.grid(x, y), 1), body.grid(i, j));
intrusionNum = intrusionNum + bitand(map.grid(x, y), bitxor(body.grid(i, j), 1));
% 貫通数の母数を計算（ローカルマップ内の障害物のある空間の全面積）
penetrationDen = penetrationDen + bitxor(map.grid(x, y), 1);
% 同次変換後の座標がグローバルマップの外の場合はグローバルマップ外の範囲を壁扱いとする
else
penetrationDen = penetrationDen + 1;
end
end
end
% penetration/intrusion rate
penetration = penetrationNum / (penetrationDen + 1) * 100;
intrusion = intrusionNum / (body.width * body.height - penetrationDen) * 100;
% likelihood
Pp = 1 / (sqrt(2 * pi) * body.Sp) * exp(-(penetration * penetration) / (2 * body.Sp * body.Sp));
Pi = 1 / (sqrt(2 * pi) * body.Si) * exp(-(intrusion * intrusion) / (2 * body.Si * body.Si));
likelihood = Pp * Pi;
w = w * likelihood;
% パーティクルの更新
px(:, np) = state;
% 重みの更新
pw(np) = w;
end
% ベクトルpwの総和が1になるように正規化
pw = Normalize(pw, NP);
[px, pw] = Resampling(px, pw, Nth, NP); %resampling
xEst = px * pw';
end
% 運動方程式
function x = fcn(x, u, dSamplingPeriod)
F = eye(3);
B = [dSamplingPeriod * cos(x(3)) 0;
dSamplingPeriod * sin(x(3)) 0;
0 dSamplingPeriod];
x = F * x + B * u;
end
% 重みの正規化
function pw = Normalize(pw, NP)
sumw = sum(pw);
if sumw ~= 0
pw = pw / sum(pw);
else
pw = zeros(1,NP) + 1/NP;
end
end
% リサンプリング
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;
resampleID = base + rand/NP;
ppx = px;
ind = 1;
for ip = 1:NP
while(resampleID(ip) > wcum(ind))
ind = ind + 1;
end
px(:,ip) = ppx(:,ind);
pw(ip) = 1/NP;
end
end
end
0 件のコメント:
コメントを投稿