巡检车在甘蔗田里跑，卡住是家常便饭。可能是轮子陷进泥里，可能是两侧甘蔗秆夹住了，也可能是 Nav2 规划了一条窄缝路但车身太宽过不去。
Nav2 自带的恢复行为——原地旋转、后退、清除代价地图——在开阔环境还行，甘蔗田里经常越转越卡。我们需要一个更聪明的脱困策略：先看哪个方向能走，再往那个方向走。
核心思路：射线投射 + 楔形评分
算法分三步：
- 从 local costmap 出发，360° 分成 12 个楔形区域
- 每个楔形投射 5 条射线，算平均自由距离
- 选得分最高的方向：距离远 + 方向偏前方优先
1
2
3
4
5
6
7
| 楔形 0
/ | \
/ 射线射线射线射线射线 \
楔形11 🤖 楔形 1
\ /
\ | /
楔形 6（正后方）
|
射线投射：逐格走，碰到障碍停下
1
2
3
4
5
6
7
8
9
10
11
| def _cast_ray(self, grid, start_col, start_row, angle, max_cells, width, height, resolution):
dx = math.cos(angle)
dy = math.sin(angle)
for step in range(1, max_cells + 1):
col = int(start_col + dx * step)
row = int(start_row + dy * step)
if col < 0 or col >= width or row < 0 or row >= height:
return step * resolution
if grid[row, col] >= self.lethal_threshold:
return (step - 1) * resolution
return max_cells * resolution
|
逐格步进，简单粗暴但可靠。max_cells = 1.0m / resolution，也就是最远看 1 米。在甘蔗田里 1 米够了——太远了看出去全是障碍，没有指导意义。
楔形评分：远 + 偏前方 = 好
1
2
3
4
5
6
7
8
9
10
11
12
| for i in range(self.num_wedges):
wedge_center = -math.pi + (i + 0.5) * wedge_angle
ray_distances = []
for r in range(self.rays_per_wedge):
ray_angle = wedge_center + (r - 2) * (wedge_angle / 5)
free_dist = self._cast_ray(grid, robot_col, robot_row, ray_angle, ...)
ray_distances.append(free_dist)
avg_distance = sum(ray_distances) / len(ray_distances)
angle_diff = abs(normalize_angle(wedge_center - robot_yaw))
penalty = 0.3 * (angle_diff / math.pi)
score = avg_distance - penalty
|
0.3 的航向惩罚权重意味着：正前方 0.7m 的通道和正后方 1.0m 的通道得分相同。如果前后都差不多远，优先往前走——因为车大概率是从后面来的，往前走是脱离当前困境最自然的方向。
全方向都堵死了？纯后退
如果所有楔形的平均自由距离都低于 0.15m，说明车被完全包围——360° 都是障碍。这时候任何转向都没用，唯一的选择是倒车。
1
2
3
4
5
6
7
| if all_low:
return EscapeVector(
angle=normalize_angle(robot_yaw + math.pi),
distance=best_distance,
score=best_score,
fallback=True,
)
|
降级模式不旋转，直接以 0.1 m/s 的速度倒退最多 3 秒。低速倒退是为了安全——万一后面也有东西，撞上不至于太惨。
两步执行：先转向，再前进
选定方向后，脱困动作分两步：
1
2
3
4
5
6
7
8
9
10
11
12
13
14
| def execute_recovery(self, escape, robot_yaw, publish_twist_fn, stop_fn, is_safety_blocked_fn):
yaw_error = normalize_angle(escape.angle - robot_yaw)
rotate_duration = abs(yaw_error) / 0.6
if rotate_duration > 0.05:
publish_twist_fn(0.0, rotate_dir * 0.6, min(rotate_duration, 5.0))
forward_duration = min(escape.distance / 0.10, 5.0)
if forward_duration > 0.05:
publish_twist_fn(0.10, 0.0, min(forward_duration, 5.0))
stop_fn()
return RecoveryResult.SUCCESS
|
两步之间都有安全检查——如果安全节点发了刹车信号，立即停止。超时 15 秒强制结束，防止脱困动作无限执行。
为什么不用 Nav2 的恢复行为
Nav2 的 BackUp 只会直线后退，Spin 只会原地旋转，ClearCostmap 只是清除障碍记录——三个行为互相独立，不感知代价地图的内容。
12 楔射线算法的优势在于看地图选方向。如果车卡在通道中间，左边是墙右边是缝，Nav2 的后退可能会把车推到更窄的地方；射线算法知道右边 30° 有 0.8m 的通道，会先转向再前进。
而且这个算法是纯计算模块，不依赖 ROS 节点——可以在任何地方调用，甚至可以在安全节点里用。
几个参数
| 参数 | 默认值 | 说明 |
|---|
num_wedges | 12 | 360° / 12 = 30° 一个楔形 |
rays_per_wedge | 5 | 每楔 5 条射线，抗噪声 |
max_ray_distance_m | 1.0 | 看多远 |
heading_penalty | 0.3 | 方向权重 |
escape_linear_vel | 0.10 | 脱困线速度，慢一点安全 |
escape_angular_vel | 0.6 | 脱困角速度 |
total_timeout_sec | 15.0 | 总超时 |
30° 一个楔形是经验值——再细了（比如 10°）方向太多不好选，再粗了（比如 60°）精度不够。5 条射线抗单条射线噪声：万一某条射线正好穿过一个障碍物间隙，其他 4 条会把它拉回来。