二关节机械手PD控制simulink仿真

simulink主程序

其中的PDcontrol函数表示的是PD控制器，PD控制器共六个输入，分别是关节1、关节2的理想关节角，实际关节角，关节角速度；共两个输出，为关节1、关节2的驱动力矩。PD控制器的控制律为：

PD控制器子程序：——PDcontrol

%% 控制器程序
function[sys,x0,str,ts]=spacemodle(t,x,u,flag)

switch flag,
case 0,
[sys,x0,str,ts]=mdlInitializeSizes;
case 3,
sys=mdlOutputs(t,x,u);
case {2,4,9}
sys=[];
otherwise
error(['Unhandled flag = ',num2str(flag)]);
end
function [sys,x0,str,ts]=mdlInitializeSizes
sizes = simsizes;
sizes.NumContStates = 0;
sizes.NumDiscStates = 0;
sizes.NumOutputs = 2;%输出变量为驱动力矩，t1，t2
sizes.NumInputs = 6;%输入变量为qd1，qd2，q1，q1的导数，q2，q2的导数
sizes.DirFeedthrough = 1;
sizes.NumSampleTimes = 1;
sys = simsizes(sizes);
x0 = [];
str = [];
ts = [0,0];

function sys=mdlOutputs(t,x,u) %描述控制器输出
R1=u(1);dr1=0;%u1和u2为两个关节的理想位置qd1和qd2
R2=u(2);dr2=0;

x(1)=u(3);%关节1的实际位置
x(2)=u(4);%关节1的速度
x(3)=u(5);%关节2的位置
x(4)=u(6);%关节2的速度

e1=R1-x(1);
e2=R2-x(2);
e=[e1;
e2];

de1=dr1-x(2);
de2=dr2-x(4);
de=[de1;
de2];

kp=[30,0;
0,30];
kd=[30,0;
0,30];

tol=kp*e+kd*de;%输出两个关节的驱动力矩

sys(1)=tol(1);
sys(2)=tol(2);

二自由度机械臂动力学模型：

机械臂模型子程序：——s_function

%% 被控对象——二自由度机械臂动力学模型
function[sys,x0,str,ts]=s_function(t,x,u,flag)

switch flag,
case 0,
[sys,x0,str,ts]=mdlInitializeSizes;
case 1,
sys=mdlDerivatives(t,x,u);
case 3,
sys=mdlOutputs(t,x,u);
case {2,4,9}
sys=[];
otherwise
error(['Unhandled flag = ',num2str(flag)]);
end

%% mdlInitializeSize初始化
function [sys,x0,str,ts]=mdlInitializeSizes
global p g %定义全局变量
sizes = simsizes;
sizes.NumContStates = 4;%连续状态变量个数为4个
sizes.NumDiscStates = 0;
sizes.NumOutputs = 4;%输出变量为q1，q1的导数，q2，q2的导数
sizes.NumInputs = 2;%输入变量为驱动力矩t1，t2
sizes.DirFeedthrough = 0;
sizes.NumSampleTimes = 0;
sys = simsizes(sizes);
x0 = [0 0 0 0];
str = [];
ts = [];

p=[2.9 0.76 0.87 3.04 0.87];
g=9.8;

%微分方程描述，用该函数了描述被控对象和自适应律
function sys=mdlDerivatives(t,x,u)
global p g
%使用全局变量
q=[x(1);x(3)];
dq=[x(2);x(4)];
DO=[p(1)+p(2)+2*p(3)*cos(q(2)),p(2)+p(3)*cos(q(2));
p(2)+p(3)*cos(q(2)),p(2) ];
%惯性矩阵

CO=[-p(3)*dq(2)*sin(q(2)) ,-p(3)*(dq(1)+dq(2))*sin(q(2));
p(3)*dq(1)*sin(q(2)) ,0];
%科氏力与向心力矩阵

tol=u(1:2);
%输入两个关节的驱动力矩
%tol=u(1:2);
% dq=[x(2);x(4)];
%状态变量q导数

