4. 无人系统运动学与动力学约束

地形约束航迹规划解决的是“哪里能飞/能走”的问题，而运动学与动力学约束解决的是“怎么飞/怎么走才做得到”的问题。前者给出可行空间，后者给出可行曲线。若忽略后者，规划器可能输出一条几何上完美、却让固定翼无法转弯、让多旋翼超出加速度上限、让地面车辆侧翻、让潜航器俯仰超限的航迹。本节先建立四类平台的运动学模型，再把最小转弯半径、爬升率、俯仰角、速度与加速度限制统一表达为航迹上的约束。

一、四类平台的运动学模型

为便于统一表达，先约定坐标系：采用局部东北天（ENU）坐标系，x 轴指向东、y 轴指向北、z 轴指向上；航向角 ψ 从 x 轴逆时针为正，俯仰角 θ 抬头为正，速度 v 沿机体前向。全文与图示均遵循该约定。

固定翼是典型的非完整约束系统，不能侧向平移，只能沿机头方向前进。其简化质点模型为：ẋ = v·cosθ·cosψ，ẏ = v·cosθ·sinψ，ż = v·sinθ。航向变化率由滚转角 φ 决定：ψ̇ = g·tanφ / v。由此可得最小转弯半径 R_min = v² / (g·tanφ_max)。可见转弯半径与速度平方成正比，速度越大越难转弯，这是固定翼航迹必须“提前转弯”的根本原因。

多旋翼可悬停、可全向移动，是完整约束系统。其水平运动由姿态倾斜产生：ẍ = g·tanφ（小角度近似），ÿ = g·tanθ。速度与加速度可独立控制，但受最大倾角限制，水平加速度上限约为 g·tanφ_max。多旋翼的约束更多体现在加速度与加加速度（jerk）上，而非转弯半径。

地面车辆与固定翼类似，属非完整约束，但受地形坡度影响显著。其运动学为 ẋ = v·cosψ，ẏ = v·sinψ，ψ̇ = v·tanδ / L，其中 δ 为前轮转角，L 为轴距。最小转弯半径 R_min = L / tanδ_max。此外，车辆还受侧倾与俯仰稳定性约束，横向加速度 a_y = v²/R 不得超过 g·(B/2h)（B 为轮距，h 为质心高度）。

水下潜航器在三维空间中运动，通常以航向角 ψ、俯仰角 θ 和速度 v 描述：ẋ = v·cosθ·cosψ，ẏ = v·cosθ·sinψ，ż = v·sinθ。其转弯半径同样受速度与最大偏航力矩限制，且俯仰角变化率受浮心与重心配置约束，通常远小于空中平台。

二、最小转弯半径与曲率约束

最小转弯半径是固定翼与地面车辆航迹规划中最硬的几何约束。对固定翼，若巡航速度 v = 25 m/s，最大滚转角 φ_max = 45°，则 R_min = 25² / (9.81 × tan45°) ≈ 63.7 m。这意味着航迹上任意一点的曲率 κ = 1/R 不得超过 1/63.7 ≈ 0.0157 m⁻¹。若规划器在障碍物拐角处生成一个半径 30 m 的转弯，该航迹在几何上可行，但固定翼实际无法跟踪，必须把转弯半径放大到 63.7 m 以上，或降低速度。实际规划中常采用“Dubins 曲线”或“Clothoid 回旋曲线”来保证曲率连续且不超过上限。

对地面车辆，若轴距 L = 2.7 m，最大前轮转角 δ_max = 35°，则 R_min = 2.7 / tan35° ≈ 3.86 m。但车辆还受侧倾约束：若轮距 B = 1.6 m、质心高度 h = 0.7 m，则横向加速度上限 a_y ≤ g·(B/2h) = 9.81 × (1.6/1.4) ≈ 11.2 m/s²。在 R = 10 m 的弯道上，允许速度 v ≤ √(a_y·R) = √(11.2 × 10) ≈ 10.6 m/s。可见转弯半径与速度必须联合校核。

三、爬升率、俯仰角与速度/加速度限制

爬升率 V_z 与俯仰角 θ、速度 v 的关系为 V_z = v·sinθ。固定翼巡航时 θ 通常限制在 ±15° 以内，若 v = 25 m/s，则最大爬升率约 25 × sin15° ≈ 6.5 m/s。多旋翼的爬升率独立于水平速度，典型上限为 5～8 m/s，但受电池功率限制，持续爬升会显著缩短续航。水下潜航器的俯仰角通常限制在 ±20° 以内，且俯仰变化率受浮心与重心配置约束，典型值不超过 5°/s，因此其航迹在垂直方向的变化必须非常平缓。

速度与加速度限制则决定航迹的时间参数化。固定翼有失速速度 v_stall，航迹速度不得低于 1.2～1.3 倍失速速度；多旋翼有最大水平速度与最大上升/下降速度；地面车辆有最大行驶速度与最大加减速度；潜航器有最大航速与最大纵向加速度。规划器在生成几何航迹后，需沿航迹弧长 s 进行速度调度，使 v(s) 满足 v_min ≤ v(s) ≤ v_max 且 |dv/dt| ≤ a_max。若某段航迹的曲率要求速度低于 v_min，则该段航迹不可行，必须重新规划几何形状。

四、教学示例：固定翼航迹约束校核

假设某固定翼平台参数为：v_cruise = 22 m/s，v_stall = 12 m/s，φ_max = 40°，θ_max = 15°，V_z_max = 5 m/s，a_max = 2 m/s²。规划器输出一段航迹，在 s = 100 m 处曲率 κ = 0.02 m⁻¹，航迹倾角 γ = 12°，速度 v = 20 m/s。校核如下：R = 1/0.02 = 50 m，而 R_min = 22² / (9.81 × tan40°) ≈ 58.8 m，故曲率超限，该点不可行；倾角 12° < 15°，满足；速度 20 m/s > 1.2 × 12 = 14.4 m/s，满足。结论是该航迹需在 s = 100 m 附近增大转弯半径或降低速度至 v ≤ √(g·tanφ_max·R) = √(9.81 × 0.839 × 50) ≈ 20.3 m/s，但 20.3 m/s 仍高于 20 m/s，说明仅降速无法满足，必须增大 R 至 58.8 m 以上。

五、四类平台约束对照