From f073bcb3f9e3c1d683e7e7e84c12dbf20a314fad Mon Sep 17 00:00:00 2001
From: windows <2877519694@qq.com>
Date: Mon, 22 Dec 2025 22:26:25 +0800
Subject: [PATCH 1/2] =?UTF-8?q?=E4=BC=98=E5=8C=96=E5=B0=8F=E8=BD=A6?=
=?UTF-8?q?=E8=BD=AC=E5=90=91=E5=8A=9F=E8=83=BD=EF=BC=8C=E9=99=8D=E4=BD=8E?=
=?UTF-8?q?=E5=B0=8F=E8=BD=A6=E8=BD=AC=E5=90=91=E9=80=9F=E5=BA=A6?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/simple_wheeled_vehicle/main.py | 391 +++++++++++++++------
src/simple_wheeled_vehicle/wheeled_car.xml | 2 +-
2 files changed, 279 insertions(+), 114 deletions(-)
diff --git a/src/simple_wheeled_vehicle/main.py b/src/simple_wheeled_vehicle/main.py
index 457a4b4b57..ea37034124 100644
--- a/src/simple_wheeled_vehicle/main.py
+++ b/src/simple_wheeled_vehicle/main.py
@@ -1,8 +1,8 @@
"""
-自动巡航小车 - 带前方障碍物检测与90度转向避障
-- 提高速度至0.0015 m/s(提高两倍)
-- 前方0.5米内有障碍物时停止并进行90度转向
-- 转向后恢复巡航速度
+自动巡航小车 - 带前方障碍物检测与智能路径规划
+- 巡航速度提高至0.003 m/s
+- 前方0.5米内有障碍物时停止并规划新路径
+- 避免摆头循环,寻找可行方向
- R 键可复位
"""
import mujoco
@@ -29,13 +29,18 @@ def on_release(k):
data = mujoco.MjData(model)
# ------------------- 参数设置 -------------------
-# 提高两倍的速度:0.0005 * 2 = 0.0015
-CRUISE_SPEED = 0.0015 # 提高两倍的巡航速度
-TURN_SPEED = CRUISE_SPEED * 0.5 # 转向时的速度(更低以确保安全)
-OBSTACLE_DISTANCE_THRESHOLD = 0.5 # 障碍物检测距离
-SAFE_DISTANCE = 0.2 # 安全距离
-TURN_ANGLE = math.pi / 2 # 90度转向角度(π/2弧度)
-TURN_DURATION = 150 # 90度转向需要更长时间
+CRUISE_SPEED = 0.003
+TURN_SPEED = CRUISE_SPEED * 0.4
+OBSTACLE_DISTANCE_THRESHOLD = 0.7 # 增加检测距离
+SAFE_DISTANCE = 0.3
+TURN_ANGLE = 0.3
+TURN_DURATION = 60
+SCAN_RANGE = 1.0 # 扩大扫描范围
+
+# 路径规划参数
+MAX_TURN_ATTEMPTS = 3 # 最大转向尝试次数
+PATH_SCAN_ANGLES = [-60, -30, 0, 30, 60] # 扫描角度(度)
+PATH_SCAN_DISTANCE = 1.5 # 路径扫描距离
# 障碍物名称列表
OBSTACLE_NAMES = [
@@ -57,67 +62,158 @@ def on_release(k):
# ------------------- 复位函数 -------------------
def reset_car():
mujoco.mj_resetData(model, data)
- data.qpos[2] = 0.03 # 设置初始高度
+ data.qpos[2] = 0.03
print("\n>>> 已复位 <<<")
# ------------------- 障碍物检测函数 -------------------
-def check_front_obstacle():
- """检测小车前方是否有障碍物"""
+def check_front_obstacle(direction_angle=0):
+ """检测指定方向是否有障碍物"""
# 获取小车当前位置
- chassis_pos = data.body(chassis_body_id).xpos # 位置 (x, y, z)
+ chassis_pos = data.body(chassis_body_id).xpos
+
+ # 获取小车当前速度方向
+ velocity = data.qvel[:2]
+ if np.linalg.norm(velocity) < 0.0001:
+ forward_direction = np.array([1.0, 0.0])
+ else:
+ forward_direction = velocity / np.linalg.norm(velocity)
+
+ # 旋转前向方向到指定角度
+ if direction_angle != 0:
+ cos_a = math.cos(direction_angle)
+ sin_a = math.sin(direction_angle)
+ rotated_direction = np.array([
+ forward_direction[0] * cos_a - forward_direction[1] * sin_a,
+ forward_direction[0] * sin_a + forward_direction[1] * cos_a
+ ])
+ forward_direction = rotated_direction
# 扫描所有障碍物
min_distance = float('inf')
closest_obstacle = None
for obs_name, obs_id in obstacle_ids.items():
- # 获取障碍物位置
obs_pos = data.body(obs_id).xpos
- # 计算相对于小车的位置
- dx = obs_pos[0] - chassis_pos[0] # x方向距离
- dy = obs_pos[1] - chassis_pos[1] # y方向距离
+ dx = obs_pos[0] - chassis_pos[0]
+ dy = obs_pos[1] - chassis_pos[1]
+
+ relative_pos = np.array([dx, dy])
+ distance = np.linalg.norm(relative_pos)
- # 计算水平距离
- distance = math.sqrt(dx**2 + dy**2)
+ if distance > 0 and distance < SCAN_RANGE:
+ obstacle_direction = relative_pos / distance
- # 只考虑小车前方的障碍物(dx > 0 表示在小车前方)
- if dx > 0 and distance < OBSTACLE_DISTANCE_THRESHOLD:
- # 计算障碍物相对于小车前向方向的横向偏移
- lateral_distance = abs(dy)
+ dot_product = np.dot(obstacle_direction, forward_direction)
+ dot_product = max(-1.0, min(1.0, dot_product))
+ angle_diff = math.acos(dot_product)
- # 如果横向距离小于小车的宽度(约0.3米),则认为在正前方
- if lateral_distance < 0.3:
- if distance < min_distance:
- min_distance = distance
- closest_obstacle = obs_name
+ if angle_diff < math.radians(45) and dx > -0.2:
+ cross_product = np.cross([forward_direction[0], forward_direction[1], 0],
+ [obstacle_direction[0], obstacle_direction[1], 0])
+ lateral_distance = abs(cross_product[2]) * distance
+
+ if angle_diff < math.radians(25) and lateral_distance < 0.5:
+ if distance < min_distance:
+ min_distance = distance
+ closest_obstacle = obs_name
- # 返回检测结果
if closest_obstacle is not None:
if min_distance < SAFE_DISTANCE:
- return 2, min_distance, closest_obstacle # 紧急停止
+ return 2, min_distance, closest_obstacle
else:
- return 1, min_distance, closest_obstacle # 检测到障碍物
+ return 1, min_distance, closest_obstacle
+
+ return 0, 0, None
+
+# ------------------- 路径扫描函数 -------------------
+def scan_path_directions():
+ """扫描多个方向的路径可行性"""
+
+ path_scores = []
+
+ for angle_deg in PATH_SCAN_ANGLES:
+ angle_rad = math.radians(angle_deg)
+
+ # 检查该方向是否有障碍物
+ obstacle_status, obstacle_distance, _ = check_front_obstacle(angle_rad)
+
+ # 计算分数:距离越远分数越高
+ if obstacle_status == 0:
+ # 无障碍物,给最高分
+ score = PATH_SCAN_DISTANCE
+ elif obstacle_status == 1:
+ # 有障碍物但距离较远
+ score = obstacle_distance
+ else:
+ # 有近距离障碍物
+ score = 0
+
+ path_scores.append((angle_rad, score, obstacle_status))
+
+ # 按分数排序
+ path_scores.sort(key=lambda x: x[1], reverse=True)
+
+ return path_scores
+
+# ------------------- 选择最佳路径 -------------------
+def choose_best_path():
+ """选择最佳路径方向"""
+
+ # 扫描所有方向
+ path_scores = scan_path_directions()
+
+ # 选择最佳方向
+ best_angle, best_score, best_status = path_scores[0]
- return 0, 0, None # 无障碍物
+ # 如果所有方向都有障碍物,选择障碍物最远的方向
+ if best_score == 0:
+ # 后退方案
+ print("所有方向都有障碍物,尝试后退")
+ return None, "BACKUP"
-# ------------------- 随机选择90度转向方向 -------------------
-def choose_90_degree_turn():
- """随机选择向左或向右90度转向"""
- if random.random() > 0.5:
- return TURN_ANGLE # 右转90度
+ # 如果最佳方向不是正前方,需要转向
+ if abs(best_angle) > math.radians(10):
+ if best_angle > 0:
+ return best_angle, f"左转{abs(math.degrees(best_angle)):.0f}度"
+ else:
+ return best_angle, f"右转{abs(math.degrees(best_angle)):.0f}度"
+ else:
+ return 0, "直行"
+
+# ------------------- 后退操作 -------------------
+def perform_backup(backup_counter):
+ """执行后退操作"""
+
+ if backup_counter < 30:
+ # 后退(负速度)
+ data.ctrl[0] = 0.0
+ data.ctrl[1] = 0.0
+ data.ctrl[2] = -TURN_SPEED * 0.5
+ data.ctrl[3] = -TURN_SPEED * 0.5
+ data.ctrl[4] = -TURN_SPEED * 0.5
+ data.ctrl[5] = -TURN_SPEED * 0.5
+ return False, backup_counter + 1
else:
- return -TURN_ANGLE # 左转90度
+ # 后退完成
+ for i in range(len(data.ctrl)):
+ data.ctrl[i] = 0.0
+ return True, 0
# ------------------- 主循环 -------------------
mujoco.mj_resetData(model, data)
# 状态变量
-CAR_STATE = "CRUISING" # 状态: CRUISING, STOPPED, TURNING, RESUME
+CAR_STATE = "CRUISING" # CRUISING, DECELERATING, STOPPED, PATH_PLANNING, TURNING, SCANNING, RESUME, BACKING_UP
turn_counter = 0
turn_angle = 0
-turn_direction = "" # 记录转向方向
+turn_direction = ""
+scan_counter = 0
+deceleration_counter = 0
+current_speed = CRUISE_SPEED
+turn_attempts = 0 # 转向尝试次数
+backup_counter = 0
with mujoco.viewer.launch_passive(model, data) as viewer:
viewer.cam.distance = 2.5
@@ -129,129 +225,200 @@ def choose_90_degree_turn():
reset_car()
CAR_STATE = "CRUISING"
turn_counter = 0
+ scan_counter = 0
+ deceleration_counter = 0
+ current_speed = CRUISE_SPEED
+ turn_attempts = 0
+ backup_counter = 0
KEYS[keyboard.KeyCode.from_char('r')] = False
# 根据当前状态执行不同操作
if CAR_STATE == "CRUISING":
- # 巡航状态:检测前方障碍物
+ # 巡航状态
obstacle_status, obstacle_distance, obstacle_name = check_front_obstacle()
if obstacle_status == 2:
- # 紧急停止(距离过近)
+ # 紧急停止
CAR_STATE = "STOPPED"
- print(f"\n紧急停止!障碍物距离: {obstacle_distance:.2f}m")
+ print(f"\n⚠️ 紧急停止!障碍物距离: {obstacle_distance:.2f}m")
turn_counter = 0
+ current_speed = 0
- # 停止所有驱动
for i in range(len(data.ctrl)):
data.ctrl[i] = 0.0
elif obstacle_status == 1:
- # 检测到前方障碍物,减速准备停止
+ # 检测到障碍物,减速
+ CAR_STATE = "DECELERATING"
+ deceleration_counter = 0
+ print(f"\n⚠️ 检测到障碍物: {obstacle_name}({obstacle_distance:.2f}m),开始减速...")
+ else:
+ # 无障碍物,高速巡航
+ data.ctrl[0] = 0.0
+ data.ctrl[1] = 0.0
+ data.ctrl[2] = CRUISE_SPEED
+ data.ctrl[3] = CRUISE_SPEED
+ data.ctrl[4] = CRUISE_SPEED
+ data.ctrl[5] = CRUISE_SPEED
+ current_speed = CRUISE_SPEED
+
+ elif CAR_STATE == "DECELERATING":
+ # 减速状态
+ deceleration_counter += 1
+
+ decel_progress = min(1.0, deceleration_counter / 15.0)
+ current_speed = CRUISE_SPEED * (1.0 - decel_progress)
+
+ data.ctrl[0] = 0.0
+ data.ctrl[1] = 0.0
+ data.ctrl[2] = current_speed
+ data.ctrl[3] = current_speed
+ data.ctrl[4] = current_speed
+ data.ctrl[5] = current_speed
+
+ if deceleration_counter > 20:
CAR_STATE = "STOPPED"
- print(f"\n检测到障碍物: {obstacle_name}({obstacle_distance:.2f}m),正在停止...")
+ print("减速完成,准备规划路径")
turn_counter = 0
- # 减速停止
- for i in range(len(data.ctrl)):
- data.ctrl[i] = 0.0
- else:
- # 无障碍物,正常巡航(两倍速度)
- data.ctrl[0] = 0.0 # 前左转向
- data.ctrl[1] = 0.0 # 前右转向
- data.ctrl[2] = CRUISE_SPEED # 前左驱动
- data.ctrl[3] = CRUISE_SPEED # 前右驱动
- data.ctrl[4] = CRUISE_SPEED # 后左驱动
- data.ctrl[5] = CRUISE_SPEED # 后右驱动
-
elif CAR_STATE == "STOPPED":
- # 已停止状态:选择转向方向
+ # 停止状态,准备路径规划
turn_counter += 1
- # 停止所有驱动
+ current_speed = 0
for i in range(len(data.ctrl)):
data.ctrl[i] = 0.0
- # 等待几帧确保完全停止
if turn_counter > 10:
- print("选择转向方向...")
+ print("正在规划路径...")
+ CAR_STATE = "PATH_PLANNING"
+ turn_counter = 0
- # 随机选择向左或向右90度转向
- turn_angle = choose_90_degree_turn()
+ elif CAR_STATE == "PATH_PLANNING":
+ # 路径规划状态
+ print("扫描可行路径...")
- # 记录转向方向
- if turn_angle > 0:
- turn_direction = "右转90度"
- else:
- turn_direction = "左转90度"
+ # 选择最佳路径
+ best_angle, path_direction = choose_best_path()
+
+ if path_direction == "BACKUP":
+ # 需要后退
+ print("所有方向都有障碍物,执行后退")
+ CAR_STATE = "BACKING_UP"
+ backup_counter = 0
+ else:
+ # 找到可行路径
+ turn_angle = best_angle
+ turn_direction = path_direction
- print(f"将执行{turn_direction}")
+ # 检查是否需要大角度转向
+ if abs(turn_angle) > math.radians(45):
+ turn_attempts += 1
+ if turn_attempts >= MAX_TURN_ATTEMPTS:
+ print(f"多次尝试后选择大角度转向: {turn_direction}")
- # 进入转向状态
+ print(f"选择路径: {turn_direction}")
CAR_STATE = "TURNING"
turn_counter = 0
+ elif CAR_STATE == "BACKING_UP":
+ # 后退状态
+ backup_complete, backup_counter = perform_backup(backup_counter)
+
+ if backup_complete:
+ print("后退完成,重新规划路径")
+ CAR_STATE = "PATH_PLANNING"
+
elif CAR_STATE == "TURNING":
- # 转向状态:执行90度转向
+ # 转向状态
turn_counter += 1
- # 设置90度转向角度
- data.ctrl[0] = turn_angle # 前左转向
- data.ctrl[1] = turn_angle # 前右转向
+ # 渐进式转向
+ progress = min(1.0, turn_counter / 10.0)
+ current_angle = turn_angle * progress
- # 低速前进,帮助转向
- data.ctrl[2] = TURN_SPEED # 前左驱动
- data.ctrl[3] = TURN_SPEED # 前右驱动
- data.ctrl[4] = TURN_SPEED # 后左驱动
- data.ctrl[5] = TURN_SPEED # 后右驱动
+ data.ctrl[0] = current_angle
+ data.ctrl[1] = current_angle
- # 显示转向信息
- if turn_counter % 30 == 0:
- print(f"正在执行{turn_direction},进度: {turn_counter}/{TURN_DURATION}")
+ if turn_counter > 5:
+ speed_progress = min(1.0, (turn_counter - 5) / 15.0)
+ current_speed = TURN_SPEED * speed_progress
+ data.ctrl[2] = current_speed
+ data.ctrl[3] = current_speed
+ data.ctrl[4] = current_speed
+ data.ctrl[5] = current_speed
+
+ if turn_counter % 15 == 0:
+ print(f"正在{turn_direction},角度: {math.degrees(current_angle):.1f}度")
- # 检查转向是否完成
if turn_counter > TURN_DURATION:
- print(f"{turn_direction}完成,准备恢复巡航")
+ print(f"{turn_direction}完成,开始扫描前方...")
+ CAR_STATE = "SCANNING"
+ turn_counter = 0
+ scan_counter = 0
+
+ elif CAR_STATE == "SCANNING":
+ # 扫描状态
+ scan_counter += 1
+
+ data.ctrl[0] = turn_angle * 0.5
+ data.ctrl[1] = turn_angle * 0.5
+ data.ctrl[2] = TURN_SPEED * 0.6
+ data.ctrl[3] = TURN_SPEED * 0.6
+ data.ctrl[4] = TURN_SPEED * 0.6
+ data.ctrl[5] = TURN_SPEED * 0.6
+ current_speed = TURN_SPEED * 0.6
+
+ if scan_counter % 10 == 0:
+ obstacle_status, obstacle_distance, obstacle_name = check_front_obstacle()
+
+ if obstacle_status == 0:
+ print("前方安全,准备恢复巡航")
+ CAR_STATE = "RESUME"
+ turn_counter = 0
+ turn_attempts = 0 # 重置尝试次数
+ else:
+ print(f"转向后仍检测到障碍物: {obstacle_name}({obstacle_distance:.2f}m),重新规划")
+ CAR_STATE = "STOPPED"
+ turn_counter = 0
+
+ if scan_counter > 50:
+ print("扫描超时,尝试恢复巡航")
CAR_STATE = "RESUME"
turn_counter = 0
elif CAR_STATE == "RESUME":
- # 恢复状态:转向后恢复直行并恢复巡航速度
+ # 恢复巡航状态
turn_counter += 1
- # 逐渐回正转向
- if turn_counter < 20:
- # 逐渐减小转向角度
- decay_factor = 1.0 - (turn_counter / 20.0)
- current_angle = turn_angle * decay_factor
- data.ctrl[0] = current_angle
- data.ctrl[1] = current_angle
-
- # 逐渐加速到巡航速度
- speed_factor = turn_counter / 20.0
- current_speed = TURN_SPEED + (CRUISE_SPEED - TURN_SPEED) * speed_factor
- data.ctrl[2] = current_speed
- data.ctrl[3] = current_speed
- data.ctrl[4] = current_speed
- data.ctrl[5] = current_speed
- else:
- # 转向已回正,恢复两倍巡航速度
+ progress = min(1.0, turn_counter / 15.0)
+
+ current_angle = turn_angle * (1.0 - progress)
+ data.ctrl[0] = current_angle
+ data.ctrl[1] = current_angle
+
+ current_speed = TURN_SPEED + (CRUISE_SPEED - TURN_SPEED) * progress
+ data.ctrl[2] = current_speed
+ data.ctrl[3] = current_speed
+ data.ctrl[4] = current_speed
+ data.ctrl[5] = current_speed
+
+ if turn_counter > 20:
data.ctrl[0] = 0.0
data.ctrl[1] = 0.0
data.ctrl[2] = CRUISE_SPEED
data.ctrl[3] = CRUISE_SPEED
data.ctrl[4] = CRUISE_SPEED
data.ctrl[5] = CRUISE_SPEED
+ current_speed = CRUISE_SPEED
- # 检查前方是否安全
obstacle_status, obstacle_distance, _ = check_front_obstacle()
if obstacle_status == 0:
- print("前方安全,恢复巡航")
+ print("恢复巡航成功")
CAR_STATE = "CRUISING"
turn_counter = 0
else:
- # 前方仍有障碍物,重新停止
- print("转向后前方仍有障碍物,重新处理")
+ print("恢复巡航时检测到障碍物,重新处理")
CAR_STATE = "STOPPED"
turn_counter = 0
@@ -260,16 +427,14 @@ def choose_90_degree_turn():
# 显示信息
vel = np.linalg.norm(data.qvel[:3])
-
- # 获取当前转向角度
current_steer = (data.ctrl[0] + data.ctrl[1]) / 2
- # 显示状态信息
status_info = f"状态: {CAR_STATE}, 速度: {vel:7.5f} m/s"
if abs(current_steer) > 0.01:
status_info += f", 转向: {math.degrees(current_steer):.1f}度"
- # 如果正在巡航,显示障碍物检测信息
+ status_info += f", 尝试次数: {turn_attempts}"
+
if CAR_STATE == "CRUISING":
obstacle_status, obstacle_distance, obstacle_name = check_front_obstacle()
if obstacle_status > 0 and obstacle_name:
diff --git a/src/simple_wheeled_vehicle/wheeled_car.xml b/src/simple_wheeled_vehicle/wheeled_car.xml
index 7e233c71e1..58ae5399a3 100644
--- a/src/simple_wheeled_vehicle/wheeled_car.xml
+++ b/src/simple_wheeled_vehicle/wheeled_car.xml
@@ -64,7 +64,7 @@
+
From 9ad83780811a627452df643425c65f71d438e21f Mon Sep 17 00:00:00 2001
From: windows <2877519694@qq.com>
Date: Tue, 23 Dec 2025 16:58:47 +0800
Subject: [PATCH 2/2] =?UTF-8?q?=E7=BB=99=E5=B0=8F=E8=BD=A6=E5=A2=9E?=
=?UTF-8?q?=E5=8A=A0=E8=AE=B0=E5=BF=86=E5=AD=A6=E4=B9=A0=E5=8A=9F=E8=83=BD?=
=?UTF-8?q?=EF=BC=8C=E8=83=BD=E8=AF=86=E5=88=AB=E9=87=8D=E5=A4=8D=E8=B7=AF?=
=?UTF-8?q?=E5=BE=84=E9=80=89=E6=8B=A9=E6=9C=80=E4=BC=98=E8=B7=AF=E7=BA=BF?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/simple_wheeled_vehicle/main.py | 530 ++++++++++++++++-----
src/simple_wheeled_vehicle/wheeled_car.xml | 2 +-
2 files changed, 414 insertions(+), 118 deletions(-)
diff --git a/src/simple_wheeled_vehicle/main.py b/src/simple_wheeled_vehicle/main.py
index ea37034124..aaf82bd86d 100644
--- a/src/simple_wheeled_vehicle/main.py
+++ b/src/simple_wheeled_vehicle/main.py
@@ -1,9 +1,10 @@
"""
-自动巡航小车 - 带前方障碍物检测与智能路径规划
-- 巡航速度提高至0.003 m/s
-- 前方0.5米内有障碍物时停止并规划新路径
-- 避免摆头循环,寻找可行方向
-- R 键可复位
+自动巡航小车 - 智能绕障与路径记忆系统
+- 巡航速度:0.003 m/s
+- 前方障碍物检测与智能路径规划
+- 路径记忆与学习功能
+- 绕障决策优化
+- R 键复位
"""
import mujoco
import mujoco.viewer
@@ -12,9 +13,15 @@
import math
import random
import time
+import json
+from collections import deque
# ------------------- 键盘监听 -------------------
-KEYS = {keyboard.KeyCode.from_char('r'): False}
+KEYS = {
+ keyboard.KeyCode.from_char('r'): False,
+ keyboard.KeyCode.from_char('d'): False, # 调试模式开关
+ keyboard.KeyCode.from_char('s'): False, # 保存路径记录
+}
def on_press(k):
if k in KEYS: KEYS[k] = True
@@ -31,16 +38,42 @@ def on_release(k):
# ------------------- 参数设置 -------------------
CRUISE_SPEED = 0.003
TURN_SPEED = CRUISE_SPEED * 0.4
-OBSTACLE_DISTANCE_THRESHOLD = 0.7 # 增加检测距离
+OBSTACLE_DISTANCE_THRESHOLD = 0.7
SAFE_DISTANCE = 0.3
TURN_ANGLE = 0.3
-TURN_DURATION = 60
-SCAN_RANGE = 1.0 # 扩大扫描范围
-
-# 路径规划参数
-MAX_TURN_ATTEMPTS = 3 # 最大转向尝试次数
-PATH_SCAN_ANGLES = [-60, -30, 0, 30, 60] # 扫描角度(度)
-PATH_SCAN_DISTANCE = 1.5 # 路径扫描距离
+TURN_DURATION = 50
+SCAN_RANGE = 1.0
+
+# 新增:智能绕障参数
+PATH_MEMORY_SIZE = 50 # 路径记忆大小
+EXPLORATION_RATE = 0.3 # 探索率(尝试新路径的概率)
+LEARNING_RATE = 0.1 # 学习率
+PATH_REWARD = 1.0 # 成功路径奖励
+PATH_PENALTY = -0.5 # 失败路径惩罚
+
+# 新增:路径评分参数
+DIRECTION_SCORES = {
+ "forward": 1.0, # 直行优先
+ "slight_left": 0.9,
+ "slight_right": 0.9,
+ "left": 0.8,
+ "right": 0.8,
+ "sharp_left": 0.6,
+ "sharp_right": 0.6,
+ "backward": 0.3, # 后退最后考虑
+}
+
+# 新增:方向定义
+DIRECTIONS = {
+ "forward": 0,
+ "slight_left": math.radians(15),
+ "slight_right": math.radians(-15),
+ "left": math.radians(30),
+ "right": math.radians(-30),
+ "sharp_left": math.radians(60),
+ "sharp_right": math.radians(-60),
+ "backward": math.radians(180),
+}
# 障碍物名称列表
OBSTACLE_NAMES = [
@@ -59,18 +92,171 @@ def on_release(k):
# 小车底盘body id
chassis_body_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, "chassis")
+# ------------------- 路径记忆类 -------------------
+class PathMemory:
+ """路径记忆与学习系统"""
+
+ def __init__(self, memory_size=50):
+ self.memory = deque(maxlen=memory_size)
+ self.path_scores = {} # 路径评分字典
+ self.obstacle_history = {} # 障碍物历史
+ self.successful_paths = [] # 成功路径记录
+ self.debug_mode = False
+
+ def add_experience(self, position, direction, success, distance_traveled):
+ """添加路径经验"""
+ key = self._create_key(position, direction)
+
+ # 更新路径评分
+ if key in self.path_scores:
+ if success:
+ self.path_scores[key] += PATH_REWARD * LEARNING_RATE
+ else:
+ self.path_scores[key] += PATH_PENALTY * LEARNING_RATE
+ else:
+ if success:
+ self.path_scores[key] = PATH_REWARD
+ else:
+ self.path_scores[key] = PATH_PENALTY
+
+ # 记录经验
+ experience = {
+ 'position': tuple(position[:2]), # 只记录x,y坐标
+ 'direction': direction,
+ 'success': success,
+ 'distance': distance_traveled,
+ 'timestamp': time.time()
+ }
+ self.memory.append(experience)
+
+ if self.debug_mode:
+ print(f"路径经验: {direction}, 成功: {success}, 评分: {self.path_scores.get(key, 0):.2f}")
+
+ def get_best_direction(self, position, available_directions):
+ """获取最佳方向(基于历史经验)"""
+ if random.random() < EXPLORATION_RATE:
+ # 探索:随机选择一个方向
+ return random.choice(available_directions)
+
+ # 利用:选择评分最高的方向
+ best_direction = None
+ best_score = -float('inf')
+
+ for direction in available_directions:
+ key = self._create_key(position, direction)
+ base_score = DIRECTION_SCORES.get(direction, 0.5)
+ memory_score = self.path_scores.get(key, 0)
+ total_score = base_score + memory_score
+
+ if total_score > best_score:
+ best_score = total_score
+ best_direction = direction
+
+ return best_direction or random.choice(available_directions)
+
+ def record_obstacle(self, obstacle_name, position):
+ """记录障碍物位置"""
+ key = f"{obstacle_name}_{int(position[0]*10)}_{int(position[1]*10)}"
+ self.obstacle_history[key] = {
+ 'name': obstacle_name,
+ 'position': tuple(position[:2]),
+ 'timestamp': time.time(),
+ 'count': self.obstacle_history.get(key, {}).get('count', 0) + 1
+ }
+
+ def is_recent_obstacle(self, position, threshold=0.5):
+ """检查位置附近是否有近期遇到的障碍物"""
+ for key, data in self.obstacle_history.items():
+ obs_pos = data['position']
+ distance = math.sqrt((obs_pos[0] - position[0])**2 + (obs_pos[1] - position[1])**2)
+ if distance < threshold and (time.time() - data['timestamp']) < 10:
+ return True
+ return False
+
+ def record_successful_path(self, start_pos, end_pos, directions):
+ """记录成功路径"""
+ path = {
+ 'start': tuple(start_pos[:2]),
+ 'end': tuple(end_pos[:2]),
+ 'directions': directions[:],
+ 'length': len(directions),
+ 'timestamp': time.time()
+ }
+ self.successful_paths.append(path)
+
+ def save_to_file(self, filename="path_memory.json"):
+ """保存路径记忆到文件"""
+ data = {
+ 'path_scores': self.path_scores,
+ 'obstacle_history': self.obstacle_history,
+ 'successful_paths': self.successful_paths[-10:], # 只保存最近10条
+ }
+ with open(filename, 'w') as f:
+ json.dump(data, f, indent=2)
+ print(f"路径记忆已保存到 {filename}")
+
+ def load_from_file(self, filename="path_memory.json"):
+ """从文件加载路径记忆"""
+ try:
+ with open(filename, 'r') as f:
+ data = json.load(f)
+ self.path_scores = data.get('path_scores', {})
+ self.obstacle_history = data.get('obstacle_history', {})
+ self.successful_paths = data.get('successful_paths', [])
+ print(f"已从 {filename} 加载路径记忆")
+ except FileNotFoundError:
+ print(f"未找到记忆文件 {filename},从头开始学习")
+
+ def _create_key(self, position, direction):
+ """创建记忆键"""
+ x, y = int(position[0]*10), int(position[1]*10)
+ return f"{x}_{y}_{direction}"
+
+ def toggle_debug(self):
+ """切换调试模式"""
+ self.debug_mode = not self.debug_mode
+ print(f"调试模式: {'开启' if self.debug_mode else '关闭'}")
+
+# 初始化路径记忆系统
+path_memory = PathMemory(PATH_MEMORY_SIZE)
+path_memory.load_from_file()
+
# ------------------- 复位函数 -------------------
def reset_car():
mujoco.mj_resetData(model, data)
data.qpos[2] = 0.03
print("\n>>> 已复位 <<<")
-# ------------------- 障碍物检测函数 -------------------
-def check_front_obstacle(direction_angle=0):
+ # 重置状态变量
+ global CAR_STATE, turn_counter, scan_counter, deceleration_counter
+ global current_speed, turn_angle, turn_direction, path_history
+ global last_success_pos, distance_since_last_obstacle
+
+ CAR_STATE = "CRUISING"
+ turn_counter = 0
+ scan_counter = 0
+ deceleration_counter = 0
+ current_speed = CRUISE_SPEED
+ turn_angle = 0
+ turn_direction = ""
+ path_history = [] # 重置路径历史
+ last_success_pos = get_car_position() # 记录起始位置
+ distance_since_last_obstacle = 0.0
+
+# ------------------- 小车位置获取 -------------------
+def get_car_position():
+ """获取小车当前位置"""
+ return data.body(chassis_body_id).xpos.copy()
+
+def get_car_velocity():
+ """获取小车当前速度"""
+ return np.linalg.norm(data.qvel[:3])
+
+# ------------------- 障碍物检测函数(增强版) -------------------
+def check_front_obstacle(direction_angle=0, scan_width=0.5):
"""检测指定方向是否有障碍物"""
- # 获取小车当前位置
- chassis_pos = data.body(chassis_body_id).xpos
+ chassis_pos = get_car_position()
# 获取小车当前速度方向
velocity = data.qvel[:2]
@@ -92,6 +278,7 @@ def check_front_obstacle(direction_angle=0):
# 扫描所有障碍物
min_distance = float('inf')
closest_obstacle = None
+ obstacle_pos = None
for obs_name, obs_id in obstacle_ids.items():
obs_pos = data.body(obs_id).xpos
@@ -109,91 +296,154 @@ def check_front_obstacle(direction_angle=0):
dot_product = max(-1.0, min(1.0, dot_product))
angle_diff = math.acos(dot_product)
- if angle_diff < math.radians(45) and dx > -0.2:
- cross_product = np.cross([forward_direction[0], forward_direction[1], 0],
- [obstacle_direction[0], obstacle_direction[1], 0])
- lateral_distance = abs(cross_product[2]) * distance
+ # 计算横向距离(考虑扫描宽度)
+ cross_product = np.cross([forward_direction[0], forward_direction[1], 0],
+ [obstacle_direction[0], obstacle_direction[1], 0])
+ lateral_distance = abs(cross_product[2]) * distance
- if angle_diff < math.radians(25) and lateral_distance < 0.5:
- if distance < min_distance:
- min_distance = distance
- closest_obstacle = obs_name
+ if angle_diff < math.radians(45) and lateral_distance < scan_width:
+ if distance < min_distance:
+ min_distance = distance
+ closest_obstacle = obs_name
+ obstacle_pos = obs_pos.copy()
if closest_obstacle is not None:
if min_distance < SAFE_DISTANCE:
- return 2, min_distance, closest_obstacle
+ return 2, min_distance, closest_obstacle, obstacle_pos
else:
- return 1, min_distance, closest_obstacle
+ return 1, min_distance, closest_obstacle, obstacle_pos
- return 0, 0, None
+ return 0, 0, None, None
-# ------------------- 路径扫描函数 -------------------
-def scan_path_directions():
- """扫描多个方向的路径可行性"""
+# ------------------- 多方向路径扫描 -------------------
+def scan_all_directions():
+ """扫描所有可能的方向"""
- path_scores = []
+ directions_info = {}
+ chassis_pos = get_car_position()
- for angle_deg in PATH_SCAN_ANGLES:
- angle_rad = math.radians(angle_deg)
+ # 检查每个定义的方向
+ for dir_name, dir_angle in DIRECTIONS.items():
+ obstacle_status, distance, obs_name, _ = check_front_obstacle(
+ dir_angle,
+ scan_width=0.4 if "sharp" in dir_name else 0.3
+ )
- # 检查该方向是否有障碍物
- obstacle_status, obstacle_distance, _ = check_front_obstacle(angle_rad)
+ # 计算方向得分
+ base_score = DIRECTION_SCORES.get(dir_name, 0.5)
- # 计算分数:距离越远分数越高
if obstacle_status == 0:
- # 无障碍物,给最高分
- score = PATH_SCAN_DISTANCE
- elif obstacle_status == 1:
+ # 无障碍物,高分
+ safety_score = 1.0
+ elif obstacle_status == 1 and distance > 0.5:
# 有障碍物但距离较远
- score = obstacle_distance
+ safety_score = 0.6
else:
# 有近距离障碍物
- score = 0
+ safety_score = 0.2
+
+ # 结合历史经验
+ memory_score = 0
+ if dir_name in ["forward", "slight_left", "slight_right"]:
+ memory_key = path_memory._create_key(chassis_pos, dir_name)
+ memory_score = path_memory.path_scores.get(memory_key, 0)
- path_scores.append((angle_rad, score, obstacle_status))
+ total_score = base_score * 0.4 + safety_score * 0.4 + memory_score * 0.2
- # 按分数排序
- path_scores.sort(key=lambda x: x[1], reverse=True)
+ directions_info[dir_name] = {
+ 'angle': dir_angle,
+ 'status': obstacle_status,
+ 'distance': distance,
+ 'obstacle': obs_name,
+ 'score': total_score
+ }
- return path_scores
+ return directions_info
-# ------------------- 选择最佳路径 -------------------
-def choose_best_path():
- """选择最佳路径方向"""
+# ------------------- 智能路径选择 -------------------
+def choose_intelligent_path():
+ """智能选择最佳路径"""
# 扫描所有方向
- path_scores = scan_path_directions()
+ directions_info = scan_all_directions()
+ chassis_pos = get_car_position()
+
+ # 过滤安全方向
+ safe_directions = []
+ for dir_name, info in directions_info.items():
+ if info['status'] == 0 or (info['status'] == 1 and info['distance'] > 0.5):
+ safe_directions.append(dir_name)
+
+ # 如果没有安全方向,返回需要后退
+ if not safe_directions:
+ # 检查是否已经尝试过很多次
+ if len(path_history) > 8:
+ return "backward", "后退并重新规划"
+ else:
+ # 选择障碍物最远的方向
+ best_dir = max(directions_info.items(), key=lambda x: x[1]['distance'])[0]
+ return best_dir, f"强制{best_dir}(距离:{directions_info[best_dir]['distance']:.2f}m)"
- # 选择最佳方向
- best_angle, best_score, best_status = path_scores[0]
+ # 使用路径记忆选择最佳方向
+ best_direction = path_memory.get_best_direction(chassis_pos, safe_directions)
- # 如果所有方向都有障碍物,选择障碍物最远的方向
- if best_score == 0:
- # 后退方案
- print("所有方向都有障碍物,尝试后退")
- return None, "BACKUP"
+ # 获取方向信息
+ info = directions_info[best_direction]
- # 如果最佳方向不是正前方,需要转向
- if abs(best_angle) > math.radians(10):
- if best_angle > 0:
- return best_angle, f"左转{abs(math.degrees(best_angle)):.0f}度"
- else:
- return best_angle, f"右转{abs(math.degrees(best_angle)):.0f}度"
+ # 生成描述文本
+ if best_direction == "forward":
+ direction_text = "直行"
+ elif best_direction == "backward":
+ direction_text = "后退"
else:
- return 0, "直行"
+ angle_deg = math.degrees(info['angle'])
+ if "left" in best_direction:
+ direction_text = f"左转{abs(angle_deg):.0f}度"
+ else:
+ direction_text = f"右转{abs(angle_deg):.0f}度"
+
+ return best_direction, direction_text
+
+# ------------------- 路径历史记录 -------------------
+def update_path_history(direction, success):
+ """更新路径历史"""
+ global path_history, last_success_pos, distance_since_last_obstacle
+
+ current_pos = get_car_position()
+ path_history.append({
+ 'direction': direction,
+ 'position': tuple(current_pos[:2]),
+ 'success': success,
+ 'time': time.time()
+ })
+
+ # 只保留最近20条记录
+ if len(path_history) > 20:
+ path_history = path_history[-20:]
+
+ # 更新距离
+ if success:
+ distance_since_last_obstacle += get_car_velocity() * 0.002 # 近似距离增量
+
+ # 如果成功行驶了一段距离,记录成功路径
+ if success and distance_since_last_obstacle > 1.0:
+ path_memory.record_successful_path(last_success_pos, current_pos,
+ [h['direction'] for h in path_history[-5:]])
+ last_success_pos = current_pos.copy()
+ distance_since_last_obstacle = 0.0
# ------------------- 后退操作 -------------------
def perform_backup(backup_counter):
"""执行后退操作"""
- if backup_counter < 30:
+ if backup_counter < 40:
# 后退(负速度)
data.ctrl[0] = 0.0
data.ctrl[1] = 0.0
- data.ctrl[2] = -TURN_SPEED * 0.5
- data.ctrl[3] = -TURN_SPEED * 0.5
- data.ctrl[4] = -TURN_SPEED * 0.5
- data.ctrl[5] = -TURN_SPEED * 0.5
+ data.ctrl[2] = -TURN_SPEED * 0.4
+ data.ctrl[3] = -TURN_SPEED * 0.4
+ data.ctrl[4] = -TURN_SPEED * 0.4
+ data.ctrl[5] = -TURN_SPEED * 0.4
return False, backup_counter + 1
else:
# 后退完成
@@ -205,42 +455,65 @@ def perform_backup(backup_counter):
mujoco.mj_resetData(model, data)
# 状态变量
-CAR_STATE = "CRUISING" # CRUISING, DECELERATING, STOPPED, PATH_PLANNING, TURNING, SCANNING, RESUME, BACKING_UP
+CAR_STATE = "CRUISING"
turn_counter = 0
turn_angle = 0
turn_direction = ""
scan_counter = 0
deceleration_counter = 0
-current_speed = CRUISE_SPEED
-turn_attempts = 0 # 转向尝试次数
backup_counter = 0
+current_speed = CRUISE_SPEED
+path_history = [] # 路径历史记录
+last_success_pos = get_car_position() # 最后成功位置
+distance_since_last_obstacle = 0.0
+last_obstacle_time = 0 # 上次遇到障碍物的时间
with mujoco.viewer.launch_passive(model, data) as viewer:
viewer.cam.distance = 2.5
viewer.cam.elevation = -25
+ print("=== 智能绕障小车启动 ===")
+ print("控制说明:")
+ print(" R - 复位小车")
+ print(" D - 切换调试模式")
+ print(" S - 保存路径记忆")
+ print("======================")
+
while viewer.is_running():
- # 检查R键复位
+ # 检查键盘输入
if KEYS.get(keyboard.KeyCode.from_char('r'), False):
reset_car()
- CAR_STATE = "CRUISING"
- turn_counter = 0
- scan_counter = 0
- deceleration_counter = 0
- current_speed = CRUISE_SPEED
- turn_attempts = 0
- backup_counter = 0
KEYS[keyboard.KeyCode.from_char('r')] = False
+ if KEYS.get(keyboard.KeyCode.from_char('d'), False):
+ path_memory.toggle_debug()
+ KEYS[keyboard.KeyCode.from_char('d')] = False
+
+ if KEYS.get(keyboard.KeyCode.from_char('s'), False):
+ path_memory.save_to_file()
+ KEYS[keyboard.KeyCode.from_char('s')] = False
+
+ # 获取小车当前位置和速度
+ car_pos = get_car_position()
+ car_vel = get_car_velocity()
+
# 根据当前状态执行不同操作
if CAR_STATE == "CRUISING":
# 巡航状态
- obstacle_status, obstacle_distance, obstacle_name = check_front_obstacle()
+ obstacle_status, obstacle_distance, obstacle_name, obs_pos = check_front_obstacle()
if obstacle_status == 2:
# 紧急停止
CAR_STATE = "STOPPED"
print(f"\n⚠️ 紧急停止!障碍物距离: {obstacle_distance:.2f}m")
+
+ # 记录障碍物
+ if obs_pos is not None:
+ path_memory.record_obstacle(obstacle_name, obs_pos)
+
+ # 记录失败的路径经验
+ path_memory.add_experience(car_pos, "forward", False, distance_since_last_obstacle)
+
turn_counter = 0
current_speed = 0
@@ -252,6 +525,15 @@ def perform_backup(backup_counter):
CAR_STATE = "DECELERATING"
deceleration_counter = 0
print(f"\n⚠️ 检测到障碍物: {obstacle_name}({obstacle_distance:.2f}m),开始减速...")
+
+ # 记录障碍物
+ if obs_pos is not None:
+ path_memory.record_obstacle(obstacle_name, obs_pos)
+
+ # 记录失败的路径经验
+ path_memory.add_experience(car_pos, "forward", False, distance_since_last_obstacle)
+
+ last_obstacle_time = time.time()
else:
# 无障碍物,高速巡航
data.ctrl[0] = 0.0
@@ -262,6 +544,9 @@ def perform_backup(backup_counter):
data.ctrl[5] = CRUISE_SPEED
current_speed = CRUISE_SPEED
+ # 更新路径历史
+ update_path_history("forward", True)
+
elif CAR_STATE == "DECELERATING":
# 减速状态
deceleration_counter += 1
@@ -290,32 +575,26 @@ def perform_backup(backup_counter):
data.ctrl[i] = 0.0
if turn_counter > 10:
- print("正在规划路径...")
+ print("正在智能规划路径...")
CAR_STATE = "PATH_PLANNING"
turn_counter = 0
elif CAR_STATE == "PATH_PLANNING":
# 路径规划状态
- print("扫描可行路径...")
+ print("扫描环境并选择最佳路径...")
- # 选择最佳路径
- best_angle, path_direction = choose_best_path()
+ # 智能选择路径
+ chosen_direction, direction_text = choose_intelligent_path()
- if path_direction == "BACKUP":
+ if chosen_direction == "backward":
# 需要后退
- print("所有方向都有障碍物,执行后退")
+ print("路径受阻,执行后退操作")
CAR_STATE = "BACKING_UP"
backup_counter = 0
else:
# 找到可行路径
- turn_angle = best_angle
- turn_direction = path_direction
-
- # 检查是否需要大角度转向
- if abs(turn_angle) > math.radians(45):
- turn_attempts += 1
- if turn_attempts >= MAX_TURN_ATTEMPTS:
- print(f"多次尝试后选择大角度转向: {turn_direction}")
+ turn_angle = DIRECTIONS[chosen_direction]
+ turn_direction = direction_text
print(f"选择路径: {turn_direction}")
CAR_STATE = "TURNING"
@@ -328,13 +607,14 @@ def perform_backup(backup_counter):
if backup_complete:
print("后退完成,重新规划路径")
CAR_STATE = "PATH_PLANNING"
+ update_path_history("backward", True)
elif CAR_STATE == "TURNING":
# 转向状态
turn_counter += 1
# 渐进式转向
- progress = min(1.0, turn_counter / 10.0)
+ progress = min(1.0, turn_counter / 8.0)
current_angle = turn_angle * progress
data.ctrl[0] = current_angle
@@ -349,18 +629,19 @@ def perform_backup(backup_counter):
data.ctrl[5] = current_speed
if turn_counter % 15 == 0:
- print(f"正在{turn_direction},角度: {math.degrees(current_angle):.1f}度")
+ print(f"正在{turn_direction},进度: {progress*100:.0f}%")
if turn_counter > TURN_DURATION:
- print(f"{turn_direction}完成,开始扫描前方...")
- CAR_STATE = "SCANNING"
+ print(f"{turn_direction}完成,开始验证路径...")
+ CAR_STATE = "PATH_VERIFICATION"
turn_counter = 0
scan_counter = 0
- elif CAR_STATE == "SCANNING":
- # 扫描状态
+ elif CAR_STATE == "PATH_VERIFICATION":
+ # 路径验证状态
scan_counter += 1
+ # 保持转向角度,低速前进验证路径
data.ctrl[0] = turn_angle * 0.5
data.ctrl[1] = turn_angle * 0.5
data.ctrl[2] = TURN_SPEED * 0.6
@@ -370,20 +651,25 @@ def perform_backup(backup_counter):
current_speed = TURN_SPEED * 0.6
if scan_counter % 10 == 0:
- obstacle_status, obstacle_distance, obstacle_name = check_front_obstacle()
+ # 检查前方是否安全
+ obstacle_status, obstacle_distance, obstacle_name, _ = check_front_obstacle()
if obstacle_status == 0:
- print("前方安全,准备恢复巡航")
+ print("路径验证通过,准备恢复巡航")
+
+ # 记录成功的路径经验
+ chosen_direction_name = [k for k, v in DIRECTIONS.items() if abs(v - turn_angle) < 0.01][0]
+ path_memory.add_experience(car_pos, chosen_direction_name, True, distance_since_last_obstacle)
+
CAR_STATE = "RESUME"
turn_counter = 0
- turn_attempts = 0 # 重置尝试次数
else:
- print(f"转向后仍检测到障碍物: {obstacle_name}({obstacle_distance:.2f}m),重新规划")
+ print(f"路径验证失败,检测到障碍物: {obstacle_name}({obstacle_distance:.2f}m)")
CAR_STATE = "STOPPED"
turn_counter = 0
- if scan_counter > 50:
- print("扫描超时,尝试恢复巡航")
+ if scan_counter > 40:
+ print("路径验证超时,尝试恢复巡航")
CAR_STATE = "RESUME"
turn_counter = 0
@@ -412,11 +698,15 @@ def perform_backup(backup_counter):
data.ctrl[5] = CRUISE_SPEED
current_speed = CRUISE_SPEED
- obstacle_status, obstacle_distance, _ = check_front_obstacle()
+ obstacle_status, obstacle_distance, _, _ = check_front_obstacle()
if obstacle_status == 0:
- print("恢复巡航成功")
+ print("成功恢复巡航")
CAR_STATE = "CRUISING"
turn_counter = 0
+
+ # 更新路径历史
+ chosen_direction_name = [k for k, v in DIRECTIONS.items() if abs(v - turn_angle) < 0.01][0]
+ update_path_history(chosen_direction_name, True)
else:
print("恢复巡航时检测到障碍物,重新处理")
CAR_STATE = "STOPPED"
@@ -426,23 +716,29 @@ def perform_backup(backup_counter):
mujoco.mj_step(model, data)
# 显示信息
- vel = np.linalg.norm(data.qvel[:3])
+ vel = car_vel
current_steer = (data.ctrl[0] + data.ctrl[1]) / 2
+ # 显示状态信息
status_info = f"状态: {CAR_STATE}, 速度: {vel:7.5f} m/s"
if abs(current_steer) > 0.01:
- status_info += f", 转向: {math.degrees(current_steer):.1f}度"
+ status_info += f", 转向: {math.degrees(current_steer):.1f}°"
- status_info += f", 尝试次数: {turn_attempts}"
+ # 显示路径历史长度
+ status_info += f", 历史: {len(path_history)}"
- if CAR_STATE == "CRUISING":
- obstacle_status, obstacle_distance, obstacle_name = check_front_obstacle()
+ # 显示记忆系统状态
+ status_info += f", 记忆: {len(path_memory.memory)}"
+
+ if path_memory.debug_mode and CAR_STATE == "CRUISING":
+ obstacle_status, obstacle_distance, obstacle_name, _ = check_front_obstacle()
if obstacle_status > 0 and obstacle_name:
- status_info += f", 前方障碍: {obstacle_name}({obstacle_distance:.2f}m)"
+ status_info += f", 障碍: {obstacle_name}({obstacle_distance:.2f}m)"
print(f"\r{status_info}", end='', flush=True)
# 同步视图
viewer.sync()
-print("\n程序结束")
\ No newline at end of file
+print("\n程序结束,保存路径记忆...")
+path_memory.save_to_file()
\ No newline at end of file
diff --git a/src/simple_wheeled_vehicle/wheeled_car.xml b/src/simple_wheeled_vehicle/wheeled_car.xml
index 58ae5399a3..908aadffe3 100644
--- a/src/simple_wheeled_vehicle/wheeled_car.xml
+++ b/src/simple_wheeled_vehicle/wheeled_car.xml
@@ -64,7 +64,7 @@
+