diff --git a/src/embodied_robot/robot_walk/Robot_move_straight.xml b/src/embodied_robot/robot_walk/Robot_move_straight.xml
index 1450940ec7..aa65f0e3a4 100644
--- a/src/embodied_robot/robot_walk/Robot_move_straight.xml
+++ b/src/embodied_robot/robot_walk/Robot_move_straight.xml
@@ -10,11 +10,11 @@
      limitations under the License.
 -->
 
-<mujoco model="Humanoid_With_Dynamic_Wall_Navigation">
+<mujoco model="Humanoid_Complex_Dynamic_Env_Navigation">
   <option timestep="0.005"/>
 
   <visual>
-    <map force="0.1" zfar="30"/>
+    <map force="0.1" zfar="50"/>  <!-- 增大可视距离适配更多障碍 -->
     <rgba haze="0.15 0.25 0.35 1"/>
     <global offwidth="2560" offheight="1440" elevation="-20" azimuth="120"/>
   </visual>
@@ -27,9 +27,11 @@
     <material name="body" texture="body" texuniform="true" rgba="0.8 0.6 .4 1"/>
     <texture name="grid" type="2d" builtin="checker" width="512" height="512" rgb1=".1 .2 .3" rgb2=".2 .3 .4"/>
     <material name="grid" texture="grid" texrepeat="1 1" texuniform="true" reflectance=".2"/>
-    <!-- 障碍物材质（区分固定/动态障碍） -->
+    <!-- 障碍物材质（区分固定/不同动态障碍） -->
     <material name="wall_gray" rgba="0.6 0.6 0.6 1"/>    <!-- 固定障碍 -->
-    <material name="wall_blue" rgba="0.3 0.5 0.8 1"/>    <!-- 动态障碍 -->
+    <material name="wall_blue" rgba="0.3 0.5 0.8 1"/>    <!-- 动态障碍1：正弦组合运动 -->
+    <material name="wall_green" rgba="0.3 0.8 0.5 1"/>   <!-- 动态障碍2：随机游走 -->
+    <material name="wall_purple" rgba="0.7 0.4 0.8 1"/>  <!-- 动态障碍3：圆周运动 -->
     <!-- 目标点材质 -->
     <material name="target_material" rgba="1 0 0 1"/>     <!-- 红色目标点 -->
     <material name="target_base_material" rgba="1 0.5 0 1"/> <!-- 橙色基座 -->
@@ -100,9 +102,12 @@
       <default class="elbow">
         <joint range="-100 50" stiffness="0"/>
       </default>
-      <!-- 动态障碍关节默认配置 -->
-      <default class="dynamic_wall_joint">
-        <joint type="slide" damping="10" stiffness="0" limited="true"/>
+      <!-- 动态障碍关节默认配置（不同阻尼适配不同运动） -->
+      <default class="dynamic_wall_joint_slide">
+        <joint type="slide" damping="8" stiffness="0" limited="true"/>
+      </default>
+      <default class="dynamic_wall_joint_rotate">
+        <joint type="slide" damping="5" stiffness="0" limited="true"/>
       </default>
     </default>
   </default>
@@ -110,34 +115,63 @@
   <worldbody>
     <geom name="floor" size="0 0 .05" type="plane" material="grid" condim="3"/>
 
-    <!-- ====================== 导航目标点（固定位置 x=8, y=0, z=0.2） ====================== -->
-    <body name="navigation_target" pos="8 0 0.2">
+    <!-- ====================== 导航目标点（固定位置 x=12, y=0, z=0.2） ====================== -->
+    <body name="navigation_target" pos="12 0 0.2">
       <geom name="target_marker" type="sphere" size="0.15" material="target_material" condim="3"/>
       <geom name="target_base" type="cylinder" size="0.18 0.02" material="target_base_material" condim="3"/>
     </body>
 
-    <!-- 障碍1：固定位置（机器人正前方3米） -->
+    <!-- 障碍1：固定障碍（机器人正前方3米） -->
     <body name="wall1" pos="3 0 0.5">
       <geom type="box" size="0.5 1 0.8" material="wall_gray" friction="1.0 0.1 0.1"/>
       <inertial pos="0 0 0" mass="100" diaginertia="1 1 1"/>
     </body>
 
-    <!-- 障碍2：动态障碍（沿Y轴左右移动，X轴固定3米） -->
-    <body name="wall2_base" pos="3.0 0.0 0.5">
-      <!-- 滑动关节：沿Y轴移动，范围-2~2米 -->
-      <joint name="wall2_slide_y" class="dynamic_wall_joint" axis="0 1 0" range="-2.0 2.0"/>
+    <!-- 障碍2：动态障碍1（X=4米，Y/Z组合正弦运动） -->
+    <body name="wall2_base" pos="4.0 0.0 0.5">
+      <!-- Y轴滑动关节 -->
+      <joint name="wall2_slide_y" class="dynamic_wall_joint_slide" axis="0 1 0" range="-3.0 3.0"/>
+      <!-- Z轴滑动关节（增加垂直方向运动） -->
+      <joint name="wall2_slide_z" class="dynamic_wall_joint_slide" axis="0 0 1" range="0.3 1.2"/>
       <!-- 动态障碍本体 -->
       <body name="wall2" pos="0 0 0">
         <geom type="box" size="0.4 0.8 0.7" material="wall_blue" friction="1.0 0.1 0.1"/>
