机器人基础之动力学

Newton-Euler动力学方法

Newton-Euler动力学方法是研究机器人动力学问题的一种重要方法。
如果一直机器人关节的位置、速度和加速度以及机器人的运动学和质量分布信息，可以采用Newton-Euler动力学求出关节需要提供的驱动力和驱动力矩。
Newton-Euler动力学方法主要包括速度和加速度的递推计算以及力和力矩的递推计算两个步骤。

外推计算速度和加速度

从连杆1到连杆n递推计算各连杆的速度和加速度，并由此计算各连杆所受的惯性力和惯性力矩，公式如下：$${}^{i+1}{\omega }_{i+1}={}_i^{i+1}R{}^i{\omega }_i+{\dot{\theta }}_{i+1}{}^{i+1}{Z}_{i+1}$$ $${}^{i+1}{\dot{\omega }}_{i+1}={}_i^{i+1}R{}^i{\dot{\omega }}_i+{}_i^{i+1}R{}^i{\omega }_i\times {\dot{\theta }}_{i+1}{}^{i+1}{Z}_{i+1}+{\ddot{\theta }}_{i+1}{}^{i+1}{Z}_{i+1}$$ $${}^{i+1}{v}_{i+1}={}_i^{i+1}R({}^i{v}_i+{}^i{\omega }_i\times {}^i{p}_{i+1})$$ $${}^{i+1}{\dot{v}}_{i+1}={}_i^{i+1}R[{}^i{\dot{v}}_i+{}^i{\dot{\omega }}_i\times {}^i{p}_{i+1}+{}^i{\omega }_i\times ({}^i{\omega }_i\times {}^i{p}_{i+1})]$$ $${}^{i+1}{\dot{v}}_{Ci+1}={}^{i+1}{\dot{v}}_{i+1}+{}^{i+1}{\dot{\omega }}_{i+1}\times {}^{i+1}{P}_{Ci+1}+{}^{i+1}{\omega }_{i+1}\times ({}^{i+1}{\omega }_{i+1}\times {}^{i+1}{P}_{Ci+1})$$ $${}^{i+1}{F}_{ci+1}=m_{i+1}{}^{i+1}{\dot{v}}_{ci+1}$$ $${}^{i+1}{N}_{ci+1}={}^{ci+1}{I}_{i+1}{}^{i+1}{\dot{\omega }}_{i+1}+{}^{i+1}{\omega }_{i+1}\times ({}^{ci+1}{I}_{i+1}{}^{i+1}{\omega }_{i+1})$$

内推计算力和力矩

从连杆n到连杆1计算各连杆内部相互作用的力和力矩，以及关节驱动力和力矩。公式如下：$${}^i{f}_i={}_{i+1}^{i}R{}^{i+1}{f}_{i+1}+{}^i{F}_{ci}$$ $${}^i{n}_i={}^i{N}_{ci}+{}_{i+1}^{i}R{}^{i+1}{n}_{i+1}+{}^i{P}_{ci}\times {}^i{F}_{ci}+{}^i{P}_{i+1}\times {}_{i+1}^{i}R{}^{i+1}{f}_{i+1}$$

MATLAB实现

function [Inertial_Force_Matrix, Drive_Force_Matrix] = NewtonEuler(DH_parameter, Pc_Matrix, Mass_Vector, Inertial_Matrix, Force_Vector, dTheta, ddTheta)
% Newton-Euler动力学方法
% DH_parameter：DH参数表；
% Pc_Matrix：各连杆质心在对应坐标系中的位置矩阵，大小为3*自由度；
% Mass_Vector：各连杆的质量；
% Inertial_Matrix：各连杆的惯性张量矩阵，大小为3*（3*自由度）；
% Force_Vector：机器人末端所受力和力矩，为6为列向量；

[dim_row, dim_col] = size(DH_parameter);

angVel = [0; 0; 0);
vel = [0; 0; 0];
angAccel = [0; 0; 0];
accel = [0; 0; 0];

Inertial_Force_Matrix = (zeros(6, dim_row));
Drive_Force_Matrix = (zeros(6, dim_row));

f = Force_Vector(1:3, 1);
n = Force_Vector(4:6, 1);

% 外推计算速度和加速度 
for i = 1 : dim_row
    T_Matrix = calT(DH_parameter, i - 1, i);
    P = T_Matrix(1:3, 4);
    T_inv = calTInv(T_Matrix);
    R = T_inv(1:3, 1:3);
    I = Inertial_Matrix(:, (3*i-2) : (3*i));

    cal_angVel = angVelTrans(R, angVel, dTheta(i));
    cal_vel = linVelTrans(R, vel, angVel, P);
    cal_angAccel = angAccelTrans(R, angAccel, angVel, dTheta(i), ddTheta(i));
    cal_accel = linAccelTrans(R, accel, angAccel, angVel, P);
    cAccel = cal_accel + cross(cal_angAccel, Pc_Matrix(:, i)) + cross(cal_angVel, cross(cal_angVel, Pc_Matrix(:, i)));
    
    F = Mass_Vector(i) * cAccel;
    N = I * cal_angAccel + cross(cal_angVel, I * cal_angVel);
    Inertial_Force_Matrix(1:3, i) = F;
    Inertial_Force_Matrix(4:6, i) = N;
    
    angVel = cal_angVel;
    vel = cal_vel;
    angAccel = cal_angAccel;
    accel = cal_accel;
end

Drive_Force_Matrix(1:3, dim_row) = f;
Drive_Force_Matrix(4:6, dim_row) = n;
    
% 内推计算力和力矩
for i = (dim_row - 1) : -1 : 1
    % 计算i+1相对于i的齐次变换矩阵
    T = calT(DH_parameter, i - 1, i);
    [R, P] = getRP(T);
    
    % 代入运动学外推得到的惯性力fi和惯性力矩ni
    fi = Inertial_Force_Matrix(1:3, i);
    ni = Inertial_Force_Matrix(4:6, i);
    
    pc = Pc_Matrix(:, i); % 质心在i中的位置
    F = R * f + fi; % f为关节所受的力， n为关节所受的力矩
    N = ni + R * n + cross(pc, fi) + cross(P, R * f);
    
    Drive_Force_Matrix(1:3, i) = F;
    Drive_Force_Matrix(4:6, i) = N;
    f = F;
    n = N; 
end