从轨迹到推力与姿态：一步一步算出无人机怎样倾斜

一架无人机沿水平通道前进 10 米，第 4 秒经过 4 米处，第 10 秒到达终点并停下。轨迹已经给出了每个时刻的位置、速度和加速度。

要执行这份运动安排，还需要确定：向前加速时倾斜多少，减速时朝哪个方向倾斜，以及旋翼一共需要提供多大的推力。

多旋翼动力学把轨迹中的加速度，转化为推力大小和方向；推力方向又决定机体需要怎样倾斜。

下面先算一次简单的水平加速，再把计算应用到整条 10 米轨迹。

一、先看悬停：推力怎样抵消重力？

1. 建立坐标和计算模型

以通道前进方向为世界坐标系的 $x$ 轴，以竖直向上为 $z$ 轴。无人机沿通道飞行，机头的水平朝向保持不变。

本例采用以下模型与参数：

项目	设置
无人机质量 $m$	1 千克
重力加速度大小 $g$	$9.8\,\mathrm{m/s^2}$
水平位置	$x(t)=p(t)$ ，沿通道由 0 米增加到 10 米
飞行高度	保持不变，因此竖直速度、竖直加速度均为 0
旋翼推力	各旋翼桨轴相对机体固定，合推力沿机体上轴
外力模型	考虑重力和旋翼推力，忽略空气阻力与风

用 $F$ 表示所有旋翼产生的总推力大小，单位为牛顿，记作 N。力、质量与加速度的关系是：

$\text{合力}=\text{质量}\times\text{加速度}.$

例如，1 千克物体受到 2 N 的水平合力，就产生 $2\,\mathrm{m/s^2}$ 的水平加速度。

2. 保持静止需要多大推力？

重力大小为：

$mg=1\times9.8=9.8\,\mathrm N,$

方向竖直向下。悬停时加速度为零，向上的推力与向下的重力平衡，因此：

$\boxed{F=mg=9.8\,\mathrm N}.$

零加速度对应合力为零。旋翼仍然需要持续提供抵消重力的推力。

二、向前加速：为什么需要倾斜机体？

1. 先算水平、竖直两个方向需要的力

假设某个时刻要求：

$a_x=2\,\mathrm{m/s^2},\qquad a_z=0.$

也就是向前加速，同时保持高度。

水平合力需要达到：

$F_x=ma_x=1\times2=2\,\mathrm N.$

竖直方向还需要抵消重力，因此推力的竖直分量为：

$F_z=mg=9.8\,\mathrm N.$

也就是说，推力需要一个向前 2 N 的水平分量来产生水平加速度，还需要一个向上 9.8 N 的竖直分量来抵消重力。旋翼合推力沿机体上轴。要同时提供这两个分量，就让机体上轴朝前倾斜，使推力指向“向前且向上”的方向。

2. 用一个直角三角形算总推力

水平分量和竖直分量互相垂直，合起来就是总推力。根据勾股关系：

$F=\sqrt{F_x^2+F_z^2}.$

代入：

$\boxed{ F=\sqrt{2^2+9.8^2} =\sqrt{100.04} \approx10.002\,\mathrm N. }$

推力分解：水平分量为 2 N，竖直分量为 9.8 N，合成后的总推力约为 10.002 N

图中的水平、竖直箭头，是同一个推力的两个分量。斜向箭头表示它们合成后的总推力。

3. 机体需要倾斜多少？

用 $\theta$ 表示推力方向相对竖直向上的有符号倾角：朝 $+x$ 方向倾斜为正，朝 $-x$ 方向倾斜为负。

直角三角形给出：

$\tan\theta=\frac{F_x}{F_z}=\frac2{9.8}.$

求出这个比值对应的角度：

$\boxed{\theta=\arctan\left(\frac2{9.8}\right)\approx11.53^\circ}.$

$\arctan$ 表示反正切：寻找一个角度，使它的正切等于括号里的数。这里将结果换算成了角度。

于是，“水平加速度 2 米/秒²、保持高度”这一运动要求，变成了“推力相对竖直向前倾斜约 11.53°，总推力约 10.002 N”。可以反过来核对。倾斜后的推力分量为：

$F_x=F\sin\theta\approx2\,\mathrm N,$

$F_z=F\cos\theta\approx9.8\,\mathrm N.$

水平分量负责加速，竖直分量负责维持高度。倾斜角度和总推力需要一起安排。

三、减速与匀速：倾斜方向由什么决定？

1. 仍然向前运动，同时朝后倾斜

