粒子濾波跟蹤移動機器人(MATLAB Robotics System Toolbox)

博客轉載自:http://www.cnblogs.com/21207-iHome/p/7097772.html

MathWorks從MATLAB 2015a開始推出與ROS集成的Robotics System Toolbox （機器人系統工具箱），它為自主移動機器人的研發提供現成的算法和硬件接口。

粒子濾波基本流程

A particle filter is a recursive, Bayesian state estimator that uses discrete particles to approximate the posterior distribution of the estimated state.

The particle filter algorithm computes the state estimate recursively and involves two steps:

• Prediction – The algorithm uses the previous state to predict the current state based on a given system model.

• Correction – The algorithm uses the current sensor measurement to correct the state estimate.

The algorithm also periodically redistributes, or resamples, the particles in the state space to match the posterior distribution of the estimated state.

The estimated state consists of all the state variables. Each particle represents a discrete state hypothesis. The set of all particles is used to help determine the final state estimate.

When using a particle filter, there is a required set of steps to create the particle filter and estimate state. The prediction and correction steps are the main iteration steps for continuously estimating state.

粒子濾波參數

1. 粒子數目：The default number of particles is 1000. Unless performance is an issue, do not use fewer than 1000 particles. A higher number of particles can improve the estimate but sacrifices performance speed, because the algorithm has to process more particles. Tuning the number of particles is the best way to improve the tracking of your particle filter.

2. 初始位置：If you have an initial estimated state, specify it as your initial mean with a covariance relative to your system. This covariance correlates to the uncertainty of your system. The ParticleFilter object distributes particles based on your covariance around the given mean. The algorithm uses this distribution of particles to get the best estimation of state, so an accurate initialization of particles helps to converge to the best state estimation quickly. If an initial state is unknown, you can evenly distribute your particles across a given state bounds. The state bounds are the limits of your state. For example, when estimating the position of a robot, the state bounds are limited to the environment that the robot can actually inhabit. In general, an even distribution of particles is a less efficient way to initialize particles to improve the speed of convergence. The plot shows how the mean and covariance specification can cluster particles much more effectively in a space rather than specifying the full state bounds.

3. 狀態轉移函數：The state transition function, StateTransitionFcn, of a particle filter helps to evolve the particles to the next state. It is used during the prediction step of the Particle Filter Workflow. In the ParticleFilter object, it is specified as a callback function that takes the previous particles, and any other necessary parameters, and outputs the predicted location. By default, the state transition function assumes a Gaussian motion model with constant velocities. The function uses a Gaussian distribution to determine the position of the particles in the next time step. For your application, it is important to have a state transition function that accurately describes how you expect the system to behave. To accurately evolve all the particles, you must develop and implement a motion model for your system. If particles are not distributed around the next state, the ParticleFilter object does not find an accurate estimate. Therefore, it is important to understand how your system can behave so that you can track it accurately.

4. Measurement Likelihood Function：After predicting the next state, you can use measurements from sensors to correct your predicted state. By specifying a MeasurementLikelihoodFcn in the ParticleFilter object, you can correct your predicted particles using the correct function. This measurement likelihood function, by definition, gives a weight for the state hypotheses (your particles) based on a given measurement. Essentially, it gives you the likelihood that the observed measurement actually matches what each particle observes. This likelihood is used as a weight on the predicted particles to help with correcting them and getting the best estimation

5. 狀態估計方法：The final step of the particle filter workflow is the selection of a single state estimate. The particles and their weights sampled across the distribution are used to give the best estimation of the actual state. However, you can use the particles information to get a single state estimate in multiple ways. With the ParticleFilter object, you can either choose the best estimate based on the particle with the highest weight or take a mean of all the particles. Specify the estimtation method in the StateEstimationMethod property as either'mean'(default) or 'maxweight'.

 Track a Robot Using Particle Filter

A remote-controlled robot is being tracked in the outdoor environment. The robot pose measurement is now provided by an on-board GPS, which is noisy. We also know the motion commands sent to the robot, but the robot will not execute the exact commanded motion due to mechanical part slacks and/or model inaccuracy. This example will show how to use robotics.ParticleFilter to reduce the effects of noise in the measurement data and get a more accurate estimation of the pose of the robot. The kinematic model of a car-like robot is described by the following non-linear system. 

移動機器人可以控制線速度和轉向角速度，其位置和姿態(, , )可以通過GPS定位系統或光學捕捉系統等手段獲取。由於存在噪聲因此測量和控制都存在一定誤差（There will be some difference between the commanded motion and the actual motion of the robot）。設系統狀態變量為 [ , , , , ,  ].

機器人轉向時可以看作繞着軸線上的瞬時曲率中心（The point that the robot rotates about is known as the ICC - Instantaneous Center of Curvature）旋轉，曲率半徑為R。假設兩輪間距為L，左右輪速度分別為v_l、v_r，則有：

根據運動學模型，可以得到系統狀態方程，狀態轉移函數的實現如下：