-        <inertial pos="0 0 0" mass="50" diaginertia="1 1 1"/> <!-- 降低质量便于移动 -->
+        <inertial pos="0 0 0" mass="50" diaginertia="1 1 1"/>
+      </body>
+    </body>
+
+    <!-- 障碍3：动态障碍2（X=6米，随机游走运动） -->
+    <body name="wall3_base" pos="6.0 0.0 0.5">
+      <!-- X轴滑动关节（小幅前后移动） -->
+      <joint name="wall3_slide_x" class="dynamic_wall_joint_slide" axis="1 0 0" range="5.0 7.0"/>
+      <!-- Y轴滑动关节（左右移动） -->
+      <joint name="wall3_slide_y" class="dynamic_wall_joint_slide" axis="0 1 0" range="-2.5 2.5"/>
+      <!-- 动态障碍本体 -->
+      <body name="wall3" pos="0 0 0">
+        <geom type="box" size="0.6 0.7 0.8" material="wall_green" friction="1.0 0.1 0.1"/>
+        <inertial pos="0 0 0" mass="60" diaginertia="1 1 1"/>
+      </body>
+    </body>
+
+    <!-- 障碍4：动态障碍3（圆周运动，中心X=8,Y=1） -->
+    <body name="wall4_rot_base" pos="8.0 1.0 0.5">
+      <!-- 旋转关节（圆周运动的中心） -->
+      <joint name="wall4_rotate" type="hinge" axis="0 0 1" damping="3" stiffness="0"/>
+      <!-- 径向滑动关节（调整圆周半径） -->
+      <joint name="wall4_radial" class="dynamic_wall_joint_rotate" axis="1 0 0" range="0.5 2.0"/>
+      <!-- 动态障碍本体 -->
+      <body name="wall4" pos="1.0 0 0">
+        <geom type="cylinder" size="0.5 0.7" material="wall_purple" friction="1.0 0.1 0.1"/>
+        <inertial pos="0 0 0" mass="40" diaginertia="1 1 1"/>
       </body>
     </body>
 
     <light name="spotlight" mode="targetbodycom" target="torso" diffuse=".8 .8 .8" specular="0.3 0.3 0.3" pos="0 -6 4" cutoff="30"/>
-    <light name="top" pos="0 0 2" mode="trackcom"/>
+    <light name="top" pos="0 0 4" mode="trackcom"/>  <!-- 提高顶光高度 -->
     <body name="torso" pos="0 0 1.282" childclass="body">
       <camera name="back" pos="-3 0 1" xyaxes="0 -1 0 1 0 2" mode="trackcom"/>
       <camera name="side" pos="0 -3 1" xyaxes="1 0 0 0 1 2" mode="trackcom"/>
+      <camera name="wide" pos="-5 -5 3" xyaxes="1 1 0 0 0 1" mode="trackcom"/> <!-- 新增广角相机 -->
       <freejoint name="root"/>
       <geom name="torso" fromto="0 -.07 0 0 .07 0" size=".07"/>
       <geom name="waist_upper" fromto="-.01 -.06 -.12 -.01 .06 -.12" size=".06"/>
@@ -216,6 +250,10 @@
   <contact>
     <exclude body1="waist_lower" body2="thigh_right"/>
     <exclude body1="waist_lower" body2="thigh_left"/>
+    <!-- 排除障碍间的碰撞 -->
+    <exclude body1="wall2" body2="wall3"/>
+    <exclude body1="wall3" body2="wall4"/>
+    <exclude body1="wall2" body2="wall4"/>
   </contact>
 
   <tendon>
@@ -251,8 +289,13 @@
     <motor name="shoulder1_left"  gear="20"  joint="shoulder1_left"/>
     <motor name="shoulder2_left"  gear="20"  joint="shoulder2_left"/>
     <motor name="elbow_left"      gear="40"  joint="elbow_left"/>
-    <!-- 动态障碍驱动电机 -->
-    <motor name="wall2_motor" gear="50" joint="wall2_slide_y" ctrlrange="-1 1"/>
+    <!-- 动态障碍驱动电机（新增多个） -->
+    <motor name="wall2_motor_y" gear="50" joint="wall2_slide_y" ctrlrange="-1 1"/>
+    <motor name="wall2_motor_z" gear="30" joint="wall2_slide_z" ctrlrange="-1 1"/>
+    <motor name="wall3_motor_x" gear="40" joint="wall3_slide_x" ctrlrange="-1 1"/>
+    <motor name="wall3_motor_y" gear="45" joint="wall3_slide_y" ctrlrange="-1 1"/>
+    <motor name="wall4_motor_rot" gear="25" joint="wall4_rotate" ctrlrange="-1 1"/>
+    <motor name="wall4_motor_rad" gear="35" joint="wall4_radial" ctrlrange="-1 1"/>
   </actuator>
 
   <keyframe>
diff --git a/src/embodied_robot/robot_walk/move_straight.py b/src/embodied_robot/robot_walk/move_straight.py
index f3b7c8a64c..960017719d 100644
--- a/src/embodied_robot/robot_walk/move_straight.py
+++ b/src/embodied_robot/robot_walk/move_straight.py
@@ -3,87 +3,145 @@
 import time
 import numpy as np
 import random
+from collections import deque
+
+# 设置随机种子保证可复现
+np.random.seed(42)
+random.seed(42)
 
 
 def control_robot(model_path):
     """
-    控制DeepMind Humanoid模型：目标导航（固定目标点）→ 动态避障 → 到达目标点停止
+    控制DeepMind Humanoid模型：复杂动态环境下的目标导航
+    特性：多动态障碍（正弦组合/随机游走/圆周运动）+ 多障碍优先级避障 + 目标导航
     """
     # 加载模型和数据
     model = mujoco.MjModel.from_xml_path(model_path)
     data = mujoco.MjData(model)
 
     # -------------------------- 核心配置：固定导航目标点 --------------------------
-    TARGET_POS = np.array([8.0, 0.0])  # 固定目标点坐标 (x=8, y=0)
-    target_reached_threshold = 0.5  # 到达目标点的距离阈值（米）
+    TARGET_POS = np.array([12.0, 0.0])  # 目标点后移至12米，适配更多障碍
+    target_reached_threshold = 0.8  # 增大到达阈值，适配复杂环境
     navigation_target_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, "navigation_target")
 
