机械臂阻抗控制Matlab仿真

这资料包含了多自由度机械臂阻抗控制Matlab代码，下载时留下联系方式，大家一起讨论！

function [sysx0strts]=impedance_control（txuflag）
% x:当前轨迹；xc：指令轨迹；xd：理想轨迹
% Fe：环境外力；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
%设置限制条件：一堵墙
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;%更新反馈力，根据作用力update控制力

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

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

ddx=inv（Dx）*（Fx-Cx*dx-Gx-Delta）;
%设置系统状态变量，定义x（1）和x（3）为目标位置的两个坐标轴信息
%x（2）和x（4）分别为它的导数
%然后根据控制算法，从ddx开始更新

sys（1）=x（2）;     
sys（2）=ddx（1）;
sys（3）=x（4）;
sys（4）=ddx（2）;
%系统状态方程分别为：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
%设置限制条件：一堵墙
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
 

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件       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