机械臂动力学——二轴机械臂动力学参数识别

本文为A matlab-based identification procedure applied to a two-degrees-of-freedom robot manipulator for engineering students学习笔记，相关数据代码可以通过Matlab程序和数据下载 论文+Matlab程序和数据CSDN下载

一、概述

1.1 二轴机械臂

1.2 参数识别概述

参数识别流程  
 
 
 
 
  该动力学模型作了简化，只考虑了围绕关节的连杆惯性矩，若考虑所有惯性矩会引入额外的识别参数，但识别方法不变。 推广到六轴机械臂时，可根据机械臂的DH参数确定最小惯性集，建立对应的动力学模型。

三、力矩测量

机械臂通过永磁直流电动机直接驱动。 在工业应用中，电机驱动器使用电流环，使实际电流在短时间内即可收敛到设置电流，使电流对机械动力学影响降低（实际上仍然存在影响，输出相对输入存在滞后）。

伺服电机一般为三个环控制，从底到高排序为电流环，速度环和位置环，电流环都需要使用，其余两环根据需要和设置模式选择使用。

五、计算速度

采集的数据只有角度信息，因此速度通过中心差分法计算获得

差分，又名差分函数或差分运算，差分的结果反映了离散量之间的一种变化，是研究离散数学的一种工具，常用函数差近似导数。

%% BLOCK 2
%This program describes the procedure to compute the joint velocity
%estimates using the central difference algorithm given in equation (21)
%First joint velocity estimation (V1)
V1(1) = 0.0;
V2(1) = 0.0;
for j=2:l-1    
   V1(j)=(qf1(j+1)-qf1(j-1))/(2*T);
   V2(j)=(qf2(j+1)-qf2(j-1))/(2*T);
end
V1(l) = V1(l-1);
V2(l) = V2(l-1);
%Set velocity vectors as row vectors
V1 = V1';
V2 = V2';

六、数据滤波

数据通过低通滤波器进行处理，过滤高频噪声。 此处用到低通滤波有两处，一为对采集的关节角度使用fir低通滤波器，另一处为对代入位置和速度的模型进行滤波。

低通滤波(Low-pass filter) 是一种过滤方式，规则为低频信号能正常通过，而超过设定临界值的高频信号则被阻隔、减弱。

6.1 角度信息fir低通滤波

使用零相移滤波器filtfilt避免低通滤波产生的时延  

%% BLOCK 1
%Filter coefficients
N    = 30;       % Order
Fc   = 0.07;     % Cutoff Frequency
flag = 'scale';  % Sampling Flag
win = nuttallwin(N+1); % Create the window vector
b  = fir1(N, Fc, 'low', win, flag);% Coefficients using the FIR1 function.
%Filtering for q1 and q2 using the filtfilt function
qf1 = filtfilt(b,1,q1);
qf2 = filtfilt(b,1,q2);

6.2 对代入位置和速度的模型进行滤波

低通滤波函数  
 
  其Z变换为

Z变换（英文：z-transformation）可将时域信号（即：离散时间序列）变换为在复频域的表达式，它在离散时间信号处理中的地位，如同拉普拉斯变换在连续时间信号处理中的地位。

%% BLOCK 4
%This program allows to implement filter (18) and then compute Yaf, Ybf and
%the elements of tau_f given in equation (12)
lambda = 30; %Cut-off frequency
%Coefficients for g(z) in (24)
A1=[1 -exp(-lambda*T)];
B1=[lambda -lambda];
%Coefficients of f(z) in (23)
A2=[1 -exp(-lambda*T)];
B2=[0 1-exp(-lambda*T)];
%Calculus of Yaf and Ybf given in (14)(16)
for i=1:2
    for j=1:9
        Yaf(:,:,i,j) = filter(B1,A1,Ya(:,:,i,j));
        Ybf(:,:,i,j) = filter(B2,A2,Yb(:,:,i,j));
    end
end
%Calculus of the elements of tau_f given in (12)
tf1 = filter(B2,A2,tau1);
tf2 = filter(B2,A2,tau2);

七、参数识别

7.1 识别框架

• On-line

On-line为驱动器-机械臂部分，通过PD闭环（位置和速度）使用力矩控制机械臂，并将输出力矩和关节角度发送到Off-line。

• Off-line

Off-line为计算机处理部分，对接收到的角度数据进行低通滤波，然后通过差分计算速度。将角度，速度和力矩代入模型中，通过滤波后通过参数识别算法获得动力学参数。 参数识别时需要控制机械臂按一定激励轨迹运动，该轨迹会影响参数识别效果，此处输入激励为  
 

设计激励轨迹的目的是为了降低矩阵Yr的条件数。可参考：Jan Swevers et al. Optimal Robot Excitation and Identification, IEEE Trans. on Robotics.

机械臂控制必需确保采用PD闭环控制，保证采集数据的可靠性。

7.2 Matlab实现

• 流程

• 程序