function predictParticles = BotStateTransition(pf, prevParticles, dT, u)
   thetas = prevParticles(:,3);
   w = u(2);
   v = u(1);
   n = length(prevParticles);
   
   % Generate velocity samples
   sd1 = 0.3;  % sd1 represents the uncertainty in the linear velocity
   sd2 = 1.5;  % sd2 represents the uncertainty in the angular velocity
   sd3 = 0.02; % sd3 is an additional perturbation on the orientation
   vh = v + (sd1)^2*randn(n,1);
   wh = w + (sd2)^2*randn(n,1);
   gamma = (sd3)^2*randn(n,1);
   
   % Add a small number to prevent div/0 error
   wh(abs(wh)<1e-19) = 1e-19;
   
   % State Transition
   predictParticles(:,1) = prevParticles(:,1) - (vh./wh).*sin(thetas) + (vh./wh).*sin(thetas + wh*dT);
   predictParticles(:,2) = prevParticles(:,2) + (vh./wh).*cos(thetas) - (vh./wh).*cos(thetas + wh*dT);
   predictParticles(:,3) = prevParticles(:,3) + wh*dT + gamma*dT;
   predictParticles(:,4) = (- (vh./wh).*sin(thetas) + (vh./wh).*sin(thetas + wh*dT))/dT;
   predictParticles(:,5) = ( (vh./wh).*cos(thetas) - (vh./wh).*cos(thetas + wh*dT))/dT;
   predictParticles(:,6) = wh + gamma;
end

然后要實現Measurement Likelihood Function，它會根據預測值與測量值之間的誤差大小來計算每個粒子的權值。與觀測值越接近的粒子權值越大，反之越小。這個例子中粒子是一個N×6的矩陣（N為粒子數目），測量值是1×3的向量。函數的實現如下：

function  likelihood = BotMeasurementLikelihood(pf, predictParticles, measurement)
   % The measurement contains all state variables
   predictMeasurement = predictParticles;

   % Calculate observed error between predicted and actual measurement
   measurementError = bsxfun(@minus, predictMeasurement(:,1:3), measurement); % applies an element-by-element binary operation to arrays
   measurementErrorNorm = sqrt(sum(measurementError.^2, 2));

   % Normal-distributed noise of measurement
   % Assuming measurements on all three pose components have the same error distribution
   measurementNoise = eye(3);

   % Convert error norms into likelihood measure.
   % Evaluate the PDF of the multivariate normal distribution
   likelihood = 1/sqrt((2*pi).^3 * det(measurementNoise)) * exp(-0.5 * measurementErrorNorm);
end

下面是整個Workflow的代碼，其中利用了MATLAB自帶的函數ExampleHelperCarBot來創建移動機器人（該模型與汽車類似，前面兩輪驅動和轉向，本例與該模型存在一定的差異），並利用相關的函數來繪制跟蹤圖像。注意當機器人運動到有屋頂的區域時，由於遮擋GPS信號丟失，返回的測量值為空。

%% Initialize Robot
rng('default'); % for repeatable result
dt = 0.05;      % Time step for simulation of the robot 
initialPose = [0  0  0  0]';
carbot = ExampleHelperCarBot(initialPose, dt);



%% Set up the Particle Filter
pf = robotics.ParticleFilter;

initialize(pf, 5000, [initialPose(1:3)', 0, 0, 0], eye(6), 'CircularVariables',[0 0 1 0 0 0]);
pf.StateEstimationMethod = 'mean';
pf.ResamplingMethod = 'systematic';

% StateTransitionFcn defines how particles evolve without measurement
pf.StateTransitionFcn = @BotStateTransition;

% MeasurementLikelihoodFcn defines how measurement affect the our estimation
pf.MeasurementLikelihoodFcn = @BotMeasurementLikelihood;

% Last best estimation for x, y and theta
lastBestGuess = [0 0 0];



%% Main Loop
% Run loop at 20 Hz for 20 seconds
r = robotics.Rate(1/dt); % The Rate object enables you to run a loop at a fixed frequency
reset(r);                % Reset the fixed-rate object

 % Reset simulation time
 simulationTime = 0;

 while simulationTime < 20 % if time is not up
 　　% Generate motion command that is to be sent to the robot
    　　% NOTE there will be some discrepancy between the commanded motion and the motion actually executed by the robot.
    　　uCmd(1) = 0.7*abs(sin(simulationTime)) + 0.1;  % linear velocity
    　　uCmd(2) = 0.08*cos(simulationTime);           % angular velocity
    　　drive(carbot, uCmd); % Move the robot forward  contaminate commanded motion with noise
    　　% Predict the carbot pose based on the motion model
    　　[statePred, covPred] = predict(pf, dt, uCmd);
　 % Get GPS reading
　 % The GPS reading is just simulated by adding Gaussian Noise to the truth data.
　 % When the robot is in the roofed area, the GPS reading will not be available, the measurement will return an empty matrix.
   measurement = exampleHelperCarBotGetGPSReading(carbot); 

   % If measurement is available, then call correct, otherwise just use predicted result
   if ~isempty(measurement)
       [stateCorrected, covCorrected] = correct(pf, measurement');
   else
       stateCorrected = statePred;  
       covCorrected = covPred;
   end

   lastBestGuess = stateCorrected(1:3);

   % Update plot
   if ~isempty(get(groot,'CurrentFigure')) % if figure is not prematurely killed
       updatePlot(carbot, pf, lastBestGuess, simulationTime);
   else
       break
   end

   waitfor(r);

   % Update simulation time
   simulationTime = simulationTime + dt;
end

可以看到在有屋頂的區域，預測值與機器人真實位置出現了較大的偏差。在機器人走出該區域后，接收到GPS測量信號對預測值進行校正，得到的估計值又收斂到真實位置。

 

參考：

Particle Filter Workflow

Particle Filter Parameters

Track a Car-Like Robot Using Particle Filter

Localize TurtleBot Using Monte Carlo Localization

機器人粒子濾波定位（蒙特卡羅定位）

Kalman-and-Bayesian-Filters-in-Python