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

MyEnigma

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

Path Smoothing using adaptive mean filter

MATLAB Robot

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;