2DOF平面机器人逆运动学Python实现错误排查及通用改造问询

一、逆运动学失效排查步骤 #

1. 正运动学基准校验 #

• 先确认正运动学公式与逆解推导的一致性：
  比如2DOF正解公式为：

  x = l1 * np.cos(theta1) + l2 * np.cos(theta1 + theta2)
  y = l1 * np.sin(theta1) + l2 * np.sin(theta1 + theta2)
  

  逆解必须基于同一坐标系（原点位置、角度正负方向）推导，若逆解中误将theta1+theta2写为theta1-theta2，必然导致末端位置偏差。
• 验证正解正确性：代入已知关节角，计算末端坐标后手动绘图核对，排除正解本身的错误。

2. 逆解公式细节检查 #

• 余弦定理项：cos(theta2) = (x² + y² - l1² - l2²)/(2*l1*l2)，需确认分母不为0，且计算时用浮点运算避免整数除法。
• 角度象限处理：必须用np.arctan2()代替np.arctan()计算theta1，避免象限判断错误。正确公式示例：

  theta2 = np.arccos((x**2 + y**2 - l1**2 - l2**2)/(2*l1*l2))
  theta1 = np.arctan2(y, x) - np.arctan2(l2*np.sin(theta2), l1 + l2*np.cos(theta2))
  

• 双解处理：2DOF逆解有肘上/肘下两种情况，若只取其中一种，可能因目标位置在另一种解的可达域内导致偏差。

3. 关节约束与数据传递检查 #

• 检查Joint结构的角度限位：若逆解算出的关节角超出min_angle/max_angle，直接截断会导致末端位置偏移，需先判断解的有效性再做约束处理。
• 核对参数传递：确保逆解函数调用的是robot.links中正确的连杆长度、关节初始角度，避免变量名混淆（比如把link1.length写成link2.length）。

4. 可视化坐标系一致性校验 #

• TKinter/Matplotlib默认坐标系为左上角原点，y轴向下，而运动学计算通常用左下角原点、y轴向上。若未做坐标转换，会出现“数值正确但视觉上未到位”的假象，需在绘图时对y坐标取反：

  # 绘图时转换坐标
  plt.scatter(target_x, -target_y, color='red')
  

二、适配任意Robot结构的逆运动学函数改造 #

针对多自由度机器人，放弃硬编码的2DOF几何解，改用基于雅克比矩阵的数值迭代法实现通用逆解：

1. 核心代码实现 #

import numpy as np

def inverse_kinematics(robot, target_pos, target_rot=None, max_iter=1000, tol=1e-6):
    # 初始化关节角（用当前关节角或正解初始值）
    theta = np.array([joint.angle for joint in robot.joints], dtype=np.float64)
    
    for _ in range(max_iter):
        # 正运动学计算当前末端位姿
        current_pos, current_rot = robot.forward_kinematics(theta)
        
        # 计算位姿误差
        pos_error = target_pos - current_pos
        if target_rot is not None:
            # 平面机器人仅需单轴旋转误差
            rot_error = np.array([target_rot - current_rot])
            error = np.concatenate([pos_error, rot_error])
        else:
            error = pos_error
        
        # 计算雅克比矩阵
        J = robot.compute_jacobian(theta)
        
        # 伪逆求解关节角增量，避免奇异值问题
        delta_theta = np.linalg.pinv(J) @ error
        
        # 更新关节角
        theta += delta_theta
        
        # 误差达标则退出迭代
        if np.linalg.norm(error) < tol:
            break
    
    # 应用关节角度约束
    theta = robot.apply_joint_limits(theta)
    return theta

2. 配套辅助函数实现 #

（1）正运动学（基于DH参数）

在Robot类中实现通用正解，通过DH矩阵连乘计算末端位姿：

def forward_kinematics(self, theta):
    T = np.eye(4)  # 基坐标系到末端的齐次变换矩阵
    for i, link in enumerate(self.links):
        # 读取DH参数：a(连杆长度), alpha(连杆扭转角), d(关节偏移), theta(关节角)
        a = link.a
        alpha = link.alpha
        d = link.d
        t = theta[i]
        
        # DH变换矩阵
        T_i = np.array([
            [np.cos(t), -np.sin(t)*np.cos(alpha), np.sin(t)*np.sin(alpha), a*np.cos(t)],
            [np.sin(t), np.cos(t)*np.cos(alpha), -np.cos(t)*np.sin(alpha), a*np.sin(t)],
            [0, np.sin(alpha), np.cos(alpha), d],
            [0, 0, 0, 1]
        ])
        T = T @ T_i
    
    # 提取平面位置和旋转角
    pos = T[:2, 3]
    rot = np.arctan2(T[1, 0], T[0, 0])
    return pos, rot

（2）雅克比矩阵计算

遍历每个关节，计算线速度/角速度对关节角的偏导数：

def compute_jacobian(self, theta):
    n_dof = len(self.joints)
    J = np.zeros((3, n_dof))  # 平面机器人：2维线速度+1维角速度
    T_prev = np.eye(4)
    
    for i in range(n_dof):
        link = self.links[i]
        a = link.a
        alpha = link.alpha
        d = link.d
        t = theta[i]
        
        # 当前关节的DH变换矩阵
        T_i = np.array([
            [np.cos(t), -np.sin(t)*np.cos(alpha), np.sin(t)*np.sin(alpha), a*np.cos(t)],
            [np.sin(t), np.cos(t)*np.cos(alpha), -np.cos(t)*np.sin(alpha), a*np.sin(t)],
            [0, np.sin(alpha), np.cos(alpha), d],
            [0, 0, 0, 1]
        ])
        T_current = T_prev @ T_i
        
        # 关节z轴方向（平面机器人z轴垂直纸面向外，取[0,0,1]）
        z_i = T_prev[:3, 2]
        # 末端相对于当前关节的位置向量
        p_n = T_current[:3, 3]
        p_i = T_prev[:3, 3]
        r = p_n - p_i
        
        # 转动关节的雅克比列向量
        J[:2, i] = np.cross(z_i, r)[:2]  # 线速度分量
        J[2, i] = z_i[2]  # 角速度分量
        
        T_prev = T_current
    
    return J

（3）关节约束应用

def apply_joint_limits(self, theta):
    for i, joint in enumerate(self.joints):
        theta[i] = np.clip(theta[i], joint.min_angle, joint.max_angle)
    return theta

3. 通用方案优势 #

• 适配任意自由度的平面/空间机器人，只需在Link结构中定义正确的DH参数即可。
• 支持冗余自由度机器人，可通过添加正则项（如delta_theta = np.linalg.pinv(J) @ (error + 0.1*(self.joint_initial - theta))）偏向初始关节角的解，避免奇异位姿。

内容的提问来源于stack exchange，提问作者Blue7