-    # -------------------------- 动态障碍初始化 --------------------------
-    # 获取动态障碍关节和电机ID
-    wall2_joint_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, "wall2_slide_y")
-    wall2_motor_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "wall2_motor")
-    wall2_body_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, "wall2")
-
-    # 动态障碍参数
-    wall2_speed = 0.8  # 移动速度
-    wall2_amplitude = 1.8  # 移动幅度（Y轴-1.8~1.8米）
-    wall2_phase = random.uniform(0, 2 * np.pi)  # 随机初始相位
-
-    # 打印电机数量（调试用）
-    print(f"模型电机数量：{model.nu}，data.ctrl长度：{len(data.ctrl)}")
-    print(f"动态障碍配置：关节ID={wall2_joint_id}，电机ID={wall2_motor_id}，初始相位={wall2_phase:.2f}")
-    print(f"导航目标点：x={TARGET_POS[0]}, y={TARGET_POS[1]} | 到达阈值：{target_reached_threshold}米")
-
-    # -------------------------- 兼容低版本MuJoCo的ID查询 --------------------------
+    # -------------------------- 多动态障碍初始化 --------------------------
+    # 障碍1（wall2）：正弦组合运动（Y+Z轴）
+    wall2_joint_ids = {
+        "y": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, "wall2_slide_y"),
+        "z": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, "wall2_slide_z")
+    }
+    wall2_motor_ids = {
+        "y": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "wall2_motor_y"),
+        "z": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "wall2_motor_z")
+    }
+    wall2_params = {
+        "y_amp": 2.5, "y_freq": 0.7, "y_phase": random.uniform(0, 2 * np.pi),
+        "z_amp": 0.4, "z_freq": 0.3, "z_phase": random.uniform(0, 2 * np.pi)
+    }
+
+    # 障碍2（wall3）：随机游走运动（X+Y轴）
+    wall3_joint_ids = {
+        "x": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, "wall3_slide_x"),
+        "y": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, "wall3_slide_y")
+    }
+    wall3_motor_ids = {
+        "x": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "wall3_motor_x"),
+        "y": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "wall3_motor_y")
+    }
+    wall3_params = {
+        "x_base": 6.0, "x_range": 1.0, "x_speed": 0.4,
+        "y_base": 0.0, "y_range": 2.0, "y_speed": 0.6,
+        "x_dir": random.choice([-1, 1]), "y_dir": random.choice([-1, 1]),
+        "x_switch": random.uniform(2.0, 4.0), "y_switch": random.uniform(1.5, 3.5)
+    }
+    wall3_last_switch = {"x": 0.0, "y": 0.0}
+
+    # 障碍3（wall4）：圆周运动（旋转+径向）
+    wall4_joint_ids = {
+        "rot": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, "wall4_rotate"),
+        "rad": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, "wall4_radial")
+    }
+    wall4_motor_ids = {
+        "rot": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "wall4_motor_rot"),
+        "rad": mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "wall4_motor_rad")
+    }
+    wall4_params = {
+        "rot_speed": 0.5, "rot_dir": random.choice([-1, 1]),
+        "rad_amp": 0.8, "rad_freq": 0.2, "rad_phase": random.uniform(0, 2 * np.pi),
+        "rad_base": 1.2
+    }
+
+    # 打印障碍配置
+    print("=" * 60)
+    print("复杂动态环境配置：")
+    print(f"• 导航目标点：({TARGET_POS[0]}, {TARGET_POS[1]})，到达阈值：{target_reached_threshold}m")
+    print(f"• 动态障碍1（wall2）：Y轴正弦(振幅{wall2_params['y_amp']}, 频率{wall2_params['y_freq']}) + Z轴正弦")
+    print(f"• 动态障碍2（wall3）：X/Y轴随机游走（速度{wall3_params['x_speed']}, {wall3_params['y_speed']}）")
+    print(f"• 动态障碍3（wall4）：圆周运动（转速{wall4_params['rot_speed']}, 径向振幅{wall4_params['rad_amp']}）")
+    print("=" * 60)
+
+    # -------------------------- 障碍检测初始化（修复核心问题） --------------------------
+    # 只检测真正的障碍本体（wall1, wall2, wall3, wall4），排除base/rot_base等基座
+    valid_wall_names = ["wall1", "wall2", "wall3", "wall4"]
     wall_ids = []
     wall_names = []
-    for i in range(model.nbody):
-        body_name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_BODY, i)
-        if body_name and body_name.startswith("wall"):
-            wall_ids.append(i)
-            wall_names.append(body_name)
-    torso_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, "torso")
+    wall_types = {}  # 标记障碍类型：fixed/dynamic1/dynamic2/dynamic3
+
+    for wall_name in valid_wall_names:
+        wall_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, wall_name)
+        if wall_id != -1:
+            wall_ids.append(wall_id)
+            wall_names.append(wall_name)
+            # 分配障碍类型
+            if wall_name == "wall1":
+                wall_types[wall_name] = "fixed"
+            elif wall_name == "wall2":
+                wall_types[wall_name] = "dynamic1"
+            elif wall_name == "wall3":
+                wall_types[wall_name] = "dynamic2"
+            elif wall_name == "wall4":
+                wall_types[wall_name] = "dynamic3"
 
-    # 打印检测到的障碍物
-    if wall_ids:
-        print(f"检测到障碍物：{wall_names}（ID：{wall_ids}）")
-    else:
-        print("警告：未检测到任何障碍物！")
+    torso_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, "torso")
+    print(f"检测到有效障碍：{[(name, wall_types[name]) for name in wall_names]}")
 
     # -------------------------- 核心参数配置 --------------------------
-    # 避障参数（适配导航+动态障碍）
+    # 多障碍避障参数
     avoid_obstacle = False
-    obstacle_distance_threshold = 1.8  # 触发避障的距离阈值
+    obstacle_distance_threshold = 2.0  # 增大避障触发距离
     obstacle_avoidance_time = 0
-    obstacle_avoidance_duration = 4.5  # 避障持续时间
+    obstacle_avoidance_duration = 5.0  # 延长避障时间
     turn_direction = 0
     return_to_path = False
     return_time = 0
-    return_duration = 3.5  # 回归导航路径的持续时间
+    return_duration = 4.0
     stop_walk = False
-    target_reached = False  # 目标点到达标记
+    target_reached = False
     closest_wall_id = -1
-    last_closest_wall_pos = np.zeros(2)  # 记录上一帧最近障碍位置
-
-    # 步态控制参数（适配导航）
-    gait_period = 2.0
-    swing_gain = 0.85
-    stance_gain = 0.8
-    forward_speed = 0.35  # 前进速度
-    heading_kp = 80.0  # 朝向目标点的比例增益
+    closest_wall_type = ""
+    closest_wall_name = ""  # 初始化closest_wall_name
+    dir_name = ""  # 初始化dir_name
+    # 障碍位置缓存（用于运动预判）
+    wall_pos_history = {name: deque(maxlen=10) for name in wall_names}
+
+    # 步态控制参数（降低速度，增强稳定性）
+    gait_period = 2.2
+    swing_gain = 0.8
+    stance_gain = 0.75
+    forward_speed = 0.3
+    heading_kp = 90.0  # 增大朝向增益
 
     # 姿态稳定参数
     torso_pitch_target = 0.0
     torso_roll_target = 0.0
