PX4 控制器架构（二）：从航点到轨迹——多旋翼设定点生成链路详解

系列定位 ：本文是 PX4 多旋翼控制器深入系列的第 2 篇 ，直接承接第 1 篇《PX4 控制器架构总览》。第 1 篇勾勒了 mc_pos_control → mc_att_control → mc_rate_control 的级联全貌，并指出 trajectory_setpoint 是位置控制器的输入。本文回答：这个输入从哪来？Navigator 与 FlightTaskAuto 如何将高层任务意图转化为底层控制器可消费的平滑轨迹？

1. 引言：设定点的三重来源

PX4 的位置控制器以统一的 trajectory_setpoint 为输入，从不关心这个设定点来自航线任务、遥控摇杆还是外部计算机。但在控制器上游，有三条截然不同的生成路径：

来源	链路	典型场景
Auto（自主导航）	Navigator → FlightModeManager → FlightTaskAuto → PositionControl	航线任务（Mission）、返航（RTL）、悬停（Loiter）
Manual（手动遥控）	RC 摇杆 → FlightModeManager → FlightTaskManual → PositionControl	Position 模式、Altitude 模式
Offboard（外部控制）	MAVLink/ROS2 → trajectory_setpoint → PositionControl	机载计算机、外部轨迹规划器

本文聚焦 Auto 链路，核心关注 Navigator 与 FlightTaskAuto 如何将离散航点转化为连续、可执行的运动学轨迹。

2. 整体架构：三层设定点生成链路

Auto 模式下的完整数据流可抽象为三个层次，按信息抽象级别从高到低排列：

Layer 1：几何规划层（Navigator）

运行频率：20 Hz （源码中通过 orb_set_interval(_local_pos_sub, 50) 将 vehicle_local_position 订阅限流为 50 ms）
输入：vehicle_local_position、vehicle_status（含 nav_state）、mission
输出：position_setpoint_triplet（previous / current / next）
职责：根据 nav_state 选择导航模式，输出带接受半径、巡航速度、偏航指令的几何航点

Layer 2：运动学轨迹层（FlightModeManager + FlightTaskAuto）

运行频率：50 Hz，与位置控制器同频运行
输入：position_setpoint_triplet、vehicle_local_position
输出：trajectory_setpoint（position / velocity / acceleration / jerk / yaw / yawspeed）
职责：将全局 WGS84 航点投影到本地 NED，通过 PositionSmoothing 生成加加速度受限的平滑轨迹

Layer 3：动力学控制层（mc_pos_control）

运行频率：50 Hz
输入：trajectory_setpoint
输出：vehicle_attitude_setpoint
职责：用 P-PID 跟踪轨迹，并生成期望姿态

关键区分 ：Layer 2 与 Layer 3 不是传统级联控制中的"外环-内环"。FlightTaskAuto 是轨迹生成器 ，mc_pos_control 是轨迹跟踪器。真正的带宽分离发生在 mc_pos_control 内部及其下游：位置环 → 姿态环 → 速率环。

3. Navigator：几何航点规划层

3.1 运行频率与模式选择

Navigator 在独立线程中运行，主循环通过 px4_poll() 等待三类话题更新：vehicle_local_position（限流至 50 ms）、vehicle_status（含 nav_state）、mission（任务列表更新）。每次循环，Navigator 根据 _vstatus.nav_state 选择一个内部导航模式对象：

`nav_state`	激活的导航模式	核心行为
`AUTO_MISSION`	Mission	顺序执行 Dataman 中的航点
`AUTO_RTL`	RTL	返航状态机
`AUTO_LOITER`	Loiter	在当前位置/指定位置悬停
`AUTO_TAKEOFF`	Takeoff	爬升至起飞高度
`AUTO_LAND`	Land	垂直降落
`AUTO_PRECLAND`	PrecLand	精确降落
`AUTO_VTOL_TAKEOFF`	VtolTakeoff	VTOL 起飞

模式对象继承自 NavigatorMode，其生命周期由 run(active) 管理：首次激活调用 on_activation()，之后每周期调用 on_active()，失活时调用 on_inactivation()。

3.2 航点三元组 `position_setpoint_triplet`

Navigator 的核心输出包含三个航点：