假设无人机正以 $1.5\,\mathrm{m/s}$ 向前运动，需要以 $-1\,\mathrm{m/s^2}$ 的加速度减速，并保持高度。

水平推力变为：

$F_x=1\times(-1)=-1\,\mathrm N.$

负号表示朝通道后方。竖直分量仍为 9.8 N，因此：

$F=\sqrt{(-1)^2+9.8^2}\approx9.851\,\mathrm N,$

$\theta=\arctan\left(\frac{-1}{9.8}\right)\approx-5.83^\circ.$

这一时刻，速度朝前，无人机仍在向终点前进；水平推力朝后，让向前的速度逐渐减小。速度描述当前怎样运动，加速度描述速度怎样变化。本例中的倾斜方向，跟随水平加速度的方向。

2. 保持水平匀速时怎样安排？

在本例忽略阻力的模型中，水平匀速对应 $a_x=0$ ，维持高度对应 $a_z=0$ 。

因此推力竖直向上，大小为 9.8 N。若加入空气阻力，匀速飞行还需要产生抵消阻力的水平分量。

将三种情况放在一起：

等高运动要求	水平加速度	有符号倾角	总推力
向前加速	$+2\,\mathrm{m/s^2}$	$+11.53^\circ$	约 10.002 N
保持水平匀速	0	$0^\circ$	9.800 N
向前运动中减速	$-1\,\mathrm{m/s^2}$	$-5.83^\circ$	约 9.851 N

3. 写成可以逐时刻使用的公式

对任意水平加速度 $a_x(t)$ ，等高运动所需的两个量为：

$\boxed{F(t)=m\sqrt{g^2+a_x(t)^2}},$

$\boxed{\theta(t)=\arctan\left(\frac{a_x(t)}g\right)}.$

质量增加时，同样的加速度要求会对应更大的推力；在这个模型中，所需倾角由 $a_x/g$ 的比值决定。

四、把计算应用到整条 10 米轨迹

1. 从位置函数得到加速度

继续使用两段七次多项式参考轨迹。第一段持续 4 秒，第二段持续 6 秒；通过中间航点时，速度为 1.9 米/秒，加速度为 $7/90$ 米/秒²。

用于复算的位置公式为：

$P_1(s)=\frac{835}{27}s^4-\frac{2419}{45}s^5 +\frac{527}{15}s^6-\frac{1121}{135}s^7,$

$\begin{aligned} P_2(s)&=4+\frac{57}{5}s+\frac75s^2-\frac{49}{8}s^3 -\frac{15}{2}s^4\\ &\quad+\frac14s^5+\frac{131}{10}s^6-\frac{261}{40}s^7. \end{aligned}$

第一段使用 $s=t/4$ ，第二段使用 $s=(t-4)/6$ 。两式中的 $s$ 都表示各自的时间进度。

对位置求两次时间导数，就得到水平加速度：

$a_x(t)= \begin{cases} P_1''(t/4)/4^2,&0\le t\le4,\\[4pt] P_2''((t-4)/6)/6^2,&4<t\le10. \end{cases}$

撇号表示对归一化时间求导。这里继续使用前面计算轨迹导数时的时间换算规则。

2. 先计算第 2 秒的推力与倾角

第 2 秒位于第一段， $s=2/4=0.5$ 。由第一段曲线得到：

$a_x(2)=\frac{P_1''(0.5)}{16}\approx0.83533\,\mathrm{m/s^2}.$

因此：

$F_x=1\times0.83533=0.83533\,\mathrm N, \qquad F_z=9.8\,\mathrm N.$

总推力为：

$F(2)=\sqrt{0.83533^2+9.8^2}\approx9.8355\,\mathrm N.$

倾角为：

$\theta(2)=\arctan\left(\frac{0.83533}{9.8}\right) \approx4.872^\circ.$

这就把第 2 秒的水平加速度，换成了约 9.8355 N 的推力和约 $4.872^\circ$ 的前倾要求。

3. 沿整条轨迹重复计算

总时刻（秒）	速度（米/秒）	水平加速度（米/秒²）	倾角	总推力（N）
0	0	0	$0^\circ$	9.8000
约 1.6940	0.8216	+0.8783	$+5.121^\circ$	9.8393
2	1.0859	+0.8353	$+4.872^\circ$	9.8355
4	1.9000	+0.0778	$+0.455^\circ$	9.8003
约 4.3949	1.9160	0	$0^\circ$	9.8000
7	1.0462	-0.5959	$-3.480^\circ$	9.8181
约 7.2151	0.9171	-0.6020	$-3.515^\circ$	9.8185
10	0	0	$0^\circ$	9.8000