-    balance_kp = 100.0
-    balance_kd = 15.0
+    balance_kp = 110.0
+    balance_kd = 18.0
 
     # 启动可视化器
     mujoco.set_mjcb_control(None)
     with viewer.launch_passive(model, data) as viewer_instance:
-        print("\nDeepMind Humanoid目标导航仿真启动！")
-        print(
-            f"导航逻辑：向目标点({TARGET_POS[0]},{TARGET_POS[1]})移动 → 检测动态障碍 → 避障 → 回归导航路径 → 到达目标点停止")
+        print("\nDeepMind Humanoid复杂动态环境导航仿真启动！")
+        print(f"导航逻辑：向目标点({TARGET_POS[0]},{TARGET_POS[1]})移动 → 多障碍检测 → 优先级避障 → 回归路径 → 到达目标")
         start_time = time.time()
 
         try:
@@ -91,21 +149,61 @@ def control_robot(model_path):
                 if not viewer_instance.is_running():
                     break
 
-                # -------------------------- 1. 驱动动态障碍移动 --------------------------
                 elapsed_time = time.time() - start_time
-                # 正弦曲线控制动态障碍Y轴往复移动
-                wall2_target_pos = wall2_amplitude * np.sin(wall2_speed * elapsed_time + wall2_phase)
-                # 控制动态障碍关节
-                if 0 <= wall2_motor_id < len(data.ctrl):
-                    data.ctrl[wall2_motor_id] = (wall2_target_pos - data.qpos[wall2_joint_id]) * 2.0  # PD控制
+
+                # -------------------------- 1. 多动态障碍运动控制 --------------------------
+                # 障碍1（wall2）：Y+Z轴正弦组合运动
+                if all(id != -1 for id in wall2_motor_ids.values()):
+                    # Y轴：主正弦运动
+                    wall2_y_target = wall2_params["y_amp"] * np.sin(
+                        wall2_params["y_freq"] * elapsed_time + wall2_params["y_phase"])
+                    # Z轴：副正弦运动（垂直方向）
+                    wall2_z_target = wall2_params["z_amp"] * np.sin(
+                        wall2_params["z_freq"] * elapsed_time + wall2_params["z_phase"]) + 0.75
+                    # PD控制
+                    data.ctrl[wall2_motor_ids["y"]] = (wall2_y_target - data.qpos[wall2_joint_ids["y"]]) * 2.5
+                    data.ctrl[wall2_motor_ids["z"]] = (wall2_z_target - data.qpos[wall2_joint_ids["z"]]) * 1.8
+
+                # 障碍2（wall3）：随机游走运动
+                if all(id != -1 for id in wall3_motor_ids.values()):
+                    # 随机切换运动方向
+                    if elapsed_time - wall3_last_switch["x"] > wall3_params["x_switch"]:
+                        wall3_params["x_dir"] *= -1
+                        wall3_params["x_switch"] = random.uniform(2.0, 4.0)
+                        wall3_last_switch["x"] = elapsed_time
+                    if elapsed_time - wall3_last_switch["y"] > wall3_params["y_switch"]:
+                        wall3_params["y_dir"] *= -1
+                        wall3_params["y_switch"] = random.uniform(1.5, 3.5)
+                        wall3_last_switch["y"] = elapsed_time
+
+                    # 计算目标位置
+                    wall3_x_target = wall3_params["x_base"] + wall3_params["x_dir"] * wall3_params["x_speed"] * (
+                                elapsed_time % 5)
+                    wall3_y_target = wall3_params["y_base"] + wall3_params["y_dir"] * wall3_params["y_speed"] * (
+                                elapsed_time % 4)
+                    # 限制范围
+                    wall3_x_target = np.clip(wall3_x_target, 5.0, 7.0)
+                    wall3_y_target = np.clip(wall3_y_target, -2.5, 2.5)
+                    # PD控制
+                    data.ctrl[wall3_motor_ids["x"]] = (wall3_x_target - data.qpos[wall3_joint_ids["x"]]) * 2.2
+                    data.ctrl[wall3_motor_ids["y"]] = (wall3_y_target - data.qpos[wall3_joint_ids["y"]]) * 2.0
+
+                # 障碍3（wall4）：圆周运动
+                if all(id != -1 for id in wall4_motor_ids.values()):
+                    # 旋转运动
+                    wall4_rot_target = wall4_params["rot_dir"] * wall4_params["rot_speed"] * elapsed_time
+                    # 径向运动（正弦变化半径）
+                    wall4_rad_target = wall4_params["rad_base"] + wall4_params["rad_amp"] * np.sin(
+                        wall4_params["rad_freq"] * elapsed_time + wall4_params["rad_phase"])
+                    # PD控制
+                    data.ctrl[wall4_motor_ids["rot"]] = (wall4_rot_target - data.qpos[wall4_joint_ids["rot"]]) * 1.5
+                    data.ctrl[wall4_motor_ids["rad"]] = (wall4_rad_target - data.qpos[wall4_joint_ids["rad"]]) * 2.0
 
                 # -------------------------- 2. 导航状态计算 --------------------------
+                yaw_error = 0.0  # 初始化
                 if torso_id != -1 and not target_reached:
-                    # 获取机器人躯干位置和朝向
                     torso_pos = data.xpos[torso_id]
                     robot_xy = torso_pos[:2]
-
-                    # 计算到目标点的距离和方向
                     target_vector = TARGET_POS - robot_xy
                     distance_to_target = np.linalg.norm(target_vector)
 
@@ -113,169 +211,205 @@ def control_robot(model_path):
                     if distance_to_target < target_reached_threshold:
                         target_reached = True
                         stop_walk = True
-                        print(
-                            f"\n\n✅ 到达目标点！最终位置：x={torso_pos[0]:.2f}, y={torso_pos[1]:.2f} | 目标点：x={TARGET_POS[0]}, y={TARGET_POS[1]}")
+                        print(f"\n\n✅ 到达目标点！")
+                        print(f"最终位置：x={torso_pos[0]:.2f}, y={torso_pos[1]:.2f}")
+                        print(f"目标点：x={TARGET_POS[0]}, y={TARGET_POS[1]} | 剩余距离：{distance_to_target:.2f}m")
                         continue
 