previous：上一航点。对多旋翼通常无效或设为当前位置；主要用于固定翼的航段跟踪。
current：当前目标航点。含经纬度、海拔、航向角、接受半径、巡航速度、航点类型等。
next：下一航点。供下游提前规划转弯速度，避免飞到航点正上方再急转。

position_setpoint_s 的关键字段：

字段	物理意义
`valid`	该航点是否有效
`type`	航点类型：POSITION / VELOCITY / LOITER / TAKEOFF / LAND / IDLE
`lat`, `lon`	WGS84 坐标，单位度
`alt`	海拔（AMSL），单位米
`yaw`	目标偏航角；`NaN` 表示由下游决定
`acceptance_radius`	水平接受半径，进入该圆即视为到达
`alt_acceptance_radius`	垂直接受半径；`NaN` 时采用默认值
`cruising_speed`	建议巡航速度；`-1` 表示使用默认
`loiter_radius`	悬停圆半径

三元组设计的精髓在于：下游轨迹生成器看到的不是孤立的点，而是一段带曲率信息的航路 。current 与 next 的存在使得飞行器可以在不精确飞过航点中心的情况下平滑过渡。

3.3 Mission 模式：航点加载与推进

Mission 航点存储在 Dataman 中（DM_KEY_WAYPOINTS_OFFBOARD_0/1）。Navigator 维护一个热点缓存，预加载当前序号附近的航点：当前序号为 k 时，缓存范围大致为 [k, k + cache_size]，反向任务时则为反向范围。

每个周期检查是否到达当前航点。对多旋翼的普通 WAYPOINT，核心判据为：

reached = ( ∥ p − p current ∥ x y ≤ r acc ) ∧ ( ∣ p z − p current , z ∣ ≤ r alt ) \text{reached} = \bigl(\|\mathbf{p} - \mathbf{p}{\text{current}}\|{xy} \leq r_{\text{acc}}\bigr) \;\land\; \bigl(|p_z - p_{\text{current},z}| \leq r_{\text{alt}}\bigr) reached=(∥p−pcurrent∥xy≤racc)∧(∣pz−pcurrent,z∣≤ralt)

其中水平距离使用 WGS84 大地距离计算，垂直接受半径 r_alt 对多旋翼默认来自参数 NAV_MC_ALT_RAD。

若到达且航点设置 autocontinue，则推进 current_seq，加载下一航点，并更新三元组。注意：推进的是 mission 话题中的当前序号，再由 set_mission_items() 重新填充三元组。固定翼还会额外利用 previous 判断是否已飞过航点，多旋翼不使用该分支。

3.4 RTL 模式：返航状态机

RTL 首先根据 RTL_TYPE 和可用任务/集结点选择返航类型：

`RTL_TYPE`	行为
0	直接返回 home 或最近的 safe point
1	优先返回 mission landing 或 rally point，否则 home
2	沿任务航线返航；有 landing 则正向，否则反向
3	直接飞到 mission landing 起点后沿 pattern 降落
4	按 waypoint 数量选最近路径沿任务航线返航
5	只考虑 rally point，不考虑 home 与 mission landing

对于普通多旋翼直接返航（RTL_LAND_DELAY = 0），典型状态序列为：

CLIMBING → MOVE_TO_LOITER → LAND → IDLE \text{CLIMBING} \rightarrow \text{MOVE\_TO\_LOITER} \rightarrow \text{LAND} \rightarrow \text{IDLE} CLIMBING→MOVE_TO_LOITER→LAND→IDLE

每个状态对应一个几何目标点：

CLIMBING：当前水平位置 + RTL 返航高度
MOVE_TO_LOITER：home 点上方悬停位置
LAND：home 点，垂直降落
IDLE：落地后怠速

状态切换条件是当前目标点被判定为"到达"。

3.5 Loiter 与 Land 的制动预投影

当多旋翼以较高速度飞行时接到 Loiter 或 Land 指令，若直接把目标点设在当前位置，飞机会因惯性冲过目标，随后控制器为消除位置误差反向加速，形成过冲-回正的 S 形摆动。

