"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₃：
- q₁ = atan2(pₑᵧ , pₑₓ)
- 设 ρ = √(pₑₓ ² + pₑᵧ ²)，z = pₑ_z
- 利用余弦定理求 q₃ ：
cosq₃ = (ρ ² + z ² − l₂ ² − l₃ ²) / (2l₂ l₃*)
→ q₃ = ±acos（肘部"上弯"或"下弯"） - q₂ = atan2(z , ρ ) − atan2(l₃ sinq₃ , l₂ + l₃ cosq₃)
（假设连杆2、3长度分别为 l₂ , l₃）
4、后段逆解（肘→末端）
已知末端姿态 R 和肘点 pₑ ，计算腕部关节 q₅ , q₆ , q₇：
- 计算从肘到末端的期望旋转：
Rₑ = (⁰R₃ · R₄ )ᵀ · R
其中 ⁰R₃ 由 q₁--q₃ 得出，R₄ 由 q₄ 决定（通常绕某轴旋转）。 - 若后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