%Data loading
clear all
clc
load data01.mat
time = 10;   %Experiment time
T = 0.001;   %Sampling period
l = time/T;  %Number of samples
q1=exps.signals(1,1).values(1:l,1); %Joint position 1
q2=exps.signals(1,2).values(1:l,1); %Joint position 2
u1=exps.signals(1,3).values(1:l,1); %Voltage 1
u2=exps.signals(1,4).values(1:l,1); %Voltage 2
% 根据获取电压计算力
% Conversion to torque according to equation (4) and using the values for Km
%and Ksa provided in Section III: Experimental Platform.
tau1 = u1*0.0897*1.5; %Torque 1
tau2 = u2*0.0551*1; %Torque 2
%Obtain simulation time vector
t=exps.time(1:10000,1);
%% BLOCK 1
%This program describes the filtering procedure to reduce quantization
%error, applied to the first and second joint position measurements.
% 关节1，2角度滤波处理
%Filter coefficients
% 低通滤波
N    = 30;       % Order，阶数
Fc   = 0.07;     % Cutoff Frequency，截止频率
flag = 'scale';  % Sampling Flag
win = nuttallwin(N+1); % Create the window vector
b  = fir1(N, Fc, 'low', win, flag);% Coefficients using the FIR1 function.
%Filtering for q1 and q2 using the filtfilt function
% 一般来说低通滤波会造成时延，使用零相移数字滤波器可以避免
qf1 = filtfilt(b,1,q1);
qf2 = filtfilt(b,1,q2);
%Plot to show the effect of filtering the position q1 and see the effect of
%quantization error.
figure(1)
plot(t,q1,'b',t,qf1,'r--','LineWidth',3);
legend('Position q_1','Filtered position q_{f1}','Location','northeast')
s = title('{\bf Quantization error}','fontsize',18);
set(s,'Interpreter','latex','FontSize',20)
xlabel('time [s]','FontSize',20),ylabel('Position q_1[rad]','FontSize',20)
set(gca,'fontsize',16),grid
axis([0.61 0.69 1.23 1.244])
%% BLOCK 2
%This program describes the procedure to compute the joint velocity
%estimates using the central difference algorithm given in equation (31)
% 通过中心差分法计算关节速度
%First joint velocity estimation
V1(1) = 0.0;
V2(1) = 0.0;
for j=2:l-1    
   V1(j)=(qf1(j+1)-qf1(j-1))/(2*T);
   V2(j)=(qf2(j+1)-qf2(j-1))/(2*T);
end
V1(l) = V1(l-1);
V2(l) = V2(l-1);
%Set velocity vectors as row vectors
V1 = V1';
V2 = V2';
%% BLOCK 3
%This program shows the construction of matrices Ya and Yb given in
%equation (7),(8)
% Ya,Yb参数
%Value r for approximating the sign function using the hyperbolic tangent
%function
r = 100;
%Matrix Ya
Ya(:,:,1,1) = V1;
Ya(:,:,1,2) = V1.*sin(qf2).*sin(qf2);
Ya(:,:,1,3) = V2.*cos(qf2);
Ya(:,:,1,4) = zeros(l,1);
Ya(:,:,1,5) = zeros(l,1);
Ya(:,:,1,6) = zeros(l,1);
Ya(:,:,1,7) = zeros(l,1);
Ya(:,:,1,8) = zeros(l,1);
Ya(:,:,1,9) = zeros(l,1);
Ya(:,:,2,1) = zeros(l,1);
Ya(:,:,2,2) = zeros(l,1);
Ya(:,:,2,3) = V1.*cos(qf2);
Ya(:,:,2,4) = V2;
Ya(:,:,2,5) = zeros(l,1);
Ya(:,:,2,6) = zeros(l,1);
Ya(:,:,2,7) = zeros(l,1);
Ya(:,:,2,8) = zeros(l,1);
Ya(:,:,2,9) = zeros(l,1);
%Matrix Yb
Yb(:,:,1,1) = zeros(l,1);
Yb(:,:,1,2) = zeros(l,1);
Yb(:,:,1,3) = zeros(l,1);
Yb(:,:,1,4) = zeros(l,1);
Yb(:,:,1,5) = zeros(l,1);
Yb(:,:,1,6) = V1;
Yb(:,:,1,7) = zeros(l,1);
Yb(:,:,1,8) = tanh(r*V1);
Yb(:,:,1,9) = zeros(l,1);
Yb(:,:,2,1) = zeros(l,1);
Yb(:,:,2,2) = -0.5*sin(2*qf2).*V1.*V1;
Yb(:,:,2,3) = sin(qf2).*V1.*V2;
Yb(:,:,2,4) = zeros(l,1);
Yb(:,:,2,5) = -sin(qf2);
Yb(:,:,2,6) = zeros(l,1);
Yb(:,:,2,7) = V2;  
Yb(:,:,2,8) = zeros(l,1);
Yb(:,:,2,9) = tanh(r*V2);
%% BLOCK 4
%This program allows to implement filter (18) and then compute Yaf, Ybf and
%the elements of tau_f given in equation (12)
% 低通滤波
lambda = 30; %Cut-off frequency
%Coefficients for g(z) in (24)
A1=[1 -exp(-lambda*T)];
B1=[lambda -lambda];
%Coefficients of f(z) in (23)
A2=[1 -exp(-lambda*T)];
B2=[0 1-exp(-lambda*T)];
%Calculus of Yaf and Ybf given in (14)(16)
for i=1:2
    for j=1:9
        Yaf(:,:,i,j) = filter(B1,A1,Ya(:,:,i,j));
        Ybf(:,:,i,j) = filter(B2,A2,Yb(:,:,i,j));
    end
