机器人动力学与控制学习笔记（六）————机器人PID控制

六、机器人鲁棒自适应PD控制

6.1  问题的提出  

        对于具有强耦合性和非线性的机器人系统而言，线性PD控制是最为简单且行之有效的控制方法，在工业机器人中得到了广泛应用。但实践表明，线性PD控制往往要求驱动机构有很大的初始输出，而实际驱动机构（通常是电机）往往不可能提供过大的初始力矩，且机械臂本身所承受的最大力矩也是有限的，这将使通过增大PD控制系数来进一步提高系数的性能受到限制。鉴于此，很多非线性PD控制方法被提出了出来，但常规的非线性PD控制器只有单纯的PD项，要求比例和微分项的系数仍较大，存在输出力矩较大的问题。

        现有一种自适应鲁棒PD控制策略，避免了初始输出力矩过大的弊端。该控制器由非线性PD反馈和补偿控制两部分构成，机器人不确定动力学部分由回归矩阵构成的自适应控制器进行补偿，并针对机器人 有界扰动的上确界是否已知设计了两种不同的扰动补偿法。该控制策略的优点在于当初始误差较大时，PD反馈起主要作用，通过非线性PD控制，避免了过大初始力矩输出；当误差较小时，自适应控制器起着主要的作用，从而保证了系统具有良好的动态性能。

6.2  机器人动力学模型及其结构特性

       考虑一个N关节的机器人力臂，其动态性能可以由以下二阶非线性微分方程描述：

                                                                                   （6.7）

式中：为关节角位移量，为机器人的惯性矩阵，表示离心力和哥氏力，为重力项，为控制力矩，为各种误差和扰动。

        机器人系统的动力学特性如下:

•  特性1   是一个斜对称矩阵。
•  特性2   惯性矩阵是对称正定矩阵，存在正数满足如下不等式：

                                                                                                 （6.8）

•  特性3   存在一个依赖于机械手参数的参数向量，使得满足线性关系：

                                                                              （6.9）

其中，为已知关节变量函数的回归矩阵，它是机器人广义坐标及其各阶导数的已知函数矩阵；是描述机器人质量特性的未知定常参数向量。

        假设1  为期望的关节角位移，的一阶导数和二阶导数存在。

        假设2  误差和扰动的范数满足：

                                                                                                         （6.10）

其中，分别为正常数，分别为跟踪误差和跟踪误差导数。

6.3  控制器的设计

        分别引入变量和，并令：

                                                                                                                                （6.11）

                                                                                                                            （6.12）
 其中常数，则可推出：

                                                                                                                                 （6.13）

        由式（6.9）中的机器人线性关系特性，取，得：  

                                                                        （6.14）

        由式（6.13）得，将其代入上式，得：

                                                                     

        将上式结合式（6.7），得：

                                                                         （6.15）

1.扰动信号的上确界已知时控制器的设计

        对于式（6.7）所示的机器人系统，在误差扰动信号的上确界已知时，采用以下控制器和自适应律，可保证系统全局渐进稳定。

                                                                       （6.16）

                                    ，   （6.17）

 的参数估计律取：

                                                                                                      （6.18）

式中

                                            

                     

                     

                                     

                                   

其中均大于零，为正定对称阵。

证明过程略。

2.扰动信号的上确界未知时控制器的设计

定理  当误差扰动信号的上确界为未知时，设计控制器为：

                                                                        （6.21）

                                                                                                                  （6.22）

                                                                                                             （6.23）

                                                                                                                   （6.24）

其中的取值同式（6.16），并保证满足式（6.19），的估计值通过式（6.18）求得，，，，为的估值，均为任意的正常数。

        对式（6.7）所示的机器人系统，并且误差扰动信号的上确界未知时，采用式（6.21）和式（6.22）的控制律可保证系统全局渐进稳定。

证明过程略。

6.4  机器人动态方程的线性化推导

        考虑动态方程：

                                                  

        为了实现控制律式（6.16）、式（6.17）和自适应律式（6.18），必须给出式（6.28）中和的表达。

        针对双力臂机器人动态方程：

                          （6.26）

其中

                                  

                                                  

                                                                  

                                                         

                                

                                                  

则式（6.25）中的第一个关节方程为：

                            

则式（6.25）中的第二个关节方程为：

                           

        根据机器人系统的动力学特性3，取回归矩阵的表达式为：

                                                             

                                                                

             

                                                                     

                                                                   

                                  