PX4 的解决方案是制动预投影 ：把悬停/降落中心设在当前速度方向上的制动停止点。该功能在 navigator_main.cpp 中由 preproject_stop_point() 实现，并被 Land::on_activation()、Loiter::reposition() 等调用。

制动距离源码调用为：

d brake = computeBrakingDistanceFromVelocity ( ∥ v x y ∥ , j max ⁡ , a max ⁡ , 0.6 j max ⁡ ) d_{\text{brake}} = \text{computeBrakingDistanceFromVelocity}(\|\mathbf{v}{xy}\|,\; j{\max},\; a_{\max},\; 0.6 \, j_{\max}) dbrake=computeBrakingDistanceFromVelocity(∥vxy∥,jmax,amax,0.6jmax)

其中 computeBrakingDistanceFromVelocity(velocity, jerk, accel, accel_delay_max) 的参数顺序为速度、加加速度、加速度、延迟加速度；对应参数分别是 MPC_JERK_AUTO、MPC_ACC_HOR 和 0.6 * MPC_JERK_AUTO。

预投影后的目标点：

p loiter = p current + v x y ∥ v x y ∥ d brake \mathbf{p}{\text{loiter}} = \mathbf{p}{\text{current}} + \frac{\mathbf{v}{xy}}{\|\mathbf{v}{xy}\|} \, d_{\text{brake}} ploiter=pcurrent+∥vxy∥vxydbrake

无预投影 时：悬停中心在当前位置，飞机会冲过后折返。

有预投影时：悬停中心在制动停止点，飞机自然减速并一次到位。

这一机制同时应用于 Loiter 激活 和 Land 激活。

4. FlightTaskAuto：从全局航点到本地轨迹

4.1 FlightModeManager 与任务选择

FlightModeManager 运行在 nav_and_controllers 工作队列上，每次 vehicle_local_position 更新时触发，并通过 set_interval_us(20_ms) 将处理频率限制为 50 Hz。它根据 nav_state 和控制模式标志选择对应的 FlightTask：

flag_control_auto_enabled 且非紧急下降 → FlightTaskAuto
POSCTL → FlightTaskManualAcceleration（默认 MPC_POS_MODE = 4）或 FlightTaskManualPosition（MPC_POS_MODE = 0）
ALTCTL → FlightTaskManualAltitudeSmoothVel（默认 MPC_POS_MODE = 4）或 FlightTaskManualAltitude（MPC_POS_MODE = 0）
紧急下降 → FlightTaskDescend

选择失败时存在降级链：Position → Altitude → Descend → Failsafe。

4.2 WGS84 到本地 NED 的转换

FlightTaskAuto 首先从 vehicle_local_position 获取本地坐标参考点（ref_lat、ref_lon、ref_alt），然后用 MapProjection 将 WGS84 坐标投影到本地切平面。

PX4 的 MapProjection::project() 基于球面方位等距投影：

c = arccos ⁡ ( sin ⁡ ϕ ref sin ⁡ ϕ + cos ⁡ ϕ ref cos ⁡ ϕ cos ⁡ ( λ − λ ref ) ) c = \arccos\bigl(\sin\phi_{\text{ref}}\sin\phi + \cos\phi_{\text{ref}}\cos\phi\cos(\lambda - \lambda_{\text{ref}})\bigr) c=arccos(sinϕrefsinϕ+cosϕrefcosϕcos(λ−λref))

k = c sin ⁡ c k = \frac{c}{\sin c} k=sincc

x = k ( cos ⁡ ϕ ref sin ⁡ ϕ − sin ⁡ ϕ ref cos ⁡ ϕ cos ⁡ ( λ − λ ref ) ) R E x = k \bigl(\cos\phi_{\text{ref}}\sin\phi - \sin\phi_{\text{ref}}\cos\phi\cos(\lambda - \lambda_{\text{ref}})\bigr) R_E x=k(cosϕrefsinϕ−sinϕrefcosϕcos(λ−λref))RE

y = k cos ⁡ ϕ sin ⁡ ( λ − λ ref ) R E y = k \cos\phi \sin(\lambda - \lambda_{\text{ref}}) R_E y=kcosϕsin(λ−λref)RE

在小范围内，该投影退化为常见的 flat-Earth 近似：

