微分方程与状态空间入门：从运动规律到状态传播

几何路径只说明“从哪里经过”，没有说明“以多快的速度经过、能不能及时减速”。规划一旦考虑这些运动限制，就需要描述位置和速度怎样随时间变化。微分方程与状态空间要回答的具体问题是：

已知现在的位置和速度，选择某个加速度后，过一小段时间会到哪里，速度又会变成多少？

下文始终从一维运动出发，先理解运动建模和状态传播，再联系到轨迹规划。

一、微分方程：描述运动怎样变化

1. 位置、速度、加速度之间的关系

用 $p(t)$、$v(t)$、$a(t)$ 分别表示时刻 $t$ 的位置、速度和加速度，它们的关系是：

$$v(t)=\frac{dp(t)}{dt},\qquad a(t)=\frac{dv(t)}{dt}.$$

变量上方的“点”表示对时间求导，因此也可以写成：

$$\boxed{\dot p=v,\qquad \dot v=a.}$$

速度描述位置变化得多快，加速度描述速度变化得多快。

例如，速度为 $2\,\mathrm{m/s}$，表示如果保持这个速度，一秒前进两米；加速度为 $1\,\mathrm{m/s^2}$，表示如果保持这个加速度，每经过一秒，速度就增加 $1\,\mathrm{m/s}$。

所以 $\ddot p=2$ 的意思就是“位置的二阶导数，也就是加速度，始终等于 2”。

2. 微分方程与初始条件

直接给出 $p(t)$，相当于告诉你整段运动的结果；给出 $\ddot p=2$，则只是告诉你运动需要遵守的变化规律。

微分方程的解是一个函数，而不是一个数。[1]

只有变化规律还不够。例如，同样以加速度 $2$ 运动，一个物体原本静止，另一个原本已经向前运动，接下来的位置就会不同。

设初始位置为 $p_0$，初始速度为 $v_0$。加速度保持为 $2$ 时：

$$v(t)=v_0+2t.$$

再对速度积分，就得到位置：

$$p(t)=p_0+v_0t+t^2.$$

例如给定 $p_0=2$、$v_0=3$，就得到 $p(t)=2+3t+t^2$。

这里不必把重点放在各种微分方程的求解技巧上，先理解这个分工：

运动规律规定“怎样变化”，初始条件规定“从哪里、以什么速度开始”。

从已知初始条件出发，按给定的运动规律和后续输入求未来运动，就是这里需要理解的初值问题。如果未来输入还没确定，就不能仅凭当前状态确定完整的未来轨迹。

二、状态：为什么不能只记录位置？

1. 同一个位置，可能对应不同的运动情况

假设两个物体都位于 $p=10\,\mathrm m$，但速度分别为：

$$v_1=0,\qquad v_2=5\,\mathrm{m/s}.$$

在理想无阻力模型中，如果之后加速度都为零，第一个物体会停在原地，第二个物体会继续前进。

因此，只知道位置，不能预测后续运动。

对于加速度受控的一维运动，需要把位置和速度一起记录：

$$\boxed{ \mathbf x= \begin{bmatrix} p\\v \end{bmatrix}. }$$

这个向量叫作状态向量。

可以把状态理解为一份“当前运动档案”：在模型和未来输入已知的前提下，它包含预测后续运动所需的信息。[2]

2. 状态空间不是实际的飞行空间

状态 $[p,v]^\mathsf T$ 可以看成一个二维平面上的点：横轴表示位置，纵轴表示速度。

例如，$(p,v)=(5,2)$ 表示“位于五米处，以每秒两米的速度运动”，而不是实际空间中的某个二维位置。

所以，物体虽然只沿一条直线运动，状态空间仍然可以是二维的。

这也解释了为什么考虑运动约束时，不能简单地把“位置相同”的两个候选状态视为完全相同：速度不同，后续能怎样运动也不同。

3. 状态与输入有什么区别？

在接下来使用的模型中：

$$\mathbf x= \begin{bmatrix} p\\v \end{bmatrix},\qquad u=a.$$

位置和速度是状态，加速度是可以选择的输入。

状态描述“现在是什么情况”，输入描述“接下来施加怎样的作用”。

这里的 $u$ 是简化模型中的净加速度，单位为 $\mathrm{m/s^2}$，不是电机转速或推力。把加速度当作输入，是一种建模选择，不意味着真实无人机能够瞬间实现任意加速度。

