串联机器人动力学线性化过程

引言
机器人的动力学辨识中，最重要的一步就是将原本的非线性方程：
t = Dynamics(q,dq,ddq,x)
整理为的线性形式。其中t为N为的向量，x为机器人的惯性参数。我们需要将其重新写为：
t = W(q,dq,ddq)*x 的形式，该方程的A是一个关于位置、速度、加速度的回归矩阵，通过最小二乘法可以辨识得到向量形式的x。
需要注意的是，这里的A是[q,dq,ddq]的函数，这里可以在写一层：
W = W[w(q,dq,ddq),alpha(q,dq,ddq),acc(q,dq,ddq)]
因此从关节运动学计算到连杆运动学是必要的。

运动学外推
运动学外推是为了计算机器人各个连杆的角速度、角加速度和线加速度，当我们得到这些参数后就可以通过该量得到连杆受到的惯性力，因此该步骤是连接运动学参数和惯性参数的桥梁：
具体如下：
对于旋转关节：
角速度： w(i) = R(i_i-1) * w(i-1) + Z * dq(i)
角角速度：alpha(i) = R(i_i-1) * alpha(i-1) + R(i_i-1) * w(i-1) × Z * dq(i) + Z * ddq(i)
线加速度： acc(i) = R(i_i-1) * {acc(i-1) + alpha(i-1) × P(i-1_i) + w(i-1) × [w(i-1) × P(i-1,i)]}
对于平移关节：
角速度： w(i) = R(i_i-1) * w(i-1)
角角速度：alpha(i) = R(i_i-1) * alpha(i-1)
线加速度： acc(i) = R(i_i-1) * {acc(i-1) + alpha(i-1) × P(i-1_i) + w(i-1) × [w(i-1) × P(i-1,i)]} + 2 * w(i) × (Z * dq(i)) + Z * ddq(i)
其中R(i,j)表示{i}坐标系下{j}系的姿态；P(i,j)表示{i}系下{j}的位置；
运动学外推从{0}系开始，其中{0}系的线加速度可以赋予[0 0 -g];
以上的W(i),alpha(i),acc(i)均表示在局部坐标系{i}下描述的，如果需要计算{0}下的值需要左乘矩阵R(0_i)

A矩阵的计算
在获得了以上的运动学信息后可以组合成各个连杆的A矩阵，其中A矩阵描述的是连杆质心在角加速度和线加速度下的对应惯性力的表达式；
A是一个6行10列的矩阵：
A{i} = [acc(i) S(alpha(i)) + S(w(i)) * S(w(i)) zeros(3,6); zeros(3,1) -S(acc(i)) K(alpha(i)) + S(w(i)) * K(w(i))]
其中S算子为反对称矩阵，K算子在按列排列的惯性参数顺序为[Ixx Iyy Izz Ixy Iyz Ixz]时为：

K(a) = [a(1)   0    0    a(2)   0      a(3)
        0     a(2)  0    a(1)   a(3)    0
        0      0   a(3)   0     a(2)   a(1)]; 

回归矩阵W的计算
回归矩阵W是一个N×10N大小的矩阵，其中每一个元素都是一个10行一列的向量，现在将W(i,j)视作第i行第j列的向量那么有：

W(i,j) = adT(i,j) * A(j) if i <= j;
W(i,j) = zeros(10, 1),   if i > j;

其中adT(i,j)表示{i}对于{j}的伴随矩阵其具体形式为：

adT(i,j) = [
  R(i_j)          Zeros(3,3)
  P(i_j)×R(i_j)    R(i_j)
  ]

至此得到了整个线性化后的动力学表达式：
t = W(q,dq,ddq)*x

需要注意的是以上获取的形式虽然理论可以通过最小二乘法进行辨识，但是工程上却有着W矩阵欠定的问题，通常而言该矩阵是不满秩的，也就是说机器人的所有10N个惯性参数是无法全部辨识的，因此我们需要一个方法来获取最小惯性参数集合。
该问题似乎可以考虑成一般的线性优化问题的处理方法？？