6DOF机械臂动力学参数辨识(1) —— 动力学方程线性化

Watch night

已于 2025-05-17 16:13:16 修改

阅读量397

点赞数 6

文章标签：算法机器学习人工智能

于 2025-04-03 22:05:00 首次发布

本文链接：https://blog.csdn.net/m0_74196747/article/details/146780120

版权

引言

机械臂动力学参数辨识内容很多，所以我将其分成五个小的模块——动力学方程线性化，获得最小惯性矩阵以及参数集，最优傅里叶轨迹的设计，数据过滤处理以及辨识模型的验证。本篇博文正如标题所示，是基于牛顿-欧拉法建立机械臂动力学方程并将其线性化，推导过程见霍伟《机器人动力学与控制》，需要该电子书籍的可以私信我，书籍封面如下：

本文编写的代码借鉴了许多大佬的代码，其中一位大佬的代码让我受益匪浅，链接如下，供各位看官细品，https://github.com/YanjunLIU-ac/Dynamic_Parameter_Identification_for_Rokae_xMate

一.动力学建模

1.MDH坐标系的建立

本文使用的机械臂为Gluon_6L3，坐标系建立及MDH参数表如下：

2.建立质心动力学方程

动力学建模有两种经典的方法，牛顿欧拉法和拉格朗日法。本文参考《机器人动力学与控制》，通过牛顿欧拉法来推导动力学方程，具体流程如下。
1）外推，求出连杆速度、加速段和质心的力、力矩。参考公式如下：

$\omega_{i+1}=_{}^{i+1}\textrm{}R_{i}\omega_{i}+Z_{i+1}\dot{\theta}_{i+1}$

$\dot{\omega}_{i}=_{}^{i}\textrm{}R_{i-1}\dot{\omega}_{i-1}+{_{}^{i}\textrm{}R_{i-1}\omega_{i-1}}\times\dot{\theta}_{i}Z_{i}+\ddot{\theta}_{i}Z_{i}$

$\dot{v}_{i}=_{}^{i}\textrm{}R_{i-1}({\dot{\omega}_{i-1}}\times\_{}^{i}\textrm{}P_{i-1}+{\omega_{i}}\times({\omega_{i}}\times_{}^{i}\textrm{}P_{i-1})+\dot{v_{i}})$

${\dot{v}{c}}_{i}={\dot{\omega}_{i}}\times Pc_{i}+ {{\omega}_{i}}\times({{\omega}_{i}}\times Pc_{i})+ {\dot{v}}_{i}$

$F_{i}=m_{i}{\dot{v}c}_{i}$

$N_{i}={Ic}_{i}{\dot{w}_{i}}+{{\omega}_{i}}\times{Ic}_{i}{\omega}_{i}$

2)内推，求出各个关节驱动力和力矩

$f_{i}=F_{i}+_{}^{i}\textrm{}R_{i+1}f_{i+1}, (f_{n+1}=0)$

$n_{i}=N_{i}+_{}^{i}\textrm{}R_{i+1}n_{i+1}+{Pc_{i}}\times F_{i}+{P_{i}}\times(_{}^{i}\textrm{}R_{i+1}f_{i+1}),(n_{n+1}=0)$

各个关节的齐次变换矩阵如下：

$_{}^{i-1}\textrm{}T_{i}=\begin{bmatrix} cos(\theta_{i}) & -sin(\theta_{i}) & 0 &a_{i} \\ sin(\theta_{i})cos(\alpha_{i})& cos(\theta_{i})cos(\alpha_{i})& -sin(\alpha_{i})& -d_{i}sin(\alpha_{i})\\ sin(\theta_{i})sin(\alpha_{i}) & cos(\theta_{i})sin(\alpha_{i}) &cos(\alpha_{i}) &d_{i}cos(\alpha_{i}) \\ 0& 0 & 0 & 1 \end{bmatrix}$

相应的旋转矩阵和平移变换如下：

$_{}^{i-1}\textrm{}R_{i}=_{}^{i-1}\textrm{}T_{i}(1:3,1:3)$

$_{}^{i-1}\textrm{}P_{i}=_{}^{i-1}\textrm{}T_{i}(1:3,4)$

用牛顿——欧拉法建立的Gluon_6L3动力学方程，并写出如下相应的代码:

