“3/1/3形七轴协作机器人”通常是指具有特定关节配置的七自由度（7-DOF）协作机械臂，其结构可分解为 前3个关节 + 中间1个关节 + 后3个关节 的构型，即所谓的“3–1–3”结构。这种结构在冗余自由度机械臂中较为常见，尤其适用于需要高灵活性、避障能力以及类人手臂运动特性的应用场景。

一、结构特点

• 前3个关节：通常构成一个类肩部结构，负责大范围的位置定位（Positioning），类似于人类肩关节。
• 中间第4个关节：常称为“肘部”或“冗余关节”，用于引入冗余自由度，使末端执行器在达到同一位置时有多种姿态选择。
• 后3个关节：构成类腕部结构，主要控制末端执行器的姿态（Orientation），类似人类手腕。

这种构型使得机器人具有运动学冗余性（Kinematic Redundancy），即对于给定的末端位姿，存在无穷多组关节解。

二、解析逆运动学（IK）求解思路

1、位姿分解

将末端执行器的目标位姿分解为位置和姿态：

• 位置：p ∈ ℝ³
• 姿态旋转矩阵：R ∈ SO(3)

---

2、肘点位置计算（给定冗余参数）

设第4关节（肘关节）的转角为 q₄（作为冗余自由度参数），则肘点在基坐标系中的位置为：

pₑ = ⁰T₃(q₁, q₂, q₃) · d₄

其中：

• ⁰T₃ 是从基座到第3关节的齐次变换矩阵（仅含前3个关节变量），
• d₄ 是沿第4关节轴方向的偏移向量（通常为 [0, 0, l₄]ᵀ，l₄ 为上臂长度）。

但更实用的做法是直接指定肘点位置 pₑ，满足：

‖pₑ − pₛ‖ = L₁（上臂长）
‖p − pₑ‖ = L₂（前臂长）

其中 pₛ 为肩部位置（通常为原点 [0, 0, 0]ᵀ），p 为末端目标位置。

因此，肘点 pₑ 必须位于两个球面的交线上：

• 球1：中心 pₛ，半径 L₁
• 球2：中心 p，半径 L₂

该交线是一个圆，其参数化形式为：

pₑ(φ) = c + r(cosφ · u + sinφ · v)

其中：

• c 是两球心连线上的最近点（圆心），
• r 是交线圆半径，
• u, v 是该圆所在平面的正交单位基向量，
• φ ∈ [0, 2π) 为自运动参数（等效于 q₄）。

✅ 此 φ 即为7自由度系统中冗余自由度的显式参数。

---

3、前段逆解（肩→肘）

已知 pₑ，求 q₁, q₂, q₃：

1. q₁ = atan2(pₑᵧ, pₑₓ)
2. 设 ρ = √(pₑₓ² + pₑᵧ²)，z = pₑ_z
3. 利用余弦定理求 q₃：
   cosq₃ = (ρ² + z² − l₂² − l₃²) / (2l₂l₃*)
   → q₃ = ±acos（肘部“上弯”或“下弯”）
4. q₂ = atan2(z, ρ) − atan2(l₃sinq₃, l₂ + l₃cosq₃)

（假设连杆2、3长度分别为 l₂, l₃）

---

4、后段逆解（肘→末端）

已知末端姿态 R 和肘点 pₑ，计算腕部关节 q₅, q₆, q₇：

1. 计算从肘到末端的期望旋转：
   Rₑ = (⁰R₃ · R₄)ᵀ · R
   其中 ⁰R₃ 由 q₁–q₃ 得出，R₄ 由 q₄ 决定（通常绕某轴旋转）。
2. 若后3轴构成“球形腕”（三轴交于一点），则可直接用 ZYX 或 ZXZ 欧拉角分解 Rₑ 得到 q₅, q₆, q₇。
   例如（假设为 R = R_z(q₅) R_y(q₆) R_z(q₇)）：
   • q₆ = acos(Rₑ(3,3))
   • q₅ = atan2(Rₑ(2,3), Rₑ(1,3))
   • q₇ = atan2(Rₑ(3,2), −Rₑ(3,1))

⚠️ 具体系数取决于DH参数定义，需根据实际机器人调整。

---

5、总结

通过引入肘点位置参数 φ（或等效关节角 q₄），7-DOF 3/1/3 构型机器人可解析求解逆运动学，通解形式为：

(q₁(φ), q₂(φ), q₃(φ), q₄(φ), q₅(φ), q₆(φ), q₇(φ))

其中 φ ∈ [0, 2π) 表征自运动流形，可用于优化避障、关节限位、能耗等目标。

6.matlab代码实现：

