资源简介
这资料包含了多自由度机械臂阻抗控制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
相关资源
- 高灵敏度GPS接收机MATLAB仿真,附捕获
- 基于MATLAB的质点弹道计算与外弹道优
- 阵列天线的matlab仿真
- MATLAB 经典程序源代码大全
- MATLAB小波软阈值去噪代码33473
- 天线阵的波束形成在MATLAB仿真程序及
- 非线性SVM算法-matlab实现
- 《MATLAB 智能算法超级学习手册》-程序
- 组合导航matlab程序
- 读取txt文件内容matlab代码实现
- Matlab实现基于相关的模板匹配程序
- matlab优化工具箱讲解
- 基于MATLAB的快速傅里叶变换
- 光纤传输中的分布傅立叶算法matlab实
- 基于matlab的图像处理源程序
- matlab 椭圆拟合程序
- 算术编码解码matlab源代码
- optical_flow 光流法 matlab 实现程序
- 引导图像滤波器 Matlab实现
- 分形几何中一些经典图形的Matlab画法
- OFDM系统MATLAB仿真代码
- SVM工具箱(matlab中运行)
- 图像小波变换MatLab源代码
- LU分解的MATLAB实现
- 冈萨雷斯数字图像处理matlab版(第三
- 替代数据法的matlab程序
- 用matlab实现的多站定位系统性能仿真
- 通过不同方法进行粗糙集属性约简m
- k近邻算法matlab实现
- matlab识别系统
评论
共有 条评论