%% dyn_central_newtoneuler.m
%@用符号运算推导得到机械臂质心处的动力学方程
%		  frame{base} -> frame{1} ... frame{6} -> frame{end-effector}
%         (frame{base} = frame{0}, frame{6} = frame{ee})
% @notes: 
% 		1. 基于迭代牛顿-欧拉法的动力学方程
% 		2. 利用改进DH法对机械臂建模
% 		3. Ic表示连杆质心的惯性张量
%       4. 所有单位均为mm(Nmm)

%% PARAMETERS
% 各个连杆转角的sine and cosine值
syms s1 c1 s2 c2 s3 c3 s4 c4 s5 c5 s6 c6 real;
% DH 参数
syms a3 a4 d2 d4 d5 d6 real;
% 关节速度
syms dQ1 dQ2 dQ3 dQ4 dQ5 dQ6 real;
% 关节加速度
syms ddQ1 ddQ2 ddQ3 ddQ4 ddQ5 ddQ6 real;
% 各个连杆质心的三维坐标
syms mc11 mc12 mc13 real;
syms mc21 mc22 mc23 real;
syms mc31 mc32 mc33 real;
syms mc41 mc42 mc43 real;
syms mc51 mc52 mc53 real;
syms mc61 mc62 mc63 real;
% 各个连杆质心处的惯性张量
syms Ic111 Ic122 Ic133 Ic112 Ic113 Ic123 real;
syms Ic211 Ic222 Ic233 Ic212 Ic213 Ic223 real;
syms Ic311 Ic322 Ic333 Ic312 Ic313 Ic323 real;
syms Ic411 Ic422 Ic433 Ic412 Ic413 Ic423 real;
syms Ic511 Ic522 Ic533 Ic512 Ic513 Ic523 real;
syms Ic611 Ic622 Ic633 Ic612 Ic613 Ic623 real;
% 各个连杆的质量
syms m1 m2 m3 m4 m5 m6 real;
% 各个关节处电机转子负载的等效转动惯量
syms Ia1 Ia2 Ia3 Ia4 Ia5 Ia6 real;
% 作用在末端执行器的外分力和分力矩
syms fe1 fe2 fe3 ne1 ne2 ne3 real;
% 重力加速度
syms g real;
% 摩擦力模型 (粘滞摩擦, 库伦摩擦)
syms fv1 fc1 fv2 fc2 fv3 fc3 fv4 fc4 fv5 fc5 fv6 fc6 real;
% rotation matrix, a = [0, -pi/2, pi/2, -pi/2, pi/2, -pi/2, pi/2];
%   R01: frame{1} 相对于 frame{0}的旋转矩阵
%   R10: frame{0} 相对于 frame{1}的旋转矩阵
R01 = [c1 -s1 0;
       s1 c1 0;
	   0 0 1];
R12 = [-s2 -c2 0;
       0 0 -1;
	   c2 -s2 0];
R23 = [c3 -s3 0;
       s3 c3 0;
	   0 0 1];
R34 = [s4 c4 0;
       -c4 s4 0;
	   0 0 1];
R45 = [c5 -s5 0;
       0 0 1;
	   -s5 -c5 0];
R56 = [c6 -s6 0;
       0 0 -1;
	   s6 c6 0];
R67 = [1 0 0;
       0 1 0;
	   0 0 1];   
R07 = R01 * R12 * R23 * R34 * R45 * R56 * R67;
R10 = R01'; R21 = R12'; R32 = R23'; R43 = R34';
R54 = R45'; R65 = R56'; R76 = R67';
disp("<INFO> PARAMETERS Loaded!!");

