다섯, 로봇 PID 제어

5.1 로봇 역학 모델 및 구조적 특성

2 차 비선형 미분 방정식으로 동적 성능을 설명 할 수있는 N- 관절 로봇을 고려하십시오.

$\ tiny M \ left (q \ right) \ ddot {q} + C \ left (q, \ dot {q} \ right) \ dot {q} + G \ left (q \ right) + F \ left (\ 점 {q} \ right) + \ tau _ {d} = \ tau$

$\ 작은 q \ in R ^ {n}$ 관절 각도 변위, $\ 작은 M \ 왼쪽 (q \ 오른쪽) \ in R ^ {n \ times n}$ 로봇의 관성 매트릭스, $\ tiny C \ left (q, \ dot {q} \ right) \ in R ^ {n}$ 원심력 및 코리올리 힘, $\ 작은 G \ 왼쪽 (q \ 오른쪽) \ in R ^ {n}$ 중력 항, $\ tiny F \ left (\ dot {q} \ right) \ in R ^ {n}$ 마찰 토크, $\ 작은 \ tau \ in R ^ {n}$ 제어 토크 및 $\ tiny \ tau_ {d} \ in R ^ {n}$ 외부 장애 는 어디에 있습니까 ?

5.2 S- 함수 기반 SIMULINK 시뮬레이션

5.2.1 S 함수 소개

S 기능 모듈은 전체 Simulink 동적 시스템의 핵심입니다 .S 기능은 시스템 기능의 약자로 그래픽이 아닌 방식으로 설명 된 기능 블록 (즉, Simulink의 시스템 모듈과 다른 컴퓨터 언어)을 나타냅니다. 사용자는 MATLAB 코드, C, C ++ 및 기타 언어로 S 함수를 작성할 수 있습니다. S 함수는 연속 시스템, 이산 시스템, 복합 시스템과 같은 동적 시스템을 설명하고 구현하기위한 특정 문법으로 구성됩니다. S 함수는 Simulink 솔버에서 관련 정보를 수신하고 솔버에 적절한 명령을 내릴 수 있습니다. 이에 대한 응답으로이 상호 작용은 simulink 시스템 모듈과 솔버 간의 상호 작용과 유사합니다. 구조 시스템의 완전한 S- 기능에는 동적 시스템을 설명하는 데 필요한 모든 기능이 포함되어 있습니다. 다른 모든 사용 사례는이 구조 시스템의 특수한 경우입니다.

5.2.2 S 기능 사용 단계

일반적으로 S 기능을 사용하는 단계는 다음과 같습니다.

(1) S 함수 소스 파일을 만듭니다. S-function 소스 파일을 생성하는 방법은 여러 가지가 있으며 Simulink는 다양한 S-function 템플릿과 예제를 제공하며 사용자는 필요에 따라 해당 템플릿이나 예제를 수정할 수 있습니다.

(2) 동적 시스템의 simulink 모델 블록 다이어그램에 S-Function 모듈을 추가하고 올바른 설정을합니다.

(3) Simulink 모델 블록 다이어그램에 정의 된 기능에 따라 입력 및 출력 포트를 연결합니다. S- 함수 사용 및 작성을 용이하게하기 위해 Simulink의 Function & Tables 모듈 라이브러리는 S- 함수 및 S- 함수 모듈 작성의 다양한 예제를 사용자에게 제공하는 S-Function 데모 모듈 그룹도 제공합니다.

5.2.3 S- 기능의 기본 기능 및 중요한 매개 변수 설정

(1) S 함수 모듈, 다양한 함수 모듈이 서로 다른 작업을 완료합니다. 이러한 함수 모듈 (함수)은 시뮬레이션 루틴 또는 콜백 함수 (콜백 함수)라고하며 초기화 (초기화), 파생 (mdlDerivative), 출력 포함 (MdlOutput) 등.

(2) NumConStates는 S 함수가 설명하는 모듈의 연속 상태 수를 나타냅니다.

