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個文件信息