8465132

我重新按代码核对了。下面只讲刚交付的“航向结束版”，数值以这版 [planner.yaml](C:/Users/35341/Documents/Codex/2026-10-05/new-chat/outputs/urinay_local_planner/config/planner.yaml) 为准。

先记住：车辆按route_order顺序走，每一段只判断什么时候进入下一段。

起点直道 start_straight ↓ 识别并确认大半圆入口 大半圆 big_semicircle ↓ 接近终点＋航向合格＋前方空白 无锥桶段 coneless ↓ 接近模板终点＋航向合格 出口小直道 exit_straight ↓ 接近直道终点＋发现左转趋势 直角 right_angle ↓ 进入本段后累计左转达到85° 连接直道 corner_to_curve ↓ 识别并确认普通弯入口 普通小弯 small_curve ↓ 接近圆弧终点＋航向合格 返回起点 return_start ↓ 回到起点且满足计圈条件 停车完成

启动时，先积累可靠路径，才允许起步。

目前要求同时满足：

参数	当前值	含义
startup_accumulation_time	0.80 秒	至少积累这么长时间
startup_min_scans	10	至少收到10帧扫描
startup_min_confirmed_cones	4	至少4个确认锥桶
startup_min_selected_gates	2	至少2个选中门
startup_min_path_length	1.50 米	中线至少这么长

同时要求搜索有效、没有进入恢复模式。积累期间移动超过startup_max_motion: 0.08米，或转动超过startup_max_yaw_deg: 3.0度，会重新积累。

① 起点直道 → 大半圆：看前面的赛道形状。

起点直道按锥桶中线走。当识别到符合下一段半径、方向的圆弧入口，并且入口连续稳定，就切到大半圆。

这里有两种识别来源：已经确认的圆弧几何模型，或者正常双侧搜索得到的锥桶门中点拟合。

主要参数是：

参数	当前值	对切换的作用
route.start_straight.min_distance	0.0 米	本段走够此距离后，才允许识别下一段
route.start_straight.max_distance	20.0 米	超过此行程还没确认入口，停车
route.big_semicircle.radius	1.85 米	要找的圆弧半径，也决定后面的固定圆弧
route.big_semicircle.angle_deg	180.0°	这段执行左转半圆
fixed_track_curve_turn_deg	12.0°	门中点搜索路径至少出现这样的同向弯曲趋势
fixed_track_fit_tolerance	0.22 米	门中点与指定圆弧的拟合容差
fixed_track_entry_max_distance	4.0 米	最远接受多远的弯道入口
fixed_track_entry_lateral_tolerance	0.55 米	入口允许偏离参考前进方向多远
fixed_track_candidate_match_distance	0.45 米	连续帧识别出的入口位置允许变化多大
fixed_track_curve_confirm_frames	2 帧	入口连续稳定2帧后切段

连续入口的方向一致性，还使用fixed_track_exit_heading_deg: 34.0。

门中点拟合分支还要求至少3个当前锥桶支持的门，以及足够的圆弧覆盖；门宽依据track_width: 1.50和fixed_track_assist_gate_width_tolerance: 0.30判断。

几何模型分支则受geometry_enabled、geometry_inner_radius/outer_radius、geometry_inlier_tolerance、geometry_min_side_cones、geometry_min_arc_span_deg、geometry_confirmation_frames影响。这些是前面的几何识别参数，先不用和切段确认帧数一起调。

这里的12°是“前方路径弯了多少”，不是车已经转了12°。提前识别入口后，固定路径中会保留通往入口的接近段。

② 大半圆 → 无锥桶段：末端、航向、空白区一起判断。

目前必须同时满足：

• 固定路径剩余长度 ≤0.40 米。
• 车辆到固定终点的距离 ≤0.40 米。
• 车头与终点方向的误差 ≤34°。
• 沿指定方向的累计转角足够。
• 指定前方区域里没有当前锥桶。
• 上述条件连续满足2帧。

对应参数：

参数	当前值	作用
fixed_track_exit_distance	0.40 米	同时约束“剩余路径”和“距终点”
fixed_track_exit_heading_deg	34.0°	终点朝向容差，同时用于检查累计转角
fixed_track_gap_min_x	0.35 米	空白区前后范围的近端
fixed_track_gap_max_x	2.0 米	空白区前后范围的远端
fixed_track_gap_half_width	1.0 米	空白区左右各1米
fixed_track_gap_confirm_frames	2	连续空白确认帧数

当前累计转角下限计算为180－34＝146°，但转到146°不会单独触发切换，终点距离等条件也必须满足。

空白区没有锥桶，指的是上述局部区域，不要求整幅雷达都看不到锥桶。

③ 无锥桶段 → 出口小直道：看模板是否接近走完。

无锥桶段按你的固定模板行驶：

左转22.5° → 直线2.232米 → 右转45° → 直线2.232米 → 左转22.5°

模板形状由下面三个数组决定：

"route.coneless.lengths": [0.0, 2.232, 0.0, 2.232, 0.0] "route.coneless.radii": [1.0, 0.0, 1.0, 0.0, 1.0] "route.coneless.angles_deg": [22.5, 0.0, -45.0, 0.0, 22.5]

退出使用：

"route.coneless.exit": path fixed_track_exit_distance: 0.40 fixed_track_exit_heading_deg: 34.0

也就是：剩余≤0.40米、距终点≤0.40米、终点朝向误差≤34°，当帧切换。

这一段不要求出口重新出现锥桶。因此fixed_track_reacquire_frames和fixed_track_reacquire_distance不控制当前无锥桶段的退出。

进入出口小直道时，直线从车辆实际位置开始生成，方向沿用上一段的理论出口方向。