(3) NumDiscStates는 불연속 상태의 수를 나타냅니다.

(4) NumOutputs 및 NumInputs는 각각 모듈 입력 및 출력의 수를 나타냅니다.

(5) 직접 피드 스루 (dirFeedthrough)는 입력 신호가 출력 끝에 나타나는지 여부를 식별하며 값은 0 또는 1입니다.

(6) NumSampleTimes는 모듈에서 사용하는주기 수이며 S 함수는 여러 샘플링주기가있는 시스템을 지원합니다.

sys 외에도 시스템의 초기 상태 변수 $\ 작은 x_ {0}$ , 설명 변수 $\ tiny str$ 및 샘플링 기간 변수 도 설정해야합니다 $\ 작은 t_ {s}$ . $\ 작은 t_ {s}$ 변수는 각 행이 샘플링 기간에 해당하는 2 열 행렬입니다. 시스템 변수 단일 샘플링 기간, 연속 시스템은 $\ tiny \ left [t_ {1}, t_ {2} \ right]$ , $\ 작은 t_ {1}$ 샘플링 기간 $\ tiny t_ {1} =-1$ 연속 샘플링주기 입력 신호를 나타내고, $\ 작은 t_ {2}$ 오프셋 량, 일반적으로 0으로 간주한다. 연속 시스템의 경우, $\ 작은 t_ {s}$ 같은 걸릴 $\ tiny \ left [-1,0 \ right]$ .

5.3 로봇의 독립적 인 PD 제어

5.3.1 통제 법 설계

중력 및 외부 간섭을 무시할 때 독립적 인 PD 제어를 사용하여 로봇 고정 소수점 제어의 요구 사항을 충족합니다.

$\ 작은 n$ 관절 조작기 방정식이 다음과 같다고 가정합니다 .

$\ tiny D \ left (q \ right) \ ddot {q} + C \ left (q, \ dot {q} \ right) \ dot {q} = \ tau$ （5.1）

이들 중에서 $\ 작은 D \ 왼쪽 (q \ 오른쪽)$ 는 IS $\ 작은 n \ times n$ 순서 긍정적 명확한 관성 행렬 $\ tiny C \ left (q, \ dot {q} \ right)$ 은 IS $\ 작은 n \ times n$ 차 원심 코리올리 힘 용어.

독립 PD 통제 법은 다음과 같습니다.
$\ 작은 \ tau = K_ {d} \ 점 {e} + K_ {p} e$ (5.2)

$\ tiny e = q_ {d} -q$ 고정 소수점 제어를 사용할 때 추적 오류를 $\ 작은 q_ {d}$ 상수 값 으로 취하십시오 $\ tiny \ dot {q_ {d}} = \ ddot {q_ {d}} \ equiv 0$ .

이때 로봇 방정식은 다음과 같습니다.

$\ tiny D \ left (q \ right) \ left (\ ddot {q_ {d}}-\ ddot {q} \ right) + C \ left (q, \ dot {q} \ right) \ left (\ dot {q_ {d}}-\ dot {q} \ 오른쪽) + K_ {d} \ dot {e} + K_ {p} e = \ 0$

그건

$\ tiny D \ left (q \ right) \ ddot {e} + C \ left (q, \ dot {q} \ right) \ dot {e} + K_ {p} e = -K_ {d} \ dot { 이자형}$ （5.3）

Lyapunov (Lyapunov) 함수를 다음과 같이 사용하십시오.

$\ tiny V = \ frac {1} {2} \ dot {e} ^ {T} D \ left (q \ right) \ dot {e} + \ frac {1} {2} {e} ^ {T} K \ 왼쪽 (p \ 오른쪽) e$

A $\ 작은 D \ 왼쪽 (q \ 오른쪽)$ 와 $\ tiny K_ {p}$ 양의 확실성이 알려진 $\ 작은 V$ 경우, 전체적으로 양의 양이 확실 할 때

