一、動力學方程

??機器人的動力學公式描述如下：

式中，$\mathbf{\tau }$表示關節驅動力矩矢量；$\mathbf{q},\dot{\mathbf{q}},\ddot{\mathbf{q}}$分別為廣義的關節位置、速度和加速度；$\mathbf{M}$為關節的空間慣量矩陣；$\mathbf{C}$為科氏力和離心力耦合矩陣；$\mathbf{G}$為重力；${\mathbf{F}}_f$為關節摩擦力。

??機器人的動力學參數包括慣性參數和摩擦參數。
??（1）慣性參數有：連桿質量$m$、相對于連桿坐標系的質心矢量$\mathbf{r}$和轉動慣量矩陣$\mathbf{I}$。其中，質心矢量$\mathbf{r}$可以表示為：

式中，$r_x$、$r_y$和$r_z$分別表示質心矢量$\mathbf{r}$在連桿坐標系下三個坐標軸的分量。轉動慣量矩陣$\mathbf{I}$為包含六個獨立元素的二維矩陣，表示為：

式中，主對角元素為慣性矩，非主對角元素為慣性積。

??（2）機器人動力學建模中常用的摩擦模型為庫倫-粘滯摩擦模型，其表達式為：

式中，$f_c$和$f_v$分別表示庫倫摩擦系數和粘滯摩擦系數；$v$表示關節速度。注意：對于庫倫摩擦系數的處理不同人有不同的處理，有的地方認為庫倫摩擦是對稱的，即當機器人關節正向旋轉和反向旋轉時，庫倫摩擦力大小相等，方向相反，也即$f_c^{+}=f_c^{?}$；也有的地方認為庫倫摩擦是非對稱的，即當機器人關節正向旋轉和反向旋轉時，庫倫摩擦力大小不相等。

二、機器人工具箱描述動力學方程

2.1 動力學參數賦值

??在機器人工具箱中，提供了如下動力學參數輸入接口：
??（1）Link.m：表示連桿的質量；
??（2）Link.r：表示連桿的質心矢量；
??（3）Link.I：表示連桿的慣量矩陣；
??（4）Link.Jm：表示驅動電機的轉動慣量；
??（5）Link.B：表示粘滯摩擦系數；
??（6）Link.Tc：表示庫倫摩擦系數；
??（7）Link.G：表示電機齒輪傳動比（默認為1）

??這里仍然以3-DOF平面機械臂為例：

%% 動力學
% RRR機械臂
clear;
close all;
clc;%               theta(z)   d(z)     a(x)     alpha(x)  
RRR_L(1) = Link([  0       0        1        0    ],'standard');
RRR_L(2) = Link([  0       0      0.8        0    ],'standard');
RRR_L(3) = Link([  0       0      0.6        0    ],'standard');% 連桿1動力學參數
RRR_L(1).m = 4.0;
RRR_L(1).r = [0.12; 0.08; 0.31];
RRR_L(1).I = [0.32 0.01 0.02;0.01 0.12 0.11;0.02 0.11 0.41];
RRR_L(1).Jm = 0.0012;
RRR_L(1).B = 0.00148;
RRR_L(1).Tc = [+0.395, -0.435];
RRR_L(1).G = 1.2;% 連桿2動力學參數
RRR_L(2).m = 15.2;
RRR_L(2).r = [-0.475; 0.097; 0.06];
RRR_L(2).I = [1.21 0.21 0.32;0.21 0.52 0.11;0.32 0.11 0.51];
RRR_L(2).Jm = 0.0048;
RRR_L(2).B = 0.00329;
RRR_L(2).Tc = [+0.462; -0.561];
RRR_L(2).G = 1.4;% 連桿3動力學參數
RRR_L(3).m = 0.6;
RRR_L(3).r = [0.01; 0.097; 0.016];
RRR_L(3).I = [0.021 0.03 0.382;0.03 0.152 0.11;0.382 0.11 0.651];
RRR_L(3).Jm = 0.0061;
RRR_L(3).B = 0.00429;
RRR_L(3).Tc = [+0.262; -0.661];
RRR_L(3).G = 1.7;three_link = SerialLink(RRR_L, 'name', '3-DOF');

