function?[sysx0strts]=impedance_control（txuflag）
%?x:當(dāng)前軌跡；xc：指令軌跡；xd：理想軌跡
%?Fe：環(huán)境外力；Fx：控制力；τ：控制力矩
switch?flag
case?0
????[sysx0strts]=mdlInitializeSizes;
case?1
????sys=mdlDerivatives（txu）;
case?3
????sys=mdlOutputs（txu）;
case?{2?4?9?}
????sys?=?[];
????otherwise?
????error（[‘Unhandled?flag?=?‘num2str（flag）]）;
end
function?[sysx0strts]=mdlInitializeSizes
global?J?Fx?Fe?
sizes?=?simsizes;
sizes.NumContStates??=?4;
sizes.NumDiscStates??=?0;
sizes.NumOutputs?????=?8;
sizes.NumInputs??????=?14;
sizes.DirFeedthrough?=?1;
sizes.NumSampleTimes?=?0;
sys=simsizes（sizes）;
x0=[0.8?0?1.0?0];???%xd（0）=xc（0）dxd（0）=dxc（0）
str=[];
ts=[];
J=0;Fx=[0?0]‘;Fe=[0?0]‘;


function?sys=mdlDerivatives（txu）
global?J?Fx?Fe
xd=[u（1）?u（4）]‘;
dxd=[u（2）?u（5）]‘;
ddxd=[u（3）?u（6）]‘;
if?xd（1）>=1.0
???xd=[1.0?xd（2）]‘;dxd=[0?dxd（2）]‘;ddxd=[0?ddxd（2）]‘;
end
%設(shè)置限制條件：一堵墻
xp=[u（7）?u（10）]‘;
dxp=[u（8）?u（11）]‘;
ddxp=[u（9）?u（12）]‘;

Mm=[0.5?0;0?0.5];
Bm=[4.8?0;0?4.8];
Km=[50?0;0?50];

l1=1;l2=1;
P=[1.66?0.42?0.63?3.75?1.25];
g=9.8;
L=[l1^2?l2^2?l1*l2?l1?l2];

pl=0.5;

M=P+pl*L;
Q=（x（1）^2+x（3）^2-l1^2-l2^2）/（2*l1*l2）;
q2=acos（Q）;
dq2=-1/sqrt（1-Q^2）;

A=x（3）/x（1）;
p1=atan（A）;
d_p1=1/（1+A^2）;

B=sqrt（x（1）^2+x（3）^2+l1^2-l2^2）/（2*l1*sqrt（x（1）^2+x（3）^2））;
p2=acos（B）;
d_p2=-1/sqrt（1-B^2）;

if?q2>0
????q1=p1-p2;
????dq1=d_p1-d_p2;
else
????q1=p1+p2;
????dq1=d_p1+d_p2;
end

J=[-sin（q1）-sin（q1+q2）?-sin（q1+q2）;
????cos（q1）+cos（q1+q2）?cos（q1+q2）];
d_J=[-dq1*cos（q1）-（dq1+dq2）*cos（q1+q2）?-（dq1+dq2）*cos（q1+q2）;
????-dq1*sin（q1）-（dq1+dq2）*sin（q1+q2）?-（dq1+dq2）*sin（q1+q2）];

D=[M（1）+M（2）+2*M（3）*cos（q2）?M（2）+M（3）*cos（q2）;
????M（2）+M（3）*cos（q2）?M（2）];
C=[-M（3）*dq2*sin（q2）?-M（3）*（dq1+dq2）*sin（q2）;
????M（3）*dq1*sin（q2）??0];
G=[M（4）*g*cos（q1）+M（5）*g*cos（q1+q2）;
???M（5）*g*cos（q1+q2）];

Dx=（inv（J））‘*D*inv（J）;
Cx=（inv（J））‘*（C-D*inv（J）*d_J）*inv（J）;
Gx=（inv（J））‘*G;

Fe=Mm*（ddxd-ddxp）+Bm*（dxd-dxp）+Km*（xd-xp）;%target?impedance

Fx=Fe+Dx*ddxp+Cx*dxp+Gx;%更新反饋力，根據(jù)作用力update控制力

dx=[x（2）?x（4）]‘;

Delta=[sin（t）?sin（t）]‘;

