S-R-S（Spherical-Roll-Spherical）構型7自由度機器人逆運動學

S-R-S即Spherical-Roll-Spherical，意為機器人基座端三個關節與末端三個關節分別相交於一點，可視作球關節，而中間的一個關節是旋轉關節。通常，將基座端的等效得到的球關節稱為肩關節（Shoulder），末端等效球關節稱為腕關節（Wrist），而中間的旋轉關節稱為肘關節（Elbow）。

D-H模型

一種符合SRS構型的7自由度機器人，D-H模型如下：

逆運動學求解

針對S-R-S構型機器人的特點，結合其冗余特性可知，此類機器人在保持末端位置、姿態不變的情況下，其大臂SE、小臂EW形成的平面SEW可在空間中繞直線SW任意旋轉，若規定參考平面為同時通過機器人末端點W及機器人基坐標系z軸所在直線的平面，則大臂、形成的平面SEW與參考平面之間的夾角ψ稱為臂型角。S-R-S構型機器人逆解的求解思路，即首先求解臂形角ψ為0時各關節角度，再求解特定臂形角下的各關節角度，具體過程如下：

• 由下圖，根據腕關節中心位置W、肩關節中心位置S、大臂長度SE、小臂長度SW，在三角形SEW中利用余弦定理求解β，從而進一步得到關節4轉角θ4。需要注意的是，根據β的反余弦函數得到的β僅在0到pi之間，而此值的相反數亦為可行的β值。從而得到兩組可能的θ4。

• 求解臂型角為0的情況下，肘關節處的D-H坐標系相對於基坐標系的姿態變換矩陣R03，記為R03_0，根據坐標系的建立特點（見下圖），R03_0的y軸與向量SE共線且方向相同，可視為向量SW繞垂直於參考平面的向量l旋轉α（向量繞向量轉動的結果由羅德里格斯公式求解）；z軸垂直與SEW平面；而x軸可由z軸與y軸的叉乘得到。由於機器人構型的對稱性，在參考平面內有多種情況的關節角度可實現同一末端位姿（如保持末端位姿下的翻肩、翻肘、翻腕），向量l的朝向、SW繞l的旋轉方向亦有所不同，存在如下的四種情況（若機器人部分關節存在偏置，即機器人構型不完全對稱，則情況相應減少）。

• R03的具體值可由R03_0繞向量SW旋轉臂型角ψ得到；而通過齊次變換矩陣相乘亦可得到以θ1、θ2、θ3的三角函數為變量的R03表達式，將R03的具體值與表達式聯立，則可以求得θ1、θ2、θ3。

• 進一步，可以根據要求的末端位姿矩陣R07、R03及R34（根據已求出的θ4計算得到）R47的具體數值，並采用與上一步驟類似的方式，聯立R47的具體數值與根據齊次變換矩陣計算得到的表達式，求解θ5、θ6、θ7。
• 至此，根據要求的機器人末端位姿及臂型角，可以得到16組關節角的組合，根據關節限位及規避器人兩組相鄰解出現關節角突變，進行取舍求得，從而完成逆解的求解，16組解的組合如下表所示。

θ4-

θ4+

l向內

l向外

l向外

l向內

α-

α+

α-

α+

R03_0、R03

R03_0、R03

R03_0、R03

R03_0、R03

θ2 #1

θ2 #2

θ6 #1

θ6 #2

θ2 #3

θ2 #4

θ6 #3

θ6 #4

θ2 #5

θ2 #6

θ6 #5

θ6 #6

θ2 #7

θ2 #8

θ6 #7

θ6 #8

θ1、θ3

θ1、θ3

θ5、θ7

θ5、θ7

θ1、θ3

θ1、θ3

θ5、θ7

θ5、θ7

θ1、θ3

θ1、θ3

θ5、θ7

θ5、θ7

θ1、θ3

θ1、θ3

θ5、θ7