??采用dyn( )函數可以查看動力學參數，如圖所示：

2.2 動力學方程中的各項表示

（1）空間慣量矩陣$\mathbf{M}(\mathbf{q})$
??機器人的空間慣量是機器人各關節的位姿的函數，在不同機器人位形時具有不同的值。機器人工具箱中可以調用robot.inertia(q)函數獲得空間慣量矩陣。
??例如：當3-DOF平面機械臂三個關節角度為30°、45°和60°時，其空間慣量矩陣為：

（2）科氏力和離心力耦合矩陣$\mathbf{C}(\mathbf{q},\dot{\mathbf{q}})$
??科氏力和離心力耦合矩陣是關節位置和速度的函數。機器人工具箱中可以調用robot.coriolis(q, qd)函數獲得該耦合矩陣。
??例如：當3-DOF平面機械臂三個關節角度為30°、45°和60°，三個關節速度為10°/s、20°/s和30°/s時，其科氏力和離心力耦合矩陣為：

（3）重力矩陣$\mathbf{G}(\mathbf{q})$
??重力矩陣與機器人的位形有關，是對各關節所受重力的描述，其值不受機器人的運動的影響。機器人工具箱中可以調用robot.gravload(q, grav)函數來獲得重力矩陣，其中grav自定義重力加速度向量。
??例如：當3-DOF平面機械臂三個關節角度為30°、45°和60°，重力加速度向量為$y$軸負向，即grav = [0; -9.8; 0]。重力矩陣為：

（4）摩擦力矩陣${\mathbf{F}}_f(\dot{\mathbf{q}})$
??摩擦力矩陣是由各關節的給定摩擦參數數值決定的，大小與各關節的速度有關。機器人工具箱中可以調用robot.friction(qd)函數來獲得重力矩陣。
??例如：當3-DOF平面機械臂三個關節速度為10°/s、20°/s和30°/s時，其摩擦力矩陣為：

三、逆動力學分析

??機器人的逆動力學分析是在給定機器人關節位置、速度和加速度時，計算得到機器人各關節所需要的力和力矩大小。機器人工具箱中可以調用robot.rne(q, qd, qdd, grav)函數來計算逆動力學。其中，q, qd, qdd分別表示機器人關節位置、速度和加速度；grav表示自定義的重力加速度矢量。除此之外，還可以添加參數fext，表示末端執行器受到的外力和力矩$[F_x,F_y,F_z,{\tau }_x,{\tau }_y,{\tau }_z]$
??例子：讓3-DOF平面機械臂按照下圖所示的軌跡運動。

代碼：

