[BF学院_卷1] -- 第十二讲：Yaw Compensation（偏航补偿）

第十二讲：Yaw Compensation（偏航补偿）

  • 为什么飞机进行 Yaw 时需要补偿
  • Earth Z 在机体系中的空间投影
  • 为什么补偿量等于 sin(Pitch)
  • 为什么 Roll 补偿和 Pitch 补偿方向不同
  • 三行源码如何共同完成 Yaw 补偿
  • angleEarthRef 在补偿中的作用

本讲对应 Betaflight 源码

源码文件
src/main/flight/pid.c

核心代码

float sinAngle = sin_approx(DEGREES_TO_RADIANS(pidRuntime.angleTarget[axis == FD_ROLL ? FD_PITCH : FD_ROLL]));

sinAngle *= (axis == FD_ROLL) ? -1.0f : 1.0f;

angleRate += pidRuntime.angleYawSetpoint * sinAngle * pidRuntime.angleEarthRef;

本讲对应控制流程

Target Angle（目标姿态角）
         │
         ▼
Angle Error（姿态误差）
         │
         ▼
Desired Angular Rate（目标角速度）
         │
         ▼
Yaw Compensation（偏航补偿）

配套脚本

12_YawCompensationGeometry.py

本讲学习目标

阅读完本讲以后，你应该能够回答：

□ 为什么 Yaw 会影响 Roll/Pitch 控制？

□ 为什么源码中需要加入 Yaw Compensation（偏航补偿）？

□ sin(Pitch) 这个补偿系数从哪里来？

□ 为什么 Earth Z 在 Body X 方向存在投影？

□ 为什么这个投影长度等于 sin(Pitch)？

□ 为什么 Roll 和 Pitch 使用不同符号？

回顾上一讲

上一讲中，我们已经知道：Angle Mode（自稳模式）会经历：

Target Angle（目标姿态角）

↓

Current Angle（当前姿态角）

↓

Angle Error（姿态角误差）

↓

Desired Angular Rate（目标角速度）

最终,飞控会把"角度误差" 转换成 "角速度目标", 然后交给 "Rate PID" 继续控制电机。

但是，这里还有一个隐藏的问题:

如果飞机只有 Roll 或者 Pitch, 那么控制非常直观。

例如：

飞机向右倾斜：

Roll Error

↓

修正 Roll

飞机向前低头：

Pitch Error

↓

修正 Pitch

但是，真实飞机是三维空间运动。

飞机不仅可以 Roll, 还可以 Pitch + Yaw 同时变化。

于是出现一个问题：

如果飞机已经存在 Pitch，此时再发生 Yaw，那么：

Yaw 产生的旋转，是否仍然完全位于水平面内？

答案是：不是。

这正是：

Yaw Compensation

存在的原因。

12.1 先看源码做了什么

先忽略数学细节，只看这三行代码。

第一行：

float sinAngle = sin_approx(DEGREES_TO_RADIANS(
    pidRuntime.angleTarget[axis == FD_ROLL ? FD_PITCH : FD_ROLL]
));

它计算：

sin(另一个轴的目标角度)

第二行：

sinAngle *= (axis == FD_ROLL) ? -1.0f : 1.0f;

它处理：

方向符号

第三行：

angleRate += pidRuntime.angleYawSetpoint * sinAngle * pidRuntime.angleEarthRef;

它把：

Yaw目标角速度

转换成：

Roll/Pitch方向上的补偿角速度

因此，这三行代码本质上完成的是：

Yaw旋转

↓

空间投影

↓

补偿Roll/Pitch

其中最关键的问题是，为什么：

Yaw * sin(Pitch)

能够产生补偿？

答案不是通过复杂公式得到的。而是来自空间几何。

12.2 阅读 Python 图形

运行：

12_YawCompensationGeometry.py

以后，主要观察：

Body Reference View

本讲不重点关注：

Reality View

而重点观察飞机自身坐标系中的：

