改进以下代码，使机器人的运动轨迹为一个半径为3的圆clear; close all; clc %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % diff_vel p2p Motion Control 两轮差速任意姿态到达目标点 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% input 输入 % Goal -----------目标位姿 % r --------------驱动轮半径(m) % l --------------轮间距，两驱动轮中心间距(m) % InitPos --------初始位姿 % goal_rad -------目标半径(m) % lin_vel_lim ----速度限幅(m/s) % lin_acc_lim ----加速度限幅(m/s^2) % ang_vel_lim ----角速度限幅(rad/s) % ang_acc_lim ----角加速度限幅(rad/s^2) % ctrl_fre -------控制频率(hz) % max_sim_time ---最大仿真时长(s) %% output 输出 % lin_vel --------车体线速度(m/s) % ang_vel --------车体角速度(rad/s)(右手定则) % theta ----------姿态角（rad） % v_l ------------左轮转动线速度(m/s) % v_r ------------右轮转动线速度(m/s) % phiL -----------左轮正方向转动角速度，记反转速度为负值(rad/s) % phiR -----------右轮正方向转动角速度，记反转速度为负值(rad/s) %% 位姿信息 % Pos = [x, y ,theta] %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% 仿真开始 InitPos = [1, 0, 0]; Goal = [5,4,0]; r = 0.15; l = 0.4; goal_rad = 0.05; ctrl_fre = 100; max_sim_time = 100; lin_vel_lim = 1.2; lin_acc_lim = lin_vel_lim/2; ang_vel_lim = 1.5; ang_acc_lim = 0.8; sim('diff_vel_motion_ctrl_system.slx'); PlotTracking; %画图

可以将机器人运动的目标点改为固定的圆周上的点，以实现运动轨迹为一个半径为3的圆。具体步骤如下：

1. 定义一个固定圆心为(0,0)、半径为3的圆，假设圆心角为theta。

2. 每次仿真时，将机器人的目标点设置为圆上的点(x,y)，其中x=3*cos(theta)、y=3*sin(theta)。

3. 每次仿真时，将theta增加一个固定值delta_theta，使机器人沿着圆周运动。

修改后的代码如下：

clear;
close all;
clc

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% diff_vel p2p Motion Control 两轮差速任意姿态到达目标点
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% input 输入
% Goal -----------目标位姿
% r --------------驱动轮半径(m)
% l --------------轮间距，两驱动轮中心间距(m)
% InitPos --------初始位姿
% goal_rad -------目标半径(m)
% lin_vel_lim ----速度限幅(m/s)
% lin_acc_lim ----加速度限幅(m/s^2)
% ang_vel_lim ----角速度限幅(rad/s)
% ang_acc_lim ----角加速度限幅(rad/s^2)
% ctrl_fre -------控制频率(hz)
% max_sim_time ---最大仿真时长(s)
%% output 输出
% lin_vel --------车体线速度(m/s)
% ang_vel --------车体角速度(rad/s)(右手定则)
% theta ----------姿态角（rad）
% v_l ------------左轮转动线速度(m/s)
% v_r ------------右轮转动线速度(m/s)
% phiL -----------左轮正方向转动角速度，记反转速度为负值(rad/s)
% phiR -----------右轮正方向转动角速度，记反转速度为负值(rad/s)
%% 位姿信息
% Pos = [x, y ,theta]
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% 仿真开始
InitPos = [1, 0, 0];
r = 0.15;
l = 0.4;
goal_rad = 0.05;
ctrl_fre = 100;
max_sim_time = 100;
lin_vel_lim = 1.2;
lin_acc_lim = lin_vel_lim/2;
ang_vel_lim = 1.5;
ang_acc_lim = 0.8;
delta_theta = pi/100; % 每次增加的圆心角

sim_time = 0;
theta = 0;
while sim_time < max_sim_time
sim('diff_vel_motion_ctrl_system.slx');
PlotTracking;
theta = theta + delta_theta;
sim_time = sim_time + 1/ctrl_fre;
Goal = [3*cos(theta), 3*sin(theta), 0];
end

% 画图
function PlotTracking
hold on;
plot(Goal(1), Goal(2), 'ro', 'LineWidth', 2);
plot(Position(:,1), Position(:,2), 'b-', 'LineWidth', 2);
axis equal;
xlabel('x (m)');
ylabel('y (m)');
title('Robot Tracking');
drawnow;
end