差速移动机器人的滑模轨迹跟踪控制的matlab控制系统设计使用simulink仿真核心控制算法 MATLAB 函数代码用于 Simulink 中的 MATLAB Function 模块。Simulink 模型搭建指南描述如何连接模块。主仿真脚本用于初始化参数、运行仿真并绘图。控制理论基础简述对于差速机器人其运动学模型为{˙cos˙sin˙⎩⎨⎧x˙vcosθy˙vsinθθ˙ω其中(,)(x,y) 是位置θ 是航向角v 是线速度ω 是角速度。滑模控制律设计思路定义跟踪误差,,ex,ey,eθ。通常构建滑模面s 使得系统状态在有限时间内收敛到滑模面并沿其滑动。一种常用的解耦或级联滑模控制策略是线速度控制v 主要消除距离误差。角速度控制ω 主要消除角度误差和横向误差。2. MATLAB 控制算法代码 (用于 Simulink)请在 Simulink 中拖入一个 MATLAB Function 模块将以下代码复制进去。该模块输入期望轨迹和当前状态输出控制量v 和ω 。matlab编辑1function [v_cmd, w_cmd] smc_controller(x_ref, y_ref, theta_ref, x_act, y_act, theta_act, v_ref)2%#codegen3% 差速机器人滑模轨迹跟踪控制器4% 输入:5% x_ref, y_ref, theta_ref: 期望轨迹状态6% x_act, y_act, theta_act: 实际机器人状态7% v_ref: 期望线速度 (前馈项)8% 输出:9% v_cmd: 控制线速度10% w_cmd: 控制角速度1112%% 参数整定 (可根据实际机器人调整)13k_x 2.0; % 纵向误差增益14k_y 3.0; % 横向误差增益15k_theta 5.0; % 角度误差增益16epsilon 0.5; % 切换增益 (克服扰动)17delta 0.1; % 边界层厚度 (用于削弱抖振使用饱和函数代替符号函数)1819%% 1. 坐标变换将全局误差转换到机器人局部坐标系20% 误差在全局坐标系21ex_g x_ref - x_act;22ey_g y_ref - y_act;23etheta_g theta_ref - theta_act;2425% 角度归一化到 [-pi, pi]26while etheta_g pi27 etheta_g etheta_g - 2pi;28end29while etheta_g -pi30 etheta_g etheta_g 2pi;31end3233% 转换到局部坐标系 (Body Frame)34% ex: 纵向误差 (前后), ey: 横向误差 (左右)35ex ex_g * cos(theta_act) ey_g * sin(theta_act);36ey -ex_g * sin(theta_act) ey_g * cos(theta_act);37etheta etheta_g;3839%% 2. 滑模控制律设计40% 定义滑模面 (简化版设计)41% s1 ex (纵向)42% s2 etheta k_y * ey * sign(v_ref) (横向与角度耦合当v0时)4344% 为了避免复杂的耦合推导这里采用一种工程上常用的级联滑模/非线性反馈形式45% 角速度控制 w: 目的是让 etheta - 0 且 ey - 046% 线性部分 切换部分47w_linear k_theta * etheta v_ref * k_y * ey * sinc(etheta); % sinc(x)sin(x)/x, 防止除零近似处理48% 注意sinc(etheta) 在 etheta0 时为 1。此处简化为直接增益或 sin(etheta)/etheta 逻辑49if abs(etheta) 1e-450 sin_term 1.0;51else52 sin_term sin(etheta)/etheta;53end54w_linear k_theta * etheta v_ref * k_y * ey * sin_term;5556% 切换项 (使用饱和函数 sat 代替 sign 以减少抖振)57w_switch epsilon * sat(etheta / delta);5859w_cmd w_linear w_switch;6061% 线速度控制 v: 主要是前馈 纵向误差修正62v_linear v_ref k_x * ex;63v_switch epsilon * sat(ex / delta);64v_cmd v_linear v_switch;6566% 限制输出 (可选模拟电机饱和)67v_max 1.5; % m/s68w_max 1.0; % rad/s69v_cmd max(min(v_cmd, v_max), -v_max);70w_cmd max(min(w_cmd, w_max), -w_max);7172end7374function y sat(u)75% 饱和函数76if u 177 y 1;78elseif u -179 y -1;80else5theta_dot w;6end然后连接三个 Integrator 模块初始条件设为 [0; 0; 0] (假设从原点出发)分别输出,,x,y,θ 。生成参考轨迹 (Reference Generator)可以使用 Clock 信号经过 Math Function (sin/cos) 生成圆形或直线轨迹。例如圆形轨迹cos()xrefRcos(ωreft)sin()yrefRsin(ωreft)/2θrefωreftπ/2 (切线方向)⋅vrefR⋅ωref连接逻辑Clock - Trajectory Gen (计算出,,,xr,yr,θr,vr)。Kinematics 输出的,,x,y,θ (实际状态) 和 Trajectory Gen 输出的参考状态一起送入 SMC Controller。SMC Controller 输出,v,ω 。,v,ω 和实际θ 送入 Kinematics 形成闭环。将,x,y 和,xr,yr1function run_diff_drive_smc_sim()2 % 清除环境3 clear; clc; close all;45 model_name ‘DiffDriveSMC_Model’;67 % 1. 检查并创建 Simulink 模型8 if ~system([‘exist(’‘’, model_name, ‘’‘, ‘‘model’’) 1’])9 fprintf(‘正在创建 Simulink 模型…\n’);10 new_system(model_name);11 open_system(model_name);1213 % — 添加模块 —14 % Clock15 add_block(‘simulink/Sources/Clock’, [model_name ‘/Clock’], ‘Position’, [50 50 80 80]);1617 % Trajectory Generator (MATLAB Function)18 add_block(‘simulink/User-Defined Functions/MATLAB Function’, [model_name ‘/TrajGen’], …19 ‘Position’, [100 40 160 100], ‘String’, ‘traj_gen’);20 set_param([model_name ‘/TrajGen’], ‘FunctionName’, ‘traj_gen’);2122 % SMC Controller (MATLAB Function)23 add_block(‘simulink/User-Defined Functions/MATLAB Function’, [model_name ‘/SMC_Controller’], …24 ‘Position’, [250 40 310 100], ‘String’, ‘smc_controller’);25 % 注意这里的代码内容需要在下面单独设置或者手动粘贴2627 % Kinematics (MATLAB Function)28 add_block(‘simulink/User-Defined Functions/MATLAB Function’, [model_name ‘/Kinematics’], …29 ‘Position’, [400 150 460 210], ‘String’, ‘kinematics_func’);3031 % Integrators (x, y, theta)32 add_block(‘simulink/Continuous/Integrator’, [model_name ‘/Int_x’], ‘Position’, [480 120 510 150], ‘InitialCondition’, ‘0’);33 add_block(‘simulink/Continuous/Integrator’, [model_name ‘/Int_y’], ‘Position’, [480 160 510 190], ‘InitialCondition’, ‘0’);34 add_block(‘simulink/Continuous/Integrator’, [model_name ‘/Int_theta’], ‘Position’, [480 200 510 230], ‘InitialCondition’, ‘0’);3536 % Mux for XY Plot37 add_block(‘simulink/Signal Routing/Mux’, [model_name ‘/Mux_Actual’], ‘Position’, [530 120 550 190], ‘Inputs’, ‘2’);38 add_block(‘simulink/Signal Routing/Mux’, [model_name ‘/Mux_Ref’], ‘Position’, [530 220 550 290], ‘Inputs’, ‘2’);39 add_block(‘simulink/Signal Routing/Mux’, [model_name ‘/Mux_Plot’], ‘Position’, [570 150 590 260], ‘Inputs’, ‘2’);4041 % XY Graph42 add_block(‘simulink/Sinks/XY Graph’, [model_name ‘/XY_Graph’], ‘Position’, [620 150 680 210]);43 set_param([model_name ‘/XY_Graph’], ‘Xmin’, ‘-2’, ‘Xmax’, ‘2’, ‘Ymin’, ‘-2’, ‘Ymax’, ‘2’);4445 % To Workspace (用于后续精细绘图)46 add_block(‘simulink/Sinks/To Workspace’, [model_name ‘/ToWS_Actual’], ‘Position’, [530 50 560 80], ‘VariableName’, ‘sim_actual’);47 add_block(‘simulink/Sinks/To Workspace’, [model_name ‘/ToWS_Ref’], ‘Position’, [530 20 560 50], ‘VariableName’, ‘sim_ref’);4849 % — 连线 —50 % Clock - TrajGen51 add_line(model_name, ‘Clock/1’, ‘TrajGen/1’);5253 % TrajGen - SMC (x_ref, y_ref, theta_ref, x_act, y_act, theta_act, v_ref)54 % 假设 TrajGen 输出 [x_r, y_r, th_r, v_r]55 % 需要反馈实际状态稍后连接5657 % SMC - Kinematics (v, w)58 add_line(model_name, ‘SMC_Controller/1’, ‘Kinematics/1’);59 add_line(model_name, ‘SMC_Controller/2’, ‘Kinematics/2’);6061 % Kinematics - Integrators62 add_line(model_name, ‘Kinematics/1’, ‘Int_x/1’);63 add_line(model_name, ‘Kinematics/2’, ‘Int_y/1’);64 add_line(model_name, ‘Kinematics/3’, ‘Int_theta/1’);6566 % Integrators - Feedback to SMC Mux67 % 构建反馈向量 [x, y, theta]68 add_block(‘simulink/Signal Routing/Mux’, [model_name ‘/Mux_Feedback’], ‘Position’, [530 300 550 360], ‘Inputs’, ‘3’);69 add_line(model_name, ‘Int_x/1’, ‘Mux_Feedback/1’);70 add_line(model_name, ‘Int_y/1’, ‘Mux_Feedback/2’);71 add_line(model_name, ‘Int_theta/1’, ‘Mux_Feedback/3’);728485function pure_code_simulation()86 %% 仿真参数87 T 20; % 仿真总时间 20s88 dt 0.01; % 步长89 t 0:dt:T;90 N length(t);9192 % 初始化状态数组93 % State [x, y, theta]94 X_act zeros(N, 3);95 X_ref zeros(N, 4); % [x, y, theta, v_ref]9697 % 初始状态98 X_act(1, [0.2, -0.2, 0]; % 故意设置初始误差99100 % 轨迹参数 (圆形轨迹)101 R 1.0; % 半径102 w_ref 0.5; % 角速度 rad/s103 v_nominal R * w_ref;104105 % 控制器增益106 k_x 2.0; k_y 3.0; k_theta 5.0;107 epsilon 0.8; delta 0.1;108109 %% 仿真循环110 for i 1:N-1111 ti t(i);112113 % 1. 生成参考轨迹114 X_ref(i, 1) R * cos(w_ref * ti);115 X_ref(i, 2) R * sin(w_ref * ti);116 X_ref(i, 3) w_ref * ti pi/2; % 切线方向117 % 角度归一化118 while X_ref(i, 3) pi, X_ref(i, 3) X_ref(i, 3) - 2pi; end119 while X_ref(i, 3) -pi, X_ref(i, 3) X_ref(i, 3) 2pi; end120 X_ref(i, 4) v_nominal;121122 % 当前实际状态123 x X_act(i, 1);124 y X_act(i, 2);125 theta X_act(i, 3);126127 % 参考状态128 xr X_ref(i, 1);129 yr X_ref(i, 2);130 thr X_ref(i, 3);131 vr_ref X_ref(i, 4);132133 % 2. 计算误差 (全局 - 局部)134 ex_g xr - x;135 ey_g yr - y;136 etheta_g thr - theta;137138 % 角度误差归一化139 while etheta_g pi, etheta_g etheta_g - 2pi; end140 while etheta_g -pi, etheta_g etheta_g 2pi; end141142 ex ex_g * cos(theta) ey_g * sin(theta);143 ey -ex_g * sin(theta) ey_g * cos(theta);144 etheta etheta_g;145146 % 3. 滑模控制律 (SMC)147 % 角速度控制148 sin_term 1.0;149 if abs(etheta) 1e-4, sin_term sin(etheta)/etheta; end150151 w_linear k_theta * etheta vr_ref * k_y * ey * sin_term;152 w_switch epsilon * sat(etheta / delta);153 w_cmd w_linear w_switch;154155 % 线速度控制156 v_linear vr_ref k_x * ex;157 v_switch epsilon * sat(ex / delta);158 v_cmd v_linear v_switch;159160 % 限幅161 v_cmd max(min(v_cmd, 1.5), -1.5);162 w_cmd max(min(w_cmd, 1.0), -1.0);163164 % 4. 运动学更新 (Euler 积分)165 x_dot v_cmd * cos(theta);166 y_dot v_cmd * sin(theta);167 theta_dot w_cmd;168169 X_act(i1, 1) x x_dot * dt;170 X_act(i1, 2) y y_dot * dt;171 X_act(i1, 3) theta theta_dot * dt;172173 % 角度归一化实际状态174 while X_act(i1, 3) pi, X_act(i1, 3) X_act(i1, 3) - 2pi; end175 while X_act(i1, 3) -pi, X_act(i1, 3) X_act(i1, 3) 2pi; end176 end177178 % 补充最后一个参考点179 ti t(end);180 X_ref(end, 1) R * cos(w_ref * ti);181 X_ref(end, 2) R * sin(w_ref * ti);182 X_ref(end, 3) w_ref * ti pi/2;183 X_ref(end, 4) v_nominal;184185 %% 绘图186 figure(‘Color’, ‘w’, ‘Name’, ‘差速机器人滑模轨迹跟踪仿真’);187188 % 轨迹对比189 subplot(1, 2, 1);190 plot(X_ref(:,1), X_ref(:,2), ‘b–’, ‘LineWidth’, 1.5, ‘DisplayName’, ‘参考轨迹’);191 hold on;192 plot(X_act(:,1), X_act(:,2), ‘r-’, ‘LineWidth’, 1.5, ‘DisplayName’, ‘实际轨迹’);193 plot(X_act(1,1), X_act(1,2), ‘go’, ‘MarkerSize’, 8, ‘DisplayName’, ‘起点’);194 plot(X_act(end,1), X_act(end,2), ‘ks’, ‘MarkerSize’, 8, ‘DisplayName’, ‘终点’);195 axis equal; grid on;196 xlabel(‘X (m)’); ylabel(‘Y (m)’);197 title(‘XY 平面轨迹跟踪’);198 legend(‘Location’, ‘best’);199200 % 误差曲线201 subplot(1, 2, 2);202 ex_err X_ref(:,1) - X_act(:,1);203 ey_err X_ref(:,2) - X_act(:,2);204 plot(t, ex_err, ‘r’, ‘DisplayName’, ‘X 轴误差’);205 hold on;206 plot(t, ey_err, ‘g’, ‘DisplayName’, ‘Y 轴误差’);207 % 计算角度误差208 e_th zeros(N,1);209 for i1:N210 err X_ref(i,3) - X_act(i,3);211 while err pi, err err - 2pi; end212 while err -pi, err err 2pi; end213 e_th(i) err;214 end215 plot(t, e_th, ‘b’, ‘DisplayName’, ‘角度误差 (rad)’);216 grid on;217 xlabel(‘Time (s)’); ylabel(‘Error’);218 title(‘跟踪误差收敛曲线’);219 legend(‘Location’, ‘best’);220221 sgtitle([‘滑模控制 (SMC) 仿真结果 (K_x’ num2str(k_x) ‘, K_\theta’ num2str(k_theta) ‘)’]);222end223224function y sat(u)225 if u 1226 y 1;227 elseif u -1228 y -1;229 else230 y u;231 end232end
差速移动机器人的滑模轨迹跟踪控制的matlab控制系统设计，使用simulink仿真
差速移动机器人的滑模轨迹跟踪控制的matlab控制系统设计使用simulink仿真核心控制算法 MATLAB 函数代码用于 Simulink 中的 MATLAB Function 模块。Simulink 模型搭建指南描述如何连接模块。主仿真脚本用于初始化参数、运行仿真并绘图。控制理论基础简述对于差速机器人其运动学模型为{˙cos˙sin˙⎩⎨⎧x˙vcosθy˙vsinθθ˙ω其中(,)(x,y) 是位置θ 是航向角v 是线速度ω 是角速度。滑模控制律设计思路定义跟踪误差,,ex,ey,eθ。通常构建滑模面s 使得系统状态在有限时间内收敛到滑模面并沿其滑动。一种常用的解耦或级联滑模控制策略是线速度控制v 主要消除距离误差。角速度控制ω 主要消除角度误差和横向误差。2. MATLAB 控制算法代码 (用于 Simulink)请在 Simulink 中拖入一个 MATLAB Function 模块将以下代码复制进去。该模块输入期望轨迹和当前状态输出控制量v 和ω 。matlab编辑1function [v_cmd, w_cmd] smc_controller(x_ref, y_ref, theta_ref, x_act, y_act, theta_act, v_ref)2%#codegen3% 差速机器人滑模轨迹跟踪控制器4% 输入:5% x_ref, y_ref, theta_ref: 期望轨迹状态6% x_act, y_act, theta_act: 实际机器人状态7% v_ref: 期望线速度 (前馈项)8% 输出:9% v_cmd: 控制线速度10% w_cmd: 控制角速度1112%% 参数整定 (可根据实际机器人调整)13k_x 2.0; % 纵向误差增益14k_y 3.0; % 横向误差增益15k_theta 5.0; % 角度误差增益16epsilon 0.5; % 切换增益 (克服扰动)17delta 0.1; % 边界层厚度 (用于削弱抖振使用饱和函数代替符号函数)1819%% 1. 坐标变换将全局误差转换到机器人局部坐标系20% 误差在全局坐标系21ex_g x_ref - x_act;22ey_g y_ref - y_act;23etheta_g theta_ref - theta_act;2425% 角度归一化到 [-pi, pi]26while etheta_g pi27 etheta_g etheta_g - 2pi;28end29while etheta_g -pi30 etheta_g etheta_g 2pi;31end3233% 转换到局部坐标系 (Body Frame)34% ex: 纵向误差 (前后), ey: 横向误差 (左右)35ex ex_g * cos(theta_act) ey_g * sin(theta_act);36ey -ex_g * sin(theta_act) ey_g * cos(theta_act);37etheta etheta_g;3839%% 2. 滑模控制律设计40% 定义滑模面 (简化版设计)41% s1 ex (纵向)42% s2 etheta k_y * ey * sign(v_ref) (横向与角度耦合当v0时)4344% 为了避免复杂的耦合推导这里采用一种工程上常用的级联滑模/非线性反馈形式45% 角速度控制 w: 目的是让 etheta - 0 且 ey - 046% 线性部分 切换部分47w_linear k_theta * etheta v_ref * k_y * ey * sinc(etheta); % sinc(x)sin(x)/x, 防止除零近似处理48% 注意sinc(etheta) 在 etheta0 时为 1。此处简化为直接增益或 sin(etheta)/etheta 逻辑49if abs(etheta) 1e-450 sin_term 1.0;51else52 sin_term sin(etheta)/etheta;53end54w_linear k_theta * etheta v_ref * k_y * ey * sin_term;5556% 切换项 (使用饱和函数 sat 代替 sign 以减少抖振)57w_switch epsilon * sat(etheta / delta);5859w_cmd w_linear w_switch;6061% 线速度控制 v: 主要是前馈 纵向误差修正62v_linear v_ref k_x * ex;63v_switch epsilon * sat(ex / delta);64v_cmd v_linear v_switch;6566% 限制输出 (可选模拟电机饱和)67v_max 1.5; % m/s68w_max 1.0; % rad/s69v_cmd max(min(v_cmd, v_max), -v_max);70w_cmd max(min(w_cmd, w_max), -w_max);7172end7374function y sat(u)75% 饱和函数76if u 177 y 1;78elseif u -179 y -1;80else5theta_dot w;6end然后连接三个 Integrator 模块初始条件设为 [0; 0; 0] (假设从原点出发)分别输出,,x,y,θ 。生成参考轨迹 (Reference Generator)可以使用 Clock 信号经过 Math Function (sin/cos) 生成圆形或直线轨迹。例如圆形轨迹cos()xrefRcos(ωreft)sin()yrefRsin(ωreft)/2θrefωreftπ/2 (切线方向)⋅vrefR⋅ωref连接逻辑Clock - Trajectory Gen (计算出,,,xr,yr,θr,vr)。Kinematics 输出的,,x,y,θ (实际状态) 和 Trajectory Gen 输出的参考状态一起送入 SMC Controller。SMC Controller 输出,v,ω 。,v,ω 和实际θ 送入 Kinematics 形成闭环。将,x,y 和,xr,yr1function run_diff_drive_smc_sim()2 % 清除环境3 clear; clc; close all;45 model_name ‘DiffDriveSMC_Model’;67 % 1. 检查并创建 Simulink 模型8 if ~system([‘exist(’‘’, model_name, ‘’‘, ‘‘model’’) 1’])9 fprintf(‘正在创建 Simulink 模型…\n’);10 new_system(model_name);11 open_system(model_name);1213 % — 添加模块 —14 % Clock15 add_block(‘simulink/Sources/Clock’, [model_name ‘/Clock’], ‘Position’, [50 50 80 80]);1617 % Trajectory Generator (MATLAB Function)18 add_block(‘simulink/User-Defined Functions/MATLAB Function’, [model_name ‘/TrajGen’], …19 ‘Position’, [100 40 160 100], ‘String’, ‘traj_gen’);20 set_param([model_name ‘/TrajGen’], ‘FunctionName’, ‘traj_gen’);2122 % SMC Controller (MATLAB Function)23 add_block(‘simulink/User-Defined Functions/MATLAB Function’, [model_name ‘/SMC_Controller’], …24 ‘Position’, [250 40 310 100], ‘String’, ‘smc_controller’);25 % 注意这里的代码内容需要在下面单独设置或者手动粘贴2627 % Kinematics (MATLAB Function)28 add_block(‘simulink/User-Defined Functions/MATLAB Function’, [model_name ‘/Kinematics’], …29 ‘Position’, [400 150 460 210], ‘String’, ‘kinematics_func’);3031 % Integrators (x, y, theta)32 add_block(‘simulink/Continuous/Integrator’, [model_name ‘/Int_x’], ‘Position’, [480 120 510 150], ‘InitialCondition’, ‘0’);33 add_block(‘simulink/Continuous/Integrator’, [model_name ‘/Int_y’], ‘Position’, [480 160 510 190], ‘InitialCondition’, ‘0’);34 add_block(‘simulink/Continuous/Integrator’, [model_name ‘/Int_theta’], ‘Position’, [480 200 510 230], ‘InitialCondition’, ‘0’);3536 % Mux for XY Plot37 add_block(‘simulink/Signal Routing/Mux’, [model_name ‘/Mux_Actual’], ‘Position’, [530 120 550 190], ‘Inputs’, ‘2’);38 add_block(‘simulink/Signal Routing/Mux’, [model_name ‘/Mux_Ref’], ‘Position’, [530 220 550 290], ‘Inputs’, ‘2’);39 add_block(‘simulink/Signal Routing/Mux’, [model_name ‘/Mux_Plot’], ‘Position’, [570 150 590 260], ‘Inputs’, ‘2’);4041 % XY Graph42 add_block(‘simulink/Sinks/XY Graph’, [model_name ‘/XY_Graph’], ‘Position’, [620 150 680 210]);43 set_param([model_name ‘/XY_Graph’], ‘Xmin’, ‘-2’, ‘Xmax’, ‘2’, ‘Ymin’, ‘-2’, ‘Ymax’, ‘2’);4445 % To Workspace (用于后续精细绘图)46 add_block(‘simulink/Sinks/To Workspace’, [model_name ‘/ToWS_Actual’], ‘Position’, [530 50 560 80], ‘VariableName’, ‘sim_actual’);47 add_block(‘simulink/Sinks/To Workspace’, [model_name ‘/ToWS_Ref’], ‘Position’, [530 20 560 50], ‘VariableName’, ‘sim_ref’);4849 % — 连线 —50 % Clock - TrajGen51 add_line(model_name, ‘Clock/1’, ‘TrajGen/1’);5253 % TrajGen - SMC (x_ref, y_ref, theta_ref, x_act, y_act, theta_act, v_ref)54 % 假设 TrajGen 输出 [x_r, y_r, th_r, v_r]55 % 需要反馈实际状态稍后连接5657 % SMC - Kinematics (v, w)58 add_line(model_name, ‘SMC_Controller/1’, ‘Kinematics/1’);59 add_line(model_name, ‘SMC_Controller/2’, ‘Kinematics/2’);6061 % Kinematics - Integrators62 add_line(model_name, ‘Kinematics/1’, ‘Int_x/1’);63 add_line(model_name, ‘Kinematics/2’, ‘Int_y/1’);64 add_line(model_name, ‘Kinematics/3’, ‘Int_theta/1’);6566 % Integrators - Feedback to SMC Mux67 % 构建反馈向量 [x, y, theta]68 add_block(‘simulink/Signal Routing/Mux’, [model_name ‘/Mux_Feedback’], ‘Position’, [530 300 550 360], ‘Inputs’, ‘3’);69 add_line(model_name, ‘Int_x/1’, ‘Mux_Feedback/1’);70 add_line(model_name, ‘Int_y/1’, ‘Mux_Feedback/2’);71 add_line(model_name, ‘Int_theta/1’, ‘Mux_Feedback/3’);728485function pure_code_simulation()86 %% 仿真参数87 T 20; % 仿真总时间 20s88 dt 0.01; % 步长89 t 0:dt:T;90 N length(t);9192 % 初始化状态数组93 % State [x, y, theta]94 X_act zeros(N, 3);95 X_ref zeros(N, 4); % [x, y, theta, v_ref]9697 % 初始状态98 X_act(1, [0.2, -0.2, 0]; % 故意设置初始误差99100 % 轨迹参数 (圆形轨迹)101 R 1.0; % 半径102 w_ref 0.5; % 角速度 rad/s103 v_nominal R * w_ref;104105 % 控制器增益106 k_x 2.0; k_y 3.0; k_theta 5.0;107 epsilon 0.8; delta 0.1;108109 %% 仿真循环110 for i 1:N-1111 ti t(i);112113 % 1. 生成参考轨迹114 X_ref(i, 1) R * cos(w_ref * ti);115 X_ref(i, 2) R * sin(w_ref * ti);116 X_ref(i, 3) w_ref * ti pi/2; % 切线方向117 % 角度归一化118 while X_ref(i, 3) pi, X_ref(i, 3) X_ref(i, 3) - 2pi; end119 while X_ref(i, 3) -pi, X_ref(i, 3) X_ref(i, 3) 2pi; end120 X_ref(i, 4) v_nominal;121122 % 当前实际状态123 x X_act(i, 1);124 y X_act(i, 2);125 theta X_act(i, 3);126127 % 参考状态128 xr X_ref(i, 1);129 yr X_ref(i, 2);130 thr X_ref(i, 3);131 vr_ref X_ref(i, 4);132133 % 2. 计算误差 (全局 - 局部)134 ex_g xr - x;135 ey_g yr - y;136 etheta_g thr - theta;137138 % 角度误差归一化139 while etheta_g pi, etheta_g etheta_g - 2pi; end140 while etheta_g -pi, etheta_g etheta_g 2pi; end141142 ex ex_g * cos(theta) ey_g * sin(theta);143 ey -ex_g * sin(theta) ey_g * cos(theta);144 etheta etheta_g;145146 % 3. 滑模控制律 (SMC)147 % 角速度控制148 sin_term 1.0;149 if abs(etheta) 1e-4, sin_term sin(etheta)/etheta; end150151 w_linear k_theta * etheta vr_ref * k_y * ey * sin_term;152 w_switch epsilon * sat(etheta / delta);153 w_cmd w_linear w_switch;154155 % 线速度控制156 v_linear vr_ref k_x * ex;157 v_switch epsilon * sat(ex / delta);158 v_cmd v_linear v_switch;159160 % 限幅161 v_cmd max(min(v_cmd, 1.5), -1.5);162 w_cmd max(min(w_cmd, 1.0), -1.0);163164 % 4. 运动学更新 (Euler 积分)165 x_dot v_cmd * cos(theta);166 y_dot v_cmd * sin(theta);167 theta_dot w_cmd;168169 X_act(i1, 1) x x_dot * dt;170 X_act(i1, 2) y y_dot * dt;171 X_act(i1, 3) theta theta_dot * dt;172173 % 角度归一化实际状态174 while X_act(i1, 3) pi, X_act(i1, 3) X_act(i1, 3) - 2pi; end175 while X_act(i1, 3) -pi, X_act(i1, 3) X_act(i1, 3) 2pi; end176 end177178 % 补充最后一个参考点179 ti t(end);180 X_ref(end, 1) R * cos(w_ref * ti);181 X_ref(end, 2) R * sin(w_ref * ti);182 X_ref(end, 3) w_ref * ti pi/2;183 X_ref(end, 4) v_nominal;184185 %% 绘图186 figure(‘Color’, ‘w’, ‘Name’, ‘差速机器人滑模轨迹跟踪仿真’);187188 % 轨迹对比189 subplot(1, 2, 1);190 plot(X_ref(:,1), X_ref(:,2), ‘b–’, ‘LineWidth’, 1.5, ‘DisplayName’, ‘参考轨迹’);191 hold on;192 plot(X_act(:,1), X_act(:,2), ‘r-’, ‘LineWidth’, 1.5, ‘DisplayName’, ‘实际轨迹’);193 plot(X_act(1,1), X_act(1,2), ‘go’, ‘MarkerSize’, 8, ‘DisplayName’, ‘起点’);194 plot(X_act(end,1), X_act(end,2), ‘ks’, ‘MarkerSize’, 8, ‘DisplayName’, ‘终点’);195 axis equal; grid on;196 xlabel(‘X (m)’); ylabel(‘Y (m)’);197 title(‘XY 平面轨迹跟踪’);198 legend(‘Location’, ‘best’);199200 % 误差曲线201 subplot(1, 2, 2);202 ex_err X_ref(:,1) - X_act(:,1);203 ey_err X_ref(:,2) - X_act(:,2);204 plot(t, ex_err, ‘r’, ‘DisplayName’, ‘X 轴误差’);205 hold on;206 plot(t, ey_err, ‘g’, ‘DisplayName’, ‘Y 轴误差’);207 % 计算角度误差208 e_th zeros(N,1);209 for i1:N210 err X_ref(i,3) - X_act(i,3);211 while err pi, err err - 2pi; end212 while err -pi, err err 2pi; end213 e_th(i) err;214 end215 plot(t, e_th, ‘b’, ‘DisplayName’, ‘角度误差 (rad)’);216 grid on;217 xlabel(‘Time (s)’); ylabel(‘Error’);218 title(‘跟踪误差收敛曲线’);219 legend(‘Location’, ‘best’);220221 sgtitle([‘滑模控制 (SMC) 仿真结果 (K_x’ num2str(k_x) ‘, K_\theta’ num2str(k_theta) ‘)’]);222end223224function y sat(u)225 if u 1226 y 1;227 elseif u -1228 y -1;229 else230 y u;231 end232end