Body X

Body Y

Body Z

以及：

EZ

其中，EZ 表示 Earth Z轴在Body坐标系中的表示。

也就是说，虽然飞机发生了：

Roll

Pitch

Yaw

但是，地球竖直方向 Earth Z 仍然存在。

飞控需要知道这个方向，在飞机坐标系中到底占据多少：

Roll方向

Pitch方向

12.3 先理解 Pitch 角的定义

观察图中的EZ，以及ProjYZ

其中，ProjYZ 表示：

EZ 投影到 Body YZ 平面得到的投影。

因此从空间关系看：

EZ

↓

ProjYZ

形成一个直角投影关系。

关键问题：

EZ 和 ProjYZ 的夹角是多少？

答案就是：

Pitch角

为什么？

因为Pitch 的定义，就是飞机：

绕 Body Y轴旋转的角度

或者，更具体点说，是飞机机头 Body X轴，在Earth XY平面上的投影 ProjEXY产生的夹角。

根据坐标系互相之间的垂直关系，也很好想到，在此时刻，

Body Z轴，在Earth XZ平面上的投影ProjEXZ产生的夹角，角度值夜是Pitch。

换句话说，Earth Z轴，与Body YZ平面 的夹角，也是Pitch。

即，EZ 和 ProjYZ 的夹角是Pitch。

所以几何关系成立：

∠(EZ, ProjYZ) = Pitch

12.4 构建平行四边形

现在观察：

12_YawCompensationGeometry.py

中的四个关键点。

根据 ProjYZ的绘制方法，我们知道ProjLineYZ是垂直于Body YZ平面的，

而 Body X轴也垂直于Body YZ平面，

而ProjYZ的一个点是原点，在Body X轴上；另外一个点是ProjLineYZ与Body YZ平面的交点，

所以，ProjYZ， ProjLineYZ 和 Body X轴在同一平面上，且ProjLineYZ与BodyX轴平行。

然后在这个平面中，过EZ做一条直线，垂直于Body X轴，并与Body X相交于BodyXPt，

由于Body X轴垂直于Body YZ平面，所以在Body YZ平面上的ProjYZ与Body X轴也是垂直关系，

而EZ到BodyXPt的连线也是垂直于Body X轴的，所以他们两也是平行的关系，于是，就得到一个平行四边形。

这就是本讲最重要的几何模型。

12.5 计算 ProjLineYZ

现在，观察图中的直角三角形。

它由：EZ, ProjYZ 和 ProjLineYZ 组成。

其中：EZ是一条单位向量。因此长度 = 1。

在12.3中，已经证明：

∠(EZ, ProjYZ) = Pitch

根据直角三角形定义：

sin(θ) = 对边 / 斜边

这里：

θ = Pitch, 对边是ProjLineYZ, 斜边是EZ

所以， ProjLineYZ = sin(Pitch) x EZ

注意：

这里没有使用任何飞控知识。

没有使用任何控制理论。

甚至没有使用任何源码。

完全来自：

初中直角三角形。

这也是 Betaflight 源码中，直接调用：

sin_approx(...)

的根本原因。因为它需要的，正是：

ProjLineYZ 这一条长度。

12.6 为什么这条长度可以直接搬到 Body X 上？

接下来，终于来到本讲最关键的一步。

很多阅读者都会问：

ProjLineYZ明明在上边，为什么源码最后却直接补偿 Body X？

答案正是上一节建立的：

平行四边形。

平行四边形有一个最基本的性质：

相对边长度相等。

因此：

ProjLineYZ = Origin → BodyXPt

所以，Earth Z 沿 Body X 的投影，自然就是：

sin(Pitch)

这一步，就是整个几何推导最重要的结论。

也是源码中：

sin_approx(...)

真正对应的物理意义。

以上，只是描述了ROLL分量为什么是读取Pitch角来进行的补偿，对于Pitch读取ROLL来补偿也是同样的原理。

