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向内

α-

α+

α-

α+

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

θ5、θ7

θ1、θ3

θ5、θ7

θ1、θ3

θ5、θ7

θ1、θ3

θ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