x ≈ ( ϕ − ϕ ref ) R E , y ≈ ( λ − λ ref ) R E cos ⁡ ϕ ref x \approx (\phi - \phi_{\text{ref}}) R_E, \quad y \approx (\lambda - \lambda_{\text{ref}}) R_E \cos\phi_{\text{ref}} x≈(ϕ−ϕref)RE,y≈(λ−λref)REcosϕref

高度转换：

z = − ( h − h ref ) z = -(h - h_{\text{ref}}) z=−(h−href)

负号因为 NED 坐标系中 z z z 轴向下。

为避免 Navigator 重复发布相同航点导致抖动，FlightTaskAuto 设置 1 mm 的变化检测阈值：只有当新航点与缓存航点在任一轴上相差超过 0.001 m 时才更新内部状态。

4.3 航点类型处理

根据 current.type，FlightTaskAuto 对原始设定点做不同预处理：

类型	处理方式
POSITION / TAKEOFF / LOITER	位置设定点设为 current 的本地坐标；速度设定点留空，由平滑器生成
LAND	调用专用降落逻辑，垂直速度按离地高度分阶段下降
VELOCITY	位置设定点留空（保持当前高度），水平速度按当前飞行方向以巡航速度填充
IDLE	位置、速度均留空；加速度设为较大的向下值，使推力归零

LOITER 类型在 FlightTaskAuto 中并不执行圆周/八字飞行，而是作为"到达该点后悬停"的位置目标。真正的绕圈飞行由独立的 FlightTaskOrbit 处理。

4.4 偏航处理

偏航设定点的优先级如下：

Weathervane（风向标） ：若参数 WV_EN 启用，且当前航点未指定偏航（current.yaw 为 NaN），机头可随风向摆动以减小侧风阻力。注意：当 Navigator 显式给定 current.yaw 时，风向标会被强制禁用。
Navigator 显式偏航 ：若 current.yaw 为有限值，直接采用。
MPC_YAW_MODE ：否则按模式计算偏航：
- towards_waypoint：指向 current 航点
- towards_home：指向 home 点
- away_from_home：远离 home 点
- along_trajectory：沿速度方向
- towards_waypoint_yaw_first：先对齐航点偏航再移动
- yaw_fixed：保持当前偏航

计算出的偏航角再经过 HeadingSmoothing 平滑，限制最大角速度 MPC_YAWRAUTO_MAX 和角加速度 MPC_YAWRAUTO_ACC。对于 towards_waypoint_yaw_first 模式，还会检查偏航是否已对齐到 MIS_YAW_ERR 范围内，未对齐时强制水平速度为零。

4.5 输出：`trajectory_setpoint`

FlightModeManager 最终发布：

字段	含义
`position[3]`	NED 位置设定点（m）
`velocity[3]`	NED 速度设定点（m/s）
`acceleration[3]`	NED 加速度设定点（m/s²）
`jerk[3]`	NED 加加速度设定点（m/s³，主要用于日志）
`yaw`	偏航角设定点（rad）
`yawspeed`	偏航角速度设定点（rad/s）

NaN 表示该自由度不由上层控制，位置控制器会跳过该维度。控制优先级为：position → velocity → acceleration → thrust；当 position 与 velocity 同时存在时，velocity 作为前馈。

5. PositionSmoothing：从航点到平滑轨迹

5.1 整体结构

PositionSmoothing 是 FlightTaskAuto 的轨迹生成核心。它接收：

当前位置 p \mathbf{p} p
航点三元组 { A , B , C } \{\mathbf{A}, \mathbf{B}, \mathbf{C}\} {A,B,C}（本地 NED）
可选的 feedforward 速度
时间步长 delta_time

动态约束（最大加速度、加加速度、速度、接受半径等）通过 setter 预先配置，而非每次调用时传入。

输出：平滑后的位置、速度、加速度、加加速度设定点。

内部流程分两步：

生成原始速度设定点 _generateVelocitySetpoint：根据航点几何与动态约束，决定当前该朝哪飞、飞多快。
生成连续轨迹 _generateTrajectory：通过三轴独立的 VelocitySmoothing 将原始速度平滑为加速度连续的轨迹，并做时间同步与拉伸。

5.2 速度方向：直飞还是转弯？