其中，。

        可见，由于的表达式不含有和，故本控制算法可实现变负载的机械手控制。

6.5  仿真实例

        被控对象为双力臂机器人动态方程式（6.26），。

        误差扰动、位置指令和系统的初始状态分别为：
                                 

                                                            
        控制参数取

                                

                                

                                

        采用S函数进行控制器和被控对象的设计。按扰动上确界已知和未知两种情况进行仿真。

仿真之一

        仿真程序

        控制主程序：chap2_2sim.mdl

        指令信号子程序：chap2_2input.m

function[sys,x0,str,ts] = input(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(falg)]);
end

function[sys,x0,str,ts] = mdlInitializeSizes
sizes = simsizes;
sizes.NumOutputs = 6;
sizes.NumInputs = 0;
sizes.DirFeedthrough = 0;
sizes.NumSampleTime = 0;
sys = simsizes(sizes);
x0 =[];
str =[];
ts =[];

function sys = mdlOutputs(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);

sys(1) = q1_d;
sys(2) = dq1_d;
sys(3) = ddq1_d;
sys(4) = q2_d;
sys(5) = dq2_d;
sys(6) = ddq2_d;

控制器子程序：chap2_2ctrl.m

function [sys,x0,str,ts] = control_strategy(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.NumOutputs      = 2;
sizes.NumInputs       = 13;
sizes.DirFeedthrough  = 1;
sizes.NumSampleTimes  = 0;
sys = simsizes(sizes);
x0 = [];
str =[];
ts = [];

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);
q2 = u(9);dq2 = u(10);

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;
ddqr = ddq_d-gama*de;

d1 = 2;d2 = 3;d3 = 6;
u1 = -(d1+d2*norm([e(1),e(2)])+d3*norm([de(1),de(2)]))*sign(y(1));
u2 = -(d1+d2*norm([e(1),e(2)])+d3*norm([de(1),de(2)]))*sign(y(2));

Kp1 = [180,0;0,190];
Kp2 = [150,0;0,150];
Kv1 = [180,0;0,190];
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)+ddqr2(2)*cos(q2)-dq2*dqr(1)*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)/(alfal+abs(e(1)))))*e(1)-(Kv1(1,1)+Kv2(1,1)/(beta1+abs(de(1))))*de(1)+R(1)+u1;
tol(2) = -(Kp1(2,2)+Kp2(2,2)/(alfal+abs(e(2)))))*e(2)-(Kv1(2,2)+Kv2(2,2)/(beta2+abs(de(2))))*de(2)+R(2)+u2;

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

自适应算法子程序：chap2_2adapt.m

function [sys,x0,str,ts] = para_estimate(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

function [sys,x0,str,ts] = mdlInitializeSizes
sizes = simsizes;
sizes.NumConStates    = 3;
sizes.NumDiscStates   = 0;
sizes.NumOutputs      = 3;
sizes.NumInputs       = 10;
sizes.DirFeedthrough  = 1;
sizes.NumSampleTimes  = 0;

sys = simsizes(sizes);
x0 = [4.1,1.9,1.7];
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);
q2 = u(9);dq2 = u(10);
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;
ddq_d = [ddq1_d,ddq2_d]';
dq_d  = [dq1_d,dq2_d]';

dqr = dq_d-gama*q_error;
ddqr = ddq_d-gama*dq_eror;

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);
S = -Gama*FAI'*y;
for i=1:1:3
  sys(i)=S(i);
end

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

被控对象子程序：chap2_2plant.m

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

function [sys,x0,str,ts] = mdlInitializeSizes
sizes = simsizes;
sizes.NumConStates    = 4;
sizes.NumDiscStates   = 4;
sizes.NumOutputs      = 4;
sizes.NumInputs       = 4;
sizes.DirFeedthrough  = 4;
sizes.NumSampleTimes  = 4;

sys = simsizes(sizes);
x0 = [0.10,0,0.10,0];
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);
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*r2cos(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_2plot.m

close all;

figure(1);
subplot(211);
plot(t,qd(:,1),'r',t,q(:,1),'b');
xlabel('time(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('time(s)');
ylabel('position tracking of link 2');
subplot(212);
plot(t,qd(:,5),'r',t,q(:,4),'b');
xlabel('time(s)');
ylabel('velocity tracking of link 2');

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');