$\ tiny \ dot {V} = \ dot {e} ^ {T} D \ ddot {e} + \ frac {1} {2} \ dot {e} ^ {T} \ dot {D} \ dot {e } + \ dot {e} ^ {T} K_ {d} e$

사용 $\ tiny \ dot {D} -2C$ 사선 대칭 $\ tiny \ dot {e} ^ {T} \ dot {D} \ dot {e} = 2 \ dot {e} ^ {T} C \ dot {e}$ , 다음

$\ tiny \ dot {V} = \ dot {e} ^ {T} D \ ddot {e} + \ dot {e} ^ {T} C \ dot {e} + \ dot {e} ^ {T} K_ {p} e = \ dot {e} ^ {T} \ left (D \ ddot {e} + C \ dot {e} + K_ {p} e \ right) =-\ dot {e} ^ {T} K_ {d} \ dot {e} \ leqslant 0$

5.3.2 수렴 분석

이후 $\ 작은 \ dot {V}$ 반 명확한 네가티브 및 $\ tiny K_ {d}$ 명확한 양의 다음 때 $\ tiny \ dot {V} \ equiv 0$ 시간 보유 $\ tiny \ dot {e} \ equiv 0$ 에 $\ tiny \ ddot {e} \ equiv 0$ . 방정식 (5.3)에 대입하면가 있고 가역성 $\ tiny K_ {p} e = 0$ 에서 $\ tiny K_ {p}$ 알 수 $\ 작은 e = 0$ 있습니다. LaSalle에 의해 알려진 정리 $\ tiny \ left (e, \ dot {e} \ right) = \ left (0,0 \ right)$ , 즉 전 세계적으로 점근 적으로 안정된 평형 점을 제어하는 로봇, 즉 $\ tiny \ left (q_ {0}, \ dot {q_ {0}} \ right)$ 시작하는 임의의 초기 조건 $\ tiny q \ rightarrow q_ {d}$ 에서 $\ tiny \ dot {q} \ rightarrow 0$ .

5.3.3 시뮬레이션 예

제어 대상 공식 (5.1)의 경우 2 관절 로봇 시스템 (중력, 마찰 및 간섭을 고려하지 않음)을 선택하면 동적 모델은 다음과 같습니다.

$\ tiny D \ left (q \ right) \ ddot {q} + C \ left (q, \ dot {q} \ right) \ dot {q} = \ tau$

그들 중

$\ tiny D \ left (q \ right) = \ begin {bmatrix} p_ {1} + p_ {2} + 2p_ {3} cosq_ {2} & p_ {2} + p_ {3} cosq_ {2} \\ p_ {2} + p_ {3} cosq_ {2} 및 p_ {2} \ end {bmatrix}$

$\ tiny C \ left (q, \ dot {q} \ right) = \ begin {bmatrix} -p_ {3} \ dot {q_ {2}} sinq_ {2} & -p_ {3} \ left (\ dot {q_ {1}} + \ dot {q_ {2}} \ right) sinq_ {2} \\ p_ {3} \ dot {q_ {1}} sinq_ {2} & 0 \ end {bmatrix}$

취 $\ tiny p = \ begin {bmatrix} 2.90 & 0.76 & 0.87 & 3.04 & 0.87 \ end {bmatrix} ^ {T}$ , $\ tiny q_ {0} = \ begin {bmatrix} 0.0 & 0.0 \ end {bmatrix} ^ {T}$ , $\ tiny \ dot {q_ {0}} = \ begin {bmatrix} 0.0 & 0.0 \ end {bmatrix} ^ {T}$ .

위치 명령은 $\ tiny q_ {d} \ left (0 \ right) = \ begin {bmatrix} 1.0 & 1.0 \ end {bmatrix} ^ {T}$ 컨트롤러 공식 (5.2) $\ tiny K_ {p} = \ begin {bmatrix} 100 & 0 \\ 0 & 100 \ end {bmatrix}$ 에서 $\ tiny K_ {d} = \ begin {bmatrix} 100 & 0 \\ 0 & 100 \ end {bmatrix}$ 이며, 시뮬레이션 결과는 그림 5.1과 그림 5.2에 나와 있습니다.

