2、运动学基础（上）：空间坐标系与齐次变换、Delta机器人正运动学推导（几何法）
各位同学，欢迎来到第二章。这一章，我们得把数学底子打牢。
说实话，搞机器人控制，最怕的就是「数学恐惧症」。一看到矩阵、坐标系就头疼。我当年刚入行时也是这样，总觉得这些理论离实际很远。直到有一次，我在调试一台Delta机器人时，因为坐标系定义错了，结果机器人直接撞上了工作台……嗯，从那以后，我再也不敢轻视运动学了。
2.1 空间坐标系：机器人的「世界观」
先问大家一个问题：你让机器人「往右走10厘米」，它怎么知道哪边是「右」？
答案很简单——它需要一个参考系。这个参考系，就是空间坐标系。
2.1.1 坐标系的基本概念
我个人习惯把坐标系想象成一把「尺子」。这把尺子有三个方向：X、Y、Z。在机器人领域，我们最常用的是右手坐标系。怎么判断？伸出你的右手，拇指指向X轴，食指指向Y轴，中指指向Z轴——对，就是这样。
重要概念：Delta机器人通常有两个坐标系：
- 基坐标系：固定在机器人底座上，是全局参考系
- 动坐标系：固定在末端执行器上，随动平台移动
说白了，基坐标系是「世界地图」，动坐标系是「你手里的指南针」。
2.1.2 坐标变换：从A点到B点
假设你站在北京（基坐标系），想知道上海（动坐标系）的位置。你需要知道两件事：
- 平移：北京到上海的距离和方向
- 旋转：你面朝的方向和上海的方向之间的夹角
在机器人里，我们用4×4的齐次变换矩阵来描述这种关系。为什么是4×4？因为3×3的旋转矩阵只能处理旋转，加上平移就麻烦了。齐次变换矩阵把旋转和平移打包在一起，一次搞定。
我的小技巧：写代码时，我习惯把齐次变换矩阵写成这样：
// 齐次变换矩阵结构体
typedef struct {
double R[3][3]; // 旋转矩阵
double t[3]; // 平移向量
double last_row[4] = {0, 0, 0, 1}; // 最后一行固定
} HomogeneousMatrix;记住，最后一行永远是[0, 0, 0, 1]。这个规律能帮你快速检查矩阵写没写错。
2.2 齐次变换：让计算变得优雅
齐次变换，说白了就是「把旋转和平移塞进一个矩阵里」。为什么这么做？因为连续变换时，矩阵乘法比单独算旋转再算平移方便得多。
2.2.1 旋转矩阵
绕X轴旋转θ角，旋转矩阵长这样：
R_x(θ) = [1, 0, 0;
0, cos(θ), -sin(θ);
0, sin(θ), cos(θ)]绕Y轴和Z轴类似，只是把sin和cos的位置换一下。我刚开始学的时候，总是记混。后来发现一个规律：绕哪个轴转，那个轴对应的行和列就是[1,0,0]或[0,1,0]或[0,0,1]，其他位置填sin和cos。
2.2.2 齐次变换矩阵的构建
把旋转矩阵R和平移向量t组合起来：
T = [R, t;
0, 1]举个例子，如果动坐标系相对于基坐标系平移了(1, 2, 3)，并且绕Z轴旋转了30°：
T = [cos30°, -sin30°, 0, 1;
sin30°, cos30°, 0, 2;
0, 0, 1, 3;
0, 0, 0, 1]避坑指南：我曾经犯过一个低级错误——把平移向量写到了旋转矩阵里。结果机器人末端位置完全不对，查了半天才发现是矩阵索引搞错了。记住：平移向量在第四列，前三列是旋转。
2.3 Delta机器人正运动学推导（几何法）
好了，理论铺垫够了。现在我们来点真格的——推导Delta机器人的正运动学。
Delta机器人有三个并联的臂，每个臂由主动臂和从动臂组成。正运动学就是：已知三个主动臂的角度，求末端执行器的位置。
2.3.1 几何模型简化
Delta机器人的结构其实很巧妙。每个臂的从动臂是一个平行四边形，这保证了末端执行器始终与基座平行。所以，我们只需要关心位置，不用管姿态。
我把每个臂简化成一个「虚拟杆」：
- 主动臂长度：L1
- 从动臂长度：L2
- 基座半径：R
- 动平台半径：r
2.3.2 推导步骤
正运动学的核心是解一个三元二次方程组。我习惯用几何法，因为直观。
第一步：把三个主动臂的关节角θ1、θ2、θ3转换成三个球铰点的坐标。
对于第i个臂，球铰点坐标是：
P_i = [R*cos(φ_i) + L1*cos(θ_i)*cos(φ_i);
R*sin(φ_i) + L1*cos(θ_i)*sin(φ_i);
L1*sin(θ_i)]其中φ_i是第i个臂在基座上的安装角度（0°、120°、240°）。
第二步：三个球铰点到末端执行器的距离都是L2。所以：
|P_1 - E| = L2
|P_2 - E| = L2
|P_3 - E| = L2其中E = [x, y, z]是末端执行器的坐标。
第三步：解这个方程组。三个方程，三个未知数，理论上可解。但直接解很麻烦，我通常用数值方法（比如牛顿-拉夫森法）来迭代求解。
实际项目中的经验：有一次，我在现场调试时发现正运动学算出来的位置和实际位置差了5mm。查了半天，发现是主动臂长度L1标定错了。Delta机器人的精度对杆长非常敏感，建议每次换臂后都重新标定一下。
2.3.3 代码实现示例
下面是我常用的正运动学求解函数（简化版）：
// Delta机器人正运动学（几何法）
// 输入：三个主动臂角度 theta1, theta2, theta3
// 输出：末端执行器位置 x, y, z
void delta_forward_kinematics(double theta1, double theta2, double theta3,
double *x, double *y, double *z) {
// 1. 计算三个球铰点坐标
double P1[3], P2[3], P3[3];
compute_sphere_joint(theta1, 0, P1); // 第一个臂，安装角0°
compute_sphere_joint(theta2, 120, P2); // 第二个臂，安装角120°
compute_sphere_joint(theta3, 240, P3); // 第三个臂，安装角240°
// 2. 用数值方法求解末端位置
// 初始猜测：取三个球铰点的中心
double E[3] = {(P1[0]+P2[0]+P3[0])/3,
(P1[1]+P2[1]+P3[1])/3,
(P1[2]+P2[2]+P3[2])/3};
// 牛顿-拉夫森迭代
for (int i = 0; i < 10; i++) {
// 计算雅可比矩阵和残差
// ...（省略具体实现）
// 更新E
}
*x = E[0];
*y = E[1];
*z = E[2];
}我的建议：实际项目中，不要每次都从头迭代求解。可以先用离线方式算好一组角度到位置的映射表，运行时直接查表+插值。这样能大大减少计算时间，尤其适合低成本控制器。
2.4 本章知识体系
为了让大家更直观地理解本章的知识结构，我画了一张图：
这张图把本章的三个核心模块串起来了。你想想看，从空间坐标系到齐次变换，再到正运动学推导，其实是一个层层递进的关系。坐标系是基础，齐次变换是工具，正运动学是应用。
本章要点回顾：
- 空间坐标系是机器人的「世界观」，Delta机器人需要基坐标系和动坐标系
- 齐次变换矩阵把旋转和平移统一起来，4×4矩阵是标准形式
- 正运动学用几何法求解，核心是解三元二次方程组
- 实际项目中，杆长标定和计算效率是两大关键点
好了，这一章的内容就到这里。数学公式看着可能有点枯燥，但相信我，这些基础打牢了，后面讲逆运动学、轨迹规划时你会轻松很多。我当年就是吃了基础不牢的亏，后来花了大把时间补课。你们现在跟着学，少走弯路。
公众号:蓝海资料掘金营，微信deep3321