本讲对应 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
存在的原因。
先忽略数学细节，只看这三行代码。
第一行：
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_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方向
观察图中的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_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轴的，所以他们两也是平行的关系，于是，就得到一个平行四边形。
这就是本讲最重要的几何模型。
现在，观察图中的直角三角形。
它由：EZ, ProjYZ 和 ProjLineYZ 组成。
其中：EZ是一条单位向量。因此长度 = 1。
在12.3中，已经证明：
∠(EZ, ProjYZ) = Pitch
根据直角三角形定义：
sin(θ) = 对边 / 斜边
这里：
θ = Pitch, 对边是ProjLineYZ, 斜边是EZ
所以， ProjLineYZ = sin(Pitch) x EZ
注意：
这里没有使用任何飞控知识。
没有使用任何控制理论。
甚至没有使用任何源码。
完全来自：
初中直角三角形。
这也是 Betaflight 源码中，直接调用：
sin_approx(...)
的根本原因。因为它需要的，正是：
ProjLineYZ 这一条长度。
接下来，终于来到本讲最关键的一步。
很多阅读者都会问：
ProjLineYZ明明在上边，为什么源码最后却直接补偿 Body X？
答案正是上一节建立的：
平行四边形。
平行四边形有一个最基本的性质：
相对边长度相等。
因此：
ProjLineYZ = Origin → BodyXPt
所以，Earth Z 沿 Body X 的投影，自然就是：
sin(Pitch)
这一步，就是整个几何推导最重要的结论。
也是源码中：
sin_approx(...)
真正对应的物理意义。
以上，只是描述了ROLL分量为什么是读取Pitch角来进行的补偿，对于Pitch读取ROLL来补偿也是同样的原理。
现在，整个几何关系已经建立完成。
再回头看源码：
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 在另一控制轴上的投影长度。
因此，源码实际上是在计算：
空间投影长度。
继续观察第三行源码：
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
整个过程，完全符合：空间向量投影。
现在，已经可以回答一个很多初学者都会提出的问题。
为什么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这一种情况。
假设：飞机已经存在 Pitch > 0° , 也就是机头抬起。
随后，飞手继续输入Yaw Right。
现在，请不要去想公式，只观察飞机真正会发生什么。
由于：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 负方向。
这一点，决定了整个补偿符号。
补偿角速度，自然也应该是负方向。
因此：
Roll Compensation = -sin(Pitch)
于是，源码直接写成：
sinAngle *= -1.0f;
整个过程，没有任何神秘公式。只是：
空间方向决定了补偿方向。
现在，再来看另一种情况。
假设：飞机已经存在Roll > 0°，也就是飞机向右侧倾斜，随后，飞手继续输入Yaw Right。
同样，不要去思考公式，仍然只观察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（角速度误差）。
此内容由惯性聚合(RSS阅读器)自动聚合整理，仅供阅读参考。 原文来自 — 版权归原作者所有。