θ5、θ7

 

θ4	1	1	1	1	1	1	1	1	2	2	2	2	2	2	2	2
θ2	1	2	1	2	3	4	3	4	5	6	5	6	7	8	7	8
θ6	1	1	2	2	3	3	4	4	5	5	6	6	7	7	8	8
θ1	1	2	1	2	3	4	3	4	5	6	5	6	7	8	7	8
θ3	1	2	1	2	3	4	3	4	5	6	5	6	7	8	7	8
θ5	1	1	2	2	3	3	4	4	5	5	6	6	7	7	8	8
θ7	1	1	2	2	3	3	4	4	5	5	6	6	7	7	8	8

逆解MATLB程序

function [joint_target] = inverse_kinematics(pose_target, psi, joint_current)
%% SRS 7-DOF robot inverse kinematics

%% D-H
d3 = 50;
d5 = 50;

offset = [0, pi, pi, pi, pi, pi, pi];

%% theta4
px = pose_target(1, 4);
py = pose_target(2, 4);
pz = pose_target(3, 4);

SE = d3;
EW = d5;
SW = sqrt(px*px + py*py + pz*pz);

beta = acos((SW*SW - SE*SE - EW*EW) / (2*SE*EW));

theta4(1) = pi - beta;
theta4(2) = pi - (-beta);

%% R03_0: R03 when psi= 0
alpha = acos((SE*SE + SW*SW - EW*EW) / (2*SE*SW));

w = [px, py, pz]';
v = [0, 0, 1]';
l = cross(w, v);

I3 = eye(3, 3);
u_l = l / norm(l);
u_l_x = ...
    [     0, -u_l(3),  u_l(2);
      u_l(3),      0, -u_l(1);
     -u_l(2), u_l(1),       0];
 
R_l_alpha(:, :, 1) = I3 + u_l_x * sin(-alpha) + u_l_x * u_l_x * (1 - cos(-alpha));
R_l_alpha(:, :, 2) = I3 + u_l_x * sin(alpha) + u_l_x * u_l_x * (1 - cos(alpha));

y03_0(:, 1) = (R_l_alpha(:, :, 1) * w) / norm(R_l_alpha(:, :, 1) * w);
y03_0(:, 2) = (R_l_alpha(:, :, 2) * w) / norm(R_l_alpha(:, :, 2) * w);

z03_0(:, 1) = -u_l;
z03_0(:, 2) = u_l;

x03_0(:, 1) = cross(y03_0(:, 1), z03_0(:, 1));
x03_0(:, 2) = cross(y03_0(:, 2), z03_0(:, 2));
x03_0(:, 3) = cross(y03_0(:, 1), z03_0(:, 2));
x03_0(:, 4) = cross(y03_0(:, 2), z03_0(:, 1));

R03_0(:, :, 1) = [x03_0(:, 1), y03_0(:, 1), z03_0(:, 1)];
R03_0(:, :, 2) = [x03_0(:, 2), y03_0(:, 2), z03_0(:, 2)];
R03_0(:, :, 3) = [x03_0(:, 3), y03_0(:, 1), z03_0(:, 2)];
R03_0(:, :, 4) = [x03_0(:, 4), y03_0(:, 2), z03_0(:, 1)];

%% R03
u_w = w / norm(w);
u_w_x = ...
    [      0, -u_w(3),  u_w(2);
      u_w(3),       0, -u_w(1);
     -u_w(2),  u_w(1),       0];
 
R_psi = I3 + u_w_x * sin(psi) + u_w_x * u_w_x * (1 - cos(psi));

R03(:, :, 1) = R_psi * R03_0(:, :, 1);
R03(:, :, 2) = R_psi * R03_0(:, :, 2);
R03(:, :, 3) = R_psi * R03_0(:, :, 3);
R03(:, :, 4) = R_psi * R03_0(:, :, 4);
 
