Path Smoothing using adaptive mean filter

確率ロボティクス (プレミアムブックス版)

posted with カエレバ

Sebastian Thrun,Wolfram Burgard,Dieter Fox マイナビ出版 2016-09-21

Amazonで探す

楽天市場で探す

Yahooショッピングで探す

読書メモ

Youtube movie:

以上の動画のように，

環境によって観測値が不安定になりがちなGPSの観測値を

ロボットの目標軌跡として使うために平滑化をするアルゴリズム．

基本的には，前後の時刻のGPSの観測値を利用したMean Filterだが，

曲がり角で軌跡が内側に平滑化されないように，

平均を取るパラメータを軌跡の曲率で動的に変化させている所がミソ．

シンプルで短絡的だが，それなりに機能する．

以下はMATLABのコード

% ----------------------------------------------------
%  Path Smoothing
%
% file         :       PathSmoothing.m
%
% Environment  :       Matlab
% Latest Update:       2009/06/06
%
% Designer(s)  :       A.sakai(AMSL)
% Author(s)    :       A.sakai(AMSL)
%
% CopyRight    :       2009, Autonomous Mobile Systems Laboratory, Meiji Univ.
%
% ----------------------------------------------------------------------------------

function [] = PathSmoothing()

close all;
clear all;
disp('PathSmoothing program start!!')

% Data Load
data=load('tsukuba1200.txt');

x=[data(:,1:2) data(:,6)];
xf=[];
b=100;
ky=5;

for i=b+1:length(x)-b-600
    xadd=x(i,:);
    yaw=0;
    for ii=0:2*b+1
        yaw=yaw+abs(x(i-b+ii,3)-x(i-b+ii+1,3));
    end
  
    bf=b-round(yaw*ky);
    if bf<round(yaw*ky)
        bf=1;
    end
    for j=1:bf
        xadd=xadd+x(i-j,:)+x(i+j,:);
    end
    xadd=xadd/(2*bf+1);
    xf=[xf;xadd];
        
end

figure(1)
plot(x(:,1), x(:,2),'b', 'Linewidth', 5);hold on;
plot(xf(:,1), xf(:,2),'r', 'Linewidth', 2);hold on;
grid on;
axis equal;
title('Path smoothing using adaptive mean filter', 'fontsize', 16, 'fontname', 'times');
legend('Raw Path','Created Path');


function [] = Circleplot(x,y,r)

R=30;
dth=2*pi/R;

Circle=[];
for i=1:R+1
    Circle=[Circle; [x+r*cos(dth*i) y+r*sin(dth*i)]]; 
end
plot(Circle(:,1), Circle(:,2),':c', 'Linewidth', 2);hold on;