PositionSmoothing 用 _isTurning() 判断飞机是否处于转弯状态。三个条件必须同时满足：

水平速度大于 0.2 m/s
速度方向与指向目标 B \mathbf{B} B 的方向夹角大于约 11.5 ° 11.5° 11.5°（即 cos ⁡ θ < 0.98 \cos\theta < 0.98 cosθ<0.98）
飞机仍在接受半径之外

不转弯时，速度方向直指目标：

u vel = B − p ∥ B − p ∥ \mathbf{u}_{\text{vel}} = \frac{\mathbf{B} - \mathbf{p}}{\|\mathbf{B} - \mathbf{p}\|} uvel=∥B−p∥B−p

转弯时 ，采用 L1 制导：将内部参考轨迹位置投影到 A B \mathbf{A}\mathbf{B} AB 航段上，得到垂足 p closest \mathbf{p}{\text{closest}} pclosest，然后沿航段向前取前视点 p L1 \mathbf{p}{\text{L1}} pL1：

p closest = A + $( p traj - A ) \cdot u A B$ u A B \mathbf{p}{\text{closest}} = \mathbf{A} + \bigl $(\\mathbf{p}_{\\text{traj}} - \\mathbf{A}) \\cdot \\mathbf{u}_{\\mathbf{AB}}\\bigr$ \, \mathbf{u}{\mathbf{AB}} pclosest=A+ $(ptraj-A)\cdotuAB$ uAB

d cross = ∥ p traj − p closest ∥ d_{\text{cross}} = \|\mathbf{p}{\text{traj}} - \mathbf{p}{\text{closest}}\| dcross=∥ptraj−pclosest∥

L 1 = max ⁡ ( r acc , 5.0 m ) L_1 = \max(r_{\text{acc}},\; 5.0\,\text{m}) L1=max(racc,5.0m)

d along = L 1 2 − d cross 2 d_{\text{along}} = \sqrt{L_1^2 - d_{\text{cross}}^2} dalong=L12−dcross2

p L1 = p closest + d along u A B \mathbf{p}{\text{L1}} = \mathbf{p}{\text{closest}} + d_{\text{along}} \, \mathbf{u}_{\mathbf{AB}} pL1=pclosest+dalonguAB

速度方向指向前视点：

u vel = p L1 − p traj ∥ p L1 − p traj ∥ \mathbf{u}{\text{vel}} = \frac{\mathbf{p}{\text{L1}} - \mathbf{p}{\text{traj}}}{\|\mathbf{p}{\text{L1}} - \mathbf{p}_{\text{traj}}\|} uvel=∥pL1−ptraj∥pL1−ptraj

这种"胡萝卜挂在前方"的策略使飞机在到达航点 B \mathbf{B} B 之前就开始转向出航段 B C \mathbf{BC} BC，实现平滑过弯。

5.3 速度大小：反向航点遍历

速度上限通过反向遍历航点链计算。核心思想是：先根据下一段航线确定在 B \mathbf{B} B 点允许的出口速度，再反推当前位置到 B \mathbf{B} B 的制动约束。

第一步：B 点出口速度

设入航段 A B \mathbf{AB} AB 与出航段（方向由 B \mathbf{B} B 指向 C \mathbf{C} C）的夹角为：

α = arccos ⁡ ( ( B − A ) ⋅ ( B − C ) ∥ B − A ∥ ∥ B − C ∥ ) \alpha = \arccos\left(\frac{(\mathbf{B}-\mathbf{A}) \cdot (\mathbf{B}-\mathbf{C})}{\|\mathbf{B}-\mathbf{A}\| \, \|\mathbf{B}-\mathbf{C}\|}\right) α=arccos(∥B−A∥∥B−C∥(B−A)⋅(B−C))

接受半径 r acc r_{\text{acc}} racc 定义了转弯圆的切线长度。转弯圆半径为：

r turn = r acc tan ⁡ α 2 r_{\text{turn}} = r_{\text{acc}} \tan\frac{\alpha}{2} rturn=racctan2α

