3、正运动学：DH参数法、建立DH坐标系、正运动学求解、MATLAB/ROS验证
正运动学，说白了就是：你给我每个关节的角度，我告诉你机械臂末端在哪儿、朝哪个方向。
我刚开始做机器人时，总觉得这玩意儿太理论了。直到有一次调试六轴臂，末端死活对不准目标点，最后发现是DH参数表里一个符号写反了。嗯，从那以后我再也不敢小看正运动学了。
这一节，咱们就把它彻底搞明白。
3.1 DH参数法——机器人界的“通用语言”
DH参数法，全称Denavit-Hartenberg参数法。它用四个参数来描述相邻两个关节之间的变换关系：
- θ (theta)：绕Z轴的旋转角（关节角）
- d：沿Z轴的平移距离（连杆偏距）
- a：沿X轴的平移距离（连杆长度）
- α (alpha)：绕X轴的旋转角（连杆扭角）
你想想看，任何一个机械臂，不管它长什么样，只要能用这四个参数把每个关节描述清楚，就能用统一的数学方法算出末端位姿。这就是DH法的厉害之处。
3.2 建立DH坐标系——手把手教你画
建立DH坐标系，有标准DH和改进DH两种。我个人更常用改进DH（也叫Craig版本），因为它在树形结构机器人里更好用。不过今天咱们讲标准DH，因为教材里最常见。
步骤其实就四步：
- 找Z轴：每个关节的轴线方向就是Z轴方向。旋转关节的Z轴沿旋转轴方向，移动关节的Z轴沿移动方向。
- 定原点：相邻两个Z轴的公垂线与Z轴的交点，就是坐标系原点。
- 画X轴：X轴沿着公垂线方向，从当前Z轴指向下一个Z轴。
- 定Y轴：右手定则，Y = Z × X。
举个例子，一个简单的两连杆机械臂：
- 关节1：绕Z0旋转，连杆长度a1
- 关节2：绕Z1旋转，连杆长度a2
对应的DH参数表就是：
| 关节 i | θ_i | d_i | a_i | α_i |
|---|---|---|---|---|
| 1 | θ1 | 0 | a1 | 0 |
| 2 | θ2 | 0 | a2 | 0 |
3.3 正运动学求解——从关节角到位姿矩阵
有了DH参数表，正运动学求解就是套公式的事。每个相邻关节的变换矩阵为：
T_i = Rot(z, θ_i) * Trans(z, d_i) * Trans(x, a_i) * Rot(x, α_i)展开成4x4矩阵就是：
T_i = [cosθ_i -sinθ_i*cosα_i sinθ_i*sinα_i a_i*cosθ_i
sinθ_i cosθ_i*cosα_i -cosθ_i*sinα_i a_i*sinθ_i
0 sinα_i cosα_i d_i
0 0 0 1 ]然后，从基座到末端的总变换矩阵就是：
T_0n = T_1 * T_2 * ... * T_n这个矩阵的左上角3x3是姿态（旋转矩阵），右上角3x1是位置（平移向量）。
举个例子，两连杆机械臂的末端位置：
x = a1*cos(θ1) + a2*cos(θ1+θ2)
y = a1*sin(θ1) + a2*sin(θ1+θ2)是不是很眼熟？其实就是平面几何里的“臂+前臂”模型。
3.4 MATLAB验证——让公式动起来
光看公式不过瘾，咱们用MATLAB跑一下。我习惯用Robotics Toolbox，它内置了DH建模功能。
% 定义两连杆机械臂的DH参数
L1 = Link([0 0 a1 0]); % [θ d a α]
L2 = Link([0 0 a2 0]);
% 创建机器人模型
robot = SerialLink([L1 L2], 'name', '两连杆臂');
% 给定关节角
q = [pi/4, pi/6]; % 单位：弧度
% 计算正运动学
T = robot.fkine(q);
% 显示末端位姿
disp('末端位姿矩阵：');
disp(T);
% 提取位置
pos = T.t;
disp(['末端位置：x=', num2str(pos(1)), ', y=', num2str(pos(2))]);
运行一下，你会看到末端位置和咱们手算的结果完全一致。这就是验证的意义——确保你的DH参数和建模没出错。
3.5 ROS验证——仿真环境里跑一跑
ROS里验证正运动学，我一般用MoveIt!的FK插件。步骤很简单：
- 在URDF里定义好机械臂的连杆和关节
- 加载到MoveIt! Setup Assistant里生成配置
- 用
moveit_commander调用get_current_state()或compute_cartesian_path()
举个例子，Python脚本里这样写：
import rospy
import moveit_commander
# 初始化
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('fk_test', anonymous=True)
# 获取机械臂组
arm = moveit_commander.MoveGroupCommander("arm_group")
# 设置目标关节角
joint_goal = arm.get_current_joint_values()
joint_goal[0] = 0.5 # 关节1
joint_goal[1] = 0.3 # 关节2
# 计算正运动学
arm.set_joint_value_target(joint_goal)
plan = arm.plan()
# 获取末端位姿
pose = arm.get_current_pose()
print("末端位置：", pose.pose.position)
print("末端姿态：", pose.pose.orientation)
ROS的好处是，你可以直接在Rviz里看到机械臂动起来。如果正运动学算错了，机械臂会扭成奇怪的样子——我见过一次，差点笑出声。
axis方向写反了，结果ROS里正运动学算出来的末端位置和MATLAB对不上。排查了一下午才发现。所以，一定要在多个工具里交叉验证。
3.6 本章知识体系
下面这张图，帮你把这一节的核心逻辑串起来：
从DH参数到坐标系建立，再到矩阵求解，最后用MATLAB和ROS双重验证——这就是正运动学的完整闭环。每一步都踩实了，后面学逆运动学、轨迹规划才能稳。
好了，这一节就到这儿。记住：正运动学是基础中的基础，花时间搞透它，值。
可可精品资料库