📚 课程目录第4章机器人运动学基础

4. 机器人运动学基础：正运动学与逆运动学、URDF模型、使用MoveIt!获取末端位姿

手眼标定的核心是将相机坐标系与机器人基坐标系关联起来，而机器人末端执行器在空间中的位姿（位置+姿态）是连接这两个坐标系的关键桥梁。本章将系统讲解机器人运动学基础，包括正/逆运动学原理、URDF模型结构，以及如何通过MoveIt!实时获取末端位姿，为后续标定计算提供数据支撑。

4.1 正运动学与逆运动学

机器人运动学描述关节空间与笛卡尔空间之间的映射关系，是机器人控制与感知的数学基础。

概念	定义	输入	输出	应用场景
正运动学	已知各关节角度/位移，求解末端执行器在基坐标系下的位姿	关节变量 q₁, q₂, …, qₙ	末端位姿矩阵 T₀ₙ	仿真验证、状态估计、碰撞检测
逆运动学	已知末端期望位姿，求解满足该位姿的关节角度组合	目标位姿矩阵 T_desired	关节变量 q₁, q₂, …, qₙ	轨迹规划、抓取控制、视觉伺服

  关键公式： 正运动学通过齐次变换矩阵链乘实现：T₀ₙ = T₀₁(q₁) · T₁₂(q₂) · … · T₍ₙ₋₁₎ₙ(qₙ)。逆运动学通常存在多解、无解或奇异解，需要结合关节限位和避障进行最优选择。

4.2 URDF模型：机器人的统一描述语言

URDF（Unified Robot Description Format）是ROS中用于描述机器人几何、运动学与动力学特性的XML格式文件。它是MoveIt!进行运动规划与碰撞检测的基础。

一个完整的URDF模型包含以下核心元素：

<link>：定义刚体部件，包含惯性、碰撞、视觉属性。
<joint>：定义两个link之间的连接关系，包含类型（revolute/prismatic/fixed等）、原点、轴方向、运动学限位。
<robot>：根元素，包含name属性，包裹所有link和joint。

<robot name="my_robot_arm">
  <link name="base_link">
    <visual>
      <geometry><box size="0.3 0.3 0.1"/></geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </visual>
  </link>

  <joint name="joint1" type="revolute">
    <parent link="base_link"/>
    <child link="link1"/>
    <origin rpy="0 0 0" xyz="0 0 0.1"/>
    <axis xyz="0 0 1"/>
    <limit lower="-3.14" upper="3.14" effort="10" velocity="1.0"/>
  </joint>
</robot>

实践建议： 使用 check_urdf 工具验证URDF语法正确性；使用 urdf_to_graphiz 生成模型拓扑图，快速检查父子关系。

4.3 使用MoveIt!获取末端位姿

MoveIt!是ROS中主流的运动规划框架，它封装了运动学求解器、碰撞检测、轨迹规划等模块。在手眼标定流程中，我们通过MoveIt!的C++/Python API获取机器人当前末端执行器在基坐标系下的位姿。

4.3.1 核心API与流程

初始化MoveGroup接口：指定规划组名称（如"arm_group"）。
获取当前状态：调用 move_group.getCurrentPose() 获取末端位姿。
提取位姿数据：从返回的 geometry_msgs::PoseStamped 中提取位置(x,y,z)和姿态(四元数)。
转换为齐次矩阵：将四元数转换为旋转矩阵，组合成4×4齐次变换矩阵。

# Python示例：通过MoveIt!获取末端位姿
import rospy
from moveit_commander import MoveGroupCommander
from geometry_msgs.msg import PoseStamped

rospy.init_node('get_ee_pose')
group = MoveGroupCommander("arm_group")

# 获取当前末端位姿
current_pose: PoseStamped = group.get_current_pose()
position = current_pose.pose.position
orientation = current_pose.pose.orientation

print(f"位置: x={position.x:.4f}, y={position.y:.4f}, z={position.z:.4f}")
print(f"姿态(四元数): x={orientation.x:.4f}, y={orientation.y:.4f}, z={orientation.z:.4f}, w={orientation.w:.4f}")

常见问题：

确保MoveIt!配置正确加载了机器人URDF和SRDF文件。
获取位姿前需确认机器人处于非奇异位形，否则运动学求解可能不稳定。
四元数转旋转矩阵时注意坐标系约定（通常使用右手系，Z轴向上）。

4.3.2 位姿数据在标定中的使用

在手眼标定中，我们通常需要记录多组机器人末端位姿与对应的标定板位姿。每组数据构成一个方程，最终通过最小二乘或封闭解法求解手眼变换矩阵。

数据组	机器人末端位姿 (T_base_to_ee)	标定板位姿 (T_cam_to_board)	用途
第1组	从MoveIt!获取	从相机标定获取	构建AX=XB方程
第2组	从MoveIt!获取	从相机标定获取	构建AX=XB方程
…	…	…	…
第N组	从MoveIt!获取	从相机标定获取	求解最优手眼矩阵

4.4 本章小结

正运动学：从关节空间到笛卡尔空间的映射，是机器人仿真的基础。
逆运动学：从笛卡尔空间到关节空间的映射，是轨迹规划的核心。
URDF模型：以树状拓扑结构描述机器人，是MoveIt!配置的输入文件。
MoveIt!位姿获取：通过 getCurrentPose() 实时获取末端位姿，为手眼标定提供关键数据。