✅作者简介热爱科研的Matlab仿真开发者擅长毕业设计辅导、数学建模、数据处理、建模仿真、程序设计、完整代码获取、论文复现及科研仿真。 往期回顾关注个人主页Matlab科研工作室 关注我领取海量matlab电子书和数学建模资料个人信条格物致知,完整Matlab代码获取及仿真咨询内容私信。 内容介绍基于 PID 的反馈控制法则通过比例P、积分I、微分D三个环节的协同作用可协调多个自主代理通过不同速度区域并以最短时间到达目标。具体如下比例环节P能根据当前误差快速产生控制作用增大比例增益可提高响应速度。例如当车辆偏离目标速度或位置时比例环节会迅速调整输出让车辆快速向目标靠近。积分环节I主要用于消除稳态误差。在车辆行驶过程中即使比例环节使车辆接近目标但可能仍存在微小偏差积分环节会不断累加该误差随着时间推移产生足够的控制量来抵消偏差确保车辆最终能准确到达目标。微分环节D具有 “预见性”可根据误差变化率调整控制输出抑制超调和振荡增强系统稳定性。如车辆在加速接近目标速度时微分环节能检测到误差减小的趋势提前施加反向作用力减缓加速防止超调使车辆能更平稳、快速地达到目标速度。在实际应用中通过调整 PID 控制器的参数Kp、Ki、Kd可以优化控制性能使多个自主代理在不同速度区域中根据各自的状态和目标实时调整速度和行驶路径从而在避障、适应路况等前提下以最短时间到达目标。⛳️ 运行结果 部分代码classdef ARobotarium handle% AROBOTARIUM This is an interface for the Robotarium class that% ensures the simulator and the robots match up properly. You should% definitely NOT MODIFY this file. Also, dont submit this file.properties (GetAccess protected, SetAccess protected)robot_handle % Handle for the robots patch objectsrobot_body % Base robot body position used for renderingboundary_patch % Path to denote the Robotariums boundaryendproperties (Constant)time_step 0.033max_linear_velocity 0.2robot_diameter 0.11wheel_radius 0.016;base_length 0.105;boundaries [-0.7, 0.7, -0.4, 0.4];endproperties (GetAccess public, SetAccess protected)% Maximum wheel velocitry of the robotsmax_wheel_velocity ARobotarium.max_linear_velocity/ARobotarium.wheel_radius;max_angular_velocity ...2*(ARobotarium.wheel_radius/ARobotarium.robot_diameter) ...*(ARobotarium.max_linear_velocity/ARobotarium.wheel_radius);number_of_robotsfigure_handleendproperties (GetAccess protected, SetAccess protected)velocitiesposesleft_ledsright_leds% Figure handle for simulatorshow_figureendmethods (Abstract)% Gettersget_poses(this)% Initializationinitialize(this, initial_conditions)%Update functionsstep(this);debug(this);endmethodsfunction this ARobotarium(number_of_robots, show_figure, figure_handle)assert(number_of_robots 0 number_of_robots 50, ...Number of robots (%i) must be 0 and 50, number_of_robots);this.number_of_robots number_of_robots;N number_of_robots;this.poses zeros(3, N);this.show_figure show_figure;this.velocities zeros(2, N);this.left_leds zeros(3, N);this.right_leds zeros(3, N);if(show_figure)if(isempty(figure_handle))this.figure_handle figure();elsethis.figure_handle figure_handle;endthis.initialize_visualization();endendfunction agents get_number_of_robots(this)agents this.number_of_robots;endfunction this set_velocities(this, ids, vs)N size(vs, 2);assert(Nthis.number_of_robots, Row size of velocities (%i) must be to number of agents (%i), ...N, this.number_of_robots);this.velocities(:, ids) vs;endfunction this set_left_leds(this, ids, rgbs)N size(rgbs, 2);assert(Nthis.number_of_robots, Row size of rgb values (%i) must be to number of agents (%i), ...N, this.number_of_robots);assert(all(all(rgbs(1:3, :) 255)) all(all(rgbs(1:3, :) 0)), RGB commands must be between 0 and 255);% Only set LED commands for the selected robotsthis.left_leds(:, ids) rgbs;endfunction this set_right_leds(this, ids, rgbs)N size(rgbs, 2);assert(Nthis.number_of_robots, Row size of rgb values (%i) must be to number of agents (%i), ...N, this.number_of_robots);assert(all(all(rgbs(1:3, :) 255)) all(all(rgbs(1:3, :) 0)), RGB commands must be between 0 and 255);% Only set LED commands for the selected robotsthis.right_leds(:, ids) rgbs;endfunction iters time2iters(this, time)iters time / this.time_step;endendmethods (Access protected)function dxu threshold(this, dxu)dxdd this.uni_to_diff(dxu);to_thresh abs(dxdd) this.max_wheel_velocity;dxdd(to_thresh) this.max_wheel_velocity*sign(dxdd(to_thresh));​dxu this.diff_to_uni(dxdd);endfunction dxdd uni_to_diff(this, dxu)r this.wheel_radius;l this.base_length;dxdd [(1/(2*r))*(2*dxu(1, :) - l*dxu(2, :)) ; ...(1/(2*r))*(2*dxu(1, :) l*dxu(2, :))];endfunction dxu diff_to_uni(this, dxdd)r this.wheel_radius;l this.base_length;dxu [r/2*(dxdd(1, :) dxdd(2, :));r/l*(dxdd(2, :) - dxdd(1, :))];endfunction errors validate(this)% VALIDATE meant to be called on each iteration of STEP.% Checks that robots are operating normally.p this.poses;b this.boundaries;N this.number_of_robots;errors {};for i 1:Nx p(1, i);y p(2, i);if(x b(1) || x b(2) || y b(3) || y b(4))errors{end1} RobotariumError.RobotsOutsideBoundaries;endendfor i 1:(N-1)for j i1:Nif(norm(p(1:2, i) - p(1:2, j)) ARobotarium.robot_diameter)errors{end1} RobotariumError.RobotsTooClose;endendenddxdd this.uni_to_diff(this.velocities);exceeding abs(dxdd) this.max_wheel_velocity;if(any(any(exceeding)))errors{end1} RobotariumError.ExceededActuatorLimits;endendend% Visualization methodsmethods (Access protected)% Initializes visualization of GRITSbotsfunction initialize_visualization(this)% Initialize variablesN this.number_of_robots;offset 0.05;% fig figure;% this.figure_handle fig;fig this.figure_handle;% Plot Robotarium boundariesb this.boundaries;boundary_points {[b(1) b(2) b(2) b(1)], [b(3) b(3) b(4) b(4)]};this.boundary_patch patch(XData, boundary_points{1}, ...YData, boundary_points{2}, ...FaceColor, none, ...LineWidth, 3, ...,EdgeColor, [0, 0, 0]);set(fig, color, white);% Set axisax fig.CurrentAxes;% Limit view to xMin/xMax/yMin/yMaxaxis(ax, [this.boundaries(1)-offset, this.boundaries(2)offset, this.boundaries(3)-offset, this.boundaries(4)offset])set(ax, PlotBoxAspectRatio, [1 1 1], DataAspectRatio, [1 1 1])% Store axes% axis(ax, off)axis(ax,on)grid onxlabel(x)ylabel(y)set(ax,units,normalized)ax.YTick -.35:.05:.35;ax.XTick -.6:.1:.6;% Static legendsetappdata(ax, LegendColorbarManualSpace, 1);setappdata(ax, LegendColorbarReclaimSpace, 1);% Apparently, this statement is necessary to avoid issues with% axes reappearing.hold on% Draw areas% slow down zonerectangle(Position,[-.3 -.2 .75 .4],EdgeColor,none,FaceColor,[1 .95 .8])% runway and taxiway for parkingrw_x -.2; rw_y -.05; rw_w .3; rw_h .1; pkw_w 0.2;rectangle(Position,[rw_x rw_y rw_w rw_h],EdgeColor,none,FaceColor,[.86 .86 .86])rectangle(Position,[rw_xrw_w rw_y pkw_w rw_h],EdgeColor,none,FaceColor,[.7 1 .7])% no fly zonerectangle(Position,[rw_x rw_yrw_h rw_wpkw_w rw_h/2],EdgeColor,none,FaceColor,[1 .35 .35])rectangle(Position,[rw_x rw_y-rw_h/2 rw_wpkw_w rw_h/2],EdgeColor,none,FaceColor,[1 .35 .35])rectangle(Position,[rw_xrw_wpkw_w rw_y-rw_h/2 rw_h/2 rw_h*2],EdgeColor,none,FaceColor,[1 .35 .35])% parking positionsplot(rw_xrw_w,0,k*,rw_xrw_w.1,0,k*,rw_xrw_w.2,0,k*)this.robot_handle cell(1, N);for i 1:Ndata gritsbot_patch;this.robot_body data.vertices;x this.poses(1, i);y this.poses(2, i);th this.poses(3, i) - pi/2;rotation_matrix [cos(th) -sin(th) x;sin(th) cos(th) y;0 0 1];transformed this.robot_body*rotation_matrix;this.robot_handle{i} patch(...Vertices, transformed(:, 1:2), ...Faces, data.faces, ...FaceColor, flat, ...FaceVertexCData, data.colors, ...EdgeColor,none);endendfunction draw_robots(this)for i 1:this.number_of_robotsx this.poses(1, i);y this.poses(2, i);th this.poses(3, i) - pi/2;rotation_matrix [...cos(th) -sin(th) x;sin(th) cos(th) y;0 0 1];transformed this.robot_body*rotation_matrix;set(this.robot_handle{i}, Vertices, transformed(:, 1:2));% Set LEDsleft this.left_leds/255;right this.right_leds/255;this.robot_handle{i}.FaceVertexCData(4, :) left(:, i);this.robot_handle{i}.FaceVertexCData(5, :) right(:, i);end​drawnow limitrateendendend​ 参考文献往期回顾扫扫下方二维码