%% 向量表示
% 关节速度
dQZ1 = [0; 0; dQ1];
dQZ2 = [0; 0; dQ2];
dQZ3 = [0; 0; dQ3];
dQZ4 = [0; 0; dQ4];
dQZ5 = [0; 0; dQ5];
dQZ6 = [0; 0; dQ6];
% 关节加速度
ddQZ1 = [0; 0; ddQ1];
ddQZ2 = [0; 0; ddQ2];
ddQZ3 = [0; 0; ddQ3];
ddQZ4 = [0; 0; ddQ4];
ddQZ5 = [0; 0; ddQ5];
ddQZ6 = [0; 0; ddQ6];
% 上一个坐标系相对与当前坐标系的位置变化
P01 = [0 ;0 ;0];
P12 = [0 ;-d2 ;0];
P23 = [a3 ;0 ;0];
P34 = [a4 ;0 ;d4];
P45 = [0 ;d5 ;0];
P56 = [0 ;-d6 ;0];
P67 = [0 ;0 ;0];
% 各个连杆质心坐标向量
Pc1 = [mc11; mc12; mc13];
Pc2 = [mc21; mc22; mc23];
Pc3 = [mc31; mc32; mc33];
Pc4 = [mc41; mc42; mc43];
Pc5 = [mc51; mc52; mc53];
Pc6 = [mc61; mc62; mc63]; 
% 各个连杆质心处惯性张量矩阵
Ic1 = [Ic111 Ic112 Ic113; Ic112 Ic122 Ic123; Ic113 Ic123 Ic133]; 
Ic2 = [Ic211 Ic212 Ic213; Ic212 Ic222 Ic223; Ic213 Ic223 Ic233]; 
Ic3 = [Ic311 Ic312 Ic313; Ic312 Ic322 Ic323; Ic313 Ic323 Ic333]; 
Ic4 = [Ic411 Ic412 Ic413; Ic412 Ic422 Ic423; Ic413 Ic423 Ic433]; 
Ic5 = [Ic511 Ic512 Ic513; Ic512 Ic522 Ic523; Ic513 Ic523 Ic533];
Ic6 = [Ic611 Ic612 Ic613; Ic612 Ic622 Ic623; Ic613 Ic623 Ic633];  
% 作用在末端执行器上的外力和外力矩
fe = [fe1; fe2; fe3]; ne = [ne1; ne2; ne3];
disp("<INFO> VECTORIZATION complete!!");

%% 迭代牛顿-欧拉法的动力学方程
% 使用克雷格的符号表达: 
%   wn = 连杆‘n’的角速度 (\omega)
% 	dwn = 连杆 'n' 的角加速度(\alpha)
% 	dvn = 连杆 'n' 线加速度 (\a)
%   dvcn = 连杆 'n' 在质心处的线加速度 (\a_c)
%   Fn = 连杆 'n' 在质心处受到的外力
%   Nn = 连杆 'n' 在质心处受到的外力矩 
w0 = [0;0;0];	% frame{base}基座标系的角速度
dw0 = [0;0;0];	% frame{base}基座标系的角加速度
dv0 = [0;0;g];	% frame{base}基座标系的线加速度 (考虑重力)

% 向外地推得到速度和加速度
w1 = R10 * w0 + dQZ1;
dw1 = R10 * dw0 + cross(R10*w0, dQZ1) + ddQZ1;
dv1 = R10 * (cross(w0, P01) + cross(w0, cross(w0, P01)) + dv0);
dvc1 = cross(dw1, Pc1) + cross(w1, cross(w1, Pc1)) + dv1;
F1 = m1 * dvc1;
N1 = Ic1 * dw1 + cross(w1, Ic1*w1);
disp("<INFO> J1 outward ITERATION complete.");

w2 = R21 * w1 + dQZ2;
dw2 = R21 * dw1 + cross(R21*w1, dQZ2) + ddQZ2;
dv2 = R21 * (cross(w1, P12) + cross(w1, cross(w1, P12)) + dv1);
dvc2 = cross(dw2, Pc2) + cross(w2, cross(w2, Pc2)) + dv2;
F2 = m2 * dvc2;
N2 = Ic2 * dw2 + cross(w2, Ic2*w2);
disp("<INFO> J2 outward ITERATION complete.");

w3 = R32 * w2 + dQZ3;
dw3 = R32 * dw2 + cross(R32*w2, dQZ3) + ddQZ3;
dv3 = R32 * (cross(w2, P23) + cross(w2, cross(w2, P23)) + dv2);
dvc3 = cross(dw3, Pc3) + cross(w3, cross(w3, Pc3)) + dv3;
F3 = m3 * dvc3;
N3 = Ic3 * dw3 + cross(w3, Ic3*w3);
disp("<INFO> J3 outward ITERATION complete.");

w4 = R43 * w3 + dQZ4;
dw4 = R43 * dw3 + cross(R43*w3, dQZ4) + ddQZ4;
dv4 = R43 * (cross(w3, P34) + cross(w3, cross(w3, P34)) + dv3);
dvc4 = cross(dw4, Pc4) + cross(w4, cross(w4, Pc4)) + dv4;
F4 = m4 * dvc4;
N4 = Ic4 * dw4 + cross(w4, Ic4*w4);
disp("<INFO> J4 outward ITERATION complete.");