ddx=inv（Dx）*（Fx-Cx*dx-Gx-Delta）;
%設(shè)置系統(tǒng)狀態(tài)變量，定義x（1）和x（3）為目標(biāo)位置的兩個(gè)坐標(biāo)軸信息
%x（2）和x（4）分別為它的導(dǎo)數(shù)
%然后根據(jù)控制算法，從ddx開始更新

sys（1）=x（2）;?????
sys（2）=ddx（1）;
sys（3）=x（4）;
sys（4）=ddx（2）;
%系統(tǒng)狀態(tài)方程分別為：dx=x（2/4），ddx為更新值


function?sys=mdlOutputs（txu）
global?J?Fx?Fe
xd=[u（1）?u（4）]‘;
dxd=[u（2）?u（5）]‘;
ddxd=[u（3）?u（6）]‘;
if?xd（1）>=1.0
???xd=[1.0?xd（2）]‘;dxd=[0?dxd（2）]‘;ddxd=[0?ddxd（2）]‘;
end
%設(shè)置限制條件：一堵墻
xp=[u（7）?u（10）]‘;
dxp=[u（8）?u（11）]‘;
ddxp=[u（9）?u（12）]‘;

Mm=[0.5?0;0?0.5];
Bm=[4.8?0;0?4.8];
Km=[50?0;0?50];


l1=1;l2=1;
P=[1.66?0.42?0.63?3.75?1.25];
g=9.8;
L=[l1^2?l2^2?l1*l2?l1?l2];

pl=0.5;

M=P+pl*L;
Q=（x（1）^2+x（3）^2-l1^2-l2^2）/（2*l1*l2）;
q2=acos（Q）;
dq2=-1/sqrt（1-Q^2）;

A=x（3）/x（1）;
p1=atan（A）;
d_p1=1/（1+A^2）;

B=sqrt（x（1）^2+x（3）^2+l1^2-l2^2）/（2*l1*sqrt（x（1）^2+x（3）^2））;
p2=acos（B）;
d_p2=-1/sqrt（1-B^2）;

if?q2>0
????q1=p1-p2;
????dq1=d_p1-d_p2;
else
?

?屬性????????????大小?????日期????時(shí)間???名稱
-----------?---------??----------?-----??----

?????文件???????8196??2018-05-11?21:13??impedance_control\.DS_Store

?????文件???????6148??2018-05-11?21:13??impedance_control\position\.DS_Store

?????文件??????88680??2018-05-11?17:23??impedance_control\position\1-1.fig

?????文件??????38781??2018-05-11?17:23??impedance_control\position\1-2.fig

?????文件??????43892??2018-05-11?17:23??impedance_control\position\1-3.fig

?????文件??????93486??2018-05-11?17:28??impedance_control\position\2-1.fig

?????文件??????40700??2018-05-11?17:28??impedance_control\position\2-2.fig

?????文件??????46737??2018-05-11?17:28??impedance_control\position\2-3.fig

?????文件???????4056??2018-05-11?17:27??impedance_control\position\impedance_control.m

?????文件????????755??2018-05-17?15:52??impedance_control\position\result_plot.m

?????文件??????27883??2018-05-10?19:28??impedance_control\position\system_block.mdl

?????文件????????982??2018-04-22?16:05??impedance_control\position\xc_produce.m

?????文件???????6148??2018-05-11?21:13??impedance_control\time\.DS_Store

?????文件?????147129??2018-05-11?17:58??impedance_control\time\1-1.fig

?????文件??????58842??2018-05-11?17:58??impedance_control\time\1-2.fig

?????文件??????71445??2018-05-11?20:37??impedance_control\time\1-3.fig

?????文件???????4140??2018-05-11?18:00??impedance_control\time\impedance_control.m

?????文件????????755??2018-05-17?15:52??impedance_control\time\result_plot.m

?????文件??????27883??2018-05-11?18:02??impedance_control\time\system_block.mdl

?????文件????????982??2018-04-22?16:05??impedance_control\time\xc_produce.m

?????目錄??????????0??2018-05-17?15:49??impedance_control\position

?????目錄??????????0??2018-05-17?15:49??impedance_control\time

?????目錄??????????0??2018-05-17?15:49??impedance_control

-----------?---------??----------?-----??----

???????????????717620????????????????????23