三、状态方程：把运动规律写成统一形式

1. 从一个二阶方程变成两个一阶方程

以加速度 $u$ 为输入时：

$$\ddot p=u.$$

引入速度 $v=\dot p$，就可以改写成：

$$\boxed{ \begin{cases} \dot p=v,\\ \dot v=u. \end{cases} }$$

第一行表示位置怎样变化，第二行表示速度怎样变化。

这只是把同一个运动规律换了一种写法，没有改变原来的模型，也没有作近似。[1]

把它们放到同一个向量里：

$$\dot{\mathbf x} = \begin{bmatrix} \dot p\\\dot v \end{bmatrix} = \begin{bmatrix} v\\u \end{bmatrix}.$$

这个模型叫作双积分器，因为从加速度到位置需要积分两次：先得到速度，再得到位置。

2. 怎样读懂 $\dot{\mathbf x}=A\mathbf x+Bu$？

同一组方程还可以写成：

$$\boxed{ \dot{\mathbf x} = \underbrace{\begin{bmatrix}0&1\\0&0\end{bmatrix}}_{A} \mathbf x + \underbrace{\begin{bmatrix}0\\1\end{bmatrix}}_{B}u. }$$

不要先背矩阵，直接展开：

$$A\mathbf x+Bu = \begin{bmatrix}0&1\\0&0\end{bmatrix} \begin{bmatrix}p\\v\end{bmatrix} + \begin{bmatrix}0\\1\end{bmatrix}u = \begin{bmatrix}v\\0\end{bmatrix} + \begin{bmatrix}0\\u\end{bmatrix} = \begin{bmatrix}v\\u\end{bmatrix}.$$

因此，$A\mathbf x$ 表示当前状态自身造成的变化：只要还有速度，位置就会改变。$Bu$ 表示输入造成的变化：加速度直接改变速度的变化率。[2]

最需要注意的是：

$A\mathbf x+Bu$ 算出的是“此刻怎样变化”，不是“下一时刻变成什么”。

这种形式叫作线性状态空间模型。“线性”描述的是方程对状态和输入的依赖方式，不是说运动轨迹只能是一条直线。

更一般的状态方程写作：

$$\dot{\mathbf x}=f(\mathbf x,\mathbf u,t).$$

这里的 $f$ 就是一条计算状态变化率的规则，可以包含非线性关系；它不是轨迹优化中的代价函数。

3. 三维情况只是把三个方向放到一起

把三维位置、速度和加速度输入分别记作 $\mathbf p$、$\mathbf v$、$\mathbf u$，仍然有：

$$\dot{\mathbf p}=\mathbf v,\qquad \dot{\mathbf v}=\mathbf u.$$

状态变成六个分量：

$$\mathbf x= \begin{bmatrix} p_x&p_y&p_z&v_x&v_y&v_z \end{bmatrix}^{\mathsf T}.$$

本质上，三个方向各自满足一组“位置由速度改变、速度由加速度改变”的关系。入门时先把一维模型弄清楚，不必急着展开六维矩阵。

四、状态传播：怎样算出下一时刻的状态？

1. 把时间分成一小段一小段

计算时，可以每隔 $h$ 秒记录一次状态：

$$t_0,t_1,t_2,\ldots,\qquad t_{k+1}=t_k+h.$$

记 $p_k=p(t_k)$、$v_k=v(t_k)$。这里的 $k$ 是时间步编号。

现在要解决的是：已知 $p_k,v_k$，在接下来的 $h$ 秒施加加速度 $u_k$，怎样算出 $p_{k+1},v_{k+1}$？

根据当前状态和输入计算之后的状态，叫作状态传播；建立这样的离散时间更新关系，是离散化要做的事情。

2. 先算速度，再算位置

先作一个关键假设：这段 $h$ 秒内，加速度保持为 $u_k$，中途不变。

速度变化量就是“加速度乘以时间”：

$$\boxed{v_{k+1}=v_k+hu_k.}$$

位置的计算还要考虑：这段时间内，速度在不断变化。

由于加速度恒定，速度从 $v_k$ 均匀地变化到 $v_{k+1}$，所以平均速度为：

$$\bar v =\frac{v_k+v_{k+1}}{2} =v_k+\frac12hu_k.$$

位移等于平均速度乘以时间，因此：

$$p_{k+1}-p_k =\bar v h =hv_k+\frac12h^2u_k.$$

