4. 路径规划算法
路径规划，说白了就是让无人机知道怎么从一个点飞到另一个点。你可能会想，直接飞直线不就行了？嗯，实际项目里远没那么简单。地形障碍、动力限制、任务需求，都会逼着你选择不同的路径生成方式。
我个人习惯把航点路径生成分成三个层次：能不能飞、好不好飞、漂不漂亮。直线插值解决第一个问题，贝塞尔曲线解决第二个，B样条曲线则兼顾了后两个。今天我们就一个一个拆开来看。
核心观点：没有最好的算法，只有最合适的场景。直线插值适合巡检、测绘等刚性任务；贝塞尔曲线适合需要精确控制起止姿态的起降段；B样条曲线则是复杂地形巡检和高速飞行的首选。
4.1 直线插值：最朴素但最可靠
直线插值，就是你在两个航点之间均匀插入中间点。比如从A点到B点，距离100米，你每隔10米插一个点，就得到10个中间航点。代码实现非常简单：
def linear_interpolate(p1, p2, num_points):
"""直线插值生成中间航点"""
points = []
for i in range(1, num_points + 1):
t = i / (num_points + 1)
x = p1[0] + t * (p2[0] - p1[0])
y = p1[1] + t * (p2[1] - p1[1])
z = p1[2] + t * (p2[2] - p1[2])
points.append((x, y, z))
return points我在项目中遇到过一个问题：直线插值生成的路径在转弯处是硬折角。无人机飞到拐点必须减速、转向、再加速，效率很低。你想想看，如果航线有几十个拐点，飞完一趟得多花多少时间？
我的习惯：直线插值只用在测绘、巡检这类对路径平滑度要求不高的任务。如果必须用，我会在拐点处加一个圆弧过渡，半径至少是无人机最小转弯半径的1.5倍。
4.2 贝塞尔曲线：优雅但全局牵一发而动全身
贝塞尔曲线的核心思想是用控制点来"牵引"曲线。你不需要直接定义路径上的每个点，只需要给出几个控制点，曲线就会自动生成一条平滑路径。最常用的是三次贝塞尔曲线，需要4个控制点。
def bezier_curve(p0, p1, p2, p3, num_points=100):
"""三次贝塞尔曲线生成"""
points = []
for i in range(num_points + 1):
t = i / num_points
# 伯恩斯坦基函数
x = (1-t)**3 * p0[0] + 3*(1-t)**2*t * p1[0] + 3*(1-t)*t**2 * p2[0] + t**3 * p3[0]
y = (1-t)**3 * p0[1] + 3*(1-t)**2*t * p1[1] + 3*(1-t)*t**2 * p2[1] + t**3 * p3[1]
z = (1-t)**3 * p0[2] + 3*(1-t)**2*t * p1[2] + 3*(1-t)*t**2 * p2[2] + t**3 * p3[2]
points.append((x, y, z))
return points贝塞尔曲线有个特点：移动任何一个控制点，整条曲线都会改变。这在某些场景下是优点，但在复杂航线设计里就成了痛点。我曾经给一个电力巡检项目设计航线，有30多个塔位需要绕行。用贝塞尔曲线，调整一个塔位的控制点，后面十几个塔位的路径全变了，气得我直接换方案。
避坑指南：贝塞尔曲线不适合长距离、多航点的航线设计。它更适合起降过渡段、精确入轨段等局部路径。如果你需要全局平滑，请直接看B样条。
4.3 B样条曲线：局部控制才是王道
B样条曲线解决了贝塞尔曲线的全局影响问题。它的核心思想是：每个控制点只影响曲线的一段区域。你调整第5个控制点，第3个和第7个之间的曲线会变化，但第1个和第10个完全不受影响。
B样条还有一个杀手级特性：C²连续。这意味着曲线的位置、速度、加速度都是连续的。无人机飞这种路径，电机转速变化平缓，不会出现突然的加减速，电池续航和飞行稳定性都会提升。
def bspline_basis(i, k, t, knots):
"""B样条基函数递归计算（Cox-de Boor公式）"""
if k == 0:
return 1.0 if knots[i] <= t < knots[i+1] else 0.0
else:
# 计算两个权重
w1 = 0.0
if knots[i+k] - knots[i] != 0:
w1 = (t - knots[i]) / (knots[i+k] - knots[i])
w2 = 0.0
if knots[i+k+1] - knots[i+1] != 0:
w2 = (knots[i+k+1] - t) / (knots[i+k+1] - knots[i+1])
return w1 * bspline_basis(i, k-1, t, knots) + w2 * bspline_basis(i+1, k-1, t, knots)关键参数：B样条的阶数k决定了曲线的平滑程度。k=2是抛物线（C¹连续），k=3是三次样条（C²连续）。我个人建议至少用k=3，除非你的飞控性能极差。
4.4 三种算法的选型对比
我把三种算法的核心差异整理成了一张表，方便你快速决策：
| 对比维度 | 直线插值 | 贝塞尔曲线 | B样条曲线 |
|---|---|---|---|
| 计算复杂度 | O(n) | O(n) | O(n·k²) |
| 连续性 | C⁰（位置连续） | C¹（速度连续） | C²（加速度连续） |
| 局部控制 | 不支持 | 不支持 | 支持 |
| 起止方向控制 | 不支持 | 支持 | 支持（需额外约束） |
| 路径长度 | 最短 | 中等 | 中等偏长 |
| 内存占用 | 低 | 低 | 中（需存储节点向量） |
| 推荐场景 | 简单巡检、测绘 | 起降过渡、精确入轨 | 复杂地形、高速飞行 |
我的经验：如果你不确定选哪个，先试B样条。它虽然实现起来比直线插值复杂一点，但后期调整成本最低。我在一个山地巡检项目里，用B样条一次生成200多个航点，后期只调整了3个控制点就完成了航线优化，省了大半天时间。
4.5 实战建议：混合使用才是最优解
说了这么多，你可能觉得B样条最厉害。但实际项目中，我很少只用一种算法。举个例子：
- 起降段：用贝塞尔曲线，精确控制起飞方向和降落角度。
- 巡航段：用B样条曲线，保证平滑飞行和局部可调。
- 目标点附近：用直线插值，确保无人机精确到达每个任务点。
这种混合策略，既保证了飞行效率，又降低了计算负担。你想想看，如果全程用B样条，起降段的控制点调整反而会引入不必要的曲线波动。分开处理，各取所长，才是工程上的最优解。
注意：混合使用不同算法时，必须在算法衔接处做连续性校验。我曾经因为忘记检查，导致无人机在算法切换点出现速度跳变，差点炸机。现在我的代码里一定会加一段连续性检测逻辑。
好了，路径规划算法就聊到这里。记住一句话：算法是工具，不是目的。你的目标是让无人机安全、高效地完成任务，而不是追求曲线有多漂亮。下次遇到航线设计任务，先想清楚需求，再选算法，别一上来就上B样条。