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

236
0
2020年12月2日 09时03分

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

一、概述

1.1 二轴机械臂

 

1

 

参数 说明
m 1 m_1m1 连杆1质量
m 2 m_2m2 连杆2质量
L 1 L_1L1 连杆1长度
L 2 L_2L2 连杆2长度
I 1 I_1I1 连杆1惯性矩阵
I 2 I_2I2 连杆2惯性矩阵
l 1 l_1l1 连杆1质心位置
l 2 l_2l2 连杆2质心位置
f v 1 f_{v1}fv1 关节1黏性摩擦系数
f v 2 f_{v2}fv2 关节2黏性摩擦系数
f c 1 f_{c1}fc1 关节1库仑摩擦系数
f c 2 f_{c2}fc2 关节2库仑摩擦系数

1.2 参数识别概述

参数识别流程

 

2

 

3

 

4

 

5

 

6

 

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

三、力矩测量

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

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

 

7

 

78

 

8

 

9

 

五、计算速度

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

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

11

 

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

 

11

12

 

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

低通滤波函数

 

13

 

14

 

其Z变换为

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

 

15

 

%% 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 识别框架

 

20

 

  • On-line

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

  • Off-line

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

 

21

 

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

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

7.2 Matlab实现

  • 流程

22

 

  • 程序
%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 结果

 

25

 

参考

如何根据电机的位置信号提取速度信号?

机器人怎么测量关节速度?

机器人动力学参数辨识

机器人动力学方程的四种形式

《机器人学导论》

发表评论

后才能评论