From 480eb3443e5d2008197b5b57cd16ebaeb2210937 Mon Sep 17 00:00:00 2001
From: windows <2877519694@qq.com>
Date: Wed, 24 Dec 2025 22:53:43 +0800
Subject: [PATCH] =?UTF-8?q?=E6=89=A9=E5=A4=A7=E5=9C=B0=E5=9B=BE=EF=BC=8C?=
=?UTF-8?q?=E6=B7=BB=E5=8A=A0=E8=BE=B9=E7=95=8C=E5=A2=99=E5=92=8C=E6=9B=B4?=
=?UTF-8?q?=E5=A4=9A=E9=9A=9C=E7=A2=8D=E7=89=A9?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/simple_wheeled_vehicle/main.py | 362 ++++++++++++++-------
src/simple_wheeled_vehicle/wheeled_car.xml | 117 ++++++-
2 files changed, 360 insertions(+), 119 deletions(-)
diff --git a/src/simple_wheeled_vehicle/main.py b/src/simple_wheeled_vehicle/main.py
index 6dca602b96..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
@@ -37,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 # 扩大扫描范围
+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, # 后退最后考虑
+}
-# 路径规划参数
-MAX_TURN_ATTEMPTS = 3 # 最大转向尝试次数
-PATH_SCAN_ANGLES = [-60, -30, 0, 30, 60] # 扫描角度(度)
-PATH_SCAN_DISTANCE = 1.5 # 路径扫描距离
+# 新增:方向定义
+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 = [
@@ -200,12 +227,36 @@ def reset_car():
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]
@@ -245,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
-
- path_scores.append((angle_rad, score, obstacle_status))
-
- # 按分数排序
- path_scores.sort(key=lambda x: x[1], reverse=True)
+ 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)
+
+ total_score = base_score * 0.4 + safety_score * 0.4 + memory_score * 0.2
+
+ 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:
# 后退完成
@@ -341,15 +455,18 @@ 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
@@ -366,13 +483,6 @@ def perform_backup(backup_counter):
# 检查键盘输入
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):
@@ -390,12 +500,20 @@ def perform_backup(backup_counter):
# 根据当前状态执行不同操作
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
@@ -407,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
@@ -417,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
@@ -445,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"
@@ -483,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
@@ -504,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
@@ -525,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
@@ -567,9 +698,9 @@ 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
@@ -585,17 +716,22 @@ 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", 尝试次数: {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)"
diff --git a/src/simple_wheeled_vehicle/wheeled_car.xml b/src/simple_wheeled_vehicle/wheeled_car.xml
index 58ae5399a3..a15b1416b6 100644
--- a/src/simple_wheeled_vehicle/wheeled_car.xml
+++ b/src/simple_wheeled_vehicle/wheeled_car.xml
@@ -8,7 +8,13 @@