4、航线规划基础算法：Dubins曲线、Clothoid曲线、直线与圆弧插值
航线规划，说白了就是让无人机知道怎么从A点飞到B点。但现实世界不是一张白纸，你不能直接拉一条直线就完事——飞机有最小转弯半径，有速度约束，有时候还得绕过禁飞区。今天咱们聊的这几种基础算法，就是解决「怎么飞得出来」这个核心问题的。
我个人习惯把这类算法分成两类：一类是几何路径生成，一类是平滑过渡。Dubins曲线和直线圆弧插值属于前者，Clothoid曲线属于后者。嗯，咱们一个一个来看。
4.1 直线与圆弧插值——最朴素的路径拼接
先讲最简单的。直线加圆弧，说白了就是「直着飞，拐弯时画个弧」。你想想看，无人机在航点之间切换，总不能到了点再急转弯吧？那得把飞机甩出去。
直线插值没什么好说的，就是两点一线。圆弧插值的关键在于：你得知道转弯半径和圆心位置。
核心公式（圆弧插值）：
// 给定起点P1、终点P2、转弯半径R
// 计算圆心O和圆弧角度θ
O.x = P1.x + R * sin(ψ)
O.y = P1.y - R * cos(ψ)
θ = atan2(P2.y - O.y, P2.x - O.x) - atan2(P1.y - O.y, P1.x - O.x)我在项目中遇到过一个问题：如果两个航点距离太近，圆弧根本画不出来。这时候要么增大转弯半径，要么在中间加一个过渡点。避坑指南：两个航点之间的直线段长度，至少要是转弯半径的2倍，否则你的航线会变成一团乱麻。
| 参数 | 说明 | 典型值 |
|---|---|---|
| 转弯半径 R | 无人机最小转弯半径 | 5-20m（视机型） |
| 圆弧角度 θ | 转弯扫过的角度 | 0°-180° |
| 直线段长度 L | 两圆弧之间的直线距离 | ≥ 2R |
4.2 Dubins曲线——最短路径的数学答案
Dubins曲线是我个人非常喜欢的一个算法。它解决的是这样一个问题：给定起点和终点的位置、朝向，怎么用最短的路径连起来？答案是——最多三段路径：左转、直行、右转，或者它们的组合。
为什么是左转和右转？因为固定翼无人机只能朝一个方向转弯（左转或右转），不能原地掉头。Dubins曲线把这个问题抽象成了几何问题，给出了6种可能的路径组合：
- LSL：左转-直行-左转
- RSR：右转-直行-右转
- LSR：左转-直行-右转
- RSL：右转-直行-左转
- RLR：右转-左转-右转
- LRL：左转-右转-左转
你可能会问：为什么会有RLR和LRL这种纯转弯的路径？因为当起点和终点朝向相反时，直行反而绕远路。我记得有一次做固定翼的航线规划，起点和终点朝向差了170度，用LSL路径算出来要飞300米，换成LRL路径只要150米——差距就是这么明显。
实用技巧：Dubins曲线的计算其实不复杂。先根据起点和终点的朝向画出两个转弯圆（左转圆和右转圆），然后找两个圆的公切线。哪条切线最短，哪条就是最优路径。我建议你在代码里把这6种路径都算一遍，取最短的那个——别偷懒，因为不同场景下最优路径可能完全不同。
4.3 Clothoid曲线——让转弯更平滑
直线加圆弧有个问题：曲率突变。从直线（曲率为0）突然变成圆弧（曲率为1/R），飞机得瞬间打满舵。实际飞行中，这会导致严重的过载，甚至让飞控系统震荡。
Clothoid曲线（也叫欧拉螺旋线）就是来解决这个问题的。它的曲率是线性变化的——从0慢慢增加到1/R，再慢慢降回0。说白了，就是让飞机「柔和地」进入转弯，再「柔和地」退出。
Clothoid曲线参数方程：
x(s) = ∫₀ˢ cos(κ₀·t + ½·c·t²) dt
y(s) = ∫₀ˢ sin(κ₀·t + ½·c·t²) dt
其中 c 为曲率变化率，κ₀ 为初始曲率这个积分没有解析解，只能用数值方法算。我在项目中用的是Fresnel积分近似，精度控制在1cm以内就够了——毕竟无人机定位误差本身就有几厘米。
注意：Clothoid曲线虽然平滑，但计算量比Dubins曲线大得多。如果你的飞控芯片算力有限（比如STM32F4系列），建议只在关键转弯点使用Clothoid，其他位置用Dubins或直线圆弧插值。我曾经在一个项目中把所有转弯都换成了Clothoid，结果飞控的CPU占用率从30%飙到了85%——得不偿失。
4.4 三种算法的对比与选型
说了这么多，到底该用哪个？我整理了一张对比表，你一看就明白：
| 算法 | 平滑度 | 计算量 | 适用场景 |
|---|---|---|---|
| 直线+圆弧 | 中等（曲率突变） | 低 | 低速无人机、巡检航线 |
| Dubins曲线 | 中等（曲率突变） | 低 | 固定翼、最短路径规划 |
| Clothoid曲线 | 高（曲率连续） | 高 | 高速飞行、载人无人机 |
我个人习惯是：低速场景用直线圆弧，固定翼用Dubins，高速或载人场景用Clothoid。当然，实际项目中往往是混合使用——比如航点之间用Dubins生成路径，然后在转弯处用Clothoid做平滑过渡。
4.5 知识体系总览
下面这张图把本章的核心逻辑串起来了。你可以看到，三种算法并不是互斥的，而是可以组合使用的：
你看，从输入到输出，再到实际应用，整个链路是清晰的。实际开发中，我建议你先用Dubins曲线快速生成一条可行路径，然后在转弯处用Clothoid做平滑——这样既保证了路径最短，又保证了飞行平滑。嗯，这就是所谓的「工程折中」。