📚 课程目录第3章路径规划基础

3、路径规划基础：从定义到实战

各位同学，今天我们来聊聊路径规划的基础。说实话，我刚入行那会儿，觉得路径规划不就是让车从A走到B嘛，有什么难的？直到第一次在田里调试，看着农机在田头来回打转，我才意识到——这里面的门道深着呢。

3.1 路径规划问题定义

路径规划，说白了就是回答三个问题：

我在哪？——农机当前的位姿（位置+朝向）
我要去哪？——目标点或作业区域
怎么去？——一条无碰撞、可执行的路径

用数学语言描述就是：给定起始状态 q_start、目标状态 q_goal、环境地图 M，找到一条连续路径 τ: [0,1] → C_free，使得 τ(0)=q_start，τ(1)=q_goal。

关键约束条件：

运动学约束——农机有最小转弯半径，不能原地掉头
碰撞约束——路径不能碰到障碍物、田埂、电线杆
作业约束——覆盖区域不能遗漏，也不能重复太多

我在项目中遇到过最头疼的问题，就是忽略了农机的转弯半径。算法规划出一条完美的直线路径，结果农机到田头根本转不过来，直接压了庄稼。嗯，从那以后，我每次做规划都会先把运动学模型写进去。

3.2 全局路径规划 vs 局部路径规划

这两个概念，我习惯用一个比喻来解释：

全局路径规划 = 你出发前看地图，规划从北京到上海走哪条高速
局部路径规划 = 开车途中遇到修路，临时绕行一小段

你想想看，如果只有全局规划，遇到突发障碍就傻眼了；如果只有局部规划，那可能永远到不了目的地。所以实际系统中，两者是配合使用的。

对比维度	全局路径规划	局部路径规划
输入信息	先验地图（卫星图、GIS数据）	实时传感器（激光雷达、视觉）
规划范围	整个作业地块	农机周围几米到几十米
计算频率	作业前一次或几次	每秒多次（实时更新）
典型算法	A*、Dijkstra、RRT	DWA、TEB、MPC
输出	参考路径（粗粒度）	控制指令（细粒度）

我的经验：全局规划可以容忍几十厘米的误差，但局部规划必须精确到厘米级。曾经有个项目，全局路径规划用5米分辨率的地图，结果局部规划死活跟不上——因为地图精度太低，局部规划器以为自己在平地上，实际已经开到沟里了。

3.3 路径平滑性指标

路径平滑性，说白了就是农机跑起来舒不舒服。如果路径拐来拐去，不仅乘客难受，对农机底盘和转向机构也是折磨。

我常用的几个指标：

曲率连续性：路径的曲率不能突变。G0连续（位置连续）是最低要求，G2连续（曲率连续）才是理想状态。
最大曲率约束：路径上任意一点的曲率不能超过农机的最小转弯半径对应的曲率。公式很简单：κ_max ≤ 1 / R_min。
曲率变化率：单位弧长上曲率的变化量。这个指标直接影响转向机构的磨损。
路径长度：在满足上述约束的前提下，路径越短越好。

避坑指南：我曾经在一个项目中，只检查了最大曲率，没检查曲率变化率。结果路径上有一段曲率从0.01突变到0.05，虽然没超限，但转向电机直接过载报警。从那以后，我每次都会加上曲率变化率的约束。

平滑性评估的代码片段，我一般这样写：

def evaluate_smoothness(path):
    # path: [(x0,y0), (x1,y1), ..., (xn,yn)]
    curvatures = []
    for i in range(1, len(path)-1):
        # 三点计算曲率
        p1, p2, p3 = path[i-1], path[i], path[i+1]
        curvature = compute_curvature(p1, p2, p3)
        curvatures.append(curvature)
    
    max_curv = max(curvatures)
    curv_rate = max(abs(curvatures[i+1] - curvatures[i]) 
                    for i in range(len(curvatures)-1))
    
    return {
        'max_curvature': max_curv,
        'curvature_rate': curv_rate,
        'is_smooth': max_curv < 0.1 and curv_rate < 0.02
    }

3.4 作业覆盖率概念

覆盖率，是农业路径规划特有的指标。工业机器人只需要从A到B，但农机必须把整个地块都走一遍。

覆盖率定义很简单：

覆盖率 = 已作业面积 / 地块总面积 × 100%

但实际中，有几个坑要注意：

重复率：同一区域被多次覆盖的比例。重复率太高，浪费燃油和时间。
遗漏率：未被覆盖的区域比例。遗漏率必须为0，否则要补喷。
边界覆盖率：地块边缘的覆盖情况。农机在边界掉头时，容易留下三角形未覆盖区域。

实战经验：我做过一个对比实验，同样的地块，用简单的往返式路径，覆盖率98.5%，重复率12%；用优化的牛耕式路径，覆盖率99.8%，重复率只有3%。别小看这1.3%的覆盖率差距，对于100亩的地，就是1.3亩没种上，损失不小。

覆盖率计算时，我习惯用栅格化方法：

def compute_coverage(path, field_polygon, swath_width):
    # 将地块离散化为栅格
    grid = discretize_field(field_polygon, resolution=0.1)
    total_cells = len(grid)
    
    # 模拟农机作业轨迹
    covered = set()
    for x, y, heading in path:
        # 根据作业幅宽标记覆盖区域
        swath = get_swath_cells(x, y, heading, swath_width)
        covered.update(swath)
    
    coverage = len(covered) / total_cells * 100
    return coverage

这里要注意，栅格分辨率不能太大，否则会漏掉细小的未覆盖区域。我一般设0.1米，对于农机作业来说足够了。