机器人 / 系统课时

用正逆运动学验证目标可达性

用正逆运动学验证目标可达性

给定一个抓取点,机械臂并不一定能把末端移动到指定位置和朝向。本篇以 ROS 2 Jazzy 的 Panda 七轴机械臂为例,先用正运动学得到已知关节配置的末端位姿,再调用 MoveIt 2 逆运动学求解并回代核验,区分真正不可达与求解器暂时找不到解。

学习目标

  • 说清 FK 与 IK 的输入、输出,以及多解、无解的区别。
  • 调用 MoveIt 2 的 /compute_fk、/compute_ik 验证末端目标。
  • 根据误差、关节限位和碰撞约束判断求解结果是否可用。

正解、逆解与可达性

正运动学(FK)输入七个关节角,结合 URDF 中的关节轴和连杆变换,计算 panda_link8 相对 panda_link0 的末端位姿:位置以米表示,朝向用四元数表示。同一套机器人模型和关节角对应确定的 FK 结果。

逆运动学(IK)反过来输入目标位姿、规划组及关节初值(seed),寻找满足目标的关节角。Panda 有七个旋转关节,同一末端位姿可能有多组关节配置;指定完整姿态后仍可能无解。逆解需要遵守关节限位,启用碰撞检测时还要满足当前 Planning Scene。

Panda 正运动学、逆运动学及回代验证关系

可达性至少分三层:有运动学解、关节配置不碰撞、存在可执行的无碰撞轨迹。/compute_ik 返回成功,只证明找到了符合本次 IK 请求的配置,不代表从当前姿态一定能规划到那里。

启动 Panda 并检查服务

适用环境为 Ubuntu 24.04、ROS 2 Jazzy,已安装 MoveIt 2 与 moveit_resources_panda_moveit_config。在终端 A 启动官方 Panda 演示:

bash
source /opt/ros/jazzy/setup.bash
ros2 launch moveit_resources_panda_moveit_config demo.launch.py

预期结果:RViz 2 显示 Panda 模型,move_group 持续运行。启动日志会随安装版本变化,不以具体时间戳为验收依据。

在终端 B 确认两个服务已发布:

bash
source /opt/ros/jazzy/setup.bash
ros2 service list -t | grep -E 'compute_(fk|ik)'

预期输出(采用演示包的默认命名空间):

text
/compute_fk [moveit_msgs/srv/GetPositionFK]
/compute_ik [moveit_msgs/srv/GetPositionIK]

若服务带命名空间,应相应修改下方脚本中的服务路径;若未出现,先检查 move_group 是否启动成功。

用 FK—IK—FK 检查同一目标

采用 Panda SRDF 中 ready 关节角(单位 rad):[0, -0.785, 0, -2.356, 0, 1.571, 0.785]。先从这个有效关节配置求 FK,再以得到的位姿请求 IK;因为目标取自 FK,可以避免随意填入一个本来就不可达的坐标。

将下列完整 Python 程序保存为 fk_ik_check.py,在终端 B 执行 python3 fk_ik_check.py。代码使用 Jazzy 的 rclpy 和 MoveIt 消息包,不需要额外的第三方 Python IK 库。

python
import math

import rclpy
from moveit_msgs.msg import MoveItErrorCodes
from moveit_msgs.srv import GetPositionFK, GetPositionIK

JOINTS = [f"panda_joint{i}" for i in range(1, 8)]
READY = [0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785]

def request(node, client, message):
    future = client.call_async(message)
    rclpy.spin_until_future_complete(node, future)
    if future.result() is None:
        raise RuntimeError("服务调用失败")
    return future.result()