그림 5.1 이중 힘 팔의 계단 응답

그림 5.2 독립 PD 제어를위한 제어 입력

시뮬레이션에서 매개 변수 $\ tiny K_ {p}$ 와 합계 $\ tiny K_ {d}$ 가 변경 되면 만족하는 한 $\ tiny K_ {d}> 0, K_ {p}> 0$ 더 나은 시뮬레이션 결과를 얻을 수 있습니다. 외부의 힘과 간섭이 전혀없는 로봇 시스템은 없으며 독립적 인 PD 제어는 기준 분석의 기초로만 사용할 수 있지만 그 분석은 의미가 있습니다.

시뮬레이션 프로그램

simulink 메인 프로그램 : chap2_1sim.mdl

컨트롤러 서브 루틴 : chap2_1ctrl.m

function[sys,x0,str,ts] = chap2_1ctrl(t,x,u,flag)
%主函数
%主函数包括四个输出：
%                    sys数组包含某个子函数返回的值
%                    x0为所有状态的初始化变量
%                    str是保留参数，总是一个空矩阵
%                    ts返回系统采样时间
%函数的四个输入分别为采样时间t、状态x、输入u和仿真流程控制状态标志变量flag
%输入参数后面还可以接续一系列的附带参数simStateCompliance
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
%初始化回调子函数
%提供状态、输入、输出、采样时间数目和初始状态变量的值
%初始化阶段，标识变量flag首先被置为0，S-function首次被调用时
%该子函数首先被调用，且为S-function模块提供下面信息
%该子函数必须存在
sizes = simsizes;                 %用于设置参数的结构体用simsizes来生成
sizes.NumOutputs      = 2;        %模块输出变量的个数
sizes.NumInputs       = 6;        %模块输入变量的个数
sizes.DirFeedthrough  = 1;        %模块是否存在直接贯通，1存在，0不存在
sizes.NumSampleTimes  = 1;        %模块的采样时间个数，至少是一个
sys = simsizes(sizes);            %返回size数据结构所包含的信息
x0 = [];
str = [];
ts = [0 0];
 
function sys = mdlOutputs(t,x,u)
%计算输出回调函数
%给定t，x，u计算输出，可以在此描述系统的输出方程
%该子函数必须存在
R1 = u(1);dr1 = 0;                %u（1）是qd（1）
R2 = u(2);dr2 = 0;                %u（2）是qd（1）
 
x(1) = u(3);                      %u（3）是q（1）
x(2) = u(4);                      %u（4）是dq（1）(就是给q（1）求导)
x(3) = u(5);                      %u（5）是q（2）
x(4) = u(6);                      %u（6）是dq（2）(就是给q（2）求导)
 
e1 = R1 - x(1);
e2 = R2 - x(3);
e = [e1,e2];                      %跟踪误差e=qd-q
 
de1 = dr1 - x(2);                 %x（2）是dq（1）(就是给q（1）求导)
de2 = dr2 - x(4);                 %x（4）是dq（2）(就是给q（2）求导)
de = [de1;de2];                   %跟踪误差的导数de=d(qd-q)
 
Kp = [30 0;0 30];                 %Kp是P参数
Kd = [30 0;0 30];                 %Kd是D参数
 
tol = Kp.*e+Kd.*de;               %独立的PD控制律
 
sys(1) = tol(1);                  
sys(2) = tol(2);


