MATLAB之机器人鲁棒自适应PD控制代码

p_input

function[sys,x0,str,ts]=input(t,x,u,flag) %指令信号程序
%[sys,x0,str,ts]系统输出变量，(t,x,u,flag)系统输入变量
switch flag%Initialization %初始化%当flag=0 则调用mdlInitializeSizes函数初始化case 0 [sys,x0,str,ts]=mdlInitializeSizes;%Outputscase 3 %当flag=3 则调用mdlOutputs函数初始化sys=mdlOutputs(t,x,u);%Unhandled flagscase{2,4,9}%如果flag=2，4，9输出为空 未使用的flag数的处理sys=[];%Unexpected flagsotherwiseerror(['Unhandled flag=',num2str(flag)]);
endfunction[sys,x0,str,ts]=mdlInitializeSizes
sizes=simsizes; %调用simsizes函数创建结构体Sizes
sizes.NumOutputs=6; %有6个输出量 关节的位置、速度、加速度
sizes.NumInputs=0; %有0个输入量
sizes.DirFeedthrough=0; %输入量中含有输出量
sizes.NumSampleTimes=0; %单个采样周期
sys=simsizes(sizes);  %根据上面设定系统初始化参数
x0=[];  %设置初始状态为空值
str=[];  %将str变量设置为空字符串
ts=[]; %假定继承输入信号的采样周期function sys=mdlOutputs(t,x,u)
q1_d=sin(2*pi*t);             %连杆1位置
q2_d=sin(2*pi*t);             %连杆2位置
dq1_d=2*pi*cos(2*pi*t);       %连杆1速度
dq2_d=2*pi*cos(2*pi*t);       %连杆2速度
ddq1_d=-(2*pi)^2*sin(2*pi*t); %连杆1加速度
ddq2_d=-(2*pi)^2*sin(2*pi*t); %连杆2加速度sys(1)=q1_d;
sys(2)=dq1_d;
sys(3)=ddq1_d;
sys(4)=q2_d;
sys(5)=dq2_d;
sys(6)=ddq2_d;

chap_2ctrlnew