-                    # 计算机器人朝向（偏航角）
+                    # 计算机器人朝向和目标方向
                     torso_quat = data.xquat[torso_id]
                     robot_yaw = np.arctan2(2 * (torso_quat[2] * torso_quat[3] - torso_quat[0] * torso_quat[1]),
                                            torso_quat[0] ** 2 - torso_quat[1] ** 2 - torso_quat[2] ** 2 + torso_quat[
                                                3] ** 2)
-                    # 计算目标方向的角度
                     target_yaw = np.arctan2(target_vector[1], target_vector[0])
-                    # 计算朝向误差（归一化到[-π, π]）
                     yaw_error = target_yaw - robot_yaw
                     yaw_error = np.arctan2(np.sin(yaw_error), np.cos(yaw_error))
 
-                # -------------------------- 3. 实时检测动态障碍 --------------------------
+                # -------------------------- 3. 多障碍检测与优先级排序 --------------------------
                 distance_to_closest_wall = float('inf')
-                closest_wall_name = ""
                 closest_wall_pos = np.zeros(2)
+                # 障碍优先级：dynamic1（wall2）> dynamic2（wall3）> dynamic3（wall4）> fixed（wall1）
+                wall_priority = {"dynamic1": 4, "dynamic2": 3, "dynamic3": 2, "fixed": 1}
 
                 if wall_ids and torso_id != -1 and not stop_walk and not target_reached:
                     torso_pos = data.xpos[torso_id]
+                    wall_distances = []
+
                     for idx, wall_id in enumerate(wall_ids):
+                        wall_name = wall_names[idx]
+                        wall_type = wall_types.get(wall_name, "fixed")
+
+                        # 获取当前位置并缓存
                         wall_pos = data.xpos[wall_id]
-                        distance = np.linalg.norm(torso_pos[:2] - wall_pos[:2])
-                        # 动态障碍预判：如果是wall2，额外计算未来0.5秒的位置
-                        if wall_names[idx] == "wall2":
-                            # 预估0.5秒后障碍位置
-                            future_wall2_pos = wall2_amplitude * np.sin(
-                                wall2_speed * (elapsed_time + 0.5) + wall2_phase)
-                            future_distance = np.linalg.norm(np.array([torso_pos[0], torso_pos[1]]) -
-                                                             np.array([wall_pos[0], future_wall2_pos]))
-                            distance = min(distance, future_distance)  # 取当前/未来距离最小值
-
-                        if distance < distance_to_closest_wall:
-                            distance_to_closest_wall = distance
-                            closest_wall_id = wall_id
-                            closest_wall_name = wall_names[idx]
-                            closest_wall_pos = wall_pos[:2]
-
-                # -------------------------- 4. 导航+避障状态切换 --------------------------
+                        wall_pos_history[wall_name].append(wall_pos[:2])
+                        # 计算距离
+                        current_distance = np.linalg.norm(torso_pos[:2] - wall_pos[:2])
+
+                        # 动态障碍位置预判（基于历史数据）
+                        predicted_distance = current_distance
+                        if len(wall_pos_history[wall_name]) > 5 and wall_type != "fixed":
+                            # 计算运动趋势
+                            pos_history = np.array(wall_pos_history[wall_name])
+                            velocity = (pos_history[-1] - pos_history[0]) / len(pos_history) * model.opt.timestep * 10
+                            # 预测0.8秒后的位置
+                            future_pos = wall_pos[:2] + velocity * 0.8
+                            predicted_distance = np.linalg.norm(torso_pos[:2] - future_pos)
+
+                        # 加入优先级权重
+                        priority_weight = wall_priority[wall_type]
+                        weighted_distance = predicted_distance / priority_weight
+
+                        wall_distances.append({
+                            "name": wall_name,
+                            "type": wall_type,
+                            "id": wall_id,
+                            "pos": wall_pos[:2],
+                            "current_dist": current_distance,
+                            "predicted_dist": predicted_distance,
+                            "weighted_dist": weighted_distance
+                        })
+
+                    # 按加权距离排序（最近的优先）
+                    wall_distances.sort(key=lambda x: x["weighted_dist"])
+
+                    if wall_distances:
+                        closest_wall = wall_distances[0]
+                        closest_wall_id = closest_wall["id"]
+                        closest_wall_name = closest_wall["name"]
+                        closest_wall_type = closest_wall["type"]
+                        closest_wall_pos = closest_wall["pos"]
+                        distance_to_closest_wall = closest_wall["predicted_dist"]
+
+                # -------------------------- 4. 多障碍避障状态切换 --------------------------
                 if closest_wall_id != -1 and torso_id != -1 and not stop_walk and not target_reached:
                     # 触发避障
                     if (distance_to_closest_wall < obstacle_distance_threshold and
                             not avoid_obstacle and not return_to_path):
                         avoid_obstacle = True
                         obstacle_avoidance_time = time.time()
-                        # 基于目标方向和障碍位置选择转向方向（更智能的避障）
+                        # 智能选择转向方向：远离障碍+朝向目标
                         torso_pos = data.xpos[torso_id]
-                        wall_relative_y = closest_wall_pos[1] - torso_pos[1]
-                        turn_direction = -1 if wall_relative_y > 0 else 1  # 障碍在右侧则左转，左侧则右转
+                        # 计算障碍相对于机器人的位置
+                        wall_relative = closest_wall_pos - torso_pos[:2]
+                        # 计算目标相对于机器人的位置
+                        target_relative = TARGET_POS - torso_pos[:2]
+                        # 叉乘判断转向方向（优先绕开障碍且朝向目标）
+                        cross_product = np.cross(np.append(wall_relative, 0), np.append(target_relative, 0))[2]
+                        turn_direction = -1 if cross_product > 0 else 1
                         dir_name = "左转" if turn_direction == -1 else "右转"
-                        print(
-                            f"\n⚠️  检测到最近障碍【{closest_wall_name}】！距离：{distance_to_closest_wall:.2f}米，开始{dir_name}避障...")
+
+                        print(f"\n⚠️  检测到高优先级障碍【{closest_wall_name}({closest_wall_type})】！")
+                        print(f"   预测距离：{distance_to_closest_wall:.2f}m | 转向方向：{dir_name}")
 
                     # 避障完成，回归导航路径
                     if avoid_obstacle and (time.time() - obstacle_avoidance_time) > obstacle_avoidance_duration:
                         avoid_obstacle = False
                         return_to_path = True
                         return_time = time.time()