12.7 源码为什么使用 sin(Pitch)？

现在，整个几何关系已经建立完成。

再回头看源码：

float sinAngle = sin_approx(DEGREES_TO_RADIANS(
    pidRuntime.angleTarget[axis == FD_ROLL ? FD_PITCH : FD_ROLL]));

这一行代码，其实已经变得非常容易理解。

首先angleTarget, 表示：

Target Angle（目标姿态角）

ROLL角可以理解为需要补偿在Body X轴上的分量。

因此, 对于Roll 控制来说，需要读取Pitch作为补偿角。

对于Pitch 控制来说，则需要读取Roll作为补偿角。

随后：

DEGREES_TO_RADIANS(...)

只是完成：

角度 -> 弧度

单位转换。

因为C 标准库中的 sin() 使用的是弧度, 而不是角度。

最后：

sin_approx(...)

计算得到 sin(Pitch) 或者 sin(Roll)。

现在，我们已经知道，这个值并不是某种经验参数。

也不是调试得到的补偿系数。它真正表示的是：

Earth Z 在另一控制轴上的投影长度。

因此，源码实际上是在计算：

空间投影长度。

12.8 为什么还要乘以 Yaw Setpoint？

继续观察第三行源码：

angleRate += pidRuntime.angleYawSetpoint * sinAngle * pidRuntime.angleEarthRef;

上一节，我们已经知道 sinAngle 表示投影比例。

但是，只有比例，还不能形成真正的补偿。

因为飞机真正正在发生变化的是：

Yaw Rotation（偏航旋转）

因此，还需要知道Yaw到底旋转得有多快。这正是：

pidRuntime.angleYawSetpoint

保存的数据。它表示：

Yaw目标角速度。

于是：

Yaw速度 × 投影比例

自然得到：

Yaw 在 Roll 或 Pitch 上真正产生的角速度。

因此，源码实际上完成的是：

Yaw Angular Rate

×

Projection Ratio

↓

Roll / Pitch Compensation Rate

整个过程，完全符合：空间向量投影。

12.9 为什么 Roll 与 Pitch 要互相补偿？

现在，已经可以回答一个很多初学者都会提出的问题。

为什么Roll 需要Pitch参与补偿？

而Pitch 又需要Roll参与补偿？

答案非常简单。

因为：Yaw永远不会影响自己所在的旋转轴。

它影响的是另外两个坐标轴。

例如：

飞机已经存在Pitch，

那么，Yaw 再继续旋转，Earth Z 就会进入 Body X。

因此，需要sin(Pitch) 补偿Roll。

本讲小结

本讲，我们没有使用复杂的数学推导。

而是借助：

12_YawCompensationGeometry.py

建立了一个简单而直观的空间几何模型。

首先，证明了：

∠(EZ, ProjYZ) = Pitch。

随后，利用：

Body X ⊥ Body YZ 平面

以及：

ProjLineYZ ⊥ Body YZ 平面

建立平行四边形。

最终得到：

ProjLineYZ = sin(Pitch)

再利用：

平行四边形对边相等

自然得到：

Earth Z在 Body X 上的投影 = sin(Pitch)。

因此，Betaflight 源码中的：

sin_approx(...)

并不是经验公式，而是：

空间几何投影

在代码中的直接实现。

最后，再乘以：

Yaw Angular Rate（偏航角速度）

就得到Yaw 在 Roll/Pitch 上真正产生的补偿角速度。

整个补偿过程完全来自空间投影。

但是，还有最后一个问题

到这里，我们已经证明了：

Earth Z 在 Body X 上的投影长度 = sin(Pitch)

同理，也能够得到：

Earth Z 在 Body Y 上的投影长度 = sin(Roll)

因此，源码中的：

float sinAngle = sin_approx(...);

已经完全能够理解。

但是，继续阅读源码，马上又会看到：