function[sys,x0,str,ts]=control_strategy(t,x,u,flag) %控制器子程序
%[sys,x0,str,ts]系统输出变量，(t,x,u,flag)系统输入变量
switch flag%Initialization %初始化%当flag=0 则调用mdlInitializeSizes函数初始化case 0 [sys,x0,str,ts]=mdlInitializeSizes;%Outputscase 1sys=mdlDerivatives(t,x,u);case 3 %当flag=3 则调用mdlOutputs函数初始化sys=mdlOutputs(t,x,u);%Unhandled flagscase{2,4,9}%如果flag=2，4，9输出为空 未使用的flag数的处理sys=[];%Unexpected flagsotherwiseerror(['Unhandled flag=',num2str(flag)]);
endfunction[sys,x0,str,ts]=mdlInitializeSizes
sizes=simsizes; %调用simsizes函数创建结构体Sizes
sizes.NumContStates=2;
sizes.NumOutputs=2; %有2个输出量   2个控制力矩
sizes.NumInputs=13; %有13个输入量
%1-2连杆位置（2）  1-2连杆速度（2）  1-2连杆加速度（2）  1-2连杆期望位置（2） 1-2连杆期望速度（2）
%质量特性参数估计值（3）
sizes.DirFeedthrough=1; %输入量中含有1输出量
sizes.NumSampleTimes=0; %单个采样周期
sys=simsizes(sizes);  %根据上面设定系统初始化参数
x0=[0;0];  %设置初始状态为空值
str=[];  %将str变量设置为空字符串
ts=[]; %假定继承输入信号的采样周期function sys=mdlDerivatives(t,x,u)
q1_d=u(1);dq1_d=u(2);ddq1_d=u(3);
q2_d=u(4);dq2_d=u(5);ddq2_d=u(6);
q1=u(7);dq1=u(8); % 定义杆件1的期望位置和期望速度
q2=u(9);dq2=u(10);% 定义杆件2的期望位置和期望速度r1=1;g=9.8;e1=g/r1;
dq_d=[dq1_d,dq2_d]';   %速度矩阵
ddq_d=[ddq1_d,ddq2_d]';%加速度矩阵e=[q1-q1_d,q2-q2_d]'; %求解位置追踪误差
de=[dq1-dq1_d,dq2-dq2_d]';%求解速度的追踪误差
f=max([1,norm(e),norm(de)]);%norm是取范数，gama=5*eye(2);  %5为常数目的放大位置追踪误差是控制参数 eye（2）是生成2*2的单位矩阵
y=gama*e+de;  %引入变量y   y是位置追踪误差放大后和速度追踪误差之合gama1=20;gama2=20;%γ1=20；γ2=20；
sys(1)=gama1*f*norm(y);%求解d的估计值（自适应率）
sys(2)=-gama2*x(2);%上确界信号估计值，function sys=mdlOutputs(t,x,u)
q1_d=u(1);dq1_d=u(2);ddq1_d=u(3);
q2_d=u(4);dq2_d=u(5);ddq2_d=u(6);
q1=u(7);dq1=u(8); % 定义杆件1的期望位置和期望速度
q2=u(9);dq2=u(10);% 定义杆件2的期望位置和期望速度dq_d=[dq1_d,dq2_d]';     %速度矩阵
ddq_d=[ddq1_d,ddq2_d]';  %加速度矩阵e=[q1-q1_d,q2-q2_d]';      %位置追踪误差
de=[dq1-dq1_d,dq2-dq2_d]'; %速度追踪误差gama=5*eye(2);
y=gama*e+de;
ddq_d=[ddq1_d,ddq2_d]'; %加速度矩阵
dq_d=[dq1_d,dq2_d]';    %速度矩阵dqr=dq_d-gama*e;     %求解变量qr 为速度减去位置追踪放大误差
ddqr=ddq_d-gama*de;   %求解变量qr 为加速度减去位置追踪放大误差f=max([1,norm(e),norm(de)]);
ut=-(x(1)*f)^2*y/(x(1)*f*norm(y)+x(2)^2+0.0000001);%计算自适应律kp1=[180,0;0,190];
kp2=[150,0;0,150];
kv1=[180,0;0,180];
kv2=[150,0;0,150];alfa1=1;alfa2=1;
beta1=1;beta2=1;p1=u(11);
p2=u(12);
p3=u(13);
P=[p1 p2 p3]';r1=1;g=9.8;e1=g/r1;
fai11=ddqr(1)+e1*cos(q2);
fai12=ddqr(1)+ddqr(2);
fai13=2*ddqr(1)*cos(q2)+ddqr(2)*cos(q2)-dq2*dqr(1)*sin(q2)-(dq1+dq2)*dqr(2)*sin(q2)+e1*cos(q1+q2);fai21=0;
fai22=fai12;
fai23=dq1*dqr(1)*sin(q2)+ddqr(1)*cos(q2)+e1*cos(q1+q2);FAI=[fai11 fai12 fai13;fai21 fai22 fai23];R=FAI*P;tol(1)=-(kp1(1,1)+kp2(1,1)/(alfa1+abs(e(1))))*e(1)-(kv1(1,1)+kv2(1,1)/(beta1+abs(de(1))))*de(1)+R(1)+ut(1);tol(2)=-(kp1(2,2)+kp2(2,2)/(alfa2+abs(e(2))))*e(2)-(kv1(2,2)+kv2(2,2)/(beta2+abs(de(2))))*de(2)+R(2)+ut(2);sys(1)=tol(1);sys(2)=tol(2);

chap_2-2plant

