第5章〓机械手自适应迭代学习控制 迭代学习控制是通过迭代修正改善某种控制目标，它的算法较为简单，且能在给定的时间范围内实现未知对象实际运行轨迹以高精度跟踪给定期望轨迹，不依赖系统的精确数学模型。因而一经推出，就在机器人控制领域得到广泛的运用。 迭代学习控制(iterative learning control，ILC)是智能控制中具有严格数学描述的一个分支。1984年，Arimoto［1］等人提出了迭代学习控制的概念，该控制方法适合于具有重复运动性质的被控对象，它不依赖于系统的精确数学模型，能以非常简单的方式处理不确定度相当高的非线性强耦合动态系统。目前,迭代学习控制在学习算法、收敛性、鲁棒性、学习速度及工程应用研究上取得了巨大的进展［2,3］。 5.1控制器增益自适应整定的机械手迭代学习控制 本节通过对文献［4］的控制方法进行详细推导及仿真分析，研究一类机械手力臂自适应迭代学习控制的设计方法。并针对该控制算法存在的问题，提出了相应的改进算法。 5.1.1问题的提出 考虑n关节机械手，其动态方程如下： D(qj(t))q¨j(t)+C(qj(t), q·j(t))q·j(t)+G(qj(t),q·j(t))+Ta(t)=Tj(t)(5.1) 其中,j为迭代次数，t∈［0,tf］，qj(t)∈Rn和 q¨j(t)∈Rn分别为关节角度，角速度和角加速度，D(qj(t))∈Rn×n为惯性项，C(qj(t),q·j(t))q·j(t)∈Rn表示离心力和哥氏力，G(qj(t),q·j(t))∈Rn为重力加摩擦力项，Ta(t)∈Rn为可重复的未知干扰，Tj(t)∈Rn为控制输入。 机械手动态方程满足如下特性： (1) D(qj(t))为对称正定的有界矩阵； (2) D·(qj(t))-2C(qj(t),q·j(t))为斜对称阵，即满足xT( D·(qj(t))-2C(qj(t),q·j(t)))x=0。 机械手动态方程满足如下假设条件： (1) 期望轨迹qd(t)在t∈［0,tf］内三阶可导； (2) 迭代过程满足初始条件，即qd(0)-qj(0)=0，q·d(0)-q·j(0)=0，j∈N。 5.1.2控制器设计 针对系统式(5.1)，如果满足机器人特性(1)和(2)以及假设(1)和(2)，则控制律设计为 Tj(t)=Kjpe(t)+Kjde·(t)+Tj-1(t)，j=0,1,…,N(5.2) 控制律中增益切换规则为 Kjp=β(j)K0p， Kjd=β(j)K0d， β(j+1)>β(j)(5.3) 其中,j=1,2,…,N，T-1(t)=0，ej(t)=qd(t)-qj(t)，e·j(t)=q·d(t)-q·j(t)，K0p和K0d为PD控制中初始的对角增益阵，且都为正定，β(j)为控制增益，且满足β(j)>1。 首先实现动态方程式(5.1)的线性化，沿着指令轨迹(qd(t),q·d(t),q¨d(t))，采用泰勒公式，对式(5.1)进行线性化，采用泰勒公式，D(q)线性化为 D(q)=D(qd)+Dqqd(q-qd)+OD(·) 其中，OD(·)为D(q)一阶展开式的残差，即 -D(q)q¨=-D(qd)q¨-Dqqdeq¨-OD(·)q¨ D(qd)q¨d-D(q)q¨=D(qd)q¨d-D(qd)q¨-Dqqdeq¨-OD(·)q¨ D(qd) e¨+Dqqdq¨e=D(qd)q¨d-D(qd)q¨-OD(·)q¨ 由于 Dqqdq¨e=Dqqd(q¨+q¨d-q¨d)e=Dqqdq¨de-Dqqde¨e 则 D(qd)e¨+Dqqdq¨de-Dqqde¨e=D(qd)q¨d-D(qd)q¨-OD(·)q¨(5.4) 同理 C(q,q·)=C(qd,q·d)+Cq qd,q·d (q-qd)+Cq·qd,q·d(q·- q·d)+OC(·) C(q,q·)e·+Cqqd,q·dq·de+Cq·qd,q·dq·de·-Cqqd,q·de·e-Cqqd,q·de·e· =C(qd,q·d)q·d-C(q,q·)q·-OC(·)q·(5.5) G(q,q·)=G(qd,q·d)+Gqqd,q·d(q-qd)+Gq·qd,q·d(q·- q·d)+OG(·) Gq·qd,q·de·+Gqqd,q·de=G(qd,q·d)-G(q,q·)+OG(·)(5.6) 由式(5.4)、式(5.5)和式(5.6)得 D(t)e¨+［C+C1］e·+Fe+n(e¨,e·,e,t)=H-(Dq¨+Cq·+G)(5.7) 其中 n(e¨,e·,e,t)=-Dqqde¨e-Cqqd,q·de·e-Cqqd,q·de·e·+OD(·)q¨+OC(·)q·-OG(·) 忽略残差项n(e¨,e·,e,t)，将式(5.1)代入式(5.7)，得第j次迭代的机器人动力学方程为 D(t)e¨j(t)+［C(t)+C1(t)］e·j(t)+F(t)ej(t)-Ta(t)=H(t)-Tj(t) 其中 D(t)=D(qd(t)) C(t)=C(qd(t),q·d(t)) C1(t)=Cq· qd(t),q·d(t) q·d(t)+Gq· qd(t),q·d(t) F(t)=Dq qd(t) q¨d(t)+Cqqd(t),q·d(t)q·d(t)+Gqqd(t) H(t)=D(q·d(t))q¨d(t)+C(qd(t),q·d(t))q·d(t)+G(qd(t)) 针对第j次迭代和第j+1次迭代，方程式(5.7)可写为 D(t)e¨j(t)+［C(t)+C1(t)］e·j(t)+F(t)ej(t)-Ta(t)=H(t)-Tj(t) D(t)e¨j+1(t)+［C(t)+C1(t)］e·j+1(t)+F(t)ej+1(t)-Ta(t)=H(t)-Tj+1(t)(5.8) 为了简单起见，取K0p=ΛK0d，并定义 yj(t)= e·j(t)+Λej(t)(5.9) 文献［4］提出了如下定理: 定理5.1假设系统式(5.1)满足机器人特性(P1，P2)和假设条件(A1，A2)。采用控制律式(5.2)及其增益切换规则式(5.3)，则对于t∈［0,tf］，有 qj(t)j→∞qd(t), q·j(t)j→∞q·d(t) 其中控制增益需要满足如下条件： lp=λmin(K0d+2C1-2ΛD)>0 lr=λmin(K0d+2C+2F/Λ-2C·1/Λ)>0 lplr≥‖F/Λ-(C+C1- ΛD)2max(5.10) 其中λmin(A)为矩阵A的最小特征值，‖M‖max=max‖M(t)‖，t∈［0,tf］，‖M‖为矩阵M的欧氏范数。 5.1.3收敛性分析 参考文献［4］，下面给出了对定理5.1的收敛性分析和说明。定义Lyapunov函数为 Vj=∫t0exp(-ρτ)yjTK0dyjdτ≥0 其中,K0d>0为PD控制中D控制项的初始增益，ρ为正实数。 由式(5.9)得 δyj=yj+1-yj= e·j+1+Λej+1-(e·j+Λej)=δe·j+Λδej(5.11) 由式(5.8)得 D(t)(e¨j+1(t)- e¨j(t)) =-［C(t)+C1(t)］(e·j+1(t)- e·j(t))- F(t)(ej+1(t)-ej(t))-(Tj+1(t)-Tj(t))(5.12) 由式(5.11)和式(5.12)得 Dδy·j = Dδe¨j+1+DΛδe·j=D(e¨j+1- e¨j)+DΛ(e·j+1- e·j) =-［C(t)+C1(t)］(e·j+1(t)- e·j(t))-F(t)(ej+1(t)-ej(t))- (Tj+1(t)-Tj(t))+DΛ(e·j+1- e·j) =-［C(t)+C1(t)］δe·j-F(t)δej-(Kj+1pej+1(t)+Kj+1de·j+1(t))+ DΛ(e·j+1- e·j) 由K0p=ΛK0d和式(5.3)知Kj+1p=ΛKj+1d，考虑到式(5.11)，可得 Dδy·j=-［C+C1］(δyj-Λδej)-F(t)δej+DΛ(e·j+1- e·j)- (ΛKj+1dej+1+Kj+1de·j+1) 由于 DΛ(e·j+1- e·j)=DΛ［(yj+1-Λej+1)-(yj-Λej)］=DΛδyj-DΛ2δej ΛKj+1dej+1+Kj+1de·j+1=Kj+1d(Λej+1+e·j+1)=Kj+1dyj+1=Kj+1d(δyj+yj) 则 Dδy·j =-(C+C1)(δyj-Λδej)-Fδej+DΛδyj-DΛ2δej-Kj+1d(δyj+yj) =-(C+C1-ΛD+Kj+1d)δyj-(F-Λ(C+C1-ΛD))δej-Kj+1dyj 则 Kj+1dyj=-Dδy·j-(C+C1-ΛD+Kj+1d)δyj -(F-Λ(C+C1-ΛD))δej (5.13) 由Vj的定义，得 Vj+1=∫t0exp(-ρτ)yj+1TK0dyj+1dτ 定义ΔVj=Vj+1-Vj，由式(5.3)和式(5.11)，并将式(5.13)代入，得 ΔVj =∫t0exp(-ρτ)(δyjT+yj)TK0d(δyjT+yj)dτ-∫t0exp(-ρτ)yjTK0dyjdτ =∫t0exp(-ρτ)(δyjTK0dδyj+2δyjTK0dyj)dτ =1β(j+1)∫t0exp(-ρτ)(δyjTKj+1dδyj+2δyjTKj+1dyj)dτ =1β(j+1)∫t0exp(-ρτ)δyjTKj+1dδyjdτ-2∫t0exp(-ρτ)δyjTDδy·jdτ- 2∫t0exp(-ρτ)δyjT((C+C1-ΛD+Kj+1d)δyj+ (F-Λ(C+C1-ΛD))δej)dτ 应用分部积分方法，根据初始条件(A2)知δyj(0)=0，则 ∫t0exp(-ρτ)δyjTDδy·jdτ=exp(-ρτ)δyjTDδyjt0-∫t0(exp(-ρτ)δyjTD)′δyjdτ =exp(-ρt)δyjT(t)D(t)δyj(t)+ρ∫t0exp(-ρτ)δyjTDδyjdτ- ∫t0exp(-ρτ)δyjTDδy·jdτ-∫t0exp(-ρτ)δyjTD·δyjdτ 将上式两端同项合并，得 2∫t0exp(-ρτ)δyjTDδy·jdτ=exp(-ρt)δyjT(t)D(t)δyj(t)+ ρ∫t0exp(-ρτ)δyjTDδyjdτ- ∫t0exp(-ρτ)δyjTD·δyjdτ 由特性(P2)，可得 ∫t0δyjTD·δyjdτ=2∫t0δyjTCδyjdτ 则 ΔVj =1β(j+1)-exp(-ρτ)δyjTD(t)δyj(t)-ρ∫t0exp(-ρτ)δyjTDδyjdτ- 2∫t0exp(-ρτ)δyjT(F-Λ(C+C1-ΛD))δejdτ- ∫t0exp(-ρτ)δyjT(Kj+1d+2C1-2ΛD)δyjdτ 由于 ∫t0exp(-ρτ)δyjTKj+1dδyjdτ =β(j+1)∫t0exp(-ρτ)δyjTK0dδyjdτ ≥∫t0exp(-ρτ)δyjTK0dδyjdτ 利用式(5.11)，并将δyj展开成δe·j+Λδej，得 ΔVj≤ 1β(j+1)-exp(-ρτ)δyjTD(t)δyj(t)-ρ∫t0exp(-ρτ)δyjTDδyjdτ- 2∫t0exp(-ρτ)δe·jT(F-Λ(C+C1-ΛD))δejdτ- 2Λ∫t0exp(-ρτ)δejT(F-Λ(C+C1-ΛD))δejdτ- ∫t0exp(-ρτ)δe·jT(K0d+2C1-2ΛD)δe·jdτ- 2Λ∫t0exp(-ρτ)δejT(K0d+2C1-2ΛD)δe·jdτ- Λ2∫t0exp(-ρτ)δejT(K0d+2C1-2ΛD)δejdτ 应用分部积分方法，根据初始条件，有δej(0)=0，则 ∫t0exp(-ρτ)δejT(K0d+2C1-2ΛD)δe·jdτ=exp(-ρτ)δejT(K0d+2C1-2ΛD)δejt0- ∫t0-ρexp(-ρτ)δejT(K0d+2C1-2ΛD)δejdτ- ∫t0exp(-ρτ)δe·jT(K0d+2C1-2ΛD)δejdτ- ∫t0exp(-ρτ)δejT(2C·1-2ΛD·)δejdτ 将上式两端同项合并，并将两端同乘以Λ，得 2Λ∫t0e-ρτδejT(K0d+2C1-2ΛD)δe·jdτ=Λexp(-ρt)δejT(K0d+2C1-2ΛD)δej+ ρΛ∫t0exp(-ρτ)δejT(K0d+2C1-2ΛD)δejdτ+ 2Λ∫t0exp(-ρτ)δejT(ΛD·- C·1)δejdτ 则 ΔVj ≤1β(j+1)-exp(-ρτ)δyjTDδyj(t)-ρ∫t0exp(-ρτ)δyjTDδyjdτ- Λexp(-ρτ)δejT(K0d+2C1-2ΛD)δej- ρΛ∫t0exp(-ρτ)δejT(K0d+2C1-2ΛD)δejdτ-∫t0exp(-ρτ)wdτ ≤1β(j+1)-exp(-ρτ)δyjTDδyj(t)-ρ∫t0exp(-ρτ)δyjTDδyjdτ- Λexp(-ρτ)δejTlpδej-ρΛ∫t0exp(-ρτ)δejTlpδejdτ-∫t0exp(-ρτ)wdτ 其中 w =δe·jT(K0d+2C1-2ΛD)δe·j+2δe·jT(F-Λ(C+C1-ΛD))δej+ 2ΛδejT(ΛD·- C·1)δej+Λ2δejT(K0d+2C1-2ΛD)δej+ 2ΛδejT(F-Λ(C+C1-ΛD))δej =δe·jT(K0d+2C1-2ΛD)δe·j+2Λδe·jT(F/Λ-(C+C1-ΛD))δej+ Λ2δejT(K0d+2C+2F/Λ-2C·1/Λ)δej 取Q=F/Λ-(C+C1-ΛD)，则由式(5.10)，得 w≥lp‖δe·‖2+2Λδe·TQδe+Λ2lr‖δe‖2 采用CauchySchwarz不等式，有 δe·TQδe ≥-‖δe·‖‖Q‖max‖δe‖ w≥lp‖δe·‖2-2Λ‖δe·‖‖Qmax‖‖δe‖+Λ2lr‖δe‖2 =lp‖δe·‖-Λlp‖Qmax‖‖δe‖2+Λ2lr-1lp‖Q‖2max‖δe‖2≥0 则ΔVj≤0，即 Vj+1≤Vj 由于K0d为正定阵，Vj>0且Vj有界，则当j→∞时，yj(t)→0。由于ej(t)和e·j(t)为两个相互独立的变量，Λ为正定常数阵，如取j→∞，则ej(t)→0，e·j(t)→0，t∈［0,tf］。 通过上面的分析，可得结论： qj(t)j→∞qd(t), q·j(t)j→∞q·d(t),t∈［0,tf］ 定理5.1中描述的控制算法的不足之处： 针对的是重复性干扰，忽略了线性化残差项n(e¨j,e·j,ej,t)，且机械手动力学方程为确定的。针对这一问题，5.2节中给出了改进的控制律。 5.1.4仿真实例 针对双关节机械手动态方程式(5.1)进行仿真，方程中的各项取 D(q)=i1+i2+2m2r2l1cosq2i2+m2r2l1cos(q2) i2+m2r2l1cos(q2)i2 C(q,q·)=-m2r2l1q·2sin(q2)-m2r2l1(q·1+ q·2)sin(q2) m2r2l1q·1sin(q2)0 G(q)=(m1r1+m2l1)gcosq1+m2r2gcos(q1+q2) m2r2gcos(q1+q2) 可重复的干扰为d1(t)=a0.3sint, d2(t)=a0.1(1-e-t)，a=1，Ta=d1d2T。 系统参数取m1=10, m2=5, l1=1, l2=0.5, r1=0.5, r2=0.25，i1=0.83+m1r21+m2l21，i2=0.3+m2r22。 角度期望轨迹q1=sin3t, q2=cos3t，取Λ=10 01，控制器参数设计为K0p=K0d=2100 0210，β(j)=2j，Kjp=2jK0p, Kjd=2jK0d, j=1,2,…,N。 系统的初始状态为x=3001T，取tf=5，迭代次数取5次。仿真结果见图5.1至图5.5所示。 图5.1双关节5次迭代的角度跟踪过程 图5.2双关节第5次的角度跟踪 图5.3双关节5次角度跟踪的误差收敛过程 图5.4双关节第5次的角速度跟踪 图5.5双关节5次角速度跟踪的误差收敛过程 仿真程序： (1) 主程序： chap5_1main.m %Adaptive switching Learning Control for 2DOF robot manipulators clear all; close all; t=［0:0.001:5］'; T1(1:5001)=0; T1=T1'; T2=T1; T=［T1 T2］; k(1:5001)=0; k=k'; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% M=5; for i=0:1:M% Start Learning Control i pause(0.01); sim('chap5_1sim',［0,5］); q1=q(:,3); q2=q(:,4); dq1=q(:,1); dq2=q(:,2); q1d=qd(:,1); q2d=qd(:,2); dq1d=qd(:,3); dq2d=qd(:,4); e1=q1d-q1; e2=q2d-q2; de1=dq1d-dq1; de2=dq2d-dq2; figure(1); subplot(211); hold on; plot(t,q1,'b',t,q1d,'r'); xlabel('time(s)');ylabel('q1d,q1'); subplot(212); hold on; plot(t,q2,'b',t,q2d,'r'); xlabel('time(s)');ylabel('q2d,q2'); j=i+1; times(j)=i; e1i(j)=max(abs(e1)); e2i(j)=max(abs(e2)); de1i(j)=max(abs(de1)); de2i(j)=max(abs(de2)); end%End of i %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% figure(2); subplot(211); plot(t,q1d,'r',t,q1,'b'); xlabel('time(s)');ylabel('q1d,q1'); subplot(212); plot(t,q2d,'r',t,q2,'b'); xlabel('time(s)');ylabel('q2d,q2'); figure(3); plot(times,e1i,'*-r',times,e2i,'o-b'); title('Change of maximum absolute value of error1 and error2 with times i'); xlabel('times');ylabel('error1 and error2'); figure(4); subplot(211); plot(t,dq1d,'r',t,dq1,'b'); xlabel('time(s)');ylabel('dq1d,dq1'); subplot(212); plot(t,dq2d,'r',t,dq2,'b'); xlabel('time(s)');ylabel('dq2d,dq2'); figure(5); plot(times,de1i,'*-r',times,de2i,'o-b'); title('Change of maximum absolute value of derror1 and derror2 with times i'); xlabel('times');ylabel('derror1 and derror2'); (2) Simulink子程序： chap5_1sim.mdl。 (3) 被控对象子程序： chap5_1plant.m。 function ［sys,x0,str,ts］ = spacemodel(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.NumContStates= 4; sizes.NumDiscStates = 0; sizes.NumOutputs = 4; sizes.NumInputs = 2; sizes.DirFeedthrough = 0; sizes.NumSampleTimes = 1; sys = simsizes(sizes); x0= ［3;0;0;1］; str = ［］; ts = ［0 0］; function sys=mdlDerivatives(t,x,u) a=1.0; d1=a*0.3*sin(t); d2=a*0.1*(1-exp(-t)); dq1=x(1); dq2=x(2); q1=x(3); q2=x(4); tol1=u(1); tol2=u(2); m1=10;m2=5; l1=1;l2=0.5; r1=0.5;r2=0.25; i1=0.83+m1*r1^2+m2*l1^2; i2=0.3+m2*r2^2; g=9.8; D=［i1+i2+2*m2*r2*l1*cos(q2) i2+m2*r2*l1*cos(q2); i2+m2*r2*l1*cos(q2) i2］; C=［-m2*r2*l1*dq2*sin(q2),-m2*r2*l1*(dq1+dq2)*sin(q2); m2*r2*l1*dq1*sin(q2),0］; G=［(m1*r1+m2*l1)*g*cos(q1)+m2*r2*g*cos(q1+q2);m2*r2*g*cos(q1+q2)］; %%%%%%%%%%%%%%%%%%%%%%%%%% D2=inv(D); T=［d1;d2］; A=-D2*C; Z=-D2*G; sys(1)=A(1,1)*x(1)+A(1,2)*x(2)+Z(1)+D2(1,1)*(-T(1)+tol1)+D2(1,2)*(-T(2)+tol2); sys(2)=A(2,1)*x(1)+A(2,2)*x(2)+Z(2)+D2(2,1)*(-T(1)+tol1)+D2(2,2)*(-T(2)+tol2); sys(3)=x(1); sys(4)=x(2); function sys=mdlOutputs(t,x,u) sys(1)=x(1);%第一个关节角速度dq1 sys(2)=x(2); %第二个关节角速度dq2 sys(3)=x(3); %第一个关节角度q1 sys(4)=x(4); %第二个关节角度q2 (4) 控制器子程序： chap5_1ctrl.m。 function ［sys,x0,str,ts］ = spacemodel(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; sizes.NumInputs = 9; sizes.DirFeedthrough = 1; sizes.NumSampleTimes = 1; sys = simsizes(sizes); x0= ［］; str = ［］; ts = ［0 0］; function sys=mdlOutputs(t,x,u) q1d=u(1);q2d=u(2); dq1d=u(3);dq2d=u(4); dq1=u(5);dq2=u(6); q1=u(7);q2=u(8); j=u(9); e1=q1d-q1; e2=q2d-q2; de1=dq1d-dq1; de2=dq2d-dq2; Fai=eye(2); Kd0=［210 0;0 210］; % Iteration number if j==0 beta=1; else beta=2*j; end sys(1)=beta*210*(e1+de1); sys(2)=beta*210*(e2+de2); (5) 指令程序： chap5_1input.m。 function ［sys,x0,str,ts］ = spacemodel(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 = 4; sizes.NumInputs = 0; sizes.DirFeedthrough = 1; sizes.NumSampleTimes = 1; sys = simsizes(sizes); x0= ［］; str = ［］; ts = ［0 0］; function sys=mdlOutputs(t,x,u) q1d=sin(3*t); q2d=cos(3*t); dq1d=3*cos(3*t); dq2d=-3*sin(3*t); sys(1)=q1d; sys(2)=q2d; sys(3)=dq1d; sys(4)=dq2d; 5.2基于增益自适应整定的机械手迭代学习控制的改进 5.2.1算法的改进 5.1节中定理5.1描述的控制算法的不足之处为： 它针对重复性干扰，忽略了线性化残差项n(e¨j,e·j,ej,t)，且机械手动力学方程为确定的。 为了弥补上述不足，本节中在控制律中加入鲁棒项，实现干扰为不重复、考虑线性化残差项n(e¨j,e·j,ej,t)的不确定机械手自适应迭代学习控制。 带有非重复干扰的不确定机器人系统动力学方程为 (D(qj(t))+ΔD(qj(t)))q¨j(t)+(C(qj(t),q·j(t))+ΔC(qj(t),q·j(t)))q·j(t)+ G(qj(t),q·j(t))+ΔG(qj(t),q·j(t))+Tja(t)=Tj(t)(5.14) 被控对象式(5.14)可写为 D(qj(t))q¨j(t)+C(qj(t),q·j(t))q·j(t)+G(qj(t),q·j(t))+dj(t)=Tj(t)(5.15) 其中 dj(t)=ΔD(qj(t))q¨j(t)+ΔC(qj(t),q·j(t))q·j(t)+ΔG(qj(t),q·j(t))+Tja(t) 沿着指令轨迹(qd(t),q·d(t),q¨d(t))，采用泰勒公式，则方程式(5.15)可线性化为 D(t)e¨+［C+C1］e·+Fe+n(e¨,e·,e,t)=H-(Dq¨+Cq·+G)(5.16) 其中 n(e¨,e·,e,t)=-Dqqde¨e-Cqqd,q·de·e-Cqqd,q·de·e·+OD(·)q¨+OC(·)q·-OG(·) 将式(5.15)代入式(5.16)，则 D(t)e¨j(t)+［C(t)+C1(t)］e·j(t)+F(t)ej(t)+n(e¨j,e·j,ej,t)-dj(t)=H(t)-Tj(t)(5.17) 取d1j(t)=-n(e¨j,e·j,ej,t)+dj(t)，则式(5.17)可写为 D(t)e¨j(t)+［C(t)+C1(t)］e·j(t)+F(t)ej(t)-d1j(t)=H(t)-Tj(t)(5.18) 于是 D(t)e¨j+1(t)+［C(t)+C1(t)］e·j+1(t)+F(t)ej+1(t)-d1j+1(t)=H(t)-Tj+1(t)(5.19) 设计鲁棒迭代学习控制律为 Tj(t)=Kjpej(t)+Kjde·j(t)+Tj-1(t)+Esgn(δyj-1), j=0,1,2,…,N(5.20) 其中，‖dj+11(t)-dj1(t)‖=‖δd1j(t)‖≤E。 证明： 收敛性分析的证明过程可参照5.1节的推导过程。 5.2.2仿真实例 针对双关节机械手动态方程式(5.1)进行仿真，取不可重复的干扰为d1(t)=a0.3sint,d2(t)=a0.1(1-e-t)，a=1为幅值为1的随机噪声，Ta=d1d2T。 角度期望轨迹q1=sin3t，q2=cos3t，取Λ=10 01，控制器取式(5.20)，控制参数取E=1.0，K0p=K0d=2100 0210，β(j)=2j，Kjp=2jK0p，Kjd=2jK0d，j=1,2,…,N。 系统的初始状态为x=3001T，迭代次数取15次。仿真结果如图5.6至图5.10所示。 图5.6双关节15次迭代的角度跟踪过程 图5.7双关节第15次的角度跟踪 图5.8双关节15次角度跟踪的误差收敛过程 图5.9双关节第15次的角速度跟踪 图5.10双关节15次角速度跟踪的误差收敛过程 仿真程序： (1) 主程序： chap5_2main.m。 clear all; close all; L=3001; t=［0:0.001:3］'; T1(1:L)=0; T1=T1'; T2=T1; T=［T1 T2］; e1(1:L)=0; e1=e1'; e2=e1; de1=e1; de2=de1; e=［e1 e2 de1 de2］; k(1:L)=0; k=k'; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% M=15; for i=0:1:M i pause(0.01); sim('chap5_2sim',［0,3］); q1=q(:,3); q2=q(:,4); dq1=q(:,1); dq2=q(:,2); q1d=qd(:,1); q2d=qd(:,2); dq1d=qd(:,3); dq2d=qd(:,4); e1=q1d-q1; e2=q2d-q2; de1=dq1d-dq1; de2=dq2d-dq2; figure(1); subplot(211); hold on; plot(t,q1,'b',t,q1d,'r'); xlabel('t (s)');ylabel('q1d,q1 (rad)'); subplot(212); hold on; plot(t,q2,'b',t,q2d,'r'); xlabel('t (s)');ylabel('q2d,q2 (rad)'); j=i+1; times(j)=i; e1i(j)=max(abs(e1)); e2i(j)=max(abs(e2)); de1i(j)=max(abs(de1)); de2i(j)=max(abs(de2)); end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% figure(2); subplot(211); plot(t,q1d,'r',t,q1,'b'); xlabel('t');ylabel('q1d,q1'); subplot(212); plot(t,q2d,'r',t,q2,'b'); xlabel('t');ylabel('q2d,q2'); figure(3); plot(times,e1i,'*-r',times,e2i,'o-b'); title('Change of maximum absolute value of error1 and error2 with times i'); xlabel('times');ylabel('error1 and error2'); figure(4); subplot(211); plot(t,dq1d,'r',t,dq1,'b'); xlabel('t');ylabel('dq1d,dq1'); subplot(212); plot(t,dq2d,'r',t,dq2,'b'); xlabel('t');ylabel('dq2d,dq2'); figure(5); plot(times,de1i,'*-r',times,de2i,'o-b'); title('Change of maximum absolute value of derror1 and derror2 with times i'); xlabel('times');ylabel('derror1 and derror2'); (2) Simulink子程序： chap5_2sim.mdl。 (3) 被控对象子程序： chap5_2plant.m。 function ［sys,x0,str,ts］ = spacemodel(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.NumContStates= 4; sizes.NumDiscStates = 0; sizes.NumOutputs = 4; sizes.NumInputs = 2; sizes.DirFeedthrough = 0; sizes.NumSampleTimes = 1; sys = simsizes(sizes); x0= ［3;0;0;1］; str = ［］; ts = ［0 0］; function sys=mdlDerivatives(t,x,u) %a=1; a=rands(1); d1=a*0.3*sin(3*pi*t); d2=a*0.1*(1-exp(-t)); dq1=x(1);%第一个关节角速度dq1 dq2=x(2); %第二个关节角速度dq2 q1=x(3); %第一个关节角度q1 q2=x(4); %第二个关节角度q2 tol1=u(1); tol2=u(2); m1=10;m2=5; l1=1;l2=0.5; r1=0.5;r2=0.25; i1=0.83+m1*r1^2+m2*l1^2; i2=0.3+m2*r2^2; g=9.8; D=［i1+i2+2*m2*r2*l1*cos(q2) i2+m2*r2*l1*cos(q2); i2+m2*r2*l1*cos(q2) i2］; C=［-m2*r2*l1*dq2*sin(q2),-m2*r2*l1*(dq1+dq2)*sin(q2); m2*r2*l1*dq1*sin(q2),0］; G=［(m1*r1+m2*l1)*g*cos(q1)+m2*r2*g*cos(q1+q2);m2*r2*g*cos(q1+q2)］; %%%%%%%%%%%%%%%%%%%%%%%%%% D2=inv(D); Ta=［d1;d2］; A=-D2*C; Z=-D2*G; sys(1)=A(1,1)*x(1)+A(1,2)*x(2)+Z(1)+D2(1,1)*(-Ta(1)+tol1)+D2(1,2)*(-Ta(2)+tol2); sys(2)=A(2,1)*x(1)+A(2,2)*x(2)+Z(2)+D2(2,1)*(-Ta(1)+tol1)+D2(2,2)*(-Ta(2)+tol2); sys(3)=x(1); sys(4)=x(2); function sys=mdlOutputs(t,x,u) sys(1)=x(1); %第一个关节角速度dq1 sys(2)=x(2); %第二个关节角速度dq2 sys(3)=x(3); %第一个关节角度q1 sys(4)=x(4); %第二个关节角度q2 (4) 控制器子程序： chap5_2ctrl.m。 function ［sys,x0,str,ts］ = spacemodel(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 = 6; sizes.NumInputs = 13; sizes.DirFeedthrough = 1; sizes.NumSampleTimes = 1; sys = simsizes(sizes); x0= ［］; str = ［］; ts = ［0 0］; function sys=mdlOutputs(t,x,u) q1d=u(1);q2d=u(2); dq1d=u(3);dq2d=u(4); dq1=u(5);dq2=u(6); q1=u(7);q2=u(8); j=u(9); e1=q1d-q1; e2=q2d-q2; de1=dq1d-dq1; de2=dq2d-dq2; e1_1=u(10); e2_1=u(11); de1_1=u(12); de2_1=u(13); Fai=eye(2); Kd0=［210 0;0 210］; if j==0 betaj=1; else betaj=2*j; end E=1.0; delta_y=［de1-de1_1+e1-e1_1;de2-de2_1+e2-e2_1］; Kp0=［210 0;0 210］; Kd0=Kp0; Kpj=betaj*Kp0; Kdj=betaj*Kd0; ej=［e1 e2］'; dej=［de1 de2］'; Deltaj=［E*sign(delta_y(1)) E*sign(delta_y(1))］'; Tj=Kpj*ej+Kdj*dej+Deltaj; sys(1)=Tj(1); sys(2)=Tj(2); sys(3)=e1; sys(4)=e2; sys(5)=de1; sys(6)=de2; (5) 指令程序： chap5_2input.m。 function ［sys,x0,str,ts］ = spacemodel(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 = 4; sizes.NumInputs = 0; sizes.DirFeedthrough = 1; sizes.NumSampleTimes = 1; sys = simsizes(sizes); x0= ［］; str = ［］; ts = ［0 0］; function sys=mdlOutputs(t,x,u) q1d=sin(3*t); q2d=cos(3*t); dq1d=3*cos(3*t); dq2d=-3*sin(3*t); sys(1)=q1d; sys(2)=q2d; sys(3)=dq1d; sys(4)=dq2d; 5.3基于切换增益的单关节机械手迭代学习控制 文献［5］针对多关节机械手提出了一种自适应迭代学习控制方法，该方法主要在经典的PD反馈控制的基础上通过迭代项克服机器人系统的未知参数和干扰带来的不确定性。本节在文献［5］的基础上，设计了单关节机械手的迭代学习控制算法，并给出了该方法的收敛性分析及仿真结果。 5.3.1问题描述 单关节机器人系统的动力学模型为 Iθ¨k+b θ·k+mglcosθk=τk+dk(t)(5.21) 其中，k为正整数，表示迭代次数，θk、θ·k和θ¨k分别代表关节第k次运行时的角度、角速度和角加速度，I为转动惯量，b为关节转动的粘性摩擦系数，m为关节的质量，l为关节的质心与关节的距离，dk(t)为单关节机器人系统参数不确性项和干扰，τk为作用于关节的控制输入。 假定关节的位置和速度可以通过反馈获得，则控制的任务就是设计一个控制律τk(t)使得θk(t)在任意t∈0，T及任意k都有界，并且当k→∞时θk(t)在任意时刻t∈0，T都收敛于对应时刻的期望轨迹θd(t)。 根据上述描述，可以给出如下合理的假设： 假设1： 期望轨迹θd(t)及其一阶、二阶导数θ·d(t)、θ¨d(t)和dk(t)有界； 假设2： 初始状态可重复，即θ·d(0)-θ·k(0)=θd(0)-θk(0)=0； 假设3： |Iθ¨dk-dk|≤β，|mglcosθ|≤kg，b≤kc； 假设4： 假设速度有界 ［8］，取|θ·k|≤M。 5.3.2自适应迭代学习控制器设计 考虑式(5.21)及假设1~3，控制律设计为［5］ τk=kpek(t)+kde·k(t)+ δ^k(t)sgn(e·k(t))(5.22) 其中 δ^k(t)=δ^k-1(t)+γ|e·k(t)|(5.23) 且δ^-1(t)=0，ek(t)=θd(t)-θk(t)。如果kp,kd,γ均大于零，则ek(t)、e·k(t)及τk(t)对于任意k都有界，且limk→∞ek(t)=limk→∞e·k(t)=0，t∈0T。 为了简化起见，在下面的描述中有些变量省略了(t)。 5.3.3收敛性分析 收敛性分析步骤如下。 1. 证明Wk的递增性 取如下的Lyapunov函数 Wk(t)=Vk(t)+12γ∫t0 δ~2k(τ)dτ(5.24) 其中，δ为用于描述模型信息的不确定项。 定义δ=β+kcM+kg，为了实现无须模型信息的控制，采用δ^k(t)自适应估计δ，并取δ~k(t)=δ-δ^k(t)。定义Vk(t)为 Vk(t)=12Ie·2k(t)+12kpe2k(t)(5.25) 则 ΔWk =Wk-Wk-1=Vk-Vk-1+12γ∫t0(δ~2k(τ)-δ~2k-1(τ))dτ =Vk-Vk-1-12γ∫t0( δ-2k+2δ~kδ-k)dτ(5.26) 其中，δ-k=δ^k-δ^k-1。 对Vk(t)求一阶导数，然后对两边积分可得 Vk(t)=Vk(0)+∫t0(Ie·ke¨k+kpeke·k)dτ(5.27) 根据假设2可知Vk(0)=0，利用式(5.21)及假设14，可得 Vk(t) =∫t0e·k(Iθ¨dk-Iθ¨k+kpek)dτ =∫t0e·k(Iθ¨dk+bθ·k+mglcosθk-τk-dk+kpek)dτ ≤∫t0δ|e·k|dτ+∫t0e·k(-τk+kpek)dτ 其中，|Iθ¨dk-dk+mglcosθk+bθ·k|≤β+kg+kcM≤δ。 将控制律式(5.22)代入上式，可得 Vk(t) ≤∫t0δ|e·k|dτ+∫t0e·k(-kde·k-δ^k(t)sgn(e·k(t)))dτ =∫t0δ|e·k|dτ+∫t0(-kde·2k-δ^k|e·k|)dτ(5.28) 由式(5.23)可得 |e·k|=1γ(δ^k-δ^k-1)=1γδ-k 则 Vk≤-kd∫t0e·2kdτ+1γ∫t0δδ-kdτ-1γ∫t0δ^kδ-kdτ=-kd∫t0e·2kdτ+1γ∫t0δ~kδ-kdτ 将上式代入式(5.26)，可得 ΔWk ≤-kd∫t0e·2kdτ+1γ∫t0δ~kδ-kdτ-Vk-1-12γ∫t0(δ-2k+2δ~kδ-k)dτ =-Vk-1-12γ∫t0δ-2kdτ-kd∫t0e·2kdτ≤0(5.29) 上式说明Wk是非增序列，现只要证明W0有界就可说明Wk是有界的。 2. 证明W0(t)的有界性 根据式(5.24)的定义，有 W0(t)=V0(t)+12γ∫t0δ~20(τ)dτ 则 W·0(t)= V·0(t)+12γδ~20(t)(5.30) 根据式(5.27)，有V0(t)=∫t0(Ie·0e¨0+kpe0e·0)dτ，由式(5.21)可得 Iθ¨0 =τ0+d0-bθ·0-mglcosθ0=kpe0+kde·0+δ^0sgn(e·0)+d0-bθ·0-mglcosθ0 则 V·0(t) =Ie·0e¨0+kpe0e·0= e·0(Ie¨0+kpe0)= e·0(Iθ¨d-Iθ¨0+kpe0) =e·0(Iθ¨d-kde·0-δ^0sgn(e·0)-d0+bθ·0+mglcosθ0) ≤δ|e·0|-kde·20-δ^0|e·0| 由式(5.23)可知δ^0(t)=γ|e·0(t)|，将上式代入式(5.30)，可得 W·0(t) ≤δ|e·0|-kde·20-δ^0|e·0|+12γδ~20=-kde·20+δ~0|e·0|+12γδ~20 =-kde·20+1γδ~0δ^0+12γδ~20=-kde·20+1γδ~0δ^0+12δ~0 由δ^0(t)=δ-δ~0(t)，代入上式可得 W·0(t)≤-kde·20+1γδ~0δ-12γδ~20(5.31) 对于λ>0，有如下不等式 1γδ~0(t)δ≤λ 1γδ~0(t)2+14λδ2 (5.32) 成立，则可得 W·0 ≤-kde·20+λ1γδ~0(t)2+14λδ2-12γδ~20(t) =-kde·20+1γλγ-12 δ~20(t)+14λδ2(5.33) 取λγ-12≤0，即λ≤12γ，则 W·0≤-kde·20+14λδ2≤14λδ2 由式(5.34)，根据一致连续性判定定理(见附录)，若函数W0在区间［0,T］上的导数有界，则W0在［0,T］上一致连续，再根据闭区间上连续函数的有界定理(见附录)，W0在［0,T］上有界。 3. 证明误差的收敛性 由W0在［0,T］上有界可知Wk有界，Wk可写为 Wk=W0+∑kj=1ΔWj(5.34) 由式(5.29)可得ΔWk≤-Vk-1，则根据式(5.25)中Vk的定义，可得 ΔWk≤-12Ie·2k-1-12kpe2k-1 则 ∑kj=1ΔWj≤-12∑kj=1(Ie·2 j-1+kpe2j-1) 从而 Wk≤W0-12∑kj=1(Ie·2j-1+kpe2j-1)(5.35) 由上式可推出 ∑kj=1(Ie·2j-1+kpe2j-1)≤2(W0-Wk)≤2W0 由于W0有界，则 limk→∞ek(t)= limk→∞e·k(t)=0，t∈0T 5.3.4仿真实例 假设单力臂的质量均匀分布，质心距连杆的转动中心为l，连杆运动的黏性摩擦系数为b，并忽略弹性摩擦，外加干扰为dk，则根据牛顿定律得到其运动方程为 Iθ¨+bθ·+mglcosθ=τ+dk 其中，mg为重力，I为转动惯量，θ为转动角度。 单自由度机械臂的系统参数为： 机械臂质量m=1,质心到关节的长度l=0.25，转动惯量I=43ml2，重力加速度g=9.8，黏性摩擦系数b=2.0，取干扰和系统不确定项的总和为dk=sint。 控制器参数选为kp=5.0，kd=5.0，γ=20。总的迭代次数为20次，每次的仿真时间为1。仿真中，为了提高控制精度，Simulink模块的仿真参数中的绝对误差和相对误差都取10-5。角度指令信号为θd=sint。采用控制律式(5.22)和自适应律式(5.23)，仿真程序中，取S=1，仿真结果如图5.11~图5.15所示。在控制律中，为了降低控制输入的抖振，并加快运算速度，可采用饱和函数代替切换函数，并取边界层厚度为0.02，见仿真程序中的S=2。 图5.1150次迭代过程中角度的跟踪过程 图5.1250次迭代后角度和角速度跟踪 图5.1350次迭代过程中误差和误差变化率的收敛过程 图5.14第50次迭代自适应项δ的变化 图5.15第50次迭代控制输入的变化 仿真程序： 针对单力臂机械手的自适应迭代学习控制。 (1) 主程序： chap5_3main.m。 clc; clear all; close all; global old_delta t=［0:0.01:1］'; k(1:101)=0; k=k'; delta(1:101)=0; delta=delta'; M=50; for i=0:1:M i pause(0.01); old_delta=delta; sim('chap5_3sim',［0,1］); q1=q(:,1); dq1=q(:,2); q1d=qd(:,1); dq1d=qd(:,2); e=q1d-q1; de=dq1d-dq1; figure(1); hold on; plot(t,q1,'b',t,q1d,'r'); xlabel('time(s)');ylabel('q1d,q1'); j=i+1; times(j)=i; ei(j) =max(abs(e)); dei(j)=max(abs(de)); end figure(2); subplot(211); plot(t,q1d,'r',t,q1,'b'); xlabel('time(s)');ylabel('q1d,q1'); subplot(212); plot(t,dq1d,'r',t,dq1,'b'); xlabel('time(s)');ylabel('dq1d,dq1'); figure(3); subplot(211); plot(times,ei,'*-r'); title('Change of maximum absolute value of error with times i'); xlabel('times');ylabel('angle tracking error'); subplot(212); plot(times,dei,'*-r'); title('Change of maximum absolute value of derror with times i'); xlabel('times');ylabel('speed tracking error'); figure(4); plot(t,delta,'r'); xlabel('time(s)');ylabel('Delta Change'); figure(5); plot(t,tol,'r'); xlabel('time(s)');ylabel('Control input'); (2) Simulink子程序： chap5_3sim.mdl。 (3) 控制器子程序： chap5_3ctrl.m。 function ［sys,x0,str,ts］ = spacemodel(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 = 1; sizes.NumInputs = 5; sizes.DirFeedthrough = 1; sizes.NumSampleTimes = 1; sys = simsizes(sizes); x0= ［］; str = ［］; ts = ［0 0］; function sys=mdlOutputs(t,x,u) q1d=u(1);dq1d=u(2); q1=u(3);dq1=u(4); e=q1d-q1; de=dq1d-dq1; Kp=5.0;Kd=5.0; delta=u(5); S=2; if S==1 tol=Kp*e+Kd*de+delta*sign(de); elseif S==2 fai=0.02; if de/fai>1 sat=1; elseif abs(de/fai)<=1 sat=de/fai; elseif de/fai<-1 sat=-1; end tol=Kp*e+Kd*de+delta*sat; end sys(1)=tol; (4) 自适应律子程序： chap5_3adapt.m。 function ［sys,x0,str,ts］ = spacemodel(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 = 1; sizes.NumInputs = 4; sizes.DirFeedthrough = 1; sizes.NumSampleTimes = 1; sys = simsizes(sizes); x0= ［］; str = ［］; ts = ［0 0］; function sys=mdlOutputs(t,x,u) q1d=u(1);dq1d=u(2); q1=u(3);dq1=u(4); de=dq1d-dq1; Gama=20; delta=Gama*de'*sign(de); % Adaptive law sys(1)=delta; (5) 被控对象子程序： chap5_3plant.m。 function ［sys,x0,str,ts］ = spacemodel(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.NumContStates= 2; sizes.NumDiscStates = 0; sizes.NumOutputs = 2; sizes.NumInputs = 1; sizes.DirFeedthrough = 0; sizes.NumSampleTimes = 1; sys = simsizes(sizes); x0= ［0;1］; str = ［］; ts = ［0 0］; function sys=mdlDerivatives(t,x,u) g=9.8; m=1.0; l=1.25; b=2.0; I=4/3*m*l^2; q=x(1); dq=x(2); tol=u(1); dk=sin(t); S=inv(I)*(tol+dk-m*g*l*cos(q)-b*dq); sys(1)=x(2); sys(2)=S; function sys=mdlOutputs(t,x,u) sys(1)=x(1);%角度 sys(2)=x(2); %角速度 (6) 指令子程序： chap5_3input.m。 function ［sys,x0,str,ts］ = spacemodel(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; sizes.NumInputs = 0; sizes.DirFeedthrough = 0; sizes.NumSampleTimes = 1; sys = simsizes(sizes); x0= ［］; str = ［］; ts = ［0 0］; function sys=mdlOutputs(t,x,u) q1d=sin(t); dq1d=cos(t); sys(1)=q1d; sys(2)=dq1d; 5.4基于切换增益的多关节机械手迭代学习控制 本节介绍一类多关节机器人力臂自适应迭代学习控制器的设计和仿真方法。 5.4.1问题的提出 多关节机器人力臂动力学方程为 M(qk(t))q¨k(t)+C(qk(t),q·k(t))q·k(t)+G(qk(t))=τk(t)+dk(t)(5.36) 其中，qk∈Rn,q·k∈Rn,q¨k∈Rn为关节角位移，角速度和角加速度量，M(qk)∈Rn×n为机器人的惯性矩阵，C(qk,q·k)q·k∈Rn表示离心力和哥氏力，G(qk)∈Rn为重力项，τ∈Rn为控制力矩，dk∈Rn为各种未建模动态和扰动。 假设系统参数未知，且系统满足如下假设： (A1) 对于t∈［0,T］，指令轨迹qd(t)，q·d(t)，q¨d(t)及干扰qd(t)有界； (A2) 初始值满足q·d(0)-q·k(0)=qd(0)-qk(0)=0。 且满足一般机器人模型所具有的如下4个特性： (P1) M(qk)∈Rn×n为对称正定且有界的矩阵； (P2) M·(qk)-2C(qk,q·k)为对称矩阵，且xT(M·(qk)-2C(qk,q·k))x=0，x∈Rn； (P3) G(qk)+C(qk,q·k)q·d(t)=Ψ(qk,q·k)ξT(t)，Ψ(qk,q·k)∈Rn×(m-1)为已知矩阵，ξ(t)∈Rm-1为未知向量； (P4) ‖C(qk,q·k)‖≤kc‖q·k‖，‖G(qk)‖