def main():
    rclpy.init()
    node = rclpy.create_node("panda_fk_ik_check")
    try:
        fk_client = node.create_client(GetPositionFK, "/compute_fk")
        ik_client = node.create_client(GetPositionIK, "/compute_ik")
        for name, client in (("/compute_fk", fk_client), ("/compute_ik", ik_client)):
            if not client.wait_for_service(timeout_sec=10.0):
                raise RuntimeError(f"服务未就绪: {name}")

        # 用 ready 关节角产生确定的可达目标
        fk_req = GetPositionFK.Request()
        fk_req.header.frame_id = "panda_link0"
        fk_req.fk_link_names = ["panda_link8"]
        fk_req.robot_state.joint_state.name = JOINTS
        fk_req.robot_state.joint_state.position = READY
        fk = request(node, fk_client, fk_req)
        print("FK 状态码:", fk.error_code.val)
        if fk.error_code.val != MoveItErrorCodes.SUCCESS:
            return
        target = fk.pose_stamped[0]

        # 以相同关节状态作 IK 初值;先隔离纯运动学问题
        ik_req = GetPositionIK.Request()
        ik_req.ik_request.group_name = "panda_arm"
        ik_req.ik_request.ik_link_name = "panda_link8"
        ik_req.ik_request.robot_state = fk_req.robot_state
        ik_req.ik_request.pose_stamped = target
        ik_req.ik_request.timeout.sec = 1
        ik_req.ik_request.avoid_collisions = False
        ik = request(node, ik_client, ik_req)
        print("IK 状态码:", ik.error_code.val)
        if ik.error_code.val != MoveItErrorCodes.SUCCESS:
            return

        values = dict(zip(ik.solution.joint_state.name,
                          ik.solution.joint_state.position))
        print("IK 关节角(rad):", [round(values[n], 3) for n in JOINTS])

        # 对逆解做 FK 回代,比较位置和姿态
        check = GetPositionFK.Request()
        check.header.frame_id = "panda_link0"
        check.fk_link_names = ["panda_link8"]
        check.robot_state = ik.solution
        back = request(node, fk_client, check)
        if back.error_code.val != MoveItErrorCodes.SUCCESS:
            raise RuntimeError(f"回代 FK 失败: {back.error_code.val}")

        goal, actual = target.pose, back.pose_stamped[0].pose
        distance = math.dist(
            (goal.position.x, goal.position.y, goal.position.z),
            (actual.position.x, actual.position.y, actual.position.z))
        a, b = goal.orientation, actual.orientation
        dot = abs(a.x*b.x + a.y*b.y + a.z*b.z + a.w*b.w)
        angle = 2.0 * math.acos(min(1.0, dot))
        print(f"位置误差(m): {distance:.6f}")
        print(f"姿态误差(rad): {angle:.6f}")
        print("回代验证:", "通过" if distance < 0.001 and angle < 0.01 else "不通过")

        # 重用目标,加入当前规划场景的碰撞约束
        ik_req.ik_request.avoid_collisions = True
        checked = request(node, ik_client, ik_req)
        print("避碰 IK 状态码:", checked.error_code.val)
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == "__main__":
    main()

预期输出(演示场景无新增障碍时;角度和误差值可能随求解器而不同):

text
FK 状态码: 1
IK 状态码: 1
IK 关节角(rad): <JOINT_VALUES>
位置误差(m): <ERROR_M>
姿态误差(rad): <ERROR_RAD>
回代验证: 通过
避碰 IK 状态码: 1

SUCCESS=1。脚本要求回代位置误差小于 0.001 m、姿态误差小于 0.01 rad;这些是本次教学验收阈值,不是 MoveIt 对所有机器人的默认承诺。占位符表示与安装环境、求解器有关的实际数值。逆解的关节角可以不同于 ready,只要回代位姿满足阈值便不构成错误。

排查无解、奇异与碰撞约束

为了制造明确不可达的测试目标,可在脚本中 target = fk.pose_stamped[0] 之后加一行 target.pose.position.x += 2.0,再运行。Panda 工作空间无法覆盖沿基座 X 方向额外移动 2 米的目标;通常会得到 NO_IK_SOLUTION=-31,脚本会在 IK 失败后提前结束。测试完成后删除该行。

遇到 IK 失败,按下面顺序缩小问题范围:

  1. 坐标系与末端链:核对 pose_stamped.header.frame_id=panda_link0、group_name=panda_arm、ik_link_name=panda_link8。FRAME_TRANSFORM_FAILURE=-21 优先检查 TF,不要把它当成目标超出工作空间。
  2. 关节限位与目标姿态:检查 URDF 限位,确认四元数有效且归一化;先用 FK 产生的位姿做基准,再改目标位置或朝向。位置可达不等于指定朝向也可达。
  3. 数值求解与奇异附近:伸直或特定关节轴对齐时,雅可比矩阵可能退化,微小末端变化需要很大关节变化。可改用另一组合法 seed,或调整目标姿态并重试;单次 -31 不足以证明数学上无解。
  4. 碰撞:对同一位姿比较 avoid_collisions=False 与 True。前者成功而后者失败时,重点检查 Planning Scene 中的障碍物、自碰撞和夹爪状态;即使避碰 IK 成功,还需要单独规划并检查整条路径。

Panda 演示配置中的 config/kinematics.yaml 为 panda_arm 指定 kdl_kinematics_plugin/KDLKinematicsPlugin,常见求解器超时设置是 kinematics_solver_timeout: 0.05 秒。本例请求里的 timeout.sec=1 是服务请求允许的搜索时间,不应误认为改变了 YAML 配置。实际抓取时,要把 IK 解作为目标候选,再让 MoveIt 2 的规划器检验从当前关节状态到目标的完整运动过程。

按自己的节奏,确认真正理解。

目录与阅读设置

阅读设置

17

记录一个想法

用正逆运动学验证目标可达性

0 / 10000 · 笔记保存到账号。未提交的修改会在关闭、离开或刷新后丢失。

登录哪都通