%% theta2
theta2(1) = acos(-R03(3, 2, 1));
theta2(2) = -acos(-R03(3, 2, 1));
theta2(3) = acos(-R03(3, 2, 2));
theta2(4) = -acos(-R03(3, 2, 2));
theta2(5) = acos(-R03(3, 2, 3));
theta2(6) = -acos(-R03(3, 2, 3));
theta2(7) = acos(-R03(3, 2, 4));
theta2(8) = -acos(-R03(3, 2, 4));

%% theta1
theta1(1) = atan2(R03(2, 2, 1)*sin(theta2(1)), R03(1, 2, 1)*sin(theta2(1)));
theta1(2) = atan2(R03(2, 2, 1)*sin(theta2(2)), R03(1, 2, 1)*sin(theta2(2)));
theta1(3) = atan2(R03(2, 2, 2)*sin(theta2(3)), R03(1, 2, 2)*sin(theta2(3)));
theta1(4) = atan2(R03(2, 2, 2)*sin(theta2(4)), R03(1, 2, 2)*sin(theta2(4)));
theta1(5) = atan2(R03(2, 2, 3)*sin(theta2(5)), R03(1, 2, 3)*sin(theta2(5)));
theta1(6) = atan2(R03(2, 2, 3)*sin(theta2(6)), R03(1, 2, 3)*sin(theta2(6)));
theta1(7) = atan2(R03(2, 2, 4)*sin(theta2(7)), R03(1, 2, 4)*sin(theta2(7)));
theta1(8) = atan2(R03(2, 2, 4)*sin(theta2(8)), R03(1, 2, 4)*sin(theta2(8)));

%% theta3
theta3(1) = atan2(R03(3, 3, 1)*sin(theta2(1)), R03(3, 1, 1)*sin(theta2(1)));
theta3(2) = atan2(R03(3, 3, 1)*sin(theta2(2)), R03(3, 1, 1)*sin(theta2(2)));
theta3(3) = atan2(R03(3, 3, 2)*sin(theta2(3)), R03(3, 1, 2)*sin(theta2(3)));
theta3(4) = atan2(R03(3, 3, 2)*sin(theta2(4)), R03(3, 1, 2)*sin(theta2(4)));
theta3(5) = atan2(R03(3, 3, 3)*sin(theta2(5)), R03(3, 1, 3)*sin(theta2(5)));
theta3(6) = atan2(R03(3, 3, 3)*sin(theta2(6)), R03(3, 1, 3)*sin(theta2(6)));
theta3(7) = atan2(R03(3, 3, 4)*sin(theta2(7)), R03(3, 1, 4)*sin(theta2(7)));
theta3(8) = atan2(R03(3, 3, 4)*sin(theta2(8)), R03(3, 1, 4)*sin(theta2(8))); 

%% R47
R34(:, :, 1) = [cos(theta4(1)), 0,  sin(theta4(1));
                sin(theta4(1)), 0, -cos(theta4(1));
                             0, 1,               0];
R34(:, :, 2) = [cos(theta4(2)), 0,  sin(theta4(2));
                sin(theta4(2)), 0, -cos(theta4(2));
                             0, 1,               0];
                         
R43(:, :, 1) = R34(:, :, 1)';
R43(:, :, 2) = R34(:, :, 2)';

R30(:, :, 1) = R03(:, :, 1)';
R30(:, :, 2) = R03(:, :, 2)';
R30(:, :, 3) = R03(:, :, 3)';
R30(:, :, 4) = R03(:, :, 4)';

R07 = pose_target(1 : 3, 1 : 3);
R47(:, :, 1) = R43(:, :, 1) * R30(:, :, 1) * R07;
R47(:, :, 2) = R43(:, :, 1) * R30(:, :, 2) * R07;
R47(:, :, 3) = R43(:, :, 2) * R30(:, :, 3) * R07;
R47(:, :, 4) = R43(:, :, 2) * R30(:, :, 4) * R07;