在不超过轨迹规划用最大向心加速度 a max ⁡ traj = MPC_XY_TRAJ_P ⋅ a max ⁡ a_{\max}^{\text{traj}} = \texttt{MPC\_XY\_TRAJ\P} \cdot a{\max} amaxtraj=MPC_XY_TRAJ_P⋅amax 的前提下， B \mathbf{B} B 点允许的最大切向速度为：

v turn = a max ⁡ traj r acc tan ⁡ α 2 v_{\text{turn}} = \sqrt{a_{\max}^{\text{traj}} \, r_{\text{acc}} \, \tan\frac{\alpha}{2}} vturn=amaxtrajracctan2α

最终 B 点出口速度为：

v exit = min ⁡ ( v turn , v cruise , v next ) v_{\text{exit}} = \min\bigl(v_{\text{turn}},\; v_{\text{cruise}},\; v_{\text{next}}\bigr) vexit=min(vturn,vcruise,vnext)

若 B \mathbf{B} B 与 C \mathbf{C} C 的距离小于接受半径（航点重叠），则飞机必须在 B \mathbf{B} B 停下， v exit = 0 v_{\text{exit}} = 0 vexit=0。

第二步：从当前位置到 B 的制动约束

已知在 B \mathbf{B} B 点需将速度降至 v exit v_{\text{exit}} vexit，当前到 B \mathbf{B} B 的剩余距离为 d = ∥ p − B ∥ d = \|\mathbf{p} - \mathbf{B}\| d=∥p−B∥。给定最大加加速度 j max ⁡ j_{\max} jmax 和最大加速度 a max ⁡ a_{\max} amax，当前最大允许速度由反向刹车问题决定：

v brake = computeMaxSpeedFromDistance ( j max ⁡ , a max ⁡ , d , v exit ) v_{\text{brake}} = \text{computeMaxSpeedFromDistance}(j_{\max},\; a_{\max},\; d,\; v_{\text{exit}}) vbrake=computeMaxSpeedFromDistance(jmax,amax,d,vexit)

其物理含义是：在距离 d d d 内，以 bang-coast-bang 加加速度曲线将速度从 v brake v_{\text{brake}} vbrake 降到 v exit v_{\text{exit}} vexit，全程不超出 a max ⁡ a_{\max} amax 和 j max ⁡ j_{\max} jmax。

第三步：速度上限合成

最终水平速度上限：

v x y = min ⁡ ( v brake , v cruise ) v_{xy} = \min(v_{\text{brake}},\; v_{\text{cruise}}) vxy=min(vbrake,vcruise)

垂直速度 v z v_z vz 由类似逻辑在"水平距离-高度"平面内计算，使用垂直接受半径 NAV_MC_ALT_RAD。

5.4 转弯速度锁定

若检测到正在转弯，水平速度不允许增加：

v x y ← min ⁡ ( v x y , v x y , previous ) v_{xy} \leftarrow \min(v_{xy},\; v_{xy,\text{previous}}) vxy←min(vxy,vxy,previous)

其物理意义是：飞机入弯后只能减速或保持速度，禁止加速入弯。这防止了飞机在弯道中因速度过高导致向心加速度不足而偏离航线。

5.5 3D 速度合成

水平速度 v x y v_{xy} vxy 与垂直速度 v z v_z vz 合成后分别对 XY 模长和 Z 模长做约束：

v sp = v total u vel \mathbf{v}{\text{sp}} = v{\text{total}} \, \mathbf{u}_{\text{vel}} vsp=vtotaluvel

∥ v sp , x y ∥ ≤ v x y , ∣ v sp , z ∣ ≤ v z \|\mathbf{v}{\text{sp},xy}\| \leq v{xy}, \quad |\mathbf{v}_{\text{sp},z}| \leq v_z ∥vsp,xy∥≤vxy,∣vsp,z∣≤vz

这样确保水平与垂直速度独立可控。

5.6 VelocitySmoothing：加加速度受限轨迹

得到原始速度目标后，三轴独立的 VelocitySmoothing 将其平滑为加速度连续的轨迹。每一段轨迹分为三个阶段：

阶段	加加速度	加速度	作用
T1	+ j max ⁡ +j_{\max} +jmax（或 − j max ⁡ -j_{\max} −jmax）	从初值 ramp 到峰值	加速/减速启动
T2	0	保持峰值（或零）	匀加速/匀速
T3	− j max ⁡ -j_{\max} −jmax（或 + j max ⁡ +j_{\max} +jmax）	从峰值 ramp 到终值	平稳结束