function[sys,x0,str,ts]=inputs_function(t,x,u,flag) %被控对象子程序
%[sys,x0,str,ts]系统输出变量，(t,x,u,flag)系统输入变量
switch flag%Initialization %初始化%当flag=0 则调用mdlInitializeSizes函数初始化case 0 [sys,x0,str,ts]=mdlInitializeSizes;case 1%当flag=1 则调用mdlDerivatives函数初始化sys=mdlDerivatives(t,x,u);%Outputscase 3 %当flag=3 则调用mdlOutputs函数初始化sys=mdlOutputs(t,x,u);%Unhandled flagscase{2,4,9}%如果flag=2，4，9输出为空 未使用的flag数的处理sys=[];%Unexpected flagsotherwiseerror(['Unhandled flag=',num2str(flag)]);
endfunction[sys,x0,str,ts]=mdlInitializeSizes
sizes=simsizes; %调用simsizes函数创建结构体Sizes
sizes.NumContStates=4;%模块连续状态变量的个数
sizes.NumDiscStates=0;%模块离散状态变量的个数
sizes.NumOutputs=4; %有4个输出量   期望速度 和 实际速度
sizes.NumInputs=2; %有2个输入量  2个转矩
sizes.DirFeedthrough=0; %输入量中含有1输出量
sizes.NumSampleTimes=0; %单个采样周期
sys=simsizes(sizes);  %根据上面设定系统初始化参数
x0=[0.10,0,0.10,0];  %设置状态变量设置为0.1 0 0.1 0
str=[];  %将str变量设置为空字符串
ts=[]; %假定继承输入信号的采样周期function sys=mdlDerivatives(t,x,u)
q1_d=sin(2*pi*t);%位置变量
q2_d=sin(2*pi*t);%位置变量
dq1_d=2*pi*cos(2*pi*t);
dq2_d=2*pi*cos(2*pi*t);
ddq1_d=-(2*pi)^2*sin(2*pi*t);
ddq2_d=-(2*pi)^2*sin(2*pi*t);e(1)=x(1)-q1_d;%位置误差
e(2)=x(3)-q2_d;%位置误差
de(1)=x(2)-dq1_d; %速度误差
de(2)=x(4)-dq2_d; %速度误差tol=[u(1);u(2)];
q1=x(1);
dq1=x(2);
q2=x(3);
dq2=x(4);m1=0.5;m2=0.5;
r1=1;r2=0.8;
g=9.8;%求解惯性矩阵
D11=(m1+m2)*r1^2+m2*r2^2+2*m2*r1*r2*cos(q2);
D12=m2*r2^2+m2*r1*r2*cos(q2);
D22=m2*r2^2;
D=[D11 D12;D12 D22];%求解离心力和哥氏力
C12=m2*r1*r2*sin(q2);
C=[-C12*dq2  -C12*(dq1+dq2);C12*dq1 0];%重力项
g1=(m1+m2)*r1*cos(q2)+m2*r2*cos(q1+q2);
g2=m2*r2*cos(q1+q2);
G=[g1*g;g2*g];
w=[1.5;1.5]+2.0*[e(1);e(2)]+5.0*[de(1);de(2)];S=inv(D)*(tol-C*[dq1;dq2]-G-w);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);

chap2_2adapt