最终得到：

$$\boxed{ \begin{aligned} p_{k+1}&=p_k+hv_k+\frac12h^2u_k,\\ v_{k+1}&=v_k+hu_k. \end{aligned} }$$

位置公式中的三项分别表示：原来的位置、原有速度带来的位移，以及这段时间内加速度额外带来的位移。

这组公式是全文最值得亲手计算的内容。

3. 一个数值例子

假设物体从原点静止出发，即 $p_k=0$、$v_k=0$。接下来 $0.1\,\mathrm s$ 内，加速度保持为 $2\,\mathrm{m/s^2}$，于是：

$$v_{k+1}=0+0.1\times2=0.2\,\mathrm{m/s},$$

$$p_{k+1}=0+0+\frac12\times0.1^2\times2=0.01\,\mathrm m.$$

虽然开始时速度为零，但它会在这段时间内加速，所以仍然产生了位移。

4. 离散状态方程的矩阵形式

把位置和速度更新放到一起：

$$\boxed{ \begin{bmatrix}p_{k+1}\\v_{k+1}\end{bmatrix} = \underbrace{\begin{bmatrix}1&h\\0&1\end{bmatrix}}_{A_d} \begin{bmatrix}p_k\\v_k\end{bmatrix} + \underbrace{\begin{bmatrix}\frac12h^2\\h\end{bmatrix}}_{B_d}u_k. }$$

也就是：

$$\mathbf x_{k+1}=A_d\mathbf x_k+B_du_k.$$

下标 $d$ 表示这是离散时间模型。与连续时间模型的区别是：$A\mathbf x+Bu$ 给出变化率，$A_d\mathbf x_k+B_du_k$ 直接给出下一步状态。

以上公式在双积分器模型、每段输入恒定的假设下是精确的；这里通过速度和位移的关系就完成了推导。

5. 欧拉法只需要先知道什么？

对更一般的状态方程，可以用当前变化率近似接下来一小段时间的变化：

$$\mathbf x(t_k+h) \approx \mathbf x(t_k)+h f(\mathbf x(t_k),\mathbf u_k,t_k).$$

这就是前向欧拉法的基本想法。[1]

对双积分器，它的位置更新只保留 $p_k+hv_k$，遗漏了 $\frac12h^2u_k$。所以上面的例子中，它会把下一时刻的位置近似为零，而不是 $0.01$ 米。

需要记住：离散化不一定意味着近似；但用欧拉法计算，一般会有数值误差。 对当前这个简单模型，直接使用已经推导出的精确更新公式即可。

五、这些公式怎样用于轨迹规划？

1. 规划不是只把位置连起来

已知初始状态和一串输入，可以反复使用更新公式，计算整段运动。

轨迹规划则反过来思考：为了到达目标，并满足速度、加速度和避障要求，应该选择怎样的一串输入？

例如“经过四米处”和“到四米处停下”是两个不同的目标：后者不仅要求 $p(T)=4$，还要求 $v(T)=0$。这就是为什么规划的起终条件经常要包含速度，而不仅仅是位置。

2. 完整例子：前进四米后停下

从 $p_0=0,v_0=0$ 出发，每段持续一秒，即 $h=1$。选择前两秒加速，后两秒减速：

$$u_0=1,\quad u_1=1,\quad u_2=-1,\quad u_3=-1.$$

每个输入都在对应的一秒内保持不变。使用前面的精确更新公式，就得到：

时间（秒）	位置（米）	速度（米/秒）
0	0	0
1	0.5	1
2	2	2
3	3.5	1
4	4	0

例如，从第二秒到第三秒，物体开始减速：

$$v_3=2+1\times(-1)=1,$$

$$p_3=2+1\times2+\frac12\times1^2\times(-1)=3.5.$$

减速不等于立刻停止。 这一秒内速度仍为正，所以物体仍然向前，只是前进得越来越慢。

这个例子构造了一条满足起终状态的运动，并没有进一步证明它在某种代价下最优。

3. 在搜索中，用输入生成下一段运动

在考虑运动约束的搜索中，可以从当前状态尝试几个不同的加速度。每个加速度都通过状态传播，产生一段候选运动和一个新的状态。

因此，扩展候选运动时，不只是问“附近哪个位置可以去”，还要问：

从当前的位置和速度出发，按照允许的加速度，实际能走出怎样的一段运动？