%% 動力學
% RRR機械臂
clear;
close all;
clc;%               theta(z)   d(z)     a(x)     alpha(x)  
RRR_L(1) = Link([  0       0        1        0    ],'standard');
RRR_L(2) = Link([  0       0      0.8        0    ],'standard');
RRR_L(3) = Link([  0       0      0.6        0    ],'standard');% 連桿1動力學參數
RRR_L(1).m = 4.0;
RRR_L(1).r = [0.12; 0.08; 0.31];
RRR_L(1).I = [0.32 0.01 0.02;0.01 0.12 0.11;0.02 0.11 0.41];
RRR_L(1).Jm = 0.0012;
RRR_L(1).B = 0.00148;
RRR_L(1).Tc = [+0.395, -0.435];
RRR_L(1).G = 1.2;% 連桿2動力學參數
RRR_L(2).m = 15.2;
RRR_L(2).r = [-0.475; 0.097; 0.06];
RRR_L(2).I = [1.21 0.21 0.32;0.21 0.52 0.11;0.32 0.11 0.51];
RRR_L(2).Jm = 0.0048;
RRR_L(2).B = 0.00329;
RRR_L(2).Tc = [+0.462; -0.561];
RRR_L(2).G = 1.4;% 連桿3動力學參數
RRR_L(3).m = 5.6;
RRR_L(3).r = [0.01; 0.097; 0.016];
RRR_L(3).I = [0.921 0.03 0.382;0.03 0.252 0.11;0.382 0.11 1.251];
RRR_L(3).Jm = 0.0061;
RRR_L(3).B = 0.00429;
RRR_L(3).Tc = [+0.262; -0.661];
RRR_L(3).G = 1.7;three_link = SerialLink(RRR_L, 'name', '3-DOF');delta_t = 0.02;
t = 0:delta_t:4;
m = length(t);theta1 = 60*sin(4*pi*t/4);
theta2 = 60*sin(2*pi*t/4);
theta3 = 30*sin(2*pi*t/4);theta1_d = 60*pi*cos(4*pi*t/4);
theta2_d = 30*pi*cos(2*pi*t/4);
theta3_d = 15*pi*cos(2*pi*t/4);theta1_dd = -60*pi*pi*sin(4*pi*t/4);
theta2_dd = -15*pi*pi*sin(2*pi*t/4);
theta3_dd = -7.5*pi*pi*sin(2*pi*t/4);q = [theta1;theta2;theta3]'*pi/180;
qd = [theta1_d;theta2_d;theta3_d]'*pi/180;
qdd = [theta1_dd;theta2_dd;theta3_dd]'*pi/180;% 關節位置、速度、加速度繪圖
figure(1)
subplot(3,1,1)
plot(t, q(:,1)*180/pi, 'b')
hold on
plot(t, q(:,2)*180/pi, 'r--')
hold on
plot(t, q(:,3)*180/pi, 'm')
xlabel('time (s)', 'Interpreter', 'latex')
ylabel('$\theta$ (deg)', 'Interpreter', 'latex')
legend('$\theta_1$','$\theta_2$','$\theta_3$', 'Interpreter', 'latex')
set(gca, 'FontName','Times New Roman')subplot(3,1,2)
plot(t, qd(:,1)*180/pi, 'b')
hold on
plot(t, qd(:,2)*180/pi, 'r--')
hold on
plot(t, qd(:,3)*180/pi, 'm')
xlabel('time (s)', 'Interpreter', 'latex')
ylabel('$\dot{\theta}$ (deg)', 'Interpreter', 'latex')
legend('$\dot{\theta_1}$','$\dot{\theta_2}$','$\dot{\theta_3}$', 'Interpreter', 'latex')
set(gca, 'FontName','Times New Roman')subplot(3,1,3)
plot(t, qdd(:,1)*180/pi, 'b')
hold on
plot(t, qdd(:,2)*180/pi, 'r--')
hold on
plot(t, qdd(:,3)*180/pi, 'm')
xlabel('time (s)', 'Interpreter', 'latex')
ylabel('$\ddot{\theta}$ (deg)', 'Interpreter', 'latex')
legend('$\ddot{\theta_1}$','$\ddot{\theta_2}$','$\ddot{\theta_3}$', 'Interpreter', 'latex')
set(gca, 'FontName','Times New Roman')set(gcf, 'color',[1 1 1]);% 運動示意
figure(2)
three_link.plot(q,'trail','b');% 逆動力學
grav = [0; -9.8; 0];
tau = three_link.rne(q, qd, qdd, grav);% 關節驅動力矩
figure(3)
plot(t,tau(:,1), 'b')
hold on
plot(t, tau(:,2), 'r--')
hold on
plot(t, tau(:,3), 'm')
xlabel('time (s)', 'Interpreter', 'latex')
ylabel('$\tau$ (N/m)', 'Interpreter', 'latex')
legend('$\tau_1$','$\tau_2$','$\tau_3$', 'Interpreter', 'latex')
set(gca, 'FontName','Times New Roman')
set(gcf, 'color',[1 1 1]);

運行結果：

四、結語

??機器人工具箱還有其他的一些應用，譬如正動力學分析、視覺相關應用等，不過筆者對這些沒有接觸過，就不誤導大家了。
??我是木頭人，以上全是個人見解，有問題請大家評論區指出，大家共同進步！！