w5 = R54 * w4 + dQZ5;
dw5 = R54 * dw4 + cross(R54*w4, dQZ5) + ddQZ5;
dv5 = R54 * (cross(w4, P45) + cross(w4, cross(w4, P45)) + dv4);
dvc5 = cross(dw5, Pc5) + cross(w5, cross(w5, Pc5)) + dv5;
F5 = m5 * dvc5;
N5 = Ic5 * dw5 + cross(w5, Ic5*w5);
disp("<INFO> J5 outward ITERATION complete.");

w6 = R65 * w5 + dQZ6;
dw6 = R65 * dw5 + cross(R65*w5, dQZ6) + ddQZ6;
dv6 = R65 * (cross(w5, P56) + cross(w5, cross(w5, P56)) + dv5);
dvc6 = cross(dw6, Pc6) + cross(w6, cross(w6, Pc6)) + dv6;
F6 = m6 * dvc6;
N6 = Ic6 * dw6 + cross(w6, Ic6*w6);
disp("<INFO> J6 outward ITERATION complete.");

% 向内递推得到力和力矩
% DEBUG: POINT 3
f6 = fe + F6;
n6 = N6 + ne + cross(Pc6, F6);
T6 = n6' * [0;0;1];
disp("<INFO> J6 inward ITERATION complete.");

f5 = R56 * f6 + F5;
n5 = N5 + R56 * n6 + cross(Pc5, F5) + cross(P56, R56 * f6);
T5 = n5' * [0;0;1];
disp("<INFO> J5 inward ITERATION complete.");

f4 = R45 * f5 + F4;
n4 = N4 + R45 * n5 + cross(Pc4, F4) + cross(P45, R45 * f5);
T4 = n4' * [0;0;1];
disp("<INFO> J4 inward ITERATION complete.");

f3 = R34 * f4 + F3;
n3 = N3 + R34 * n4 + cross(Pc3, F3) + cross(P34, R34 * f4);
T3 = n3' * [0;0;1];
disp("<INFO> J3 inward ITERATION complete.");

f2 = R23 * f3 + F2;
n2 = N2 + R23 * n3 + cross(Pc2, F2) + cross(P23, R23 * f3);
T2 = n2' * [0;0;1];
disp("<INFO> J2 inward ITERATION complete.");

f1 = R12 * f2 + F1;
n1 = N1 + R12 * n2 + cross(Pc1, F1) + cross(P12, R12 * f2);
T1 = n1' * [0;0;1];
disp("<INFO> J1 inward ITERATION complete.");

%% 带摩擦的迭代动力学模型 (6x1 syms)
T1 = T1 + Ia1*ddQ1 + fv1*dQ1 + fc1*sign(dQ1);
T2 = T2 + Ia2*ddQ2 + fv2*dQ2 + fc2*sign(dQ2);
T3 = T3 + Ia3*ddQ3 + fv3*dQ3 + fc3*sign(dQ3);
T4 = T4 + Ia4*ddQ4 + fv4*dQ4 + fc4*sign(dQ4);
T5 = T5 + Ia5*ddQ5 + fv5*dQ5 + fc5*sign(dQ5);
T6 = T6 + Ia6*ddQ6 + fv6*dQ6 + fc6*sign(dQ6);
centraTAU = [T1; T2; T3; T4; T5; T6];
disp("<INFO> DYNAMIC EQUATION obtained");

3.建立各连杆动力学

欧拉公式相对连杆质心C坐标系建立，惯性矩阵Ic是相对于质心的惯性矩阵，欧拉公式只能在定点或者质心处才能成立，在动点或者非质心处不能使用，即不能在连杆处直接使用。而为了分离惯性参数，需要将惯性矩阵Ic表示相对于连杆坐标系原点处的惯性矩阵，这里我们需要用到平行轴定理，公式如下。

$Ic=Io-(Pc^{T}PcI-PcPc^{T})$

将上述公式的Ic用Io代替，就得到了连杆处的动力学方程，这样就可以为后面线性化动力学方程做基础，代码如下：

