踩坑/Robotics Stack Exchange·2016-11-08

迭代 IK 如何保持 TCP 姿态恒定？

用雅可比伪逆做 IK 时想约束 TCP 姿态；采纳答案建议按路径点求解并固定姿态，或分三段迭代。

AI 整理

• 本体 SDK 与控制

本文由 AI 根据公开内容整理，可能有误，请以原文为准：Inverse kinematics: how to specify TCP constraints in an iterative algorithm?（Robotics Stack Exchange #11019）

状态：已解决

问题

提问者用 q_vel = pinv(J)*error(:,k) 迭代求 6 DoF 机器人 IK，想在从初始位姿到目标位姿运动时保持末端姿态恒定并避障，但不知如何实现零空间约束中的 w、q_0。

环境

Linux / ROS 机器人软件与控制通用开发环境

解决办法

1. 若要保持姿态恒定，沿直线或避障所需路径离散出多个 TCP 位置点，并把每个点的末端姿态设为同一个常量，逐点求 IK 后顺序发送关节坐标。
2. 若继续用原迭代法，先把当前位姿调整到所需恒定姿态：计算 postition_current_with_const_orientation - pose_current，迭代到 RMS 误差低于阈值。
3. 再在恒定姿态下移动到目标位置：计算 postition_target_with_const_orientation - pose_current，迭代到 RMS 误差低于阈值。
4. 最后再把 TCP 姿态调整到目标位姿：计算 pose_target - pose_current，迭代到 RMS 误差低于阈值。

原问题作者：HighVoltage；采纳答案作者：50k4。原内容依 CC BY-SA 4.0 授权，本页摘要同样依 CC BY-SA 4.0 授权。

来源

本页事实对应的公开页面，以原文为准。

1. Robotics Stack Exchange #11019：Inverse kinematics: how to specify TCP constraints in an iterative algorithm?访问 2026-10-22