在3/1/3 构型七轴机器人（即：肩部 3 轴 + 肘部 1 轴 + 腕部 3 轴）中，利用几何法求解逆运动学时，肘关节点（Elbow Point）的确定是关键步骤。该点位于上臂与前臂的连接处（通常为第4关节中心），其位置由末端执行器位姿和冗余自由度（如肘部偏移角或肘平面方向）共同决定。
以下介绍一种基于几何投影的肘点计算方法，适用于具有类人臂结构的 7-DOF 机器人。
1. 基本参数定义（可直接复制）
- 腕点位置（Wrist Center）：
p_w = [x_w, y_w, z_w]ᵀ - 肩关节中心（基坐标系原点）：
p_s = [0, 0, 0]ᵀ - 上臂长度（肩 → 肘）：L₁
- 前臂长度（肘 → 腕）：L₂
- 从肩到腕的向量：
r = p_w − p_s = [x_w, y_w, z_w]ᵀ - 肩腕距离：
D = ‖r‖ = √(x_w² + y_w² + z_w²)
注：仅当|L₁ − L₂| ≤ D ≤ L₁ + L₂时，存在可行解。
2. 肘关节点位置p_e = [x_e, y_e, z_e]ᵀ的计算
在三角形“肩–肘–腕”中，利用余弦定理和向量投影确定肘点。
(1) 计算肩部三角形内角 α（在肩点处）
α=arccos(2⋅L1⋅DL12+D2−L22)
α = arccos( (L₁² + D² − L₂²) / (2·L₁·D) )
(2) 定义肘部偏置方向（冗余自由度）
引入肘平面法向量或肘参考方向。常用方法是定义一个肘指向参考点p_ref（如人体肘自然下垂方向），构造肘偏移平面。
更通用的做法：定义单位向量k为肘偏置方向（通常取世界Z轴或用户指定方向），则肘点位于由r和k张成的平面内，并垂直于该平面的法向。
但最简几何法假设肘点位于肩–腕连线与肘偏置方向所确定的平面内。若忽略偏置（即肘在肩腕矢量平面内），则肘点可直接通过旋转向量获得。
(3) 构造局部坐标系并旋转
令：
- e_r = r / D（肩→腕单位向量）
- 选择一个不与e_r平行的参考向量（如[0, 0, 1]ᵀ），计算平面法向：
n = e_r × [0, 0, 1]ᵀ
若‖n‖ ≈ 0（即 r 沿 Z 轴），改用[1, 0, 0]ᵀ - 单位法向：e_n = n / ‖n‖
- 平面内垂直于e_r的单位向量：e_⊥ = e_n × e_r
- 此e_⊥方向即为肘点偏离肩腕线的方向（对应冗余自由度）。
引入肘角参数ψ（Elbow Angle 或 Elbow Bias Angle），用于控制肘点在圆周上的位置（ψ = 0 表示“外展”，ψ = π 表示“内收”）。
则肘点位置为：
p_e = L₁·cos(α)·e_r + L₁·sin(α)·(cos(ψ)·e_⊥ + sin(ψ)·e_n)
但对多数 3/1/3 机器人，ψ 固定为 0 或 π（因肘关节为单轴，只能在一个平面内弯曲）。此时简化为：
p_e = L₁·cos(α)·e_r ± L₁·sin(α)·e_⊥
其中 “+” 对应一种肘构型（如右肘向外），“−” 对应另一种（左肘向内）。
3. 计算公式汇总：
设：
D = √(x_w² + y_w² + z_w²)
e_r = [x_w/D, y_w/D, z_w/D]ᵀ
选择参考轴（如 Z 轴k = [0, 0, 1]ᵀ），计算：
n = e_r × k = [y_w/D, −x_w/D, 0]ᵀ
n_norm = √((y_w/D)² + (x_w/D)²) = √(x_w² + y_w²)/D
若n_norm > ε（例如 ε = 1e−6），则：
e_⊥ = [y_w/√(x_w² + y_w²), −x_w/√(x_w² + y_w²), 0]ᵀ
否则（腕点在 Z 轴上）：
e_⊥ = [1, 0, 0]ᵀ
计算角度：
α = arccos( (L₁² + D² − L₂²) / (2·L₁·D) )
则肘关节点位置为：
p_e = L₁·cos(α)·e_r ± L₁·sin(α)·e_⊥
展开分量形式（以非奇异情况为例）：
x_e = L₁·cos(α)·(x_w/D) ± L₁·sin(α)·(y_w / √(x_w² + y_w²))
y_e = L₁·cos(α)·(y_w/D) ∓ L₁·sin(α)·(x_w / √(x_w² + y_w²))
z_e = L₁·cos(α)·(z_w/D)
注意：± 符号对应两种肘部构型（Elbow-Up / Elbow-Down）。
使用说明
- L₁、L₂：机器人连杆参数（已知）
- p_w：由末端目标位姿减去工具偏移得到
- ψ（或 ±）：由用户指定肘部偏好（冗余解选择）
- 所有符号（θ、α、π、±、√、·、ᵀ 等）均为标准 Unicode，粘贴到 Word 后可直接显示，无需额外设置。
如需进一步求解各关节角（θ₁～θ₇），可在获得p_e后分别解肩部（θ₁, θ₂, θ₃）和肘部（θ₄）角度。
_python版本)