%% dyn_inertia_centra2link.m
% @brief:将质心处的惯性张量转化成连杆处的惯性张量
%
% @notes: 基于质心处的动力学参数组成如下：
%         {m, mc1, mc2, mc3, Ic11, Ic22, Ic33, Ic12, Ic13, Ic23, Ia, fv, fc}
%        基于连杆处的动力学参数组成如下：
%         {m, mc1, mc2, mc3, Io11, Io22, Io33, Io12, Io13, Io23, Ia, fv, fc}
% @notes: 所有单位均为mm(Nmm)

%% PARAMETERS
% 连杆的惯性张量
%   Io denotes I^{O_i}, where {O_i} is the origin of link frame {i}.
syms Io111 Io122 Io133 Io112 Io113 Io123 real;
syms Io211 Io222 Io233 Io212 Io213 Io223 real;
syms Io311 Io322 Io333 Io312 Io313 Io323 real;
syms Io411 Io422 Io433 Io412 Io413 Io423 real;
syms Io511 Io522 Io533 Io512 Io513 Io523 real;
syms Io611 Io622 Io633 Io612 Io613 Io623 real;
% position of CoM
% Pc1 = [mc11; mc12; mc13]; 
% Pc2 = [mc21; mc22; mc23]; 
% Pc3 = [mc31; mc32; mc33]; 
% Pc4 = [mc41; mc42; mc43]; 
% Pc5 = [mc51; mc52; mc53]; 
% Pc6 = [mc61; mc62; mc63]; 
% Pc7 = [mc71; mc72; mc73];
disp("<INFO> PARAMETERS Loaded!!");

%% VECTORIZATION
% 各个连杆的惯性张量矩阵
Io1 = [Io111 Io112 Io113; Io112 Io122 Io123; Io113 Io123 Io133]; 
Io2 = [Io211 Io212 Io213; Io212 Io222 Io223; Io213 Io223 Io233]; 
Io3 = [Io311 Io312 Io313; Io312 Io322 Io323; Io313 Io323 Io333]; 
Io4 = [Io411 Io412 Io413; Io412 Io422 Io423; Io413 Io423 Io433]; 
Io5 = [Io511 Io512 Io513; Io512 Io522 Io523; Io513 Io523 Io533]; 
Io6 = [Io611 Io612 Io613; Io612 Io622 Io623; Io613 Io623 Io633];  
disp("<INFO> VECTORIZATION complete!!");

%% INSTANTIATION
% 用平行轴定理将Ic 转化成 Io
I = sym(eye(3));
Ic1 = Io1 - m1 * (Pc1'*Pc1*I - Pc1*Pc1');
Ic2 = Io2 - m2 * (Pc2'*Pc2*I - Pc2*Pc2');
Ic3 = Io3 - m3 * (Pc3'*Pc3*I - Pc3*Pc3');
Ic4 = Io4 - m4 * (Pc4'*Pc4*I - Pc4*Pc4');
Ic5 = Io5 - m5 * (Pc5'*Pc5*I - Pc5*Pc5');
Ic6 = Io6 - m6 * (Pc6'*Pc6*I - Pc6*Pc6');
disp("<INFO> CONVERTED centralInertia to linkInertia.");
% 替换动力学模型中的Ic
linkTAU = subs(centraTAU,  ...
              {'Ic111', 'Ic122', 'Ic133', 'Ic112', 'Ic113', 'Ic123',  ...
	           'Ic211', 'Ic222', 'Ic233', 'Ic212', 'Ic213', 'Ic223',  ...
	           'Ic311', 'Ic322', 'Ic333', 'Ic312', 'Ic313', 'Ic323',  ...
	           'Ic411', 'Ic422', 'Ic433', 'Ic412', 'Ic413', 'Ic423',  ...
	           'Ic511', 'Ic522', 'Ic533', 'Ic512', 'Ic513', 'Ic523',  ...
	           'Ic611', 'Ic622', 'Ic633', 'Ic612', 'Ic613', 'Ic623',}, ...
              {Ic1(1, 1), Ic1(2, 2), Ic1(3, 3), Ic1(1, 2), Ic1(1, 3), Ic1(2, 3),  ...
	           Ic2(1, 1), Ic2(2, 2), Ic2(3, 3), Ic2(1, 2), Ic2(1, 3), Ic2(2, 3),  ...
	           Ic3(1, 1), Ic3(2, 2), Ic3(3, 3), Ic3(1, 2), Ic3(1, 3), Ic3(2, 3),  ...
	           Ic4(1, 1), Ic4(2, 2), Ic4(3, 3), Ic4(1, 2), Ic4(1, 3), Ic4(2, 3),  ...
	           Ic5(1, 1), Ic5(2, 2), Ic5(3, 3), Ic5(1, 2), Ic5(1, 3), Ic5(2, 3),  ...
	           Ic6(1, 1), Ic6(2, 2), Ic6(3, 3), Ic6(1, 2), Ic6(1, 3), Ic6(2, 3)});
