3. 系统建模基础：运动学模型、动力学模型与状态空间表示
各位同学，欢迎来到第三章。这一章，咱们要啃一块硬骨头——系统建模。
说实话，我刚开始做机器人控制那会儿，觉得建模就是写写公式，没啥大不了的。直到有一次，我负责一个六轴机械臂的抓取项目，在仿真里跑得飞起，一上真机就抖得像帕金森患者。后来才发现，是动力学模型里少算了一个关节的摩擦项。嗯，从那以后，我再也不敢小看建模这一步了。
你想想看，控制算法再漂亮，如果模型是错的，那就是在沙子上盖大楼。所以，咱们今天把基础打牢。
3.1 运动学模型：正运动学与逆运动学
运动学，说白了就是研究“动”的几何关系，不考虑力。它解决两个问题：
- 正运动学：我知道每个关节转了多少角度，求末端执行器在哪儿。
- 逆运动学：我知道末端执行器要去哪儿，反推每个关节该转多少。
我个人习惯，先讲正运动学，因为它相对简单，是逆运动学的基础。
3.1.1 正运动学：从关节空间到操作空间
正运动学的核心工具是齐次变换矩阵。每个关节用一个4x4的矩阵表示，把相邻两个连杆的坐标变换关系描述清楚。
对于串联机器人，正运动学公式长这样：
T_0^n = T_0^1 * T_1^2 * ... * T_{n-1}^n其中，T_0^n 表示从基座到末端执行器的总变换矩阵。每个 T_{i-1}^i 由四个参数决定：连杆长度 a_i、连杆扭角 α_i、关节距离 d_i、关节转角 θ_i。这就是经典的Denavit-Hartenberg (D-H) 参数法。
核心要点：正运动学是唯一确定的。给定一组关节角度，末端位姿是唯一的。这一点在调试时非常有用——你可以用正运动学验证你的逆运动学算得对不对。
3.1.2 逆运动学：从操作空间到关节空间
逆运动学就麻烦多了。同一个末端位姿，可能对应多组关节角度解，也可能无解。
我在项目中遇到过最头疼的情况：一个六轴机器人，末端要走到某个点，逆运动学算出了8组解。选哪一组？
我的经验是：
- 优先选关节角度变化最小的解（省时间、省能量）
- 避开奇异位形（关节接近极限位置）
- 考虑避障需求
逆运动学的求解方法主要有两种：
- 解析法：通过几何关系直接推导出封闭解。速度快，但只适用于特定构型的机器人（比如后三个关节轴线交于一点的Pieper准则）。
- 数值法：用迭代方式逼近解。通用性强，但计算量大，且可能不收敛。
避坑指南：我曾经在项目里用数值法做逆运动学，结果迭代到奇异点附近，雅可比矩阵奇异，算法直接崩了。后来我加了阻尼最小二乘法（DLS），才稳住。记住：数值法一定要处理奇异情况。
3.2 动力学模型：拉格朗日法与牛顿-欧拉法
运动学只关心“动”，动力学关心“为什么动”——力和运动的关系。
为什么要建动力学模型？因为控制需要力。你让电机输出多大扭矩，才能让机器人按期望轨迹运动？这就是动力学要回答的问题。
3.2.1 拉格朗日法：从能量角度看问题
拉格朗日法基于能量守恒，公式简洁优雅：
L = K - P
τ = d/dt(∂L/∂q̇) - ∂L/∂q其中，L是拉格朗日函数，K是动能，P是势能，q是广义坐标，τ是广义力。
我个人很喜欢拉格朗日法，因为它不需要考虑内部约束力。你只需要写出系统的动能和势能，然后套公式就行。
举个例子，一个二连杆机械臂：
- 动能：两个连杆的平动动能 + 转动动能
- 势能：两个连杆的重力势能
- 代入拉格朗日方程，得到两个关节的力矩方程
小技巧：拉格朗日法推导出来的动力学方程，天然具有结构化的形式：M(q)q̈ + C(q,q̇)q̇ + G(q) = τ。其中M是惯性矩阵，C是科里奥利力和离心力项，G是重力项。这个形式对后续的控制设计非常友好。
3.2.2 牛顿-欧拉法：从力平衡角度看问题
牛顿-欧拉法更直观，它直接对每个连杆做受力分析：
- 牛顿方程：F = m * a（平动部分）
- 欧拉方程：τ = I * α + ω × (I * ω)（转动部分）
这个方法需要从基座到末端递推计算速度和加速度（正向递推），再从末端到基座递推计算力和力矩（反向递推）。
我记得有一次做高速抓取项目，拉格朗日法算出来的模型在低速时没问题，一上高速就偏差很大。换成牛顿-欧拉法，把每个关节的摩擦和惯性耦合都显式建模，效果就好多了。
两种方法的对比：
| 特性 | 拉格朗日法 | 牛顿-欧拉法 |
|---|---|---|
| 推导复杂度 | 随自由度增加呈指数增长 | 线性增长 |
| 计算效率 | 较低（需要计算大量偏导数） | 较高（递推计算） |
| 物理意义 | 能量角度，全局理解 | 力/力矩角度，局部理解 |
| 适用场景 | 自由度少（<4），理论研究 | 自由度多，实时控制 |
我的建议：做理论研究或自由度少的机器人，用拉格朗日法；做工程实现或自由度多的机器人，用牛顿-欧拉法。如果条件允许，两种方法互相验证，最稳妥。
3.3 状态空间表示：从微分方程到控制模型
运动学和动力学模型，最终都要转化成状态空间表示，才能用于控制设计。
状态空间表示的核心思想：用一组一阶微分方程描述系统动态。
ẋ = f(x, u)
y = g(x, u)其中，x是状态向量，u是输入向量，y是输出向量。
对于机器人系统，状态通常选为：
- 关节角度 q
- 关节角速度 q̇
所以状态向量 x = [q; q̇]。
动力学方程 M(q)q̈ + C(q,q̇)q̇ + G(q) = τ 可以改写为：
ẋ = [q̇; M(q)^{-1}(τ - C(q,q̇)q̇ - G(q))]这就是一个标准的非线性状态空间方程。
实用技巧：如果系统工作点附近变化不大，可以对状态空间方程做线性化处理，得到线性时不变系统：ẋ = Ax + Bu。这样就能用经典的PID、LQR等控制方法了。
为什么要用状态空间表示？因为：
- 统一框架：不管是单关节还是多关节，都能用同一套数学语言描述
- 便于分析：可控性、可观性、稳定性分析都有现成工具
- 便于设计：现代控制理论（LQR、MPC、滑模控制等）都基于状态空间
避坑指南：我曾经在项目里直接用非线性状态空间模型做MPC，结果计算量太大，控制器周期跟不上。后来改成在工作点附近线性化，用线性MPC，计算量降了两个数量级。所以，别盲目追求“精确模型”，要考虑实时性。
3.4 本章知识体系总览
为了让大家更直观地理解本章的知识结构，我画了一张图：
这张图把本章的三个核心模块串起来了。运动学是基础，提供几何关系；动力学是核心，提供力与运动的关系；状态空间是桥梁，把动力学模型转化成控制算法能用的形式。
好了，这一章的内容就到这里。建模是个慢功夫，别着急，多动手推导几次，自然就熟了。