【原创】柔性机械臂标准LQR、非标准LQR控制

liujinxin

模型：柔性机械臂位置控制包括刚体位移(转动)和振动位移(考虑一阶模态振型)，如何在机械臂移动的过程中抑制端部振动，达到位置精度，下面采用LQR输出调节器，状态调节器，并考虑集中非标准的LQR调节器，控制效果明显~~原创~~请支持~~
模型~~



代码：

%%%%%%%%%%%%%标准LQR输出调节器
clc
clear
w=100;
bb=0.8;
A=[0 1 0 0;-w 0 1*w 0;0 0 0 1;0 0 0 0];
B=[0;1-bb;0;1];
I=eye(4);
C=[1 0 0 0];
D=0;
Q=diag([1 0 0 0]);
R=1;
[K,P,E]=lqr(A,B,Q,R);%help lqr
sscl=ss(A-B*K,[0;0;0;0],C,0); %help ss
[y t x]=initial(sscl,[pi/2 0 pi/2 0],15); %help initial
figure(1);
subplot(2,1,1)
plot(t,K*x','r','linewidth',2)
hold on
plot(t,y(:,1),'linewidth',2)
hold on
plot(t,x(:,2),'m','linewidth',2)
legend('输入力矩U','输出总体转角φ1=φ','输出总体角速度φ2=dφ/dt')
grid on
xlabel('时间/s')
ylabel('力矩/Nm;角度/rad;角速度/rad/s')
subplot(2,1,2)
plot(t,x(:,3),'g','linewidth',2)
hold on
plot(t,x(:,4),'c','linewidth',2)
grid on
legend('输出刚性转角θ1=θ','输出刚性角速度θ2=dθ/dt')
xlabel('时间/s')
ylabel('角度/rad;角速度/rad/s')

%%%%%%%%%%%%标准LQR状态调节器
clc
clear
w=100;
bb=0.8;
A=[0 1 0 0;-w 0 1*w 0;0 0 0 1;0 0 0 0];
B=[0;1-bb;0;1];
I=eye(4);
C=[1 0 0 0];
D=0;
Q=diag([1 1 1 1]);
R=1;
[K,P,E]=lqr(A,B,Q,R);%help lqr
sscl=ss(A-B*K,[0;0;0;0],C,0); %help ss
[y t x]=initial(sscl,[pi/2 0 pi/2 0],15); %help initial
figure(2);
subplot(2,1,1)
plot(t,K*x','r','linewidth',2)
hold on
plot(t,y(:,1),'linewidth',2)
hold on
plot(t,x(:,2),'m','linewidth',2)
legend('输入力矩U','输出总体转角φ1=φ','输出总体角速度φ2=dφ/dt')
grid on
xlabel('时间/s')
ylabel('力矩/Nm;角度/rad;角速度/rad/s')
subplot(2,1,2)
plot(t,x(:,3),'g','linewidth',2)
hold on
plot(t,x(:,4),'c','linewidth',2)
grid on
legend('输出刚性转角θ1=θ','输出刚性角速度θ2=dθ/dt')
xlabel('时间/s')
ylabel('角度/rad;角速度/rad/s')


%%%%%%具有规定衰减速度的LQR状态调节器
clc
clear
w=100;
bb=0.8;
alpha=2;
A=[alpha 1 0 0;-w alpha 1*w 0;0 0 alpha 1;0 0 0 alpha];
B=[0;1-bb;0;1];
C=[1 0 0 0];
D=0;
Q=diag([80 80 1 1]);
R=1;
[K,P,E]=lqr(A,B,Q,R);%help lqr
sscl=ss(A-B*K,[0;0;0;0],C,0); %help ss
[y t x]=initial(sscl,[pi/2 0 pi/2 0],10); %help initial
figure(3);
subplot(2,1,1)
plot(t,K*x','r','linewidth',2)
hold on
plot(t,y(:,1),'linewidth',2)
hold on
plot(t,x(:,2),'m','linewidth',2)
legend('输入力矩U','输出总体转角φ1=φ','输出总体角速度φ2=dφ/dt')
grid on
xlabel('时间/s')
ylabel('力矩/Nm;角度/rad;角速度/rad/s')
subplot(2,1,2)
plot(t,x(:,3),'g','linewidth',2)
hold on
plot(t,x(:,4),'c','linewidth',2)
grid on
legend('输出刚性转角θ1=θ','输出刚性角速度θ2=dθ/dt')
xlabel('时间/s')
ylabel('角度/rad;角速度/rad/s')

%%%%%%具有非零设定点的LQR状态调节器
clc
clear
w=100;
bb=0.8;
l=1000;
alpha=0;
A=[alpha 1 0 0;-w alpha 1*w 0;0 0 alpha 1;0 0 0 alpha];
B=[0;1-bb;0;1];
C=[1 0 0 0];
D=0;
Q=diag([1 1 1 1]);
R=1;
[K,P,E]=lqr(A,B,Q,R);%help lqr
sscl=ss(A-B*K,[0;0;0;0],C,0); %help ss
[y t x]=initial(sscl,[pi/2 0 pi/2 0],10); %help initial
figure(4);
subplot(2,1,1)
plot(t,K*x','r','linewidth',2)
x(:,1)=x(:,1)+pi/4;
x(:,3)=x(:,3)+pi/4;
hold on
plot(t,x(:,1),'linewidth',2)
hold on
plot(t,x(:,2),'m','linewidth',2)
legend('输入力矩U','输出总体转角φ1=φ','输出总体角速度φ2=dφ/dt')
grid on
xlabel('时间/s')
ylabel('力矩/Nm;角度/rad;角速度/rad/s')
subplot(2,1,2)
plot(t,x(:,3),'g','linewidth',2)
hold on
plot(t,x(:,4),'c','linewidth',2)
grid on
legend('输出刚性转角θ1=θ','输出刚性角速度θ2=dθ/dt')
xlabel('时间/s')
ylabel('角度/rad;角速度/rad/s')

liujinxin

自己顶一下

www.zdscj.com|

帮楼主顶一下，学习学习！

代号农民工

顶一个楼主辛苦了，加油