disp("<INFO> INSTANTIATION complete!!");

二.线性化动力学方程

我们定义13个待辨识的动力学参数，分别是：一阶矩，惯性张量，质量，电机转子的等效转动惯量以及用粘滞摩擦和库伦摩擦来近似的等效摩擦，符号如下：

$mx,my,mz,Ioxx,Ixy,Ioxz,Ioyy,Ioyz,Iozz,m,Ia,Fc,Fv$

只需要将上述得到的在连杆处的动力学方程中包含如上13个动力学参数的项，用符号变量1进行替换，就可以分离动力学参数，代码如下：

%% dyn_param_linearization.m
% @brief: 将动力学方程转化成线性形式 `tau=WP`
%         
% @notes: W 是回归矩阵, 只和 [q, qd, qdd]有关, 维度为(6, 13x6)
%         P 是标准动力学参数集, 维度为(13x6, 1)
%         
% @notes: 所有单位均为mm(Nmm)
%% PARAMETER
% 每个关节动力学参数的个数
% {m,mc1,mc2,mc3,Ioxx,Ioyy,Iozz,Ioxy,Ioxz,Ioyz,Ia,fv,fc}
pnum_per_joint = 13;  
%6个关节总共的动力学参数个数
pnum_sum = pnum_per_joint * 6;

