4、运动学与动力学建模：机器人运动学基础、无人机动力学模型、车辆自行车模型
各位同学，欢迎来到第四章。
前面几章我们聊了数值优化和凸优化，那些是「内功」。从这章开始，我们要把内功和「招式」结合起来。招式是什么？就是你要优化的对象——机器人、无人机、车辆——它们本身的运动规律。
说白了，你连这个家伙怎么动、能怎么动都不知道，你怎么规划轨迹？
这一章，我们就来搞定三样东西：机器人运动学、无人机动力学、车辆自行车模型。这三样东西，基本覆盖了地面移动机器人和空中机器人的主流建模需求。
核心观点：建模不是越精确越好，而是「够用就好」。你想想看，一个模型如果参数太多，优化起来慢得要死，而且容易陷入局部最优。我个人的习惯是：先建一个最简单的模型跑通流程，再逐步加细节。
4.1 机器人运动学基础
先聊机器人运动学。这里说的「运动学」，只关心位置、速度、加速度这些几何量，不关心力、力矩这些动力学量。
嗯，这里要注意：运动学是纯几何关系，和物理质量无关。
4.1.1 位姿表示
一个机器人在二维平面里，它的状态通常用三个量表示：(x, y, θ)。x和y是位置，θ是朝向角。
但在三维空间里，事情就复杂了。你需要一个旋转矩阵R和一个平移向量t，合起来叫SE(3)变换矩阵。
// 二维位姿的齐次变换矩阵
T = [cosθ -sinθ x]
[sinθ cosθ y]
[0 0 1]我在项目中遇到过一个问题：用欧拉角表示旋转时，出现了「万向锁」。当时调试了半天，发现机器人在某个姿态下突然少了一个自由度。后来我改用四元数，问题就解决了。所以，我建议：做轨迹优化时，尽量用四元数或旋转矩阵，别用欧拉角。
4.1.2 微分运动学
有了位姿，我们还要知道它怎么随时间变化。这就是微分运动学，也叫速度运动学。
对于差速轮机器人，它的运动学模型是这样的：
// 差速轮机器人运动学
dx/dt = v * cos(θ)
dy/dt = v * sin(θ)
dθ/dt = ω其中v是线速度，ω是角速度。这两个量就是你的控制输入。
个人经验：这个模型虽然简单，但非常实用。我在做仓储机器人项目时，就是用这个模型做路径跟踪。不过要注意，实际机器人有轮子打滑、轮胎变形等问题，模型会有偏差。所以优化时，我通常会加一个「模型不确定性」的鲁棒约束。
4.2 无人机动力学模型
无人机比地面机器人复杂一些。因为它有6个自由度（位置3个，姿态3个），但只有4个控制输入（四个电机的推力）。这是个典型的欠驱动系统。
4.2.1 四旋翼动力学方程
四旋翼的动力学模型，可以写成这样：
// 位置动力学
m * d²r/dt² = R * f_thrust + m * g
// 姿态动力学
I * dω/dt = τ - ω × (I * ω)这里：
m是无人机质量r是位置向量R是旋转矩阵f_thrust是总推力向量g是重力加速度I是惯性矩阵ω是角速度τ是力矩
为什么会这样？因为无人机不能直接控制水平位置，它必须通过倾斜机身来产生水平方向的推力分量。这就是欠驱动系统的特点。
避坑指南：我曾经在做一个无人机编队项目时，直接用了简化模型，忽略了电机响应延迟。结果实际飞行时，无人机在高速转弯时出现了严重的跟踪误差。后来我加了一阶低通滤波器来模拟电机响应，效果就好多了。所以，电机响应延迟不能忽略，尤其是做敏捷机动时。
4.2.2 常用简化模型
在实际的轨迹优化中，我们很少直接用完整的动力学模型。太复杂了，优化起来很慢。
常用的简化方式有两种：
- 微分平坦模型：把无人机的状态用位置、偏航角及其导数来表示。这样可以把复杂的动力学约束转化为代数约束。
- 点质量模型：忽略姿态动力学，只考虑位置和速度。适合做粗略的路径规划。
| 模型类型 | 精度 | 计算效率 | 适用场景 |
|---|---|---|---|
| 完整动力学 | 高 | 低 | 精确控制、敏捷机动 |
| 微分平坦 | 中 | 高 | 轨迹优化、路径规划 |
| 点质量 | 低 | 极高 | 粗略路径搜索 |
4.3 车辆自行车模型
最后聊车辆。汽车的运动学模型，最经典的就是自行车模型。
为什么叫自行车模型？因为它把车辆的两个前轮、两个后轮分别合并成一个轮子，就像自行车一样。这个模型简单，但能很好地描述车辆的运动特性。
4.3.1 运动学自行车模型
假设车辆在低速行驶，不考虑轮胎侧偏，那么运动学模型是：
// 自行车运动学模型
dx/dt = v * cos(θ)
dy/dt = v * sin(θ)
dθ/dt = v * tan(δ) / L其中：
v是后轴中心的速度δ是前轮转向角L是轴距
这个模型的关键是：转弯半径由转向角和轴距决定。你想想看，轴距越长，同样的转向角下转弯半径越大。这就是为什么大卡车转弯比小轿车费劲。
4.3.2 动力学自行车模型
如果车辆高速行驶，轮胎会有侧偏，这时候就要用动力学模型：
// 动力学自行车模型（简化版）
m * (dv/dt - v * ω) = F_f * cos(δ) + F_r
I * dω/dt = L_f * F_f * cos(δ) - L_r * F_r这里引入了轮胎力F_f和F_r，它们和轮胎侧偏角有关。
我的建议：做轨迹优化时，先用运动学模型。如果优化出来的轨迹在高速段表现不好，再换成动力学模型。不要一上来就用复杂的模型，那是给自己找麻烦。
4.4 本章知识体系
为了让大家更直观地理解这三类模型的关系，我画了一张图：
这张图展示了三类模型的核心内容和适用场景。你可以看到，它们之间是有联系的——车辆自行车模型其实是机器人运动学的一个特例，而无人机动力学则更复杂一些。
总结一下：
- 机器人运动学：基础中的基础，位姿表示和微分运动学必须熟练掌握
- 无人机动力学：欠驱动系统的典型代表，注意电机响应延迟
- 车辆自行车模型：低速用运动学，高速用动力学，别搞混了
我个人觉得，这一章的内容是后续所有轨迹优化算法的基础。你建模建得好，优化就成功了一半。
好了，这一章就到这里。下一章我们会聊「约束处理与代价函数设计」，到时候会用到今天讲的这些模型。