从上两篇我们可以了解到，利用旋量法可以在螺旋运动中获得螺旋轴（screw axis） [公式] 。

通过螺旋轴可以将坐标系变换矩阵T矩阵

[公式]

转化为指数形式

[公式]

其中 [公式] 。

另外，和T矩阵的左乘右乘一样，要区分当前坐标系是相对于固定坐标系{S}变换还是自身坐标系{B}变换。而且两者之间的螺旋轴的变换关系为

[公式]

根据这些，我试着给Innfos-Gluon建个模。之前的DH法，按着自己的想法去建立模型，到后面的计算变得很复杂。所以还是太年轻，这次我就不给自己添加难度了，模仿大佬们的模型建立吧。

Gluon模型

我们设定基坐标系为固定坐标系{s}，末端坐标系{b}。确定好各关节轴线，以及坐标系原点为轴线相交点，类似于DH法。并在{s}下，确定末端位姿T矩阵为M。

[公式]

指数积公式（Product of Exponentials(PoE) Formula）

我们分两个方向进行建模。

1. 螺旋轴在基坐标系中，也就是固定坐标系{s}；

2. 螺旋轴在末端坐标系中，也就是自身坐标系{b}；

在T矩阵时，固定坐标系就是左乘，自身坐标系就是右乘。把指数积等价于T矩阵，所以也一样。

  • 在基坐标系上的螺旋轴

[公式]

2. 将所有螺旋轴记录下，并列表。

3. 左乘各T矩阵的指数形式，得到PoE公式。

[公式]

  • 在末端坐标系下的螺旋轴
  1. 同样，先找到螺旋轴 [公式] ，只不过这次是相对于末端坐标系的。同样是第4关节，

[公式]

2. 将所有螺旋轴记录下，并列表。

3. 左乘各T矩阵的指数形式，得到PoE公式。

[公式]

4. 若相对于末端坐标系，那么我们可以通过螺旋轴变换关系来求PoE公式。已知末端相对于固定坐标系的变换矩阵为 [公式] ，那么

[公式]

根据矩阵指数性质， [公式] ，变换矩阵可改写为

[公式]

Matlab正运算仿真

H1=105.03;W1=80.09;L1=174.42;W2=84.51;L2=174.42;W3=80.09;H2=80.09;W4=44.36;
M=[-1 0 0 L1+L2;0 0 1 W1-W2+W3+W4;0 1 0 H1-H2;0 0 0 1]  % Home Configuration of End-Effector 
%% Spatial Twist
Slist = [[0; 0; 1; 0; 0; 0], ...
        [0; 1; 0; -H1; 0; 0],...
        [0; 1; 0; -H1; 0; L1],...
        [0; 1; 0; -H1; 0; L1+L2],...
        [0; 0; -1; -W1+W2-W3; L1+L2;0],...
        [0; 1; 0; H2-H1; 0; L1+L2]];   %The joint screw axes in the space frame
thetalist = [pi/6; pi/6; pi/6; pi/6; pi/6; pi/6];

Ts=FKinSpace(M,Slist,thetalist)
%% Body Twist
Blist = [[0; 1; 0; W1-W2+W3+W4; 0; L1+L2], ...
        [0; 0; 1; H2; -L1-L2; 0],...
        [0; 0; 1; H2; -L2; 0],...
        [0 ;0; 1; H2; 0; 0],...
        [0; -1; 0; -W4; 0;0],...
        [0; 0; 1; 0; 0; 0]];   %The joint screw axes in the end-effector frame
thetalist = [pi/6; pi/6; pi/6; pi/6; pi/6; pi/6];

Tb=FKinBody(M,Blist,thetalist)

%%
function T = FKinSpace(M, Slist, thetalist)
T = M;
for i = size(thetalist): -1: 1
    T = MatrixExp6(VecTose3(Slist(:, i) * thetalist(i))) * T;
end
end
%%
function T = FKinBody(M, Blist, thetalist)
T = M;
for i = 1: size(thetalist)
    T = T * MatrixExp6(VecTose3(Blist(:, i) * thetalist(i)));
end
end
%%
function T = MatrixExp6(se3mat)
omgtheta = so3ToVec(se3mat(1: 3, 1: 3));
if NearZero(norm(omgtheta))
    T = [eye(3), se3mat(1: 3, 4); 0, 0, 0, 1];
else
    [omghat, theta] = AxisAng3(omgtheta);
    omgmat = se3mat(1: 3, 1: 3) / theta; 
    T = [MatrixExp3(se3mat(1: 3, 1: 3)), ...
         (eye(3) * theta + (1 - cos(theta)) * omgmat ...
          + (theta - sin(theta)) * omgmat * omgmat) ...
            * se3mat(1: 3, 4) / theta;
         0, 0, 0, 1];
end
end
%%
function se3mat = VecTose3(V)
se3mat = [VecToso3(V(1: 3)), V(4: 6); 0, 0, 0, 0];
end
%%
function so3mat = VecToso3(omg)
so3mat = [0, -omg(3), omg(2); omg(3), 0, -omg(1); -omg(2), omg(1), 0];
end
%%
function omg = so3ToVec(so3mat)
omg = [so3mat(3, 2); so3mat(1, 3); so3mat(2, 1)];
end
%%
function judge = NearZero(near)
judge = norm(near) < 1e-6;
end
%%
function [omghat, theta] = AxisAng3(expc3)
theta = norm(expc3);
omghat = expc3 / theta;
end
%%
function  R = MatrixExp3(so3mat)
omgtheta = so3ToVec(so3mat);
if NearZero(norm(omgtheta))
    R = eye(3);
else
    [omghat, theta] = AxisAng3(omgtheta);
    omgmat = so3mat / theta;
    R = eye(3) + sin(theta) * omgmat + (1 - cos(theta)) * omgmat * omgmat;
end
end

从结果看出，两种建模方式是一样的。若将角度设为 [公式] ，结果为

与之前DH法中直立的初始位置是相同的，由于末端坐标轴的x，z轴相反，所以姿态矩阵不一样。