S 曲线的优势在于加速度连续：传统梯形速度规划在加减速切换处存在加速度跳变，会导致姿态指令突变；而 jerk-limited 轨迹的加速度是连续的，仅加加速度在阶段边界处跳变。

时间同步

X/Y/Z 三轴各自独立求解 ( T 1 , T 2 , T 3 ) (T_1, T_2, T_3) (T1,T2,T3)，但通常不同。timeSynchronization() 取三轴中最长总时间 T max ⁡ T_{\max} Tmax，让短轴延长 T 2 T_2 T2（恒定加速度阶段），使三轴同时到达目标，防止"X 到位、Y 还在飞"的扭曲轨迹。

时间拉伸

当真实飞机因风扰或姿态饱和落后于内部参考轨迹时，积分步长乘以系数：

η = 1 − clamp ( e e max ⁡ , 0 , 1 ) \eta = 1 - \text{clamp}\left(\frac{e}{e_{\max}},\; 0,\; 1\right) η=1−clamp(emaxe,0,1)

其中 e e e 为飞机到参考轨迹的水平距离， e max ⁡ e_{\max} emax 为 MPC_XY_ERR_MAX。轨迹自动"放慢"等飞机追上，而非直接跳变设定点。

条件限制 ：拉伸仅在飞机落在参考轨迹后方时生效（飞机到轨迹的向量与轨迹速度向量的点积大于等于零）。若飞机超前于轨迹，点积小于零，time_stretch 强制为 1，不会加速追赶。

6. 各模式的行为特征

6.1 Auto Mission 飞行剖面

一个典型多航点 Mission 的速度剖面可描述为：

直线段 ：以 MPC_XY_CRUISE 匀速飞行；
入弯前：根据剩余距离和出口速度提前数十米开始减速；
转弯中：速度锁定为入弯前值，沿 L1 前视点切线通过接受半径球；
出弯后：重新加速至巡航速度。

接受半径 r acc r_{\text{acc}} racc 是调节"精度 vs 效率"的关键旋钮：狭小环境中设为 1--3 m，飞机精确通过每个航点但转弯较急；开阔测绘场景中可增大至 20--50 m，飞机以接近巡航速度流畅过弯。

6.2 Land 降落速度曲线

降落过程根据离地高度分档控制垂直速度：

高度区间	垂直速度	对应参数
h > h 1 h > h_1 h>h1	v max ⁡ , dn v_{\max,\text{dn}} vmax,dn	`MPC_Z_VEL_MAX_DN`
h 2 < h ≤ h 1 h_2 < h \leq h_1 h2<h≤h1	线性插值至 v land v_{\text{land}} vland	`MPC_LAND_ALT1/2`
h 3 < h ≤ h 2 h_3 < h \leq h_2 h3<h≤h2	v land v_{\text{land}} vland	`MPC_LAND_SPEED`
h ≤ h 3 h \leq h_3 h≤h3（有测距仪）	v crwl v_{\text{crwl}} vcrwl	`MPC_LAND_CRWL`

其中 h 1 , h 2 , h 3 h_1, h_2, h_3 h1,h2,h3 分别对应 MPC_LAND_ALT1/2/3。分阶段减速避免了高空缓慢下降浪费时间，同时确保触地前速度足够低。

6.3 Manual Position 模式：摇杆映射

手动 Position 模式（默认 MPC_POS_MODE = 4）将摇杆偏转映射为目标加速度：

a sp = k stick s \mathbf{a}{\text{sp}} = k{\text{stick}} \, \mathbf{s} asp=ksticks

再通过 VelocitySmoothing 积分得到速度/位置设定点。摇杆回中时目标加速度为零，VelocitySmoothing 将速度平滑降至零，实现"松手即停"。

相比旧版速度模式（摇杆直接映射目标速度），加速度模式的操控直觉更接近真实飞机的油门响应：推杆越大，加速度越大；松杆后飞机按惯性滑行一段再停下。

7. 关键参数与调参建议

7.1 航点精度与飞行效率

