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

> 原文链接：https://robotics.hellobike.com/pitfalls/2016-11-08-robotics-se-11019
> 频道：pitfalls | 更新日期：2016-11-08 | 标签：auto, stackexchange, inverse-kinematics, matlab, manipulator
> 核心摘要：用雅可比伪逆做 IK 时想约束 TCP 姿态；采纳答案建议按路径点求解并固定姿态，或分三段迭代。

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

::: v-pre
> 本文由 AI 根据公开内容整理，可能有误，请以原文为准：[Inverse kinematics: how to specify TCP constraints in an iterative algorithm?](https://robotics.stackexchange.com/questions/11019/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 授权。
:::

## 原始参考来源 (Sources)

- [Robotics Stack Exchange #11019：Inverse kinematics: how to specify TCP constraints in an iterative algorithm?](https://robotics.stackexchange.com/questions/11019/inverse-kinematics-how-to-specify-tcp-constraints-in-an-iterative-algorithm)