表中约 1.6940 秒和 7.2151 秒对应加速度极值。最高速度出现在约 4.3949 秒，此刻水平加速度为零，机体正经过水平姿态。

沿十米轨迹计算的倾角：先朝前倾斜，再回到水平，随后朝后倾斜减速

从这张图可以看到完整过程：机体先逐渐前倾以产生正加速度，再回到水平附近，随后后倾以产生负加速度，最后回到水平姿态。

沿十米轨迹计算的总推力：始终略高于或等于悬停所需的 9.8 N

总推力在加速和减速阶段都略有增加，因为水平分量的正负改变方向，而分量的平方共同决定推力大小。图中放大了 9.8 N 附近的变化，纵轴从 9.795 N 开始。

五、Jerk 与 Snap 怎样联系到姿态变化？

1. 加速度改变，倾角也随之改变

在水平等高运动中：

$\theta(t)=\arctan\left(\frac{a_x(t)}g\right).$

如果加速度从 0 逐渐增加到 $0.98\,\mathrm{m/s^2}$ ，倾角就从 0 增加到：

$\arctan(0.98/9.8)=\arctan(0.1)\approx5.71^\circ.$

在 1 秒内完成这次变化，平均倾角变化率约为 $5.71^\circ/\mathrm s$ ；在 0.2 秒内完成，则约为 $28.55^\circ/\mathrm s$ 。

相同的倾斜变化，安排的时间越短，要求的转动越快。

2. 用 Jerk 算倾角变化率

加速度的变化率是 Jerk，记为 $j_x=da_x/dt$ 。反正切函数 $\arctan u$ 的导数为 $1/(1+u^2)$ 。这里 $u=a_x/g$ ，它的时间导数为 $j_x/g$ ，所以：

$\dot\theta(t) =\frac{1}{1+(a_x/g)^2}\times\frac{j_x}{g} =\boxed{\frac{g\,j_x(t)}{g^2+a_x(t)^2}}.$

公式中的角度以弧度计。结果乘以 $180/\pi$ ，就能换成每秒多少度。

以第 4 秒的航点为例：

$a_x=\frac7{90},\qquad j_x=-\frac{49}{288}.$

代入：

$\dot\theta(4) =\frac{9.8\times(-49/288)}{9.8^2+(7/90)^2} \approx-0.01736\,\mathrm{rad/s} \approx-0.995^\circ/\mathrm s.$

此时倾角约为 $+0.455^\circ$ ，倾角变化率为负：机体仍略微向前倾斜，同时正在朝水平姿态转回。

沿轨迹计算的倾角变化率：Jerk 决定倾角随时间变化的快慢，航点处约为每秒负 0.995 度

起终 Jerk 都为零，因此这条等高直线轨迹在两端的倾角变化率也为零，与静止悬停的倾角、倾角变化率相衔接。

3. Snap 对应更进一步的转动需求

在小倾角的线性近似下， $\tan\theta\approx\theta$ ，角度仍以弧度计。于是：

$\theta\approx\frac{a_x}{g},\qquad \dot\theta\approx\frac{j_x}{g},\qquad \ddot\theta\approx\frac{\sigma_x}{g}.$

$\sigma_x$ 是水平位置的四阶时间导数，也就是 Snap。三个关系依次对应：

轨迹中的量	小倾角模型中的对应量
加速度 $a_x$	倾角 $\theta$
Jerk $j_x$	倾角变化率 $\dot\theta$
Snap $\sigma_x$	倾角变化率的变化，即角加速度 $\ddot\theta$

例如，在这组近似关系中， $\sigma_x=0.98\,\mathrm{m/s^4}$ 对应：

$\ddot\theta\approx0.98/9.8=0.1\,\mathrm{rad/s^2}.$

机体绕一个固定轴转动时，可进一步用“转动惯量乘以角加速度”计算所需力矩。完整三维转动还要计入各方向之间的耦合。

这就给 Jerk、Snap 的轨迹设计提供了物理联系：它们影响姿态随时间变化的需求。实际角速度、力矩和电机限制，需要通过相应动力学关系继续计算与检查。

六、把推力计算扩展到三维

1. 三个方向一起计算

在世界坐标系中，用：

$\mathbf p(t)= \begin{bmatrix}x(t)\\y(t)\\z(t)\end{bmatrix}, \qquad \mathbf a(t)= \begin{bmatrix}\ddot x(t)\\\ddot y(t)\\\ddot z(t)\end{bmatrix}$

表示位置和加速度。令 $\mathbf e_3=(0,0,1)^\top$ ，表示世界坐标系竖直向上的单位方向。