-                        print(f"✅ 避障完成，开始回归导航路径...")
+                        print(f"✅ 避障完成，回归导航路径...")
 
                     # 回归完成，继续向目标点移动
                     if return_to_path and (time.time() - return_time) > return_duration:
                         return_to_path = False
-                        print(f"✅ 已回归导航路径，继续向目标点移动...")
+                        print(f"✅ 回归路径完成，继续向目标点移动...")
 
                 # -------------------------- 5. 步态周期计算 --------------------------
                 cycle = elapsed_time % gait_period
                 phase = cycle / gait_period
 
                 # -------------------------- 6. 关节控制核心逻辑 --------------------------
-                data.ctrl[:len(data.ctrl) - 1] = 0.0  # 重置机器人控制指令（保留动态障碍电机）
+                data.ctrl[:model.nu - 6] = 0.0  # 重置机器人控制指令（保留障碍电机）
 
                 if stop_walk or target_reached:
-                    # 停止状态：所有关节归零，保持站立
                     continue
 
                 elif return_to_path:
-                    # 回归导航路径模式：转向回到目标方向
+                    # 回归导航路径：精准朝向目标点
                     return_phase = (time.time() - return_time) / return_duration
-                    return_speed = 1.3 * np.cos(return_phase * np.pi)
+                    return_speed = 1.5 * np.cos(return_phase * np.pi)
 
                     # 朝向目标点回正
-                    torso_quat = data.xquat[torso_id]
-                    robot_yaw = np.arctan2(2 * (torso_quat[2] * torso_quat[3] - torso_quat[0] * torso_quat[1]),
-                                           torso_quat[0] ** 2 - torso_quat[1] ** 2 - torso_quat[2] ** 2 + torso_quat[
-                                               3] ** 2)
-                    target_vector = TARGET_POS - data.xpos[torso_id][:2]
-                    target_yaw = np.arctan2(target_vector[1], target_vector[0])
-                    yaw_error = target_yaw - robot_yaw
-                    yaw_error = np.arctan2(np.sin(yaw_error), np.cos(yaw_error))
+                    if torso_id != -1:
+                        torso_quat = data.xquat[torso_id]
+                        robot_yaw = np.arctan2(2 * (torso_quat[2] * torso_quat[3] - torso_quat[0] * torso_quat[1]),
+                                               torso_quat[0] ** 2 - torso_quat[1] ** 2 - torso_quat[2] ** 2 +
+                                               torso_quat[3] ** 2)
+                        target_vector = TARGET_POS - data.xpos[torso_id][:2]
+                        target_yaw = np.arctan2(target_vector[1], target_vector[0])
+                        yaw_error = target_yaw - robot_yaw
+                        yaw_error = np.arctan2(np.sin(yaw_error), np.cos(yaw_error))
 
-                    # 转向回正控制
+                    # 转向控制
                     abdomen_z_act_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "abdomen_z")
                     hip_z_right_act_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "hip_z_right")
                     hip_z_left_act_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "hip_z_left")
 
-                    if 0 <= abdomen_z_act_id < len(data.ctrl) - 1:
+                    if 0 <= abdomen_z_act_id < model.nu - 6:
                         data.ctrl[abdomen_z_act_id] = heading_kp * yaw_error * return_speed
-                    if 0 <= hip_z_right_act_id < len(data.ctrl) - 1:
-                        data.ctrl[hip_z_right_act_id] = -yaw_error * return_speed * 0.7
-                    if 0 <= hip_z_left_act_id < len(data.ctrl) - 1:
-                        data.ctrl[hip_z_left_act_id] = yaw_error * return_speed * 0.7
+                    if 0 <= hip_z_right_act_id < model.nu - 6:
+                        data.ctrl[hip_z_right_act_id] = -yaw_error * return_speed * 0.8
+                    if 0 <= hip_z_left_act_id < model.nu - 6:
+                        data.ctrl[hip_z_left_act_id] = yaw_error * return_speed * 0.8
 
-                    # 保持基本站立姿态
+                    # 稳定站立
                     for side in ["right", "left"]:
                         hip_y_act_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, f"hip_y_{side}")
                         knee_act_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, f"knee_{side}")
                         ankle_y_act_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, f"ankle_y_{side}")
-                        if 0 <= hip_y_act_id < len(data.ctrl) - 1:
-                            data.ctrl[hip_y_act_id] = -0.7
-                        if 0 <= knee_act_id < len(data.ctrl) - 1:
-                            data.ctrl[knee_act_id] = 1.0
-                        if 0 <= ankle_y_act_id < len(data.ctrl) - 1:
-                            data.ctrl[ankle_y_act_id] = 0.3
+                        if 0 <= hip_y_act_id < model.nu - 6:
+                            data.ctrl[hip_y_act_id] = -0.8
+                        if 0 <= knee_act_id < model.nu - 6:
+                            data.ctrl[knee_act_id] = 1.1
+                        if 0 <= ankle_y_act_id < model.nu - 6:
+                            data.ctrl[ankle_y_act_id] = 0.4
 
                 elif avoid_obstacle:
-                    # 避障模式：转向绕开最近障碍
+                    # 多障碍避障模式：增强转向稳定性
                     avoid_phase = (time.time() - obstacle_avoidance_time) / obstacle_avoidance_duration
-                    turn_speed = 1.4 * np.sin(avoid_phase * np.pi)  # 转向力度
+                    turn_speed = 1.6 * np.sin(avoid_phase * np.pi)
 
                     # 转向控制
                     hip_z_right_act_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "hip_z_right")
                     hip_z_left_act_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "hip_z_left")
                     abdomen_z_act_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "abdomen_z")
 