function[sys,x0,str,ts]=inputpara_estimate(t,x,u,flag) %自适应算法子程序
%[sys,x0,str,ts]系统输出变量，(t,x,u,flag)系统输入变量
switch flag%Initialization %初始化%当flag=0 则调用mdlInitializeSizes函数初始化case 0 [sys,x0,str,ts]=mdlInitializeSizes;case 1%当flag=1 则调用mdlDerivatives函数初始化sys=mdlDerivatives(t,x,u);%Outputscase 3 %当flag=3 则调用mdlOutputs函数初始化sys=mdlOutputs(t,x,u);%Unhandled flagscase{2,4,9}%如果flag=2，4，9输出为空 未使用的flag数的处理sys=[];%Unexpected flagsotherwiseerror(['Unhandled flag=',num2str(flag)]);
endfunction[sys,x0,str,ts]=mdlInitializeSizes
sizes=simsizes; %调用simsizes函数创建结构体Sizes
sizes.NumContStates=3;%模块连续状态变量的个数
sizes.NumDiscStates=0;%模块离散状态变量的个数
sizes.NumOutputs=3; %有3个输出量   参数估计率
sizes.NumInputs=10; %有10个输入量
%1-2连杆位置（2）  1-2连杆速度（2）  1-2连杆加速度（2）  1-2连杆期望位置（2） 1-2连杆期望速度（2）
sizes.DirFeedthrough=1; %输入量中含有1输出量
sizes.NumSampleTimes=0; %单个采样周期
sys=simsizes(sizes);  %根据上面设定系统初始化参数
x0=[4.1,1.9,1.7];  %设置状态变量设置为4.1 1.9 1.7
str=[];  %将str变量设置为空字符串
ts=[]; %假定继承输入信号的采样周期function sys=mdlDerivatives(t,x,u)
q1_d=u(1);dq1_d=u(2);ddq1_d=u(3);
q2_d=u(4);dq2_d=u(5);ddq2_d=u(6);
q1=u(7);dq1=u(8); % 定义杆件1的期望位置和期望速度
q2=u(9);dq2=u(10);% 定义杆件2的期望位置和期望速度q_error=[q1-q1_d,q2-q2_d]';   %计算位置跟踪误差
dq_error=[dq1-dq1_d,dq2-dq2_d]';  %计算速度跟踪误差gama=5*eye(2);    %题目中给出
y=gama*q_error+dq_error;  %计算变量y
ddq_d=[ddq1_d,ddq2_d]';  %加速度矩阵
dq_d=[dq1_d,dq2_d]';     %速度矩阵dqr=dq_d-gama*q_error; %计算速度变量qr
ddqr=ddq_d-gama*dq_error; %计算加速度变量qr%求解已知关节变量函数的回归矩阵（机器人广义坐标及其各阶导数的已知函数矩阵）
%回归矩阵由关节动态矩阵求解获得，求解过程中将p1=(m1+m2)(r1)^2,p2=m2(r2)^2,p3=m2(r1)(r3),e1=g/r1,g=9.8
g=9.8;r1=1;
e1=g/r1;fai11=ddqr(1)+e1*cos(q2);
fai12=ddqr(1)+ddqr(2);
fai13=2*ddqr(1)*cos(q2)+ddqr(2)*cos(q2)-dq2*dqr(1)*sin(q2)-(dq1+dq2)*dqr(2)*sin(q2)+e1*cos(q1+q2);fai21=0;
fai22=fai12;
fai23=dq1*dqr(1)*sin(q2)+ddqr(1)*cos(q2)+e1*cos(q1+q2);
FAI=[fai11 fai12 fai13;fai21 fai22 fai23];Gama=5*eye(3);%5为常数目的放大位置追踪误差是控制参数 eye（2）是生成2*2的单位矩阵
S=-Gama*FAI'*y; %计算参数估计律for i=1:1:3sys(i)=S(i);
endfunction sys=mdlOutputs(t,x,u)
sys(1)=x(1);
sys(2)=x(2);
sys(3)=x(3);

绘制图像

close all;figure(1);
subplot(211);
plot(t,qd(:,1),'r',t,q(:,1),'b');
xlabel('times(s)');ylabel('position tracking of link 1');
subplot(212);
plot(t,qd(:,2),'r',t,q(:,2),'b');
xlabel('time(s)');ylabel('velocity tracking of link 1');figure(2);
subplot(211);
plot(t,qd(:,4),'r',t,q(:,3),'b');
xlabel('times(s)');ylabel('position tracking of link 1');
subplot(212);
plot(t,qd(:,5),'r',t,q(:,4),'b');
xlabel('time(s)');ylabel('velocity tracking of link 1');m1=0.5;m2=0.5;
r1=1;r2=0.8;
p1=(m1+m2)*r1^2;
p2=m2*r2^2;
p3=m2*r1*r2;figure(3);
subplot(311);
plot(t,p(:,1),'r',t,p1,'b');
xlabel('time(s)');ylabel('p1 estimate value');
subplot(312);
plot(t,p(:,2),'r',t,p2,'b');
xlabel('time(s)');ylabel('p2 estimate value');
subplot(313)
plot(t,p(:,3),'r',t,p3,'b');
xlabel('time(s)');ylabel('p3 estimate value');

