4. 数学模型建立：运动学模型、动力学模型、驱动与执行器模型、传感器模型

做运动控制仿真，最核心的一步就是建数学模型。说白了，就是把物理世界的运动规律，翻译成计算机能算的数学公式。我这些年踩过的坑，十有八九都出在这个环节——模型建得不对，后面仿真跑得再花哨也是白搭。

4.1 运动学模型

运动学模型只关心“怎么动”，不关心“为什么动”。它描述的是位置、速度、加速度之间的关系，跟力、力矩这些无关。

我个人习惯把运动学模型分成两类：

• 正运动学：已知关节角度，求末端位姿。比如六轴机器人，给你六个关节角，算出末端在空间的位置和姿态。
• 逆运动学：已知末端位姿，求关节角度。这个更常用，但也更头疼——解不唯一，甚至可能无解。

举个例子，一个简单的二连杆机构：

// 二连杆正运动学
x = L1 * cos(q1) + L2 * cos(q1 + q2);
y = L1 * sin(q1) + L2 * sin(q1 + q2);

嗯，这里要注意：运动学模型里，时间不是直接变量。我们通过微分关系，把速度、加速度映射到关节空间和任务空间。雅可比矩阵就是干这个活的。

4.2 动力学模型

动力学模型比运动学复杂一个量级。它要回答：给多大的力，能产生多大的加速度？

标准形式是：

M(q) * q̈ + C(q, q̇) * q̇ + G(q) = τ

其中：

• M(q)：惯性矩阵，跟位姿有关
• C(q, q̇)：科里奥利力和离心力项
• G(q)：重力项
• τ：驱动力矩

你想想看，这个方程看着简单，但实际推导起来特别繁琐。尤其是多轴系统，M矩阵里全是耦合项。

我建议用拉格朗日法或者牛顿-欧拉法来推导。拉格朗日法适合手算，牛顿-欧拉法适合编程实现。

4.3 驱动与执行器模型

驱动模型把“控制指令”变成“实际力/力矩”。常见的驱动方式有：

驱动类型	典型模型	关键参数
直流电机	一阶惯性 + 反电动势	电阻、电感、转矩常数
伺服电机	二阶系统 + 电流环	带宽、阻尼比、响应时间
液压缸	非线性流量方程	阀口面积、油液弹性模量
气动执行器	可压缩流体模型	气缸容积、气体常数

驱动模型里最容易忽略的是“饱和”和“死区”。电机有最大扭矩，液压阀有最大开口。这些非线性特性，在仿真里必须体现。

我一般这样建模：

// 直流电机简化模型
function motorModel(u, omega)
    // u: 输入电压, omega: 当前转速
    tau = Kt * (u - Ke * omega) / R;
    // 限幅
    if tau > tau_max then tau = tau_max;
    if tau < -tau_max then tau = -tau_max;
    return tau;
end

4.4 传感器模型

传感器模型负责把“真实物理量”变成“测量值”。这里的关键是：传感器永远有误差。

常见的传感器模型包括：

• 编码器：位置测量，有量化误差和零位偏移
• 陀螺仪：角速度测量，有漂移和噪声
• 加速度计：线加速度测量，有偏置和振动噪声
• 力传感器：接触力测量，有温漂和迟滞

我常用的传感器建模套路：

// 带噪声的编码器模型
function encoderModel(truePos)
    // 量化误差：分辨率0.001度
    quantized = round(truePos / 0.001) * 0.001;
    // 零位偏移：固定偏差
    offset = 0.005;  // 单位：度
    // 随机噪声
    noise = randn() * 0.002;
    return quantized + offset + noise;
end

为什么会这样？因为真实传感器就是这样工作的。你想想看，编码器输出的是脉冲数，不是连续角度。陀螺仪数据里永远有零漂。这些细节不建模，仿真结果就跟实际对不上。

4.5 模型集成与验证

四个模型建好之后，怎么把它们拼起来？我的做法是：

1. 先独立验证：每个子模型单独测试，看输入输出是否合理
2. 再开环联调：驱动→执行器→运动学→传感器，信号流走一遍
3. 最后闭环验证：加上控制器，看整个系统能不能稳定

这里有个关键点：模型集成时，信号类型和单位必须统一。我见过最离谱的bug，是角度用了弧度，但控制器里当成了度——结果系统直接飞了。

下面这张图是我常用的模型结构，你可以参考：

这张图展示了信号在四个模型之间的流向。控制器发出指令，经过驱动、执行器，变成实际的力。动力学模型算出加速度，运动学模型算出位置和速度。传感器再把测量值反馈给控制器——闭环就形成了。