第三章 运动学基础与正逆解
各位同学，今天我们来聊聊机器人运动学。说实话，这是整个机器人控制里最绕不开的基础。你想想看，要让机器人动起来，总得知道它该往哪走吧？这就是运动学要解决的问题。
我个人习惯把运动学分成两半来看：正运动学和逆运动学。正运动学，就是我知道每个关节转了多少度，然后算出末端在哪儿。逆运动学正好反过来——我知道末端要去的位置，反推每个关节该转多少。嗯，听起来挺对称的，对吧？
正运动学推导
正运动学其实没那么玄乎。说白了，就是一堆坐标变换的叠加。我刚开始做机器人时，总觉得这东西很抽象，后来发现它就是「从基座开始，一级一级往末端算」的过程。
拿一个六轴工业机器人举例。我们从基座开始，给每个关节建一个坐标系。然后呢？用齐次变换矩阵把相邻两个坐标系的关系描述出来。最后把这些矩阵乘起来，就得到了末端相对于基座的位姿。
核心公式：
T_06 = T_01 × T_12 × T_23 × T_34 × T_45 × T_56
其中 T_ij 表示从关节 i 到关节 j 的变换矩阵。
这里有个坑，我当年踩过。DH参数的定义方式有好几种，标准DH和改进DH容易搞混。标准DH是把坐标系建在连杆的前端，改进DH建在后端。你一旦混用，算出来的结果全错。
注意：用DH参数时，一定要先确认你用的是哪种约定。我建议团队内部统一用一种，别换来换去。
逆运动学数值解法（IK）
正运动学算起来很直接，但逆运动学就麻烦多了。为什么呢？因为同一个末端位置，可能对应好几组关节角度。甚至有些位置根本到不了。
逆运动学有解析解和数值解两种方法。解析解快，但不是所有机器人都有。数值解通用，但慢一些。我个人在实际项目中，大部分时候都用数值解法，尤其是处理非标准构型的机器人时。
数值解法的思路其实很简单：先猜一组关节角度，算一下末端位置，看看跟目标差多少。然后根据这个误差，调整关节角度。反复迭代，直到误差小到可以接受。
最常用的方法是牛顿-拉夫森法。它的核心是雅可比矩阵——这个矩阵描述了关节速度与末端速度之间的关系。
// 伪代码：牛顿-拉夫森法求解IK
function inverseKinematics(targetPose, initialGuess) {
q = initialGuess; // 初始猜测
for (i = 0; i < maxIterations; i++) {
currentPose = forwardKinematics(q);
error = targetPose - currentPose;
if (norm(error) < tolerance) break;
J = computeJacobian(q); // 计算雅可比矩阵
delta_q = J_inv * error; // 求解关节增量
q = q + delta_q;
}
return q;
}经验之谈：我曾经遇到一个情况，IK怎么迭代都不收敛。后来发现是初始猜测离目标太远了。解决办法很简单——先用正运动学算几个中间点，分段求解。这招很管用。
在仿真中验证运动学
理论推导完了，代码写好了，接下来干嘛？验证啊！我见过太多人直接拿真机跑，结果撞了。仿真验证这一步，千万别省。
我个人习惯用这样的验证流程：
- 正运动学验证：给一组关节角度，算末端位置。然后在仿真环境里手动把关节调到这些角度，看看末端是不是在同一个位置。
- 逆运动学验证：在仿真里点一个目标位置，用IK算出关节角度，再驱动仿真模型过去。看看末端是不是真的到了目标点。
- 轨迹连续性验证：让机器人走一条连续轨迹，观察关节角度是否平滑变化。如果有突变，说明IK可能跳到了不同的解分支。
这里我画了一张图，帮你理清运动学验证的整体逻辑：
你看这个流程，其实就是一个闭环。正运动学算完了，用逆运动学反算回来，再到仿真里验证。三个环节互相印证，缺一不可。
验证小技巧：我一般会在仿真里画一条直线轨迹，让机器人末端沿着走。然后记录实际轨迹和理论轨迹的偏差。如果偏差在1毫米以内，说明运动学模型基本靠谱。如果偏差很大，多半是DH参数搞错了。
说到仿真工具，我个人比较喜欢用CoppeliaSim（以前叫V-REP）。它内置了运动学求解器，可以快速验证。不过要注意，仿真器的物理引擎有时候会引入额外的误差，尤其是关节有摩擦或者柔性的时候。所以仿真验证通过后，最好再用纯运动学仿真（不带物理）确认一遍。
嗯，这里还要提一个常见的坑。有些同学在仿真里验证IK时，发现末端位置对了，但姿态不对。为什么会这样？因为IK求解时，位置和姿态是耦合的。你只约束了位置，没约束姿态，那机器人就会随便选一个姿态来满足位置要求。解决办法很简单——在IK目标里同时指定位置和姿态。
避坑指南：我曾经在调试一个七轴机器人时，IK总是收敛到奇异点附近。后来发现是雅可比矩阵的条件数太大了。解决办法是加一个阻尼项，也就是用阻尼最小二乘法（DLS）代替纯牛顿法。虽然收敛慢一点，但稳定多了。
最后总结一下。运动学是机器人控制的地基。正运动学让你理解机器人的几何关系，逆运动学让你能控制机器人到达目标位置。而仿真验证，就是检验你理解得对不对的试金石。这三步走扎实了，后面做轨迹规划、动力学控制才会顺手。
好了，这一章的内容就到这里。记住，多动手算一算，多到仿真里跑一跑。光看书是学不会运动学的。