1 简介
家庭机器人将成为未来数字化家庭中的重要一员,它不但能自主地完成打扫房间,照顾老人等家务,而且还能看家护院,教育与娱乐孩子,甚至还有管理其它家电产品等功能.近几年来,国内外很多的机器人研究机构以及著名的电器公司都在进行家庭机器人的研制. 家庭机器人属于移动机器人的范畴,它需要在无人干预的条件下,在家庭环境中安全的从一个地方运动到另外一个地方,完成主人分配的任务.本文基于粒子滤波器实现机器人栅格地图路径规划。
2 部分代码
%% Cleanclearclose allclcaddpath(genpath('rbpf_gmapping'))rng('default')%% Setup Parameters% General Parametersts = 1; % Sampling Time (unit: s)num_cells = 10; % Number of Cells (unit: number per meter)num_particles = 3; % Number of Particles (unit: number)num_samples = 10; % Number of Samples (unit: number)cov_odom = [0.01 0; 0 0.001]; % Odometry Covarience (unit: velocity)max_speed = 4; % Max_speed * num_cells (unit: cells/timestep)usable_area = 4*num_cells; % Radius Usable Data (unit: cells)% Controller Parameterscon_kp_v = 0.4; % Velocity Control Commandcon_kp_w = 0.8; % Angular Velocity Control Command% Motion Model Velocity Error Parameters a=[0.1 0.001 0.001 0.1 0.0001 0.0001]; % Laser Range FinderLRF.MaxDistance = 20*num_cells; % Maximum Measurement Distance (unit: cells)LRF.AngleResolution = 0.5; % Measurement Angle Resolution (unit: degree)LRF.FOV = 180; % Measurement Field of View (unit: degree)LRF.MaxAngle = 90; % Maximum Measurement Angle (unit: degree)LRF.Sigma = [0.01, 0.01]; % Range Sensor Deviation (unit: degree; cells)%% Generate Map% Exterior Mapexterior_x = [150,225,225,250,250,150,150,25,25,100,100,150];exterior_y = [25,25,75,75,200,200,275,275,75,75,25,25];% Interior Mapinterior_x_1 = [125,200,200,125,125];interior_y_1 = [50,50,75,75,50];interior_x_2 = [50,125,125,50,50];interior_y_2 = [200,200,250,250,200];interior_x_3 = [50,100,100,50,50];interior_y_3 = [100,100,175,175,100];interior_x_4 = [125,225,225,125,125];interior_y_4 = [100,100,175,175,100];map = [ exterior_x(1:end-1),interior_x_1(1:end-1),interior_x_2(1:end-1),interior_x_3(1:end-1),interior_x_4(1:end-1);... exterior_y(1:end-1),interior_y_1(1:end-1),interior_y_2(1:end-1),interior_y_3(1:end-1),interior_y_4(1:end-1);... exterior_x(2:end),interior_x_1(2:end),interior_x_2(2:end),interior_x_3(2:end),interior_x_4(2:end);... exterior_y(2:end),interior_y_1(2:end),interior_y_2(2:end),interior_y_3(2:end),interior_y_4(2:end)];via_points = [110,40;200,40;210,50;220,90;240,100;240,180;220,190;150,190;140,200;... 140,250;130,260;50,260;40,250;40,200;50,190;100,190;110,180;110,100;100,90;50,90;... 40,100;40,180;50,190;220,190;240,180;240,100;230,90;120,90;110,80;110,40];% Generate Path Using Points and Desired Velocity as Boundary Conditionspath = mstraj(via_points,[max_speed, max_speed],[],[via_points(1,1) via_points(1,2)],ts,0);%% Grid Occupancy Initializationnum_steps = size(path, 1);bound_scale = 1.2; % The Scale of Boundary to The Mapmap_length = max(max(map)) * bound_scale; % Length of the Gridmap_width = map_length; % Width of the Grid init_log = zeros(map_width, map_length); % Initialize the Log Probability of the Map%% Data Initialization% Velocity commands and robot statescon_cmd = zeros(2, num_steps);x_states = zeros(3, num_steps);x0 = [via_points(1,1), via_points(1,2), 0];% Particle Filter StructurePF.Iter = 1; % Iteration NumberPF.NumP = num_particles; % Number of ParticlesPF.Weight = zeros(PF.NumP, num_steps);% Particle WeightsPF.Sigma = LRF.Sigma; % The noise of SensorPF.Particles = zeros(3, PF.NumP, num_steps); % Particles PositionPF.Probability = zeros(map_width, map_length, PF.NumP); % Probability of Grid MapPF.Log = zeros(map_width, map_length, PF.NumP); % Log Odds ProbabilityPF.Log0 = 0; % L0 for Inverse Range Sensor ModelPF.GridSize = [map_length, map_width,]; % Grid SizePF.Xstate = zeros(3, num_steps); % Estimate PositionPF.CovOdom = cov_odom; % Variance Matrix of OdometryPF.UsableArea = usable_area; % Laser Scanner Usable AreaPF.Best = ones(1, num_steps); % Best Particle Index with Highest Weight%% Initializationtic;x_states(:, 1) = x0; % Initialize Particle PositionPF.Xstate(:, 1) = x0; % Initialize Estimatelaser_data = LaserData(PF.Xstate(:, 1), map, LRF);% Initialize the Grid Map[~, L_initial] = GridMapping(init_log, PF.Xstate(:, 1), laser_data, ... PF.UsableArea, PF.Log0, PF.GridSize, LRF);[P_initial, L_initial] = GridMapping(L_initial, PF.Xstate(:, 1), laser_data, ... PF.UsableArea, PF.Log0, PF.GridSize, LRF);% Intialize Variablesfor i = 1:num_particles PF.Probability(:,:,i) = P_initial; PF.Log(:,:,i) = L_initial; PF.Weight(i, 1) = 1/num_particles; PF.Particles(:, i, 1) = x0;end%% Plot Map and pathfig_mapping = figure;set(gcf,'units','normalized','outerposition',[0.1 0.2 0.8 0.6]);subplot(1,3,1)hold onfor d = 1:size(map,2) plot([map(1,d) map(3,d)],[map(2,d) map(4,d)],'k')endgrid onplot(path(:, 1), path(:, 2), 'r')axis([0 300 0 300])axis square%% Start Simulationfor k = 2:num_steps % 1. Compute Error Signals error = sqrt((path(k,1) - x_states(1,k-1))^2 + (path(k,2) - x_states(2,k-1))^2); th = atan2(path(k,2)-x_states(2,k-1), path(k,1)-x_states(1,k-1)); error_th = th - x_states(3, k-1); error_th = mod(error_th + pi, 2*pi) - pi; % 2. Control Signal con_cmd(1,k) = con_kp_v*error; con_cmd(2,k) = con_kp_w*error_th; % 3. True Motion Model x_states(:,k) = MotionCommandModel(x_states(:,k-1),con_cmd(:,k),ts); % 4. LRF Data laser_data = LaserData(x_states(:,k),map,LRF); % 5. The Rao-Blackwellized Particle Filter PF.Iter = k; PF = RaoBlackPF(x_states(:,k),laser_data,LRF,con_cmd(:,k),a,PF,num_samples,ts,fig_mapping);%% Plot the Mapping Procedure figure(fig_mapping) subplot(1,3,2) caxis([0, 1]); colormap gray; colormap(flipud(colormap)); hold on; imagesc(flipud(PF.Probability(:,:,PF.Best(k)))); plot(PF.Xstate(1,k), PF.Xstate(2,k), 'dk') plot(x_states(1,k), x_states(2,k), 'or') axis([0 300 0 300]) axis square hold off pause(0.0001)endrmpath(genpath('rbpf_gmapping'))toc
3 仿真结果
4 参考文献
博主简介：擅长智能优化算法、神经网络预测、信号处理、元胞自动机、图像处理、路径规划、无人机等多种领域的Matlab仿真，相关matlab代码问题可私信交流。
部分理论引用网络文献，若有侵权联系博主删除。
