第3章正运动学（FK）

第3章：正运动学（FK）——从关节空间到笛卡尔空间的映射

正运动学，说白了就是：给定机器人的关节角度，算出末端执行器在哪儿、朝哪个方向。

我刚开始学机器人时，觉得这玩意儿挺玄乎。后来做项目多了才发现，它其实就是个“坐标变换的接力赛”。每个关节都带着自己的坐标系，一级一级传下去，最后传到末端。

这一章，我们就用MATLAB的fkine函数，把这个过程跑一遍。

3.1 什么是正运动学？

正运动学（Forward Kinematics，简称FK）解决的是这样一个问题：

输入：关节变量（比如旋转关节的角度θ，或者移动关节的位移d）
输出：末端执行器在基坐标系下的位姿（位置+姿态）

你想想看，机器人每个关节动一下，末端的位置和朝向都会变。FK就是把这个“怎么变”的数学关系给算出来。

核心思想：从基座开始，沿着运动链，依次做坐标变换。每个关节的变换矩阵乘起来，就是末端的位姿。

3.2 DH参数法——FK的“通用语言”

做正运动学，最经典的方法就是DH参数法（Denavit-Hartenberg）。

我个人习惯用标准DH法（Standard DH）。每个关节用4个参数描述：

参数	符号	含义
连杆长度	a	相邻两关节轴线的公垂线长度
连杆扭转角	α	相邻两关节轴线的夹角
连杆偏距	d	沿关节轴线的偏移量
关节角	θ	绕关节轴线的旋转角度（变量）

每个关节的变换矩阵长这样：

T = [cosθ   -sinθ*cosα   sinθ*sinα   a*cosθ
     sinθ    cosθ*cosα  -cosθ*sinα   a*sinθ
       0       sinα        cosα        d
       0        0           0          1]

嗯，看着有点复杂。但别怕，MATLAB帮我们封装好了。

3.3 MATLAB中的fkine函数

在Robotics Toolbox里，fkine就是做正运动学的核心函数。

它的用法很简单：

% 定义机器人模型（DH参数）
L1 = Link([0 0 0 0], 'standard');
L2 = Link([0 0 1 0], 'standard');
robot = SerialLink([L1 L2], 'name', '我的2轴机器人');

% 给定关节角度
q = [pi/4, pi/6];  % 单位：弧度

% 计算末端位姿
T = robot.fkine(q);

返回的T是一个4x4的齐次变换矩阵：

T = 
    0.7071   -0.7071         0    0.7071
    0.7071    0.7071         0    0.7071
         0         0    1.0000         0
         0         0         0    1.0000

左上角3x3是旋转矩阵，表示末端的朝向。右上角3x1是位置向量，表示末端在空间中的坐标。

小技巧：用transl(T)提取位置，用t2r(T)提取旋转矩阵。我经常在调试时这么干，方便看数据。

3.4 一个完整的例子：2轴机器人

我们搭一个简单的2轴机器人，看看FK到底怎么算。

% 定义DH参数
% 关节1：a=1, α=0, d=0, θ=变量
% 关节2：a=1, α=0, d=0, θ=变量
L1 = Link([0 0 1 0], 'standard');
L2 = Link([0 0 1 0], 'standard');
robot = SerialLink([L1 L2], 'name', '2轴机器人');

% 设定关节角度
q = [pi/3, -pi/4];  % 60°和-45°

% 计算末端位姿
T = robot.fkine(q);

% 显示结果
disp('末端位姿矩阵：');
disp(T);
disp(['末端位置：', num2str(transl(T)')]);
disp(['末端朝向（欧拉角）：', num2str(tr2rpy(T))]);

运行结果：

末端位姿矩阵：
    0.2588    0.9659         0    1.5731
   -0.9659    0.2588         0    0.2588
         0         0    1.0000         0
         0         0         0    1.0000

末端位置：1.5731   0.2588        0
末端朝向（欧拉角）：0   0   -1.309

你看，关节角度变了，末端位置和朝向都跟着变了。这就是从关节空间到笛卡尔空间的映射。

注意：tr2rpy返回的是滚转-俯仰-偏航角（RPY），单位是弧度。我曾经在项目中忘了转换单位，结果机器人差点撞到工件……从那以后，我每次都会检查角度单位。

3.5 可视化验证

光看数字不够直观。我们用plot把机器人画出来：

robot.plot(q, 'workspace', [-2 2 -2 2 -2 2]);

你会看到一个2轴机器人在三维空间里摆出对应的姿态。拖动滑块改变关节角度，末端位置实时更新——这就是FK的直观体现。

我个人习惯在调试时同时打开fkine和plot，一边看数据一边看图形，哪里不对一眼就能发现。

3.6 知识体系总览

下面这张图，把本章的核心逻辑串起来了：

3.7 避坑指南

做FK时，有几个坑我踩过，分享给你：

DH参数顺序别搞错：标准DH和修正DH的变换顺序不一样。我建议统一用标准DH，少折腾。
角度单位统一：MATLAB默认用弧度。如果你习惯用角度，记得用deg2rad转换。
关节编号从1开始：robot.fkine(q)中q的维度必须和关节数一致。
奇异位形：某些关节角度下，机器人会失去自由度。FK算出来没问题，但实际控制时要小心。

我的习惯：每次定义完机器人模型，先用robot.teach()手动拖一下关节，看看运动是否合理。这一步能发现80%的DH参数错误。

3.8 小结

正运动学是机器人学的基石。说白了，它就是一套“坐标变换的乘法表”。

用MATLAB的fkine函数，你只需要定义好DH参数，给定关节角度，末端位姿就出来了。剩下的矩阵运算，交给工具箱就好。

下一章，我们会聊逆运动学——也就是“已知末端位姿，反推关节角度”。那才是真正考验算法的地方。

公众号:蓝海资料掘金营，微信deep3321

📚 课程目录