Sympy.solve无法消去中间变量的机器人臂运动学求解问题

背景 #

我正在搭建DIY机械臂，需要根据工作区域和负载设计连杆长度、关节配置及电机规格，因此开发Python参数化程序，实现三个核心功能：

• 绘制机械臂可达区域
• 求解末端执行器位置对电机角度的灵敏度
• 生成正逆运动学变换矩阵

已经用Sympy实现了两关节系统的正运动学求解及可达区域绘制，但在构建通用求解器时遇到问题：当求解包含中间变量（如M2X、M2T等）的方程组时，Sympy.solve无法自动消去这些中间变量得到闭合解，即使使用exclude参数也无效，导致结果仍包含中间变量，进而无法得到灵敏度的具体表达式。

最小复现代码 #

import sympy as sp

# 定义变量
theta1, theta2 = sp.symbols('theta1 theta2')
L1, L2 = sp.symbols('L1 L2')
M2X, M2T = sp.symbols('M2X M2T')
x, y = sp.symbols('x y')

# 含中间变量的方程组
eqs = [
    M2X - L1 * sp.cos(theta1),
    M2T - L1 * sp.sin(theta1),
    x - M2X - L2 * sp.cos(theta1 + theta2),
    y - M2T - L2 * sp.sin(theta1 + theta2)
]

# 尝试求解逆运动学，消去M2X、M2T
solutions = sp.solve(eqs, (theta1, theta2), exclude=[M2X, M2T])
print(solutions)
# 输出仍包含M2X和M2T，未得到仅含x,y,L1,L2的闭合解

具体问题 #

1. 如何让Sympy.solve消去冗余变量得到闭合解？
2. 是否有其他Sympy函数或方法可正确求解此类方程组？

解决方案 #

1. 手动代入消元（最直接可控的方法） #

既然Sympy.solve的exclude参数未按预期工作，可先手动从方程组中解出中间变量的表达式，再代入剩余方程，将问题转化为标准的逆运动学求解：

# 先从前面两个方程解出中间变量的表达式
M2X_expr = sp.solve(eqs[0], M2X)[0]
M2T_expr = sp.solve(eqs[1], M2T)[0]

# 代入后面两个方程，得到仅含目标变量的简化方程组
reduced_eqs = [
    eqs[2].subs({M2X: M2X_expr, M2T: M2T_expr}),
    eqs[3].subs({M2X: M2X_expr, M2T: M2T_expr})
]

# 求解简化后的方程组
solutions = sp.solve(reduced_eqs, (theta1, theta2))
print(solutions)
# 现在得到仅含x,y,L1,L2的闭合解

2. 使用sp.nonlinsolve处理非线性方程组 #

机器人运动学方程组通常是非线性的，Sympy的nonlinsolve函数专门针对这类场景设计，能更智能地处理变量依赖关系，自动消去冗余中间变量：

from sympy import nonlinsolve

# 直接求解所有变量，nonlinsolve会自动处理中间变量的消去
all_solutions = nonlinsolve(eqs, (theta1, theta2, M2X, M2T))
# 提取仅包含theta1、theta2的解
theta_solutions = [sol[:2] for sol in all_solutions]
print(theta_solutions)

3. 先化简再求解 #

如果方程组结构复杂，可先对所有方程进行符号化简，再求解所有变量后过滤出目标变量的解：

# 化简方程组，降低表达式复杂度
simplified_eqs = [sp.simplify(eq) for eq in eqs]
# 求解所有变量
all_solutions = sp.solve(simplified_eqs, (theta1, theta2, M2X, M2T))
# 提取theta1、theta2的解，自动剔除中间变量
theta_solutions = [(sol[theta1], sol[theta2]) for sol in all_solutions]

关键注意事项 #

• 两关节机械臂的逆运动学通常存在两个物理可行解，Sympy的求解函数会返回所有可能解，需根据实际机械约束（如关节角度范围）筛选有效解。
• 确保所有符号变量定义清晰，避免变量名冲突或未定义的情况。

内容的提问来源于stack exchange，提问作者05032 Mendicant Bias