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 @@ + + + + + + @@ -32,9 +38,107 @@ - + + + + + - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -63,11 +167,12 @@ - - - + + + - + +