田里跑自主机器人，路径规划是块硬骨头。几种常用算法试下来，我觉得这五种必须知道：A*、Dijkstra、DWA、RRT 和人工势场。各有各的脾气，用好了才稳。
先把地图和定位弄对
路径算得再好，基础数据差也没用。普通 GPS 在田里晃个几米太常见，RTK 几乎是刚需。写一个 Go 的客户端从基准站拉差分数据，移动站就可以把定位干到厘米级。
client.Connect("ntrip.base.com:2101")
client.Auth("user", "pass")
client.MountPoint("/RTCM3")
go func() {
for data := range client.Stream() {
rover.ApplyCorrection(data)
}
}()
有了高精度坐标，再融合 IMU 的姿态补偿和卡尔曼滤波，能输出稳定的位姿。至于地图，常规就两种：栅格地图把田切成小格子，标记可走、不可走，适合播种喷洒这类精细活；拓扑地图只留关键节点和道路连接，存得少，适合大范围快速导航。我一般全局用拓扑，局部再转成栅格去避障。各有各的适用场合：
| 指标 | 栅格地图 | 拓扑地图 |
|---|---|---|
| 存储开销 | 高 | 低 |
| 更新频率 | 实时动态 | 周期性 |
判定一个格子是不是障碍，有时还得动态调阈值，比如作物高了就把栅格值上调，用下面这个小函数就行：
def is_obstacle(grid_map, x, y, threshold=0.8):
return grid_map[x][y] > threshold
这张图是感知到执行的闭环：传感器、摄像头把数据送到边缘端处理，再汇总到云端规划，最后指令下到行走机构。说起来简单，做起来坑不少。
整个导航过程基本循环就是：
graph TD
A[开始] --> B{获取传感器数据}
B --> C[执行 SLAM 建图]
C --> D[调用路径规划算法]
D --> E[生成控制指令]
E --> F[驱动电机执行]
F --> G[实时修正位置]
G --> B
五种算法逐个看
A*：全局规划的标配
A* 的灵魂是 f(n)=g(n)+h(n)，用一个优先队列挑 f 最小的节点扩展，出来的就是最短路径。实现起来并不复杂：
def a_star(grid, start, goal):
open_set = PriorityQueue()
open_set.put((0, start))
came_from = {}
g_score = {start: 0}
while not open_set.empty():
current = open_set.get()[1]
if current == goal:
return reconstruct_path(came_from, current)
for neighbor in get_neighbors(current, grid):
tentative_g = g_score[current] + cost(current, neighbor)
if tentative_g < g_score.get(neighbor, float('inf')):
came_from[neighbor] = current
g_score[neighbor] = tentative_g
f_score = tentative_g + heuristic(neighbor, goal)
open_set.put((f_score, neighbor))
return []
在田里用的时候，启发函数换成曼哈顿距离，但要把土壤湿度、坡度这些折算进代价。让机器人自动绕开烂泥地，规划出来的路才真能走。
Dijkstra：慢是慢了，但绝对最优
Dijkstra 没有启发引导，硬生生往外扩散，保证给你最优解。在几百个节点的田块图上，这点延迟根本不算事儿。最小堆版本：
import heapq
def dijkstra(graph, start, end):
queue = [(0, start)]
distances = {node: float('inf') for node in graph}
distances[start] = 0
previous = {}
while queue:
current_dist, u = heapq.heappop(queue)
if u == end: break
for v, weight in graph[u].items():
new_dist = current_dist + weight
if new_dist < distances[v]:
distances[v] = new_dist
previous[v] = u
heapq.heappush(queue, (new_dist, v))
return previous, distances[end]
建图时把平坦区权重设 1，斜坡设 1.5，障碍物就不连边。Dijkstra 出结果很稳，不像 A* 启发函数设计不好会走弯路。
几个算法性能简单比一下：
| 算法 | 路径最优性 | 计算耗时 (ms) |
|---|---|---|
| Dijkstra | 最优 | 48 |
| A* | 最优 | 32 |
| 贪心 | 次优 | 18 |
时间是在一张普通栅格图上测的，具体得看自己的地图。
DWA：实时避障的利器
全局路径有了，行驶中还得躲开突然窜出的东西。DWA 的做法是在当前速度允许的范围内，枚举一堆 (v, ω) 组合，模拟一小段轨迹，然后打分。分数看三点：朝向目标近不近、离障碍远不远、速度快不快。
void DWAPlanner::computeVelocityCommands(double& cmd_v, double& cmd_w) {
double vs[5] = {robot_v_min, robot_v_max, robot_w_min, robot_w_max, acc_limit};
for (double v = vs[0]; v <= vs[1]; v += dv) {
for (double w = vs[2]; w <= vs[3]; w += dw) {
if (isInDynamicWindow(v, w, vs)) {
double score = evaluateTrajectory(v, w);
if (score > best_score) {
best_v = v; best_w = w;
}
}
}
}
}
调参可以参考下表，然后再上路微调。
| 参数名 | 作用 | 推荐值（TurtleBot3） |
|---|---|---|
| max_vel_x | 最大前进速度（m/s） | 0.26 |
| min_vel_theta | 最小旋转速度（rad/s） | 0.4 |
| sim_time | 轨迹预测时间（s） | 1.0 |
泥地惯性大，加速度就得设低些。
RRT：没地图也能撞出路来
很多田根本没有现成地图，RRT 这时候就管用了。它纯靠随机采样往外长树，碰到障碍就换个方向，直到有分支靠近目标。
def rrt_step(tree, goal, step_size):
random_point = sample_free_space()
nearest_node = find_nearest(tree, random_point)
new_point = extend_towards(nearest_node, random_point, step_size)
if is_collision_free(nearest_node, new_point):
tree.add_node(new_point)
tree.add_edge(nearest_node, new_point)
if distance(new_point, goal) < step_size:
return True
return False
这样找出来的路弯弯绕绕，直接让机器走会急转磨轮胎。所以必须平滑一下，我用三次 B 样条插值：
def smooth_path(path, degree=3):
t = np.linspace(0, 1, len(path))
t_smooth = np.linspace(0, 1, 5 * len(path))
x, y = zip(*path)
cs_x, cs_y = CubicSpline(t, x), CubicSpline(t, y)
return [(cs_x(t_val), cs_y(t_val)) for t_val in t_smooth]
平滑后再输给 PID 控制器，走起来才像样。
人工势场：简单，但容易陷进去
人工势场就建一个虚拟势场：目标吸，障碍推，合力方向就是该走的方向。代码也很短：
def compute_total_force(robot_pos, goal_pos, obstacles):
attractive = k_att * (goal_pos - robot_pos)
repulsive = np.zeros(2)
for obs in obstacles:
dist = np.linalg.norm(robot_pos - obs)
if dist < d0:
repulsive += k_rep * (1/dist - 1/d0) * (1/dist**2) * (robot_pos - obs)/dist
return attractive + repulsive
缺点就是容易陷入局部极小，引力斥力扯平了机器人就不动了。一般加点随机扰动强行推出来，或者直接用 A* 做全局引导。不能单靠它干活。
辅助环节：融合、控制与取舍
多传感器融合也是个必须过的坎。激光雷达和摄像头的数据得先时间对齐，我一般用最近邻：
def sync_sensors(lidar_ts, camera_ts):
aligned = []
for lidar_t in lidar_ts:
closest_cam = min(camera_ts, key=lambda x: abs(x - lidar_t))
aligned.append((lidar_t, closest_cam))
return aligned
对齐之后再做融合，看需要是前融合还是后融合：
| 方法 | 优点 | 适用场景 |
|---|---|---|
| 前融合 | 信息完整，原始级整合 | 静态地图构建 |
| 后融合 | 计算快，鲁棒 | 动态障碍跟踪 |
跟踪出动态目标后，再用来更新栅格地图，路径规划就能实时避开。
路径生成了，跟踪也得稳。最简单的 PID 横向控制就能对付：
double error = target_y - current_y;
integral += error * dt;
double derivative = (error - prev_error) / dt;
double output = Kp * error + Ki * integral + Kd * derivative;
prev_error = error;
参数用 Ziegler-Nichols 搞个初值，然后上车试。测过几组，Kp=1.2, Ki=0.08, Kd=0.2 的时候偏差能压到 10cm 以内，挺稳定的。这个方法跟具体机子关联大，自己的机器还得自己测。
未来的影子与现在的坑
现在不少地方开始在边缘设备上跑轻量模型做实时感知，比如用 Jetson 跑 YOLOv5 识别病虫害：
import cv2, torch
model = torch.hub.load('ultralytics/yolov5', 'yolov5s', device='cpu')
cap = cv2.VideoCapture("rtsp://camera-ip:554/stream")
while True:
ret, frame = cap.read()
if not ret: break
results = model(frame)
detections = results.pandas().xyxy[0]
for _, row in detections.iterrows():
if row['confidence'] > 0.6:
cv2.rectangle(frame, (int(row['xmin']), int(row['ymin'])),
(int(row['xmax']), int(row['ymax'])), (0,255,0), 2)
if len(detections[detections.confidence > 0.8]) > 0:
upload_to_cloud(compress_frame(frame))
还有能源这块，田里设备供电得自给。实际部署中有个大概的参考：
| 设备类型 | 功耗（W） | 供电方案 | 实测续航 |
|---|---|---|---|
| LoRa 网关 | 5 | 太阳能板 + 锂电池 | 连续阴天 7 天 |
| 巡检机器人 | 120 | 换电机器人自动补能 | 每日作业 16 小时 |
这些数字来自实实在在的部署，不是纸上谈兵。
每种算法都没有绝对的好坏。我的选择通常是：RTK 建好底图，A* 或 Dijkstra 做全局，DWA 管局部避障；碰到完全没地图的新田，先用 RRT 探路再平滑；人工势场偶尔拿来在线微调，但不会独立用。实际落地，把基础打扎实比追新概念靠谱得多。
