竞赛机器人技术文档
工作空间: /home/abot/U3SKU4
机器人: 全向底盘 ABOT，RPLidar 激光雷达
场地: 9×9 网格 40cm，3.6m×3.6m
任务: 语音唤醒 → 4检测点VLM识别 → 任务点(31-51) → 终点
一、系统架构
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
| 麦克风(PyAudio)
│
▼
start.py (Snowboy唤醒词检测)
│ pub "start" → /start_mission
▼
main.py (主控节点)
│
├─→ 检测点 [10,11,12,13]: move_base goto → VLM识别
├─→ 任务点 [ID_MAP映射]: subprocess auto_single_point_test
└─→ 终点 [16]: move_base goto → end24
│
▼
move_base (ROS导航栈)
├─ global_planner (Dijkstra 5Hz)
├─ local_planner (TEB 10Hz)
├─ global_costmap (静态地图 + 激光)
└─ local_costmap (滑动窗口 2m×2m + 激光)
│
▼
AMCL (自适应蒙特卡洛定位)
├─ 120激光束匹配
└─ 500-2000粒子滤波
|
二、启动流程
U3SKU4.sh — 10个gnome-terminal标签页
| Tab |
延迟 |
命令 |
作用 |
| 1 |
0s |
roscore |
ROS 主节点 |
| 2 |
3s |
robot_with_imu.launch |
底盘驱动 + IMU + 激光雷达 |
| 3 |
4s |
navigation.launch |
地图服务 + AMCL + move_base |
| 4 |
4s |
usb_cam_with_calibration |
USB摄像头 |
| 5 |
4s |
ar_track_camera |
AR标签跟踪 |
| 6 |
4s |
vlm_node.launch |
视觉大模型(VLM) |
| 7 |
4s |
multi_goal.launch |
主控节点 main.py |
| 8 |
4s |
view_nav.launch |
RViz 可视化 |
| 9 |
4s |
TTS.py |
语音合成 |
| 10 |
4s |
GameStart.launch |
语音唤醒 start.py |
比赛模式 vs 调试模式
1
2
3
|
<param name="skip_voice_activation" value="false"/>
<param name="skip_voice_delay" value="4.0"/>
|
三、任务流程
3.1 完整流程
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
| 语音唤醒 "Hey Start"
│
▼
start_mission_callback:
│
├─ 检测点 [10,11,12,13] 逐个:
│ ┌─ move_base goto(goals[point])
│ ├─ VLM识别: set /detect=1 → call service → 8s超时
│ ├─ ID_MAP[task_id] → goal_idx
│ └─ task_list.append((task_id, goal_idx))
│
├─ 任务点:
│ for task_id, goal_idx in task_list:
│ subprocess auto_single_point_test
│ → 精准停靠 + 激光安全 + 多入口重试
│
├─ 终点:
│ _goto_if_valid(16, "终点")
│ end24() 后退+右移
│
└─ 汇总报告
|
3.2 ID_MAP 映射
| VLM返回值 |
goals索引 |
说明 |
| 31,32,33 |
1,2,3 |
第一组任务点 |
| 40,41,42 |
4,5,6 |
第二组任务点 |
| 49,50,51 |
7,8,9 |
第三组任务点 |
3.3 检测点与终点坐标
| 索引 |
X |
Y |
Yaw° |
用途 |
| 10 |
0.15 |
1.65 |
185 |
检测点1 |
| 11 |
1.52 |
3.05 |
90 |
检测点2 |
| 12 |
2.85 |
1.60 |
0 |
检测点3 |
| 13 |
1.60 |
0.20 |
-90 |
检测点4 |
| 16 |
0.20 |
3.13 |
180 |
终点 |
3.4 auto_single_point_test 停车流程
1
2
3
4
5
6
7
8
9
10
11
12
| auto_single_point_test(x, y, yaw):
① try_direct_center move_base直达(2s超时)
② generate_entries 生成4方向入口(距目标28cm)
③ sort_entries_by_obstacle 激光评分排序(最多3入口)
④ for entry in entries:
goto_entry move_base到入口(5s超时)
align_to_yaw 旋转对准(3s超时)
enter_to_target_box P控制分段降速驶入(6s超时)
├─ apply_laser_safety 前方<10cm急停
├─ is_obstacle_parking_good 挡板挡住够近→接受
└─ backoff_from_obstacle 脱困重试
⑤ exit_from_entry 退出18cm(给下个任务腾空间)
|
四、参数配置
4.1 TEB 局部规划器
| 参数 |
值 |
说明 |
max_vel_x |
0.35 |
前进极速 m/s |
max_vel_x_backwards |
0.15 |
后退极速 |
max_vel_y |
0.20 |
横移极速(全向) |
max_vel_theta |
0.60 |
旋转极速 rad/s |
acc_lim_x |
0.50 |
纵向加速度 |
acc_lim_y |
0.40 |
横向加速度 |
acc_lim_theta |
0.50 |
角加速度 |
min_obstacle_dist |
0.08 |
硬障碍距离(不可逾越) |
inflation_dist |
0.10 |
软障碍距离(减速区) |
weight_obstacle |
50 |
障碍物权重 |
weight_optimaltime |
2.5 |
速度权重 |
weight_viapoint |
0.4 |
路径跟随强度(0=自由绕障) |
oscillation_v_eps |
0.06 |
线速度震荡阈值 |
oscillation_omega_eps |
0.12 |
角速度震荡阈值 |
dt_ref |
0.30 |
轨迹时间步长 |
penalty_epsilon |
0.03 |
优化收敛阈值 |
feasibility_check_no_poses |
3 |
可行性检查帧数 |
max_global_plan_lookahead_dist |
1.5 |
前视距离 m |
xy_goal_tolerance |
0.10 |
位置到达容差 m |
yaw_goal_tolerance |
0.25 |
角度到达容差 rad |
4.2 代价地图
| 参数 |
值 |
说明 |
inflation_radius |
0.08 |
障碍物膨胀半径 m |
cost_scaling_factor |
5.0 |
代价指数衰减速度 |
obstacle_range |
4.0 |
障碍物标记范围 m |
raytrace_range |
4.5 |
障碍物清除范围 m |
footprint |
[[0.205,0.18],…] |
机器人外轮廓 40×36cm |
4.3 move_base
| 参数 |
值 |
说明 |
planner_frequency |
5.0 |
全局规划频率 Hz |
controller_frequency |
10.0 |
局部控制频率 Hz |
oscillation_distance |
0.15 |
震荡判定范围 m |
oscillation_timeout |
8.0 |
震荡超时 s |
controller_patience |
10.0 |
控制器忍耐 s |
| recovery |
clear_costmap only |
不旋转恢复(防丢定位) |
4.4 AMCL 定位
| 参数 |
值 |
说明 |
laser_max_beams |
120 |
激光匹配点数 |
update_min_d |
0.05 |
平移触发更新 m |
update_min_a |
0.10 |
旋转触发更新 rad |
odom_model_type |
omni |
全向里程计模型 |
odom_alpha3 |
0.25 |
横移噪声系数 |
min_particles |
500 |
最少粒子数 |
max_particles |
2000 |
最多粒子数 |
laser_model_type |
likelihood_field |
似然场匹配 |
4.5 auto_single_point_test 停车
| 参数 |
值 |
说明 |
entry_offset |
0.28 |
入口距目标 m |
front_stop_dist |
0.10 |
前方急停 m |
side_stop_dist |
0.10 |
侧方急停 m |
any_stop_dist |
0.06 |
全向急停 m |
enter_timeout |
6.0 |
入框超时 s |
nav_timeout |
5.0 |
导航超时 s |
align_timeout |
3.0 |
对齐超时 s |
direct_entry_timeout |
4.0 |
直驱超时 s |
max_enter_v |
0.15 |
入框最大速度 |
exit_distance |
0.18 |
退出距离 m |
exit_speed |
0.12 |
退出速度 m/s |
obstacle_park_accept_dist |
0.12 |
障碍泊车接受距离 m |
五、安全机制
5.1 正常导航阶段
1
2
3
4
5
| TEB 硬约束: min_obstacle_dist = 0.08m (轨迹不可逾越)
TEB 软约束: inflation_dist = 0.10m (减速区)
costmap: inflation_radius = 0.08m (障碍物膨胀)
清扫恢复: clear_costmap (不旋转)
震荡恢复: v_eps=0.06, omega_eps=0.12
|
5.2 auto_single_point_test 停车阶段
1
2
3
4
5
| 关闭三层障碍: obstacle_layer + inflation_layer + static_layer
实时激光: 20Hz扫描 → 前方<10cm急停, 任意<6cm全停
多入口: 4方向评分 → 最多3个重试
障碍泊车: L型挡板 → 够近接受
脱困: 后退0.7s → 重试
|
5.3 紧急制动参数
| 层 |
触发距离 |
动作 |
| auto 全向 |
<6cm |
全停 |
| auto 前方 |
<10cm |
停前进 |
| auto 侧方 |
<10cm |
停横移 |
| TEB 硬约束 |
<8cm |
轨迹拒绝 |
| oscillation |
>6cm/s波动 |
5s恢复 |
六、里程计
6.1 硬件架构
1
2
3
4
5
6
7
8
9
10
| 轮式编码器 (4×麦克纳姆轮)
│ 每轮脉冲计数 → 合成 vx, vy, vz, vth
▼
/wheel_odom (abot_driver 发布)
│
├─→ robot_pose_ekf (EKF融合 200Hz)
│ 输入: /wheel_odom (vx) + /imu/data_raw (陀螺仪z轴)
│ 输出: /odom (fused odometry TF)
│
└─→ TEB (直接读 /wheel_odom 做速度约束)
|
6.2 EKF 融合策略
1
2
3
4
5
6
7
| 麦轮横向速度不可靠 → 只融合 vx（前进速度）
陀螺仪偏航角速度可靠 → 融合 z 轴角速度，替代编码器差分
过程噪声:
vy: 0.1 (最大 — 无侧向速度观测)
yaw: 0.01 (最小 — 信任陀螺仪)
vx: 0.05 (中等)
|
6.3 AMCL 校正循环
1
2
3
4
| /odom → 粒子预测 → 120条激光束匹配地图(似然场) → 粒子重采样 → /tf(map→odom)
↓
每 5cm 平移或 5.7° 旋转触发一次更新
min_particles=500, max=2000
|
6.4 横移偏差处理
1
2
| odom_alpha3=0.25: 横移10cm → 粒子扩散±2.5cm → 激光校正后修正
比原始0.15更容忍横移打滑，全向底盘必要
|
七、避障逻辑
7.1 三层避障架构
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
| Layer 1: global_costmap
├─ static_layer: 预建 .pgm 地图
├─ obstacle_layer: /scan_filtered 激光数据
└─ inflation_layer: 障碍物周围 8cm 膨胀
→ 180×180格, 2cm分辨率, 全局规划用
Layer 2: local_costmap
├─ obstacle_layer: /scan_filtered 实时激光
└─ inflation_layer: 障碍物周围 8cm 膨胀
→ 100×100格, 2m×2m滑动窗口, TEB用
Layer 3: TEB 优化器
├─ min_obstacle_dist=0.08m (硬约束, 轨迹不可逾越)
├─ inflation_dist=0.10m (软约束, 产生减速代价)
└─ weight_obstacle=50 (最高权重)
|
7.2 TEB 避障决策 (每100ms一帧)
1
2
3
4
5
6
7
8
| ① 沿全局路径前1.5m采样候选轨迹序列
② 从costmap采样25个障碍物位姿
③ 计算每轨迹点离障碍物的距离:
<0.08m → 轨迹直接拒绝
0.08~0.10m → 产生高代价
>0.10m → 无代价
④ 代价函数: J = 50×障碍 + 2.5×时间 + 0.4×跟随 + 平滑项
⑤ 选代价最低轨迹 → 发布第一帧速度指令
|
7.3 绕挡板时间线
1
2
3
4
5
6
| 0.0s: 激光未检测到挡板 → 全局路径直线穿板
0.1s: 激光看到挡板(4m范围) → local/global costmap 同时标记
0.2s: planner_frequency=5Hz → 全局规划重算绕行路线
0.3s: TEB 收到新路径 → weight_viapoint=0.4 自由偏离旧路线
→ 沿绕行路线前进
总延迟: ~0.3s → 机器人位移 <10cm
|
7.4 窄缝处理
1
2
| 正常行驶: inflation=8cm → 需52cm有效宽 → 40cm窄缝被标记为不可过 → 绕行
停车阶段: 关三层障碍层 → 无视窄缝 → 激光安全兜底(前方<10cm急停)
|
7.5 避障参数权衡
| 参数 |
大→小 |
效果 |
weight_obstacle |
80→50 |
靠障碍更近, 硬约束保护 |
weight_viapoint |
1.0→0.4 |
自由绕板, 不穿板 |
inflation_radius |
0.10→0.08 |
窄缝标记更多, 绕宽路 |
min_obstacle_dist |
0.10→0.08 |
更近才硬拒绝 |
八、路径规划
8.1 全局规划 (Dijkstra, 5Hz)
1
2
3
4
5
6
7
8
9
10
| 输入: global_costmap (180×180, 2cm)
算法: Dijkstra 最短路径
allow_unknown: false → 不穿未知区域
lethal_cost: 253 → 代价>253 不可通
cost_factor: 3.0 → 代价倍数
路线特征: 沿最低代价走 → 自然避开障碍物和膨胀区
窄缝被膨胀堵 → 自动选宽路
每次0.2s重规划 → 临时挡板快速响应
|
8.2 局部规划 (TEB, 10Hz)
1
2
3
4
5
6
7
8
| 输入: 全局路径 + local_costmap (100×100, 2m×2m)
算法: Timed Elastic Band (时间弹性带)
① 沿全局路径采样5-10个候选轨迹点
② 每个点: 位置(x,y) + 时间间隔(dt)
③ 非线性优化: 最小化通过时间 + 最大化障碍距离
④ 约束: vx<0.35, vy<0.20, vth<0.60, ax<0.50
⑤ 输出: 第一帧 /cmd_vel
|
8.3 路径跟随自由度
1
2
| weight_viapoint=1.0: 紧贴全局路径 → 穿板→震荡→ABORT
weight_viapoint=0.4: 宽松跟随 → 绕板自由 → 开阔区±4cm微摆
|
8.4 规划失效兜底
1
2
3
| 全局无解 (5s) → goto返回False → 检测点重试1次 → 跳过
局部无解 → shrink_horizon → oscillation_recovery(5s)
→ controller_patience(10s) → clear_costmap → ABORT
|
九、主程序关键机制
9.1 服务超时
| 服务 |
超时 |
失败处理 |
| VLM连接 |
15s |
跳过识别，返回”无” |
| TTS连接 |
15s |
静默运行 |
| VLM调用 |
8s |
跳过本检测点 |
| 任务点停车 |
20s |
kill进程，打日志继续 |
| 导航move_base |
45s |
返回False，重试或跳过 |
9.2 防重复启动
1
2
3
4
5
| self._running = True
try:
finally:
self._running = False
|
9.3 检测点重试
move_base 失败 → 等1s → 重试1次 → 仍失败 → 跳过
十、文件清单
核心程序
| 文件 |
行数 |
作用 |
scripts/main.py |
~340 |
主控节点 |
scripts/auto_single_point_test.py |
1849 |
精准停靠节点 |
scripts/start.py |
73 |
语音唤醒节点 |
scripts/decoder.py |
283 |
Snowboy热词检测器 |
scripts/detect.py |
194 |
Snowboy SWIG封装 |
参数文件
| 文件 |
作用 |
params/carto/teb_local_planner_params.yaml |
TEB局部规划 |
params/carto/costmap_common_params.yaml |
代价地图共通 |
params/carto/move_base_params.yaml |
move_base控制 |
params/carto/global_costmap_params.yaml |
全局代价地图 |
params/carto/local_costmap_params.yaml |
局部代价地图 |
params/carto/global_planner_params.yaml |
全局规划器 |
params/carto/dwa_local_planner_params.yaml |
DWA备用规划器 |
启动文件
| 文件 |
作用 |
launch/multi_goal.launch |
主控节点启动配置 |
launch/navigation.launch |
导航栈启动 |
launch/include/move_base.launch.xml |
move_base配置 |
launch/include/amcl.launch.xml |
AMCL定位配置 |
launch/include/gmapping.launch.xml |
建图配置 |
U3SKU4.sh |
一键启动脚本 |
mapping.sh |
建图脚本 |
save_map.sh |
保存地图脚本 |
十一、调试工具
11.1 快速诊断
1
2
3
4
| rosnode list
rosparam get /move_base/TebLocalPlannerROS
rostopic echo /cmd_vel -n5
rostopic echo /amcl_pose -n1
|
11.2 常见错误
| 错误 |
原因 |
解决 |
未找到目标参数 goalListYaw |
launch参数缺失 |
检查multi_goal.launch |
oscillating...ABORT |
障碍挡住目标 |
调整坐标或绕行参数 |
VLM检测超时10s |
API响应慢 |
检查摄像头+API |
任务点 超时20s |
auto节点卡住 |
检查目标坐标可达性 |
索引越界 |
goals数组不够长 |
检查launch坐标数量 |
11.3 现场调参指南
详见 DEBUG_GUIDE.md，包含12种症状的详细诊断和调整步骤。
11.4 建图指南
详见 GRID_POINTS.md，包含9×9网格坐标、建图走法、质量要点。
十二、版本历史
| 阶段 |
主要修改 |
| 基础修复 |
TTS服务类型、参数命名、服务降级、边界检查 |
| 避障优化 |
速度提升、权重平衡、震荡放宽、绕行自由 |
| 定位强化 |
AMCL光束增加、更新频率提升、噪声模型调整 |
| 停车系统 |
auto_single_point_test 集成、超时收紧、激光安全收紧 |
| 主控逻辑 |
重入锁、VLM超时、检测点重试、汇总报告 |
| 建图 |
gmapping参数优化 |
| 文档 |
DEBUG_GUIDE + GRID_POINTS + TECHNICAL_DOC |
