之前已经讲解了TECS高度与速度控制器，今天是PX4固定翼控制器系列讲解的最后一期，主题是PX4的位置控制器。PX4 1.12及其之前的版本，使用的位置控制器为L1控制器。1.13及其之后的版本，PX4更新了NPFG控制器。NPFG控制器在较强风速下有着更好的路径跟踪效果，且解决了风速大于空速情况下，可能导致飞机失控的问题。本文将简单介绍L1控制器，着重介绍NPFG控制器。

引言——L1控制器

首先简单介绍一下L1控制器，该控制器[1]由MIT的人员提出。

核心思想方法：

• 选择参考点：首先在期望路径上选择一个参考点，该参考点位于飞机当前位置的前方一定距离处（距离为L1）。
• 计算横向加速度期望指令：基于选定的参考点，控制算法生成一个横向加速度命令${a_s}_{cmd}$，使飞机的速度方向逐渐对齐到参考点所在的路径方向。这一加速度命令考虑了飞机的惯性速度，并根据飞机当前位置与参考点之间的相对角度计算。

相关公式计算为：
$$\{\begin{array}{l}{a_s}_{cmd}=2\frac{V^2}{L_1}sin\eta \\ L_1=2Rsin\eta \end{array}$$

滚转指令生成

回看系列第二期文章中的讲解，根据侧向期望加速度，可以根据几何方法计算的得到期望滚转角。
$$\{\begin{array}{l}Lcos\varphi =mg\\ Lsin\varphi =m{a_s}_{cmd}\end{array}$$

最终可以得到期望滚转角为${\varphi }_{sp}=arctan({a_s}_{cmd}/g)$

关键特性：

1. 比例-微分控制：对于直线路径，控制律近似为传统的比例-微分（PD）控制器，但对于曲线路径，它包含了前馈控制元素，使得无人机能够紧密跟随复杂的曲线路径。
2. 前馈控制：控制方法中加入了前馈控制元素，这使得在飞机接近曲线路径时能够提前调整飞行方向，减少路径误差。
3. 自适应速度调整：算法使用飞机的惯性速度来计算横向加速度命令，这使得控制系统能够适应由于外界扰动（如风）引起的速度变化，从而提高轨迹跟踪的准确性。
   其他详细推导讲解可以查看文末参考文献[1]。

PX4软件适配

在PX4软件中，有两个主要调节的参数为FW_L1_PERIOD和FW_L1_DAMPING。我们查看一下PX4关于L1的代码，位置PX4-Autopilot-main/src/lib/l1/ECL_L1_Pos_Controller.cpp。
代码最末尾的函数规定了几个过程变量的计算方式：
$$\{\begin{array}{l}{L_1}_{ratio}=\frac{1}{\pi }\ast {L_1}_d\ast {L_1}_p\\ {\omega }_{heading}=\frac{\sqrt{2}\ast \pi }{{L_1}_p}\end{array}$$

void ECL_L1_Pos_Controller::set_l1_period(float period)
{
	_L1_period = period;
	/* calculate the ratio introduced in [2] */
	_L1_ratio = 1.0f / M_PI_F * _L1_damping * _L1_period;
	/* calculate normalized frequency for heading tracking */
	_heading_omega = sqrtf(2.0f) * M_PI_F / _L1_period;
}

void ECL_L1_Pos_Controller::set_l1_damping(float damping)
{
	_L1_damping = damping;
	/* calculate the ratio introduced in [2] */
	_L1_ratio = 1.0f / M_PI_F * _L1_damping * _L1_period;
	/* calculate the L1 gain (following [2]) */
	_K_L1 = 4.0f * _L1_damping * _L1_damping;
}

主要定义了三种模式下的L1_distance和lateral_accelation的计算方式。

• navigate_waypoints模式：查看文件中ECL_L1_Pos_Controller::navigate_waypoints函数。相关计算方式为

_L1_distance = _L1_ratio * ground_speed;
 _lateral_accel = _K_L1 * ground_speed * ground_speed / _L1_distance * sinf(eta);

由此可以得到L1_distance和lateral_accelation的计算公式如下：
$$\{\begin{array}{l}{L_1}_{distance}=\frac{1}{\pi }\ast {L_1}_d\ast {L_1}_p\ast V_{ground}\\ {a_s}_{cmd}=\frac{4{L_1^2}_d{V}_{ground}^2}{{L_1}_{diatance}}sin\eta \end{array}$$

需要注意的是这里L1的取法和论文中的原版不一致。想要读懂这段代码的逻辑可能会有些复杂，这里我查到了AP中的L1控制器的图解，和PX4中所给出的计算逻辑一致。如下图所示。两者的计算方式也基本一致。

由此可以看出，调试period和damping这两个参数时，先调节period，控制参考点的距离在一个大致合理的范围。随后调节damping来微调震荡情况。可以先调节period到稍微振荡，再将period适当增大2-3；最后微调damping。damping取值范围为0.65-0.85，每次微调变化幅度为0.05。

• navigate_loiter模式：该模式为盘旋模式，内部分了两种情况。

Capture模式：该模式用于当飞机在圆圈外的飞行控制。在飞机与圆心的连线上选取参考点，使用L1控制器。

_L1_distance = _L1_ratio * ground_speed;
float lateral_accel_sp_center = _K_L1 * ground_speed * ground_speed / _L1_distance * sinf(eta);

Circle模式：当飞机跟踪到圆圈附近时，不再使用L1控制器，改为使用PD控制器加向心加速度前馈。
PD控制：

float xtrack_vel_circle = -ltrack_vel_center;
float xtrack_err_circle = vector_A_to_airplane.length() - radius;
float lateral_accel_sp_circle_pd = (xtrack_err_circle * K_crosstrack + xtrack_vel_circle * K_velocity);

向心加速度前馈：

float tangent_vel = xtrack_vel_center * loiter_direction;
float lateral_accel_sp_circle_centripetal = tangent_vel * tangent_vel / math::max((0.5f * radius), (radius + xtrack_err_circle));

两者叠加：

float lateral_accel_sp_circle = loiter_direction * (lateral_accel_sp_circle_pd + lateral_accel_sp_circle_centripetal);

• navigate_heading模式：该模式用于定向飞行。代码中相关量计算方式为

$$\{\begin{array}{l}{a_s}_{cmd}=2sin\eta {\omega }_v\\ {L_1}_{distance}=V_{ground}/{\omega }_{heading}\\ {\omega }_v=V_{ground}{\omega }_{heading}\\ {\omega }_{heading}=\sqrt{2}\pi /{L_1}_p\end{array}$$

给出${L_1}_{distance}$和${a_s}_{cmd}$的计算方式为
$$\{\begin{array}{l}{a_s}_{cmd}=2\sqrt{2}\pi sin\eta V_{ground}/{L_1}_p\\ =2\frac{V_{ground}^2}{{L_1}_{distance}}sin\eta \\ {L_1}_{distance}=\frac{V_{ground}{L_1}_p}{\sqrt{2}\pi }\end{array}$$

小结

综上介绍了L1制导律在PX4中的应用。L1_PERIOD主要决定参考距离的长短，值越大参考距离越大，转弯越缓慢；值越小，参考距离越小，转弯越陡；L1_DAMPING则是微调转弯时候的期望加速度${a_s}_{cmd}$。在调试这两个参数时，先调试period，period越小，转弯越快。当飞机转弯出现些许震荡后，将period适当增大2-3。随后调整damping。

NPFG控制器

在PX41.13及其之后的版本，可以使用NPFG位置控制器，全称Nonlinear Path Following Guidance（非线性路径跟踪制导律）[2]。L1控制器在计算参考方向、参考距离、侧向加速度指令时，大都采用地速作为参考，没有引入空速的闭环控制，因此L1控制器在强风天气下的路径跟踪控制效果较差。NPFG相较于L1，引入了新的期望空速矢量，来抵御强风的干扰，其路径跟踪效果相较于L1有着显著提升。接下来我们来介绍NPFG控制器。英文网站参考链接[3]。

空速圆

如下图所示，我们橙色矢量为空速矢量$V_a$。以飞机起始位置，作风速矢量平移后得到灰色的飞机位置，以灰色飞机位置为圆心，空速大小为半径做圆得到空速圆（橙色虚线圆）。空速圆为空速矢量$V_a$可到达的最大范围，蓝色矢量为风速矢量$V_w$，空速矢量与风速矢量合成可得到白色矢量地速$V_g$。

当风速大小$|V_w|$小于空速大小$|V_a|$时，飞机位置位于空速圆内，合成后的$V_g$可以指向任意方向。

超出空速圆

如下图所示，当风速较大时，出现”过量风“$|V_w|>|V_a|$时，无论空速矢量和风速矢量如何合成，合成后的地速矢量永远在一个锥形区域内，称之为可行方向锥。

因此锥内的范围是可飞行方向。

锥外的范围是不可飞行方向。

空速圆内、可视范围内路径跟踪

当$|V_w|<|V_a|$飞机位于空速圆内时，飞机跟踪蓝色期望路径，绿色为期望飞行方向（相对于地面）（选取方法类似L1控制器，在与期望路径最近点初，向前一段选取期望路径点，期望路径点与飞机连线为期望飞行方向）。

有了期望的飞行方向（相对于地面）后，将绿色矢量与空速圆的交点和空速圆圆心连接，即可得到期望的参考空速矢量（橙色虚线）。空速矢量与速度矢量合成后，得到灰色的地速矢量结果。在整个动态飞行过程中，计算是连续的。

空速圆外、可行方向锥内路径跟踪

当$|V_w|>|V_a|$飞机位于空速圆外时，如果期望的飞行方向（绿色矢量）位于可行方向锥内，任意方向均可通过空速矢量和风速矢量合成。

极限条件为：期望飞行方向与可行方向锥相切。这种情况下保证最小化到期望路径的位置误差。

空速圆外、可行方向锥外路径跟踪

当$|V_w|>|V_a|$飞机位于空速圆外，且期望的飞行方向（绿色矢量）位于可行方向锥外，无论怎样合成空速矢量和风速矢量，均无法到达期望方向。

为了防止强风下失控，这里的安全机制设计为：将偏离航向的发生率降至最低。飞机空速方向与风速方向相反，飞机顶风飞行。但由于风速大于空速，飞机顶着风也会被向后”吹跑“。

空速补偿

根据上面的讲解不难看出，飞行范围主要取决于可行方向锥的范围，那我们是否可以尝试扩大可向方向锥的范围来实现更大范围的飞行？

之前的橙色空速圆为设定的默认空速值，实际上飞机还有可用的最大空速值。对于默认空速值可行方向锥范围外的飞行方向，可以尝试增大空速来获取更大的可行方向锥。

若期望飞行方向（绿色矢量），与橙色空速圆（设定的默认空速）无交点，与红色空速圆（最大空速圆）有交点。

在绿色线上选取圆环内一点，将该点与圆心相连，即可得到期望的空速矢量，空速矢量与风速矢量合成，即可得到灰色地速矢量。这里选取图中飞机位置与垂线外侧的那一部分的点，可以获得较大的地速。

总结

根据上述分析，可以查看以下GIF图。图中总结了如何根据期望飞行方向选择空速方向，并说明了在何种情况下需要启用空速补偿以实现更佳的飞行效果。在开启空速补偿后，即便是在强风情况下，可行方向有着显著改善。

使用教程

开启方式

如果希望启用NPFG，需要以下条件：

1. PX4 1.13及其以上版本
   • 仅针对固定翼飞行器有效；
   • 设置参数FW_USE_NPFG=1；
2. 有效的风速估计
   • 使用空速计；
   • 并/或启用合成侧滑融合 EKF2_FUSE_BETA=1。

调参

NPFG主要改进在于期望空速矢量的选取，该控制器是由L1派生而来，因此NPFG_PERIOD和NPFG_DAMPING可以直接沿用L1_PERIOD和L1_DAMPING的值。越小的NPFG_PERIOD会带来越激进的转弯效果。

自动稳定

如果没有调好NPFG_PERIOD，出现过于激进的转弯，且你想使用自动稳定。可以设置参数NPFG_LB_PERIOD=1，该模式下会自动识别滚转周期来设定合理的NPFG_PERIOD。如果开启该功能，需要正确设定NPFG_ROLL_TC。
为了得到正确的NPFG_ROLL_TC：

• 需要将日志记录调整为高频率；
• 在定高或姿态模式下，长周期和短周期下，完整得左右打杆滚转数次，记录下日志；
• 得到滚转角的输入和输出响应，将其拟合出一阶传递函数；
• 一阶传递函数的时间常数值设定给NPFG_ROLL_TC。

强风下行为预设

强风下有两种预设行为：
最小前向地速飞行：
设置参数：NPFG_EN_MIN_GSP=1
设定期望的最小前向地速：FW_GND_SPD_MIN
飞机缓慢飞行至期望轨迹处，到达轨迹后，制导保持与最小地速相同的速度。

持续跟踪：
设置参数：NPFG_TRACK_KEEP=1
设定FW_GND_SPD_MIN=0
当飞机离期望轨迹较远时，NPFG_GSP_MAX_TK（m/s）是 NPFG 为返回航迹所命令的最大前向地速；一旦安全回到航迹，命令的地速辅助将被归零。

改进对比

采用NPFG后，强风下的路径跟踪效果有着显著提升。
原制导律路径跟踪效果：

NPFG路径跟踪效果：

总结

本期文章完成了PX4控制器讲解的最后一期，L1控制器+NPFG控制器。NPFG在强风环境下有着显著优势，感兴趣的朋友可以根据文章的介绍调试飞行一下，文末会放上相关文献及参考链接。

参考文献及链接

[1]PARK, Sanghyuk; DEYST, John; HOW, Jonathan. A new nonlinear guidance logic for trajectory tracking. In: AIAA guidance, navigation, and control conference and exhibit. 2004. p. 4900.
[2]STASTNY, Thomas; SIEGWART, Roland. On flying backwards: Preventing run-away of small, low-speed, fixed-wing UAVs in strong winds. In: 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2019. p. 5198-5205.
[3]https://github.com/PX4/PX4-Autopilot/pull/18810
[4]https://www.youtube.com/watch?v=LY6hYBCdy-0

END

迅翼SwiftWing致力于固定翼技术共享，汇聚固定翼领域技术极客，推动固定翼技术持续创新！