注：该部分摘自刘金琨《机器人控制系统的设计与MATLAB仿真》第二章节地机器人鲁棒自适应PD控制。

MATLAB之机器人鲁棒自适应PD控制代码相关推荐

【matlab】单摆鲁棒滑模控制matlab实现从简到繁
前言这是前几周现代鲁棒控制课程一位同学的大作业,ppt做的很详细,但是并没有分享代码,我就根据ppt里的单摆模型以及所涉及到的鲁棒滑模控制方法和思路,自己搭了一套仿真程序,非常便捷,需要切换控制方法 ...
VSC/SMC(十六)——自适应鲁棒滑模控制
目录 1.参数不定和扰动不定但有界的系统 2.滑模控制自适应律设计 2.1控制律设计总结 3.仿真分析 3.1 PD控制 3.2普通自适应律 3.3映射自适应律 3.4总结 4学习问题 1.参数不定和 ...
鲁棒优化入门（二）——基于matlab+yalmip求解鲁棒优化问题
上一篇博客简单介绍了可以用来求解鲁棒优化的两个工具箱: 鲁棒优化入门(一)--工具箱Xprog和RSOME的安装与使用其实大家可能没有想过,matlab+yalmip工具箱也可以处理一些简单的鲁棒优 ...
一种基于Matlab的快速鲁棒特征点(surf)的图像拼接技术
一.引言快速鲁棒特征(Speeded-up robust feature,SURF)算法在图像匹配.模式识别.图像拼接等众多领域有着广泛的应用[1].有学者提出一种基于快速鲁棒特征(SURF)的眼底 ...
从控制理论的根轨迹法和稳定性分析谈到舵机PD控制代码实现
在上一次谈到基于MPU6050的基于一阶互补滤波算法实现后,本来想接着就自适应一阶互补滤波和卡尔曼滤波再写一篇的,但是卡尔曼滤波算法我自己写出来并进行姿态解算后发现效果不很好,才疏学浅,等我调好了再写 ...
浅谈UML中常用的几种图——鲁棒图
什么是鲁棒图鲁棒图包含 3 种元素(如图 8-2 所示),它们分别是边界对象.控制对象.实体对象: 边界对象对模拟外部环境和未来系统之间的交互进行建模.边界对象负责接收外部输入,处理内部内容的解释, ...
uml通信图画法_UML 交互图（顺序图、通信图、鲁棒图、定时图）
交互与交互图交互的概念一次交互就是指在特定语境中,为了实现某一个目标,而在一组对象之间进行交换的一组消息所表示的行为消息 UML中的4种交互图顺序图:顺序图是一种强调消息时间顺序的交互图,为读 ...
【鲁棒控制】平面2R型机器人的鲁棒逆推跟踪控制(matlab实现)
鲁棒跟踪逆推控制器设计前言:对于模型准确的对象,我们可以设计控制器直接进行控制,但通常实际情况下总是存在着种种不确定因素,如参数变化,未建模动态变化等,鲁棒控制就是在模型不精确和其他变化因素的条件下 ...
【泡泡机器人转载】SLAM: 现在，未来和鲁棒年代
摘要 SLAM是由同步环境地图构建和运行于其中的机器人状态估计组成.SLAM领域过去30年取得了令人瞩目的成就,其在大场景应用成为了可能,这一技术被见证了成功地被应用于工业领域.本文中,我们研究了SL ...

MATLAB之机器人鲁棒自适应PD控制代码

MATLAB之机器人鲁棒自适应PD控制代码相关推荐

最新文章

热门文章