问题描述 #

我在Drake中实现了如下逆运动学求解程序：

def solveIK(plant: MultibodyPlant, plant_context: Context, target_pose: RigidTransform, frame_name: str, q0=1e-10*np.ones(7)):
    ik = InverseKinematics(plant, plant_context=plant_context, with_joint_limits=True)
    ik.AddPositionConstraint(plant.GetFrameByName(frame_name),
                             np.array([0,0,0]),
                             plant.world_frame(),
                             target_pose.translation(),
                             target_pose.translation()
    )
    ik.AddOrientationConstraint(
        plant.GetFrameByName(frame_name),
        RotationMatrix(RollPitchYaw(0,0,0)),
        plant.world_frame(),
        target_pose.rotation(),
        0.0
    )
    prog = ik.get_mutable_prog()
    q = ik.q()
    if np.abs(q0).sum() > 1e-4:
        prog.AddQuadraticErrorCost(np.eye(7), q0, q)
    prog.SetInitialGuess(q, q0)
    solver = SnoptSolver()
    solver_options = SolverOptions()
    solver_options.SetOption(solver.id(), "Major iterations limit", 2000)
    result = solver.Solve(prog, solver_options=solver_options)
    if not result.is_success():
        print(target_pose)
        print(result.get_solver_details().info)
        raise ValueError("Inverse Kinematics Failed")
    q_res = result.GetSolution()
    return q_res

但这段代码无法稳定求解关节角度：

• 对于末端执行器位姿：位置[ 0.478, -0.022, 0.13 ]、RPY[pi,0,pi]，求解正常，且该位姿处于机器人工作空间内，机器人既不接近奇异点也不触碰关节极限。
• 但将位置调整为[0.5, -0.022, 0.13]（保持姿态不变）时，求解失败，SNOPT报错信息为13 - nonlinear infeasibilities minimized，我不太理解这个错误在逆运动学场景中的含义。

想请教各位，为什么这段代码无法稳定求解IK问题？

问题分析与解决方案 #

先解释下SNOPT报错13 - nonlinear infeasibilities minimized的含义：这个错误表示求解器已经尽力最小化约束的违反程度，但最终还是无法满足所有设定的硬约束——换句话说，它找不到一个能完全符合你设置的位置和姿态要求的关节配置，只能给出一个最接近的结果，但不符合你的成功判定条件。

接下来分析可能的原因和对应的解决办法：

1. 过于刚性的约束设置

你给位置和姿态约束都设置了0公差，这意味着求解器必须找到一个关节配置，让末端执行器的位置和姿态完全匹配目标值。哪怕目标位姿理论上可达，数值求解器也可能因为微小的数值误差、或者搜索路径的问题，无法找到完全满足约束的解。

解决办法：给约束添加一个合理的微小公差，比如：

• 位置约束的上下限设置为target_pose.translation() ± 1e-3（1毫米的误差）
• 姿态约束的角度公差设置为1e-2弧度（约0.57度）
  这样既满足实际应用的精度要求，也给求解器足够的容错空间。修改后的约束代码示例：

# 位置约束添加微小公差
pos_tol = 1e-3
ik.AddPositionConstraint(plant.GetFrameByName(frame_name),
                         np.array([0,0,0]),
                         plant.world_frame(),
                         target_pose.translation() - pos_tol,
                         target_pose.translation() + pos_tol
)
# 姿态约束添加角度公差
rot_tol = 1e-2
ik.AddOrientationConstraint(
    plant.GetFrameByName(frame_name),
    RotationMatrix(RollPitchYaw(0,0,0)),
    plant.world_frame(),
    target_pose.rotation(),
    rot_tol
)

2. 初始猜测的选择不合理

你的默认初始猜测q0是接近0的关节配置，当目标位姿离这个初始配置对应的末端位姿较远时，求解器很容易陷入局部最优解，无法找到正确的关节配置。比如你第二个目标位姿只是x方向移动了2.2cm，但如果初始猜测对应的末端位置离这个目标较远，求解器可能找不到路径。

解决办法：

• 如果是连续求解邻近的目标位姿（比如机器人轨迹规划），把上一次成功求解的关节角度作为下一次的初始猜测，而不是用默认的接近0的值。
• 如果是单次求解，可以先调用Drake的FindIKSolution（一个更鲁棒的快速IK求解器）得到一个可行的初始猜测，再用你的优化式IK进行精调。

3. 约束定义方式的优化

你分别添加了位置和姿态约束，其实可以尝试用AddCartesianPoseConstraint直接合并位姿约束，这个函数的数值实现可能更稳定，尤其是在处理旋转约束时。示例代码：

ik.AddCartesianPoseConstraint(
    plant.GetFrameByName(frame_name),
    RigidTransform(),  # 末端执行器自身坐标系的原点
    plant.world_frame(),
    target_pose,
    pos_tol,  # 位置公差
    rot_tol   # 姿态角度公差
)

4. 求解器参数调整

虽然你设置了最大迭代次数，但SNOPT的其他参数也可能影响求解结果，比如可行性公差。你可以尝试调整求解器的可行性容忍度，或者换用其他求解器（比如Ipopt）试试，不同的求解器在处理非线性优化问题时的表现会有差异。示例：

# 调整SNOPT的可行性公差
solver_options.SetOption(solver.id(), "Feasibility tolerance", 1e-6)
# 或者换用Ipopt求解器
solver = IpoptSolver()

5. 验证目标位姿的可达性

虽然你认为目标位姿在工作空间内，但可以用Drake的可视化工具（比如Meshcat）手动拖动末端执行器到目标位姿，看看机器人是否能到达，或者是否接近关节极限。如果目标位姿确实处于可达区域的边缘，求解器的搜索难度会大大增加，这时候就需要依赖更好的初始猜测或者更宽松的约束。

备注：内容来源于stack exchange，提问作者Art Boyarov