参数	默认值	调参建议
`NAV_ACC_RAD`	10 m	狭小环境降至 1--3 m；开阔场景可增至 20--50 m
`NAV_MC_ALT_RAD`	0.8 m	垂直接受半径，通常无需调整
`MPC_XY_CRUISE`	5 m/s	默认巡航速度
`MPC_XY_VEL_MAX`	12 m/s	硬速度上限

7.2 动态约束

参数	默认值	物理意义
`MPC_ACC_HOR`	3 m/s²	最大水平加速度
`MPC_JERK_AUTO`	4 m/s³	Auto 模式最大加加速度
`MPC_XY_TRAJ_P`	0.3	转弯加速度预算缩放，越小过弯越慢
`MPC_XY_ERR_MAX`	5 m	时间拉伸开始起作用的水平误差阈值

调参原则 ： j max ⁡ j_{\max} jmax 越大，轨迹响应越激进、飞行时间越短，但姿态抖动风险增加； a max ⁡ a_{\max} amax 和 j max ⁡ j_{\max} jmax 应与机体推重比匹配。

7.3 降落参数

参数	默认值	物理意义
`MPC_LAND_ALT1`	10 m	开始降速的高度 h 1 h_1 h1
`MPC_LAND_ALT2`	5 m	过渡区下界 h 2 h_2 h2
`MPC_LAND_ALT3`	1 m	测距仪生效高度 h 3 h_3 h3
`MPC_LAND_SPEED`	0.7 m/s	标准降落速度
`MPC_LAND_CRWL`	0.3 m/s	触地前爬行速度

7.4 航向参数

参数	默认值	物理意义
`MPC_YAW_MODE`	0	航向模式
`MPC_YAWRAUTO_MAX`	45 °/s	最大航向角速度
`MPC_YAWRAUTO_ACC`	20 °/s²	最大航向角加速度
`MIS_YAW_ERR`	12 °	航向对齐判定阈值

8. 总结

本文从源码层面梳理了多旋翼设定点生成的完整链路：

Navigator（几何规划层） ：以 20 Hz 运行，根据 nav_state 选择 Mission/RTL/Loiter/Takeoff/Land 等模式，输出 position_setpoint_triplet。关键设计是三元组 、接受半径 和制动预投影。
FlightTaskAuto（运动学轨迹层） ：以 50 Hz 运行，将全局 WGS84 航点通过球面 MapProjection 转换到本地 NED，处理航点类型与偏航，再调用 PositionSmoothing 生成 jerk-limited 平滑轨迹。核心算法包括：
- L1 制导：转弯时跟踪航段前视点；
- 反向速度规划：根据剩余距离和出口速度约束计算当前速度上限；
- 转弯速度约束：利用接受半径球的切线几何计算最大转弯速度；
- VelocitySmoothing：bang-coast-bang 加加速度受限轨迹；
- 时间同步：三轴统一到同一时间尺度；
- 时间拉伸：飞机落后时自动放慢参考轨迹，超前时不加速追赶。
mc_pos_control（动力学控制层） ：消费 trajectory_setpoint，执行 P-PID 串级跟踪。控制器本身不关心设定点来源，这种解耦使同一套控制器可服务于 Auto、Manual、Offboard 等多种模式。

理解这条链路的意义在于：当你发现飞机"转弯时速度掉太多"、"降落时抖"或"航线飞行不够流畅"时，问题往往不在 mc_pos_control 的 PID 参数，而在上层的接受半径设置 、加加速度限制 或巡航速度配置。调试飞行品质应当从上到下逐层定位，而非直接调最底层的姿态增益。

关于我们：

灵智实验室（LingzhiLab）成立于2020年，核心团队源自西北工业大学，由一群深耕无人系统、自动控制与机器人技术的青年工程师与科研人员组成。我们始终秉持"开放、协同、智能、可靠"的理念，致力于推动无人智能体在复杂环境下的自主感知、决策与控制能力。

实验室聚焦于基于开源飞控（如PX4）与ROS 2的深度融合，构建高可靠、模块化、可扩展的无人系统软件架构。依托扎实的工程实践与学术背景，灵智实验室积极参与开源社区建设，助力科研教育与产业落地。