%缺省的其他函数如下
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                                                                             %
%function sys = mdlUpdate(t,x,u)                                              %
%状态更新回调子函数                                                             %
%给定t、x、u计算离散状态的更新                                                  %
%每个仿真步内必然调用该子函数，不论是否有意义                                     %
%除了在此描述系统的离散状态方程外，还可以在此添加其他每个仿真步内都必须执行的代码    %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                                                                             %
%function sys = mdlGetTimeOfNextVarHit(t,x,u)                                 %
%计算下一个采样时间                                                             %
%仅在系统是变采样时间系统时调用                                                  %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%function sys = mdlTerminate(t,x,u)                                           %
%仿真结束时要调用的回调函数                                                     %
%在仿真结束时，可以在此完成仿真结束所需的必要工作                                 %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

제어 된 객체 서브 루틴 : chap2_1plant.m

%S-function for continuous state equation
function[sys,x0,str,ts] = chap2_1plant(t,x,u,flag)
switch flag
%Initialization
case 0
     [sys,x0,str,ts] = mdlInitialSizes;
case 1
     sys = mdlDerivatives(t,x,u);   %此时要计算连续状态的微分
%Outputs
case 3
     sys = mdlOutputs(t,x,u);       %此时要计算输出
%Unhandled flags
case{2,4,9}
     sys = [];
%Unexpected flags
otherwise
     error(['Unhandled flag = ',num2str(flag)]);
end
%mdlInitialSizes
function[sys, x0,str,ts] = mdlInitialSizes
global p g
sizes = simsizes;                    %用于设置参数的结构体用simsizes来生成
sizes.NumContStates     = 4;         %模块连续状态变量的个数，缺省为0
sizes.NumDiscStates     = 0;         %模块离散状态变量的个数，缺省为0
sizes.NumOutputs        = 4;         %模块输出变量的个数，缺省为0
sizes.NumInputs         = 2;         %模块输入变量的个数，缺省为0
sizes.DirFeedthrough    = 0;         %模块是否存在直接贯通，1存在，0不存在
sizes.NumSampleTimes    = 0;         %模块的采样时间个数，至少是一个
sys = simsizes(sizes);               %设置完后赋给sys输出，返回size数据结构所包含的信息
x0 = [0 0 0 0];                      %状态变量设置为空，表示没有状态变量
str = [];                            %保留参数，置[]就可以，没什么用
ts = [];                             %采样周期，设为0表示是连续系统
 
p = [2.9 0.76 0.87 3.04 0.87];
g = 9.8;
function sys = mdlDerivatives(t,x,u)
%计算导数回调子函数
%给定t，x，u计算连续状态的导数，可以在此给出系统的连续状态方程
%该子函数可以不存在
global p g
 
D0 = [p(1)+p(2)+2*p(3)*cos(x(3)) p(2)+p(3)*cos(x(3));
    p(2)+p(3)*cos(x(3)) p(2)];
C0 = [-p(3)*x(4)*sin(x(3)) -p(3)*(x(2)+x(4))*sin(x(3));
    p(3)*x(2)*sin(x(3)) 0];
tol = u(1:2);                        %取输入变量u(u是矩阵)中的第一个和第二个元素
dq = [x(2);x(4)];                    %取x（2）=dq(1)和x（4）=dq(2)
 
S = inv(D0).*(tol-C0.*dq);           %求S=ddq
 
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_1plot.m

close all;

t = out.t.Data;
x1 = out.x1.Data;
x2 = out.x2.Data; 
tol = out.tol.Data;

figure(1);
subplot(211);
plot(t,x1(:,1),'r',t,x1(:,2),'b');
xlabel('time(s)');
ylabel('position tracking of link 1');
subplot(212);
plot(t,x2(:,1),'r',t,x2(:,2),'b');
xlabel('time(s)');
ylabel('position tracking of link 2');
 
figure(2);
subplot(211);
plot(t,tol(:,1),'r');
xlabel('time(s)');
ylabel('tol1');
subplot(212);
plot(t,tol(:,2),'r');
xlabel('time(s)');
ylabel('tol2');

참고 : 코드에 많은 주석이 추가되었으므로 직접 확인하십시오.

로봇 역학 및 제어 연구 노트 (5)-로봇 PID 제어