④ 出口小直道 → 直角：接近末端，再看搜索路径有没有转弯趋势。

这段固定直线长度是：

"route.exit_straight.lengths": [1.50]

切入直角要求同时满足：

• 剩余路径和距终点都 ≤fixed_track_exit_distance: 0.40米。
• 终点朝向误差 ≤fixed_track_exit_heading_deg: 34.0度。
• 辅助搜索路径出现符合下一段方向的转弯趋势，目前下一段是左转。

这里的趋势判断在源码中固定为约3°，不是上面的12°。它检查搜索路径前最多8个点，要求有效前进长度至少0.30米，并出现至少3°的同向累计转折；允许恢复路径提供趋势证据。

这些条件当帧满足就切段，这里不使用fixed_track_curve_confirm_frames的2帧确认。

如果车辆严格沿直线走，1.50米直道在走到约1.10米时，就可能进入允许切换的范围：

1.50－0.40＝1.10米

因此，觉得直角开始太早或太晚，优先核对：

• route.exit_straight.lengths：实际直道长度。
• fixed_track_exit_distance：允许提前接近终点多少距离。

切换时，以车辆当时的位置、航向激活L形直角引导。

⑤ 直角 → 连接直道：新版看车辆累计航向变化。

目前配置是：

"route.right_angle.angle_deg": 90.0 "route.right_angle.corner_in_length": 1.0 "route.right_angle.corner_out_length": 1.5 corner_exit_turn_deg: 85.0

各参数的分工：

参数	控制什么
angle_deg: 90.0	L形参考方向左转90°；填-90.0为右转
corner_in_length: 1.0	激活点到参考拐点的距离
corner_out_length: 1.5	拐点后的备用引导长度
corner_exit_turn_deg: 85.0	车辆实际累计转到多少度，判定本段结束

进入直角段时从零累计，沿指定方向净转角达到85°，当帧进入corner_to_curve。

这一切换不要求走满出口线，也不等待出口锥桶和连续确认。反向摆动会扣回累计值，跨越 ±180°会正确处理。

旧corner_handoff_*已停用；fixed_track_exit_distance和fixed_track_exit_heading_deg也不控制新版直角结束。

航向取自/odom_combined的姿态；上游融合了IMU时，使用的就是融合后的航向。

⑥ 连接直道 → 普通小弯：重新按锥桶搜索，确认下一圆弧入口。

进入corner_to_curve后，车辆按照正常搜索中线行驶，并以直角退出时的实际车头方向作为入口识别参考。

切换方法与“起点直道 → 大半圆”相同，主要区别是：

"route.corner_to_curve.min_distance": 0.0 "route.corner_to_curve.max_distance": 6.0 "route.small_curve.radius": 2.0 "route.small_curve.angle_deg": 90.0

含义是：

• 立即允许识别下一弯道。
• 寻找半径2米的左弯入口。
• 入口连续稳定2帧后，切到普通小弯。
• 本段行驶超过6米还没找到入口，则停车。

共用的识别参数仍是前面的fixed_track_curve_turn_deg、fixed_track_fit_tolerance、入口距离/横偏、位置匹配和确认帧数。

你当前代码中的普通小弯，确认入口后会跟踪固定圆弧。搜索中线可以提供有限横向修正，修正由fixed_track_correction_limit和fixed_track_correction_gain控制。

⑦ 普通小弯 → 返回起点：接近圆弧终点且航向合格。

当前：

"route.small_curve.exit": path "route.small_curve.angle_deg": 90.0 fixed_track_exit_distance: 0.40 fixed_track_exit_heading_deg: 34.0

要求同时满足：

• 剩余路径 ≤0.40米。
• 距终点 ≤0.40米。
• 终点朝向误差 ≤34°。
• 累计同向转角达到代码要求，当前下限为90－34＝56°。

满足后当帧进入return_start。这里同样不能仅凭转到56°就退出，还要满足终点位置条件；不检查前方空白区。

⑧ 返回起点 → 完成停车：看位置、方向和整圈行程。

返回起点段继续使用正常锥桶搜索。必须先走到这个阶段，才会执行完成判断。

参数	当前值	完成条件
lap_departure_distance	3.0 米	曾经离开起点至少这么远
lap_min_distance	15.0 米	累计行程至少这么长
lap_position_tolerance	0.45 米	回到起点附近这个范围内
lap_heading_tolerance_deg	20.0°	与起步方向误差不超过20°
lap_confirm_frames	2	连续2帧满足，锁定完成
route.return_start.max_distance	12.0 米	返回段超过12米还没满足终点条件，停车

第一帧满足终点条件就先停车，第二帧确认完成。完成状态锁定，重新发车需要重启节点。

还有几个容易混用的参数，需要单独分清：

参数或情况	实际含义
route.*.speed_pwm	当前段的速度指令，不决定何时切段
route.*.max_distance	不是固定路径长度，也不是“走到这里自动切段”
固定段激活后的max_distance	当前代码不会把它作为该固定路径的行程退出条件
当前主要生效的等待行程上限	起点搜索20米、连接搜索6米、返回起点12米
fixed_track_max_tracking_error: 0.60	跟踪固定引导时偏差过大停车，不表示切到下一段
safety_stop	目标点位置或原始雷达安全检查触发，不表示切段失败
scan_timeout/odom_timeout	数据超时停车，与赛段完成条件分开
交回搜索后没有路径	当前固定赛道模式直接停车，lost_coast_scans不提供滑行缓冲

现场看日志时，[判断] 寻找下一段对应入口识别，[判断] 退出检查对应普通固定段末端，[判断] 直角结束对应累计航向，[判断] 回起点对应计圈。[切段]才表示已经切换；[停车]要按它打印的状态码单独找原因。