%% INSTANTIATION
%  e.g. [t1, t2]' = A * b = [A1, A2] * [b1, b2]'
%  where t1 = A11 * b1 + A12 * b2
%        t2 = A21 * b1 + A22 * b2
%        A1 = [A11, A21]', A2 = [A12, A22]' denote column vectors
%  to figure out A1: we can assign {b1=1, b2=0} in matrix A
%                A2: we can assign {b1=0, b2=1} in matrix A
%  because variable b1 and b2 are linearly independent, no non-linear term like b1*b2 in equation
Q_ = zeros(pnum_sum, 1);	% index matrix
W = sym(zeros(6, pnum_sum));	% regression matrix
for i = 1:pnum_sum
    Q_ = zeros(pnum_sum, 1);
	% index of joint-1, e.g. k1 = [0, 1, 2, 3, 4, 5] for 6-joint robot
    k1 = fix((i-1) / pnum_per_joint);%从关节1-6中选择
	% index of param-1, e.g. k2 = [0, 1, 2, ..., 12] for standard 13 params	
    k2 = mod(i-1, pnum_per_joint);%从1-13个动力学参数中选择		
    
    Q_(i) = 1;
    %将所选择的动力学参数用1替换，进而将动力学参数分离
    if (k2>=1 && k2<=3)		% for k2 = {mc1, mc2, mc3}
        Q_(k1 * pnum_per_joint + 1) = 1;   % for m
  
        W(:,i) = subs(linkTAU, ...
		              {'m1', 'mc11', 'mc12', 'mc13', 'Io111', 'Io122', 'Io133', 'Io112', 'Io113', 'Io123', 'Ia1', 'fv1', 'fc1', ...
                       'm2', 'mc21', 'mc22', 'mc23', 'Io211', 'Io222', 'Io233', 'Io212', 'Io213', 'Io223', 'Ia2', 'fv2', 'fc2', ...
                       'm3', 'mc31', 'mc32', 'mc33', 'Io311', 'Io322', 'Io333', 'Io312', 'Io313', 'Io323', 'Ia3', 'fv3', 'fc3', ...
                       'm4', 'mc41', 'mc42', 'mc43', 'Io411', 'Io422', 'Io433', 'Io412', 'Io413', 'Io423', 'Ia4', 'fv4', 'fc4', ...
                       'm5', 'mc51', 'mc52', 'mc53', 'Io511', 'Io522', 'Io533', 'Io512', 'Io513', 'Io523', 'Ia5', 'fv5', 'fc5', ...
                       'm6', 'mc61', 'mc62', 'mc63', 'Io611', 'Io622', 'Io633', 'Io612', 'Io613', 'Io623', 'Ia6', 'fv6', 'fc6'}, ...
					  {Q_(1), Q_(2), Q_(3), Q_(4), Q_(5), Q_(6), Q_(7), Q_(8), Q_(9), Q_(10), Q_(11), Q_(12), Q_(13), ...
					   Q_(14), Q_(15), Q_(16), Q_(17), Q_(18), Q_(19), Q_(20), Q_(21), Q_(22), Q_(23), Q_(24), Q_(25), Q_(26), ...
					   Q_(27), Q_(28), Q_(29), Q_(30), Q_(31), Q_(32), Q_(33), Q_(34), Q_(35), Q_(36), Q_(37), Q_(38), Q_(39), ...
					   Q_(40), Q_(41), Q_(42), Q_(43), Q_(44), Q_(45), Q_(46), Q_(47), Q_(48), Q_(49), Q_(50), Q_(51), Q_(52), ...
					   Q_(53), Q_(54), Q_(55), Q_(56), Q_(57), Q_(58), Q_(59), Q_(60), Q_(61), Q_(62), Q_(63), Q_(64), Q_(65), ...
				       Q_(66), Q_(67), Q_(68), Q_(69), Q_(70), Q_(71), Q_(72), Q_(73), Q_(74), Q_(75), Q_(76), Q_(77), Q_(78)}) ...
			     - W(:, k1 * pnum_per_joint + 1);
    else
        W(:,i) = subs(linkTAU, ...
					  {'m1', 'mc11', 'mc12', 'mc13', 'Io111', 'Io122', 'Io133', 'Io112', 'Io113', 'Io123', 'Ia1', 'fv1', 'fc1', ...
                       'm2', 'mc21', 'mc22', 'mc23', 'Io211', 'Io222', 'Io233', 'Io212', 'Io213', 'Io223', 'Ia2', 'fv2', 'fc2', ...
                       'm3', 'mc31', 'mc32', 'mc33', 'Io311', 'Io322', 'Io333', 'Io312', 'Io313', 'Io323', 'Ia3', 'fv3', 'fc3', ...
                       'm4', 'mc41', 'mc42', 'mc43', 'Io411', 'Io422', 'Io433', 'Io412', 'Io413', 'Io423', 'Ia4', 'fv4', 'fc4', ...
                       'm5', 'mc51', 'mc52', 'mc53', 'Io511', 'Io522', 'Io533', 'Io512', 'Io513', 'Io523', 'Ia5', 'fv5', 'fc5', ...
                       'm6', 'mc61', 'mc62', 'mc63', 'Io611', 'Io622', 'Io633', 'Io612', 'Io613', 'Io623', 'Ia6', 'fv6', 'fc6'}, ...
				      {Q_(1), Q_(2), Q_(3), Q_(4), Q_(5), Q_(6), Q_(7), Q_(8), Q_(9), Q_(10), Q_(11), Q_(12), Q_(13), ...
					   Q_(14), Q_(15), Q_(16), Q_(17), Q_(18), Q_(19), Q_(20), Q_(21), Q_(22), Q_(23), Q_(24), Q_(25), Q_(26), ...
					   Q_(27), Q_(28), Q_(29), Q_(30), Q_(31), Q_(32), Q_(33), Q_(34), Q_(35), Q_(36), Q_(37), Q_(38), Q_(39), ...
					   Q_(40), Q_(41), Q_(42), Q_(43), Q_(44), Q_(45), Q_(46), Q_(47), Q_(48), Q_(49), Q_(50), Q_(51), Q_(52), ...
					   Q_(53), Q_(54), Q_(55), Q_(56), Q_(57), Q_(58), Q_(59), Q_(60), Q_(61), Q_(62), Q_(63), Q_(64), Q_(65), ...
					   Q_(66), Q_(67), Q_(68), Q_(69), Q_(70), Q_(71), Q_(72), Q_(73), Q_(74), Q_(75), Q_(76), Q_(77), Q_(78)});
    end

    disp(['<INFO> Param No.', num2str(i), ' SUBSTITUTED!!']);
end