南航非线性大作业.docx
- 文档编号:3847035
- 上传时间:2022-11-25
- 格式:DOCX
- 页数:14
- 大小:173.34KB
南航非线性大作业.docx
《南航非线性大作业.docx》由会员分享,可在线阅读,更多相关《南航非线性大作业.docx(14页珍藏版)》请在冰豆网上搜索。
南航非线性大作业
南航非线性大作业
课程:
非线性系统控制
题目:
二阶机械臂的非线性反馈设计
姓名:
刘洋洋
学号:
SX1203179
专业:
导航制导与控制
2013年6月28日
一、被控对象动力学模型
被控对象为两杆机械臂,
为第一级杆转角,
为第二级杆相对第一级杆的转角,
为负载位置坐标,
。
系统的动力学模型如下:
输出方程:
状态方程:
其中:
式中部分相关的参数为:
二、被控对象反馈线性化
取状态量
,
,
,
,
系统化为:
令
,
,
可得:
可知,系统有相对阶向量
,构造矩阵
得:
则,当
时,有
可逆,有反馈控制律:
和变量替换:
将原系统转换成如下线性系统:
三、控制器设计
采用LQR方法设计控制器,利用Matlab的LQR工具得到一组反馈增益:
搭建simulink仿真程序如下图
设置系统初始状态为
,目标信号
,载荷在
平面运动轨迹为:
五、部分源代码
系统动力学模型模块model.m
function[sys,x0,str,ts]=model(t,x,u,flag)
switchflag,
case0,
[sys,x0,str,ts]=mdlInitializeSizes;
case1,
sys=mdlDerivatives(t,x,u);
case2,
sys=mdlUpdate(t,x,u);
case3,
sys=mdlOutputs(t,x,u);
case4,
sys=mdlGetTimeOfNextVarHit(t,x,u);
case9,
sys=mdlTerminate(t,x,u);
otherwise
error(['Unhandledflag=',num2str(flag)]);
end
function[sys,x0,str,ts]=mdlInitializeSizes
sizes=simsizes;
sizes.NumContStates=4;
sizes.NumDiscStates=0;
sizes.NumOutputs=6;
sizes.NumInputs=2;
sizes.DirFeedthrough=1;
sizes.NumSampleTimes=1;%atleastonesampletimeisneeded
sys=simsizes(sizes);
x0=[90*pi/180,-170*pi/180,0,0];
str=[];
ts=[00];
functionsys=mdlDerivatives(t,x,u)
theta1=x
(1);
theta2=x
(2);
d_theta1=x(3);
d_theta2=x(4);
u1=u
(1);
u2=u
(2);
m=10;
g=9.81;
l1=0.432;
l2=0.432;
m1=15.19;
m2=11.36;
a1=1/3*m1*l1^2+1/3*m2*l2^2+m2*l1^2;
a2=m2*l1*l2;
a3=1/3*m2*l2^2;
a4=1/2*m1*g*l1+m2*g*l1;
a5=1/2*m2*g*l2;
J=[-l1*sin(theta1)-l2*sin(theta1+theta2)-l2*sin(theta1+theta2);l1*cos(theta1)+l2*cos(theta1+theta2)l2*cos(theta1+theta2)];
dJ=[-l1*d_theta1*cos(theta1)-l2*(d_theta1+d_theta2)*cos(theta1+theta2)-l2*(d_theta1+d_theta2)*cos(theta1+theta2);...
-l1*d_theta1*sin(theta1)-l2*(d_theta1+d_theta2)*sin(theta1+theta2)-l2*(d_theta1+d_theta2)*sin(theta1+theta2)];
M=[a1+a2*cos(theta2)a3+(a2/2)*cos(theta2);a3+(a2/2)*cos(theta2)a3]+m*J'*J;
N=[-a2*sin(theta2)*(d_theta1*d_theta2+d_theta2^2/2);a2*sin(theta2)*d_theta1^2/2]+m*J'*dJ*[d_theta1;d_theta2];
G=[a4*cos(theta1)+a5*cos(theta1+theta2);a5*cos(theta1+theta2)]+m*J'*[g;g];
D=M;
E=-N-G;
d_x1=d_theta1;
d_x2=d_theta2;
M_st=D\E+D\[u1;u2];
d_x3=M_st
(1);
d_x4=M_st
(2);
sys=[d_x1,d_x2,d_x3,d_x4];
functionsys=mdlUpdate(t,x,u)
sys=[];
functionsys=mdlOutputs(t,x,u)
theta1=x
(1);
theta2=x
(2);
d_theta1=x(3);
d_theta2=x(4);
l1=0.432;
l2=0.432;
Px=l1*cos(theta1)+l2*cos(theta1+theta2);
Py=l1*sin(theta1)+l2*sin(theta1+theta2);
sys=[Px,Py,theta1,theta2,d_theta1,d_theta2];
functionsys=mdlGetTimeOfNextVarHit(t,x,u)
sampleTime=1;
sys=t+sampleTime;
functionsys=mdlTerminate(t,x,u)
sys=[];
反馈线性化模块fbl.m
function[sys,x0,str,ts]=fbl(t,x,u,flag)
switchflag,
case0,
[sys,x0,str,ts]=mdlInitializeSizes;
case1,
sys=mdlDerivatives(t,x,u);
case2,
sys=mdlUpdate(t,x,u);
case3,
sys=mdlOutputs(t,x,u);
case4,
sys=mdlGetTimeOfNextVarHit(t,x,u);
case9,
sys=mdlTerminate(t,x,u);
otherwise
error(['Unhandledflag=',num2str(flag)]);
end
function[sys,x0,str,ts]=mdlInitializeSizes
sizes=simsizes;
sizes.NumContStates=0;
sizes.NumDiscStates=0;
sizes.NumOutputs=2;
sizes.NumInputs=6;
sizes.DirFeedthrough=1;
sizes.NumSampleTimes=1;%atleastonesampletimeisneeded
sys=simsizes(sizes);
x0=[];
str=[];
ts=[00];
functionsys=mdlDerivatives(t,x,u)
sys=[];
functionsys=mdlUpdate(t,x,u)
sys=[];
functionsys=mdlOutputs(t,x,u)
theta1=u
(1);
theta2=u
(2);
d_theta1=u(3);
d_theta2=u(4);
ift==0
theta1=10*pi/180;
theta2=10*pi/180;
else
;
end
ift==0.01575
1;
;
else
;
end
v1=u(5);
v2=u(6);
m=10;
g=9.81;
l1=0.432;
l2=0.432;
m1=15.19;
m2=11.36;
a1=1/3*m1*l1^2+1/3*m2*l2^2+m2*l1^2;
a2=m2*l1*l2;
a3=1/3*m2*l2^2;
a4=1/2*m1*g*l1+m2*g*l1;
a5=1/2*m2*g*l2;
J=[-l1*sin(theta1)-l2*sin(theta1+theta2)-l2*sin(theta1+theta2);l1*cos(theta1)+l2*cos(theta1+theta2)l2*cos(theta1+theta2)];
dJ=[-l1*d_theta1*cos(theta1)-l2*(d_theta1+d_theta2)*cos(theta1+theta2)-l2*(d_theta1+d_theta2)*cos(theta1+theta2);...
-l1*d_theta1*sin(theta1)-l2*(d_theta1+d_theta2)*sin(theta1+theta2)-l2*(d_theta1+d_theta2)*sin(theta1+theta2)];
M=[a1+a2*cos(theta2)a3+(a2/2)*cos(theta2);a3+(a2/2)*cos(theta2)a3]+m*J'*J;
N=[-a2*sin(theta2)*(d_theta1*d_theta2+d_theta2^2/2);a2*sin(theta2)*d_theta1^2/2]+m*J'*dJ*[d_theta1;d_theta2];
G=[a4*cos(theta1)+a5*cos(theta1+theta2);a5*cos(theta1+theta2)]+m*J'*[g;g];
D=M;
E=-N-G;
M_st=D\E;
f3x=M_st
(1);
f4x=M_st
(2);
M_st1=D\[10;01];
g31x=M_st1(1,1);
g32x=M_st1(1,2);
g41x=M_st1(2,1);
g42x=M_st1(2,2);
Lf2h1=-l1*cos(theta1)*d_theta1^2-l1*sin(theta1)*f3x-l2*cos(theta1+theta2)*(d_theta1+d_theta2)^2-l2*sin(theta1+theta2)*(f3x+f4x);
Lf2h2=-l1*sin(theta1)*d_theta1^2+l1*cos(theta1)*f3x-l2*sin(theta1+theta2)*(d_theta1+d_theta2)^2+l2*cos(theta1+theta2)*(f3x+f4x);
A=J*[g31xg32x;g41xg42x];
out=-A\[Lf2h1;Lf2h2]+A\[v1;v2];
u1=out
(1);
u2=out
(2);
sys=[u1,u2];
functionsys=mdlGetTimeOfNextVarHit(t,x,u)
sampleTime=1;
sys=t+sampleTime;
functionsys=mdlTerminate(t,x,u)
sys=[];
- 配套讲稿:
如PPT文件的首页显示word图标,表示该PPT已包含配套word讲稿。双击word图标可打开word文档。
- 特殊限制:
部分文档作品中含有的国旗、国徽等图片,仅作为作品整体效果示例展示,禁止商用。设计者仅对作品中独创性部分享有著作权。
- 关 键 词:
- 非线性 作业