这样的短运动片段可以用作运动基元。搜索再检查它是否碰撞、是否超速，以及是否有助于到达目标。

这里先理解输入、运动片段和新状态的关系即可，不需要立即学习具体搜索算法。

4. 在优化中，运动规律就是约束

如果把各时刻的位置、速度和输入都交给优化器选择，相邻状态之间仍然必须满足：

$$p_{k+1}-p_k-hv_k-\frac12h^2u_k=0,$$

$$v_{k+1}-v_k-hu_k=0.$$

这些等式的意思是：下一步不能随便选，必须能从上一步按运动规律到达。 将动力学写成优化约束，是直接轨迹优化的一种基本思路。[4]

在一维模型中，还可以加入：

$$|v_k|\le v_{\max},\qquad |u_k|\le a_{\max},$$

分别限制速度和加速度。再结合起终状态与避障要求，就构成了规划问题的重要部分。

注意，两个端点都不碰撞，不代表连接它们的整段运动都不碰撞；碰撞检查需要关注中间经过的区域。

六、选读：简化模型与线性化

1. 简化模型不等于完整飞行动力学

双积分器把加速度直接当作输入，省略了姿态怎样变化、推力怎样产生等过程。它有助于理解运动与规划，但不能单独保证真实飞行器一定能够执行所得轨迹。[3]

前面的启停例子就有一个特点：加速度从 $+1$ 突然切换到 $-1$。如果还要限制加速度改变得多快，可以进一步把加速度也加入状态，并把它的变化率作为输入：

$$\mathbf x= \begin{bmatrix}p\\v\\a\end{bmatrix},\qquad j=\dot a.$$

$j$ 称为 jerk。此时，位置、速度和加速度都要连续地演化；限制 jerk，就能限制加速度变化的快慢。这对应后续会遇到的三积分器模型。

现阶段只需要理解：选择哪些状态、把什么当作输入，取决于准备保留哪些运动限制。

2. 线性化：在附近用简单关系作近似

真实运动模型可能包含非线性关系。线性化的思想是：在某个参考状态附近，用较简单的线性关系近似描述小变化。[3]

例如，在忽略阻力的平面模型中，约定正倾角使推力朝水平正方向倾斜，水平加速度满足：

$$a_x=\frac{F}{m}\sin\theta,$$

其中 $F$ 是总推力，$m$ 是质量，$\theta$ 是倾角。

为了只看倾角的影响，先把推力固定为悬停时的 $F=mg$，其中 $g$ 是重力加速度。于是 $a_x=g\sin\theta$。

当倾角很小、并用弧度表示时，$\sin\theta\approx\theta$，就有：

$$\boxed{a_x\approx g\theta.}$$

原来的三角函数关系，在水平悬停附近就近似成了“倾角越大，水平加速度按比例增大”。这只是说明局部近似的含义，并不是完整飞行模型的线性化推导。

线性化的近似只在参考状态附近适用。 以后需要处理更复杂的模型时，再学习怎样用 Jacobian 统一计算这些局部关系。

另外，线性化和离散化不是同一件事：前者简化状态与变化率之间的关系，后者建立跨越一个时间间隔的状态更新关系。

七、回顾：分清三个对象

对象	符号	回答的问题
当前状态	$\mathbf x$	现在在哪里，以什么速度运动？
状态变化率	$\dot{\mathbf x}$	当前位置和速度正在怎样变化？
下一时刻状态	$\mathbf x_{k+1}$	经过一段时间后，位置和速度变成多少？

本文的主线是：先用位置和速度组成状态，再用运动方程描述状态怎样变化，最后在每段输入恒定的假设下，推导出下一步状态的计算公式。

状态传播是“已知输入，计算运动”；规划则进一步寻找“怎样选择输入，才能得到满足目标和限制的运动”。

能读懂这个模型、解释各项含义，并用它算出一段加速或减速运动，就掌握了这一部分最直接的入门工具。 后续的搜索与优化，是在这些运动规律和约束之上寻找合适的运动方案。

参考资料

[1] Notes on Diffy Qs：微分方程导论、一阶系统与初值问题、数值求解方法。

[2] Stanford EE263：线性系统讲义：状态空间模型。本文只使用其中的状态与状态方程概念，不要求通读整份讲义。

[3] Underactuated Robotics：非线性运动模型与线性化。

[4] Underactuated Robotics：轨迹优化。