dh 模型 a矩阵 trans_机器人工程师进阶之路（八）指数积（PoE）建立机械臂模型及正运动…

梁政：机器人工程师进阶之路（六）旋量法（上）zhuanlan.zhihu.com

梁政：机器人工程师进阶之路（七）旋量法（下）zhuanlan.zhihu.com

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

$equation?tex=%5Cmathcal%7BS%7D%3D%28%5Comega%2Cv%29$
。通过螺旋轴可以将坐标系变换矩阵T矩阵

$equation?tex=T%3D%5Cleft%5B%5Cbegin%7Barray%7D%7B%7D+++R%26p%5C%5C+++0%261%5C%5C+%5Cend%7Barray%7D%5Cright%5D%5Cin+SE%283%29%5C%5C$

转化为指数形式

$equation?tex=T%3De%5E%7B%5B%5Cmathcal%7BS%7D%5D%5Ctheta%7D%3D%5Cleft%5B%5Cbegin%7Barray%7D%7B%7D+++e%5E%7B%5B%5Comega%5D%5Ctheta%7D%26%2A%5C%5C+++0%261%5C%5C+%5Cend%7Barray%7D%5Cright%5D%5C%5C$

其中

$equation?tex=%5BS%5D%5Ctheta%5Cin+se%283%29%2C%2A%3D%28I%2B%281-cos%5Ctheta%29%5B%5Chat%7B%5Comega%7D%5D%2B%28%5Ctheta-sin%7B%5Ctheta%7D%29%5B%5Chat%7B%5Comega%7D%5D%5E2%29v$
。

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

$equation?tex=%5Cmathcal%7BS%7D%3D%5BAd_%7BT_%7Bsb%7D%7D%5D%5Cmathcal%7BB%7D%5C%5C$

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

Gluon模型

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

$equation?tex=M%3D%5Cleft%5B%5Cbegin%7Barray%7D%7B%7D+++-1%260%260%26L_1%2BL_2%5C%5C+++0%260%261%26W_1-W_2%2BW_3%2BW_4%5C%5C0%261%260%26H_1-H_2%5C%5C0%260%260%261%5C%5C+%5Cend%7Barray%7D%5Cright%5D%5C%5C$

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

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

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

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

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

在基坐标系上的螺旋轴

确定螺旋轴。我们的机械臂都是旋转副，那么各自必然有一个旋转轴

$equation?tex=%5Comega_i$
。同时相对于坐标原点会产生一个线速度方向

$equation?tex=v_i$
。合起来将得到螺旋轴

$equation?tex=%5Cmathcal%7BS%7D_i%3D%28%5Comega_i%2Cv_i%29+$
。例如，第4关节如下图。

我们可设第4关节得旋量为

$equation?tex=%5Cleft%5C%7B+q_4%2C%5Comega_4%2C0+%5Cright%5C%7D$
，

$equation?tex=%5Comega_4%3D%280%2C1%2C0%29%5ET$
，

$equation?tex=q_4%3D%28L_1%2BL_2%2CW_1-W_2%2CH_1%29%5ET$
那么螺旋轴可被计算为

$equation?tex=%5Cmathcal%7BS%7D_4%3D%5Cleft%5B%5Cbegin%7Barray%7D%7B%7D+++%5Chat%7B%5Comega%7D_4%5C%5C++-+%5Chat%7B%5Comega%7D_4%5Ctimes+q_4%5C%5C+%5Cend%7Barray%7D%5Cright%5D%3D%5Cleft%28%5Cbegin%7Barray%7D%7B%7D+++0%5C%5C1%5C%5C0+%5C%5C+-H_1%5C%5C0%5C%5CL_1%2BL_2%5C%5C+%5Cend%7Barray%7D%5Cright%29%5C%5C$

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

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

$equation?tex=T%28%5Ctheta%29%3De%5E%7B%5B%5Cmathcal%7BS_1%7D%5D%5Ctheta_1%7De%5E%7B%5B%5Cmathcal%7BS_2%7D%5D%5Ctheta_2%7De%5E%7B%5B%5Cmathcal%7BS_3%7D%5D%5Ctheta_3%7De%5E%7B%5B%5Cmathcal%7BS_4%7D%5D%5Ctheta_4%7De%5E%7B%5B%5Cmathcal%7BS_5%7D%5D%5Ctheta_5%7De%5E%7B%5B%5Cmathcal%7BS_6%7D%5D%5Ctheta_6%7DM%5C%5C$