s=inv(DO)*(tol-CO*dq);%由机械臂的动力学方程，得到二阶导数
sys(1)=x(2);
sys(2)=s(1);
sys(3)=x(4);
sys(4)=s(2);

function sys=mdlOutputs(t,x,u)
sys(1)=x(1);
sys(2)=x(2);
sys(3)=x(3);
sys(4)=x(4);

绘图子程序

close all;
figure(1);
subplot(211);
plot(t,x1(:,1),'r',t,x1(:,2),'b');
xlabel('time(s)');ylabel('position tracking of link 1');
subplot(212);
plot(t,x2(:,1),'r',t,x2(:,2),'b');
xlabel('time(s)');ylabel('position tracking of link 2');

figure(2);
subplot(211);
plot(t,tol(:,1),'r')
xlabel('time(s)');ylabel('tol1');
subplot(212);
plot(t,tol(:,2),'r')
xlabel('time(s)');ylabel('tol2');

二关节机械手PD控制simulink仿真相关推荐

2021-03-29 自动控制-滑模控制 Simulink仿真
自动控制-滑模控制 Simulink仿真滑模控制是一种相当简单而且控制性能优越的控制方法.它的控制效果优越体现在哪里呢?主要是两点: 1.滑动模态可以进行设计,调节的参数少,响应速度快. 2.对扰动 ...
PWM/转速单闭环/转速电流双闭环电机控制Simulink仿真
PWM/转速单闭环/转速电流双闭环电机控制Simulink仿真实验室之前一直在用BBB板和ARM板通过驱动器控制电机,最近实验室新买了一台DSPACE平台,具有运行参数可视化.实时性高等特点,但由于 ...
电动汽车整车控制SIMULINK仿真
整车控制SIMULINK仿真建立电机子系统建立动力电池子系统建立变速器子系统建立车辆动力学子系统建立驾驶子系统建立风扇和水泵子系统数据显示文件下载地址: https://downloa ...
永磁同步电机（PMSM）最小损耗控制Simulink仿真
本篇文章主要进行永磁同步电机的最小损耗控制simulink仿真. 完整的阐述了最小损耗控制原理,考虑铁耗的永磁同步电机模型的搭建,FOC控制的整体搭建. 大家觉得本篇文章写得不错的话给博主点个赞和收藏 ...
二相混合式步进电机闭环矢量控制simulink仿真（含仿真文件）
目录在位置跟踪伺服系统中二相混合式步进电机闭环矢量控制simulink仿真闭环矢量控制系统simulink图闭环矢量控制系统系统结构框图二相混合式步进电机的数学模型矩角特性电压方程机械 ...
m基于内外环PD控制算法的四旋翼无人机飞行控制simulink仿真
目录 1.算法仿真效果 2.算法涉及理论知识概要 3.MATLAB核心程序 4.完整算法代码文件 1.算法仿真效果 matlab2022a仿真结果如下: 2.算法涉及理论知识概要随着无人飞行器技术的 ...
异步电机变压变频控制（Asynchronous VVVF）-恒压频比控制Simulink仿真
本篇为异步电机变压变频控制(Asynchronous VVVF)Simulink仿真教程本篇文章的仿真分享,需要simulink仿真文件的评论区留下邮箱吧,点个关注收藏吧!!!!! 希望大家点赞,收 ...
永磁同步电机力矩控制simulink仿真，永磁同步发电机simulink控制
永磁同步电机力矩控制仿真,异步电机仿真,单轴的电流调节器适用于力矩控制,发电控制,独特的MTPA控制+弱磁控制,以及电流路径规划,实现全速范围内的力矩调节,适用于轨道交通领域以及电动汽车领域. 列车运 ...
永磁同步电机恒压频比（V/F）控制Simulink仿真
1 V/F介绍 V/F控制作为一种简单的开环控制方式,常常用于异步电机的变频起动,同样也可以用于同步电机的起动,但是用在永磁同步电机中存在失步,效率低,转速不稳定等问题. 先说说为什么永磁同步电机会失 ...

二关节机械手PD控制simulink仿真

二关节机械手PD控制simulink仿真相关推荐

最新文章

热门文章