sinAngle *= (axis == FD_ROLL) ? -1.0f : 1.0f;

这里，又出现了一个新的问题。

如果 sin(Pitch) 已经表示投影长度。

那么, 为什么Roll 补偿还要再乘：-1？而 Pitch 补偿 却保持：+1？

也就是说，我们已经知道了补偿有多大, 但是，还不知道应该向哪一个方向补偿。

下面，继续利用刚才建立的空间几何模型，分析：

Yaw Compensation（偏航补偿）真正的补偿方向。

很多人第一反应都会想到什么？

看到-1.0f, 很多人的第一反应都是：是不是数学推导要求这样？

或者：是不是坐标系规定？

实际上，都不是。

这里完全没有新的数学推导, 也没有新的三角函数。

它只来自：

飞机真正会往哪边转。

也就是说：

补偿方向 = 飞机运动方向。

而不是数学公式。

我们先观察 Roll 补偿

为了避免两个轴同时分析，现在只讨论Roll这一种情况。

假设：飞机已经存在 Pitch > 0° , 也就是机头抬起。

随后，飞手继续输入Yaw Right。

现在，请不要去想公式，只观察飞机真正会发生什么。

Earth Z 会向哪边偏？

由于：Pitch > 0°，Earth Z 已经不再落在Body Z 上。

它已经具有Body X 分量。

于是，当飞机继续Yaw Right时，这个 Body X 分量，就会开始参与Roll 控制。

但是，它究竟会推动飞机向右滚？还是向左滚？这才是真正的问题。

直接观察飞机运动

现在，不要看代码。

想象一下，飞机保持Pitch = +45°， 后继续Yaw Right。

由于Earth Z 永远保持竖直，飞机却正在绕竖直方向旋转。

因此，对于飞机自己来说，Earth Z 会逐渐：

朝 Body X 的负方向移动。

注意，这里不是Body X 正方向，而是Body X 负方向。

这一点，决定了整个补偿符号。

Body X 负方向意味着什么？

补偿角速度，自然也应该是负方向。

因此：

Roll Compensation = -sin(Pitch)

于是，源码直接写成：

sinAngle *= -1.0f;

整个过程，没有任何神秘公式。只是：

空间方向决定了补偿方向。

为什么 Pitch 补偿却不用乘 -1？

现在，再来看另一种情况。

假设：飞机已经存在Roll > 0°，也就是飞机向右侧倾斜，随后，飞手继续输入Yaw Right。

同样，不要去思考公式，仍然只观察Earth Z 相对于飞机的运动方向。

Earth Z 会向哪边移动？

由于飞机已经发生Roll，因此，Earth Z 已经具有Body Y 分量。

现在，飞机继续发生Yaw Right，观察飞机坐标系可以发现：

这一次，Earth Z 在 Body Y 方向上的运动，与刚才 Roll 补偿时正好相反。

它会朝着Body Y 正方向移动。

因此，Body Y 上得到的投影，天然就是正方向。

于是，Pitch 补偿直接采用：+sin(Roll) 即可。

回到源码

现在，重新阅读第二行源码：

sinAngle *= (axis == FD_ROLL) ? -1.0f : 1.0f;

他正好和我们刚才分析的方向一致。

本讲小结

现在，我们已经完整理解了：

Yaw Compensation（偏航补偿）

这一部分源码，以及：

sin()补偿大小

↓

补偿方向

↓

Yaw Compensation

之间的关系。

至此，这三行源码，已经全部分析完成。

下一讲预告

现在，飞控已经完成了：

Yaw Compensation（偏航补偿）

接下来，控制链将继续向下推进。

下一讲，我们将正式进入：

Rate PID（角速度控制器）

看看飞控如何根据：

Desired Angular Rate（目标角速度）

和

Current Angular Rate（当前角速度）

计算：

Rate Error（角速度误差）。

posted on 2026-08-03 14:39  longyue  阅读(8)  评论(0)    收藏  举报

导航