4、航线规划算法：直线插值、圆弧过渡、B样条曲线平滑、Dubins路径规划

航线规划，说白了就是解决「飞机怎么从一个点飞到另一个点」的问题。你可能会想，直接拉一条直线不就行了？嗯，实际飞起来可没那么简单。无人机有最小转弯半径，有速度约束，还有避障需求。我这些年调试过的飞机，因为航线规划太粗糙而炸掉的，至少有三架。

这一章，我们就把四种最常用的航线规划算法掰开揉碎讲清楚。从最简单的直线插值，到能处理复杂约束的Dubins路径，我会把每个算法的适用场景、实现细节、还有我踩过的坑，都告诉你。

4.1 直线插值：最基础，也最容易出问题

直线插值，就是把两个航点之间的路径用一条直线段表示。听起来简单吧？但这里有个坑——航点密度。我见过有人把两个相距500米的航点之间只插了一个中间点，结果飞机飞过去的时候，遇到侧风直接偏出去几十米。

直线插值的核心公式其实就一个线性插值：

// 两点之间的线性插值
P(t) = P0 + t * (P1 - P0),  t ∈ [0, 1]

// 实际代码实现
function linearInterpolate(p0, p1, numPoints) {
    const points = [];
    for (let i = 0; i <= numPoints; i++) {
        const t = i / numPoints;
        points.push({
            x: p0.x + t * (p1.x - p0.x),
            y: p0.y + t * (p1.y - p0.y),
            z: p0.z + t * (p1.z - p0.z)
        });
    }
    return points;
}

直线插值的优缺点很明显：

• 优点： 计算量极小，实现简单，适合直线航段较多的任务（比如测绘航线）。
• 缺点： 航点处速度方向突变，飞机会急转弯；没有平滑处理，不适合高速飞行。

4.2 圆弧过渡：让转弯更柔和

直线插值最大的问题是什么？航点处速度方向突变。你想想看，飞机以10m/s的速度飞到航点，突然要转90度——这谁受得了？

圆弧过渡的思路很简单：在两条直线段之间插入一段圆弧，让飞机沿着圆弧平滑转弯。圆弧的半径决定了转弯的急缓程度。

// 圆弧过渡核心逻辑
function arcTransition(p0, p1, p2, radius) {
    // p0->p1 和 p1->p2 之间插入圆弧
    const v1 = normalize({x: p1.x - p0.x, y: p1.y - p0.y});
    const v2 = normalize({x: p2.x - p1.x, y: p2.y - p1.y});
    
    // 计算转弯角度
    const angle = Math.acos(dot(v1, v2));
    
    // 计算圆弧起点和终点
    const arcStart = {
        x: p1.x - radius * v1.x / Math.tan(angle/2),
        y: p1.y - radius * v1.y / Math.tan(angle/2)
    };
    const arcEnd = {
        x: p1.x + radius * v2.x / Math.tan(angle/2),
        y: p1.y + radius * v2.y / Math.tan(angle/2)
    };
    
    // 生成圆弧上的点
    return generateArcPoints(arcStart, arcEnd, p1, radius);
}

圆弧过渡的适用场景：

• 多航点连续飞行（比如电力巡检的塔间航线）
• 需要保持恒定速度的任务
• 飞行器有明确的转弯半径约束

4.3 B样条曲线平滑：让航线像丝绸一样顺滑

圆弧过渡虽然解决了转弯问题，但它的平滑度是有限的——圆弧和直线段的连接处仍然有曲率突变。如果你想让航线真正「丝滑」，那就得上B样条曲线。

B样条曲线的核心思想是：用一组控制点来定义一条平滑曲线，曲线不一定要穿过控制点，但会被控制点「吸引」。这样做的好处是，你可以通过调整控制点的位置来局部修改航线，而不影响整体。

// 三次均匀B样条曲线
function bSplineInterpolate(controlPoints, numSamples) {
    const points = [];
    const n = controlPoints.length;
    
    for (let i = 0; i < n - 3; i++) {
        for (let j = 0; j < numSamples; j++) {
            const t = j / numSamples;
            const t2 = t * t;
            const t3 = t2 * t;
            
            // 三次B样条基函数
            const b0 = (1 - t) * (1 - t) * (1 - t) / 6;
            const b1 = (3 * t3 - 6 * t2 + 4) / 6;
            const b2 = (-3 * t3 + 3 * t2 + 3 * t + 1) / 6;
            const b3 = t3 / 6;
            
            points.push({
                x: b0 * controlPoints[i].x + b1 * controlPoints[i+1].x + 
                   b2 * controlPoints[i+2].x + b3 * controlPoints[i+3].x,
                y: b0 * controlPoints[i].y + b1 * controlPoints[i+1].y + 
                   b2 * controlPoints[i+2].y + b3 * controlPoints[i+3].y
            });
        }
    }
    return points;
}

B样条曲线的优缺点：

我个人习惯在航拍任务中优先使用B样条。有一次给一个景区做航拍，甲方要求视频画面不能有任何抖动。直线插值飞出来的画面，每到航点就晃一下；换成B样条之后，画面稳得像架在轨道上一样。

4.4 Dubins路径规划：固定翼的必修课

前面三种算法都是针对多旋翼或者不考虑动力学约束的情况。但如果你飞的是固定翼，那就必须考虑一个硬约束——最小转弯半径。固定翼不能悬停，不能原地转弯，它只能沿着圆弧飞行。

Dubins路径就是专门解决这个问题的。它的核心思想是：在起点和终点之间，用直线段和固定半径的圆弧段组合出一条可行路径。Dubins证明了，在二维平面上，最短的可行路径一定是以下六种组合之一：

• LSL： 左转-直线-左转
• LSR： 左转-直线-右转
• RSL： 右转-直线-左转
• RSR： 右转-直线-右转
• RLR： 右转-左转-右转
• LRL： 左转-右转-左转

// Dubins路径长度计算（简化版）
function dubinsLength(start, end, radius) {
    // 计算起点和终点的朝向
    const theta1 = start.yaw;
    const theta2 = end.yaw;
    
    // 计算相对位置
    const dx = end.x - start.x;
    const dy = end.y - start.y;
    
    // 计算六种路径的长度，取最短
    const paths = [
        { type: 'LSL', length: calculateLSL(dx, dy, theta1, theta2, radius) },
        { type: 'LSR', length: calculateLSR(dx, dy, theta1, theta2, radius) },
        { type: 'RSL', length: calculateRSL(dx, dy, theta1, theta2, radius) },
        { type: 'RSR', length: calculateRSR(dx, dy, theta1, theta2, radius) },
        { type: 'RLR', length: calculateRLR(dx, dy, theta1, theta2, radius) },
        { type: 'LRL', length: calculateLRL(dx, dy, theta1, theta2, radius) }
    ];
    
    return paths.reduce((min, p) => p.length < min.length ? p : min);
}

Dubins路径的适用场景：

• 固定翼无人机航线规划
• 需要精确控制起点和终点朝向的任务（比如降落航线）
• 有最小转弯半径约束的任何飞行器

4.5 四种算法的对比与选择

说了这么多，到底该用哪个？我整理了一个对比表，方便你快速决策：

嗯，四种算法都讲完了。你可能会问，实际项目中是不是只用一种？其实不是。我经常在一个任务里混合使用——直线段用直线插值，转弯处用圆弧过渡，关键航段用B样条优化。灵活组合，才是工程实践的真谛。