在末端坐标系下的螺旋轴

同样，先找到螺旋轴

$equation?tex=%5Cmathcal%7BB%7D_i%3D%28%5Comega_i%2Cv_i%29+$
，只不过这次是相对于末端坐标系的。同样是第4关节，

$equation?tex=%5Comega_4%3D%280%2C0%2C1%29%5ET$
，

$equation?tex=q_4%3D%280%2CH_2%2C-W_1-W_2%29%5ET$
，螺旋轴可被计算为

$equation?tex=%5Cmathcal%7BB%7D_4%3D%5Cleft%5B%5Cbegin%7Barray%7D%7B%7D+++%5Chat%7B%5Comega%7D_4%5C%5C++-+%5Chat%7B%5Comega%7D_4%5Ctimes+q_4%5C%5C+%5Cend%7Barray%7D%5Cright%5D%3D%5Cleft%28%5Cbegin%7Barray%7D%7B%7D+++0%5C%5C0%5C%5C1+%5C%5C+H_2%5C%5C0%5C%5C0%5C%5C+%5Cend%7Barray%7D%5Cright%29%5C%5C$

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

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

$equation?tex=T%28%5Ctheta%29%3DMe%5E%7B%5B%5Cmathcal%7BB_1%7D%5D%5Ctheta_1%7De%5E%7B%5B%5Cmathcal%7BB_2%7D%5D%5Ctheta_2%7De%5E%7B%5B%5Cmathcal%7BB_3%7D%5D%5Ctheta_3%7De%5E%7B%5B%5Cmathcal%7BB_4%7D%5D%5Ctheta_4%7De%5E%7B%5B%5Cmathcal%7BB_5%7D%5D%5Ctheta_5%7De%5E%7B%5B%5Cmathcal%7BB_6%7D%5D%5Ctheta_6%7D%5C%5C$

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

$equation?tex=T_%7Bsb%7D%3DM$
，那么

$equation?tex=%5Cmathcal%7BS%7D%3D%5BAd_%7BT_%7Bsb%7D%7D%5D%5Cmathcal%7BB%7D%5Crightarrow+%5Cmathcal%7BB%7D%3D%5BAd_%7BM%5E%7B-1%7D%7D%5D%5Cmathcal%7BS%7D%5C%5C+%5B%5Cmathcal%7BB%7D%5D%3DM%5E%7B-1%7D%5B%5Cmathcal%7BS%7D%5DM%5C%5C$

根据矩阵指数性质，

$equation?tex=Me%5E%7BM%5E%7B-1%7DPM%7D%3De%5EPM$
，变换矩阵可改写为

$equation?tex=%5Cbegin%7Balign%7DT%28%5Ctheta%29%26%3De%5E%7B%5B%5Cmathcal%7BS_1%7D%5D%5Ctheta_1%7D...e%5E%7B%5B%5Cmathcal%7BS_6%7D%5D%5Ctheta_6%7DM%5C%5C%26%3De%5E%7B%5B%5Cmathcal%7BS_1%7D%5D%5Ctheta_1%7D...Me%5E%7BM%5E%7B-1%7D%5B%5Cmathcal%7BS_6%7D%5DM%5Ctheta_6%7D%5C%5C%26%3DMe%5E%7BM%5E%7B-1%7D%5B%5Cmathcal%7BS_1%7D%5DM%5Ctheta_1%7D...e%5E%7BM%5E%7B-1%7D%5B%5Cmathcal%7BS_6%7D%5DM%5Ctheta_6%7D%5C%5C%26%3DMe%5E%7B%5B%5Cmathcal%7BB_1%7D%5D%5Ctheta_1%7D...e%5E%7B%5B%5Cmathcal%7BB_6%7D%5D%5Ctheta_6%7D%5Cend%7Balign%7D+%5C%5C$

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

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

$equation?tex=%5Ctheta%3D%5B-90%2C-90%2C0%2C-90%2C0%2C0%5D$
，结果为

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

梁政：机器人工程师进阶之路（三）6轴机械臂DH法下的变换关系zhuanlan.zhihu.com

原文链接：https://blog.csdn.net/weixin_34079140/article/details/113579256

Gluon模型

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

Matlab正运算仿真

你可能也喜欢