-                    if 0 <= hip_z_right_act_id < len(data.ctrl) - 1:
-                        data.ctrl[hip_z_right_act_id] = turn_direction * turn_speed * 1.0
-                    if 0 <= hip_z_left_act_id < len(data.ctrl) - 1:
-                        data.ctrl[hip_z_left_act_id] = -turn_direction * turn_speed * 1.0
-                    if 0 <= abdomen_z_act_id < len(data.ctrl) - 1:
-                        data.ctrl[abdomen_z_act_id] = turn_direction * turn_speed * 1.8
+                    if 0 <= hip_z_right_act_id < model.nu - 6:
+                        data.ctrl[hip_z_right_act_id] = turn_direction * turn_speed * 1.2
+                    if 0 <= hip_z_left_act_id < model.nu - 6:
+                        data.ctrl[hip_z_left_act_id] = -turn_direction * turn_speed * 1.2
+                    if 0 <= abdomen_z_act_id < model.nu - 6:
+                        data.ctrl[abdomen_z_act_id] = turn_direction * turn_speed * 2.0
 
-                    # 保持平衡
+                    # 增强平衡控制
                     for side in ["right", "left"]:
                         hip_y_act_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, f"hip_y_{side}")
                         knee_act_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, f"knee_{side}")
                         ankle_x_act_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, f"ankle_x_{side}")
-                        if 0 <= hip_y_act_id < len(data.ctrl) - 1:
-                            data.ctrl[hip_y_act_id] = -1.0
-                        if 0 <= knee_act_id < len(data.ctrl) - 1:
-                            data.ctrl[knee_act_id] = 1.2
-                        if 0 <= ankle_x_act_id < len(data.ctrl) - 1:
-                            data.ctrl[ankle_x_act_id] = 0.2
+                        if 0 <= hip_y_act_id < model.nu - 6:
+                            data.ctrl[hip_y_act_id] = -1.1
+                        if 0 <= knee_act_id < model.nu - 6:
+                            data.ctrl[knee_act_id] = 1.3
+                        if 0 <= ankle_x_act_id < model.nu - 6:
+                            data.ctrl[ankle_x_act_id] = 0.3
 
                 else:
                     # 正常导航模式：向目标点移动
-                    torso_quat = data.xquat[torso_id]
-                    robot_yaw = np.arctan2(2 * (torso_quat[2] * torso_quat[3] - torso_quat[0] * torso_quat[1]),
-                                           torso_quat[0] ** 2 - torso_quat[1] ** 2 - torso_quat[2] ** 2 + torso_quat[
-                                               3] ** 2)
-                    target_vector = TARGET_POS - data.xpos[torso_id][:2]
-                    target_yaw = np.arctan2(target_vector[1], target_vector[0])
-                    yaw_error = target_yaw - robot_yaw
-                    yaw_error = np.arctan2(np.sin(yaw_error), np.cos(yaw_error))
-
-                    # 朝向目标点的转向控制
-                    abdomen_z_act_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "abdomen_z")
-                    if 0 <= abdomen_z_act_id < len(data.ctrl) - 1:
-                        data.ctrl[abdomen_z_act_id] = heading_kp * yaw_error * 0.1
-
-                    # 腿部步态控制（向目标点前进）
+                    if torso_id != -1:
+                        torso_quat = data.xquat[torso_id]
+                        robot_yaw = np.arctan2(2 * (torso_quat[2] * torso_quat[3] - torso_quat[0] * torso_quat[1]),
+                                               torso_quat[0] ** 2 - torso_quat[1] ** 2 - torso_quat[2] ** 2 +
+                                               torso_quat[3] ** 2)
+                        target_vector = TARGET_POS - data.xpos[torso_id][:2]
+                        target_yaw = np.arctan2(target_vector[1], target_vector[0])
+                        yaw_error = target_yaw - robot_yaw
+                        yaw_error = np.arctan2(np.sin(yaw_error), np.cos(yaw_error))
+
+                        # 朝向目标点的转向控制
+                        abdomen_z_act_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "abdomen_z")
+                        if 0 <= abdomen_z_act_id < model.nu - 6:
+                            data.ctrl[abdomen_z_act_id] = heading_kp * yaw_error * 0.12
+
+                    # 腿部步态控制
                     for side, sign in [("right", 1), ("left", -1)]:
                         swing_phase = (phase + 0.5 * sign) % 1.0
 
@@ -287,36 +421,36 @@ def control_robot(model_path):
                         ankle_y_act_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, f"ankle_y_{side}")
                         ankle_x_act_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, f"ankle_x_{side}")
 
-                        # 腿部关节控制（适配导航）
-                        if 0 <= hip_x_act_id < len(data.ctrl) - 1:
+                        # 腿部关节控制
+                        if 0 <= hip_x_act_id < model.nu - 6:
                             data.ctrl[hip_x_act_id] = swing_gain * np.sin(2 * np.pi * swing_phase) * forward_speed
-                        if 0 <= hip_z_act_id < len(data.ctrl) - 1:
+                        if 0 <= hip_z_act_id < model.nu - 6:
                             data.ctrl[hip_z_act_id] = stance_gain * np.cos(
-                                2 * np.pi * swing_phase) * 0.2 + yaw_error * 0.1
-                        if 0 <= hip_y_act_id < len(data.ctrl) - 1:
-                            data.ctrl[hip_y_act_id] = -0.9 * np.sin(2 * np.pi * swing_phase) - 0.4
-                        if 0 <= knee_act_id < len(data.ctrl) - 1:
-                            data.ctrl[knee_act_id] = 1.2 * np.sin(2 * np.pi * swing_phase) + 0.7
-                        if 0 <= ankle_y_act_id < len(data.ctrl) - 1:
-                            data.ctrl[ankle_y_act_id] = 0.3 * np.cos(2 * np.pi * swing_phase)
-                        if 0 <= ankle_x_act_id < len(data.ctrl) - 1:
-                            data.ctrl[ankle_x_act_id] = 0.15 * np.sin(2 * np.pi * swing_phase)
+                                2 * np.pi * swing_phase) * 0.2 + yaw_error * 0.12
+                        if 0 <= hip_y_act_id < model.nu - 6:
+                            data.ctrl[hip_y_act_id] = -0.95 * np.sin(2 * np.pi * swing_phase) - 0.45
+                        if 0 <= knee_act_id < model.nu - 6:
+                            data.ctrl[knee_act_id] = 1.25 * np.sin(2 * np.pi * swing_phase) + 0.75
+                        if 0 <= ankle_y_act_id < model.nu - 6:
+                            data.ctrl[ankle_y_act_id] = 0.35 * np.cos(2 * np.pi * swing_phase)
+                        if 0 <= ankle_x_act_id < model.nu - 6:
+                            data.ctrl[ankle_x_act_id] = 0.18 * np.sin(2 * np.pi * swing_phase)
 
                     # 躯干稳定控制
                     abdomen_x_act_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "abdomen_z")
                     abdomen_y_act_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "abdomen_y")
                     abdomen_z_act_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "abdomen_x")
 