重力向下，大小为 $mg$ ，所以牛顿定律写成：

$m\mathbf a=\mathbf F-mg\mathbf e_3.$

将重力项移到另一侧：

$\boxed{\mathbf F=m(\mathbf a+g\mathbf e_3)}.$

展开成三个方向，就是：

$\boxed{ \mathbf F= \begin{bmatrix} m a_x\\ m a_y\\ m(a_z+g) \end{bmatrix}. }$

例如，仍取 $m=1$ ，某一时刻要求：

$\mathbf a=(1,2,1)^\top\,\mathrm{m/s^2}.$

则所需推力向量为：

$\mathbf F=(1,2,10.8)^\top\,\mathrm N,$

大小为：

$F=\sqrt{1^2+2^2+10.8^2}\approx11.029\,\mathrm N.$

竖直分量中的 9.8 N 用于抵消重力，另外的 1 N 用于产生向上的加速度。

2. 从推力方向到姿态

将推力向量除以它的长度，得到所需的单位方向：

$\boxed{ \mathbf b_3=\frac{\mathbf F}{\|\mathbf F\|} =\frac{\mathbf a+g\mathbf e_3}{\|\mathbf a+g\mathbf e_3\|}. }$

$\|\cdot\|$ 表示向量长度。 $\mathbf b_3$ 表示机体上轴在世界坐标系中应当指向哪里。这个换算要求推力向量长度大于零。

机体上轴确定后，还要用偏航要求确定机头朝向。前面的直线例子保持机头朝向通道，姿态变化可以用一个平面倾角描述；三维轨迹则需要同时处理机体各个轴的方向。

3. 这与微分平坦性有什么关系？

对于这里采用的标准多旋翼模型，可以选取位置与偏航角：

$(x,y,z,\psi)$

作为平坦输出。在映射非退化、轨迹具有所需导数的条件下，能够由这些输出及其有限阶时间导数恢复相应的状态和输入。

前面已经算出了其中一部分：

这种由轨迹向状态与输入的换算，是微分平坦性在多旋翼轨迹规划中的核心用途。它将位置曲线与飞行器的推力、姿态及转动需求联系起来。

七、把飞行限制写回轨迹要求

1. 倾角上限对应怎样的加速度上限？

在水平等高模型中，有：

$|a_x|=g\tan|\theta|.$

假设给这个算例新增两项规划限制：

$|\theta|\le5^\circ,\qquad F\le10.2\,\mathrm N.$

这两个值是本例选定的检查参数。倾角不超过 $5^\circ$ ，对应：

$|a_x|\le9.8\tan5^\circ\approx0.8574\,\mathrm{m/s^2}.$

前面的轨迹最大加速度约为 $0.8783\,\mathrm{m/s^2}$ ，因此：

检查项目	轨迹需求	上限	结果
原有加速度限制	$0.8783\,\mathrm{m/s^2}$	$1\,\mathrm{m/s^2}$	满足
新增倾角限制	$5.121^\circ$	$5^\circ$	超出
新增总推力限制	9.8393 N	10.2 N	满足

把轨迹代入动力学模型，可以将新的飞行限制转换为需要重新检查的运动条件。

2. 延长时间，会怎样改变倾角需求？

假设任务允许调整到达时刻，以及航点处的速度、加速度。把整条运动的时间统一放大到原来的 1.02 倍：

$\widetilde p(t)=p\left(\frac{t}{1.02}\right), \qquad 0\le t\le10.2.$

几何位置的经过顺序保持不变。原来第 4 秒到达的航点，现在第 4.08 秒到达；原来第 10 秒到达的终点，现在第 10.2 秒到达。

求导后，速度除以 1.02，加速度除以 $1.02^2$ 。最大加速度变为：

$|\widetilde a_x|_{\max} =\frac{0.8782528}{1.02^2} \approx0.84415\,\mathrm{m/s^2}.$

对应最大倾角：

$|\widetilde\theta|_{\max} =\arctan\left(\frac{0.84415}{9.8}\right) \approx4.923^\circ.$

重新计算后：

项目	原时间安排	时间统一放大 1.02 倍
航点到达时刻	4 秒	4.08 秒
终点到达时刻	10 秒	10.2 秒
航点速度	1.9000 米/秒	约 1.8627 米/秒
航点加速度	约 0.0778 米/秒²	约 0.0748 米/秒²
最大倾角大小	约 $5.121^\circ$	约 $4.923^\circ$
最大总推力	约 9.8393 N	约 9.8363 N