end
%Calculus of the elements of tau_f given in (12)
tf1 = filter(B2,A2,tau1);
tf2 = filter(B2,A2,tau2);
%% BLOCK 5
%Construction of the elements of Yf and tau_f given in (32)
% 构建Yf和tau_f，公式Yf * theta = tau_f，theta是动力学参数
for i=1:9
    sum1 = Yaf(:,:,1,i)+Ybf(:,:,1,i);
    sum2 = Yaf(:,:,2,i)+Ybf(:,:,2,i);
    Yf(:,i) = [sum1;sum2]; 
end
tf=[tf1;tf2];
%% BLOCK 6
%Implementation of the LS algorithm
%Calculus of the parameter vector theta
% 最小二乘解求解动力学参数
theta = (Yf'*Yf)^(-1)*(Yf'*tf);
%Alternative procedure to obtain the time evolution plot of the parameter
%estimates
% step by step 展示标定参数的变化
for i=1:l
    Yff(:,:,i) = [Yf(i,:);Yf(10000+i,:)];
    tff(:,:,i) = [tf1(i);tf2(i)];
end
 P = zeros(9,9); %Initialization
 Z = zeros(9,1); %Initialization
%Implementation of equation (32) step by step
for i=1:l
    P = P + (Yff(:,:,i)')*Yff(:,:,i);
    Z = Z + (Yff(:,:,i)')*tff(:,:,i);
    thetai(:,i) = P^(-1)*Z;  
end
%Numerical visualization of the parameter estimates
clc
theta
%% BLOCK 7
%Code to generate the graphs included in the paper for the parameter
%estimates
figure(2)
subplot(3,3,1)
plot(t,thetai(1,:),'LineWidth',1.5);
s = title('$\hat{\theta}_1$','fontsize',14);
set(s,'Interpreter','latex','FontSize',14)
set(gca,'fontsize',12),grid
axis([0 10 0 0.05]);
subplot(3,3,2)
plot(t,thetai(2,:),'LineWidth',1.5);
s = title('$\hat{\theta}_2$','fontsize',14);
set(s,'Interpreter','latex','FontSize',14)
set(gca,'fontsize',12),grid
axis([0 10 -0.01 0.01]);
subplot(3,3,3)
plot(t,thetai(3,:),'LineWidth',1.5);
s = title('$\hat{\theta}_3$','fontsize',14);
set(s,'Interpreter','latex','FontSize',14)
set(gca,'fontsize',12),grid
axis([0 10 0 0.005]);
subplot(3,3,4)
plot(t,thetai(4,:),'LineWidth',1.5);
s = title('$\hat{\theta}_4$','fontsize',14);
set(s,'Interpreter','latex','FontSize',14)
set(gca,'fontsize',12),grid
axis([0 10 0 0.002]);
subplot(3,3,5)
plot(t,thetai(5,:),'LineWidth',1.5);
s = title('$\hat{\theta}_5$','fontsize',14);
set(s,'Interpreter','latex','FontSize',14)
set(gca,'fontsize',12),grid
axis([0 10 0 0.18]);
subplot(3,3,6)
plot(t,thetai(6,:),'LineWidth',1.5);
s = title('$\hat{\theta}_6$','fontsize',14);
set(s,'Interpreter','latex','FontSize',14)
set(gca,'fontsize',12),grid
axis([0 10 -0.004 0.008]);
subplot(3,3,7)
plot(t,thetai(7,:),'LineWidth',1.5);
s = title('$\hat{\theta}_7$','fontsize',14);
set(s,'Interpreter','latex','FontSize',14)
xlabel('time [s]','FontSize',12)
set(gca,'fontsize',12),grid
axis([0 10 -0.02 0.02]);
subplot(3,3,8)
plot(t,thetai(8,:),'LineWidth',1.5);
s = title('$\hat{\theta}_8$','fontsize',14);
set(s,'Interpreter','latex','FontSize',14)
xlabel('time [s]','FontSize',12)
set(gca,'fontsize',12),grid
axis([0 10 -0.01 0.04]);
subplot(3,3,9)
plot(t,thetai(9,:),'LineWidth',1.5);
s = title('$\hat{\theta}_9$','fontsize',14);
set(s,'Interpreter','latex','FontSize',14)
xlabel('time [s]','FontSize',12)
set(gca,'fontsize',12),grid
axis([0 10 0 0.08]);

7.3 结果

参考

如何根据电机的位置信号提取速度信号？ 机器人怎么测量关节速度？ 机器人动力学参数辨识 机器人动力学方程的四种形式 《机器人学导论》