Matlab-Simulink 差速小车循路

双差动单元的四轮差动小车的循路控制。

clear
clc
close all
%--------------------------------------------------------------------------
%% Initialization
R = 5;      % 大圆半径 The radius of the circular path
r = 2;      %小圆半径 The radius of the transition circular path
ds = 0.4;   % 离散距离 Discrete distance
D = 0.5;    % 轮距 Track width
wheel_R = 0.1;  % 车轮半径 The wheel radius
w0 = 0.07/wheel_R; % 轮子初始转速 Initial angular velocity
%--------------------------------------------------------------------------
disThreshold = 0.04;    % 距离阈值，小车与目标点距离进入该阈值后目标点变成下一个点 
                        % The lower limit of the threshold value of target point selection
InitHead = 90*pi/180;   % 小车方向基准3点钟方向为0度（即x轴正方向），顺时针为正
                        % 3 o ‘clock be the reference direction zero degrees
head = -90*pi/180;      % 小车初始指向为12点钟方向，The initial Angle is 12 o ‘clock
head = head + InitHead;
endTheta = -1.5*pi-asin（r/（R-r））;  % endTheta 为终点角度 
                                   % The end angle that can make the circle be closed
%--------------------------------------------------------------------------
% 计算三个角度 Three central angles
theta_1 = asin（r/（R-r））; %The center of a small circle
theta_2 = pi/2 - theta_1;
theta_3 = pi - theta_2;
%% Generate temporary path
% straight-line portion
% 计算路径1的点
y_1 = （R-r） * cos（theta_1）;
%The point at which a straight line meets the small circle
line_1y = 0:ds:y_1;
line_1x = zeros（size（line_1y））;
path_1 = [line_1x;line_1y];% Generate partial path of straight line
% circular arc portion
% 计算路径2的点
% 计算小圆中心坐标 The coordinates of the center of small circle
rx = r;
ry = y_1;
% 以ds距离采样，计算每次间隔的圆心角
% The central Angle between two sampling points is calculated according to
% the sampling distance ds
dtheta_2 = 2*asin（ds/2/r）;
% 计算每个采样点的角度
theta = pi:-dtheta_2:（pi-theta_3）;
% 第二条路径
path_2 = [r*cos（theta）+rx;r*sin（theta）+ry];% Generate partial path of circular arc

%% Generate expected path
% 计算路径3的点 Generation of the expect circular path 
% 以ds距离采样，计算每次间隔的圆心角
dtheta_3 = 2*asin（ds/2/R）;
% 计算每个采样点的角度
theta = pi/2-theta_1:-dtheta_3:endTheta;   
path_3 =[R*cos（theta）;R*sin（theta）];
path = [path_1path_2path_3];
transT = [cos（head）-sin（head）;
         sin（head）cos（head）];
path = transT*path;
len = size（path2）;
ax = 0:size（path2）-1;
%follow path
path1x = path（12:length（path））;
path1y = path（22:length（path））;
path2 = path;
ax2 = 0:size（path2）-2;
figure
plot（path（1:）path（2:）‘.r‘）;
axis（[-66-66]）;
axis equal
save data.mat

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2019-03-12 03:40  simulation D-d robot\
     文件        8627  2019-03-12 03:41  simulation D-d robot\data.mat
     文件        2683  2019-03-12 02:33  simulation D-d robot\dataInit.m
     文件         736  2019-03-11 08:53  simulation D-d robot\PlotRobot.m
     文件         540  2019-03-12 03:05  simulation D-d robot\result.m
     文件       53380  2019-03-12 03:40  simulation D-d robot\simulation_model.slx
     文件        4726  2019-03-12 03:25  simulation D-d robot\simulation_model.slxc
     目录           0  2019-03-12 03:25  simulation D-d robot\slprj\
     目录           0  2019-03-12 03:25  simulation D-d robot\slprj\sim\
     目录           0  2019-03-12 03:25  simulation D-d robot\slprj\sim\varcache\
     目录           0  2019-03-12 03:25  simulation D-d robot\slprj\sim\varcache\simulation_model\
     文件         392  2019-03-12 03:25  simulation D-d robot\slprj\sim\varcache\simulation_model\checksumOfCache.mat
     目录           0  2019-03-12 03:25  simulation D-d robot\slprj\sim\varcache\simulation_model\tmwinternal\
     文件         312  2019-03-12 03:25  simulation D-d robot\slprj\sim\varcache\simulation_model\tmwinternal\simulink_cache.xml
     文件        3792  2019-03-12 03:25  simulation D-d robot\slprj\sim\varcache\simulation_model\varInfo.mat
     目录           0  2019-03-12 03:25  simulation D-d robot\slprj\_jitprj\
     文件       10276  2019-03-12 03:25  simulation D-d robot\slprj\_jitprj\s2uirV6UfHP6nQpn3iLR9qD.l
     文件         891  2019-03-12 03:25  simulation D-d robot\slprj\_jitprj\s2uirV6UfHP6nQpn3iLR9qD.mat
     文件        8924  2019-03-12 03:25  simulation D-d robot\slprj\_jitprj\sBRlD0PpHrsoAky8Y2uSCgH.l
     文件         871  2019-03-12 03:25  simulation D-d robot\slprj\_jitprj\sBRlD0PpHrsoAky8Y2uSCgH.mat
     文件       10816  2019-03-12 03:25  simulation D-d robot\slprj\_jitprj\smhheaVmTwOZS4Mz9iDhBB.l
     文件         885  2019-03-12 03:25  simulation D-d robot\slprj\_jitprj\smhheaVmTwOZS4Mz9iDhBB.mat
     目录           0  2019-03-12 03:25  simulation D-d robot\slprj\_sfprj\
     目录           0  2019-03-12 03:25  simulation D-d robot\slprj\_sfprj\EMLReport\
     目录           0  2019-03-12 03:25  simulation D-d robot\slprj\_sfprj\EMLReport\s2uirV6UfHP6nQpn3iLR9qD\
     文件        2647  2019-03-12 03:25  simulation D-d robot\slprj\_sfprj\EMLReport\s2uirV6UfHP6nQpn3iLR9qD.mat
     目录           0  2019-03-12 03:25  simulation D-d robot\slprj\_sfprj\EMLReport\sBRlD0PpHrsoAky8Y2uSCgH\
     文件        2486  2019-03-12 03:25  simulation D-d robot\slprj\_sfprj\EMLReport\sBRlD0PpHrsoAky8Y2uSCgH.mat
     目录           0  2019-03-12 03:25  simulation D-d robot\slprj\_sfprj\EMLReport\smhheaVmTwOZS4Mz9iDhBB\
     文件       30425  2019-03-12 03:25  simulation D-d robot\slprj\_sfprj\EMLReport\smhheaVmTwOZS4Mz9iDhBB.mat
     目录           0  2019-03-12 03:25  simulation D-d robot\slprj\_sfprj\precompile\
............此处省略11个文件信息