本文由 linuxROS 整理发布，首发于 linuxros.cn，转载请注明出处。

MoveIt2运动学模块：正向求解与逆向求解

导读：机械臂末端如何知道自己在哪？关节角度怎么换算成笛卡尔坐标？本文详解MoveIt2运动学求解原理，帮你搞懂正逆运动学在机械臂控制中的应用与求解器选型。

原理简析

运动学是机械臂控制的基础，分为正向运动学（FK）和逆向运动学（IK）两部分。

正向运动学（FK）：已知关节角度，求末端执行器位置姿态。简单说就是"我知道每个关节转了多少，末端在哪"。

逆向运动学（IK）：已知末端目标位置，求需要的关节角度。难点在于同一个位置可能有多个解，也可能无解（超出工作空间或奇异位形）。

flowchart LR A["关节角度"] -->|"FK"| B["末端位置"] B -.->|"反向"| A C["末端目标"] -->|"IK"| D["关节角度"] D -.->|"反向"| C

实操步骤

运动学求解流程

MoveIt2 收到规划请求后，会调用配置的 IK 插件求解目标关节角，再交给规划器在关节空间内搜索路径。

来自 linuxros.cn · linuxROS

flowchart TB A(["输入目标姿态"]) --> B{"求解器选择"} B -->|"数值解"| C["KDL / TRAC-IK"] B -->|"解析解"| D["IKFast"] C --> E["多解验证"] D --> E E --> F{"碰撞检查"} F -->|"通过"| G["返回关节角度"] F -->|"碰撞"| H["尝试下一解"] H --> E

常用求解器对比

求解器	类型	适用场景	特点
KDL	数值解	通用、默认	稳定，关节限位处易陷局部极小
TRAC-IK	数值解	实时控制、复杂姿态	多线程并行，成功率显著高于 KDL
IKFast	解析解	6/7 轴标准臂	微秒级求解，需 OpenRAVE 离线生成

避坑点：KDL 在关节限位附近容易求解失败，TRAC-IK 通过并发运行 KDL 改进版与 SQP 优化算法规避此问题；IKFast 不支持 mimic 关节。

代码实现

配置运动学插件（kinematics.yaml）

# kinematics.yaml
manipulator:
  kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin
  kinematics_solver_timeout: 0.005
  solve_type: Speed   # 可选 Speed / Distance / Manipulation1 / Manipulation2

注意：TRAC-IK 不需要 kinematics_solver_attempts 参数，内部已有重启机制；solve_type 决定返回最优解的策略。

正向运动学调用（C++）

// forward_kinematics.cpp
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_state/robot_state.h>

void FKExample(const moveit::core::RobotModelPtr& model)
{
    auto joint_model_group = model->getJointModelGroup("manipulator");
    std::vector<double> joint_values = {0.0, 0.5, 1.0, 0.3, 0.5, 0.2};

    // 通过 RobotState 调用 FK
    moveit::core::RobotState robot_state(model);
    robot_state.setJointGroupPositions(joint_model_group, joint_values);
    robot_state.update();

    const std::string tip_link =
        joint_model_group->getLinkModelNames().back();
    const Eigen::Isometry3d& pose =
        robot_state.getGlobalLinkTransform(tip_link);
}

逆向运动学调用（C++）

// inverse_kinematics.cpp
#include <moveit/move_group_interface/move_group_interface.h>

void IKExample(moveit::planning_interface::MoveGroupInterface& move_group)
{
    geometry_msgs::msg::Pose target_pose;
    target_pose.position.x = 0.4;
    target_pose.position.y = 0.1;
    target_pose.position.z = 0.3;
    target_pose.orientation.w = 1.0;

    // 通过 setPoseTarget 触发 IK（内部自动调用配置的求解器）
    move_group.setPoseTarget(target_pose);

    // 显式调用 setFromIK 获取多组解
    moveit::core::RobotStatePtr kinematic_state =
        move_group.getCurrentState(10.0);

    const moveit::core::JointModelGroup* jmg =
        move_group.getJointModelGroup();
    bool success = kinematic_state->setFromIK(
        jmg,
        target_pose,
        0.1,   // timeout
        5);    // attempts
}

常见问题解决

Q1：IK 求解失败怎么办？
A：先用 ros2 run moveit_kinematics test_ik 工具验证目标位姿是否可达；增大 kinematics_solver_timeout；从 KDL 切换到 TRAC-IK。

Q2：解析解和数值解哪个好？
A：解析解（IKFast）速度快至微秒级，但需要离线生成且仅支持特定结构；数值解通用但计算量大。工业场景若机器人结构固定，优先 IKFast。

Q3：多解如何选择？
A：TRAC-IK 提供 Distance（与当前姿态最近）、Manipulation1/2（可操作度最优）等策略；也可在 setFromIK 后回调中自定义评分。

总结

运动学是机械臂控制的基础，MoveIt2 通过插件化设计支持 KDL、TRAC-IK、IKFast 三大求解器。FK 相对简单，IK 是难点。掌握 TRAC-IK 的配置与 solve_type 调优，能应对大多数 6 轴机械臂场景。下期讲解规划器模块，搞懂 RRT 到 CHOMP 的路径搜索原理。