[论文解读] TORM: Fast and Accurate Trajectory Optimization of Redundant Manipulator given an End-Effector Path

Mincheul Kang, Heechan Shin+2 more|arXiv (Cornell University)|Sep 27, 2019

Robotic Mechanisms and Dynamics参考文献 31被引用 4

一句话总结

TORM 提出了一种两阶段梯度下降优化框架，将基于雅可比矩阵的逆运动学与运动规划相结合，沿预设末端执行器路径生成平滑、无碰撞且高度精确的冗余机械臂轨迹。通过解耦约束满足与路径跟踪目标，并采用迭代探索策略，TORM 在平均和最小位姿误差方面均优于当前最先进方法，具备稳健的收敛性和随时性能。

ABSTRACT

A redundant manipulator has multiple inverse kinematics solutions per end-effector pose. Accordingly, there can be many trajectories for joints that follow a given endeffector path in the Cartesian space. In this paper, we present a trajectory optimization of a redundant manipulator (TORM) to synthesize a trajectory that follows a given end-effector path accurately, while achieving smoothness and collisionfree manipulation. Our method holistically incorporates three desired properties into the trajectory optimization process by integrating the Jacobian-based inverse kinematics solving method and an optimization-based motion planning approach. Specifically, we optimize a trajectory using two-stage gradient descent to reduce potential competition between different properties during the update. To avoid falling into local minima, we iteratively explore different candidate trajectories with our local update. We compare our method with state-of-the-art methods in test scenes including external obstacles and two non-obstacle problems. Our method robustly minimizes the pose error in a progressive manner while satisfying various desirable properties.

研究动机与目标

• 解决在给定期望末端执行器路径的前提下，为冗余机械臂生成平滑、无碰撞且高度精确的关节轨迹的挑战。
• 将路径精度、运动学可行性与避障能力整体集成到单一优化框架中。
• 通过两阶段更新与迭代探索策略，克服轨迹优化中的局部极小值与目标梯度冲突问题。
• 在有限规划时间内实现高精度解，支持实时应用。
• 在无障碍物与含障碍物场景下均进行验证，包括真实机器人实验。

提出的方法

• 该方法采用两阶段梯度下降（TSGD），交替更新以实现约束满足与路径跟踪精度，降低目标之间的竞争。
• 将基于雅可比矩阵的逆运动学与基于优化的运动规划相结合，确保运动学可行性和碰撞避免。
• 采用迭代探索策略，通过测试替代轨迹并逐步优化解，以逃离局部极小值。
• 该方法高效计算函数梯度，实现快速收敛与随时性能。
• 优化过程应用于末端执行器路径上的子采样位姿，通过插值完成完整轨迹重建。
• 使用符号距离函数实现实时外部障碍物碰撞检测。

Figure 1: These figures show a sequence of maneuvering the Fetch manipulator to follow the specified end-effector path (red lines). Our method generates the trajectory that accurately follows the given end-effector path, while avoiding obstacles such as the pack of A4 paper and the table.

实验结果

研究问题

• RQ1 统一的优化框架能否有效平衡冗余机械臂轨迹规划中的路径精度、平滑性与避障性能？
• RQ2 将优化解耦为两个阶段在多大程度上提升了收敛性并减少了目标间的冲突？
• RQ3 迭代探索在多大程度上增强了轨迹优化对局部极小值的鲁棒性？
• RQ4 TORM 在位姿误差、规划时间与多样化环境下的鲁棒性方面，与最先进方法相比表现如何？
• RQ5 该方法能否实现高精度解的随时性能，支持实时部署？

主要发现

• TORM 在所有对比方法中实现了最低的平均与最小位姿误差，平均误差为 1.91e-5，显著优于 RelaxedIK 与 Stampede。
• 该方法逐步且稳健地降低位姿误差，表现出强大的随时性能，随时间推移持续改善。
• 消融实验表明，若禁用两阶段梯度下降，平均位姿误差将增加超过 10 倍，证明其在解决冲突中的必要性。
• 若不采用迭代探索，方法易陷入局部极小值，导致 20 次试验中最小与最大位姿误差差异最大（8.50e-5 vs. 7.65e-5）。
• 该方法成功在真实 Fetch 机器人上生成无碰撞轨迹，验证了其实际可行性。
• TORM 保持了较低的轨迹长度（9.5 rad）与初始解时间（5.2s），表明其在实际部署中的高效性与实用性。

Figure 2: This figure shows our problem that is synthesizing a feasible and accurate trajectory $\xi$ for a given end-effector path $\mathcal{X}$ . The red line is $\mathcal{X}$ , which is approximated by end-effector poses $\widetilde{\mathcal{X}}=(\mathrm{x}_{0},\mathrm{x}_{1},...,\mathrm{x}_{n})$