function q_sols = ik_313_7dof(p_target, R_target, L1, L2, phi_vec)
% 解析逆运动学求解：3/1/3 构型七自由度协作机器人
%
% 输入：
%   p_target : 3x1 向量，末端位置 [px; py; pz]
%   R_target : 3x3 旋转矩阵，末端姿态
%   L1       : 上臂长度（肩到肘）
%   L2       : 前臂长度（肘到腕中心）
%   phi_vec  : 冗余参数向量（肘平面角度），单位：弧度，如 linspace(0, 2*pi, 10)
%
% 输出：
%   q_sols   : Nx7 矩阵，每行为一组关节解 [q1 q2 q3 q4 q5 q6 q7]

    % 肩部位置（基坐标系原点）
    p_shoulder = [0; 0; 0];
    
    % 末端位置
    p = p_target(:);
    
    % 检查可达性
    d = norm(p - p_shoulder);
    if d > L1 + L2 || d < abs(L1 - L2)
        error('目标位置超出工作空间！');
    end
    
    % 计算肘部圆所在平面
    % 单位向量从肩指向末端
    e = (p - p_shoulder) / d;
    
    % 圆心位置（在肩-末端连线上）
    a = (L1^2 - L2^2 + d^2) / (2 * d);
    c = p_shoulder + a * e;  % 圆心
    
    % 圆半径
    r = sqrt(L1^2 - a^2);
    
    % 构造圆平面正交基 {u, v}
    % 选择一个不与 e 平行的向量（避免奇异）
    if abs(e(3)) < 0.9
        temp = [0; 0; 1];
    else
        temp = [1; 0; 0];
    end
    u = cross(e, temp);
    u = u / norm(u);
    v = cross(e, u);
    v = v / norm(v);
    
    N = length(phi_vec);
    q_sols = zeros(N, 7);
    
    for i = 1:N
        phi = phi_vec(i);
        
        % 肘点位置
        p_elbow = c + r * (cos(phi) * u + sin(phi) * v);
        
        %% 前段逆解：q1, q2, q3（肩 → 肘）
        % q1: 绕Z轴旋转，使X轴对准肘点投影
        q1 = atan2(p_elbow(2), p_elbow(1));
        
        % 在肩局部平面内求解
        px_local = norm(p_elbow(1:2));  % 投影到XY平面的长度
        pz_local = p_elbow(3);
        
        % 余弦定理求 q3（肘关节角，注意定义方式）
        cos_q3 = (px_local^2 + pz_local^2 - L1^2 - L2^2) / (2 * L1 * L2);
        % 夹紧数值误差
        cos_q3 = max(min(cos_q3, 1), -1);
        q3 = acos(cos_q3);  % 默认取“肘下弯”（也可取 -q3 得上弯）
        
        % q2: 肩俯仰角
        alpha = atan2(pz_local, px_local);
        beta = atan2(L2 * sin(q3), L1 + L2 * cos(q3));
        q2 = alpha - beta;
        
        %% 中间关节 q4：肘扭转角
        % 定义：q4 是上臂（肩→肘）坐标系绕其自身Z轴的扭转角，
        % 用于对齐腕部基座方向。
        % 这里我们通过期望的腕部方向反推 q4。
        
        % 构建肩→肘的旋转（仅用 q1, q2, q3）
        R01 = rotz(q1);
        R12 = roty(q2);
        R23 = roty(q3);  % 假设 q3 为绕Y轴（典型Puma构型）
        R03 = R01 * R12 * R23;
        
        % 肘坐标系Z轴（上臂方向）
        z_elbow = R03(:, 3);  % 应与 (p_elbow - p_shoulder) 同向
        
        % 腕中心位置（等于末端位置，因球形腕）
        p_wrist = p;  % 假设工具中心在腕交点
        
        % 从肘到腕的向量
        v_ew = p_wrist - p_elbow;
        v_ew = v_ew / norm(v_ew);
        
        % 腕部期望的Z轴（在基坐标系中）应为 v_ew
        % 但我们需计算：在肘坐标系中，腕Z轴的方向
        v_ew_local = R03' * v_ew;
        
        % q4 定义为绕上臂轴（肘Z轴）的旋转，使X/Y对齐
        % 假设肘关节 q4 绕 Z 轴旋转（标准DH）
        q4 = atan2(v_ew_local(2), v_ew_local(1));
        
        %% 后段逆解：q5, q6, q7（腕部）
        % 构建从基座到肘后（含q4）的旋转
        R34 = rotz(q4);
        R04 = R03 * R34;
        
        % 腕部所需旋转：R_wrist = R04' * R_target
        R_wrist = R04' * R_target;
        
        % 假设腕部为 Rz(q5) * Ry(q6) * Rz(q7) —— 即 ZYZ 欧拉角
        % 提取 ZYZ 欧拉角
        if abs(R_wrist(3,3) - 1) < 1e-6
            % 奇异：q6 ≈ 0
            q6 = 0;
            q5 = 0;
            q7 = atan2(R_wrist(2,1), R_wrist(1,1));
        elseif abs(R_wrist(3,3) + 1) < 1e-6
            % 奇异：q6 ≈ pi
            q6 = pi;
            q5 = 0;
            q7 = atan2(-R_wrist(2,1), -R_wrist(1,1));
        else
            q6 = acos(R_wrist(3,3));
            q5 = atan2(R_wrist(2,3), R_wrist(1,3));
            q7 = atan2(R_wrist(3,2), -R_wrist(3,1));
        end
        
        % 存储解
        q_sols(i, :) = [q1, q2, q3, q4, q5, q6, q7];
    end
end

%% 辅助函数：绕Z轴旋转
function R = rotz(theta)
    R = [cos(theta), -sin(theta), 0;
         sin(theta),  cos(theta), 0;
         0,           0,          1];
end

%% 辅助函数：绕Y轴旋转
function R = roty(theta)
    R = [cos(theta),  0, sin(theta);
         0,           1, 0;
        -sin(theta),  0, cos(theta)];
end