%% theta6
theta6(1) = acos(-R47(3, 3, 1));
theta6(2) = -acos(-R47(3, 3, 1));
theta6(3) = acos(-R47(3, 3, 2));
theta6(4) = -acos(-R47(3, 3, 2));
theta6(5) = acos(-R47(3, 3, 3));
theta6(6) = -acos(-R47(3, 3, 3));
theta6(7) = acos(-R47(3, 3, 4));
theta6(8) = -acos(-R47(3, 3, 4));

%% theta5
theta5(1) = atan2(R47(2, 3, 1)*sin(theta6(1)), R47(1, 3, 1)*sin(theta6(1)));
theta5(2) = atan2(R47(2, 3, 1)*sin(theta6(2)), R47(1, 3, 1)*sin(theta6(2)));
theta5(3) = atan2(R47(2, 3, 2)*sin(theta6(3)), R47(1, 3, 2)*sin(theta6(3)));
theta5(4) = atan2(R47(2, 3, 2)*sin(theta6(4)), R47(1, 3, 2)*sin(theta6(4)));
theta5(5) = atan2(R47(2, 3, 3)*sin(theta6(5)), R47(1, 3, 3)*sin(theta6(5)));
theta5(6) = atan2(R47(2, 3, 3)*sin(theta6(6)), R47(1, 3, 3)*sin(theta6(6)));
theta5(7) = atan2(R47(2, 3, 4)*sin(theta6(7)), R47(1, 3, 4)*sin(theta6(7)));
theta5(8) = atan2(R47(2, 3, 4)*sin(theta6(8)), R47(1, 3, 4)*sin(theta6(8)));

%% theta7
theta7(1) = atan2(-R47(3, 2, 1)*sin(theta6(1)), R47(3, 1, 1)*sin(theta6(1)));
theta7(2) = atan2(-R47(3, 2, 1)*sin(theta6(2)), R47(3, 1, 1)*sin(theta6(2)));
theta7(3) = atan2(-R47(3, 2, 2)*sin(theta6(3)), R47(3, 1, 2)*sin(theta6(3)));
theta7(4) = atan2(-R47(3, 2, 2)*sin(theta6(4)), R47(3, 1, 2)*sin(theta6(4)));
theta7(5) = atan2(-R47(3, 2, 3)*sin(theta6(5)), R47(3, 1, 3)*sin(theta6(5)));
theta7(6) = atan2(-R47(3, 2, 3)*sin(theta6(6)), R47(3, 1, 3)*sin(theta6(6)));
theta7(7) = atan2(-R47(3, 2, 4)*sin(theta6(7)), R47(3, 1, 4)*sin(theta6(7)));
theta7(8) = atan2(-R47(3, 2, 4)*sin(theta6(8)), R47(3, 1, 4)*sin(theta6(8)));

%% select
% theta4
theta4(1) = theta4(1) - offset(4);
theta4(2) = theta4(2) - offset(4);

% theta1
for i = 1 : 8
    theta1(i) = theta1(i) - offset(1);
    if theta1(i) > pi
        theta1(i) =  theta1(i) - 2 * pi;
    elseif theta1(i) < -pi
        theta1(i) = theta1(i) + 2 * pi;
    end
end

% theta2
for i = 1 : 8
    theta2(i) = theta2(i) - offset(2);
    if theta2(i) >= pi
        theta2(i) =  theta2(i) - 2 * pi;
    elseif theta2(i) < -pi
        theta2(i) = theta2(i) + 2 * pi;
    end
end

% theta3
for i = 1 : 8
    theta3(i) = theta3(i) - offset(3);
    if theta3(i) > pi
        theta3(i) =  theta3(i) - 2 * pi;
    elseif theta3(i) < -pi
        theta3(i) = theta3(i) + 2 * pi;
    end
end

