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 @@
+