4、机器人运动学建模：坐标系与变换、齐次变换矩阵、使用Simulink构建正运动学模型
好，咱们进入正题。运动学建模，说白了就是搞清楚机器人各个关节动的时候，末端到底跑到了哪里。你想想看，一个六轴机械臂，每个关节转个角度，末端位置怎么算？这就是正运动学要干的事。
我个人习惯，拿到一个机器人，第一件事不是写代码，而是先在脑子里把坐标系理清楚。坐标系乱了，后面全白搭。我在项目中遇到过好几次，就是因为坐标系定义不统一，联调的时候末端抓取偏差好几厘米，查了两天才发现是基坐标系和工具坐标系搞反了。
4.1 坐标系与变换：机器人运动的“语言”
机器人运动学里，坐标系就是我们的“语言”。每个关节、每个连杆，都有自己的坐标系。我们说的“变换”，就是从一个坐标系看另一个坐标系。
举个例子。你站在地上看机器人的基座，这是基坐标系{B}。机器人的手爪上也有一个坐标系，叫工具坐标系{T}。我们要知道的，就是{T}相对于{B}的位置和姿态。
位置好理解，就是x、y、z三个坐标值。姿态呢？就是坐标系怎么转过去的。这里我建议你记住一个概念：旋转矩阵。它是一个3x3的矩阵，描述了坐标轴的旋转关系。
核心要点：一个刚体在空间中的位姿，由位置矢量（3个参数）和旋转矩阵（3x3，9个参数，但只有3个自由度）共同描述。一共6个自由度。
嗯，这里要注意。旋转矩阵虽然看起来有9个数，但它不是随便填的。它必须满足正交性——行和列都是单位向量，且两两正交。我刚开始学的时候，总觉得这玩意儿太抽象，直到有一次自己手算DH参数，算出来的旋转矩阵行列式不是1，才发现矩阵写错了。从那以后，我每次算完都会检查一下行列式是不是等于+1。
4.2 齐次变换矩阵：把旋转和平移打包
旋转矩阵只能描述姿态，位置还得单独写。这在实际编程中很麻烦。所以就有了齐次变换矩阵，它把旋转和平移打包成一个4x4的矩阵。
形式是这样的：
T = [ R p ]
[ 0 1 ]其中R是3x3旋转矩阵，p是3x1位置矢量，最后一行是[0 0 0 1]。
为什么用4x4？说白了就是为了让矩阵乘法能同时处理旋转和平移。你想想看，如果只用3x3矩阵，平移就得单独加，写代码的时候要多写好几行。用齐次矩阵，一个乘法就搞定。
我的小技巧：在Simulink里建模时，我习惯把每个关节的齐次变换矩阵单独封装成一个子系统。这样调试的时候，哪个关节的矩阵算错了，一眼就能看出来。我曾经在一个项目里，就是因为某个关节的DH参数符号写反了，导致整个运动学模型输出全乱套。后来用子系统隔离，十分钟就定位到了问题。
齐次变换矩阵还有一个好处：可以链式相乘。比如从基座到关节1的变换是T01，关节1到关节2是T12，那么基座到关节2就是T02 = T01 * T12。依此类推，一直到末端。这就是正运动学的核心思想。
4.3 使用Simulink构建正运动学模型
好，理论讲完了，咱们来点实际的。怎么在Simulink里搭一个正运动学模型？
我一般分三步走：
- 定义DH参数表——这是输入，每个关节的a、alpha、d、theta。
- 构建单个关节的变换矩阵——用Simulink的矩阵运算模块。
- 链式相乘得到末端位姿——把每个关节的矩阵乘起来。
咱们以一个简单的2自由度平面机械臂为例。DH参数表如下：
| 关节i | a (m) | alpha (rad) | d (m) | theta (rad) |
|---|---|---|---|---|
| 1 | 1.0 | 0 | 0 | q1 |
| 2 | 0.8 | 0 | 0 | q2 |
每个关节的齐次变换矩阵公式是：
T_i = [ cos(theta) -sin(theta)*cos(alpha) sin(theta)*sin(alpha) a*cos(theta) ]
[ sin(theta) cos(theta)*cos(alpha) -cos(theta)*sin(alpha) a*sin(theta) ]
[ 0 sin(alpha) cos(alpha) d ]
[ 0 0 0 1 ]在Simulink里，我用MATLAB Function模块来写这个公式。代码很简单：
function T = dh_transform(a, alpha, d, theta)
T = [cos(theta), -sin(theta)*cos(alpha), sin(theta)*sin(alpha), a*cos(theta);
sin(theta), cos(theta)*cos(alpha), -cos(theta)*sin(alpha), a*sin(theta);
0, sin(alpha), cos(alpha), d;
0, 0, 0, 1];
end然后，把两个关节的矩阵用矩阵乘法模块串起来。输入是q1和q2，输出就是末端位姿矩阵T02。
避坑指南：我曾经犯过一个低级错误——矩阵乘法的顺序搞反了。记住，从基座到末端，是T01 * T12，不是T12 * T01。矩阵乘法不满足交换律，顺序错了，末端位置能差出十万八千里。所以每次搭完模型，我都会先给一个已知角度（比如q1=0, q2=0），手动算一下末端位置对不对。
模型搭好之后，你可以用Scope模块观察末端位置随关节角度的变化。我习惯再加一个MATLAB Animation模块，把机械臂的骨架画出来，这样看起来直观多了。调试的时候，看着动画里机械臂动，心里踏实。
嗯，这里再补充一点。如果你做的是六轴机器人，千万别手写六个矩阵乘法。用For循环子系统，把DH参数表存成一个数组，循环乘过去。代码量少，还不容易出错。我在做一款协作机器人时就是这么干的，后来换了一款机器人，只需要改DH参数表，模型几乎不用动。
最后，验证模型正确性的方法：给一组关节角度，用模型算出的末端位姿，跟实际机器人示教器上读出来的数据对比。误差在毫米级以内，就算过关。如果差得远，先检查DH参数，再检查矩阵乘法顺序。八成是这两个地方出问题。
好了，这一节就到这里。下一节咱们聊逆运动学——知道末端位置，怎么反推关节角度。那才是真正烧脑的地方。