-                    pitch = 2 * (torso_quat[1] * torso_quat[3] - torso_quat[0] * torso_quat[2])
-                    roll = 2 * (torso_quat[0] * torso_quat[1] + torso_quat[2] * torso_quat[3])
+                    if torso_id != -1:
+                        pitch = 2 * (torso_quat[1] * torso_quat[3] - torso_quat[0] * torso_quat[2])
+                        roll = 2 * (torso_quat[0] * torso_quat[1] + torso_quat[2] * torso_quat[3])
 
-                    # 平衡补偿
-                    if 0 <= abdomen_x_act_id < len(data.ctrl) - 1:
-                        data.ctrl[abdomen_x_act_id] = balance_kp * (torso_roll_target - roll) - balance_kd * \
-                                                      data.qvel[abdomen_x_act_id]
-                    if 0 <= abdomen_y_act_id < len(data.ctrl) - 1:
-                        data.ctrl[abdomen_y_act_id] = balance_kp * (torso_pitch_target - pitch) - balance_kd * \
-                                                      data.qvel[abdomen_y_act_id]
+                        if 0 <= abdomen_x_act_id < model.nu - 6:
+                            data.ctrl[abdomen_x_act_id] = balance_kp * (torso_roll_target - roll) - balance_kd * \
+                                                          data.qvel[abdomen_x_act_id]
+                        if 0 <= abdomen_y_act_id < model.nu - 6:
+                            data.ctrl[abdomen_y_act_id] = balance_kp * (torso_pitch_target - pitch) - balance_kd * \
+                                                          data.qvel[abdomen_y_act_id]
 
                     # 手臂自然摆动
                     for side, sign in [("right", 1), ("left", -1)]:
@@ -324,38 +458,36 @@ def control_robot(model_path):
                         shoulder2_act_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, f"shoulder2_{side}")
                         elbow_act_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, f"elbow_{side}")
 
-                        shoulder1_cmd = 0.2 * np.sin(2 * np.pi * (phase + 0.5 * sign))
-                        shoulder2_cmd = 0.15 * np.cos(2 * np.pi * (phase + 0.5 * sign))
-                        elbow_cmd = -0.4 * np.sin(2 * np.pi * (phase + 0.5 * sign)) - 0.25
+                        shoulder1_cmd = 0.22 * np.sin(2 * np.pi * (phase + 0.5 * sign))
+                        shoulder2_cmd = 0.17 * np.cos(2 * np.pi * (phase + 0.5 * sign))
+                        elbow_cmd = -0.42 * np.sin(2 * np.pi * (phase + 0.5 * sign)) - 0.27
 
-                        if 0 <= shoulder1_act_id < len(data.ctrl) - 1:
+                        if 0 <= shoulder1_act_id < model.nu - 6:
                             data.ctrl[shoulder1_act_id] = shoulder1_cmd
-                        if 0 <= shoulder2_act_id < len(data.ctrl) - 1:
+                        if 0 <= shoulder2_act_id < model.nu - 6:
                             data.ctrl[shoulder2_act_id] = shoulder2_cmd
-                        if 0 <= elbow_act_id < len(data.ctrl) - 1:
+                        if 0 <= elbow_act_id < model.nu - 6:
                             data.ctrl[elbow_act_id] = elbow_cmd
 
                 # -------------------------- 7. 仿真推进 --------------------------
                 mujoco.mj_step(model, data)
                 viewer_instance.sync()
 
-                # 实时状态输出（含导航信息）
-                if torso_id != -1 and int(elapsed_time * 2) % 2 == 0:
+                # -------------------------- 8. 实时状态输出 --------------------------
+                if torso_id != -1 and int(elapsed_time * 1) % 1 == 0:
                     if target_reached:
                         status = "✅ 到达目标点"
-                        nav_info = "—"
-                        dist_info = "—"
-                        wall2_pos_info = "—"
+                        nav_info = f"剩余距离：0.00m"
+                        obstacle_info = "—"
                     elif stop_walk:
                         status = "已停止"
                         nav_info = "—"
-                        dist_info = "—"
-                        wall2_pos_info = "—"
+                        obstacle_info = "—"
                     else:
                         if return_to_path:
                             status = "回归导航路径中"
                         elif avoid_obstacle:
-                            status = f"避障中（{dir_name}）"
+                            status = f"避障中（{dir_name} | {closest_wall_name}）"
                         else:
                             status = "向目标点移动中"
 
@@ -364,18 +496,14 @@ def control_robot(model_path):
                         nav_info = f"剩余{distance_to_target:.2f}m | 朝向误差{np.degrees(yaw_error):.1f}°"
 
                         # 障碍信息
-                        dist_info = f"{distance_to_closest_wall:.2f}m（{closest_wall_name}）"
-
-                        # 动态障碍位置信息
-                        if wall2_body_id != -1:
-                            wall2_current_pos = data.xpos[wall2_body_id]
-                            wall2_pos_info = f"wall2(Y):{wall2_current_pos[1]:.2f}m"
+                        if closest_wall_name:
+                            obstacle_info = f"{closest_wall_name}({closest_wall_type})：{distance_to_closest_wall:.2f}m"
                         else:
-                            wall2_pos_info = "wall2:未知"
+                            obstacle_info = "无"
 
                     torso_pos = data.xpos[torso_id]
                     print(
-                        f"\r时间：{elapsed_time:.1f}s | 位置：x={torso_pos[0]:.2f}, y={torso_pos[1]:.2f} | 导航：{nav_info} | 障碍：{dist_info} | 动态障碍：{wall2_pos_info} | 状态：{status}",
+                        f"\r时间：{elapsed_time:.1f}s | 位置：x={torso_pos[0]:.2f}, y={torso_pos[1]:.2f} | 导航：{nav_info} | 障碍：{obstacle_info} | 状态：{status}",
                         end="")
 
                 time.sleep(model.opt.timestep * 2)
