第一讲：轨迹跟踪基础——运动学模型、坐标系与轨迹表示
各位同学，欢迎来到《复杂轨迹跟踪与精准控制实战课程》。
我是你们这门课的主讲。做机器人控制这行，掐指一算也有十几年了。从最早的工业机械臂，到后来折腾移动底盘，再到现在的复合机器人，我踩过的坑，说实话，比你们走过的路可能还多那么几条（笑）。
今天咱们开篇，不讲虚的。直接聊最核心的东西：轨迹跟踪的基础。
你想想看，不管你是想让机械臂画个圆，还是让AGV小车走个S弯，本质上都是在干同一件事——让机器人的实际运动，去逼近一条我们事先规划好的理想路径。
那怎么逼近？第一步，你得先搞清楚三件事：
- 机器人怎么动？（运动学模型）
- 它在哪？目标在哪？（坐标系变换）
- 那条理想路径长什么样？（轨迹表示方法）
这三件事，就是今天这堂课的全部内容。也是整个课程的基石。
核心观点： 轨迹跟踪的本质，是一个“误差调节”问题。而误差的定义，完全依赖于运动学模型和坐标系。模型错了，坐标系乱了，后面控制得再好也是白搭。
1.1 运动学模型：给机器人“立法”
什么叫运动学模型？说白了，就是描述机器人“怎么动”的数学规则。它不关心力，不关心扭矩，只关心位置、速度和几何约束。
我个人习惯，把运动学模型分成两类：
- 非完整约束模型（比如差速轮小车、汽车）
- 完整约束模型（比如全向轮、多关节机械臂）
举个例子。差速轮小车，它不能横着走。你给它一个向左的速度指令，它必须先转弯，再前进。这就是非完整约束。我在做仓储AGV项目时，一开始没注意这个约束，直接用全向轮的算法去控差速底盘，结果小车在原地疯狂打转……嗯，那场面，挺尴尬的。
对于差速轮，运动学模型长这样：
// 差速轮运动学模型（简化版）
// 输入：线速度 v，角速度 w
// 输出：状态量 [x, y, theta] 的导数
dx/dt = v * cos(theta)
dy/dt = v * sin(theta)
dtheta/dt = w你看，状态量是 [x, y, theta]，但控制量只有 [v, w]。三个状态，两个控制，这就是非完整约束的典型特征。
我的经验： 建立运动学模型时，一定要先问自己三个问题：
- 机器人有几个自由度？
- 有几个独立的控制输入？
- 是否存在不可积分的微分约束？
这三个问题想清楚了，模型基本不会错。
1.2 坐标系变换：搞清楚“我在哪，目标在哪”
轨迹跟踪，本质上是在不同坐标系之间来回穿梭。
你想想看：
- 传感器数据（激光雷达、摄像头）是在传感器坐标系下
- 机器人本体是在机体坐标系下
- 规划好的轨迹是在世界坐标系下
这三个坐标系如果不统一，你连误差都算不出来。
我常用的工具是齐次变换矩阵。它用一个4x4的矩阵，同时表达了旋转和平移。
// 齐次变换矩阵 T 的结构
// T = [ R t ]
// [ 0 1 ]
// 其中 R 是 3x3 旋转矩阵，t 是 3x1 平移向量
// 例如：将点 p_body 从机体坐标系变换到世界坐标系
p_world = T_body_to_world * p_body这里有个坑，我必须要提醒你们。
我曾经踩过的坑： 旋转顺序！欧拉角的旋转顺序（ZYX、XYZ等）不同，结果完全不同。我在一个项目里，用了ZYX顺序的欧拉角，但库函数默认是XYZ顺序，结果机械臂末端位置差了十几厘米。查了整整两天才找到原因。
所以，我现在的习惯是：能用四元数，尽量不用欧拉角。四元数虽然难理解一点，但至少没有万向锁问题，也不存在顺序歧义。
1.3 轨迹表示方法：给机器人画一条“理想路线”
轨迹，就是机器人要跟随的那条理想路径，再加上时间信息。
常见的表示方法有三种：
| 方法 | 优点 | 缺点 | 典型应用 |
|---|---|---|---|
| 多项式 | 简单、计算快、导数易求 | 高阶时容易震荡（Runge现象） | 点到点运动、简单路径 |
| 样条曲线（B样条） | 局部可控、平滑、连续性好 | 实现稍复杂 | 复杂路径、高速轨迹 |
| 贝塞尔曲线 | 直观、凸包性质好 | 全局控制、修改一点影响全局 | 图形设计、简单路径 |
我个人最常用的是三次样条。为什么？因为它能保证位置、速度、加速度都连续（C2连续）。对于大多数机器人控制场景，这就够了。
来看一个三次多项式的例子：
// 三次多项式轨迹：x(t) = a0 + a1*t + a2*t^2 + a3*t^3
// 给定起点和终点的位置、速度约束，求解系数
// 假设：
// t=0 时，x=0, v=0
// t=T 时，x=1, v=0
// 求解方程组：
// a0 = 0
// a1 = 0
// a0 + a1*T + a2*T^2 + a3*T^3 = 1
// a1 + 2*a2*T + 3*a3*T^2 = 0
// 解得：
// a2 = 3/T^2
// a3 = -2/T^3关键点： 轨迹的“平滑性”直接影响跟踪精度。如果轨迹本身加速度不连续，机器人就会产生冲击，导致跟踪误差突然增大。我见过太多人只关注位置连续，忽略了速度和加速度连续，结果实际跑起来抖得不行。
1.4 三者如何协同工作？
好，现在我们把三块内容串起来。
轨迹跟踪的完整流程是这样的：
- 规划层：在世界坐标系下，用样条曲线生成一条理想轨迹
p_d(t) - 感知层：通过传感器，获取机器人当前位姿
p_actual（通常在机体坐标系下） - 变换层：用齐次变换矩阵，把
p_actual转换到世界坐标系下 - 误差计算：计算
e(t) = p_d(t) - p_actual(t) - 控制层：根据误差
e(t)和运动学模型，计算控制量（比如v和w）
你看，每一步都离不开我们今天讲的内容。
一个小技巧： 在实际工程中，我通常会在误差计算这一步，把误差从世界坐标系转换到机体坐标系。为什么？因为控制指令最终是在机体坐标系下执行的。这样做的好处是，控制律设计更直观，物理意义更清晰。
好了，今天的内容就到这里。轨迹跟踪的基础，说白了就是这三板斧：模型、坐标系、轨迹表示。把这三点吃透了，后面的控制算法才能有的放矢。
记住，做控制，不要急着调参数。先把模型建对，把坐标系理清，把轨迹表示选好。基础打牢了，后面事半功倍。