% theta5
for i = 1 : 8
    theta5(i) = theta5(i) - offset(5);
    if theta5(i) > pi
        theta5(i) =  theta5(i) - 2 * pi;
    elseif theta5(i) < -pi
        theta5(i) = theta5(i) + 2 * pi;
    end
end

% theta6
for i = 1 : 8
    theta6(i) = theta6(i) - offset(6);
    if theta6(i) > pi
        theta6(i) =  theta6(i) - 2 * pi;
    elseif theta6(i) < -pi
        theta6(i) = theta6(i) + 2 * pi;
    end
end

% theta7
for i = 1 : 8
    theta7(i) = theta7(i) - offset(7);
    if theta7(i) > pi
        theta7(i) =  theta7(i) - 2 * pi;
    elseif theta7(i) < -pi
        theta7(i) = theta7(i) + 2 * pi;
    end
end

joint_target_array = zeros(16, 7);
joint_target_array(1, :) = [theta1(1), theta2(1), theta3(1), theta4(1), theta5(1), theta6(1), theta7(1)];
joint_target_array(2, :) = [theta1(2), theta2(2), theta3(2), theta4(1), theta5(1), theta6(1), theta7(1)];
joint_target_array(3, :) = [theta1(1), theta2(1), theta3(1), theta4(1), theta5(2), theta6(2), theta7(2)];
joint_target_array(4, :) = [theta1(2), theta2(2), theta3(2), theta4(1), theta5(2), theta6(2), theta7(2)];
joint_target_array(5, :) = [theta1(3), theta2(3), theta3(3), theta4(1), theta5(3), theta6(3), theta7(3)];
joint_target_array(6, :) = [theta1(4), theta2(4), theta3(4), theta4(1), theta5(3), theta6(3), theta7(3)];
joint_target_array(7, :) = [theta1(3), theta2(3), theta3(3), theta4(1), theta5(4), theta6(4), theta7(4)];
joint_target_array(8, :) = [theta1(4), theta2(4), theta3(4), theta4(1), theta5(4), theta6(4), theta7(4)];
joint_target_array(9, :) = [theta1(5), theta2(5), theta3(5), theta4(2), theta5(5), theta6(5), theta7(5)];
joint_target_array(10, :) = [theta1(6), theta2(6), theta3(6), theta4(2), theta5(5), theta6(5), theta7(5)];
joint_target_array(11, :) = [theta1(5), theta2(5), theta3(5), theta4(2), theta5(6), theta6(6), theta7(6)];
joint_target_array(12, :) = [theta1(6), theta2(6), theta3(6), theta4(2), theta5(6), theta6(6), theta7(6)];
joint_target_array(13, :) = [theta1(7), theta2(7), theta3(7), theta4(2), theta5(7), theta6(7), theta7(7)];
joint_target_array(14, :) = [theta1(8), theta2(8), theta3(8), theta4(2), theta5(7), theta6(7), theta7(7)];
joint_target_array(15, :) = [theta1(7), theta2(7), theta3(7), theta4(2), theta5(8), theta6(8), theta7(8)];
joint_target_array(16, :) = [theta1(8), theta2(8), theta3(8), theta4(2), theta5(8), theta6(8), theta7(8)];

joint_diff_array = zeros(16, 1);
for i = 1 : 16
    joint_diff_array(i) = sum(abs(joint_target_array(i, :) - joint_current));
end
[~, min_diff_index] = min(joint_diff_array);
joint_target = joint_target_array(min_diff_index, :);

end

奇異性分析

• 當機器人腕關節中心在1軸軸線上時，機器人出現肩部奇異。
• 當機器人肘關節關節角為零時，機器人出現肘關節奇異。
• 當機器人5軸與7軸共線時，機器人出現腕關節奇異。
• 除上述機器人奇異情況，此算法在機器人腕關節中心處於極坐標系z軸上時，出現算法奇異，無法定義參考平面，此時需要將參考平面的定義直線改為x軸。