From e1ea0231cdaf844ccd7ae93f065ecdc06c0d14e9 Mon Sep 17 00:00:00 2001
From: XYTX0427 <481177497@qq.com>
Date: Fri, 19 Dec 2025 18:41:14 +0800
Subject: [PATCH 01/21] =?UTF-8?q?=E6=8F=90=E4=BA=A4=E9=80=89=E9=A2=98?=
 =?UTF-8?q?=E5=92=8C=E8=AF=95=E9=AA=8C=E4=BB=A3=E7=A0=81?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/Unmanned_aerial_vehicle/README.md |   1 +
 src/Unmanned_aerial_vehicle/main.py   | 102 ++++++++++++++++++++++++++
 2 files changed, 103 insertions(+)
 create mode 100644 src/Unmanned_aerial_vehicle/README.md
 create mode 100644 src/Unmanned_aerial_vehicle/main.py

diff --git a/src/Unmanned_aerial_vehicle/README.md b/src/Unmanned_aerial_vehicle/README.md
new file mode 100644
index 0000000000..b00460f809
--- /dev/null
+++ b/src/Unmanned_aerial_vehicle/README.md
@@ -0,0 +1 @@
+无人机导航
\ No newline at end of file
diff --git a/src/Unmanned_aerial_vehicle/main.py b/src/Unmanned_aerial_vehicle/main.py
new file mode 100644
index 0000000000..04a5ea1fdf
--- /dev/null
+++ b/src/Unmanned_aerial_vehicle/main.py
@@ -0,0 +1,102 @@
+import math
+
+# 定义无人机类，包含核心导航属性和方法
+class UAVAutonomousNavigation:
+    def __init__(self):
+        # 无人机当前位置（经纬度，单位：度）
+        self.current_lon = 0.0
+        self.current_lat = 0.0
+        # 预设航点列表（格式：[纬度, 经度]）
+        self.waypoints = []
+        # 当前目标航点索引
+        self.target_waypoint_idx = 0
+        # 到达航点的判定阈值（米）
+        self.arrival_threshold = 5.0
+
+    def set_waypoints(self, waypoints_list):
+        """设置航点列表"""
+        self.waypoints = waypoints_list
+        self.target_waypoint_idx = 0  # 重置目标航点索引
+
+    def calculate_distance(self, lat1, lon1, lat2, lon2):
+        """
+        计算两个经纬度点之间的地面距离（米）
+        采用简化的Haversine公式，适用于近距离无人机导航
+        """
+        # 地球半径（米）
+        R = 6371000.0
+        # 角度转弧度
+        lat1_rad = math.radians(lat1)
+        lon1_rad = math.radians(lon1)
+        lat2_rad = math.radians(lat2)
+        lon2_rad = math.radians(lon2)
+
+        # 经纬度差值
+        dlat = lat2_rad - lat1_rad
+        dlon = lon2_rad - lon1_rad
+
+        # Haversine公式
+        a = math.sin(dlat / 2)**2 + math.cos(lat1_rad) * math.cos(lat2_rad) * math.sin(dlon / 2)**2
+        c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))
+        distance = R * c
+        return distance
+
+    def update_position(self, new_lat, new_lon):
+        """更新无人机当前位置"""
+        self.current_lat = new_lat
+        self.current_lon = new_lon
+
+    def navigate_to_waypoints(self):
+        """核心航点导航逻辑：依次飞向每个航点"""
+        if not self.waypoints:
+            print("未设置航点，导航终止")
+            return
+
+        # 获取当前目标航点
+        target_lat, target_lon = self.waypoints[self.target_waypoint_idx]
+        # 计算当前位置到目标航点的距离
+        distance_to_target = self.calculate_distance(
+            self.current_lat, self.current_lon, target_lat, target_lon
+        )
+
+        print(f"当前位置：({self.current_lat:.6f}, {self.current_lon:.6f})")
+        print(f"目标航点{self.target_waypoint_idx+1}：({target_lat:.6f}, {target_lon:.6f})")
+        print(f"到目标航点距离：{distance_to_target:.2f} 米")
+
+        # 判断是否到达当前航点
+        if distance_to_target < self.arrival_threshold:
+            print(f"已到达航点{self.target_waypoint_idx+1}")
+            self.target_waypoint_idx += 1
+            # 判断是否完成所有航点
+            if self.target_waypoint_idx >= len(self.waypoints):
+                print("所有航点导航完成！")
+                return
+            else:
+                print(f"切换至下一个航点：{self.target_waypoint_idx+1}")
+        else:
+            # 生成飞行指令（实际场景中需替换为无人机SDK的飞行控制指令）
+            print("飞行指令：向目标航点飞行（航向/速度需根据实际SDK调整）")
+
+# ------------------- 测试代码 -------------------
+if __name__ == "__main__":
+    # 初始化无人机导航实例
+    uav = UAVAutonomousNavigation()
+
+    # 设置航点列表（示例：3个航点）
+    waypoints = [
+        (39.908823, 116.397470),  # 航点1：天安门
+        (39.997563, 116.337624),  # 航点2：颐和园
+        (40.006401, 116.397029)   # 航点3：圆明园
+    ]
+    uav.set_waypoints(waypoints)
+
+    # 模拟无人机位置更新与导航过程
+    # 模拟初始位置（靠近航点1）
+    uav.update_position(39.908820, 116.397468)
+    uav.navigate_to_waypoints()
+
+    print("-" * 50)
+
+    # 模拟无人机飞到航点1，更新位置
+    uav.update_position(39.908823, 116.397470)
+    uav.navigate_to_waypoints()
\ No newline at end of file

From ab1c3922464162dfa8852344f928dcd1edeb908b Mon Sep 17 00:00:00 2001
From: XYTX0427 <481177497@qq.com>
Date: Fri, 19 Dec 2025 20:59:18 +0800
Subject: [PATCH 02/21] =?UTF-8?q?=E6=8F=90=E4=BA=A4=E9=80=89=E9=A2=98?=
 =?UTF-8?q?=E5=92=8C=E8=AF=95=E9=AA=8C=E4=BB=A3=E7=A0=81?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/Unmanned_aerial_vehicle/main.py | 295 ++++++++++++++++++++--------
 1 file changed, 218 insertions(+), 77 deletions(-)

diff --git a/src/Unmanned_aerial_vehicle/main.py b/src/Unmanned_aerial_vehicle/main.py
index 04a5ea1fdf..3529838050 100644
--- a/src/Unmanned_aerial_vehicle/main.py
+++ b/src/Unmanned_aerial_vehicle/main.py
@@ -1,102 +1,243 @@
 import math
+import logging
+from typing import List, Tuple
+
+# 配置日志，方便追踪运行状态
+logging.basicConfig(
+    level=logging.INFO,
+    format="%(asctime)s - %(levelname)s - %(message)s",
+    datefmt="%Y-%m-%d %H:%M:%S"
+)
+logger = logging.getLogger("UAV_Autonomous_Navigation")
+
 
-# 定义无人机类，包含核心导航属性和方法
 class UAVAutonomousNavigation:
+    """无人机自主导航核心类：整合航点导航、避障、路径重规划"""
+
     def __init__(self):
-        # 无人机当前位置（经纬度，单位：度）
-        self.current_lon = 0.0
-        self.current_lat = 0.0
-        # 预设航点列表（格式：[纬度, 经度]）
-        self.waypoints = []
-        # 当前目标航点索引
-        self.target_waypoint_idx = 0
-        # 到达航点的判定阈值（米）
-        self.arrival_threshold = 5.0
-
-    def set_waypoints(self, waypoints_list):
-        """设置航点列表"""
-        self.waypoints = waypoints_list
-        self.target_waypoint_idx = 0  # 重置目标航点索引
-
-    def calculate_distance(self, lat1, lon1, lat2, lon2):
+        # 无人机核心状态
+        self.current_position: Tuple[float, float] = (0.0, 0.0)  # (纬度, 经度)
+        self.current_heading: float = 0.0  # 当前航向（度，0=正北，90=正东）
+        self.safety_distance: float = 10.0  # 避障安全距离（米）
+        self.arrival_threshold: float = 5.0  # 到达航点判定阈值（米）
+
+        # 导航相关参数
+        self.waypoints: List[Tuple[float, float]] = []  # 航点列表
+        self.target_waypoint_idx: int = 0  # 当前目标航点索引
+        self.is_obstacle_detected: bool = False  # 是否检测到障碍物
+        self.obstacle_distance: float = float("inf")  # 障碍物距离（初始为无穷大）
+
+    def set_waypoints(self, waypoints: List[Tuple[float, float]]) -> None:
         """
-        计算两个经纬度点之间的地面距离（米）
-        采用简化的Haversine公式，适用于近距离无人机导航
+        设置航点列表，增加输入验证
+        :param waypoints: 航点列表，每个元素为(纬度, 经度)元组
         """
-        # 地球半径（米）
-        R = 6371000.0
-        # 角度转弧度
-        lat1_rad = math.radians(lat1)
-        lon1_rad = math.radians(lon1)
-        lat2_rad = math.radians(lat2)
-        lon2_rad = math.radians(lon2)
-
-        # 经纬度差值
-        dlat = lat2_rad - lat1_rad
-        dlon = lon2_rad - lon1_rad
-
-        # Haversine公式
-        a = math.sin(dlat / 2)**2 + math.cos(lat1_rad) * math.cos(lat2_rad) * math.sin(dlon / 2)**2
-        c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))
-        distance = R * c
-        return distance
-
-    def update_position(self, new_lat, new_lon):
-        """更新无人机当前位置"""
-        self.current_lat = new_lat
-        self.current_lon = new_lon
-
-    def navigate_to_waypoints(self):
-        """核心航点导航逻辑：依次飞向每个航点"""
-        if not self.waypoints:
-            print("未设置航点，导航终止")
+        try:
+            # 验证航点格式
+            if not isinstance(waypoints, list):
+                raise TypeError("航点必须为列表格式")
+            for idx, wp in enumerate(waypoints):
+                if not isinstance(wp, tuple) or len(wp) != 2:
+                    raise ValueError(f"第{idx + 1}个航点格式错误，需为(纬度, 经度)元组")
+                if not (isinstance(wp[0], (int, float)) and isinstance(wp[1], (int, float))):
+                    raise ValueError(f"第{idx + 1}个航点经纬度必须为数字")
+
+            self.waypoints = waypoints
+            self.target_waypoint_idx = 0
+            logger.info(f"成功设置{len(waypoints)}个航点，初始目标航点：1")
+
+        except (TypeError, ValueError) as e:
+            logger.error(f"设置航点失败：{str(e)}")
+            raise
+
+    def calculate_haversine_distance(self, lat1: float, lon1: float, lat2: float, lon2: float) -> float:
+        """
+        高精度计算两个经纬度点的地面距离（米）
+        采用Haversine公式，适配无人机低空导航场景
+        """
+        try:
+            # 地球平均半径（米）
+            EARTH_RADIUS = 6371008.8
+            # 角度转弧度
+            lat1_rad = math.radians(lat1)
+            lon1_rad = math.radians(lon1)
+            lat2_rad = math.radians(lat2)
+            lon2_rad = math.radians(lon2)
+
+            # 经纬度差值
+            dlat = lat2_rad - lat1_rad
+            dlon = lon2_rad - lon1_rad
+
+            # Haversine核心计算
+            a = math.sin(dlat / 2) ** 2 + math.cos(lat1_rad) * math.cos(lat2_rad) * math.sin(dlon / 2) ** 2
+            c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))
+            distance = EARTH_RADIUS * c
+
+            logger.debug(f"距离计算：({lat1:.6f},{lon1:.6f}) → ({lat2:.6f},{lon2:.6f}) = {distance:.2f}米")
+            return distance
+
+        except Exception as e:
+            logger.error(f"距离计算失败：{str(e)}")
+            raise
+
+    def update_uav_state(self, new_lat: float, new_lon: float, new_heading: float = None) -> None:
+        """
+        更新无人机位置和航向
+        :param new_lat: 新纬度
+        :param new_lon: 新经度
+        :param new_heading: 新航向（可选，不传入则保持原航向）
+        """
+        try:
+            if not (isinstance(new_lat, (int, float)) and isinstance(new_lon, (int, float))):
+                raise ValueError("经纬度必须为数字")
+
+            self.current_position = (new_lat, new_lon)
+            if new_heading is not None:
+                if not isinstance(new_heading, (int, float)) or new_heading < 0 or new_heading > 360:
+                    raise ValueError("航向需为0-360之间的数字")
+                self.current_heading = new_heading
+
+            logger.info(f"无人机状态更新：位置({new_lat:.6f},{new_lon:.6f})，航向{self.current_heading:.1f}°")
+
+        except ValueError as e:
+            logger.error(f"更新无人机状态失败：{str(e)}")
+            raise
+
+    def update_obstacle_data(self, distance: float) -> None:
+        """
+        更新障碍物检测数据（模拟激光雷达/超声波传感器输入）
+        :param distance: 前方障碍物距离（米），None表示未检测到
+        """
+        if distance is None:
+            self.is_obstacle_detected = False
+            self.obstacle_distance = float("inf")
+            logger.info("未检测到障碍物")
             return
 
-        # 获取当前目标航点
+        try:
+            if not isinstance(distance, (int, float)) or distance < 0:
+                raise ValueError("障碍物距离需为非负数字")
+
+            self.obstacle_distance = distance
+            self.is_obstacle_detected = distance < self.safety_distance
+
+            if self.is_obstacle_detected:
+                logger.warning(f"检测到前方障碍物！距离：{distance:.2f}米（安全阈值：{self.safety_distance}米）")
+            else:
+                logger.info(f"前方障碍物距离：{distance:.2f}米（安全）")
+
+        except ValueError as e:
+            logger.error(f"更新障碍物数据失败：{str(e)}")
+            raise
+
+    def replan_path(self) -> List[Tuple[float, float]]:
+        """
+        路径重规划（简化版）：遇到障碍时生成绕行航点
+        :return: 重规划后的航点列表
+        """
+        logger.info("触发路径重规划：生成绕行航点")
+
+        # 获取当前位置和目标航点
+        current_lat, current_lon = self.current_position
         target_lat, target_lon = self.waypoints[self.target_waypoint_idx]
-        # 计算当前位置到目标航点的距离
-        distance_to_target = self.calculate_distance(
-            self.current_lat, self.current_lon, target_lat, target_lon
+
+        # 计算绕行偏移量（向左偏移0.0001度，约11米，适配经纬度坐标系）
+        offset_lat = (target_lat - current_lat) * 0.1  # 纬度偏移
+        offset_lon = (target_lon - current_lon) * 0.1  # 经度偏移
+
+        # 生成绕行航点（当前位置 → 绕行点 → 原目标航点）
+        detour_point = (
+            current_lat + offset_lon,  # 左偏移经度
+            current_lon - offset_lat  # 左偏移纬度
+        )
+
+        # 重规划航点列表：保留已完成航点 + 绕行点 + 剩余航点
+        replanned_waypoints = (
+                self.waypoints[:self.target_waypoint_idx] +
+                [detour_point] +
+                self.waypoints[self.target_waypoint_idx:]
         )
 
-        print(f"当前位置：({self.current_lat:.6f}, {self.current_lon:.6f})")
-        print(f"目标航点{self.target_waypoint_idx+1}：({target_lat:.6f}, {target_lon:.6f})")
-        print(f"到目标航点距离：{distance_to_target:.2f} 米")
+        logger.info(f"路径重规划完成：新增绕行航点{detour_point}")
+        return replanned_waypoints
+
+    def navigate(self) -> None:
+        """
+        核心导航逻辑：
+        1. 检查航点是否为空
+        2. 检测障碍物，必要时重规划路径
+        3. 飞向目标航点，到达后切换下一个
+        """
+        # 检查航点
+        if not self.waypoints:
+            logger.error("导航终止：未设置航点")
+            return
+
+        # 检查是否完成所有航点
+        if self.target_waypoint_idx >= len(self.waypoints):
+            logger.info("导航完成：所有航点已到达")
+            return
+
+        # 障碍物检测与路径重规划
+        if self.is_obstacle_detected:
+            self.waypoints = self.replan_path()
+
+        # 获取目标航点
+        target_lat, target_lon = self.waypoints[self.target_waypoint_idx]
+        current_lat, current_lon = self.current_position
 
-        # 判断是否到达当前航点
+        # 计算到目标航点的距离
+        distance_to_target = self.calculate_haversine_distance(
+            current_lat, current_lon, target_lat, target_lon
+        )
+
+        # 判断是否到达目标航点
         if distance_to_target < self.arrival_threshold:
-            print(f"已到达航点{self.target_waypoint_idx+1}")
+            logger.info(f"到达航点{self.target_waypoint_idx + 1}：({target_lat:.6f},{target_lon:.6f})")
             self.target_waypoint_idx += 1
-            # 判断是否完成所有航点
-            if self.target_waypoint_idx >= len(self.waypoints):
-                print("所有航点导航完成！")
-                return
-            else:
-                print(f"切换至下一个航点：{self.target_waypoint_idx+1}")
+            # 重置障碍物状态
+            self.is_obstacle_detected = False
+            self.obstacle_distance = float("inf")
         else:
-            # 生成飞行指令（实际场景中需替换为无人机SDK的飞行控制指令）
-            print("飞行指令：向目标航点飞行（航向/速度需根据实际SDK调整）")
+            # 生成飞行指令（实际场景替换为无人机SDK API）
+            logger.info(
+                f"飞向航点{self.target_waypoint_idx + 1}："
+                f"当前距离{distance_to_target:.2f}米，航向{self.current_heading:.1f}°"
+            )
+
 
 # ------------------- 测试代码 -------------------
 if __name__ == "__main__":
-    # 初始化无人机导航实例
-    uav = UAVAutonomousNavigation()
+    # 初始化导航实例
+    uav_nav = UAVAutonomousNavigation()
 
-    # 设置航点列表（示例：3个航点）
-    waypoints = [
+    # 1. 设置航点（示例：3个真实地理坐标）
+    waypoints_list = [
         (39.908823, 116.397470),  # 航点1：天安门
         (39.997563, 116.337624),  # 航点2：颐和园
-        (40.006401, 116.397029)   # 航点3：圆明园
+        (40.006401, 116.397029)  # 航点3：圆明园
     ]
-    uav.set_waypoints(waypoints)
+    uav_nav.set_waypoints(waypoints_list)
+
+    # 2. 模拟无人机初始状态
+    uav_nav.update_uav_state(39.908820, 116.397468, 270.0)  # 初始位置靠近航点1，航向270°（正西）
+
+    # 3. 模拟导航过程1：无障碍物，飞向航点1
+    logger.info("\n===== 导航阶段1：无障碍物 =====")
+    uav_nav.update_obstacle_data(20.0)  # 障碍物距离20米（安全）
+    uav_nav.navigate()
 
-    # 模拟无人机位置更新与导航过程
-    # 模拟初始位置（靠近航点1）
-    uav.update_position(39.908820, 116.397468)
-    uav.navigate_to_waypoints()
+    # 4. 模拟到达航点1，更新位置
+    uav_nav.update_uav_state(39.908823, 116.397470)
+    uav_nav.navigate()
 
-    print("-" * 50)
+    # 5. 模拟导航阶段2：检测到障碍物，触发路径重规划
+    logger.info("\n===== 导航阶段2：检测到障碍物 =====")
+    uav_nav.update_obstacle_data(8.0)  # 障碍物距离8米（触发避障）
+    uav_nav.navigate()
 
-    # 模拟无人机飞到航点1，更新位置
-    uav.update_position(39.908823, 116.397470)
-    uav.navigate_to_waypoints()
\ No newline at end of file
+    # 6. 模拟绕开障碍后继续导航
+    logger.info("\n===== 导航阶段3：绕开障碍后 =====")
+    uav_nav.update_obstacle_data(None)  # 无障碍物
+    uav_nav.update_uav_state(39.950000, 116.360000)  # 模拟飞到绕行点
+    uav_nav.navigate()
\ No newline at end of file

From 8867bfac303e182a253c2b11303bb7f9decd1224 Mon Sep 17 00:00:00 2001
From: XYTX0427 <481177497@qq.com>
Date: Fri, 19 Dec 2025 22:57:25 +0800
Subject: [PATCH 03/21] =?UTF-8?q?=E8=AF=95=E9=AA=8C=E6=96=B0=E4=BB=A3?=
 =?UTF-8?q?=E7=A0=81?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/Unmanned_aerial_vehicle/main.py | 354 ++++++++++------------------
 1 file changed, 118 insertions(+), 236 deletions(-)

diff --git a/src/Unmanned_aerial_vehicle/main.py b/src/Unmanned_aerial_vehicle/main.py
index 3529838050..679e2b45e0 100644
--- a/src/Unmanned_aerial_vehicle/main.py
+++ b/src/Unmanned_aerial_vehicle/main.py
@@ -1,243 +1,125 @@
 import math
-import logging
-from typing import List, Tuple
-
-# 配置日志，方便追踪运行状态
-logging.basicConfig(
-    level=logging.INFO,
-    format="%(asctime)s - %(levelname)s - %(message)s",
-    datefmt="%Y-%m-%d %H:%M:%S"
-)
-logger = logging.getLogger("UAV_Autonomous_Navigation")
-
-
-class UAVAutonomousNavigation:
-    """无人机自主导航核心类：整合航点导航、避障、路径重规划"""
-
-    def __init__(self):
-        # 无人机核心状态
-        self.current_position: Tuple[float, float] = (0.0, 0.0)  # (纬度, 经度)
-        self.current_heading: float = 0.0  # 当前航向（度，0=正北，90=正东）
-        self.safety_distance: float = 10.0  # 避障安全距离（米）
-        self.arrival_threshold: float = 5.0  # 到达航点判定阈值（米）
-
-        # 导航相关参数
-        self.waypoints: List[Tuple[float, float]] = []  # 航点列表
-        self.target_waypoint_idx: int = 0  # 当前目标航点索引
-        self.is_obstacle_detected: bool = False  # 是否检测到障碍物
-        self.obstacle_distance: float = float("inf")  # 障碍物距离（初始为无穷大）
-
-    def set_waypoints(self, waypoints: List[Tuple[float, float]]) -> None:
-        """
-        设置航点列表，增加输入验证
-        :param waypoints: 航点列表，每个元素为(纬度, 经度)元组
-        """
-        try:
-            # 验证航点格式
-            if not isinstance(waypoints, list):
-                raise TypeError("航点必须为列表格式")
-            for idx, wp in enumerate(waypoints):
-                if not isinstance(wp, tuple) or len(wp) != 2:
-                    raise ValueError(f"第{idx + 1}个航点格式错误，需为(纬度, 经度)元组")
-                if not (isinstance(wp[0], (int, float)) and isinstance(wp[1], (int, float))):
-                    raise ValueError(f"第{idx + 1}个航点经纬度必须为数字")
-
-            self.waypoints = waypoints
-            self.target_waypoint_idx = 0
-            logger.info(f"成功设置{len(waypoints)}个航点，初始目标航点：1")
-
-        except (TypeError, ValueError) as e:
-            logger.error(f"设置航点失败：{str(e)}")
-            raise
-
-    def calculate_haversine_distance(self, lat1: float, lon1: float, lat2: float, lon2: float) -> float:
-        """
-        高精度计算两个经纬度点的地面距离（米）
-        采用Haversine公式，适配无人机低空导航场景
-        """
-        try:
-            # 地球平均半径（米）
-            EARTH_RADIUS = 6371008.8
-            # 角度转弧度
-            lat1_rad = math.radians(lat1)
-            lon1_rad = math.radians(lon1)
-            lat2_rad = math.radians(lat2)
-            lon2_rad = math.radians(lon2)
-
-            # 经纬度差值
-            dlat = lat2_rad - lat1_rad
-            dlon = lon2_rad - lon1_rad
-
-            # Haversine核心计算
-            a = math.sin(dlat / 2) ** 2 + math.cos(lat1_rad) * math.cos(lat2_rad) * math.sin(dlon / 2) ** 2
-            c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))
-            distance = EARTH_RADIUS * c
-
-            logger.debug(f"距离计算：({lat1:.6f},{lon1:.6f}) → ({lat2:.6f},{lon2:.6f}) = {distance:.2f}米")
-            return distance
-
-        except Exception as e:
-            logger.error(f"距离计算失败：{str(e)}")
-            raise
-
-    def update_uav_state(self, new_lat: float, new_lon: float, new_heading: float = None) -> None:
-        """
-        更新无人机位置和航向
-        :param new_lat: 新纬度
-        :param new_lon: 新经度
-        :param new_heading: 新航向（可选，不传入则保持原航向）
-        """
-        try:
-            if not (isinstance(new_lat, (int, float)) and isinstance(new_lon, (int, float))):
-                raise ValueError("经纬度必须为数字")
-
-            self.current_position = (new_lat, new_lon)
-            if new_heading is not None:
-                if not isinstance(new_heading, (int, float)) or new_heading < 0 or new_heading > 360:
-                    raise ValueError("航向需为0-360之间的数字")
-                self.current_heading = new_heading
-
-            logger.info(f"无人机状态更新：位置({new_lat:.6f},{new_lon:.6f})，航向{self.current_heading:.1f}°")
-
-        except ValueError as e:
-            logger.error(f"更新无人机状态失败：{str(e)}")
-            raise
-
-    def update_obstacle_data(self, distance: float) -> None:
-        """
-        更新障碍物检测数据（模拟激光雷达/超声波传感器输入）
-        :param distance: 前方障碍物距离（米），None表示未检测到
-        """
-        if distance is None:
-            self.is_obstacle_detected = False
-            self.obstacle_distance = float("inf")
-            logger.info("未检测到障碍物")
+import random
+import time
+
+
+class Drone:
+    """无人机核心类，模拟无人机的状态和行为"""
+
+    def __init__(self, start_x=0.0, start_y=0.0, start_z=0.0):
+        # 无人机当前位置 (x, y, z) 坐标
+        self.x = start_x
+        self.y = start_y
+        self.z = start_z
+        # 无人机飞行速度 (m/s)
+        self.speed = 2.0
+        # 无人机状态：idle(待机)/flying(飞行)/avoiding(避障)
+        self.status = "idle"
+        # 目标位置
+        self.target_x = 0.0
+        self.target_y = 0.0
+        self.target_z = 0.0
+        # 避障距离阈值 (米)
+        self.obstacle_threshold = 3.0
+
+    def set_target(self, target_x, target_y, target_z):
+        """设置无人机的目标位置"""
+        self.target_x = target_x
+        self.target_y = target_y
+        self.target_z = target_z
+        self.status = "flying"
+        print(f"无人机目标已设置：({target_x}, {target_y}, {target_z})，开始飞行")
+
+    def calculate_distance_to_target(self):
+        """计算当前位置到目标位置的直线距离"""
+        dx = self.target_x - self.x
+        dy = self.target_y - self.y
+        dz = self.target_z - self.z
+        return math.sqrt(dx ** 2 + dy ** 2 + dz ** 2)
+
+    def detect_obstacle(self, obstacles):
+        """模拟障碍物检测：检查是否有障碍物在避障阈值范围内"""
+        for (ox, oy, oz) in obstacles:
+            dx = ox - self.x
+            dy = oy - self.y
+            dz = oz - self.z
+            distance = math.sqrt(dx ** 2 + dy ** 2 + dz ** 2)
+            if distance < self.obstacle_threshold:
+                return True, (ox, oy, oz)  # 检测到障碍物，返回障碍物位置
+        return False, None
+
+    def avoid_obstacle(self, obstacle_pos):
+        """简单避障策略：向障碍物右侧偏移飞行"""
+        ox, oy, _ = obstacle_pos
+        # 计算偏移方向（右侧1米）
+        self.x += (self.x - ox) * 0.5  # 远离障碍物x方向
+        self.y += (self.y - oy) * 0.5  # 远离障碍物y方向
+        print(f"检测到障碍物({ox}, {oy})，已避障，当前位置：({self.x:.2f}, {self.y:.2f})")
+        self.status = "avoiding"
+
+    def update_position(self):
+        """更新无人机位置，向目标点移动"""
+        if self.status != "flying":
             return
 
-        try:
-            if not isinstance(distance, (int, float)) or distance < 0:
-                raise ValueError("障碍物距离需为非负数字")
-
-            self.obstacle_distance = distance
-            self.is_obstacle_detected = distance < self.safety_distance
-
-            if self.is_obstacle_detected:
-                logger.warning(f"检测到前方障碍物！距离：{distance:.2f}米（安全阈值：{self.safety_distance}米）")
-            else:
-                logger.info(f"前方障碍物距离：{distance:.2f}米（安全）")
-
-        except ValueError as e:
-            logger.error(f"更新障碍物数据失败：{str(e)}")
-            raise
-
-    def replan_path(self) -> List[Tuple[float, float]]:
-        """
-        路径重规划（简化版）：遇到障碍时生成绕行航点
-        :return: 重规划后的航点列表
-        """
-        logger.info("触发路径重规划：生成绕行航点")
-
-        # 获取当前位置和目标航点
-        current_lat, current_lon = self.current_position
-        target_lat, target_lon = self.waypoints[self.target_waypoint_idx]
-
-        # 计算绕行偏移量（向左偏移0.0001度，约11米，适配经纬度坐标系）
-        offset_lat = (target_lat - current_lat) * 0.1  # 纬度偏移
-        offset_lon = (target_lon - current_lon) * 0.1  # 经度偏移
-
-        # 生成绕行航点（当前位置 → 绕行点 → 原目标航点）
-        detour_point = (
-            current_lat + offset_lon,  # 左偏移经度
-            current_lon - offset_lat  # 左偏移纬度
-        )
-
-        # 重规划航点列表：保留已完成航点 + 绕行点 + 剩余航点
-        replanned_waypoints = (
-                self.waypoints[:self.target_waypoint_idx] +
-                [detour_point] +
-                self.waypoints[self.target_waypoint_idx:]
-        )
-
-        logger.info(f"路径重规划完成：新增绕行航点{detour_point}")
-        return replanned_waypoints
-
-    def navigate(self) -> None:
-        """
-        核心导航逻辑：
-        1. 检查航点是否为空
-        2. 检测障碍物，必要时重规划路径
-        3. 飞向目标航点，到达后切换下一个
-        """
-        # 检查航点
-        if not self.waypoints:
-            logger.error("导航终止：未设置航点")
+        # 计算方向向量
+        dx = self.target_x - self.x
+        dy = self.target_y - self.y
+        dz = self.target_z - self.z
+        distance = self.calculate_distance_to_target()
+
+        # 如果距离小于步长，直接到达目标
+        if distance < self.speed:
+            self.x = self.target_x
+            self.y = self.target_y
+            self.z = self.target_z
+            self.status = "idle"
+            print(f"已到达目标位置：({self.x:.2f}, {self.y:.2f}, {self.z:.2f})")
             return
 
-        # 检查是否完成所有航点
-        if self.target_waypoint_idx >= len(self.waypoints):
-            logger.info("导航完成：所有航点已到达")
-            return
+        # 归一化方向向量，按速度移动
+        self.x += (dx / distance) * self.speed
+        self.y += (dy / distance) * self.speed
+        self.z += (dz / distance) * self.speed
+
+    def get_status(self):
+        """获取无人机当前状态和位置"""
+        return {
+            "position": (round(self.x, 2), round(self.y, 2), round(self.z, 2)),
+            "status": self.status,
+            "distance_to_target": round(self.calculate_distance_to_target(), 2)
+        }
+
 
-        # 障碍物检测与路径重规划
-        if self.is_obstacle_detected:
-            self.waypoints = self.replan_path()
-
-        # 获取目标航点
-        target_lat, target_lon = self.waypoints[self.target_waypoint_idx]
-        current_lat, current_lon = self.current_position
-
-        # 计算到目标航点的距离
-        distance_to_target = self.calculate_haversine_distance(
-            current_lat, current_lon, target_lat, target_lon
-        )
-
-        # 判断是否到达目标航点
-        if distance_to_target < self.arrival_threshold:
-            logger.info(f"到达航点{self.target_waypoint_idx + 1}：({target_lat:.6f},{target_lon:.6f})")
-            self.target_waypoint_idx += 1
-            # 重置障碍物状态
-            self.is_obstacle_detected = False
-            self.obstacle_distance = float("inf")
-        else:
-            # 生成飞行指令（实际场景替换为无人机SDK API）
-            logger.info(
-                f"飞向航点{self.target_waypoint_idx + 1}："
-                f"当前距离{distance_to_target:.2f}米，航向{self.current_heading:.1f}°"
-            )
-
-
-# ------------------- 测试代码 -------------------
+# -------------------------- 主程序：无人机自主导航仿真 --------------------------
 if __name__ == "__main__":
-    # 初始化导航实例
-    uav_nav = UAVAutonomousNavigation()
-
-    # 1. 设置航点（示例：3个真实地理坐标）
-    waypoints_list = [
-        (39.908823, 116.397470),  # 航点1：天安门
-        (39.997563, 116.337624),  # 航点2：颐和园
-        (40.006401, 116.397029)  # 航点3：圆明园
-    ]
-    uav_nav.set_waypoints(waypoints_list)
-
-    # 2. 模拟无人机初始状态
-    uav_nav.update_uav_state(39.908820, 116.397468, 270.0)  # 初始位置靠近航点1，航向270°（正西）
-
-    # 3. 模拟导航过程1：无障碍物，飞向航点1
-    logger.info("\n===== 导航阶段1：无障碍物 =====")
-    uav_nav.update_obstacle_data(20.0)  # 障碍物距离20米（安全）
-    uav_nav.navigate()
-
-    # 4. 模拟到达航点1，更新位置
-    uav_nav.update_uav_state(39.908823, 116.397470)
-    uav_nav.navigate()
-
-    # 5. 模拟导航阶段2：检测到障碍物，触发路径重规划
-    logger.info("\n===== 导航阶段2：检测到障碍物 =====")
-    uav_nav.update_obstacle_data(8.0)  # 障碍物距离8米（触发避障）
-    uav_nav.navigate()
-
-    # 6. 模拟绕开障碍后继续导航
-    logger.info("\n===== 导航阶段3：绕开障碍后 =====")
-    uav_nav.update_obstacle_data(None)  # 无障碍物
-    uav_nav.update_uav_state(39.950000, 116.360000)  # 模拟飞到绕行点
-    uav_nav.navigate()
\ No newline at end of file
+    # 1. 初始化无人机（起点：0,0,10米高度）
+    drone = Drone(start_x=0.0, start_y=0.0, start_z=10.0)
+
+    # 2. 设置目标位置（终点：20, 15, 10米高度）
+    drone.set_target(target_x=20.0, target_y=15.0, target_z=10.0)
+
+    # 3. 模拟环境中的障碍物（坐标列表）
+    obstacles = [(10, 8, 10), (15, 12, 9)]  # 两个障碍物位置
+
+    # 4. 自主导航主循环
+    print("\n=== 无人机自主导航开始 ===")
+    while drone.status != "idle":
+        # 检测障碍物
+        obstacle_detected, obstacle_pos = drone.detect_obstacle(obstacles)
+        if obstacle_detected:
+            drone.avoid_obstacle(obstacle_pos)
+            # 避障后恢复飞行状态
+            drone.status = "flying"
+
+        # 更新位置
+        drone.update_position()
+
+        # 打印实时状态
+        status = drone.get_status()
+        print(f"当前位置：{status['position']} | 距离目标：{status['distance_to_target']}米 | 状态：{status['status']}")
+
+        # 模拟实时刷新（每秒更新一次位置）
+        time.sleep(1)
+
+    print("\n=== 无人机自主导航完成 ===")
\ No newline at end of file

From 0788d1618563fbbc0bd4df80496907161d7b11be Mon Sep 17 00:00:00 2001
From: XYTX0427 <481177497@qq.com>
Date: Sun, 21 Dec 2025 16:24:42 +0800
Subject: [PATCH 04/21] =?UTF-8?q?=E8=AF=95=E9=AA=8C=E6=96=B0=E7=8E=AF?=
 =?UTF-8?q?=E5=A2=83?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/Unmanned_aerial_vehicle/main.py | 275 ++++++++++++++++------------
 1 file changed, 156 insertions(+), 119 deletions(-)

diff --git a/src/Unmanned_aerial_vehicle/main.py b/src/Unmanned_aerial_vehicle/main.py
index 679e2b45e0..5533b25647 100644
--- a/src/Unmanned_aerial_vehicle/main.py
+++ b/src/Unmanned_aerial_vehicle/main.py
@@ -1,125 +1,162 @@
 import math
-import random
 import time
 
 
-class Drone:
-    """无人机核心类，模拟无人机的状态和行为"""
-
-    def __init__(self, start_x=0.0, start_y=0.0, start_z=0.0):
-        # 无人机当前位置 (x, y, z) 坐标
-        self.x = start_x
-        self.y = start_y
-        self.z = start_z
-        # 无人机飞行速度 (m/s)
-        self.speed = 2.0
-        # 无人机状态：idle(待机)/flying(飞行)/avoiding(避障)
-        self.status = "idle"
+class DroneAutonomousNavigation:
+    def __init__(self):
+        """初始化无人机导航模拟器（无硬件依赖）"""
+        # 模拟无人机当前位置 [纬度, 经度, 高度(m)]
+        self.current_position = [39.908823, 116.397470, 10.0]  # 初始位置（天安门附近）
         # 目标位置
-        self.target_x = 0.0
-        self.target_y = 0.0
-        self.target_z = 0.0
-        # 避障距离阈值 (米)
-        self.obstacle_threshold = 3.0
-
-    def set_target(self, target_x, target_y, target_z):
-        """设置无人机的目标位置"""
-        self.target_x = target_x
-        self.target_y = target_y
-        self.target_z = target_z
-        self.status = "flying"
-        print(f"无人机目标已设置：({target_x}, {target_y}, {target_z})，开始飞行")
-
-    def calculate_distance_to_target(self):
-        """计算当前位置到目标位置的直线距离"""
-        dx = self.target_x - self.x
-        dy = self.target_y - self.y
-        dz = self.target_z - self.z
-        return math.sqrt(dx ** 2 + dy ** 2 + dz ** 2)
-
-    def detect_obstacle(self, obstacles):
-        """模拟障碍物检测：检查是否有障碍物在避障阈值范围内"""
-        for (ox, oy, oz) in obstacles:
-            dx = ox - self.x
-            dy = oy - self.y
-            dz = oz - self.z
-            distance = math.sqrt(dx ** 2 + dy ** 2 + dz ** 2)
-            if distance < self.obstacle_threshold:
-                return True, (ox, oy, oz)  # 检测到障碍物，返回障碍物位置
-        return False, None
-
-    def avoid_obstacle(self, obstacle_pos):
-        """简单避障策略：向障碍物右侧偏移飞行"""
-        ox, oy, _ = obstacle_pos
-        # 计算偏移方向（右侧1米）
-        self.x += (self.x - ox) * 0.5  # 远离障碍物x方向
-        self.y += (self.y - oy) * 0.5  # 远离障碍物y方向
-        print(f"检测到障碍物({ox}, {oy})，已避障，当前位置：({self.x:.2f}, {self.y:.2f})")
-        self.status = "avoiding"
-
-    def update_position(self):
-        """更新无人机位置，向目标点移动"""
-        if self.status != "flying":
-            return
-
-        # 计算方向向量
-        dx = self.target_x - self.x
-        dy = self.target_y - self.y
-        dz = self.target_z - self.z
-        distance = self.calculate_distance_to_target()
-
-        # 如果距离小于步长，直接到达目标
-        if distance < self.speed:
-            self.x = self.target_x
-            self.y = self.target_y
-            self.z = self.target_z
-            self.status = "idle"
-            print(f"已到达目标位置：({self.x:.2f}, {self.y:.2f}, {self.z:.2f})")
-            return
-
-        # 归一化方向向量，按速度移动
-        self.x += (dx / distance) * self.speed
-        self.y += (dy / distance) * self.speed
-        self.z += (dz / distance) * self.speed
-
-    def get_status(self):
-        """获取无人机当前状态和位置"""
-        return {
-            "position": (round(self.x, 2), round(self.y, 2), round(self.z, 2)),
-            "status": self.status,
-            "distance_to_target": round(self.calculate_distance_to_target(), 2)
-        }
-
-
-# -------------------------- 主程序：无人机自主导航仿真 --------------------------
+        self.target_position = None
+        # 导航状态
+        self.is_navigating = False
+
+    def set_current_position(self, lat, lon, alt):
+        """手动设置当前位置（模拟GPS更新）"""
+        self.current_position = [lat, lon, alt]
+        print(f"✅ 更新当前位置：纬度{lat:.6f}, 经度{lon:.6f}, 高度{alt:.1f}m")
+
+    def calculate_gps_distance(self, pos1, pos2):
+        """
+        纯Python实现GPS两点距离计算（半正矢公式）
+        :param pos1: [lat, lon, alt] 起点
+        :param pos2: [lat, lon, alt] 终点
+        :return: 地面距离（米）
+        """
+        # 地球半径（米）
+        EARTH_RADIUS = 6371000.0
+
+        # 转换为弧度
+        lat1, lon1 = math.radians(pos1[0]), math.radians(pos1[1])
+        lat2, lon2 = math.radians(pos2[0]), math.radians(pos2[1])
+
+        # 计算经纬度差值
+        dlat = lat2 - lat1
+        dlon = lon2 - lon1
+
+        # 半正矢公式核心计算
+        a = math.sin(dlat / 2) ** 2 + math.cos(lat1) * math.cos(lat2) * math.sin(dlon / 2) ** 2
+        c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))
+        distance = EARTH_RADIUS * c
+
+        return round(distance, 2)
+
+    def generate_straight_path(self, start_pos, target_pos, waypoint_count=5):
+        """
+        生成直线插值路径（无避障）
+        :param waypoint_count: 中间航点数量
+        :return: 航点列表 [[lat, lon, alt], ...]
+        """
+        path = []
+        # 计算每个维度的步长
+        lat_step = (target_pos[0] - start_pos[0]) / (waypoint_count + 1)
+        lon_step = (target_pos[1] - start_pos[1]) / (waypoint_count + 1)
+        alt_step = (target_pos[2] - start_pos[2]) / (waypoint_count + 1)
+
+        # 生成中间航点
+        for i in range(1, waypoint_count + 1):
+            lat = start_pos[0] + lat_step * i
+            lon = start_pos[1] + lon_step * i
+            alt = start_pos[2] + alt_step * i
+            path.append([round(lat, 6), round(lon, 6), round(alt, 1)])
+
+        # 添加最终目标点
+        path.append([target_pos[0], target_pos[1], target_pos[2]])
+        return path
+
+    def simulate_fly_to_waypoint(self, waypoint):
+        """
+        模拟飞向单个航点（逐步更新位置）
+        :param waypoint: 目标航点 [lat, lon, alt]
+        """
+        # 每次移动的步长（模拟无人机飞行，每次移动0.00001度经纬度）
+        LAT_STEP = 0.00001
+        LON_STEP = 0.00001
+        ALT_STEP = 0.5  # 高度每次移动0.5米
+
+        # 持续移动直到到达航点（距离<1米）
+        while True:
+            distance = self.calculate_gps_distance(self.current_position, waypoint)
+            if distance < 1.0:
+                print(f"✅ 到达航点：{waypoint} (距离{distance}m)")
+                break
+
+            # 计算移动方向并更新位置
+            current_lat, current_lon, current_alt = self.current_position
+            target_lat, target_lon, target_alt = waypoint
+
+            # 纬度调整
+            if current_lat < target_lat:
+                new_lat = current_lat + LAT_STEP
+            elif current_lat > target_lat:
+                new_lat = current_lat - LAT_STEP
+            else:
+                new_lat = current_lat
+
+            # 经度调整
+            if current_lon < target_lon:
+                new_lon = current_lon + LON_STEP
+            elif current_lon > target_lon:
+                new_lon = current_lon - LON_STEP
+            else:
+                new_lon = current_lon
+
+            # 高度调整
+            if current_alt < target_alt:
+                new_alt = current_alt + ALT_STEP
+            elif current_alt > target_alt:
+                new_alt = current_alt - ALT_STEP
+            else:
+                new_alt = current_alt
+
+            # 更新位置
+            self.set_current_position(new_lat, new_lon, new_alt)
+            # 模拟飞行延迟
+            time.sleep(0.1)
+
+    def navigate_to_target(self, target_lat, target_lon, target_alt):
+        """
+        自主导航主函数（纯算法模拟）
+        """
+        self.target_position = [target_lat, target_lon, target_alt]
+        self.is_navigating = True
+
+        print("\n🚀 开始自主导航任务")
+        print(f"📌 起点：{self.current_position}")
+        print(f"🎯 终点：{self.target_position}")
+
+        # 1. 生成路径
+        path = self.generate_straight_path(self.current_position, self.target_position)
+        print(f"\n🗺️  生成路径完成，共{len(path)}个航点：")
+        for i, wp in enumerate(path):
+            print(f"   航点{i + 1}：{wp}")
+
+        # 2. 依次飞向每个航点
+        print("\n✈️  开始飞向目标...")
+        for i, waypoint in enumerate(path):
+            print(f"\n--- 飞向第{i + 1}个航点 ---")
+            self.simulate_fly_to_waypoint(waypoint)
+
+        # 3. 导航完成
+        self.is_navigating = False
+        print("\n🎉 导航任务完成！已到达目标点")
+
+
+# ------------------- 测试代码 -------------------
 if __name__ == "__main__":
-    # 1. 初始化无人机（起点：0,0,10米高度）
-    drone = Drone(start_x=0.0, start_y=0.0, start_z=10.0)
-
-    # 2. 设置目标位置（终点：20, 15, 10米高度）
-    drone.set_target(target_x=20.0, target_y=15.0, target_z=10.0)
-
-    # 3. 模拟环境中的障碍物（坐标列表）
-    obstacles = [(10, 8, 10), (15, 12, 9)]  # 两个障碍物位置
-
-    # 4. 自主导航主循环
-    print("\n=== 无人机自主导航开始 ===")
-    while drone.status != "idle":
-        # 检测障碍物
-        obstacle_detected, obstacle_pos = drone.detect_obstacle(obstacles)
-        if obstacle_detected:
-            drone.avoid_obstacle(obstacle_pos)
-            # 避障后恢复飞行状态
-            drone.status = "flying"
-
-        # 更新位置
-        drone.update_position()
-
-        # 打印实时状态
-        status = drone.get_status()
-        print(f"当前位置：{status['position']} | 距离目标：{status['distance_to_target']}米 | 状态：{status['status']}")
-
-        # 模拟实时刷新（每秒更新一次位置）
-        time.sleep(1)
-
-    print("\n=== 无人机自主导航完成 ===")
\ No newline at end of file
+    # 初始化导航模拟器
+    drone = DroneAutonomousNavigation()
+
+    # 设置目标点（比如：北京奥林匹克公园，高度50米）
+    target_lat = 39.990168
+    target_lon = 116.397204
+    target_alt = 50.0
+
+    # 执行自主导航
+    try:
+        drone.navigate_to_target(target_lat, target_lon, target_alt)
+    except KeyboardInterrupt:
+        print("\n🛑 导航任务被手动终止")
+    finally:
+        print("\n🛬 无人机已悬停/降落")
\ No newline at end of file

From 70ee8802909580025714081f4fbaccd6202fdd0f Mon Sep 17 00:00:00 2001
From: XYTX0427 <481177497@qq.com>
Date: Sun, 21 Dec 2025 21:00:21 +0800
Subject: [PATCH 05/21] =?UTF-8?q?=E9=87=8D=E6=96=B0=E6=8F=90=E4=BA=A4?=
 =?UTF-8?q?=E9=80=89=E9=A2=98?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/Robotic_arm/README.md                            | 1 +
 src/{Unmanned_aerial_vehicle => Robotic_arm}/main.py | 0
 src/Unmanned_aerial_vehicle/README.md                | 1 -
 3 files changed, 1 insertion(+), 1 deletion(-)
 create mode 100644 src/Robotic_arm/README.md
 rename src/{Unmanned_aerial_vehicle => Robotic_arm}/main.py (100%)
 delete mode 100644 src/Unmanned_aerial_vehicle/README.md

diff --git a/src/Robotic_arm/README.md b/src/Robotic_arm/README.md
new file mode 100644
index 0000000000..fc1e2dead9
--- /dev/null
+++ b/src/Robotic_arm/README.md
@@ -0,0 +1 @@
+机械臂
\ No newline at end of file
diff --git a/src/Unmanned_aerial_vehicle/main.py b/src/Robotic_arm/main.py
similarity index 100%
rename from src/Unmanned_aerial_vehicle/main.py
rename to src/Robotic_arm/main.py
diff --git a/src/Unmanned_aerial_vehicle/README.md b/src/Unmanned_aerial_vehicle/README.md
deleted file mode 100644
index b00460f809..0000000000
--- a/src/Unmanned_aerial_vehicle/README.md
+++ /dev/null
@@ -1 +0,0 @@
-无人机导航
\ No newline at end of file

From 7f88a0b42ac7cf08e71513641db12c145be20c1a Mon Sep 17 00:00:00 2001
From: XYTX0427 <481177497@qq.com>
Date: Sun, 21 Dec 2025 21:42:23 +0800
Subject: [PATCH 06/21] =?UTF-8?q?=E5=AE=9E=E7=8E=B0=E5=9F=BA=E7=A1=80?=
 =?UTF-8?q?=E7=9A=84=E6=9C=BA=E6=A2=B0=E8=87=82=E5=BB=BA=E6=A8=A1.?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/Robotic_arm/main.py | 314 ++++++++++++++++++++--------------------
 1 file changed, 155 insertions(+), 159 deletions(-)

diff --git a/src/Robotic_arm/main.py b/src/Robotic_arm/main.py
index 5533b25647..0529a86787 100644
--- a/src/Robotic_arm/main.py
+++ b/src/Robotic_arm/main.py
@@ -1,162 +1,158 @@
-import math
-import time
-
-
-class DroneAutonomousNavigation:
-    def __init__(self):
-        """初始化无人机导航模拟器（无硬件依赖）"""
-        # 模拟无人机当前位置 [纬度, 经度, 高度(m)]
-        self.current_position = [39.908823, 116.397470, 10.0]  # 初始位置（天安门附近）
-        # 目标位置
-        self.target_position = None
-        # 导航状态
-        self.is_navigating = False
-
-    def set_current_position(self, lat, lon, alt):
-        """手动设置当前位置（模拟GPS更新）"""
-        self.current_position = [lat, lon, alt]
-        print(f"✅ 更新当前位置：纬度{lat:.6f}, 经度{lon:.6f}, 高度{alt:.1f}m")
-
-    def calculate_gps_distance(self, pos1, pos2):
-        """
-        纯Python实现GPS两点距离计算（半正矢公式）
-        :param pos1: [lat, lon, alt] 起点
-        :param pos2: [lat, lon, alt] 终点
-        :return: 地面距离（米）
-        """
-        # 地球半径（米）
-        EARTH_RADIUS = 6371000.0
-
-        # 转换为弧度
-        lat1, lon1 = math.radians(pos1[0]), math.radians(pos1[1])
-        lat2, lon2 = math.radians(pos2[0]), math.radians(pos2[1])
-
-        # 计算经纬度差值
-        dlat = lat2 - lat1
-        dlon = lon2 - lon1
-
-        # 半正矢公式核心计算
-        a = math.sin(dlat / 2) ** 2 + math.cos(lat1) * math.cos(lat2) * math.sin(dlon / 2) ** 2
-        c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))
-        distance = EARTH_RADIUS * c
-
-        return round(distance, 2)
-
-    def generate_straight_path(self, start_pos, target_pos, waypoint_count=5):
-        """
-        生成直线插值路径（无避障）
-        :param waypoint_count: 中间航点数量
-        :return: 航点列表 [[lat, lon, alt], ...]
-        """
-        path = []
-        # 计算每个维度的步长
-        lat_step = (target_pos[0] - start_pos[0]) / (waypoint_count + 1)
-        lon_step = (target_pos[1] - start_pos[1]) / (waypoint_count + 1)
-        alt_step = (target_pos[2] - start_pos[2]) / (waypoint_count + 1)
-
-        # 生成中间航点
-        for i in range(1, waypoint_count + 1):
-            lat = start_pos[0] + lat_step * i
-            lon = start_pos[1] + lon_step * i
-            alt = start_pos[2] + alt_step * i
-            path.append([round(lat, 6), round(lon, 6), round(alt, 1)])
-
-        # 添加最终目标点
-        path.append([target_pos[0], target_pos[1], target_pos[2]])
-        return path
-
-    def simulate_fly_to_waypoint(self, waypoint):
-        """
-        模拟飞向单个航点（逐步更新位置）
-        :param waypoint: 目标航点 [lat, lon, alt]
-        """
-        # 每次移动的步长（模拟无人机飞行，每次移动0.00001度经纬度）
-        LAT_STEP = 0.00001
-        LON_STEP = 0.00001
-        ALT_STEP = 0.5  # 高度每次移动0.5米
-
-        # 持续移动直到到达航点（距离<1米）
-        while True:
-            distance = self.calculate_gps_distance(self.current_position, waypoint)
-            if distance < 1.0:
-                print(f"✅ 到达航点：{waypoint} (距离{distance}m)")
-                break
-
-            # 计算移动方向并更新位置
-            current_lat, current_lon, current_alt = self.current_position
-            target_lat, target_lon, target_alt = waypoint
-
-            # 纬度调整
-            if current_lat < target_lat:
-                new_lat = current_lat + LAT_STEP
-            elif current_lat > target_lat:
-                new_lat = current_lat - LAT_STEP
-            else:
-                new_lat = current_lat
-
-            # 经度调整
-            if current_lon < target_lon:
-                new_lon = current_lon + LON_STEP
-            elif current_lon > target_lon:
-                new_lon = current_lon - LON_STEP
-            else:
-                new_lon = current_lon
-
-            # 高度调整
-            if current_alt < target_alt:
-                new_alt = current_alt + ALT_STEP
-            elif current_alt > target_alt:
-                new_alt = current_alt - ALT_STEP
-            else:
-                new_alt = current_alt
-
-            # 更新位置
-            self.set_current_position(new_lat, new_lon, new_alt)
-            # 模拟飞行延迟
-            time.sleep(0.1)
-
-    def navigate_to_target(self, target_lat, target_lon, target_alt):
-        """
-        自主导航主函数（纯算法模拟）
-        """
-        self.target_position = [target_lat, target_lon, target_alt]
-        self.is_navigating = True
-
-        print("\n🚀 开始自主导航任务")
-        print(f"📌 起点：{self.current_position}")
-        print(f"🎯 终点：{self.target_position}")
-
-        # 1. 生成路径
-        path = self.generate_straight_path(self.current_position, self.target_position)
-        print(f"\n🗺️  生成路径完成，共{len(path)}个航点：")
-        for i, wp in enumerate(path):
-            print(f"   航点{i + 1}：{wp}")
-
-        # 2. 依次飞向每个航点
-        print("\n✈️  开始飞向目标...")
-        for i, waypoint in enumerate(path):
-            print(f"\n--- 飞向第{i + 1}个航点 ---")
-            self.simulate_fly_to_waypoint(waypoint)
-
-        # 3. 导航完成
-        self.is_navigating = False
-        print("\n🎉 导航任务完成！已到达目标点")
-
-
-# ------------------- 测试代码 -------------------
-if __name__ == "__main__":
-    # 初始化导航模拟器
-    drone = DroneAutonomousNavigation()
-
-    # 设置目标点（比如：北京奥林匹克公园，高度50米）
-    target_lat = 39.990168
-    target_lon = 116.397204
-    target_alt = 50.0
+import mujoco
+import mujoco.viewer
+import numpy as np
+import os
+import tempfile
+import time  # 新增：备用的睡眠函数
+
+# ====================== 1. 定义机械臂 XML 模型 ======================
+# 6自由度机械臂的 MuJoCo XML 描述
+arm_xml = """
+<mujoco model="6dof_arm">
+  <compiler angle="radian" inertiafromgeom="true"/>
+  <option timestep="0.005" gravity="0 0 -9.81"/>
+
+  <!-- 视觉和物理材质 -->
+  <asset>
+    <material name="gray" rgba="0.7 0.7 0.7 1"/>
+    <material name="blue" rgba="0.2 0.4 0.8 1"/>
+    <material name="red" rgba="0.8 0.2 0.2 1"/>
+  </asset>
+
+  <!-- 世界体 -->
+  <worldbody>
+    <!-- 地面 -->
+    <geom name="floor" type="plane" size="5 5 0.1" pos="0 0 0" material="gray"/>
+
+    <!-- 机械臂基座 -->
+    <body name="base" pos="0 0 0">
+      <geom name="base_geom" type="cylinder" size="0.15 0.1" pos="0 0 0" material="gray"/>
+      <joint name="joint0" type="hinge" axis="0 0 1" pos="0 0 0.1"/>
+
+      <!-- 连杆1 (肩部旋转) -->
+      <body name="link1" pos="0 0 0.1">
+        <geom name="link1_geom" type="capsule" size="0.05" fromto="0 0 0 0 0 0.3" material="blue"/>
+        <joint name="joint1" type="hinge" axis="0 1 0" pos="0 0 0.3"/>
+
+        <!-- 连杆2 (肘部旋转) -->
+        <body name="link2" pos="0 0 0.3">
+          <geom name="link2_geom" type="capsule" size="0.05" fromto="0 0 0 0.4 0 0" material="blue"/>
+          <joint name="joint2" type="hinge" axis="0 1 0" pos="0.4 0 0"/>
+
+          <!-- 连杆3 (前臂) -->
+          <body name="link3" pos="0.4 0 0">
+            <geom name="link3_geom" type="capsule" size="0.04" fromto="0 0 0 0.35 0 0" material="blue"/>
+            <joint name="joint3" type="hinge" axis="1 0 0" pos="0.35 0 0"/>
+
+            <!-- 连杆4 (腕部旋转1) -->
+            <body name="link4" pos="0.35 0 0">
+              <geom name="link4_geom" type="capsule" size="0.04" fromto="0 0 0 0 0 0.25" material="blue"/>
+              <joint name="joint4" type="hinge" axis="0 1 0" pos="0 0 0.25"/>
+
+              <!-- 连杆5 (腕部旋转2) -->
+              <body name="link5" pos="0 0 0.25">
+                <geom name="link5_geom" type="capsule" size="0.03" fromto="0 0 0 0 0 0.2" material="blue"/>
+                <joint name="joint5" type="hinge" axis="1 0 0" pos="0 0 0.2"/>
+
+                <!-- 末端执行器 -->
+                <body name="end_effector" pos="0 0 0.2">
+                  <geom name="ee_geom" type="box" size="0.08 0.08 0.08" pos="0 0 0" material="red"/>
+                </body>
+              </body>
+            </body>
+          </body>
+        </body>
+      </body>
+    </body>
+  </worldbody>
+
+  <!-- 关节控制器 -->
+  <actuator>
+    <motor name="motor0" joint="joint0" ctrlrange="-3.14 3.14" gear="100"/>
+    <motor name="motor1" joint="joint1" ctrlrange="-1.57 1.57" gear="100"/>
+    <motor name="motor2" joint="joint2" ctrlrange="-1.57 1.57" gear="100"/>
+    <motor name="motor3" joint="joint3" ctrlrange="-3.14 3.14" gear="100"/>
+    <motor name="motor4" joint="joint4" ctrlrange="-1.57 1.57" gear="100"/>
+    <motor name="motor5" joint="joint5" ctrlrange="-3.14 3.14" gear="100"/>
+  </actuator>
+</mujoco>
+"""
+
+
+# ====================== 2. 模型加载和仿真控制 ======================
+def create_arm_simulation():
+    """创建并运行机械臂仿真"""
+    # 将XML字符串写入临时文件（MuJoCo需要文件路径加载）
+    with tempfile.NamedTemporaryFile(mode='w', suffix='.xml', delete=False) as f:
+        f.write(arm_xml)
+        xml_path = f.name
 
-    # 执行自主导航
     try:
-        drone.navigate_to_target(target_lat, target_lon, target_alt)
-    except KeyboardInterrupt:
-        print("\n🛑 导航任务被手动终止")
+        # 加载模型和数据
+        model = mujoco.MjModel.from_xml_path(xml_path)
+        data = mujoco.MjData(model)
+
+        print("✅ 机械臂模型加载成功！")
+        print(f"🔧 关节数量：{model.njnt}")
+        print(f"🔧 执行器数量：{model.nu}")
+
+        # 设置初始关节角度
+        initial_joint_angles = [0, 0.2, -0.5, 0, 0.3, 0]
+        data.qpos[:6] = initial_joint_angles
+
+        # 启动可视化界面
+        with mujoco.viewer.launch_passive(model, data) as viewer:
+            print("\n🎮 仿真已启动！按 Ctrl+C 退出")
+            print("💡 机械臂会自动缓慢运动，展示关节控制效果")
+
+            # 仿真循环
+            step = 0
+            while viewer.is_running():
+                # 控制频率：每20步更新一次关节目标
+                if step % 20 == 0:
+                    # 生成周期性的关节控制指令（让机械臂缓慢摆动）
+                    t = data.time
+                    target_angles = [
+                        0.2 * np.sin(t * 0.5),  # joint0: 基座旋转
+                        0.3 + 0.2 * np.sin(t),  # joint1: 肩部
+                        -0.6 + 0.2 * np.cos(t),  # joint2: 肘部
+                        0.1 * np.sin(t * 1.2),  # joint3: 前臂
+                        0.2 * np.cos(t * 0.8),  # joint4: 腕部1
+                        0.1 * np.sin(t * 1.5)  # joint5: 腕部2
+                    ]
+                    # 设置控制指令
+                    data.ctrl[:6] = target_angles
+
+                # 运行一步仿真
+                mujoco.mj_step(model, data)
+
+                # 更新可视化
+                viewer.sync()
+
+                # 修复：兼容不同版本的睡眠函数
+                try:
+                    # 尝试调用新版 MuJoCo 的 sleep 函数（归属到 utils）
+                    mujoco.utils.mju_sleep(1 / 60)
+                except AttributeError:
+                    try:
+                        # 尝试调用旧版 MuJoCo 的 sleep 函数（主模块）
+                        mujoco.mju_sleep(1 / 60)
+                    except AttributeError:
+                        # 终极备用：使用 Python 内置的 time.sleep
+                        time.sleep(1 / 60)
+
+                step += 1
+
+    except Exception as e:
+        print(f"❌ 仿真出错：{e}")
     finally:
-        print("\n🛬 无人机已悬停/降落")
\ No newline at end of file
+        # 删除临时XML文件
+        os.unlink(xml_path)
+
+
+if __name__ == "__main__":
+    # 检查MuJoCo版本
+    print(f"🔍 MuJoCo 版本：{mujoco.__version__}")
+
+    # 启动仿真
+    create_arm_simulation()
\ No newline at end of file

From 7e1c538a21b1fb70bbd8675bda90dfed1e9dbfcb Mon Sep 17 00:00:00 2001
From: XYTX0427 <481177497@qq.com>
Date: Mon, 22 Dec 2025 09:35:35 +0800
Subject: [PATCH 07/21] =?UTF-8?q?=E4=BC=98=E5=8C=96=E6=9C=BA=E6=A2=B0?=
 =?UTF-8?q?=E8=87=82=E7=9A=84=E8=BF=90=E5=8A=A8=E8=BD=A8=E8=BF=B9?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/Robotic_arm/main.py | 143 ++++++++++++++++++----------------------
 1 file changed, 63 insertions(+), 80 deletions(-)

diff --git a/src/Robotic_arm/main.py b/src/Robotic_arm/main.py
index 0529a86787..b7ee4eee6a 100644
--- a/src/Robotic_arm/main.py
+++ b/src/Robotic_arm/main.py
@@ -3,58 +3,56 @@
 import numpy as np
 import os
 import tempfile
-import time  # 新增：备用的睡眠函数
+import time
+from scipy import interpolate
 
-# ====================== 1. 定义机械臂 XML 模型 ======================
-# 6自由度机械臂的 MuJoCo XML 描述
+
+# ====================== 轨迹优化核心（纯原生） ======================
+def quintic_polynomial(start, end, T, t):
+    """五次多项式插值：保证位置/速度/加速度连续"""
+    s0, v0, a0 = start, 0, 0
+    s1, v1, a1 = end, 0, 0
+
+    a = s0
+    b = v0
+    c = a0 / 2
+    d = (20 * (s1 - s0) - (8 * v1 + 12 * v0) * T - (3 * a0 - a1) * T ** 2) / (2 * T ** 3)
+    e = (30 * (s0 - s1) + (14 * v1 + 16 * v0) * T + (3 * a0 - 2 * a1) * T ** 2) / (2 * T ** 4)
+    f = (12 * (s1 - s0) - (6 * v1 + 6 * v0) * T - (a0 - a1) * T ** 2) / (2 * T ** 5)
+
+    return a + b * t + c * t ** 2 + d * t ** 3 + e * t ** 4 + f * t ** 5
+
+
+# ====================== 机械臂模型 ======================
 arm_xml = """
 <mujoco model="6dof_arm">
   <compiler angle="radian" inertiafromgeom="true"/>
   <option timestep="0.005" gravity="0 0 -9.81"/>
-
-  <!-- 视觉和物理材质 -->
   <asset>
     <material name="gray" rgba="0.7 0.7 0.7 1"/>
     <material name="blue" rgba="0.2 0.4 0.8 1"/>
     <material name="red" rgba="0.8 0.2 0.2 1"/>
   </asset>
-
-  <!-- 世界体 -->
   <worldbody>
-    <!-- 地面 -->
     <geom name="floor" type="plane" size="5 5 0.1" pos="0 0 0" material="gray"/>
-
-    <!-- 机械臂基座 -->
     <body name="base" pos="0 0 0">
       <geom name="base_geom" type="cylinder" size="0.15 0.1" pos="0 0 0" material="gray"/>
       <joint name="joint0" type="hinge" axis="0 0 1" pos="0 0 0.1"/>
-
-      <!-- 连杆1 (肩部旋转) -->
       <body name="link1" pos="0 0 0.1">
         <geom name="link1_geom" type="capsule" size="0.05" fromto="0 0 0 0 0 0.3" material="blue"/>
         <joint name="joint1" type="hinge" axis="0 1 0" pos="0 0 0.3"/>
-
-        <!-- 连杆2 (肘部旋转) -->
         <body name="link2" pos="0 0 0.3">
           <geom name="link2_geom" type="capsule" size="0.05" fromto="0 0 0 0.4 0 0" material="blue"/>
           <joint name="joint2" type="hinge" axis="0 1 0" pos="0.4 0 0"/>
-
-          <!-- 连杆3 (前臂) -->
           <body name="link3" pos="0.4 0 0">
             <geom name="link3_geom" type="capsule" size="0.04" fromto="0 0 0 0.35 0 0" material="blue"/>
             <joint name="joint3" type="hinge" axis="1 0 0" pos="0.35 0 0"/>
-
-            <!-- 连杆4 (腕部旋转1) -->
             <body name="link4" pos="0.35 0 0">
               <geom name="link4_geom" type="capsule" size="0.04" fromto="0 0 0 0 0 0.25" material="blue"/>
               <joint name="joint4" type="hinge" axis="0 1 0" pos="0 0 0.25"/>
-
-              <!-- 连杆5 (腕部旋转2) -->
               <body name="link5" pos="0 0 0.25">
                 <geom name="link5_geom" type="capsule" size="0.03" fromto="0 0 0 0 0 0.2" material="blue"/>
                 <joint name="joint5" type="hinge" axis="1 0 0" pos="0 0 0.2"/>
-
-                <!-- 末端执行器 -->
                 <body name="end_effector" pos="0 0 0.2">
                   <geom name="ee_geom" type="box" size="0.08 0.08 0.08" pos="0 0 0" material="red"/>
                 </body>
@@ -65,8 +63,6 @@
       </body>
     </body>
   </worldbody>
-
-  <!-- 关节控制器 -->
   <actuator>
     <motor name="motor0" joint="joint0" ctrlrange="-3.14 3.14" gear="100"/>
     <motor name="motor1" joint="joint1" ctrlrange="-1.57 1.57" gear="100"/>
@@ -79,80 +75,67 @@
 """
 
 
-# ====================== 2. 模型加载和仿真控制 ======================
-def create_arm_simulation():
-    """创建并运行机械臂仿真"""
-    # 将XML字符串写入临时文件（MuJoCo需要文件路径加载）
+# ====================== 仿真运行 ======================
+def run_simulation():
+    # 临时XML文件
     with tempfile.NamedTemporaryFile(mode='w', suffix='.xml', delete=False) as f:
         f.write(arm_xml)
         xml_path = f.name
 
     try:
-        # 加载模型和数据
+        # 加载模型
         model = mujoco.MjModel.from_xml_path(xml_path)
         data = mujoco.MjData(model)
-
-        print("✅ 机械臂模型加载成功！")
-        print(f"🔧 关节数量：{model.njnt}")
-        print(f"🔧 执行器数量：{model.nu}")
-
-        # 设置初始关节角度
-        initial_joint_angles = [0, 0.2, -0.5, 0, 0.3, 0]
-        data.qpos[:6] = initial_joint_angles
-
-        # 启动可视化界面
+        print("✅ 模型加载成功！")
+
+        # 轨迹关键点（关节空间）
+        waypoints = [
+            [0, 0.2, -0.5, 0, 0.3, 0],
+            [0.5, 0.5, -0.8, 0.2, 0.5, 0.3],
+            [0.8, 0.3, -0.6, 0.4, 0.2, 0.5],
+            [0.5, 0.5, -0.8, 0.2, 0.5, 0.3],
+            [0, 0.2, -0.5, 0, 0.3, 0]
+        ]
+        segment_time = 3.0  # 每段轨迹时长
+
+        # 启动可视化
         with mujoco.viewer.launch_passive(model, data) as viewer:
-            print("\n🎮 仿真已启动！按 Ctrl+C 退出")
-            print("💡 机械臂会自动缓慢运动，展示关节控制效果")
-
-            # 仿真循环
-            step = 0
+            print("🎮 仿真启动！机械臂沿平滑轨迹运动（按Ctrl+C退出）")
             while viewer.is_running():
-                # 控制频率：每20步更新一次关节目标
-                if step % 20 == 0:
-                    # 生成周期性的关节控制指令（让机械臂缓慢摆动）
-                    t = data.time
-                    target_angles = [
-                        0.2 * np.sin(t * 0.5),  # joint0: 基座旋转
-                        0.3 + 0.2 * np.sin(t),  # joint1: 肩部
-                        -0.6 + 0.2 * np.cos(t),  # joint2: 肘部
-                        0.1 * np.sin(t * 1.2),  # joint3: 前臂
-                        0.2 * np.cos(t * 0.8),  # joint4: 腕部1
-                        0.1 * np.sin(t * 1.5)  # joint5: 腕部2
-                    ]
-                    # 设置控制指令
-                    data.ctrl[:6] = target_angles
-
-                # 运行一步仿真
+                # 计算当前轨迹段
+                t_total = data.time
+                seg_idx = int(t_total // segment_time) % (len(waypoints) - 1)
+                t_seg = t_total % segment_time
+
+                # 生成平滑轨迹
+                target = []
+                for i in range(6):
+                    pos = quintic_polynomial(
+                        waypoints[seg_idx][i],
+                        waypoints[seg_idx + 1][i],
+                        segment_time,
+                        t_seg
+                    )
+                    # 限制关节范围
+                    pos = np.clip(pos, model.actuator_ctrlrange[i][0], model.actuator_ctrlrange[i][1])
+                    target.append(pos)
+
+                # 控制机械臂
+                data.ctrl[:6] = target
                 mujoco.mj_step(model, data)
-
-                # 更新可视化
                 viewer.sync()
 
-                # 修复：兼容不同版本的睡眠函数
+                # 帧率控制
                 try:
-                    # 尝试调用新版 MuJoCo 的 sleep 函数（归属到 utils）
                     mujoco.utils.mju_sleep(1 / 60)
-                except AttributeError:
-                    try:
-                        # 尝试调用旧版 MuJoCo 的 sleep 函数（主模块）
-                        mujoco.mju_sleep(1 / 60)
-                    except AttributeError:
-                        # 终极备用：使用 Python 内置的 time.sleep
-                        time.sleep(1 / 60)
-
-                step += 1
+                except:
+                    time.sleep(1 / 60)
 
     except Exception as e:
-        print(f"❌ 仿真出错：{e}")
+        print(f"❌ 错误：{e}")
     finally:
-        # 删除临时XML文件
         os.unlink(xml_path)
 
 
 if __name__ == "__main__":
-    # 检查MuJoCo版本
-    print(f"🔍 MuJoCo 版本：{mujoco.__version__}")
-
-    # 启动仿真
-    create_arm_simulation()
\ No newline at end of file
+    run_simulation()  # 修复：删掉了多余的 + 号
\ No newline at end of file

From a186214a0ef62615251c8c331bc8d351fcf17837 Mon Sep 17 00:00:00 2001
From: XYTX0427 <481177497@qq.com>
Date: Mon, 22 Dec 2025 10:52:53 +0800
Subject: [PATCH 08/21] =?UTF-8?q?=E6=9C=BA=E6=A2=B0=E8=87=82=E8=BD=A8?=
 =?UTF-8?q?=E8=BF=B9=E7=9A=84=E7=89=A9=E7=90=86=E7=BA=A6=E6=9D=9F?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/Robotic_arm/main.py | 132 +++++++++++++++++++++++++++++++++-------
 1 file changed, 109 insertions(+), 23 deletions(-)

diff --git a/src/Robotic_arm/main.py b/src/Robotic_arm/main.py
index b7ee4eee6a..71db2161ac 100644
--- a/src/Robotic_arm/main.py
+++ b/src/Robotic_arm/main.py
@@ -6,13 +6,33 @@
 import time
 from scipy import interpolate
 
-
-# ====================== 轨迹优化核心（纯原生） ======================
-def quintic_polynomial(start, end, T, t):
-    """五次多项式插值：保证位置/速度/加速度连续"""
+# ====================== 1. 定义机械臂物理约束参数（核心） ======================
+# 参考UR5机械臂参数，可根据实际硬件调整
+CONSTRAINTS = {
+    "max_vel": [1.0, 0.8, 0.8, 1.2, 0.9, 1.2],  # 各关节最大角速度 (rad/s)
+    "max_acc": [0.5, 0.4, 0.4, 0.6, 0.5, 0.6],  # 各关节最大角加速度 (rad/s²)
+    "max_jerk": [0.3, 0.2, 0.2, 0.4, 0.3, 0.4],  # 各关节最大加加速度 (rad/s³)
+    "ctrl_limit": [-10.0, 10.0]  # 电机控制量限制
+}
+
+
+# ====================== 2. 带约束的五次多项式轨迹生成 ======================
+def constrained_quintic_polynomial(start, end, total_time, t, joint_idx):
+    """
+    带约束的五次多项式插值
+    :param start: 起点角度
+    :param end: 终点角度
+    :param total_time: 轨迹段总时间
+    :param t: 当前段内时间 (0<=t<=total_time)
+    :param joint_idx: 关节索引（0-5）
+    :return: 约束后的位置、速度、加速度
+    """
+    # 基础边界条件（启停时速度/加速度为0）
     s0, v0, a0 = start, 0, 0
     s1, v1, a1 = end, 0, 0
 
+    T = total_time
+    # 五次多项式系数计算
     a = s0
     b = v0
     c = a0 / 2
@@ -20,10 +40,53 @@ def quintic_polynomial(start, end, T, t):
     e = (30 * (s0 - s1) + (14 * v1 + 16 * v0) * T + (3 * a0 - 2 * a1) * T ** 2) / (2 * T ** 4)
     f = (12 * (s1 - s0) - (6 * v1 + 6 * v0) * T - (a0 - a1) * T ** 2) / (2 * T ** 5)
 
-    return a + b * t + c * t ** 2 + d * t ** 3 + e * t ** 4 + f * t ** 5
+    # 计算原始位置、速度、加速度、加加速度
+    pos = a + b * t + c * t ** 2 + d * t ** 3 + e * t ** 4 + f * t ** 5
+    vel = b + 2 * c * t + 3 * d * t ** 2 + 4 * e * t ** 3 + 5 * f * t ** 4
+    acc = 2 * c + 6 * d * t + 12 * e * t ** 2 + 20 * f * t ** 3
+    jerk = 6 * d + 24 * e * t + 60 * f * t ** 2
+
+    # 应用约束（核心：超出则截断）
+    max_vel = CONSTRAINTS["max_vel"][joint_idx]
+    max_acc = CONSTRAINTS["max_acc"][joint_idx]
+    max_jerk = CONSTRAINTS["max_jerk"][joint_idx]
+
+    vel = np.clip(vel, -max_vel, max_vel)
+    acc = np.clip(acc, -max_acc, max_acc)
+    jerk = np.clip(jerk, -max_jerk, max_jerk)
+
+    # 可选：如果速度/加速度被截断，反向修正位置（更严谨）
+    # 这里简化处理，直接返回约束后的位置（基础场景足够）
+    return pos, vel, acc
+
+
+# ====================== 3. 闭环约束控制（实时修正） ======================
+def closed_loop_constraint_control(data, target_joints, joint_idx):
+    """
+    闭环PD控制 + 约束检查，实时修正控制指令
+    """
+    # PD控制参数（可根据实际机械臂标定）
+    k_p = 8.0  # 比例系数
+    k_d = 0.2  # 微分系数
+
+    # 获取当前关节状态（仿真中读取，实际为编码器数据）
+    current_pos = data.qpos[joint_idx]
+    current_vel = data.qvel[joint_idx]
+
+    # 计算误差
+    pos_error = target_joints[joint_idx] - current_pos
+    vel_error = -current_vel  # 速度误差：目标速度为0（启停阶段）
 
+    # 计算原始控制量
+    ctrl = k_p * pos_error + k_d * vel_error
 
-# ====================== 机械臂模型 ======================
+    # 约束控制量（避免电机过载）
+    ctrl = np.clip(ctrl, CONSTRAINTS["ctrl_limit"][0], CONSTRAINTS["ctrl_limit"][1])
+
+    return ctrl
+
+
+# ====================== 4. 机械臂模型（不变） ======================
 arm_xml = """
 <mujoco model="6dof_arm">
   <compiler angle="radian" inertiafromgeom="true"/>
@@ -75,18 +138,21 @@ def quintic_polynomial(start, end, T, t):
 """
 
 
-# ====================== 仿真运行 ======================
-def run_simulation():
+# ====================== 5. 带约束的仿真主逻辑 ======================
+def run_constrained_simulation():
     # 临时XML文件
     with tempfile.NamedTemporaryFile(mode='w', suffix='.xml', delete=False) as f:
         f.write(arm_xml)
         xml_path = f.name
 
     try:
-        # 加载模型
+        # 加载模型和数据
         model = mujoco.MjModel.from_xml_path(xml_path)
         data = mujoco.MjData(model)
-        print("✅ 模型加载成功！")
+        print("✅ 带约束的机械臂模型加载成功！")
+        print("🔧 约束参数：")
+        print(f"   最大关节速度：{CONSTRAINTS['max_vel']} rad/s")
+        print(f"   最大关节加速度：{CONSTRAINTS['max_acc']} rad/s²")
 
         # 轨迹关键点（关节空间）
         waypoints = [
@@ -100,42 +166,62 @@ def run_simulation():
 
         # 启动可视化
         with mujoco.viewer.launch_passive(model, data) as viewer:
-            print("🎮 仿真启动！机械臂沿平滑轨迹运动（按Ctrl+C退出）")
+            print("\n🎮 带约束的机械臂仿真启动！")
+            print("💡 特征：速度/加速度/控制量约束 + 闭环PD控制")
+            print("💡 按 Ctrl+C 退出")
+
             while viewer.is_running():
-                # 计算当前轨迹段
+                # 1. 计算当前轨迹段
                 t_total = data.time
                 seg_idx = int(t_total // segment_time) % (len(waypoints) - 1)
                 t_seg = t_total % segment_time
 
-                # 生成平滑轨迹
-                target = []
+                # 2. 生成带约束的目标关节角度
+                target_joints = []
+                joint_vels = []  # 记录约束后的速度（用于调试）
                 for i in range(6):
-                    pos = quintic_polynomial(
+                    pos, vel, acc = constrained_quintic_polynomial(
                         waypoints[seg_idx][i],
                         waypoints[seg_idx + 1][i],
                         segment_time,
-                        t_seg
+                        t_seg,
+                        i
                     )
-                    # 限制关节范围
+                    # 额外约束关节角度在可控范围
                     pos = np.clip(pos, model.actuator_ctrlrange[i][0], model.actuator_ctrlrange[i][1])
-                    target.append(pos)
+                    target_joints.append(pos)
+                    joint_vels.append(vel)
+
+                # 3. 闭环约束控制：修正控制指令
+                ctrl_signals = []
+                for i in range(6):
+                    ctrl = closed_loop_constraint_control(data, target_joints, i)
+                    ctrl_signals.append(ctrl)
+
+                # 4. 应用控制指令
+                data.ctrl[:6] = ctrl_signals
+
+                # 5. 打印关键状态（每50步打印一次，方便调试）
+                if int(data.time * 100) % 50 == 0:
+                    print(f"\n⏱️  时间：{data.time:.2f}s")
+                    print(f"   关节0当前速度：{data.qvel[0]:.3f} rad/s (约束上限：{CONSTRAINTS['max_vel'][0]})")
+                    print(f"   关节0控制量：{ctrl_signals[0]:.3f} (约束范围：{CONSTRAINTS['ctrl_limit']})")
 
-                # 控制机械臂
-                data.ctrl[:6] = target
+                # 6. 运行仿真步
                 mujoco.mj_step(model, data)
                 viewer.sync()
 
-                # 帧率控制
+                # 7. 帧率控制
                 try:
                     mujoco.utils.mju_sleep(1 / 60)
                 except:
                     time.sleep(1 / 60)
 
     except Exception as e:
-        print(f"❌ 错误：{e}")
+        print(f"❌ 仿真出错：{e}")
     finally:
         os.unlink(xml_path)
 
 
 if __name__ == "__main__":
-    run_simulation()  # 修复：删掉了多余的 + 号
\ No newline at end of file
+    run_constrained_simulation()
\ No newline at end of file

From cb3f4511ea57c3abc6c672f420a177e3d693c018 Mon Sep 17 00:00:00 2001
From: XYTX0427 <481177497@qq.com>
Date: Mon, 22 Dec 2025 11:03:37 +0800
Subject: [PATCH 09/21] =?UTF-8?q?=E6=9C=BA=E6=A2=B0=E8=87=82=E7=9A=84?=
 =?UTF-8?q?=E8=90=BD=E5=9C=B0=E9=81=BF=E5=BC=80=E9=9A=9C=E7=A2=8D?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/Robotic_arm/main.py | 284 ++++++++++++++++++++++++++--------------
 1 file changed, 185 insertions(+), 99 deletions(-)

diff --git a/src/Robotic_arm/main.py b/src/Robotic_arm/main.py
index 71db2161ac..3ea2964733 100644
--- a/src/Robotic_arm/main.py
+++ b/src/Robotic_arm/main.py
@@ -6,33 +6,39 @@
 import time
 from scipy import interpolate
 
-# ====================== 1. 定义机械臂物理约束参数（核心） ======================
-# 参考UR5机械臂参数，可根据实际硬件调整
+# ====================== 1. 全局配置（物理约束+避障参数） ======================
 CONSTRAINTS = {
-    "max_vel": [1.0, 0.8, 0.8, 1.2, 0.9, 1.2],  # 各关节最大角速度 (rad/s)
-    "max_acc": [0.5, 0.4, 0.4, 0.6, 0.5, 0.6],  # 各关节最大角加速度 (rad/s²)
-    "max_jerk": [0.3, 0.2, 0.2, 0.4, 0.3, 0.4],  # 各关节最大加加速度 (rad/s³)
-    "ctrl_limit": [-10.0, 10.0]  # 电机控制量限制
+    "max_vel": [1.0, 0.8, 0.8, 1.2, 0.9, 1.2],
+    "max_acc": [0.5, 0.4, 0.4, 0.6, 0.5, 0.6],
+    "max_jerk": [0.3, 0.2, 0.2, 0.4, 0.3, 0.4],
+    "ctrl_limit": [-10.0, 10.0]
 }
 
+OBSTACLE_CONFIG = {
+    "k_att": 0.8,
+    "k_rep": 0.6,
+    "rep_radius": 0.3,
+    "obstacle_list": [
+        [0.6, 0.1, 0.5, 0.1],
+        [0.4, -0.1, 0.6, 0.08]
+    ]
+}
+
+CART_WAYPOINTS = [
+    [0.5, 0.0, 0.6],
+    [0.7, 0.2, 0.7],
+    [0.8, 0.1, 0.8],
+    [0.7, 0.2, 0.7],
+    [0.5, 0.0, 0.6]
+]
+
 
-# ====================== 2. 带约束的五次多项式轨迹生成 ======================
+# ====================== 2. 物理约束轨迹生成 ======================
 def constrained_quintic_polynomial(start, end, total_time, t, joint_idx):
-    """
-    带约束的五次多项式插值
-    :param start: 起点角度
-    :param end: 终点角度
-    :param total_time: 轨迹段总时间
-    :param t: 当前段内时间 (0<=t<=total_time)
-    :param joint_idx: 关节索引（0-5）
-    :return: 约束后的位置、速度、加速度
-    """
-    # 基础边界条件（启停时速度/加速度为0）
     s0, v0, a0 = start, 0, 0
     s1, v1, a1 = end, 0, 0
 
     T = total_time
-    # 五次多项式系数计算
     a = s0
     b = v0
     c = a0 / 2
@@ -40,61 +46,111 @@ def constrained_quintic_polynomial(start, end, total_time, t, joint_idx):
     e = (30 * (s0 - s1) + (14 * v1 + 16 * v0) * T + (3 * a0 - 2 * a1) * T ** 2) / (2 * T ** 4)
     f = (12 * (s1 - s0) - (6 * v1 + 6 * v0) * T - (a0 - a1) * T ** 2) / (2 * T ** 5)
 
-    # 计算原始位置、速度、加速度、加加速度
     pos = a + b * t + c * t ** 2 + d * t ** 3 + e * t ** 4 + f * t ** 5
     vel = b + 2 * c * t + 3 * d * t ** 2 + 4 * e * t ** 3 + 5 * f * t ** 4
     acc = 2 * c + 6 * d * t + 12 * e * t ** 2 + 20 * f * t ** 3
-    jerk = 6 * d + 24 * e * t + 60 * f * t ** 2
 
-    # 应用约束（核心：超出则截断）
-    max_vel = CONSTRAINTS["max_vel"][joint_idx]
-    max_acc = CONSTRAINTS["max_acc"][joint_idx]
-    max_jerk = CONSTRAINTS["max_jerk"][joint_idx]
+    vel = np.clip(vel, -CONSTRAINTS["max_vel"][joint_idx], CONSTRAINTS["max_vel"][joint_idx])
+    acc = np.clip(acc, -CONSTRAINTS["max_acc"][joint_idx], CONSTRAINTS["max_acc"][joint_idx])
 
-    vel = np.clip(vel, -max_vel, max_vel)
-    acc = np.clip(acc, -max_acc, max_acc)
-    jerk = np.clip(jerk, -max_jerk, max_jerk)
-
-    # 可选：如果速度/加速度被截断，反向修正位置（更严谨）
-    # 这里简化处理，直接返回约束后的位置（基础场景足够）
     return pos, vel, acc
 
 
-# ====================== 3. 闭环约束控制（实时修正） ======================
+# ====================== 3. 闭环PD控制 ======================
 def closed_loop_constraint_control(data, target_joints, joint_idx):
-    """
-    闭环PD控制 + 约束检查，实时修正控制指令
-    """
-    # PD控制参数（可根据实际机械臂标定）
-    k_p = 8.0  # 比例系数
-    k_d = 0.2  # 微分系数
+    k_p = 8.0
+    k_d = 0.2
 
-    # 获取当前关节状态（仿真中读取，实际为编码器数据）
     current_pos = data.qpos[joint_idx]
     current_vel = data.qvel[joint_idx]
 
-    # 计算误差
     pos_error = target_joints[joint_idx] - current_pos
-    vel_error = -current_vel  # 速度误差：目标速度为0（启停阶段）
+    vel_error = -current_vel
 
-    # 计算原始控制量
     ctrl = k_p * pos_error + k_d * vel_error
-
-    # 约束控制量（避免电机过载）
     ctrl = np.clip(ctrl, CONSTRAINTS["ctrl_limit"][0], CONSTRAINTS["ctrl_limit"][1])
 
     return ctrl
 
 
-# ====================== 4. 机械臂模型（不变） ======================
-arm_xml = """
-<mujoco model="6dof_arm">
+# ====================== 4. 人工势场法避障 ======================
+def artificial_potential_field(ee_pos, target_pos):
+    ee_pos = np.array(ee_pos)
+    target_pos = np.array(target_pos)
+    obstacle_list = OBSTACLE_CONFIG["obstacle_list"]
+    k_att = OBSTACLE_CONFIG["k_att"]
+    k_rep = OBSTACLE_CONFIG["k_rep"]
+    rep_radius = OBSTACLE_CONFIG["rep_radius"]
+
+    # 引力
+    att_force = k_att * (target_pos - ee_pos)
+
+    # 斥力
+    rep_force = np.zeros(3)
+    for obs in obstacle_list:
+        obs_pos = np.array(obs[:3])
+        obs_radius = obs[3]
+        dist = np.linalg.norm(ee_pos - obs_pos)
+
+        if dist < rep_radius + obs_radius:
+            if dist < 1e-6:
+                dist = 1e-6
+            rep_dir = (ee_pos - obs_pos) / dist
+            rep_force += k_rep * (1 / (dist - obs_radius) - 1 / rep_radius) * (1 / dist ** 2) * rep_dir
+
+    # 修正目标位置
+    corrected_target = ee_pos + att_force + rep_force
+    corrected_target = np.clip(corrected_target, [0.3, -0.4, 0.2], [0.9, 0.4, 1.0])
+
+    return corrected_target.tolist()
+
+
+# ====================== 5. 笛卡尔轨迹平滑 ======================
+def smooth_cartesian_trajectory(waypoints, num_points=200):
+    x = np.array([p[0] for p in waypoints])
+    y = np.array([p[1] for p in waypoints])
+    z = np.array([p[2] for p in waypoints])
+
+    t = np.linspace(0, 1, len(x))
+    t_new = np.linspace(0, 1, num_points)
+
+    spline_x = interpolate.CubicSpline(t, x, bc_type='natural')
+    spline_y = interpolate.CubicSpline(t, y, bc_type='natural')
+    spline_z = interpolate.CubicSpline(t, z, bc_type='natural')
+
+    return np.vstack((spline_x(t_new), spline_y(t_new), spline_z(t_new))).T
+
+
+# ====================== 6. 替代逆运动学：关节轨迹预生成 ======================
+def precompute_joint_waypoints(model, data, cart_waypoints):
+    """预计算笛卡尔轨迹对应的关节角度（兼容旧版MuJoCo）"""
+    joint_waypoints = []
+    ee_site_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "ee_site")
+
+    for cart_pos in cart_waypoints:
+        # 重置数据
+        mujoco.mj_resetData(model, data)
+        # 设置目标位置到data中（旧版mj_inverse的要求）
+        data.site_xpos[ee_site_id] = cart_pos
+        # 调用逆运动学（仅model和data参数）
+        mujoco.mj_inverse(model, data)
+        # 保存关节角度
+        joint_waypoints.append(data.qpos[:6].copy())
+
+    return joint_waypoints
+
+
+# ====================== 7. 机械臂模型（带障碍物） ======================
+def get_arm_xml_with_obstacles():
+    arm_xml = """
+<mujoco model="6dof_arm_with_obstacles">
   <compiler angle="radian" inertiafromgeom="true"/>
   <option timestep="0.005" gravity="0 0 -9.81"/>
   <asset>
     <material name="gray" rgba="0.7 0.7 0.7 1"/>
     <material name="blue" rgba="0.2 0.4 0.8 1"/>
     <material name="red" rgba="0.8 0.2 0.2 1"/>
+    <material name="obstacle" rgba="1 0 0 0.5"/>
   </asset>
   <worldbody>
     <geom name="floor" type="plane" size="5 5 0.1" pos="0 0 0" material="gray"/>
@@ -118,6 +174,7 @@ def closed_loop_constraint_control(data, target_joints, joint_idx):
                 <joint name="joint5" type="hinge" axis="1 0 0" pos="0 0 0.2"/>
                 <body name="end_effector" pos="0 0 0.2">
                   <geom name="ee_geom" type="box" size="0.08 0.08 0.08" pos="0 0 0" material="red"/>
+                  <site name="ee_site" pos="0 0 0" type="sphere" size="0.01" rgba="1 0 0 1"/>
                 </body>
               </body>
             </body>
@@ -125,6 +182,15 @@ def closed_loop_constraint_control(data, target_joints, joint_idx):
         </body>
       </body>
     </body>
+    """
+
+    for i, obs in enumerate(OBSTACLE_CONFIG["obstacle_list"]):
+        x, y, z, r = obs
+        arm_xml += f"""
+    <geom name="obstacle_{i}" type="sphere" size="{r}" pos="{x} {y} {z}" material="obstacle"/>
+        """
+
+    arm_xml += """
   </worldbody>
   <actuator>
     <motor name="motor0" joint="joint0" ctrlrange="-3.14 3.14" gear="100"/>
@@ -135,83 +201,101 @@ def closed_loop_constraint_control(data, target_joints, joint_idx):
     <motor name="motor5" joint="joint5" ctrlrange="-3.14 3.14" gear="100"/>
   </actuator>
 </mujoco>
-"""
+    """
+    return arm_xml
 
 
-# ====================== 5. 带约束的仿真主逻辑 ======================
-def run_constrained_simulation():
-    # 临时XML文件
+# ====================== 8. 主仿真逻辑（修复逆运动学调用） ======================
+def run_obstacle_avoidance_simulation():
+    arm_xml = get_arm_xml_with_obstacles()
     with tempfile.NamedTemporaryFile(mode='w', suffix='.xml', delete=False) as f:
         f.write(arm_xml)
         xml_path = f.name
 
     try:
-        # 加载模型和数据
         model = mujoco.MjModel.from_xml_path(xml_path)
         data = mujoco.MjData(model)
-        print("✅ 带约束的机械臂模型加载成功！")
-        print("🔧 约束参数：")
-        print(f"   最大关节速度：{CONSTRAINTS['max_vel']} rad/s")
-        print(f"   最大关节加速度：{CONSTRAINTS['max_acc']} rad/s²")
-
-        # 轨迹关键点（关节空间）
-        waypoints = [
-            [0, 0.2, -0.5, 0, 0.3, 0],
-            [0.5, 0.5, -0.8, 0.2, 0.5, 0.3],
-            [0.8, 0.3, -0.6, 0.4, 0.2, 0.5],
-            [0.5, 0.5, -0.8, 0.2, 0.5, 0.3],
-            [0, 0.2, -0.5, 0, 0.3, 0]
-        ]
-        segment_time = 3.0  # 每段轨迹时长
-
-        # 启动可视化
+        print("✅ 带避障的机械臂模型加载成功！")
+        print(
+            f"🔧 避障参数：斥力半径={OBSTACLE_CONFIG['rep_radius']}m，障碍物数量={len(OBSTACLE_CONFIG['obstacle_list'])}")
+
+        # 预计算关节轨迹关键点（兼容旧版MuJoCo）
+        joint_waypoints = precompute_joint_waypoints(model, data, CART_WAYPOINTS)
+        # 平滑关节轨迹
+        num_joint_points = 200
+        smooth_joint_traj = []
+        for joint_idx in range(6):
+            joint_vals = [wp[joint_idx] for wp in joint_waypoints]
+            t = np.linspace(0, 1, len(joint_vals))
+            t_new = np.linspace(0, 1, num_joint_points)
+            spline = interpolate.CubicSpline(t, joint_vals, bc_type='natural')
+            smooth_joint_traj.append(spline(t_new))
+        # 转置为 [轨迹点, 关节]
+        smooth_joint_traj = np.array(smooth_joint_traj).T
+
+        traj_length = len(smooth_joint_traj)
+        ee_site_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "ee_site")
+        segment_time = 5.0
+
         with mujoco.viewer.launch_passive(model, data) as viewer:
-            print("\n🎮 带约束的机械臂仿真启动！")
-            print("💡 特征：速度/加速度/控制量约束 + 闭环PD控制")
+            print("\n🎮 机械臂避障仿真启动！")
+            print("💡 核心功能：人工势场法避障 + 物理约束 + PD闭环控制")
+            print("💡 可视化：红色半透明球体为障碍物，机械臂自动绕开")
             print("💡 按 Ctrl+C 退出")
 
             while viewer.is_running():
-                # 1. 计算当前轨迹段
+                # 1. 计算当前轨迹索引
                 t_total = data.time
-                seg_idx = int(t_total // segment_time) % (len(waypoints) - 1)
-                t_seg = t_total % segment_time
-
-                # 2. 生成带约束的目标关节角度
-                target_joints = []
-                joint_vels = []  # 记录约束后的速度（用于调试）
-                for i in range(6):
-                    pos, vel, acc = constrained_quintic_polynomial(
-                        waypoints[seg_idx][i],
-                        waypoints[seg_idx + 1][i],
-                        segment_time,
-                        t_seg,
-                        i
-                    )
-                    # 额外约束关节角度在可控范围
-                    pos = np.clip(pos, model.actuator_ctrlrange[i][0], model.actuator_ctrlrange[i][1])
-                    target_joints.append(pos)
-                    joint_vels.append(vel)
-
-                # 3. 闭环约束控制：修正控制指令
+                traj_idx = int((t_total / segment_time) * traj_length) % traj_length
+
+                # 2. 获取末端当前位置
+                ee_pos = data.site_xpos[ee_site_id].tolist()
+
+                # 3. 原始关节目标
+                raw_joint_target = smooth_joint_traj[traj_idx]
+
+                # 4. 笛卡尔空间避障修正（核心）
+                # 先通过正运动学获取原始笛卡尔目标
+                mujoco.mj_forward(model, data)  # 更新正运动学
+                raw_cart_target = data.site_xpos[ee_site_id].copy()
+                # 避障修正笛卡尔目标
+                corrected_cart_target = artificial_potential_field(ee_pos, raw_cart_target)
+                # 修正后的笛卡尔目标转关节目标（兼容旧版）
+                data.site_xpos[ee_site_id] = corrected_cart_target
+                mujoco.mj_inverse(model, data)
+                target_joints = data.qpos[:6].copy()
+
+                # 5. 应用物理约束 + 闭环控制
                 ctrl_signals = []
                 for i in range(6):
+                    # 约束关节角度
+                    target_joints[i] = np.clip(target_joints[i], model.actuator_ctrlrange[i][0],
+                                               model.actuator_ctrlrange[i][1])
+                    # 闭环PD控制
                     ctrl = closed_loop_constraint_control(data, target_joints, i)
                     ctrl_signals.append(ctrl)
 
-                # 4. 应用控制指令
+                # 6. 发送控制指令
                 data.ctrl[:6] = ctrl_signals
 
-                # 5. 打印关键状态（每50步打印一次，方便调试）
-                if int(data.time * 100) % 50 == 0:
-                    print(f"\n⏱️  时间：{data.time:.2f}s")
-                    print(f"   关节0当前速度：{data.qvel[0]:.3f} rad/s (约束上限：{CONSTRAINTS['max_vel'][0]})")
-                    print(f"   关节0控制量：{ctrl_signals[0]:.3f} (约束范围：{CONSTRAINTS['ctrl_limit']})")
+                # 7. 打印状态
+                if int(t_total) % 1 == 0 and int(t_total) != 0:
+                    obs_distances = []
+                    for obs in OBSTACLE_CONFIG["obstacle_list"]:
+                        dist = np.linalg.norm(np.array(ee_pos) - np.array(obs[:3]))
+                        obs_distances.append(dist)
+                    min_obs_dist = min(obs_distances) if obs_distances else 0
+
+                    print(f"\n⏱️  时间：{t_total:.2f}s")
+                    print(f"   末端当前位置：{np.round(ee_pos, 3)}")
+                    print(f"   修正后目标位置：{np.round(corrected_cart_target, 3)}")
+                    print(f"   与最近障碍距离：{min_obs_dist:.3f}m (斥力半径：{OBSTACLE_CONFIG['rep_radius']}m)")
 
-                # 6. 运行仿真步
+                # 8. 运行仿真步
                 mujoco.mj_step(model, data)
                 viewer.sync()
 
-                # 7. 帧率控制
+                # 9. 帧率控制
                 try:
                     mujoco.utils.mju_sleep(1 / 60)
                 except:
@@ -219,9 +303,11 @@ def run_constrained_simulation():
 
     except Exception as e:
         print(f"❌ 仿真出错：{e}")
+        import traceback
+        traceback.print_exc()
     finally:
         os.unlink(xml_path)
 
 
 if __name__ == "__main__":
-    run_constrained_simulation()
\ No newline at end of file
+    run_obstacle_avoidance_simulation()
\ No newline at end of file

From d6b3d3754ab8f4530bae599ef3d2c0190cd7680c Mon Sep 17 00:00:00 2001
From: XYTX0427 <481177497@qq.com>
Date: Mon, 22 Dec 2025 17:57:09 +0800
Subject: [PATCH 10/21] =?UTF-8?q?=E7=BA=A6=E6=9D=9F=E4=B8=8E=E9=81=BF?=
 =?UTF-8?q?=E9=9A=9C=E4=B8=8B=E6=97=B6=E9=97=B4=E6=9C=80=E4=BC=98?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/Robotic_arm/main.py | 340 ++++++++++++++++++++++++----------------
 1 file changed, 209 insertions(+), 131 deletions(-)

diff --git a/src/Robotic_arm/main.py b/src/Robotic_arm/main.py
index 3ea2964733..4b9677fbe0 100644
--- a/src/Robotic_arm/main.py
+++ b/src/Robotic_arm/main.py
@@ -6,74 +6,153 @@
 import time
 from scipy import interpolate
 
-# ====================== 1. 全局配置（物理约束+避障参数） ======================
+# ====================== 1. 全局配置（物理约束+避障+时间最优参数） ======================
+# 机械臂物理约束（UR5参考参数）
 CONSTRAINTS = {
-    "max_vel": [1.0, 0.8, 0.8, 1.2, 0.9, 1.2],
-    "max_acc": [0.5, 0.4, 0.4, 0.6, 0.5, 0.6],
-    "max_jerk": [0.3, 0.2, 0.2, 0.4, 0.3, 0.4],
-    "ctrl_limit": [-10.0, 10.0]
+    "max_vel": [1.0, 0.8, 0.8, 1.2, 0.9, 1.2],  # 各关节最大角速度 (rad/s)
+    "max_acc": [0.5, 0.4, 0.4, 0.6, 0.5, 0.6],  # 各关节最大角加速度 (rad/s²)
+    "max_jerk": [0.3, 0.2, 0.2, 0.4, 0.3, 0.4],  # 最大加加速度 (rad/s³)
+    "ctrl_limit": [-10.0, 10.0]  # 电机控制量限制
 }
 
+# 避障参数
 OBSTACLE_CONFIG = {
-    "k_att": 0.8,
-    "k_rep": 0.6,
-    "rep_radius": 0.3,
-    "obstacle_list": [
+    "k_att": 0.8,  # 引力系数
+    "k_rep": 0.6,  # 斥力系数
+    "rep_radius": 0.3,  # 斥力作用半径
+    "obstacle_list": [  # 障碍物：[x,y,z,半径]
         [0.6, 0.1, 0.5, 0.1],
         [0.4, -0.1, 0.6, 0.08]
     ]
 }
 
+# 笛卡尔轨迹关键点
 CART_WAYPOINTS = [
-    [0.5, 0.0, 0.6],
-    [0.7, 0.2, 0.7],
-    [0.8, 0.1, 0.8],
-    [0.7, 0.2, 0.7],
-    [0.5, 0.0, 0.6]
+    [0.5, 0.0, 0.6],  # 起点
+    [0.7, 0.2, 0.7],  # 中间点（障碍区）
+    [0.8, 0.1, 0.8],  # 终点
+    [0.7, 0.2, 0.7],  # 回中间点
+    [0.5, 0.0, 0.6]  # 回起点
 ]
 
 
-# ====================== 2. 物理约束轨迹生成 ======================
-def constrained_quintic_polynomial(start, end, total_time, t, joint_idx):
-    s0, v0, a0 = start, 0, 0
-    s1, v1, a1 = end, 0, 0
-
-    T = total_time
-    a = s0
-    b = v0
-    c = a0 / 2
-    d = (20 * (s1 - s0) - (8 * v1 + 12 * v0) * T - (3 * a0 - a1) * T ** 2) / (2 * T ** 3)
-    e = (30 * (s0 - s1) + (14 * v1 + 16 * v0) * T + (3 * a0 - 2 * a1) * T ** 2) / (2 * T ** 4)
-    f = (12 * (s1 - s0) - (6 * v1 + 6 * v0) * T - (a0 - a1) * T ** 2) / (2 * T ** 5)
-
-    pos = a + b * t + c * t ** 2 + d * t ** 3 + e * t ** 4 + f * t ** 5
-    vel = b + 2 * c * t + 3 * d * t ** 2 + 4 * e * t ** 3 + 5 * f * t ** 4
-    acc = 2 * c + 6 * d * t + 12 * e * t ** 2 + 20 * f * t ** 3
-
-    vel = np.clip(vel, -CONSTRAINTS["max_vel"][joint_idx], CONSTRAINTS["max_vel"][joint_idx])
-    acc = np.clip(acc, -CONSTRAINTS["max_acc"][joint_idx], CONSTRAINTS["max_acc"][joint_idx])
-
-    return pos, vel, acc
-
-
-# ====================== 3. 闭环PD控制 ======================
-def closed_loop_constraint_control(data, target_joints, joint_idx):
-    k_p = 8.0
-    k_d = 0.2
-
-    current_pos = data.qpos[joint_idx]
-    current_vel = data.qvel[joint_idx]
-
-    pos_error = target_joints[joint_idx] - current_pos
-    vel_error = -current_vel
-
-    ctrl = k_p * pos_error + k_d * vel_error
-    ctrl = np.clip(ctrl, CONSTRAINTS["ctrl_limit"][0], CONSTRAINTS["ctrl_limit"][1])
-
-    return ctrl
-
-
-# ====================== 4. 人工势场法避障 ======================
+# ====================== 2. 核心：时间最优轨迹生成（梯形速度曲线） ======================
+def time_optimal_trajectory(start, end, joint_idx):
+    """
+    生成单个关节的时间最优轨迹（梯形速度曲线）
+    :param start: 起点角度 (rad)
+    :param end: 终点角度 (rad)
+    :param joint_idx: 关节索引（0-5）
+    :return: 最优运动时间 + 轨迹点数组 [时间, 位置, 速度, 加速度]
+    """
+    max_vel = CONSTRAINTS["max_vel"][joint_idx]
+    max_acc = CONSTRAINTS["max_acc"][joint_idx]
+    delta_pos = end - start  # 位置差
+    sign = np.sign(delta_pos)  # 运动方向
+
+    # 步骤1：计算达到最大速度所需的加速时间和位移
+    t_acc = max_vel / max_acc  # 加速到最大速度的时间
+    s_acc = 0.5 * max_acc * t_acc ** 2  # 加速阶段位移
+
+    # 步骤2：判断是否能达到最大速度（决定是梯形/三角形速度曲线）
+    if abs(delta_pos) < 2 * s_acc:
+        # 位移太小，无法匀速（三角形曲线）
+        t_acc = np.sqrt(abs(delta_pos) / max_acc)
+        t_const = 0  # 无匀速阶段
+        total_time = 2 * t_acc
+    else:
+        # 能达到最大速度（梯形曲线）
+        t_const = (abs(delta_pos) - 2 * s_acc) / max_vel  # 匀速时间
+        total_time = 2 * t_acc + t_const
+
+    # 步骤3：生成离散轨迹点（1ms步长，保证精度）
+    dt = 0.001
+    time_list = np.arange(0, total_time + dt, dt)
+    pos_list = []
+    vel_list = []
+    acc_list = []
+
+    for t in time_list:
+        if t < t_acc:
+            # 加速阶段
+            pos = start + sign * 0.5 * max_acc * t ** 2
+            vel = sign * max_acc * t
+            acc = sign * max_acc
+        elif t < t_acc + t_const:
+            # 匀速阶段
+            pos = start + sign * (s_acc + max_vel * (t - t_acc))
+            vel = sign * max_vel
+            acc = 0
+        else:
+            # 减速阶段
+            t_dec = t - (t_acc + t_const)
+            pos = end - sign * 0.5 * max_acc * t_dec ** 2
+            vel = sign * (max_vel - max_acc * t_dec)
+            acc = -sign * max_acc
+
+        pos_list.append(pos)
+        vel_list.append(vel)
+        acc_list.append(acc)
+
+    # 封装轨迹数据
+    traj_data = np.vstack((time_list, pos_list, vel_list, acc_list)).T
+    return total_time, traj_data
+
+
+# ====================== 3. 多关节时间最优轨迹同步 ======================
+def sync_joint_trajectories(joint_waypoints):
+    """
+    同步所有关节的时间最优轨迹（保证同时到达目标点）
+    :param joint_waypoints: 关节轨迹关键点 [[j0,j1,...j5], ...]
+    :return: 全局时间最优轨迹数组 [时间, j0_pos, j1_pos, ..., j5_pos]
+    """
+    num_joints = 6
+    segment_trajs = []  # 存储每段轨迹的各关节数据
+
+    # 遍历每段轨迹（关键点之间的段）
+    for seg_idx in range(len(joint_waypoints) - 1):
+        start_wp = joint_waypoints[seg_idx]
+        end_wp = joint_waypoints[seg_idx + 1]
+        joint_trajs = []
+        seg_max_time = 0
+
+        # 为每个关节生成时间最优轨迹
+        for j in range(num_joints):
+            seg_time, traj_data = time_optimal_trajectory(start_wp[j], end_wp[j], j)
+            joint_trajs.append(traj_data)
+            if seg_time > seg_max_time:
+                seg_max_time = seg_time  # 取最长时间作为段总时间
+
+        # 同步所有关节轨迹（拉伸到段总时间）
+        synced_seg_traj = []
+        dt = 0.001
+        seg_time_list = np.arange(0, seg_max_time + dt, dt)
+
+        for t in seg_time_list:
+            row = [t]
+            for j in range(num_joints):
+                # 找到当前时间对应的关节位置（插值补全）
+                j_traj = joint_trajs[j]
+                if t > j_traj[-1, 0]:
+                    pos = j_traj[-1, 1]  # 已到达目标，保持位置
+                else:
+                    pos = np.interp(t, j_traj[:, 0], j_traj[:, 1])
+                row.append(pos)
+            synced_seg_traj.append(row)
+
+        segment_trajs.append(np.array(synced_seg_traj))
+
+    # 拼接所有段的轨迹
+    global_traj = segment_trajs[0]
+    for seg in segment_trajs[1:]:
+        # 时间偏移（累加前序段的总时间）
+        seg[:, 0] += global_traj[-1, 0]
+        global_traj = np.vstack((global_traj, seg))
+
+    return global_traj
+
+
+# ====================== 4. 避障+闭环控制（复用已有逻辑） ======================
 def artificial_potential_field(ee_pos, target_pos):
     ee_pos = np.array(ee_pos)
     target_pos = np.array(target_pos)
@@ -105,45 +184,26 @@ def artificial_potential_field(ee_pos, target_pos):
     return corrected_target.tolist()
 
 
-# ====================== 5. 笛卡尔轨迹平滑 ======================
-def smooth_cartesian_trajectory(waypoints, num_points=200):
-    x = np.array([p[0] for p in waypoints])
-    y = np.array([p[1] for p in waypoints])
-    z = np.array([p[2] for p in waypoints])
-
-    t = np.linspace(0, 1, len(x))
-    t_new = np.linspace(0, 1, num_points)
-
-    spline_x = interpolate.CubicSpline(t, x, bc_type='natural')
-    spline_y = interpolate.CubicSpline(t, y, bc_type='natural')
-    spline_z = interpolate.CubicSpline(t, z, bc_type='natural')
-
-    return np.vstack((spline_x(t_new), spline_y(t_new), spline_z(t_new))).T
+def closed_loop_constraint_control(data, target_joints, joint_idx):
+    k_p = 8.0
+    k_d = 0.2
 
+    current_pos = data.qpos[joint_idx]
+    current_vel = data.qvel[joint_idx]
 
-# ====================== 6. 替代逆运动学：关节轨迹预生成 ======================
-def precompute_joint_waypoints(model, data, cart_waypoints):
-    """预计算笛卡尔轨迹对应的关节角度（兼容旧版MuJoCo）"""
-    joint_waypoints = []
-    ee_site_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "ee_site")
+    pos_error = target_joints[joint_idx] - current_pos
+    vel_error = -current_vel
 
-    for cart_pos in cart_waypoints:
-        # 重置数据
-        mujoco.mj_resetData(model, data)
-        # 设置目标位置到data中（旧版mj_inverse的要求）
-        data.site_xpos[ee_site_id] = cart_pos
-        # 调用逆运动学（仅model和data参数）
-        mujoco.mj_inverse(model, data)
-        # 保存关节角度
-        joint_waypoints.append(data.qpos[:6].copy())
+    ctrl = k_p * pos_error + k_d * vel_error
+    ctrl = np.clip(ctrl, CONSTRAINTS["ctrl_limit"][0], CONSTRAINTS["ctrl_limit"][1])
 
-    return joint_waypoints
+    return ctrl
 
 
-# ====================== 7. 机械臂模型（带障碍物） ======================
+# ====================== 5. 机械臂模型（带障碍物） ======================
 def get_arm_xml_with_obstacles():
     arm_xml = """
-<mujoco model="6dof_arm_with_obstacles">
+<mujoco model="6dof_arm_time_optimal">
   <compiler angle="radian" inertiafromgeom="true"/>
   <option timestep="0.005" gravity="0 0 -9.81"/>
   <asset>
@@ -205,8 +265,22 @@ def get_arm_xml_with_obstacles():
     return arm_xml
 
 
-# ====================== 8. 主仿真逻辑（修复逆运动学调用） ======================
-def run_obstacle_avoidance_simulation():
+# ====================== 6. 预计算关节关键点（兼容旧版MuJoCo） ======================
+def precompute_joint_waypoints(model, data, cart_waypoints):
+    joint_waypoints = []
+    ee_site_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "ee_site")
+
+    for cart_pos in cart_waypoints:
+        mujoco.mj_resetData(model, data)
+        data.site_xpos[ee_site_id] = cart_pos
+        mujoco.mj_inverse(model, data)
+        joint_waypoints.append(data.qpos[:6].copy())
+
+    return joint_waypoints
+
+
+# ====================== 7. 主仿真逻辑（时间最优+避障+约束） ======================
+def run_time_optimal_simulation():
     arm_xml = get_arm_xml_with_obstacles()
     with tempfile.NamedTemporaryFile(mode='w', suffix='.xml', delete=False) as f:
         f.write(arm_xml)
@@ -215,60 +289,60 @@ def run_obstacle_avoidance_simulation():
     try:
         model = mujoco.MjModel.from_xml_path(xml_path)
         data = mujoco.MjData(model)
-        print("✅ 带避障的机械臂模型加载成功！")
-        print(
-            f"🔧 避障参数：斥力半径={OBSTACLE_CONFIG['rep_radius']}m，障碍物数量={len(OBSTACLE_CONFIG['obstacle_list'])}")
+        print("✅ 时间最优机械臂模型加载成功！")
+        print(f"🔧 物理约束：最大速度={CONSTRAINTS['max_vel'][0]}rad/s，最大加速度={CONSTRAINTS['max_acc'][0]}rad/s²")
+        print(f"🔧 避障参数：斥力半径={OBSTACLE_CONFIG['rep_radius']}m")
 
-        # 预计算关节轨迹关键点（兼容旧版MuJoCo）
+        # 步骤1：预计算笛卡尔对应的关节关键点
         joint_waypoints = precompute_joint_waypoints(model, data, CART_WAYPOINTS)
-        # 平滑关节轨迹
-        num_joint_points = 200
-        smooth_joint_traj = []
-        for joint_idx in range(6):
-            joint_vals = [wp[joint_idx] for wp in joint_waypoints]
-            t = np.linspace(0, 1, len(joint_vals))
-            t_new = np.linspace(0, 1, num_joint_points)
-            spline = interpolate.CubicSpline(t, joint_vals, bc_type='natural')
-            smooth_joint_traj.append(spline(t_new))
-        # 转置为 [轨迹点, 关节]
-        smooth_joint_traj = np.array(smooth_joint_traj).T
-
-        traj_length = len(smooth_joint_traj)
+
+        # 步骤2：生成时间最优同步轨迹
+        global_traj = sync_joint_trajectories(joint_waypoints)
+        total_opt_time = global_traj[-1, 0]
+        print(f"\n⏱️  时间最优轨迹生成完成！总运动时间：{total_opt_time:.2f}s")
+
         ee_site_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "ee_site")
-        segment_time = 5.0
+        traj_length = len(global_traj)
 
         with mujoco.viewer.launch_passive(model, data) as viewer:
-            print("\n🎮 机械臂避障仿真启动！")
-            print("💡 核心功能：人工势场法避障 + 物理约束 + PD闭环控制")
-            print("💡 可视化：红色半透明球体为障碍物，机械臂自动绕开")
+            print("\n🎮 时间最优机械臂仿真启动！")
+            print("💡 核心功能：梯形速度曲线（时间最优）+ 避障 + 物理约束")
+            print("💡 特征：机械臂以最短时间运动，同时避开障碍物、不超物理极限")
             print("💡 按 Ctrl+C 退出")
 
             while viewer.is_running():
-                # 1. 计算当前轨迹索引
-                t_total = data.time
-                traj_idx = int((t_total / segment_time) * traj_length) % traj_length
+                # 1. 获取当前仿真时间
+                t_sim = data.time
 
-                # 2. 获取末端当前位置
-                ee_pos = data.site_xpos[ee_site_id].tolist()
+                # 2. 超出总时间则循环
+                if t_sim > total_opt_time:
+                    mujoco.mj_resetData(model, data)
+                    continue
 
-                # 3. 原始关节目标
-                raw_joint_target = smooth_joint_traj[traj_idx]
+                # 3. 查找当前时间对应的目标关节角度（插值）
+                target_joints = []
+                for j in range(6):
+                    pos = np.interp(t_sim, global_traj[:, 0], global_traj[:, j + 1])
+                    target_joints.append(pos)
 
-                # 4. 笛卡尔空间避障修正（核心）
-                # 先通过正运动学获取原始笛卡尔目标
-                mujoco.mj_forward(model, data)  # 更新正运动学
+                # 4. 避障修正（实时调整目标）
+                ee_pos = data.site_xpos[ee_site_id].tolist()
+                # 正运动学获取当前笛卡尔目标
+                mujoco.mj_forward(model, data)
                 raw_cart_target = data.site_xpos[ee_site_id].copy()
-                # 避障修正笛卡尔目标
+                # 避障修正
                 corrected_cart_target = artificial_potential_field(ee_pos, raw_cart_target)
-                # 修正后的笛卡尔目标转关节目标（兼容旧版）
+                # 修正关节目标
                 data.site_xpos[ee_site_id] = corrected_cart_target
                 mujoco.mj_inverse(model, data)
-                target_joints = data.qpos[:6].copy()
+                corrected_joint_target = data.qpos[:6].copy()
+                # 融合时间最优和避障目标（加权）
+                target_joints = [0.8 * target_joints[i] + 0.2 * corrected_joint_target[i] for i in range(6)]
 
-                # 5. 应用物理约束 + 闭环控制
+                # 5. 物理约束+闭环控制
                 ctrl_signals = []
                 for i in range(6):
-                    # 约束关节角度
+                    # 约束关节角度范围
                     target_joints[i] = np.clip(target_joints[i], model.actuator_ctrlrange[i][0],
                                                model.actuator_ctrlrange[i][1])
                     # 闭环PD控制
@@ -278,18 +352,22 @@ def run_obstacle_avoidance_simulation():
                 # 6. 发送控制指令
                 data.ctrl[:6] = ctrl_signals
 
-                # 7. 打印状态
-                if int(t_total) % 1 == 0 and int(t_total) != 0:
+                # 7. 打印关键状态（每0.5秒）
+                if int(t_sim * 2) % 1 == 0 and t_sim > 0:
+                    # 计算当前关节速度
+                    joint_vel = [data.qvel[i] for i in range(6)]
+                    max_vel = max([abs(v) for v in joint_vel])
+                    # 计算末端与最近障碍距离
                     obs_distances = []
                     for obs in OBSTACLE_CONFIG["obstacle_list"]:
                         dist = np.linalg.norm(np.array(ee_pos) - np.array(obs[:3]))
                         obs_distances.append(dist)
                     min_obs_dist = min(obs_distances) if obs_distances else 0
 
-                    print(f"\n⏱️  时间：{t_total:.2f}s")
-                    print(f"   末端当前位置：{np.round(ee_pos, 3)}")
-                    print(f"   修正后目标位置：{np.round(corrected_cart_target, 3)}")
-                    print(f"   与最近障碍距离：{min_obs_dist:.3f}m (斥力半径：{OBSTACLE_CONFIG['rep_radius']}m)")
+                    print(f"\n⏱️  仿真时间：{t_sim:.2f}s / 最优总时间：{total_opt_time:.2f}s")
+                    print(f"   最大关节速度：{max_vel:.3f}rad/s (上限：{CONSTRAINTS['max_vel'][0]})")
+                    print(f"   末端与最近障碍距离：{min_obs_dist:.3f}m")
+                    print(f"   末端位置：{np.round(ee_pos, 3)}")
 
                 # 8. 运行仿真步
                 mujoco.mj_step(model, data)
@@ -310,4 +388,4 @@ def run_obstacle_avoidance_simulation():
 
 
 if __name__ == "__main__":
-    run_obstacle_avoidance_simulation()
\ No newline at end of file
+    run_time_optimal_simulation()
\ No newline at end of file

From a90f7cfc3adf86dd69afe24dbc564fbe09d32eb4 Mon Sep 17 00:00:00 2001
From: XYTX0427 <481177497@qq.com>
Date: Mon, 22 Dec 2025 20:07:42 +0800
Subject: [PATCH 11/21] =?UTF-8?q?=E8=A1=A5=E5=85=85=E6=9C=BA=E6=A2=B0?=
 =?UTF-8?q?=E8=87=82=E8=90=BD=E5=9C=B0=E9=81=BF=E9=9A=9C=E7=9A=84=E8=BF=90?=
 =?UTF-8?q?=E5=8A=A8=E6=95=88=E6=9E=9C?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/Robotic_arm/main.py | 340 ++++++++++++++++------------------------
 1 file changed, 131 insertions(+), 209 deletions(-)

diff --git a/src/Robotic_arm/main.py b/src/Robotic_arm/main.py
index 4b9677fbe0..3ea2964733 100644
--- a/src/Robotic_arm/main.py
+++ b/src/Robotic_arm/main.py
@@ -6,153 +6,74 @@
 import time
 from scipy import interpolate
 
-# ====================== 1. 全局配置（物理约束+避障+时间最优参数） ======================
-# 机械臂物理约束（UR5参考参数）
+# ====================== 1. 全局配置（物理约束+避障参数） ======================
 CONSTRAINTS = {
-    "max_vel": [1.0, 0.8, 0.8, 1.2, 0.9, 1.2],  # 各关节最大角速度 (rad/s)
-    "max_acc": [0.5, 0.4, 0.4, 0.6, 0.5, 0.6],  # 各关节最大角加速度 (rad/s²)
-    "max_jerk": [0.3, 0.2, 0.2, 0.4, 0.3, 0.4],  # 最大加加速度 (rad/s³)
-    "ctrl_limit": [-10.0, 10.0]  # 电机控制量限制
+    "max_vel": [1.0, 0.8, 0.8, 1.2, 0.9, 1.2],
+    "max_acc": [0.5, 0.4, 0.4, 0.6, 0.5, 0.6],
+    "max_jerk": [0.3, 0.2, 0.2, 0.4, 0.3, 0.4],
+    "ctrl_limit": [-10.0, 10.0]
 }
 
-# 避障参数
 OBSTACLE_CONFIG = {
-    "k_att": 0.8,  # 引力系数
-    "k_rep": 0.6,  # 斥力系数
-    "rep_radius": 0.3,  # 斥力作用半径
-    "obstacle_list": [  # 障碍物：[x,y,z,半径]
+    "k_att": 0.8,
+    "k_rep": 0.6,
+    "rep_radius": 0.3,
+    "obstacle_list": [
         [0.6, 0.1, 0.5, 0.1],
         [0.4, -0.1, 0.6, 0.08]
     ]
 }
 
-# 笛卡尔轨迹关键点
 CART_WAYPOINTS = [
-    [0.5, 0.0, 0.6],  # 起点
-    [0.7, 0.2, 0.7],  # 中间点（障碍区）
-    [0.8, 0.1, 0.8],  # 终点
-    [0.7, 0.2, 0.7],  # 回中间点
-    [0.5, 0.0, 0.6]  # 回起点
+    [0.5, 0.0, 0.6],
+    [0.7, 0.2, 0.7],
+    [0.8, 0.1, 0.8],
+    [0.7, 0.2, 0.7],
+    [0.5, 0.0, 0.6]
 ]
 
 
-# ====================== 2. 核心：时间最优轨迹生成（梯形速度曲线） ======================
-def time_optimal_trajectory(start, end, joint_idx):
-    """
-    生成单个关节的时间最优轨迹（梯形速度曲线）
-    :param start: 起点角度 (rad)
-    :param end: 终点角度 (rad)
-    :param joint_idx: 关节索引（0-5）
-    :return: 最优运动时间 + 轨迹点数组 [时间, 位置, 速度, 加速度]
-    """
-    max_vel = CONSTRAINTS["max_vel"][joint_idx]
-    max_acc = CONSTRAINTS["max_acc"][joint_idx]
-    delta_pos = end - start  # 位置差
-    sign = np.sign(delta_pos)  # 运动方向
-
-    # 步骤1：计算达到最大速度所需的加速时间和位移
-    t_acc = max_vel / max_acc  # 加速到最大速度的时间
-    s_acc = 0.5 * max_acc * t_acc ** 2  # 加速阶段位移
-
-    # 步骤2：判断是否能达到最大速度（决定是梯形/三角形速度曲线）
-    if abs(delta_pos) < 2 * s_acc:
-        # 位移太小，无法匀速（三角形曲线）
-        t_acc = np.sqrt(abs(delta_pos) / max_acc)
-        t_const = 0  # 无匀速阶段
-        total_time = 2 * t_acc
-    else:
-        # 能达到最大速度（梯形曲线）
-        t_const = (abs(delta_pos) - 2 * s_acc) / max_vel  # 匀速时间
-        total_time = 2 * t_acc + t_const
-
-    # 步骤3：生成离散轨迹点（1ms步长，保证精度）
-    dt = 0.001
-    time_list = np.arange(0, total_time + dt, dt)
-    pos_list = []
-    vel_list = []
-    acc_list = []
-
-    for t in time_list:
-        if t < t_acc:
-            # 加速阶段
-            pos = start + sign * 0.5 * max_acc * t ** 2
-            vel = sign * max_acc * t
-            acc = sign * max_acc
-        elif t < t_acc + t_const:
-            # 匀速阶段
-            pos = start + sign * (s_acc + max_vel * (t - t_acc))
-            vel = sign * max_vel
-            acc = 0
-        else:
-            # 减速阶段
-            t_dec = t - (t_acc + t_const)
-            pos = end - sign * 0.5 * max_acc * t_dec ** 2
-            vel = sign * (max_vel - max_acc * t_dec)
-            acc = -sign * max_acc
-
-        pos_list.append(pos)
-        vel_list.append(vel)
-        acc_list.append(acc)
-
-    # 封装轨迹数据
-    traj_data = np.vstack((time_list, pos_list, vel_list, acc_list)).T
-    return total_time, traj_data
-
-
-# ====================== 3. 多关节时间最优轨迹同步 ======================
-def sync_joint_trajectories(joint_waypoints):
-    """
-    同步所有关节的时间最优轨迹（保证同时到达目标点）
-    :param joint_waypoints: 关节轨迹关键点 [[j0,j1,...j5], ...]
-    :return: 全局时间最优轨迹数组 [时间, j0_pos, j1_pos, ..., j5_pos]
-    """
-    num_joints = 6
-    segment_trajs = []  # 存储每段轨迹的各关节数据
-
-    # 遍历每段轨迹（关键点之间的段）
-    for seg_idx in range(len(joint_waypoints) - 1):
-        start_wp = joint_waypoints[seg_idx]
-        end_wp = joint_waypoints[seg_idx + 1]
-        joint_trajs = []
-        seg_max_time = 0
-
-        # 为每个关节生成时间最优轨迹
-        for j in range(num_joints):
-            seg_time, traj_data = time_optimal_trajectory(start_wp[j], end_wp[j], j)
-            joint_trajs.append(traj_data)
-            if seg_time > seg_max_time:
-                seg_max_time = seg_time  # 取最长时间作为段总时间
-
-        # 同步所有关节轨迹（拉伸到段总时间）
-        synced_seg_traj = []
-        dt = 0.001
-        seg_time_list = np.arange(0, seg_max_time + dt, dt)
-
-        for t in seg_time_list:
-            row = [t]
-            for j in range(num_joints):
-                # 找到当前时间对应的关节位置（插值补全）
-                j_traj = joint_trajs[j]
-                if t > j_traj[-1, 0]:
-                    pos = j_traj[-1, 1]  # 已到达目标，保持位置
-                else:
-                    pos = np.interp(t, j_traj[:, 0], j_traj[:, 1])
-                row.append(pos)
-            synced_seg_traj.append(row)
-
-        segment_trajs.append(np.array(synced_seg_traj))
-
-    # 拼接所有段的轨迹
-    global_traj = segment_trajs[0]
-    for seg in segment_trajs[1:]:
-        # 时间偏移（累加前序段的总时间）
-        seg[:, 0] += global_traj[-1, 0]
-        global_traj = np.vstack((global_traj, seg))
-
-    return global_traj
-
-
-# ====================== 4. 避障+闭环控制（复用已有逻辑） ======================
+# ====================== 2. 物理约束轨迹生成 ======================
+def constrained_quintic_polynomial(start, end, total_time, t, joint_idx):
+    s0, v0, a0 = start, 0, 0
+    s1, v1, a1 = end, 0, 0
+
+    T = total_time
+    a = s0
+    b = v0
+    c = a0 / 2
+    d = (20 * (s1 - s0) - (8 * v1 + 12 * v0) * T - (3 * a0 - a1) * T ** 2) / (2 * T ** 3)
+    e = (30 * (s0 - s1) + (14 * v1 + 16 * v0) * T + (3 * a0 - 2 * a1) * T ** 2) / (2 * T ** 4)
+    f = (12 * (s1 - s0) - (6 * v1 + 6 * v0) * T - (a0 - a1) * T ** 2) / (2 * T ** 5)
+
+    pos = a + b * t + c * t ** 2 + d * t ** 3 + e * t ** 4 + f * t ** 5
+    vel = b + 2 * c * t + 3 * d * t ** 2 + 4 * e * t ** 3 + 5 * f * t ** 4
+    acc = 2 * c + 6 * d * t + 12 * e * t ** 2 + 20 * f * t ** 3
+
+    vel = np.clip(vel, -CONSTRAINTS["max_vel"][joint_idx], CONSTRAINTS["max_vel"][joint_idx])
+    acc = np.clip(acc, -CONSTRAINTS["max_acc"][joint_idx], CONSTRAINTS["max_acc"][joint_idx])
+
+    return pos, vel, acc
+
+
+# ====================== 3. 闭环PD控制 ======================
+def closed_loop_constraint_control(data, target_joints, joint_idx):
+    k_p = 8.0
+    k_d = 0.2
+
+    current_pos = data.qpos[joint_idx]
+    current_vel = data.qvel[joint_idx]
+
+    pos_error = target_joints[joint_idx] - current_pos
+    vel_error = -current_vel
+
+    ctrl = k_p * pos_error + k_d * vel_error
+    ctrl = np.clip(ctrl, CONSTRAINTS["ctrl_limit"][0], CONSTRAINTS["ctrl_limit"][1])
+
+    return ctrl
+
+
+# ====================== 4. 人工势场法避障 ======================
 def artificial_potential_field(ee_pos, target_pos):
     ee_pos = np.array(ee_pos)
     target_pos = np.array(target_pos)
@@ -184,26 +105,45 @@ def artificial_potential_field(ee_pos, target_pos):
     return corrected_target.tolist()
 
 
-def closed_loop_constraint_control(data, target_joints, joint_idx):
-    k_p = 8.0
-    k_d = 0.2
+# ====================== 5. 笛卡尔轨迹平滑 ======================
+def smooth_cartesian_trajectory(waypoints, num_points=200):
+    x = np.array([p[0] for p in waypoints])
+    y = np.array([p[1] for p in waypoints])
+    z = np.array([p[2] for p in waypoints])
 
-    current_pos = data.qpos[joint_idx]
-    current_vel = data.qvel[joint_idx]
+    t = np.linspace(0, 1, len(x))
+    t_new = np.linspace(0, 1, num_points)
 
-    pos_error = target_joints[joint_idx] - current_pos
-    vel_error = -current_vel
+    spline_x = interpolate.CubicSpline(t, x, bc_type='natural')
+    spline_y = interpolate.CubicSpline(t, y, bc_type='natural')
+    spline_z = interpolate.CubicSpline(t, z, bc_type='natural')
 
-    ctrl = k_p * pos_error + k_d * vel_error
-    ctrl = np.clip(ctrl, CONSTRAINTS["ctrl_limit"][0], CONSTRAINTS["ctrl_limit"][1])
+    return np.vstack((spline_x(t_new), spline_y(t_new), spline_z(t_new))).T
 
-    return ctrl
+
+# ====================== 6. 替代逆运动学：关节轨迹预生成 ======================
+def precompute_joint_waypoints(model, data, cart_waypoints):
+    """预计算笛卡尔轨迹对应的关节角度（兼容旧版MuJoCo）"""
+    joint_waypoints = []
+    ee_site_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "ee_site")
+
+    for cart_pos in cart_waypoints:
+        # 重置数据
+        mujoco.mj_resetData(model, data)
+        # 设置目标位置到data中（旧版mj_inverse的要求）
+        data.site_xpos[ee_site_id] = cart_pos
+        # 调用逆运动学（仅model和data参数）
+        mujoco.mj_inverse(model, data)
+        # 保存关节角度
+        joint_waypoints.append(data.qpos[:6].copy())
+
+    return joint_waypoints
 
 
-# ====================== 5. 机械臂模型（带障碍物） ======================
+# ====================== 7. 机械臂模型（带障碍物） ======================
 def get_arm_xml_with_obstacles():
     arm_xml = """
-<mujoco model="6dof_arm_time_optimal">
+<mujoco model="6dof_arm_with_obstacles">
   <compiler angle="radian" inertiafromgeom="true"/>
   <option timestep="0.005" gravity="0 0 -9.81"/>
   <asset>
@@ -265,22 +205,8 @@ def get_arm_xml_with_obstacles():
     return arm_xml
 
 
-# ====================== 6. 预计算关节关键点（兼容旧版MuJoCo） ======================
-def precompute_joint_waypoints(model, data, cart_waypoints):
-    joint_waypoints = []
-    ee_site_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "ee_site")
-
-    for cart_pos in cart_waypoints:
-        mujoco.mj_resetData(model, data)
-        data.site_xpos[ee_site_id] = cart_pos
-        mujoco.mj_inverse(model, data)
-        joint_waypoints.append(data.qpos[:6].copy())
-
-    return joint_waypoints
-
-
-# ====================== 7. 主仿真逻辑（时间最优+避障+约束） ======================
-def run_time_optimal_simulation():
+# ====================== 8. 主仿真逻辑（修复逆运动学调用） ======================
+def run_obstacle_avoidance_simulation():
     arm_xml = get_arm_xml_with_obstacles()
     with tempfile.NamedTemporaryFile(mode='w', suffix='.xml', delete=False) as f:
         f.write(arm_xml)
@@ -289,60 +215,60 @@ def run_time_optimal_simulation():
     try:
         model = mujoco.MjModel.from_xml_path(xml_path)
         data = mujoco.MjData(model)
-        print("✅ 时间最优机械臂模型加载成功！")
-        print(f"🔧 物理约束：最大速度={CONSTRAINTS['max_vel'][0]}rad/s，最大加速度={CONSTRAINTS['max_acc'][0]}rad/s²")
-        print(f"🔧 避障参数：斥力半径={OBSTACLE_CONFIG['rep_radius']}m")
+        print("✅ 带避障的机械臂模型加载成功！")
+        print(
+            f"🔧 避障参数：斥力半径={OBSTACLE_CONFIG['rep_radius']}m，障碍物数量={len(OBSTACLE_CONFIG['obstacle_list'])}")
 
-        # 步骤1：预计算笛卡尔对应的关节关键点
+        # 预计算关节轨迹关键点（兼容旧版MuJoCo）
         joint_waypoints = precompute_joint_waypoints(model, data, CART_WAYPOINTS)
-
-        # 步骤2：生成时间最优同步轨迹
-        global_traj = sync_joint_trajectories(joint_waypoints)
-        total_opt_time = global_traj[-1, 0]
-        print(f"\n⏱️  时间最优轨迹生成完成！总运动时间：{total_opt_time:.2f}s")
-
+        # 平滑关节轨迹
+        num_joint_points = 200
+        smooth_joint_traj = []
+        for joint_idx in range(6):
+            joint_vals = [wp[joint_idx] for wp in joint_waypoints]
+            t = np.linspace(0, 1, len(joint_vals))
+            t_new = np.linspace(0, 1, num_joint_points)
+            spline = interpolate.CubicSpline(t, joint_vals, bc_type='natural')
+            smooth_joint_traj.append(spline(t_new))
+        # 转置为 [轨迹点, 关节]
+        smooth_joint_traj = np.array(smooth_joint_traj).T
+
+        traj_length = len(smooth_joint_traj)
         ee_site_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "ee_site")
-        traj_length = len(global_traj)
+        segment_time = 5.0
 
         with mujoco.viewer.launch_passive(model, data) as viewer:
-            print("\n🎮 时间最优机械臂仿真启动！")
-            print("💡 核心功能：梯形速度曲线（时间最优）+ 避障 + 物理约束")
-            print("💡 特征：机械臂以最短时间运动，同时避开障碍物、不超物理极限")
+            print("\n🎮 机械臂避障仿真启动！")
+            print("💡 核心功能：人工势场法避障 + 物理约束 + PD闭环控制")
+            print("💡 可视化：红色半透明球体为障碍物，机械臂自动绕开")
             print("💡 按 Ctrl+C 退出")
 
             while viewer.is_running():
-                # 1. 获取当前仿真时间
-                t_sim = data.time
+                # 1. 计算当前轨迹索引
+                t_total = data.time
+                traj_idx = int((t_total / segment_time) * traj_length) % traj_length
 
-                # 2. 超出总时间则循环
-                if t_sim > total_opt_time:
-                    mujoco.mj_resetData(model, data)
-                    continue
+                # 2. 获取末端当前位置
+                ee_pos = data.site_xpos[ee_site_id].tolist()
 
-                # 3. 查找当前时间对应的目标关节角度（插值）
-                target_joints = []
-                for j in range(6):
-                    pos = np.interp(t_sim, global_traj[:, 0], global_traj[:, j + 1])
-                    target_joints.append(pos)
+                # 3. 原始关节目标
+                raw_joint_target = smooth_joint_traj[traj_idx]
 
-                # 4. 避障修正（实时调整目标）
-                ee_pos = data.site_xpos[ee_site_id].tolist()
-                # 正运动学获取当前笛卡尔目标
-                mujoco.mj_forward(model, data)
+                # 4. 笛卡尔空间避障修正（核心）
+                # 先通过正运动学获取原始笛卡尔目标
+                mujoco.mj_forward(model, data)  # 更新正运动学
                 raw_cart_target = data.site_xpos[ee_site_id].copy()
-                # 避障修正
+                # 避障修正笛卡尔目标
                 corrected_cart_target = artificial_potential_field(ee_pos, raw_cart_target)
-                # 修正关节目标
+                # 修正后的笛卡尔目标转关节目标（兼容旧版）
                 data.site_xpos[ee_site_id] = corrected_cart_target
                 mujoco.mj_inverse(model, data)
-                corrected_joint_target = data.qpos[:6].copy()
-                # 融合时间最优和避障目标（加权）
-                target_joints = [0.8 * target_joints[i] + 0.2 * corrected_joint_target[i] for i in range(6)]
+                target_joints = data.qpos[:6].copy()
 
-                # 5. 物理约束+闭环控制
+                # 5. 应用物理约束 + 闭环控制
                 ctrl_signals = []
                 for i in range(6):
-                    # 约束关节角度范围
+                    # 约束关节角度
                     target_joints[i] = np.clip(target_joints[i], model.actuator_ctrlrange[i][0],
                                                model.actuator_ctrlrange[i][1])
                     # 闭环PD控制
@@ -352,22 +278,18 @@ def run_time_optimal_simulation():
                 # 6. 发送控制指令
                 data.ctrl[:6] = ctrl_signals
 
-                # 7. 打印关键状态（每0.5秒）
-                if int(t_sim * 2) % 1 == 0 and t_sim > 0:
-                    # 计算当前关节速度
-                    joint_vel = [data.qvel[i] for i in range(6)]
-                    max_vel = max([abs(v) for v in joint_vel])
-                    # 计算末端与最近障碍距离
+                # 7. 打印状态
+                if int(t_total) % 1 == 0 and int(t_total) != 0:
                     obs_distances = []
                     for obs in OBSTACLE_CONFIG["obstacle_list"]:
                         dist = np.linalg.norm(np.array(ee_pos) - np.array(obs[:3]))
                         obs_distances.append(dist)
                     min_obs_dist = min(obs_distances) if obs_distances else 0
 
-                    print(f"\n⏱️  仿真时间：{t_sim:.2f}s / 最优总时间：{total_opt_time:.2f}s")
-                    print(f"   最大关节速度：{max_vel:.3f}rad/s (上限：{CONSTRAINTS['max_vel'][0]})")
-                    print(f"   末端与最近障碍距离：{min_obs_dist:.3f}m")
-                    print(f"   末端位置：{np.round(ee_pos, 3)}")
+                    print(f"\n⏱️  时间：{t_total:.2f}s")
+                    print(f"   末端当前位置：{np.round(ee_pos, 3)}")
+                    print(f"   修正后目标位置：{np.round(corrected_cart_target, 3)}")
+                    print(f"   与最近障碍距离：{min_obs_dist:.3f}m (斥力半径：{OBSTACLE_CONFIG['rep_radius']}m)")
 
                 # 8. 运行仿真步
                 mujoco.mj_step(model, data)
@@ -388,4 +310,4 @@ def run_time_optimal_simulation():
 
 
 if __name__ == "__main__":
-    run_time_optimal_simulation()
\ No newline at end of file
+    run_obstacle_avoidance_simulation()
\ No newline at end of file

From 30a36bf7322615270629a5805e7ef3f422d03765 Mon Sep 17 00:00:00 2001
From: XYTX0427 <481177497@qq.com>
Date: Mon, 22 Dec 2025 20:25:45 +0800
Subject: [PATCH 12/21] =?UTF-8?q?=E4=BC=98=E5=8C=96=E6=9C=BA=E6=A2=B0?=
 =?UTF-8?q?=E8=87=82=E8=BF=90=E5=8A=A8=E7=9A=84=E9=B2=81=E6=A3=92=E6=80=A7?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/Robotic_arm/main.py | 314 +++++++++++++++++++++++++++++-----------
 1 file changed, 230 insertions(+), 84 deletions(-)

diff --git a/src/Robotic_arm/main.py b/src/Robotic_arm/main.py
index 3ea2964733..02ecbd8176 100644
--- a/src/Robotic_arm/main.py
+++ b/src/Robotic_arm/main.py
@@ -5,8 +5,13 @@
 import tempfile
 import time
 from scipy import interpolate
+from sklearn.cluster import DBSCAN
+import warnings
 
-# ====================== 1. 全局配置（物理约束+避障参数） ======================
+warnings.filterwarnings("ignore")
+
+# ====================== 1. 全局配置（鲁棒性优化参数） ======================
+# 物理约束（UR5参考）
 CONSTRAINTS = {
     "max_vel": [1.0, 0.8, 0.8, 1.2, 0.9, 1.2],
     "max_acc": [0.5, 0.4, 0.4, 0.6, 0.5, 0.6],
@@ -14,26 +19,57 @@
     "ctrl_limit": [-10.0, 10.0]
 }
 
+# 避障基础参数（鲁棒性优化版）
 OBSTACLE_CONFIG = {
-    "k_att": 0.8,
-    "k_rep": 0.6,
-    "rep_radius": 0.3,
-    "obstacle_list": [
-        [0.6, 0.1, 0.5, 0.1],
-        [0.4, -0.1, 0.6, 0.08]
+    "base_k_att": 0.8,  # 基础引力系数
+    "base_k_rep": 0.6,  # 基础斥力系数
+    "rep_radius": 0.3,  # 斥力作用半径
+    "stagnant_threshold": 0.01,  # 停滞速度阈值 (m/s)
+    "stagnant_time": 1.0,  # 停滞判定时间 (s)
+    "guide_offset": 0.1,  # 局部最优引导偏移量 (m)
+    "obstacle_list": [  # 障碍物列表 [x,y,z,半径]
+        [0.6, 0.1, 0.5, 0.1],  # 障碍1：易导致局部最优
+        [0.55, 0.05, 0.55, 0.08],  # 障碍2：密集障碍
+        [0.4, -0.1, 0.6, 0.08]  # 障碍3
     ]
 }
 
+# 笛卡尔轨迹关键点（易触发局部最优的路径）
 CART_WAYPOINTS = [
-    [0.5, 0.0, 0.6],
-    [0.7, 0.2, 0.7],
-    [0.8, 0.1, 0.8],
-    [0.7, 0.2, 0.7],
-    [0.5, 0.0, 0.6]
+    [0.5, 0.0, 0.6],  # 起点
+    [0.6, 0.0, 0.58],  # 中间点（障碍夹缝，易局部最优）
+    [0.8, 0.1, 0.8],  # 终点
+    [0.6, 0.0, 0.58],  # 回中间点
+    [0.5, 0.0, 0.6]  # 回起点
 ]
 
+# 全局变量：记录停滞开始时间
+stagnant_start_time = None
+
+
+# ====================== 2. 新增：兼容所有版本的末端速度计算 ======================
+def get_ee_cartesian_velocity(model, data, ee_site_id):
+    """
+    计算末端执行器的笛卡尔速度（兼容所有MuJoCo版本）
+    原理：通过雅可比矩阵将关节速度转换为末端笛卡尔速度
+    """
+    # 获取雅可比矩阵（6xN，前3行是平移速度，后3行是旋转速度）
+    jacp = np.zeros((3, model.nv))  # 平移雅可比
+    jacr = np.zeros((3, model.nv))  # 旋转雅可比
+
+    # 计算末端site的雅可比矩阵
+    mujoco.mj_jacSite(model, data, jacp, jacr, ee_site_id)
+
+    # 关节速度（data.qvel）
+    joint_vel = data.qvel[:6]  # 仅取前6个关节速度
+
+    # 笛卡尔平移速度 = 雅可比 × 关节速度
+    ee_cart_vel = jacp @ joint_vel
+
+    return ee_cart_vel
 
-# ====================== 2. 物理约束轨迹生成 ======================
+
+# ====================== 3. 物理约束轨迹生成（原有逻辑） ======================
 def constrained_quintic_polynomial(start, end, total_time, t, joint_idx):
     s0, v0, a0 = start, 0, 0
     s1, v1, a1 = end, 0, 0
@@ -48,7 +84,7 @@ def constrained_quintic_polynomial(start, end, total_time, t, joint_idx):
 
     pos = a + b * t + c * t ** 2 + d * t ** 3 + e * t ** 4 + f * t ** 5
     vel = b + 2 * c * t + 3 * d * t ** 2 + 4 * e * t ** 3 + 5 * f * t ** 4
-    acc = 2 * c + 6 * d * t + 12 * e * t ** 2 + 20 * f * t ** 3
+    acc = 2 * c + 6 * d * t + 12 * e * t ** 2 + 20 * f * t ** 4
 
     vel = np.clip(vel, -CONSTRAINTS["max_vel"][joint_idx], CONSTRAINTS["max_vel"][joint_idx])
     acc = np.clip(acc, -CONSTRAINTS["max_acc"][joint_idx], CONSTRAINTS["max_acc"][joint_idx])
@@ -56,7 +92,7 @@ def constrained_quintic_polynomial(start, end, total_time, t, joint_idx):
     return pos, vel, acc
 
 
-# ====================== 3. 闭环PD控制 ======================
+# ====================== 4. 闭环PD控制（原有逻辑） ======================
 def closed_loop_constraint_control(data, target_joints, joint_idx):
     k_p = 8.0
     k_d = 0.2
@@ -73,19 +109,133 @@ def closed_loop_constraint_control(data, target_joints, joint_idx):
     return ctrl
 
 
-# ====================== 4. 人工势场法避障 ======================
-def artificial_potential_field(ee_pos, target_pos):
+# ====================== 5. 鲁棒性优化1：局部最优检测与规避 ======================
+def check_local_optimum(ee_vel, ee_pos, target_pos):
+    """
+    检测是否陷入局部最优，并生成引导目标跳出陷阱
+    :return: is_local_opt (是否局部最优), guide_target (引导目标位置)
+    """
+    global stagnant_start_time
+
+    # 计算末端合速度
+    vel_mag = np.linalg.norm(ee_vel)
+    threshold = OBSTACLE_CONFIG["stagnant_threshold"]
+    max_stagnant_time = OBSTACLE_CONFIG["stagnant_time"]
+
+    if vel_mag < threshold:
+        if stagnant_start_time is None:
+            stagnant_start_time = time.time()
+        # 超过停滞时间，判定为局部最优
+        elif time.time() - stagnant_start_time > max_stagnant_time:
+            print(f"\n⚠️  检测到局部最优！末端速度={vel_mag:.4f}m/s < 阈值={threshold}m/s")
+            # 生成引导目标：向原始目标方向偏移
+            dir_to_target = np.array(target_pos) - np.array(ee_pos)
+            if np.linalg.norm(dir_to_target) < 1e-6:
+                dir_to_target = np.array([0.0, 0.0, 0.1])  # 避免除零
+            else:
+                dir_to_target = dir_to_target / np.linalg.norm(dir_to_target)
+
+            guide_target = np.array(ee_pos) + dir_to_target * OBSTACLE_CONFIG["guide_offset"]
+            print(f"📌 生成引导目标：{np.round(guide_target, 3)} (偏移{OBSTACLE_CONFIG['guide_offset']}m)")
+            stagnant_start_time = None  # 重置计时器
+            return True, guide_target.tolist()
+    else:
+        stagnant_start_time = None  # 速度正常，重置计时器
+
+    return False, target_pos
+
+
+# ====================== 6. 鲁棒性优化2：自适应势场参数 ======================
+def adaptive_potential_params(ee_pos, obstacle_list):
+    """
+    根据障碍距离/数量自适应调整引力/斥力系数
+    - 距离越近，斥力越大；障碍越多，引力越小
+    """
+    base_k_att = OBSTACLE_CONFIG["base_k_att"]
+    base_k_rep = OBSTACLE_CONFIG["base_k_rep"]
+
+    # 计算与最近障碍的距离
+    obs_distances = [np.linalg.norm(np.array(ee_pos) - np.array(obs[:3])) for obs in obstacle_list]
+    min_dist = min(obs_distances) if obs_distances else 1.0
+    obs_count = len(obstacle_list)
+
+    # 距离自适应斥力系数：距离<0.2m时，斥力翻倍
+    k_rep = base_k_rep if min_dist > 0.2 else base_k_rep * 2.0
+    # 数量自适应引力系数：障碍>2个时，引力降低50%
+    k_att = base_k_att if obs_count <= 2 else base_k_att * 0.5
+
+    return k_att, k_rep
+
+
+# ====================== 7. 鲁棒性优化3：碰撞冗余检测 ======================
+def collision_check_approx(ee_pos, joint_pos, obstacle_list, safety_margin=0.05):
+    """
+    近似碰撞检测（工程简化版）：检测末端+关键关节与障碍的距离
+    :return: is_collision (是否碰撞), min_safe_dist (最小安全距离)
+    """
+    # 检测末端执行器
+    ee_collision = False
+    min_ee_dist = 100.0
+    for obs in obstacle_list:
+        obs_pos = np.array(obs[:3])
+        obs_radius = obs[3]
+        dist = np.linalg.norm(np.array(ee_pos) - obs_pos)
+        min_ee_dist = min(min_ee_dist, dist)
+        if dist < obs_radius + safety_margin:
+            ee_collision = True
+            break
+
+    # 检测关键关节（简化：仅检测关节2/3/4）
+    joint_collision = False
+    # 仿真中通过data获取关节位置（实际场景需正运动学计算）
+    # 这里简化为基于关节角度的近似检测
+    joint_2_3_4_idx = [2, 3, 4]
+    for idx in joint_2_3_4_idx:
+        # 近似关节位置（基于机械臂模型）
+        joint_pos_approx = np.array([
+            0.4 + 0.35 * np.cos(joint_pos[2]),
+            0.0 + 0.35 * np.sin(joint_pos[2]),
+            0.5 + 0.25 * np.sin(joint_pos[3])
+        ])
+        for obs in obstacle_list:
+            obs_pos = np.array(obs[:3])
+            obs_radius = obs[3]
+            dist = np.linalg.norm(joint_pos_approx - obs_pos)
+            if dist < obs_radius + safety_margin:
+                joint_collision = True
+                break
+        if joint_collision:
+            break
+
+    is_collision = ee_collision or joint_collision
+    if is_collision:
+        print(f"\n🚨 碰撞风险！末端与最近障碍距离={min_ee_dist:.3f}m < 安全裕度={safety_margin}m")
+
+    return is_collision, min_ee_dist
+
+
+# ====================== 8. 鲁棒性优化后的避障核心逻辑 ======================
+def robust_artificial_potential_field(ee_pos, ee_vel, target_pos, obstacle_list):
+    """
+    鲁棒版人工势场法：局部最优规避 + 自适应参数
+    """
     ee_pos = np.array(ee_pos)
     target_pos = np.array(target_pos)
-    obstacle_list = OBSTACLE_CONFIG["obstacle_list"]
-    k_att = OBSTACLE_CONFIG["k_att"]
-    k_rep = OBSTACLE_CONFIG["k_rep"]
     rep_radius = OBSTACLE_CONFIG["rep_radius"]
 
-    # 引力
-    att_force = k_att * (target_pos - ee_pos)
+    # 步骤1：检测局部最优，生成引导目标
+    is_local_opt, guide_target = check_local_optimum(ee_vel, ee_pos, target_pos)
+    current_target = np.array(guide_target) if is_local_opt else target_pos
+
+    # 步骤2：自适应调整引力/斥力系数
+    k_att, k_rep = adaptive_potential_params(ee_pos, obstacle_list)
+    print(
+        f"\n🔧 自适应参数：k_att={k_att:.1f}, k_rep={k_rep:.1f} (最近障碍距离={min([np.linalg.norm(ee_pos - np.array(obs[:3])) for obs in obstacle_list]):.3f}m)")
 
-    # 斥力
+    # 步骤3：计算引力（指向当前目标）
+    att_force = k_att * (current_target - ee_pos)
+
+    # 步骤4：计算斥力（远离所有障碍）
     rep_force = np.zeros(3)
     for obs in obstacle_list:
         obs_pos = np.array(obs[:3])
@@ -96,54 +246,34 @@ def artificial_potential_field(ee_pos, target_pos):
             if dist < 1e-6:
                 dist = 1e-6
             rep_dir = (ee_pos - obs_pos) / dist
+            # 优化斥力公式：避免距离过近时斥力突变
             rep_force += k_rep * (1 / (dist - obs_radius) - 1 / rep_radius) * (1 / dist ** 2) * rep_dir
 
-    # 修正目标位置
+    # 步骤5：合力修正目标位置，添加边界约束
     corrected_target = ee_pos + att_force + rep_force
     corrected_target = np.clip(corrected_target, [0.3, -0.4, 0.2], [0.9, 0.4, 1.0])
 
     return corrected_target.tolist()
 
 
-# ====================== 5. 笛卡尔轨迹平滑 ======================
-def smooth_cartesian_trajectory(waypoints, num_points=200):
-    x = np.array([p[0] for p in waypoints])
-    y = np.array([p[1] for p in waypoints])
-    z = np.array([p[2] for p in waypoints])
-
-    t = np.linspace(0, 1, len(x))
-    t_new = np.linspace(0, 1, num_points)
-
-    spline_x = interpolate.CubicSpline(t, x, bc_type='natural')
-    spline_y = interpolate.CubicSpline(t, y, bc_type='natural')
-    spline_z = interpolate.CubicSpline(t, z, bc_type='natural')
-
-    return np.vstack((spline_x(t_new), spline_y(t_new), spline_z(t_new))).T
-
-
-# ====================== 6. 替代逆运动学：关节轨迹预生成 ======================
+# ====================== 9. 逆运动学预计算（兼容旧版MuJoCo） ======================
 def precompute_joint_waypoints(model, data, cart_waypoints):
-    """预计算笛卡尔轨迹对应的关节角度（兼容旧版MuJoCo）"""
     joint_waypoints = []
     ee_site_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "ee_site")
 
     for cart_pos in cart_waypoints:
-        # 重置数据
         mujoco.mj_resetData(model, data)
-        # 设置目标位置到data中（旧版mj_inverse的要求）
         data.site_xpos[ee_site_id] = cart_pos
-        # 调用逆运动学（仅model和data参数）
         mujoco.mj_inverse(model, data)
-        # 保存关节角度
         joint_waypoints.append(data.qpos[:6].copy())
 
     return joint_waypoints
 
 
-# ====================== 7. 机械臂模型（带障碍物） ======================
+# ====================== 10. 机械臂模型（带密集障碍可视化） ======================
 def get_arm_xml_with_obstacles():
     arm_xml = """
-<mujoco model="6dof_arm_with_obstacles">
+<mujoco model="6dof_arm_with_obstacles_robust">
   <compiler angle="radian" inertiafromgeom="true"/>
   <option timestep="0.005" gravity="0 0 -9.81"/>
   <asset>
@@ -151,6 +281,7 @@ def get_arm_xml_with_obstacles():
     <material name="blue" rgba="0.2 0.4 0.8 1"/>
     <material name="red" rgba="0.8 0.2 0.2 1"/>
     <material name="obstacle" rgba="1 0 0 0.5"/>
+    <material name="critical_obstacle" rgba="1 0 0 0.7"/>  <!-- 易导致局部最优的障碍 -->
   </asset>
   <worldbody>
     <geom name="floor" type="plane" size="5 5 0.1" pos="0 0 0" material="gray"/>
@@ -184,10 +315,12 @@ def get_arm_xml_with_obstacles():
     </body>
     """
 
+    # 不同障碍用不同透明度区分（关键障碍更醒目）
     for i, obs in enumerate(OBSTACLE_CONFIG["obstacle_list"]):
         x, y, z, r = obs
+        material = "critical_obstacle" if i == 0 else "obstacle"
         arm_xml += f"""
-    <geom name="obstacle_{i}" type="sphere" size="{r}" pos="{x} {y} {z}" material="obstacle"/>
+    <geom name="obstacle_{i}" type="sphere" size="{r}" pos="{x} {y} {z}" material="{material}"/>
         """
 
     arm_xml += """
@@ -205,8 +338,8 @@ def get_arm_xml_with_obstacles():
     return arm_xml
 
 
-# ====================== 8. 主仿真逻辑（修复逆运动学调用） ======================
-def run_obstacle_avoidance_simulation():
+# ====================== 11. 主仿真逻辑（鲁棒性优化核心） ======================
+def run_robust_obstacle_avoidance_simulation():
     arm_xml = get_arm_xml_with_obstacles()
     with tempfile.NamedTemporaryFile(mode='w', suffix='.xml', delete=False) as f:
         f.write(arm_xml)
@@ -215,13 +348,12 @@ def run_obstacle_avoidance_simulation():
     try:
         model = mujoco.MjModel.from_xml_path(xml_path)
         data = mujoco.MjData(model)
-        print("✅ 带避障的机械臂模型加载成功！")
-        print(
-            f"🔧 避障参数：斥力半径={OBSTACLE_CONFIG['rep_radius']}m，障碍物数量={len(OBSTACLE_CONFIG['obstacle_list'])}")
+        print("✅ 鲁棒版避障机械臂模型加载成功！")
+        print(f"🔧 鲁棒性优化：局部最优规避 + 自适应参数 + 碰撞冗余检测")
+        print(f"🔧 障碍数量：{len(OBSTACLE_CONFIG['obstacle_list'])} (含1个易导致局部最优的关键障碍)")
 
-        # 预计算关节轨迹关键点（兼容旧版MuJoCo）
+        # 预计算关节轨迹
         joint_waypoints = precompute_joint_waypoints(model, data, CART_WAYPOINTS)
-        # 平滑关节轨迹
         num_joint_points = 200
         smooth_joint_traj = []
         for joint_idx in range(6):
@@ -230,72 +362,86 @@ def run_obstacle_avoidance_simulation():
             t_new = np.linspace(0, 1, num_joint_points)
             spline = interpolate.CubicSpline(t, joint_vals, bc_type='natural')
             smooth_joint_traj.append(spline(t_new))
-        # 转置为 [轨迹点, 关节]
         smooth_joint_traj = np.array(smooth_joint_traj).T
 
         traj_length = len(smooth_joint_traj)
         ee_site_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "ee_site")
-        segment_time = 5.0
+        segment_time = 8.0  # 增加轨迹时间，适配鲁棒性优化
 
         with mujoco.viewer.launch_passive(model, data) as viewer:
-            print("\n🎮 机械臂避障仿真启动！")
-            print("💡 核心功能：人工势场法避障 + 物理约束 + PD闭环控制")
-            print("💡 可视化：红色半透明球体为障碍物，机械臂自动绕开")
+            print("\n🎮 鲁棒版机械臂避障仿真启动！")
+            print("💡 核心优化：自动规避局部最优 + 自适应势场参数 + 碰撞冗余检测")
+            print("💡 可视化：深红色障碍为易导致局部最优的关键障碍")
             print("💡 按 Ctrl+C 退出")
 
             while viewer.is_running():
-                # 1. 计算当前轨迹索引
+                # 1. 时间与轨迹索引
                 t_total = data.time
                 traj_idx = int((t_total / segment_time) * traj_length) % traj_length
 
-                # 2. 获取末端当前位置
+                # 2. 获取末端位置 + 计算末端速度（兼容旧版MuJoCo）
                 ee_pos = data.site_xpos[ee_site_id].tolist()
+                ee_vel = get_ee_cartesian_velocity(model, data, ee_site_id).tolist()  # 替换site_xvel
 
                 # 3. 原始关节目标
                 raw_joint_target = smooth_joint_traj[traj_idx]
 
-                # 4. 笛卡尔空间避障修正（核心）
-                # 先通过正运动学获取原始笛卡尔目标
-                mujoco.mj_forward(model, data)  # 更新正运动学
+                # 4. 正运动学获取原始笛卡尔目标
+                mujoco.mj_forward(model, data)
                 raw_cart_target = data.site_xpos[ee_site_id].copy()
-                # 避障修正笛卡尔目标
-                corrected_cart_target = artificial_potential_field(ee_pos, raw_cart_target)
-                # 修正后的笛卡尔目标转关节目标（兼容旧版）
+
+                # 5. 鲁棒版避障修正（核心！）
+                corrected_cart_target = robust_artificial_potential_field(
+                    ee_pos, ee_vel, raw_cart_target, OBSTACLE_CONFIG["obstacle_list"]
+                )
+
+                # 6. 逆解得到修正后的关节目标
                 data.site_xpos[ee_site_id] = corrected_cart_target
                 mujoco.mj_inverse(model, data)
                 target_joints = data.qpos[:6].copy()
 
-                # 5. 应用物理约束 + 闭环控制
+                # 7. 碰撞冗余检测（安全兜底）
+                is_collision, min_safe_dist = collision_check_approx(
+                    ee_pos, target_joints, OBSTACLE_CONFIG["obstacle_list"]
+                )
+                if is_collision:
+                    # 碰撞时紧急调整：增大斥力，远离障碍
+                    emergency_rep = np.array(ee_pos) - np.array(OBSTACLE_CONFIG["obstacle_list"][0][:3])
+                    emergency_rep = emergency_rep / np.linalg.norm(emergency_rep) * 0.05
+                    corrected_cart_target = np.array(corrected_cart_target) + emergency_rep
+                    data.site_xpos[ee_site_id] = corrected_cart_target
+                    mujoco.mj_inverse(model, data)
+                    target_joints = data.qpos[:6].copy()
+                    print(f"🆘 紧急避障：修正目标位置={np.round(corrected_cart_target, 3)}")
+
+                # 8. 物理约束 + 闭环控制
                 ctrl_signals = []
                 for i in range(6):
-                    # 约束关节角度
                     target_joints[i] = np.clip(target_joints[i], model.actuator_ctrlrange[i][0],
                                                model.actuator_ctrlrange[i][1])
-                    # 闭环PD控制
                     ctrl = closed_loop_constraint_control(data, target_joints, i)
                     ctrl_signals.append(ctrl)
 
-                # 6. 发送控制指令
+                # 9. 发送控制指令
                 data.ctrl[:6] = ctrl_signals
 
-                # 7. 打印状态
+                # 10. 打印关键状态（每1秒）
                 if int(t_total) % 1 == 0 and int(t_total) != 0:
-                    obs_distances = []
-                    for obs in OBSTACLE_CONFIG["obstacle_list"]:
-                        dist = np.linalg.norm(np.array(ee_pos) - np.array(obs[:3]))
-                        obs_distances.append(dist)
+                    obs_distances = [np.linalg.norm(np.array(ee_pos) - np.array(obs[:3])) for obs in
+                                     OBSTACLE_CONFIG["obstacle_list"]]
                     min_obs_dist = min(obs_distances) if obs_distances else 0
 
                     print(f"\n⏱️  时间：{t_total:.2f}s")
-                    print(f"   末端当前位置：{np.round(ee_pos, 3)}")
-                    print(f"   修正后目标位置：{np.round(corrected_cart_target, 3)}")
-                    print(f"   与最近障碍距离：{min_obs_dist:.3f}m (斥力半径：{OBSTACLE_CONFIG['rep_radius']}m)")
+                    print(f"   末端位置：{np.round(ee_pos, 3)}")
+                    print(f"   修正目标：{np.round(corrected_cart_target, 3)}")
+                    print(f"   末端速度：{np.round(np.linalg.norm(ee_vel), 4)}m/s")
+                    print(f"   最近障碍：{min_obs_dist:.3f}m | 碰撞风险：{'是' if is_collision else '否'}")
 
-                # 8. 运行仿真步
+                # 11. 仿真步运行
                 mujoco.mj_step(model, data)
                 viewer.sync()
 
-                # 9. 帧率控制
+                # 12. 帧率控制
                 try:
                     mujoco.utils.mju_sleep(1 / 60)
                 except:
@@ -310,4 +456,4 @@ def run_obstacle_avoidance_simulation():
 
 
 if __name__ == "__main__":
-    run_obstacle_avoidance_simulation()
\ No newline at end of file
+    run_robust_obstacle_avoidance_simulation()
\ No newline at end of file

From e4717a43485816853d9d780899a15a9573461d4d Mon Sep 17 00:00:00 2001
From: XYTX0427 <481177497@qq.com>
Date: Tue, 23 Dec 2025 18:56:43 +0800
Subject: [PATCH 13/21] =?UTF-8?q?=E6=9C=BA=E6=A2=B0=E8=87=82=E8=90=BD?=
 =?UTF-8?q?=E5=9C=B0=E9=81=BF=E5=BC=80=E9=9A=9C=E7=A2=8D?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/Robotic_arm/main.py | 314 +++++++++++-----------------------------
 1 file changed, 84 insertions(+), 230 deletions(-)

diff --git a/src/Robotic_arm/main.py b/src/Robotic_arm/main.py
index 02ecbd8176..3ea2964733 100644
--- a/src/Robotic_arm/main.py
+++ b/src/Robotic_arm/main.py
@@ -5,13 +5,8 @@
 import tempfile
 import time
 from scipy import interpolate
-from sklearn.cluster import DBSCAN
-import warnings
 
-warnings.filterwarnings("ignore")
-
-# ====================== 1. 全局配置（鲁棒性优化参数） ======================
-# 物理约束（UR5参考）
+# ====================== 1. 全局配置（物理约束+避障参数） ======================
 CONSTRAINTS = {
     "max_vel": [1.0, 0.8, 0.8, 1.2, 0.9, 1.2],
     "max_acc": [0.5, 0.4, 0.4, 0.6, 0.5, 0.6],
@@ -19,57 +14,26 @@
     "ctrl_limit": [-10.0, 10.0]
 }
 
-# 避障基础参数（鲁棒性优化版）
 OBSTACLE_CONFIG = {
-    "base_k_att": 0.8,  # 基础引力系数
-    "base_k_rep": 0.6,  # 基础斥力系数
-    "rep_radius": 0.3,  # 斥力作用半径
-    "stagnant_threshold": 0.01,  # 停滞速度阈值 (m/s)
-    "stagnant_time": 1.0,  # 停滞判定时间 (s)
-    "guide_offset": 0.1,  # 局部最优引导偏移量 (m)
-    "obstacle_list": [  # 障碍物列表 [x,y,z,半径]
-        [0.6, 0.1, 0.5, 0.1],  # 障碍1：易导致局部最优
-        [0.55, 0.05, 0.55, 0.08],  # 障碍2：密集障碍
-        [0.4, -0.1, 0.6, 0.08]  # 障碍3
+    "k_att": 0.8,
+    "k_rep": 0.6,
+    "rep_radius": 0.3,
+    "obstacle_list": [
+        [0.6, 0.1, 0.5, 0.1],
+        [0.4, -0.1, 0.6, 0.08]
     ]
 }
 
-# 笛卡尔轨迹关键点（易触发局部最优的路径）
 CART_WAYPOINTS = [
-    [0.5, 0.0, 0.6],  # 起点
-    [0.6, 0.0, 0.58],  # 中间点（障碍夹缝，易局部最优）
-    [0.8, 0.1, 0.8],  # 终点
-    [0.6, 0.0, 0.58],  # 回中间点
-    [0.5, 0.0, 0.6]  # 回起点
+    [0.5, 0.0, 0.6],
+    [0.7, 0.2, 0.7],
+    [0.8, 0.1, 0.8],
+    [0.7, 0.2, 0.7],
+    [0.5, 0.0, 0.6]
 ]
 
-# 全局变量：记录停滞开始时间
-stagnant_start_time = None
-
-
-# ====================== 2. 新增：兼容所有版本的末端速度计算 ======================
-def get_ee_cartesian_velocity(model, data, ee_site_id):
-    """
-    计算末端执行器的笛卡尔速度（兼容所有MuJoCo版本）
-    原理：通过雅可比矩阵将关节速度转换为末端笛卡尔速度
-    """
-    # 获取雅可比矩阵（6xN，前3行是平移速度，后3行是旋转速度）
-    jacp = np.zeros((3, model.nv))  # 平移雅可比
-    jacr = np.zeros((3, model.nv))  # 旋转雅可比
-
-    # 计算末端site的雅可比矩阵
-    mujoco.mj_jacSite(model, data, jacp, jacr, ee_site_id)
-
-    # 关节速度（data.qvel）
-    joint_vel = data.qvel[:6]  # 仅取前6个关节速度
-
-    # 笛卡尔平移速度 = 雅可比 × 关节速度
-    ee_cart_vel = jacp @ joint_vel
-
-    return ee_cart_vel
 
-
-# ====================== 3. 物理约束轨迹生成（原有逻辑） ======================
+# ====================== 2. 物理约束轨迹生成 ======================
 def constrained_quintic_polynomial(start, end, total_time, t, joint_idx):
     s0, v0, a0 = start, 0, 0
     s1, v1, a1 = end, 0, 0
@@ -84,7 +48,7 @@ def constrained_quintic_polynomial(start, end, total_time, t, joint_idx):
 
     pos = a + b * t + c * t ** 2 + d * t ** 3 + e * t ** 4 + f * t ** 5
     vel = b + 2 * c * t + 3 * d * t ** 2 + 4 * e * t ** 3 + 5 * f * t ** 4
-    acc = 2 * c + 6 * d * t + 12 * e * t ** 2 + 20 * f * t ** 4
+    acc = 2 * c + 6 * d * t + 12 * e * t ** 2 + 20 * f * t ** 3
 
     vel = np.clip(vel, -CONSTRAINTS["max_vel"][joint_idx], CONSTRAINTS["max_vel"][joint_idx])
     acc = np.clip(acc, -CONSTRAINTS["max_acc"][joint_idx], CONSTRAINTS["max_acc"][joint_idx])
@@ -92,7 +56,7 @@ def constrained_quintic_polynomial(start, end, total_time, t, joint_idx):
     return pos, vel, acc
 
 
-# ====================== 4. 闭环PD控制（原有逻辑） ======================
+# ====================== 3. 闭环PD控制 ======================
 def closed_loop_constraint_control(data, target_joints, joint_idx):
     k_p = 8.0
     k_d = 0.2
@@ -109,133 +73,19 @@ def closed_loop_constraint_control(data, target_joints, joint_idx):
     return ctrl
 
 
-# ====================== 5. 鲁棒性优化1：局部最优检测与规避 ======================
-def check_local_optimum(ee_vel, ee_pos, target_pos):
-    """
-    检测是否陷入局部最优，并生成引导目标跳出陷阱
-    :return: is_local_opt (是否局部最优), guide_target (引导目标位置)
-    """
-    global stagnant_start_time
-
-    # 计算末端合速度
-    vel_mag = np.linalg.norm(ee_vel)
-    threshold = OBSTACLE_CONFIG["stagnant_threshold"]
-    max_stagnant_time = OBSTACLE_CONFIG["stagnant_time"]
-
-    if vel_mag < threshold:
-        if stagnant_start_time is None:
-            stagnant_start_time = time.time()
-        # 超过停滞时间，判定为局部最优
-        elif time.time() - stagnant_start_time > max_stagnant_time:
-            print(f"\n⚠️  检测到局部最优！末端速度={vel_mag:.4f}m/s < 阈值={threshold}m/s")
-            # 生成引导目标：向原始目标方向偏移
-            dir_to_target = np.array(target_pos) - np.array(ee_pos)
-            if np.linalg.norm(dir_to_target) < 1e-6:
-                dir_to_target = np.array([0.0, 0.0, 0.1])  # 避免除零
-            else:
-                dir_to_target = dir_to_target / np.linalg.norm(dir_to_target)
-
-            guide_target = np.array(ee_pos) + dir_to_target * OBSTACLE_CONFIG["guide_offset"]
-            print(f"📌 生成引导目标：{np.round(guide_target, 3)} (偏移{OBSTACLE_CONFIG['guide_offset']}m)")
-            stagnant_start_time = None  # 重置计时器
-            return True, guide_target.tolist()
-    else:
-        stagnant_start_time = None  # 速度正常，重置计时器
-
-    return False, target_pos
-
-
-# ====================== 6. 鲁棒性优化2：自适应势场参数 ======================
-def adaptive_potential_params(ee_pos, obstacle_list):
-    """
-    根据障碍距离/数量自适应调整引力/斥力系数
-    - 距离越近，斥力越大；障碍越多，引力越小
-    """
-    base_k_att = OBSTACLE_CONFIG["base_k_att"]
-    base_k_rep = OBSTACLE_CONFIG["base_k_rep"]
-
-    # 计算与最近障碍的距离
-    obs_distances = [np.linalg.norm(np.array(ee_pos) - np.array(obs[:3])) for obs in obstacle_list]
-    min_dist = min(obs_distances) if obs_distances else 1.0
-    obs_count = len(obstacle_list)
-
-    # 距离自适应斥力系数：距离<0.2m时，斥力翻倍
-    k_rep = base_k_rep if min_dist > 0.2 else base_k_rep * 2.0
-    # 数量自适应引力系数：障碍>2个时，引力降低50%
-    k_att = base_k_att if obs_count <= 2 else base_k_att * 0.5
-
-    return k_att, k_rep
-
-
-# ====================== 7. 鲁棒性优化3：碰撞冗余检测 ======================
-def collision_check_approx(ee_pos, joint_pos, obstacle_list, safety_margin=0.05):
-    """
-    近似碰撞检测（工程简化版）：检测末端+关键关节与障碍的距离
-    :return: is_collision (是否碰撞), min_safe_dist (最小安全距离)
-    """
-    # 检测末端执行器
-    ee_collision = False
-    min_ee_dist = 100.0
-    for obs in obstacle_list:
-        obs_pos = np.array(obs[:3])
-        obs_radius = obs[3]
-        dist = np.linalg.norm(np.array(ee_pos) - obs_pos)
-        min_ee_dist = min(min_ee_dist, dist)
-        if dist < obs_radius + safety_margin:
-            ee_collision = True
-            break
-
-    # 检测关键关节（简化：仅检测关节2/3/4）
-    joint_collision = False
-    # 仿真中通过data获取关节位置（实际场景需正运动学计算）
-    # 这里简化为基于关节角度的近似检测
-    joint_2_3_4_idx = [2, 3, 4]
-    for idx in joint_2_3_4_idx:
-        # 近似关节位置（基于机械臂模型）
-        joint_pos_approx = np.array([
-            0.4 + 0.35 * np.cos(joint_pos[2]),
-            0.0 + 0.35 * np.sin(joint_pos[2]),
-            0.5 + 0.25 * np.sin(joint_pos[3])
-        ])
-        for obs in obstacle_list:
-            obs_pos = np.array(obs[:3])
-            obs_radius = obs[3]
-            dist = np.linalg.norm(joint_pos_approx - obs_pos)
-            if dist < obs_radius + safety_margin:
-                joint_collision = True
-                break
-        if joint_collision:
-            break
-
-    is_collision = ee_collision or joint_collision
-    if is_collision:
-        print(f"\n🚨 碰撞风险！末端与最近障碍距离={min_ee_dist:.3f}m < 安全裕度={safety_margin}m")
-
-    return is_collision, min_ee_dist
-
-
-# ====================== 8. 鲁棒性优化后的避障核心逻辑 ======================
-def robust_artificial_potential_field(ee_pos, ee_vel, target_pos, obstacle_list):
-    """
-    鲁棒版人工势场法：局部最优规避 + 自适应参数
-    """
+# ====================== 4. 人工势场法避障 ======================
+def artificial_potential_field(ee_pos, target_pos):
     ee_pos = np.array(ee_pos)
     target_pos = np.array(target_pos)
+    obstacle_list = OBSTACLE_CONFIG["obstacle_list"]
+    k_att = OBSTACLE_CONFIG["k_att"]
+    k_rep = OBSTACLE_CONFIG["k_rep"]
     rep_radius = OBSTACLE_CONFIG["rep_radius"]
 
-    # 步骤1：检测局部最优，生成引导目标
-    is_local_opt, guide_target = check_local_optimum(ee_vel, ee_pos, target_pos)
-    current_target = np.array(guide_target) if is_local_opt else target_pos
-
-    # 步骤2：自适应调整引力/斥力系数
-    k_att, k_rep = adaptive_potential_params(ee_pos, obstacle_list)
-    print(
-        f"\n🔧 自适应参数：k_att={k_att:.1f}, k_rep={k_rep:.1f} (最近障碍距离={min([np.linalg.norm(ee_pos - np.array(obs[:3])) for obs in obstacle_list]):.3f}m)")
+    # 引力
+    att_force = k_att * (target_pos - ee_pos)
 
-    # 步骤3：计算引力（指向当前目标）
-    att_force = k_att * (current_target - ee_pos)
-
-    # 步骤4：计算斥力（远离所有障碍）
+    # 斥力
     rep_force = np.zeros(3)
     for obs in obstacle_list:
         obs_pos = np.array(obs[:3])
@@ -246,34 +96,54 @@ def robust_artificial_potential_field(ee_pos, ee_vel, target_pos, obstacle_list)
             if dist < 1e-6:
                 dist = 1e-6
             rep_dir = (ee_pos - obs_pos) / dist
-            # 优化斥力公式：避免距离过近时斥力突变
             rep_force += k_rep * (1 / (dist - obs_radius) - 1 / rep_radius) * (1 / dist ** 2) * rep_dir
 
-    # 步骤5：合力修正目标位置，添加边界约束
+    # 修正目标位置
     corrected_target = ee_pos + att_force + rep_force
     corrected_target = np.clip(corrected_target, [0.3, -0.4, 0.2], [0.9, 0.4, 1.0])
 
     return corrected_target.tolist()
 
 
-# ====================== 9. 逆运动学预计算（兼容旧版MuJoCo） ======================
+# ====================== 5. 笛卡尔轨迹平滑 ======================
+def smooth_cartesian_trajectory(waypoints, num_points=200):
+    x = np.array([p[0] for p in waypoints])
+    y = np.array([p[1] for p in waypoints])
+    z = np.array([p[2] for p in waypoints])
+
+    t = np.linspace(0, 1, len(x))
+    t_new = np.linspace(0, 1, num_points)
+
+    spline_x = interpolate.CubicSpline(t, x, bc_type='natural')
+    spline_y = interpolate.CubicSpline(t, y, bc_type='natural')
+    spline_z = interpolate.CubicSpline(t, z, bc_type='natural')
+
+    return np.vstack((spline_x(t_new), spline_y(t_new), spline_z(t_new))).T
+
+
+# ====================== 6. 替代逆运动学：关节轨迹预生成 ======================
 def precompute_joint_waypoints(model, data, cart_waypoints):
+    """预计算笛卡尔轨迹对应的关节角度（兼容旧版MuJoCo）"""
     joint_waypoints = []
     ee_site_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "ee_site")
 
     for cart_pos in cart_waypoints:
+        # 重置数据
         mujoco.mj_resetData(model, data)
+        # 设置目标位置到data中（旧版mj_inverse的要求）
         data.site_xpos[ee_site_id] = cart_pos
+        # 调用逆运动学（仅model和data参数）
         mujoco.mj_inverse(model, data)
+        # 保存关节角度
         joint_waypoints.append(data.qpos[:6].copy())
 
     return joint_waypoints
 
 
-# ====================== 10. 机械臂模型（带密集障碍可视化） ======================
+# ====================== 7. 机械臂模型（带障碍物） ======================
 def get_arm_xml_with_obstacles():
     arm_xml = """
-<mujoco model="6dof_arm_with_obstacles_robust">
+<mujoco model="6dof_arm_with_obstacles">
   <compiler angle="radian" inertiafromgeom="true"/>
   <option timestep="0.005" gravity="0 0 -9.81"/>
   <asset>
@@ -281,7 +151,6 @@ def get_arm_xml_with_obstacles():
     <material name="blue" rgba="0.2 0.4 0.8 1"/>
     <material name="red" rgba="0.8 0.2 0.2 1"/>
     <material name="obstacle" rgba="1 0 0 0.5"/>
-    <material name="critical_obstacle" rgba="1 0 0 0.7"/>  <!-- 易导致局部最优的障碍 -->
   </asset>
   <worldbody>
     <geom name="floor" type="plane" size="5 5 0.1" pos="0 0 0" material="gray"/>
@@ -315,12 +184,10 @@ def get_arm_xml_with_obstacles():
     </body>
     """
 
-    # 不同障碍用不同透明度区分（关键障碍更醒目）
     for i, obs in enumerate(OBSTACLE_CONFIG["obstacle_list"]):
         x, y, z, r = obs
-        material = "critical_obstacle" if i == 0 else "obstacle"
         arm_xml += f"""
-    <geom name="obstacle_{i}" type="sphere" size="{r}" pos="{x} {y} {z}" material="{material}"/>
+    <geom name="obstacle_{i}" type="sphere" size="{r}" pos="{x} {y} {z}" material="obstacle"/>
         """
 
     arm_xml += """
@@ -338,8 +205,8 @@ def get_arm_xml_with_obstacles():
     return arm_xml
 
 
-# ====================== 11. 主仿真逻辑（鲁棒性优化核心） ======================
-def run_robust_obstacle_avoidance_simulation():
+# ====================== 8. 主仿真逻辑（修复逆运动学调用） ======================
+def run_obstacle_avoidance_simulation():
     arm_xml = get_arm_xml_with_obstacles()
     with tempfile.NamedTemporaryFile(mode='w', suffix='.xml', delete=False) as f:
         f.write(arm_xml)
@@ -348,12 +215,13 @@ def run_robust_obstacle_avoidance_simulation():
     try:
         model = mujoco.MjModel.from_xml_path(xml_path)
         data = mujoco.MjData(model)
-        print("✅ 鲁棒版避障机械臂模型加载成功！")
-        print(f"🔧 鲁棒性优化：局部最优规避 + 自适应参数 + 碰撞冗余检测")
-        print(f"🔧 障碍数量：{len(OBSTACLE_CONFIG['obstacle_list'])} (含1个易导致局部最优的关键障碍)")
+        print("✅ 带避障的机械臂模型加载成功！")
+        print(
+            f"🔧 避障参数：斥力半径={OBSTACLE_CONFIG['rep_radius']}m，障碍物数量={len(OBSTACLE_CONFIG['obstacle_list'])}")
 
-        # 预计算关节轨迹
+        # 预计算关节轨迹关键点（兼容旧版MuJoCo）
         joint_waypoints = precompute_joint_waypoints(model, data, CART_WAYPOINTS)
+        # 平滑关节轨迹
         num_joint_points = 200
         smooth_joint_traj = []
         for joint_idx in range(6):
@@ -362,86 +230,72 @@ def run_robust_obstacle_avoidance_simulation():
             t_new = np.linspace(0, 1, num_joint_points)
             spline = interpolate.CubicSpline(t, joint_vals, bc_type='natural')
             smooth_joint_traj.append(spline(t_new))
+        # 转置为 [轨迹点, 关节]
         smooth_joint_traj = np.array(smooth_joint_traj).T
 
         traj_length = len(smooth_joint_traj)
         ee_site_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "ee_site")
-        segment_time = 8.0  # 增加轨迹时间，适配鲁棒性优化
+        segment_time = 5.0
 
         with mujoco.viewer.launch_passive(model, data) as viewer:
-            print("\n🎮 鲁棒版机械臂避障仿真启动！")
-            print("💡 核心优化：自动规避局部最优 + 自适应势场参数 + 碰撞冗余检测")
-            print("💡 可视化：深红色障碍为易导致局部最优的关键障碍")
+            print("\n🎮 机械臂避障仿真启动！")
+            print("💡 核心功能：人工势场法避障 + 物理约束 + PD闭环控制")
+            print("💡 可视化：红色半透明球体为障碍物，机械臂自动绕开")
             print("💡 按 Ctrl+C 退出")
 
             while viewer.is_running():
-                # 1. 时间与轨迹索引
+                # 1. 计算当前轨迹索引
                 t_total = data.time
                 traj_idx = int((t_total / segment_time) * traj_length) % traj_length
 
-                # 2. 获取末端位置 + 计算末端速度（兼容旧版MuJoCo）
+                # 2. 获取末端当前位置
                 ee_pos = data.site_xpos[ee_site_id].tolist()
-                ee_vel = get_ee_cartesian_velocity(model, data, ee_site_id).tolist()  # 替换site_xvel
 
                 # 3. 原始关节目标
                 raw_joint_target = smooth_joint_traj[traj_idx]
 
-                # 4. 正运动学获取原始笛卡尔目标
-                mujoco.mj_forward(model, data)
+                # 4. 笛卡尔空间避障修正（核心）
+                # 先通过正运动学获取原始笛卡尔目标
+                mujoco.mj_forward(model, data)  # 更新正运动学
                 raw_cart_target = data.site_xpos[ee_site_id].copy()
-
-                # 5. 鲁棒版避障修正（核心！）
-                corrected_cart_target = robust_artificial_potential_field(
-                    ee_pos, ee_vel, raw_cart_target, OBSTACLE_CONFIG["obstacle_list"]
-                )
-
-                # 6. 逆解得到修正后的关节目标
+                # 避障修正笛卡尔目标
+                corrected_cart_target = artificial_potential_field(ee_pos, raw_cart_target)
+                # 修正后的笛卡尔目标转关节目标（兼容旧版）
                 data.site_xpos[ee_site_id] = corrected_cart_target
                 mujoco.mj_inverse(model, data)
                 target_joints = data.qpos[:6].copy()
 
-                # 7. 碰撞冗余检测（安全兜底）
-                is_collision, min_safe_dist = collision_check_approx(
-                    ee_pos, target_joints, OBSTACLE_CONFIG["obstacle_list"]
-                )
-                if is_collision:
-                    # 碰撞时紧急调整：增大斥力，远离障碍
-                    emergency_rep = np.array(ee_pos) - np.array(OBSTACLE_CONFIG["obstacle_list"][0][:3])
-                    emergency_rep = emergency_rep / np.linalg.norm(emergency_rep) * 0.05
-                    corrected_cart_target = np.array(corrected_cart_target) + emergency_rep
-                    data.site_xpos[ee_site_id] = corrected_cart_target
-                    mujoco.mj_inverse(model, data)
-                    target_joints = data.qpos[:6].copy()
-                    print(f"🆘 紧急避障：修正目标位置={np.round(corrected_cart_target, 3)}")
-
-                # 8. 物理约束 + 闭环控制
+                # 5. 应用物理约束 + 闭环控制
                 ctrl_signals = []
                 for i in range(6):
+                    # 约束关节角度
                     target_joints[i] = np.clip(target_joints[i], model.actuator_ctrlrange[i][0],
                                                model.actuator_ctrlrange[i][1])
+                    # 闭环PD控制
                     ctrl = closed_loop_constraint_control(data, target_joints, i)
                     ctrl_signals.append(ctrl)
 
-                # 9. 发送控制指令
+                # 6. 发送控制指令
                 data.ctrl[:6] = ctrl_signals
 
-                # 10. 打印关键状态（每1秒）
+                # 7. 打印状态
                 if int(t_total) % 1 == 0 and int(t_total) != 0:
-                    obs_distances = [np.linalg.norm(np.array(ee_pos) - np.array(obs[:3])) for obs in
-                                     OBSTACLE_CONFIG["obstacle_list"]]
+                    obs_distances = []
+                    for obs in OBSTACLE_CONFIG["obstacle_list"]:
+                        dist = np.linalg.norm(np.array(ee_pos) - np.array(obs[:3]))
+                        obs_distances.append(dist)
                     min_obs_dist = min(obs_distances) if obs_distances else 0
 
                     print(f"\n⏱️  时间：{t_total:.2f}s")
-                    print(f"   末端位置：{np.round(ee_pos, 3)}")
-                    print(f"   修正目标：{np.round(corrected_cart_target, 3)}")
-                    print(f"   末端速度：{np.round(np.linalg.norm(ee_vel), 4)}m/s")
-                    print(f"   最近障碍：{min_obs_dist:.3f}m | 碰撞风险：{'是' if is_collision else '否'}")
+                    print(f"   末端当前位置：{np.round(ee_pos, 3)}")
+                    print(f"   修正后目标位置：{np.round(corrected_cart_target, 3)}")
+                    print(f"   与最近障碍距离：{min_obs_dist:.3f}m (斥力半径：{OBSTACLE_CONFIG['rep_radius']}m)")
 
-                # 11. 仿真步运行
+                # 8. 运行仿真步
                 mujoco.mj_step(model, data)
                 viewer.sync()
 
-                # 12. 帧率控制
+                # 9. 帧率控制
                 try:
                     mujoco.utils.mju_sleep(1 / 60)
                 except:
@@ -456,4 +310,4 @@ def run_robust_obstacle_avoidance_simulation():
 
 
 if __name__ == "__main__":
-    run_robust_obstacle_avoidance_simulation()
\ No newline at end of file
+    run_obstacle_avoidance_simulation()
\ No newline at end of file

From 5b5525f68d3b92a26172318a9b7437df700682e2 Mon Sep 17 00:00:00 2001
From: XYTX0427 <481177497@qq.com>
Date: Wed, 24 Dec 2025 19:41:22 +0800
Subject: [PATCH 14/21] =?UTF-8?q?=E9=81=BF=E9=9A=9C=E7=9A=84=E9=B2=81?=
 =?UTF-8?q?=E6=A3=92=E6=80=A7=E4=BC=98=E5=8C=96?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/Robotic_arm/main.py | 314 +++++++++++++++++++++++++++++-----------
 1 file changed, 230 insertions(+), 84 deletions(-)

diff --git a/src/Robotic_arm/main.py b/src/Robotic_arm/main.py
index 3ea2964733..02ecbd8176 100644
--- a/src/Robotic_arm/main.py
+++ b/src/Robotic_arm/main.py
@@ -5,8 +5,13 @@
 import tempfile
 import time
 from scipy import interpolate
+from sklearn.cluster import DBSCAN
+import warnings
 
-# ====================== 1. 全局配置（物理约束+避障参数） ======================
+warnings.filterwarnings("ignore")
+
+# ====================== 1. 全局配置（鲁棒性优化参数） ======================
+# 物理约束（UR5参考）
 CONSTRAINTS = {
     "max_vel": [1.0, 0.8, 0.8, 1.2, 0.9, 1.2],
     "max_acc": [0.5, 0.4, 0.4, 0.6, 0.5, 0.6],
@@ -14,26 +19,57 @@
     "ctrl_limit": [-10.0, 10.0]
 }
 
+# 避障基础参数（鲁棒性优化版）
 OBSTACLE_CONFIG = {
-    "k_att": 0.8,
-    "k_rep": 0.6,
-    "rep_radius": 0.3,
-    "obstacle_list": [
-        [0.6, 0.1, 0.5, 0.1],
-        [0.4, -0.1, 0.6, 0.08]
+    "base_k_att": 0.8,  # 基础引力系数
+    "base_k_rep": 0.6,  # 基础斥力系数
+    "rep_radius": 0.3,  # 斥力作用半径
+    "stagnant_threshold": 0.01,  # 停滞速度阈值 (m/s)
+    "stagnant_time": 1.0,  # 停滞判定时间 (s)
+    "guide_offset": 0.1,  # 局部最优引导偏移量 (m)
+    "obstacle_list": [  # 障碍物列表 [x,y,z,半径]
+        [0.6, 0.1, 0.5, 0.1],  # 障碍1：易导致局部最优
+        [0.55, 0.05, 0.55, 0.08],  # 障碍2：密集障碍
+        [0.4, -0.1, 0.6, 0.08]  # 障碍3
     ]
 }
 
+# 笛卡尔轨迹关键点（易触发局部最优的路径）
 CART_WAYPOINTS = [
-    [0.5, 0.0, 0.6],
-    [0.7, 0.2, 0.7],
-    [0.8, 0.1, 0.8],
-    [0.7, 0.2, 0.7],
-    [0.5, 0.0, 0.6]
+    [0.5, 0.0, 0.6],  # 起点
+    [0.6, 0.0, 0.58],  # 中间点（障碍夹缝，易局部最优）
+    [0.8, 0.1, 0.8],  # 终点
+    [0.6, 0.0, 0.58],  # 回中间点
+    [0.5, 0.0, 0.6]  # 回起点
 ]
 
+# 全局变量：记录停滞开始时间
+stagnant_start_time = None
+
+
+# ====================== 2. 新增：兼容所有版本的末端速度计算 ======================
+def get_ee_cartesian_velocity(model, data, ee_site_id):
+    """
+    计算末端执行器的笛卡尔速度（兼容所有MuJoCo版本）
+    原理：通过雅可比矩阵将关节速度转换为末端笛卡尔速度
+    """
+    # 获取雅可比矩阵（6xN，前3行是平移速度，后3行是旋转速度）
+    jacp = np.zeros((3, model.nv))  # 平移雅可比
+    jacr = np.zeros((3, model.nv))  # 旋转雅可比
+
+    # 计算末端site的雅可比矩阵
+    mujoco.mj_jacSite(model, data, jacp, jacr, ee_site_id)
+
+    # 关节速度（data.qvel）
+    joint_vel = data.qvel[:6]  # 仅取前6个关节速度
+
+    # 笛卡尔平移速度 = 雅可比 × 关节速度
+    ee_cart_vel = jacp @ joint_vel
+
+    return ee_cart_vel
 
-# ====================== 2. 物理约束轨迹生成 ======================
+
+# ====================== 3. 物理约束轨迹生成（原有逻辑） ======================
 def constrained_quintic_polynomial(start, end, total_time, t, joint_idx):
     s0, v0, a0 = start, 0, 0
     s1, v1, a1 = end, 0, 0
@@ -48,7 +84,7 @@ def constrained_quintic_polynomial(start, end, total_time, t, joint_idx):
 
     pos = a + b * t + c * t ** 2 + d * t ** 3 + e * t ** 4 + f * t ** 5
     vel = b + 2 * c * t + 3 * d * t ** 2 + 4 * e * t ** 3 + 5 * f * t ** 4
-    acc = 2 * c + 6 * d * t + 12 * e * t ** 2 + 20 * f * t ** 3
+    acc = 2 * c + 6 * d * t + 12 * e * t ** 2 + 20 * f * t ** 4
 
     vel = np.clip(vel, -CONSTRAINTS["max_vel"][joint_idx], CONSTRAINTS["max_vel"][joint_idx])
     acc = np.clip(acc, -CONSTRAINTS["max_acc"][joint_idx], CONSTRAINTS["max_acc"][joint_idx])
@@ -56,7 +92,7 @@ def constrained_quintic_polynomial(start, end, total_time, t, joint_idx):
     return pos, vel, acc
 
 
-# ====================== 3. 闭环PD控制 ======================
+# ====================== 4. 闭环PD控制（原有逻辑） ======================
 def closed_loop_constraint_control(data, target_joints, joint_idx):
     k_p = 8.0
     k_d = 0.2
@@ -73,19 +109,133 @@ def closed_loop_constraint_control(data, target_joints, joint_idx):
     return ctrl
 
 
-# ====================== 4. 人工势场法避障 ======================
-def artificial_potential_field(ee_pos, target_pos):
+# ====================== 5. 鲁棒性优化1：局部最优检测与规避 ======================
+def check_local_optimum(ee_vel, ee_pos, target_pos):
+    """
+    检测是否陷入局部最优，并生成引导目标跳出陷阱
+    :return: is_local_opt (是否局部最优), guide_target (引导目标位置)
+    """
+    global stagnant_start_time
+
+    # 计算末端合速度
+    vel_mag = np.linalg.norm(ee_vel)
+    threshold = OBSTACLE_CONFIG["stagnant_threshold"]
+    max_stagnant_time = OBSTACLE_CONFIG["stagnant_time"]
+
+    if vel_mag < threshold:
+        if stagnant_start_time is None:
+            stagnant_start_time = time.time()
+        # 超过停滞时间，判定为局部最优
+        elif time.time() - stagnant_start_time > max_stagnant_time:
+            print(f"\n⚠️  检测到局部最优！末端速度={vel_mag:.4f}m/s < 阈值={threshold}m/s")
+            # 生成引导目标：向原始目标方向偏移
+            dir_to_target = np.array(target_pos) - np.array(ee_pos)
+            if np.linalg.norm(dir_to_target) < 1e-6:
+                dir_to_target = np.array([0.0, 0.0, 0.1])  # 避免除零
+            else:
+                dir_to_target = dir_to_target / np.linalg.norm(dir_to_target)
+
+            guide_target = np.array(ee_pos) + dir_to_target * OBSTACLE_CONFIG["guide_offset"]
+            print(f"📌 生成引导目标：{np.round(guide_target, 3)} (偏移{OBSTACLE_CONFIG['guide_offset']}m)")
+            stagnant_start_time = None  # 重置计时器
+            return True, guide_target.tolist()
+    else:
+        stagnant_start_time = None  # 速度正常，重置计时器
+
+    return False, target_pos
+
+
+# ====================== 6. 鲁棒性优化2：自适应势场参数 ======================
+def adaptive_potential_params(ee_pos, obstacle_list):
+    """
+    根据障碍距离/数量自适应调整引力/斥力系数
+    - 距离越近，斥力越大；障碍越多，引力越小
+    """
+    base_k_att = OBSTACLE_CONFIG["base_k_att"]
+    base_k_rep = OBSTACLE_CONFIG["base_k_rep"]
+
+    # 计算与最近障碍的距离
+    obs_distances = [np.linalg.norm(np.array(ee_pos) - np.array(obs[:3])) for obs in obstacle_list]
+    min_dist = min(obs_distances) if obs_distances else 1.0
+    obs_count = len(obstacle_list)
+
+    # 距离自适应斥力系数：距离<0.2m时，斥力翻倍
+    k_rep = base_k_rep if min_dist > 0.2 else base_k_rep * 2.0
+    # 数量自适应引力系数：障碍>2个时，引力降低50%
+    k_att = base_k_att if obs_count <= 2 else base_k_att * 0.5
+
+    return k_att, k_rep
+
+
+# ====================== 7. 鲁棒性优化3：碰撞冗余检测 ======================
+def collision_check_approx(ee_pos, joint_pos, obstacle_list, safety_margin=0.05):
+    """
+    近似碰撞检测（工程简化版）：检测末端+关键关节与障碍的距离
+    :return: is_collision (是否碰撞), min_safe_dist (最小安全距离)
+    """
+    # 检测末端执行器
+    ee_collision = False
+    min_ee_dist = 100.0
+    for obs in obstacle_list:
+        obs_pos = np.array(obs[:3])
+        obs_radius = obs[3]
+        dist = np.linalg.norm(np.array(ee_pos) - obs_pos)
+        min_ee_dist = min(min_ee_dist, dist)
+        if dist < obs_radius + safety_margin:
+            ee_collision = True
+            break
+
+    # 检测关键关节（简化：仅检测关节2/3/4）
+    joint_collision = False
+    # 仿真中通过data获取关节位置（实际场景需正运动学计算）
+    # 这里简化为基于关节角度的近似检测
+    joint_2_3_4_idx = [2, 3, 4]
+    for idx in joint_2_3_4_idx:
+        # 近似关节位置（基于机械臂模型）
+        joint_pos_approx = np.array([
+            0.4 + 0.35 * np.cos(joint_pos[2]),
+            0.0 + 0.35 * np.sin(joint_pos[2]),
+            0.5 + 0.25 * np.sin(joint_pos[3])
+        ])
+        for obs in obstacle_list:
+            obs_pos = np.array(obs[:3])
+            obs_radius = obs[3]
+            dist = np.linalg.norm(joint_pos_approx - obs_pos)
+            if dist < obs_radius + safety_margin:
+                joint_collision = True
+                break
+        if joint_collision:
+            break
+
+    is_collision = ee_collision or joint_collision
+    if is_collision:
+        print(f"\n🚨 碰撞风险！末端与最近障碍距离={min_ee_dist:.3f}m < 安全裕度={safety_margin}m")
+
+    return is_collision, min_ee_dist
+
+
+# ====================== 8. 鲁棒性优化后的避障核心逻辑 ======================
+def robust_artificial_potential_field(ee_pos, ee_vel, target_pos, obstacle_list):
+    """
+    鲁棒版人工势场法：局部最优规避 + 自适应参数
+    """
     ee_pos = np.array(ee_pos)
     target_pos = np.array(target_pos)
-    obstacle_list = OBSTACLE_CONFIG["obstacle_list"]
-    k_att = OBSTACLE_CONFIG["k_att"]
-    k_rep = OBSTACLE_CONFIG["k_rep"]
     rep_radius = OBSTACLE_CONFIG["rep_radius"]
 
-    # 引力
-    att_force = k_att * (target_pos - ee_pos)
+    # 步骤1：检测局部最优，生成引导目标
+    is_local_opt, guide_target = check_local_optimum(ee_vel, ee_pos, target_pos)
+    current_target = np.array(guide_target) if is_local_opt else target_pos
+
+    # 步骤2：自适应调整引力/斥力系数
+    k_att, k_rep = adaptive_potential_params(ee_pos, obstacle_list)
+    print(
+        f"\n🔧 自适应参数：k_att={k_att:.1f}, k_rep={k_rep:.1f} (最近障碍距离={min([np.linalg.norm(ee_pos - np.array(obs[:3])) for obs in obstacle_list]):.3f}m)")
 
-    # 斥力
+    # 步骤3：计算引力（指向当前目标）
+    att_force = k_att * (current_target - ee_pos)
+
+    # 步骤4：计算斥力（远离所有障碍）
     rep_force = np.zeros(3)
     for obs in obstacle_list:
         obs_pos = np.array(obs[:3])
@@ -96,54 +246,34 @@ def artificial_potential_field(ee_pos, target_pos):
             if dist < 1e-6:
                 dist = 1e-6
             rep_dir = (ee_pos - obs_pos) / dist
+            # 优化斥力公式：避免距离过近时斥力突变
             rep_force += k_rep * (1 / (dist - obs_radius) - 1 / rep_radius) * (1 / dist ** 2) * rep_dir
 
-    # 修正目标位置
+    # 步骤5：合力修正目标位置，添加边界约束
     corrected_target = ee_pos + att_force + rep_force
     corrected_target = np.clip(corrected_target, [0.3, -0.4, 0.2], [0.9, 0.4, 1.0])
 
     return corrected_target.tolist()
 
 
-# ====================== 5. 笛卡尔轨迹平滑 ======================
-def smooth_cartesian_trajectory(waypoints, num_points=200):
-    x = np.array([p[0] for p in waypoints])
-    y = np.array([p[1] for p in waypoints])
-    z = np.array([p[2] for p in waypoints])
-
-    t = np.linspace(0, 1, len(x))
-    t_new = np.linspace(0, 1, num_points)
-
-    spline_x = interpolate.CubicSpline(t, x, bc_type='natural')
-    spline_y = interpolate.CubicSpline(t, y, bc_type='natural')
-    spline_z = interpolate.CubicSpline(t, z, bc_type='natural')
-
-    return np.vstack((spline_x(t_new), spline_y(t_new), spline_z(t_new))).T
-
-
-# ====================== 6. 替代逆运动学：关节轨迹预生成 ======================
+# ====================== 9. 逆运动学预计算（兼容旧版MuJoCo） ======================
 def precompute_joint_waypoints(model, data, cart_waypoints):
-    """预计算笛卡尔轨迹对应的关节角度（兼容旧版MuJoCo）"""
     joint_waypoints = []
     ee_site_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "ee_site")
 
     for cart_pos in cart_waypoints:
-        # 重置数据
         mujoco.mj_resetData(model, data)
-        # 设置目标位置到data中（旧版mj_inverse的要求）
         data.site_xpos[ee_site_id] = cart_pos
-        # 调用逆运动学（仅model和data参数）
         mujoco.mj_inverse(model, data)
-        # 保存关节角度
         joint_waypoints.append(data.qpos[:6].copy())
 
     return joint_waypoints
 
 
-# ====================== 7. 机械臂模型（带障碍物） ======================
+# ====================== 10. 机械臂模型（带密集障碍可视化） ======================
 def get_arm_xml_with_obstacles():
     arm_xml = """
-<mujoco model="6dof_arm_with_obstacles">
+<mujoco model="6dof_arm_with_obstacles_robust">
   <compiler angle="radian" inertiafromgeom="true"/>
   <option timestep="0.005" gravity="0 0 -9.81"/>
   <asset>
@@ -151,6 +281,7 @@ def get_arm_xml_with_obstacles():
     <material name="blue" rgba="0.2 0.4 0.8 1"/>
     <material name="red" rgba="0.8 0.2 0.2 1"/>
     <material name="obstacle" rgba="1 0 0 0.5"/>
+    <material name="critical_obstacle" rgba="1 0 0 0.7"/>  <!-- 易导致局部最优的障碍 -->
   </asset>
   <worldbody>
     <geom name="floor" type="plane" size="5 5 0.1" pos="0 0 0" material="gray"/>
@@ -184,10 +315,12 @@ def get_arm_xml_with_obstacles():
     </body>
     """
 
+    # 不同障碍用不同透明度区分（关键障碍更醒目）
     for i, obs in enumerate(OBSTACLE_CONFIG["obstacle_list"]):
         x, y, z, r = obs
+        material = "critical_obstacle" if i == 0 else "obstacle"
         arm_xml += f"""
-    <geom name="obstacle_{i}" type="sphere" size="{r}" pos="{x} {y} {z}" material="obstacle"/>
+    <geom name="obstacle_{i}" type="sphere" size="{r}" pos="{x} {y} {z}" material="{material}"/>
         """
 
     arm_xml += """
@@ -205,8 +338,8 @@ def get_arm_xml_with_obstacles():
     return arm_xml
 
 
-# ====================== 8. 主仿真逻辑（修复逆运动学调用） ======================
-def run_obstacle_avoidance_simulation():
+# ====================== 11. 主仿真逻辑（鲁棒性优化核心） ======================
+def run_robust_obstacle_avoidance_simulation():
     arm_xml = get_arm_xml_with_obstacles()
     with tempfile.NamedTemporaryFile(mode='w', suffix='.xml', delete=False) as f:
         f.write(arm_xml)
@@ -215,13 +348,12 @@ def run_obstacle_avoidance_simulation():
     try:
         model = mujoco.MjModel.from_xml_path(xml_path)
         data = mujoco.MjData(model)
-        print("✅ 带避障的机械臂模型加载成功！")
-        print(
-            f"🔧 避障参数：斥力半径={OBSTACLE_CONFIG['rep_radius']}m，障碍物数量={len(OBSTACLE_CONFIG['obstacle_list'])}")
+        print("✅ 鲁棒版避障机械臂模型加载成功！")
+        print(f"🔧 鲁棒性优化：局部最优规避 + 自适应参数 + 碰撞冗余检测")
+        print(f"🔧 障碍数量：{len(OBSTACLE_CONFIG['obstacle_list'])} (含1个易导致局部最优的关键障碍)")
 
-        # 预计算关节轨迹关键点（兼容旧版MuJoCo）
+        # 预计算关节轨迹
         joint_waypoints = precompute_joint_waypoints(model, data, CART_WAYPOINTS)
-        # 平滑关节轨迹
         num_joint_points = 200
         smooth_joint_traj = []
         for joint_idx in range(6):
@@ -230,72 +362,86 @@ def run_obstacle_avoidance_simulation():
             t_new = np.linspace(0, 1, num_joint_points)
             spline = interpolate.CubicSpline(t, joint_vals, bc_type='natural')
             smooth_joint_traj.append(spline(t_new))
-        # 转置为 [轨迹点, 关节]
         smooth_joint_traj = np.array(smooth_joint_traj).T
 
         traj_length = len(smooth_joint_traj)
         ee_site_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "ee_site")
-        segment_time = 5.0
+        segment_time = 8.0  # 增加轨迹时间，适配鲁棒性优化
 
         with mujoco.viewer.launch_passive(model, data) as viewer:
-            print("\n🎮 机械臂避障仿真启动！")
-            print("💡 核心功能：人工势场法避障 + 物理约束 + PD闭环控制")
-            print("💡 可视化：红色半透明球体为障碍物，机械臂自动绕开")
+            print("\n🎮 鲁棒版机械臂避障仿真启动！")
+            print("💡 核心优化：自动规避局部最优 + 自适应势场参数 + 碰撞冗余检测")
+            print("💡 可视化：深红色障碍为易导致局部最优的关键障碍")
             print("💡 按 Ctrl+C 退出")
 
             while viewer.is_running():
-                # 1. 计算当前轨迹索引
+                # 1. 时间与轨迹索引
                 t_total = data.time
                 traj_idx = int((t_total / segment_time) * traj_length) % traj_length
 
-                # 2. 获取末端当前位置
+                # 2. 获取末端位置 + 计算末端速度（兼容旧版MuJoCo）
                 ee_pos = data.site_xpos[ee_site_id].tolist()
+                ee_vel = get_ee_cartesian_velocity(model, data, ee_site_id).tolist()  # 替换site_xvel
 
                 # 3. 原始关节目标
                 raw_joint_target = smooth_joint_traj[traj_idx]
 
-                # 4. 笛卡尔空间避障修正（核心）
-                # 先通过正运动学获取原始笛卡尔目标
-                mujoco.mj_forward(model, data)  # 更新正运动学
+                # 4. 正运动学获取原始笛卡尔目标
+                mujoco.mj_forward(model, data)
                 raw_cart_target = data.site_xpos[ee_site_id].copy()
-                # 避障修正笛卡尔目标
-                corrected_cart_target = artificial_potential_field(ee_pos, raw_cart_target)
-                # 修正后的笛卡尔目标转关节目标（兼容旧版）
+
+                # 5. 鲁棒版避障修正（核心！）
+                corrected_cart_target = robust_artificial_potential_field(
+                    ee_pos, ee_vel, raw_cart_target, OBSTACLE_CONFIG["obstacle_list"]
+                )
+
+                # 6. 逆解得到修正后的关节目标
                 data.site_xpos[ee_site_id] = corrected_cart_target
                 mujoco.mj_inverse(model, data)
                 target_joints = data.qpos[:6].copy()
 
-                # 5. 应用物理约束 + 闭环控制
+                # 7. 碰撞冗余检测（安全兜底）
+                is_collision, min_safe_dist = collision_check_approx(
+                    ee_pos, target_joints, OBSTACLE_CONFIG["obstacle_list"]
+                )
+                if is_collision:
+                    # 碰撞时紧急调整：增大斥力，远离障碍
+                    emergency_rep = np.array(ee_pos) - np.array(OBSTACLE_CONFIG["obstacle_list"][0][:3])
+                    emergency_rep = emergency_rep / np.linalg.norm(emergency_rep) * 0.05
+                    corrected_cart_target = np.array(corrected_cart_target) + emergency_rep
+                    data.site_xpos[ee_site_id] = corrected_cart_target
+                    mujoco.mj_inverse(model, data)
+                    target_joints = data.qpos[:6].copy()
+                    print(f"🆘 紧急避障：修正目标位置={np.round(corrected_cart_target, 3)}")
+
+                # 8. 物理约束 + 闭环控制
                 ctrl_signals = []
                 for i in range(6):
-                    # 约束关节角度
                     target_joints[i] = np.clip(target_joints[i], model.actuator_ctrlrange[i][0],
                                                model.actuator_ctrlrange[i][1])
-                    # 闭环PD控制
                     ctrl = closed_loop_constraint_control(data, target_joints, i)
                     ctrl_signals.append(ctrl)
 
-                # 6. 发送控制指令
+                # 9. 发送控制指令
                 data.ctrl[:6] = ctrl_signals
 
-                # 7. 打印状态
+                # 10. 打印关键状态（每1秒）
                 if int(t_total) % 1 == 0 and int(t_total) != 0:
-                    obs_distances = []
-                    for obs in OBSTACLE_CONFIG["obstacle_list"]:
-                        dist = np.linalg.norm(np.array(ee_pos) - np.array(obs[:3]))
-                        obs_distances.append(dist)
+                    obs_distances = [np.linalg.norm(np.array(ee_pos) - np.array(obs[:3])) for obs in
+                                     OBSTACLE_CONFIG["obstacle_list"]]
                     min_obs_dist = min(obs_distances) if obs_distances else 0
 
                     print(f"\n⏱️  时间：{t_total:.2f}s")
-                    print(f"   末端当前位置：{np.round(ee_pos, 3)}")
-                    print(f"   修正后目标位置：{np.round(corrected_cart_target, 3)}")
-                    print(f"   与最近障碍距离：{min_obs_dist:.3f}m (斥力半径：{OBSTACLE_CONFIG['rep_radius']}m)")
+                    print(f"   末端位置：{np.round(ee_pos, 3)}")
+                    print(f"   修正目标：{np.round(corrected_cart_target, 3)}")
+                    print(f"   末端速度：{np.round(np.linalg.norm(ee_vel), 4)}m/s")
+                    print(f"   最近障碍：{min_obs_dist:.3f}m | 碰撞风险：{'是' if is_collision else '否'}")
 
-                # 8. 运行仿真步
+                # 11. 仿真步运行
                 mujoco.mj_step(model, data)
                 viewer.sync()
 
-                # 9. 帧率控制
+                # 12. 帧率控制
                 try:
                     mujoco.utils.mju_sleep(1 / 60)
                 except:
@@ -310,4 +456,4 @@ def run_obstacle_avoidance_simulation():
 
 
 if __name__ == "__main__":
-    run_obstacle_avoidance_simulation()
\ No newline at end of file
+    run_robust_obstacle_avoidance_simulation()
\ No newline at end of file

From 2d1256eeb406f73efa591becc43be48f10b7f8d5 Mon Sep 17 00:00:00 2001
From: XYTX0427 <481177497@qq.com>
Date: Wed, 24 Dec 2025 21:36:20 +0800
Subject: [PATCH 15/21] =?UTF-8?q?=E6=9C=BA=E6=A2=B0=E8=87=82=E9=81=BF?=
 =?UTF-8?q?=E9=9A=9C=E5=90=8E=E6=97=B6=E9=97=B4=E6=9C=80=E4=BC=98?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/Robotic_arm/main.py | 617 +++++++++++++++++++++++-----------------
 1 file changed, 359 insertions(+), 258 deletions(-)

diff --git a/src/Robotic_arm/main.py b/src/Robotic_arm/main.py
index 02ecbd8176..3c13f17028 100644
--- a/src/Robotic_arm/main.py
+++ b/src/Robotic_arm/main.py
@@ -5,21 +5,22 @@
 import tempfile
 import time
 from scipy import interpolate
-from sklearn.cluster import DBSCAN
+import cvxpy as cp  # 用于能耗最优的二次规划
 import warnings
 
 warnings.filterwarnings("ignore")
 
-# ====================== 1. 全局配置（鲁棒性优化参数） ======================
-# 物理约束（UR5参考）
+# ====================== 1. 全局配置（鲁棒+效率双优化） ======================
+# 物理约束（UR5工业级参数）
 CONSTRAINTS = {
-    "max_vel": [1.0, 0.8, 0.8, 1.2, 0.9, 1.2],
-    "max_acc": [0.5, 0.4, 0.4, 0.6, 0.5, 0.6],
-    "max_jerk": [0.3, 0.2, 0.2, 0.4, 0.3, 0.4],
+    "max_vel": [1.0, 0.8, 0.8, 1.2, 0.9, 1.2],  # 关节最大速度 (rad/s)
+    "max_acc": [0.5, 0.4, 0.4, 0.6, 0.5, 0.6],  # 关节最大加速度 (rad/s²)
+    "max_jerk": [0.3, 0.2, 0.2, 0.4, 0.3, 0.4],  # 关节最大加加速度 (rad/s³)
+    "max_torque": [15.0, 15.0, 10.0, 5.0, 5.0, 3.0],  # 关节最大扭矩 (N·m)
     "ctrl_limit": [-10.0, 10.0]
 }
 
-# 避障基础参数（鲁棒性优化版）
+# 避障鲁棒性参数
 OBSTACLE_CONFIG = {
     "base_k_att": 0.8,  # 基础引力系数
     "base_k_rep": 0.6,  # 基础斥力系数
@@ -34,246 +35,332 @@
     ]
 }
 
-# 笛卡尔轨迹关键点（易触发局部最优的路径）
+# 效率优化参数（工业场景可配置）
+EFFICIENCY_CONFIG = {
+    "time_weight": 0.6,  # 时间权重（0-1，越大越优先时间）
+    "energy_weight": 0.4,  # 能耗权重（0-1，越大越优先能耗）
+    "traj_interp_points": 50,  # 轨迹插值点数
+    "safety_margin": 0.05,  # 碰撞安全裕度 (m)
+    "opt_horizon": 1.0  # 优化时域 (s)
+}
+
+# 笛卡尔轨迹关键点（工业典型路径）
 CART_WAYPOINTS = [
     [0.5, 0.0, 0.6],  # 起点
-    [0.6, 0.0, 0.58],  # 中间点（障碍夹缝，易局部最优）
+    [0.6, 0.0, 0.58],  # 中间点（障碍夹缝）
     [0.8, 0.1, 0.8],  # 终点
     [0.6, 0.0, 0.58],  # 回中间点
     [0.5, 0.0, 0.6]  # 回起点
 ]
 
-# 全局变量：记录停滞开始时间
+# 全局变量
 stagnant_start_time = None
+total_motion_time = 0.0  # 累计运动时间
+total_energy_consume = 0.0  # 累计能耗
 
+# 预定义关节惯性参数（适配所有MuJoCo版本）
+JOINT_INERTIA = [0.01, 0.02, 0.015, 0.01, 0.008, 0.005]
+JOINT_GRAVITY = [0.5, 0.8, 0.6, 0.3, 0.2, 0.1]
 
-# ====================== 2. 新增：兼容所有版本的末端速度计算 ======================
-def get_ee_cartesian_velocity(model, data, ee_site_id):
-    """
-    计算末端执行器的笛卡尔速度（兼容所有MuJoCo版本）
-    原理：通过雅可比矩阵将关节速度转换为末端笛卡尔速度
-    """
-    # 获取雅可比矩阵（6xN，前3行是平移速度，后3行是旋转速度）
-    jacp = np.zeros((3, model.nv))  # 平移雅可比
-    jacr = np.zeros((3, model.nv))  # 旋转雅可比
 
-    # 计算末端site的雅可比矩阵
+# ====================== 2. 基础工具函数（兼容所有MuJoCo版本） ======================
+def get_ee_cartesian_velocity(model, data, ee_site_id):
+    """计算末端笛卡尔速度（兼容所有MuJoCo版本）"""
+    jacp = np.zeros((3, model.nv))
+    jacr = np.zeros((3, model.nv))
     mujoco.mj_jacSite(model, data, jacp, jacr, ee_site_id)
-
-    # 关节速度（data.qvel）
-    joint_vel = data.qvel[:6]  # 仅取前6个关节速度
-
-    # 笛卡尔平移速度 = 雅可比 × 关节速度
+    joint_vel = data.qvel[:6]
     ee_cart_vel = jacp @ joint_vel
-
     return ee_cart_vel
 
 
-# ====================== 3. 物理约束轨迹生成（原有逻辑） ======================
-def constrained_quintic_polynomial(start, end, total_time, t, joint_idx):
-    s0, v0, a0 = start, 0, 0
-    s1, v1, a1 = end, 0, 0
-
-    T = total_time
-    a = s0
-    b = v0
-    c = a0 / 2
-    d = (20 * (s1 - s0) - (8 * v1 + 12 * v0) * T - (3 * a0 - a1) * T ** 2) / (2 * T ** 3)
-    e = (30 * (s0 - s1) + (14 * v1 + 16 * v0) * T + (3 * a0 - 2 * a1) * T ** 2) / (2 * T ** 4)
-    f = (12 * (s1 - s0) - (6 * v1 + 6 * v0) * T - (a0 - a1) * T ** 2) / (2 * T ** 5)
-
-    pos = a + b * t + c * t ** 2 + d * t ** 3 + e * t ** 4 + f * t ** 5
-    vel = b + 2 * c * t + 3 * d * t ** 2 + 4 * e * t ** 3 + 5 * f * t ** 4
-    acc = 2 * c + 6 * d * t + 12 * e * t ** 2 + 20 * f * t ** 4
-
-    vel = np.clip(vel, -CONSTRAINTS["max_vel"][joint_idx], CONSTRAINTS["max_vel"][joint_idx])
-    acc = np.clip(acc, -CONSTRAINTS["max_acc"][joint_idx], CONSTRAINTS["max_acc"][joint_idx])
+def calculate_joint_torque(model, data, joint_idx):
+    """计算关节扭矩（能耗核心指标，适配新版MuJoCo）"""
+    # 方案1：使用预定义惯性参数（兼容所有版本）
+    inertia = JOINT_INERTIA[joint_idx]
+    gravity_comp = JOINT_GRAVITY[joint_idx]
 
-    return pos, vel, acc
+    # 方案2（备选）：从data中获取实时扭矩（新版MuJoCo推荐）
+    # torque = data.qfrc_actuator[joint_idx]  # 实际输出扭矩
 
+    # 计算关节加速度（数值微分）
+    joint_acc = np.gradient(data.qvel[joint_idx]) if data.time > 0 else 0.0
+    torque = inertia * joint_acc + gravity_comp
+    torque = np.clip(torque, -CONSTRAINTS["max_torque"][joint_idx], CONSTRAINTS["max_torque"][joint_idx])
+    return torque
 
-# ====================== 4. 闭环PD控制（原有逻辑） ======================
-def closed_loop_constraint_control(data, target_joints, joint_idx):
-    k_p = 8.0
-    k_d = 0.2
 
-    current_pos = data.qpos[joint_idx]
-    current_vel = data.qvel[joint_idx]
-
-    pos_error = target_joints[joint_idx] - current_pos
-    vel_error = -current_vel
-
-    ctrl = k_p * pos_error + k_d * vel_error
-    ctrl = np.clip(ctrl, CONSTRAINTS["ctrl_limit"][0], CONSTRAINTS["ctrl_limit"][1])
-
-    return ctrl
-
-
-# ====================== 5. 鲁棒性优化1：局部最优检测与规避 ======================
+# ====================== 3. 鲁棒性避障（保留原有核心逻辑） ======================
 def check_local_optimum(ee_vel, ee_pos, target_pos):
-    """
-    检测是否陷入局部最优，并生成引导目标跳出陷阱
-    :return: is_local_opt (是否局部最优), guide_target (引导目标位置)
-    """
+    """检测局部最优并生成引导目标"""
     global stagnant_start_time
-
-    # 计算末端合速度
     vel_mag = np.linalg.norm(ee_vel)
-    threshold = OBSTACLE_CONFIG["stagnant_threshold"]
-    max_stagnant_time = OBSTACLE_CONFIG["stagnant_time"]
-
-    if vel_mag < threshold:
+    if vel_mag < OBSTACLE_CONFIG["stagnant_threshold"]:
         if stagnant_start_time is None:
             stagnant_start_time = time.time()
-        # 超过停滞时间，判定为局部最优
-        elif time.time() - stagnant_start_time > max_stagnant_time:
-            print(f"\n⚠️  检测到局部最优！末端速度={vel_mag:.4f}m/s < 阈值={threshold}m/s")
-            # 生成引导目标：向原始目标方向偏移
+        elif time.time() - stagnant_start_time > OBSTACLE_CONFIG["stagnant_time"]:
+            print(f"\n⚠️  检测到局部最优！末端速度={vel_mag:.4f}m/s")
             dir_to_target = np.array(target_pos) - np.array(ee_pos)
-            if np.linalg.norm(dir_to_target) < 1e-6:
-                dir_to_target = np.array([0.0, 0.0, 0.1])  # 避免除零
-            else:
-                dir_to_target = dir_to_target / np.linalg.norm(dir_to_target)
-
+            dir_to_target = dir_to_target / np.linalg.norm(dir_to_target) if np.linalg.norm(
+                dir_to_target) > 1e-6 else np.array([0, 0, 0.1])
             guide_target = np.array(ee_pos) + dir_to_target * OBSTACLE_CONFIG["guide_offset"]
-            print(f"📌 生成引导目标：{np.round(guide_target, 3)} (偏移{OBSTACLE_CONFIG['guide_offset']}m)")
-            stagnant_start_time = None  # 重置计时器
+            stagnant_start_time = None
             return True, guide_target.tolist()
     else:
-        stagnant_start_time = None  # 速度正常，重置计时器
-
+        stagnant_start_time = None
     return False, target_pos
 
 
-# ====================== 6. 鲁棒性优化2：自适应势场参数 ======================
 def adaptive_potential_params(ee_pos, obstacle_list):
-    """
-    根据障碍距离/数量自适应调整引力/斥力系数
-    - 距离越近，斥力越大；障碍越多，引力越小
-    """
-    base_k_att = OBSTACLE_CONFIG["base_k_att"]
-    base_k_rep = OBSTACLE_CONFIG["base_k_rep"]
-
-    # 计算与最近障碍的距离
+    """自适应势场参数"""
     obs_distances = [np.linalg.norm(np.array(ee_pos) - np.array(obs[:3])) for obs in obstacle_list]
     min_dist = min(obs_distances) if obs_distances else 1.0
-    obs_count = len(obstacle_list)
-
-    # 距离自适应斥力系数：距离<0.2m时，斥力翻倍
-    k_rep = base_k_rep if min_dist > 0.2 else base_k_rep * 2.0
-    # 数量自适应引力系数：障碍>2个时，引力降低50%
-    k_att = base_k_att if obs_count <= 2 else base_k_att * 0.5
-
+    k_rep = OBSTACLE_CONFIG["base_k_rep"] if min_dist > 0.2 else OBSTACLE_CONFIG["base_k_rep"] * 2.0
+    k_att = OBSTACLE_CONFIG["base_k_att"] if len(obstacle_list) <= 2 else OBSTACLE_CONFIG["base_k_att"] * 0.5
     return k_att, k_rep
 
 
-# ====================== 7. 鲁棒性优化3：碰撞冗余检测 ======================
-def collision_check_approx(ee_pos, joint_pos, obstacle_list, safety_margin=0.05):
-    """
-    近似碰撞检测（工程简化版）：检测末端+关键关节与障碍的距离
-    :return: is_collision (是否碰撞), min_safe_dist (最小安全距离)
-    """
-    # 检测末端执行器
-    ee_collision = False
-    min_ee_dist = 100.0
-    for obs in obstacle_list:
-        obs_pos = np.array(obs[:3])
-        obs_radius = obs[3]
-        dist = np.linalg.norm(np.array(ee_pos) - obs_pos)
-        min_ee_dist = min(min_ee_dist, dist)
-        if dist < obs_radius + safety_margin:
-            ee_collision = True
-            break
-
-    # 检测关键关节（简化：仅检测关节2/3/4）
-    joint_collision = False
-    # 仿真中通过data获取关节位置（实际场景需正运动学计算）
-    # 这里简化为基于关节角度的近似检测
-    joint_2_3_4_idx = [2, 3, 4]
-    for idx in joint_2_3_4_idx:
-        # 近似关节位置（基于机械臂模型）
-        joint_pos_approx = np.array([
-            0.4 + 0.35 * np.cos(joint_pos[2]),
-            0.0 + 0.35 * np.sin(joint_pos[2]),
-            0.5 + 0.25 * np.sin(joint_pos[3])
-        ])
-        for obs in obstacle_list:
-            obs_pos = np.array(obs[:3])
-            obs_radius = obs[3]
-            dist = np.linalg.norm(joint_pos_approx - obs_pos)
-            if dist < obs_radius + safety_margin:
-                joint_collision = True
-                break
-        if joint_collision:
-            break
-
-    is_collision = ee_collision or joint_collision
-    if is_collision:
-        print(f"\n🚨 碰撞风险！末端与最近障碍距离={min_ee_dist:.3f}m < 安全裕度={safety_margin}m")
-
-    return is_collision, min_ee_dist
-
-
-# ====================== 8. 鲁棒性优化后的避障核心逻辑 ======================
 def robust_artificial_potential_field(ee_pos, ee_vel, target_pos, obstacle_list):
-    """
-    鲁棒版人工势场法：局部最优规避 + 自适应参数
-    """
+    """鲁棒版人工势场法"""
     ee_pos = np.array(ee_pos)
     target_pos = np.array(target_pos)
-    rep_radius = OBSTACLE_CONFIG["rep_radius"]
 
-    # 步骤1：检测局部最优，生成引导目标
+    # 局部最优规避
     is_local_opt, guide_target = check_local_optimum(ee_vel, ee_pos, target_pos)
     current_target = np.array(guide_target) if is_local_opt else target_pos
 
-    # 步骤2：自适应调整引力/斥力系数
+    # 自适应参数
     k_att, k_rep = adaptive_potential_params(ee_pos, obstacle_list)
-    print(
-        f"\n🔧 自适应参数：k_att={k_att:.1f}, k_rep={k_rep:.1f} (最近障碍距离={min([np.linalg.norm(ee_pos - np.array(obs[:3])) for obs in obstacle_list]):.3f}m)")
 
-    # 步骤3：计算引力（指向当前目标）
+    # 引力+斥力计算
     att_force = k_att * (current_target - ee_pos)
-
-    # 步骤4：计算斥力（远离所有障碍）
     rep_force = np.zeros(3)
     for obs in obstacle_list:
         obs_pos = np.array(obs[:3])
         obs_radius = obs[3]
         dist = np.linalg.norm(ee_pos - obs_pos)
+        if dist < OBSTACLE_CONFIG["rep_radius"] + obs_radius:
+            rep_dir = (ee_pos - obs_pos) / (dist + 1e-6)
+            rep_force += k_rep * (1 / (dist - obs_radius) - 1 / OBSTACLE_CONFIG["rep_radius"]) * (
+                        1 / dist ** 2) * rep_dir
 
-        if dist < rep_radius + obs_radius:
-            if dist < 1e-6:
-                dist = 1e-6
-            rep_dir = (ee_pos - obs_pos) / dist
-            # 优化斥力公式：避免距离过近时斥力突变
-            rep_force += k_rep * (1 / (dist - obs_radius) - 1 / rep_radius) * (1 / dist ** 2) * rep_dir
-
-    # 步骤5：合力修正目标位置，添加边界约束
+    # 修正目标并约束
     corrected_target = ee_pos + att_force + rep_force
     corrected_target = np.clip(corrected_target, [0.3, -0.4, 0.2], [0.9, 0.4, 1.0])
 
     return corrected_target.tolist()
 
 
-# ====================== 9. 逆运动学预计算（兼容旧版MuJoCo） ======================
-def precompute_joint_waypoints(model, data, cart_waypoints):
-    joint_waypoints = []
+def collision_check_approx(ee_pos, joint_pos, obstacle_list):
+    """碰撞冗余检测"""
+    ee_collision = False
+    min_ee_dist = 100.0
+    for obs in obstacle_list:
+        obs_pos = np.array(obs[:3])
+        obs_radius = obs[3]
+        dist = np.linalg.norm(np.array(ee_pos) - obs_pos)
+        min_ee_dist = min(min_ee_dist, dist)
+        if dist < obs_radius + EFFICIENCY_CONFIG["safety_margin"]:
+            ee_collision = True
+            break
+    return ee_collision, min_ee_dist
+
+
+# ====================== 4. 效率优化核心：时间最优轨迹规划 ======================
+def time_optimal_joint_trajectory(start_joint, end_joint, seg_time):
+    """
+    时间最优关节轨迹（梯形速度曲线，满足速度/加速度约束）
+    :return: 时间最优的关节位置/速度/加速度轨迹
+    """
+    n_joints = 6
+    traj_points = EFFICIENCY_CONFIG["traj_interp_points"]
+    t_steps = np.linspace(0, seg_time, traj_points)
+
+    # 初始化轨迹数组
+    opt_pos = np.zeros((traj_points, n_joints))
+    opt_vel = np.zeros((traj_points, n_joints))
+    opt_acc = np.zeros((traj_points, n_joints))
+
+    for j in range(n_joints):
+        delta = end_joint[j] - start_joint[j]
+        max_vel = CONSTRAINTS["max_vel"][j]
+        max_acc = CONSTRAINTS["max_acc"][j]
+
+        # 计算梯形速度曲线的关键时间点
+        t_acc = max_vel / max_acc  # 加速时间
+        s_acc = 0.5 * max_acc * t_acc ** 2  # 加速段位移
+
+        if abs(delta) < 2 * s_acc:
+            # 三角形速度曲线（未到最大速度）
+            t_joint = 2 * np.sqrt(abs(delta) / max_acc)
+            for i, t in enumerate(t_steps):
+                if t <= t_joint / 2:
+                    opt_pos[i, j] = start_joint[j] + 0.5 * max_acc * t ** 2 * np.sign(delta)
+                    opt_vel[i, j] = max_acc * t * np.sign(delta)
+                    opt_acc[i, j] = max_acc * np.sign(delta)
+                else:
+                    t_rem = t_joint - t
+                    opt_pos[i, j] = end_joint[j] - 0.5 * max_acc * t_rem ** 2 * np.sign(delta)
+                    opt_vel[i, j] = max_acc * t_rem * np.sign(delta)
+                    opt_acc[i, j] = -max_acc * np.sign(delta)
+        else:
+            # 梯形速度曲线（达到最大速度）
+            t_const = (abs(delta) - 2 * s_acc) / max_vel  # 匀速时间
+            t_joint = 2 * t_acc + t_const
+            for i, t in enumerate(t_steps):
+                if t <= t_acc:
+                    # 加速段
+                    opt_pos[i, j] = start_joint[j] + 0.5 * max_acc * t ** 2 * np.sign(delta)
+                    opt_vel[i, j] = max_acc * t * np.sign(delta)
+                    opt_acc[i, j] = max_acc * np.sign(delta)
+                elif t <= t_acc + t_const:
+                    # 匀速段
+                    opt_pos[i, j] = start_joint[j] + (s_acc + max_vel * (t - t_acc)) * np.sign(delta)
+                    opt_vel[i, j] = max_vel * np.sign(delta)
+                    opt_acc[i, j] = 0.0
+                else:
+                    # 减速段
+                    t_rem = t_joint - t
+                    opt_pos[i, j] = end_joint[j] - 0.5 * max_acc * t_rem ** 2 * np.sign(delta)
+                    opt_vel[i, j] = max_acc * t_rem * np.sign(delta)
+                    opt_acc[i, j] = -max_acc * np.sign(delta)
+
+        # 约束速度/加速度
+        opt_vel[:, j] = np.clip(opt_vel[:, j], -max_vel, max_vel)
+        opt_acc[:, j] = np.clip(opt_acc[:, j], -max_acc, max_acc)
+
+    return opt_pos, opt_vel, opt_acc
+
+
+# ====================== 5. 效率优化核心：能耗最优二次规划（兼容所有求解器） ======================
+def energy_optimal_trajectory(joint_waypoints, seg_time):
+    """
+    能耗最优轨迹（二次规划求解，最小化扭矩平方积分）
+    :return: 能耗最优的关节位置轨迹
+    """
+    n_joints = 6
+    n_points = len(joint_waypoints)
+    t_step = seg_time / (n_points - 1)
+
+    # 定义优化变量
+    q = cp.Variable((n_joints, n_points))  # 关节位置
+    qd = cp.Variable((n_joints, n_points))  # 关节速度
+    qdd = cp.Variable((n_joints, n_points))  # 关节加速度
+
+    # 代价函数：最小化能耗（扭矩平方积分≈加速度平方积分）
+    energy_cost = cp.sum_squares(qdd)
+    time_cost = cp.sum(cp.max(cp.abs(qd), axis=1))  # 时间代价：速度越大时间越短
+    total_cost = EFFICIENCY_CONFIG["time_weight"] * time_cost + EFFICIENCY_CONFIG["energy_weight"] * energy_cost
+
+    # 约束条件
+    constraints = []
+    # 初始/终止条件
+    constraints.append(q[:, 0] == joint_waypoints[0])
+    constraints.append(q[:, -1] == joint_waypoints[-1])
+    constraints.append(qd[:, 0] == 0)
+    constraints.append(qd[:, -1] == 0)
+    # 速度/加速度约束
+    for j in range(n_joints):
+        constraints.append(qd[j, :] <= CONSTRAINTS["max_vel"][j])
+        constraints.append(qd[j, :] >= -CONSTRAINTS["max_vel"][j])
+        constraints.append(qdd[j, :] <= CONSTRAINTS["max_acc"][j])
+        constraints.append(qdd[j, :] >= -CONSTRAINTS["max_acc"][j])
+    # 动力学约束（差分）
+    for i in range(n_points - 1):
+        constraints.append(qd[:, i + 1] == (q[:, i + 1] - q[:, i]) / t_step)
+        constraints.append(qdd[:, i + 1] == (qd[:, i + 1] - qd[:, i]) / t_step)
+
+    # 求解二次规划（自动选择可用求解器，增加容错）
+    prob = cp.Problem(cp.Minimize(total_cost), constraints)
+    try:
+        # 优先尝试ECOS求解器
+        prob.solve(solver=cp.ECOS, verbose=False)
+    except:
+        try:
+            # 备选：OSQP求解器（CVXPY默认推荐）
+            prob.solve(solver=cp.OSQP, verbose=False)
+        except:
+            # 最后：使用CVXPY自动选择的求解器
+            prob.solve(verbose=False)
+
+    if prob.status != cp.OPTIMAL:
+        print("⚠️  能耗优化求解失败，降级为时间最优轨迹")
+        return None
+    return q.value.T
+
+
+# ====================== 6. 效率+鲁棒融合：避障轨迹的双优优化（修复索引越界） ======================
+def optimize_obstacle_traj_with_efficiency(model, data, ee_pos, target_pos, obstacle_list):
+    """
+    融合避障鲁棒性+时间/能耗最优的轨迹规划
+    :return: 优化后的关节目标、当前段能耗
+    """
+    global total_motion_time, total_energy_consume
+
+    # 步骤1：鲁棒避障修正笛卡尔目标
+    ee_vel = get_ee_cartesian_velocity(model, data, mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "ee_site"))
+    corrected_cart_target = robust_artificial_potential_field(ee_pos, ee_vel, target_pos, obstacle_list)
+
+    # 步骤2：逆解得到关节目标
     ee_site_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "ee_site")
+    data.site_xpos[ee_site_id] = corrected_cart_target
+    mujoco.mj_inverse(model, data)
+    end_joint = data.qpos[:6].copy()
+    start_joint = data.qpos[:6].copy()  # 当前关节位置为起点
+
+    # 步骤3：时间最优轨迹初值
+    seg_time = 2.0  # 初始段时间
+    time_opt_pos, time_opt_vel, time_opt_acc = time_optimal_joint_trajectory(start_joint, end_joint, seg_time)
+
+    # 步骤4：能耗最优优化（二次规划）
+    energy_opt_pos = energy_optimal_trajectory(time_opt_pos, seg_time)
+    if energy_opt_pos is None:
+        final_joint_traj = time_opt_pos
+    else:
+        final_joint_traj = energy_opt_pos
+
+    # 步骤5：计算当前段能耗（扭矩平方积分，修复索引越界错误）
+    seg_energy = 0.0
+    # 遍历每个轨迹点
+    for traj_idx in range(len(final_joint_traj)):
+        # 跳过第一个点（无加速度）
+        if traj_idx == 0:
+            continue
+
+        # 遍历每个关节计算扭矩
+        for joint_idx in range(6):
+            # 获取当前轨迹点和上一个轨迹点的关节角度
+            curr_angle = final_joint_traj[traj_idx, joint_idx]
+            prev_angle = final_joint_traj[traj_idx - 1, joint_idx]
+
+            # 计算关节速度（差分）
+            dt = seg_time / len(final_joint_traj)
+            joint_vel = (curr_angle - prev_angle) / dt
+
+            # 计算关节加速度（差分，使用前一个速度）
+            if traj_idx == 1:
+                joint_acc = joint_vel / dt
+            else:
+                prev_vel = (final_joint_traj[traj_idx - 1, joint_idx] - final_joint_traj[traj_idx - 2, joint_idx]) / dt
+                joint_acc = (joint_vel - prev_vel) / dt
+
+            # 计算扭矩和能耗（积分）
+            torque = JOINT_INERTIA[joint_idx] * joint_acc + JOINT_GRAVITY[joint_idx]
+            seg_energy += np.square(torque) * dt
 
-    for cart_pos in cart_waypoints:
-        mujoco.mj_resetData(model, data)
-        data.site_xpos[ee_site_id] = cart_pos
-        mujoco.mj_inverse(model, data)
-        joint_waypoints.append(data.qpos[:6].copy())
+    # 更新全局统计
+    total_motion_time += seg_time
+    total_energy_consume += seg_energy
 
-    return joint_waypoints
+    # 返回当前时刻的关节目标（取第一个插值点）
+    return final_joint_traj[0], corrected_cart_target, seg_energy
 
 
-# ====================== 10. 机械臂模型（带密集障碍可视化） ======================
+# ====================== 7. 机械臂模型（修复XML语法错误） ======================
 def get_arm_xml_with_obstacles():
+    """生成带障碍的机械臂XML模型（修复inertial标签错误）"""
     arm_xml = """
-<mujoco model="6dof_arm_with_obstacles_robust">
+<mujoco model="6dof_arm_efficiency_optimized">
   <compiler angle="radian" inertiafromgeom="true"/>
   <option timestep="0.005" gravity="0 0 -9.81"/>
   <asset>
@@ -281,29 +368,43 @@ def get_arm_xml_with_obstacles():
     <material name="blue" rgba="0.2 0.4 0.8 1"/>
     <material name="red" rgba="0.8 0.2 0.2 1"/>
     <material name="obstacle" rgba="1 0 0 0.5"/>
-    <material name="critical_obstacle" rgba="1 0 0 0.7"/>  <!-- 易导致局部最优的障碍 -->
+    <material name="critical_obstacle" rgba="1 0 0 0.7"/>
   </asset>
   <worldbody>
     <geom name="floor" type="plane" size="5 5 0.1" pos="0 0 0" material="gray"/>
+    <!-- 基座（包含inertial标签） -->
     <body name="base" pos="0 0 0">
+      <inertial pos="0 0 0" mass="5.0" diaginertia="0.01 0.01 0.01"/>
       <geom name="base_geom" type="cylinder" size="0.15 0.1" pos="0 0 0" material="gray"/>
       <joint name="joint0" type="hinge" axis="0 0 1" pos="0 0 0.1"/>
+      <!-- 连杆1 -->
       <body name="link1" pos="0 0 0.1">
+        <inertial pos="0 0 0.15" mass="1.2" diaginertia="0.02 0.02 0.02"/>
         <geom name="link1_geom" type="capsule" size="0.05" fromto="0 0 0 0 0 0.3" material="blue"/>
         <joint name="joint1" type="hinge" axis="0 1 0" pos="0 0 0.3"/>
+        <!-- 连杆2 -->
         <body name="link2" pos="0 0 0.3">
+          <inertial pos="0.2 0 0" mass="1.0" diaginertia="0.015 0.015 0.015"/>
           <geom name="link2_geom" type="capsule" size="0.05" fromto="0 0 0 0.4 0 0" material="blue"/>
           <joint name="joint2" type="hinge" axis="0 1 0" pos="0.4 0 0"/>
+          <!-- 连杆3 -->
           <body name="link3" pos="0.4 0 0">
+            <inertial pos="0.175 0 0" mass="0.8" diaginertia="0.01 0.01 0.01"/>
             <geom name="link3_geom" type="capsule" size="0.04" fromto="0 0 0 0.35 0 0" material="blue"/>
             <joint name="joint3" type="hinge" axis="1 0 0" pos="0.35 0 0"/>
+            <!-- 连杆4 -->
             <body name="link4" pos="0.35 0 0">
+              <inertial pos="0 0 0.125" mass="0.6" diaginertia="0.008 0.008 0.008"/>
               <geom name="link4_geom" type="capsule" size="0.04" fromto="0 0 0 0 0 0.25" material="blue"/>
               <joint name="joint4" type="hinge" axis="0 1 0" pos="0 0 0.25"/>
+              <!-- 连杆5 -->
               <body name="link5" pos="0 0 0.25">
+                <inertial pos="0 0 0.1" mass="0.4" diaginertia="0.008 0.008 0.008"/>
                 <geom name="link5_geom" type="capsule" size="0.03" fromto="0 0 0 0 0 0.2" material="blue"/>
                 <joint name="joint5" type="hinge" axis="1 0 0" pos="0 0 0.2"/>
+                <!-- 末端执行器 -->
                 <body name="end_effector" pos="0 0 0.2">
+                  <inertial pos="0 0 0" mass="0.2" diaginertia="0.005 0.005 0.005"/>
                   <geom name="ee_geom" type="box" size="0.08 0.08 0.08" pos="0 0 0" material="red"/>
                   <site name="ee_site" pos="0 0 0" type="sphere" size="0.01" rgba="1 0 0 1"/>
                 </body>
@@ -313,16 +414,15 @@ def get_arm_xml_with_obstacles():
         </body>
       </body>
     </body>
+    <!-- 障碍物 -->
     """
-
-    # 不同障碍用不同透明度区分（关键障碍更醒目）
+    # 添加障碍
     for i, obs in enumerate(OBSTACLE_CONFIG["obstacle_list"]):
         x, y, z, r = obs
         material = "critical_obstacle" if i == 0 else "obstacle"
         arm_xml += f"""
     <geom name="obstacle_{i}" type="sphere" size="{r}" pos="{x} {y} {z}" material="{material}"/>
         """
-
     arm_xml += """
   </worldbody>
   <actuator>
@@ -338,9 +438,12 @@ def get_arm_xml_with_obstacles():
     return arm_xml
 
 
-# ====================== 11. 主仿真逻辑（鲁棒性优化核心） ======================
-def run_robust_obstacle_avoidance_simulation():
+# ====================== 8. 仿真主逻辑 ======================
+def run_efficiency_optimized_simulation():
+    """运行效率+鲁棒双优化的仿真"""
+    global total_motion_time, total_energy_consume
     arm_xml = get_arm_xml_with_obstacles()
+
     with tempfile.NamedTemporaryFile(mode='w', suffix='.xml', delete=False) as f:
         f.write(arm_xml)
         xml_path = f.name
@@ -348,100 +451,90 @@ def run_robust_obstacle_avoidance_simulation():
     try:
         model = mujoco.MjModel.from_xml_path(xml_path)
         data = mujoco.MjData(model)
-        print("✅ 鲁棒版避障机械臂模型加载成功！")
-        print(f"🔧 鲁棒性优化：局部最优规避 + 自适应参数 + 碰撞冗余检测")
-        print(f"🔧 障碍数量：{len(OBSTACLE_CONFIG['obstacle_list'])} (含1个易导致局部最优的关键障碍)")
-
-        # 预计算关节轨迹
-        joint_waypoints = precompute_joint_waypoints(model, data, CART_WAYPOINTS)
-        num_joint_points = 200
-        smooth_joint_traj = []
-        for joint_idx in range(6):
-            joint_vals = [wp[joint_idx] for wp in joint_waypoints]
-            t = np.linspace(0, 1, len(joint_vals))
-            t_new = np.linspace(0, 1, num_joint_points)
-            spline = interpolate.CubicSpline(t, joint_vals, bc_type='natural')
-            smooth_joint_traj.append(spline(t_new))
-        smooth_joint_traj = np.array(smooth_joint_traj).T
-
-        traj_length = len(smooth_joint_traj)
+        print("✅ 效率+鲁棒双优化机械臂模型加载成功！")
+        print(f"🔧 效率配置：时间权重={EFFICIENCY_CONFIG['time_weight']}, 能耗权重={EFFICIENCY_CONFIG['energy_weight']}")
+        print(f"🔧 鲁棒配置：局部最优规避 + 自适应势场 + 碰撞冗余检测")
+
+        # 预计算笛卡尔轨迹对应的关节起点
         ee_site_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "ee_site")
-        segment_time = 8.0  # 增加轨迹时间，适配鲁棒性优化
+        joint_waypoints = []
+        for cart_pos in CART_WAYPOINTS:
+            mujoco.mj_resetData(model, data)
+            data.site_xpos[ee_site_id] = cart_pos
+            mujoco.mj_inverse(model, data)
+            joint_waypoints.append(data.qpos[:6].copy())
 
         with mujoco.viewer.launch_passive(model, data) as viewer:
-            print("\n🎮 鲁棒版机械臂避障仿真启动！")
-            print("💡 核心优化：自动规避局部最优 + 自适应势场参数 + 碰撞冗余检测")
-            print("💡 可视化：深红色障碍为易导致局部最优的关键障碍")
-            print("💡 按 Ctrl+C 退出")
+            print("\n🎮 效率+鲁棒双优化仿真启动！")
+            print("💡 核心：避障安全 + 时间/能耗最优（工业级需求）")
+            print("💡 按 Ctrl+C 退出\n")
+
+            traj_idx = 0
+            current_waypoint = 0
+            last_print_time = 0.0
 
             while viewer.is_running():
-                # 1. 时间与轨迹索引
                 t_total = data.time
-                traj_idx = int((t_total / segment_time) * traj_length) % traj_length
-
-                # 2. 获取末端位置 + 计算末端速度（兼容旧版MuJoCo）
                 ee_pos = data.site_xpos[ee_site_id].tolist()
-                ee_vel = get_ee_cartesian_velocity(model, data, ee_site_id).tolist()  # 替换site_xvel
 
-                # 3. 原始关节目标
-                raw_joint_target = smooth_joint_traj[traj_idx]
-
-                # 4. 正运动学获取原始笛卡尔目标
-                mujoco.mj_forward(model, data)
-                raw_cart_target = data.site_xpos[ee_site_id].copy()
-
-                # 5. 鲁棒版避障修正（核心！）
-                corrected_cart_target = robust_artificial_potential_field(
-                    ee_pos, ee_vel, raw_cart_target, OBSTACLE_CONFIG["obstacle_list"]
+                # 切换笛卡尔目标点
+                if current_waypoint < len(CART_WAYPOINTS):
+                    target_cart = CART_WAYPOINTS[current_waypoint]
+                    # 距离目标点<0.01m时切换下一个
+                    if np.linalg.norm(np.array(ee_pos) - np.array(target_cart)) < 0.01:
+                        current_waypoint = (current_waypoint + 1) % len(CART_WAYPOINTS)
+                        print(f"\n🔄 切换到目标点 {current_waypoint}: {np.round(target_cart, 3)}")
+                else:
+                    target_cart = CART_WAYPOINTS[-1]
+
+                # 融合避障+效率优化的轨迹规划
+                target_joints, corrected_cart, seg_energy = optimize_obstacle_traj_with_efficiency(
+                    model, data, ee_pos, target_cart, OBSTACLE_CONFIG["obstacle_list"]
                 )
 
-                # 6. 逆解得到修正后的关节目标
-                data.site_xpos[ee_site_id] = corrected_cart_target
-                mujoco.mj_inverse(model, data)
-                target_joints = data.qpos[:6].copy()
-
-                # 7. 碰撞冗余检测（安全兜底）
-                is_collision, min_safe_dist = collision_check_approx(
-                    ee_pos, target_joints, OBSTACLE_CONFIG["obstacle_list"]
-                )
+                # 碰撞检测与紧急避障
+                is_collision, min_obs_dist = collision_check_approx(ee_pos, target_joints,
+                                                                    OBSTACLE_CONFIG["obstacle_list"])
                 if is_collision:
-                    # 碰撞时紧急调整：增大斥力，远离障碍
                     emergency_rep = np.array(ee_pos) - np.array(OBSTACLE_CONFIG["obstacle_list"][0][:3])
                     emergency_rep = emergency_rep / np.linalg.norm(emergency_rep) * 0.05
-                    corrected_cart_target = np.array(corrected_cart_target) + emergency_rep
-                    data.site_xpos[ee_site_id] = corrected_cart_target
+                    corrected_cart = np.array(corrected_cart) + emergency_rep
+                    data.site_xpos[ee_site_id] = corrected_cart
                     mujoco.mj_inverse(model, data)
                     target_joints = data.qpos[:6].copy()
-                    print(f"🆘 紧急避障：修正目标位置={np.round(corrected_cart_target, 3)}")
+                    print(f"🆘 紧急避障：修正目标={np.round(corrected_cart, 3)}")
 
-                # 8. 物理约束 + 闭环控制
+                # 闭环PD控制（带扭矩约束）
                 ctrl_signals = []
                 for i in range(6):
-                    target_joints[i] = np.clip(target_joints[i], model.actuator_ctrlrange[i][0],
-                                               model.actuator_ctrlrange[i][1])
-                    ctrl = closed_loop_constraint_control(data, target_joints, i)
+                    k_p = 8.0
+                    k_d = 0.2
+                    current_pos = data.qpos[i]
+                    current_vel = data.qvel[i]
+                    pos_error = target_joints[i] - current_pos
+                    vel_error = -current_vel
+                    ctrl = k_p * pos_error + k_d * vel_error
+                    # 扭矩约束（转换为控制量约束）
+                    max_ctrl = CONSTRAINTS["max_torque"][i] / 100.0  # gear=100，直接计算
+                    ctrl = np.clip(ctrl, -max_ctrl, max_ctrl)
                     ctrl_signals.append(ctrl)
-
-                # 9. 发送控制指令
                 data.ctrl[:6] = ctrl_signals
 
-                # 10. 打印关键状态（每1秒）
-                if int(t_total) % 1 == 0 and int(t_total) != 0:
-                    obs_distances = [np.linalg.norm(np.array(ee_pos) - np.array(obs[:3])) for obs in
-                                     OBSTACLE_CONFIG["obstacle_list"]]
-                    min_obs_dist = min(obs_distances) if obs_distances else 0
-
-                    print(f"\n⏱️  时间：{t_total:.2f}s")
-                    print(f"   末端位置：{np.round(ee_pos, 3)}")
-                    print(f"   修正目标：{np.round(corrected_cart_target, 3)}")
-                    print(f"   末端速度：{np.round(np.linalg.norm(ee_vel), 4)}m/s")
-                    print(f"   最近障碍：{min_obs_dist:.3f}m | 碰撞风险：{'是' if is_collision else '否'}")
-
-                # 11. 仿真步运行
+                # 打印效率统计（每2秒）
+                if t_total - last_print_time > 2.0 and t_total > 0:
+                    ee_vel = get_ee_cartesian_velocity(model, data, ee_site_id)
+                    avg_vel = np.linalg.norm(ee_vel)
+                    avg_energy = total_energy_consume / t_total if t_total > 0 else 0.0
+                    print(f"\n⏱️  时间：{t_total:.2f}s | 累计运动时间：{total_motion_time:.2f}s")
+                    print(f"   末端位置：{np.round(ee_pos, 3)} | 目标位置：{np.round(corrected_cart, 3)}")
+                    print(f"   末端速度：{avg_vel:.4f}m/s | 最近障碍：{min_obs_dist:.3f}m")
+                    print(f"   累计能耗：{total_energy_consume:.2f}J | 平均能耗：{avg_energy:.2f}J/s")
+                    print(f"   碰撞风险：{'是' if is_collision else '否'}")
+                    last_print_time = t_total
+
+                # 仿真步运行
                 mujoco.mj_step(model, data)
                 viewer.sync()
-
-                # 12. 帧率控制
                 try:
                     mujoco.utils.mju_sleep(1 / 60)
                 except:
@@ -453,7 +546,15 @@ def run_robust_obstacle_avoidance_simulation():
         traceback.print_exc()
     finally:
         os.unlink(xml_path)
+        # 打印最终效率统计
+        print(f"\n📊 仿真结束 - 效率统计")
+        print(f"   总运动时间：{total_motion_time:.2f}s")
+        print(f"   总能耗：{total_energy_consume:.2f}J")
+        print(
+            f"   时间/能耗综合得分：{total_motion_time * EFFICIENCY_CONFIG['time_weight'] + total_energy_consume * EFFICIENCY_CONFIG['energy_weight']:.2f}")
 
 
 if __name__ == "__main__":
-    run_robust_obstacle_avoidance_simulation()
\ No newline at end of file
+    # 安装依赖（首次运行需执行）
+    # pip install cvxpy scipy ecos osqp
+    run_efficiency_optimized_simulation()
\ No newline at end of file

From 3fea2e263b3f03f56552058afdef08732205aa9e Mon Sep 17 00:00:00 2001
From: XYTX0427 <481177497@qq.com>
Date: Thu, 25 Dec 2025 10:25:44 +0800
Subject: [PATCH 16/21] =?UTF-8?q?=E5=A2=9E=E5=BC=BA=E6=9C=BA=E6=A2=B0?=
 =?UTF-8?q?=E8=87=82=E6=A0=B8=E5=BF=83=E5=8A=9F=E8=83=BD=E8=BF=90=E4=BD=9C?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/Robotic_arm/main.py | 690 ++++++++++++++++++++++++++--------------
 1 file changed, 447 insertions(+), 243 deletions(-)

diff --git a/src/Robotic_arm/main.py b/src/Robotic_arm/main.py
index 3c13f17028..fdadc83717 100644
--- a/src/Robotic_arm/main.py
+++ b/src/Robotic_arm/main.py
@@ -5,67 +5,330 @@
 import tempfile
 import time
 from scipy import interpolate
-import cvxpy as cp  # 用于能耗最优的二次规划
+import cvxpy as cp
 import warnings
+import argparse
+from dataclasses import dataclass
+from collections import deque
+import logging
 
+# ====================== 0. 初始化配置与日志 ======================
 warnings.filterwarnings("ignore")
 
-# ====================== 1. 全局配置（鲁棒+效率双优化） ======================
-# 物理约束（UR5工业级参数）
-CONSTRAINTS = {
-    "max_vel": [1.0, 0.8, 0.8, 1.2, 0.9, 1.2],  # 关节最大速度 (rad/s)
-    "max_acc": [0.5, 0.4, 0.4, 0.6, 0.5, 0.6],  # 关节最大加速度 (rad/s²)
-    "max_jerk": [0.3, 0.2, 0.2, 0.4, 0.3, 0.4],  # 关节最大加加速度 (rad/s³)
-    "max_torque": [15.0, 15.0, 10.0, 5.0, 5.0, 3.0],  # 关节最大扭矩 (N·m)
-    "ctrl_limit": [-10.0, 10.0]
-}
-
-# 避障鲁棒性参数
-OBSTACLE_CONFIG = {
-    "base_k_att": 0.8,  # 基础引力系数
-    "base_k_rep": 0.6,  # 基础斥力系数
-    "rep_radius": 0.3,  # 斥力作用半径
-    "stagnant_threshold": 0.01,  # 停滞速度阈值 (m/s)
-    "stagnant_time": 1.0,  # 停滞判定时间 (s)
-    "guide_offset": 0.1,  # 局部最优引导偏移量 (m)
-    "obstacle_list": [  # 障碍物列表 [x,y,z,半径]
-        [0.6, 0.1, 0.5, 0.1],  # 障碍1：易导致局部最优
-        [0.55, 0.05, 0.55, 0.08],  # 障碍2：密集障碍
-        [0.4, -0.1, 0.6, 0.08]  # 障碍3
+# 日志配置（结构化输出）
+logging.basicConfig(
+    level=logging.INFO,
+    format="%(asctime)s - %(module)s - %(levelname)s - %(message)s",
+    handlers=[logging.StreamHandler()]
+)
+logger = logging.getLogger("robotic_arm")
+
+
+# 解析命令行参数
+def parse_args():
+    parser = argparse.ArgumentParser(description="机械臂效率+鲁棒双优化仿真（核心增强版）")
+    parser.add_argument("--traj-points", type=int, default=20, help="轨迹插值点数（平衡精度与速度）")
+    parser.add_argument("--time-weight", type=float, default=0.6, help="时间权重（0-1）")
+    parser.add_argument("--energy-weight", type=float, default=0.4, help="能耗权重（0-1）")
+    parser.add_argument("--smooth-factor", type=float, default=0.2, help="轨迹平滑系数（0-1）")
+    return parser.parse_args()
+
+
+args = parse_args()
+
+
+# ====================== 1. 配置参数（结构化管理） ======================
+@dataclass
+class RobotConfig:
+    # 物理约束（UR5工业级参数）
+    max_vel: list = None
+    max_acc: list = None
+    max_jerk: list = None
+    max_torque: list = None
+    ctrl_limit: tuple = (-10.0, 10.0)
+
+    # 避障鲁棒性参数
+    base_k_att: float = 0.8
+    base_k_rep: float = 0.6
+    rep_radius: float = 0.3
+    stagnant_threshold: float = 0.01
+    stagnant_time: float = 1.0
+    guide_offset: float = 0.1
+    obstacle_list: list = None
+
+    # 效率优化参数
+    time_weight: float = args.time_weight
+    energy_weight: float = args.energy_weight
+    traj_interp_points: int = args.traj_points
+    safety_margin: float = 0.05
+    opt_horizon: float = 1.0
+    smooth_factor: float = args.smooth_factor
+
+    # 能耗计算参数（工业级电机参数）
+    motor_efficiency: float = 0.85  # 电机效率
+    joint_friction: list = None  # 关节摩擦系数
+
+    # 笛卡尔轨迹关键点
+    cart_waypoints: list = None
+
+
+# 初始化配置
+config = RobotConfig(
+    max_vel=[1.0, 0.8, 0.8, 1.2, 0.9, 1.2],
+    max_acc=[0.5, 0.4, 0.4, 0.6, 0.5, 0.6],
+    max_jerk=[0.3, 0.2, 0.2, 0.4, 0.3, 0.4],
+    max_torque=[15.0, 15.0, 10.0, 5.0, 5.0, 3.0],
+    obstacle_list=[
+        [0.6, 0.1, 0.5, 0.1],
+        [0.55, 0.05, 0.55, 0.08],
+        [0.4, -0.1, 0.6, 0.08]
+    ],
+    joint_friction=[0.001, 0.002, 0.0015, 0.001, 0.0008, 0.0005],
+    cart_waypoints=[
+        [0.5, 0.0, 0.6],
+        [0.6, 0.0, 0.58],
+        [0.8, 0.1, 0.8],
+        [0.6, 0.0, 0.58],
+        [0.5, 0.0, 0.6]
     ]
-}
-
-# 效率优化参数（工业场景可配置）
-EFFICIENCY_CONFIG = {
-    "time_weight": 0.6,  # 时间权重（0-1，越大越优先时间）
-    "energy_weight": 0.4,  # 能耗权重（0-1，越大越优先能耗）
-    "traj_interp_points": 50,  # 轨迹插值点数
-    "safety_margin": 0.05,  # 碰撞安全裕度 (m)
-    "opt_horizon": 1.0  # 优化时域 (s)
-}
-
-# 笛卡尔轨迹关键点（工业典型路径）
-CART_WAYPOINTS = [
-    [0.5, 0.0, 0.6],  # 起点
-    [0.6, 0.0, 0.58],  # 中间点（障碍夹缝）
-    [0.8, 0.1, 0.8],  # 终点
-    [0.6, 0.0, 0.58],  # 回中间点
-    [0.5, 0.0, 0.6]  # 回起点
-]
+)
 
 # 全局变量
 stagnant_start_time = None
-total_motion_time = 0.0  # 累计运动时间
-total_energy_consume = 0.0  # 累计能耗
+total_motion_time = 0.0
+total_energy_consume = 0.0
+traj_history = deque(maxlen=50)  # 轨迹历史（用于可视化）
+collision_warning = False
 
-# 预定义关节惯性参数（适配所有MuJoCo版本）
+# 预定义关节惯性参数
 JOINT_INERTIA = [0.01, 0.02, 0.015, 0.01, 0.008, 0.005]
 JOINT_GRAVITY = [0.5, 0.8, 0.6, 0.3, 0.2, 0.1]
 
 
-# ====================== 2. 基础工具函数（兼容所有MuJoCo版本） ======================
+# ====================== 2. 核心增强：全链路碰撞检测 ======================
+def full_arm_collision_check(model, data, config, return_min_dist=True):
+    """
+    检测所有连杆与障碍物的碰撞（核心增强）
+    :param return_min_dist: 是否返回最近距离
+    :return: 是否碰撞, 最近距离（可选）
+    """
+    collision = False
+    min_dist = float("inf")
+
+    # 所有需要检测的连杆/末端
+    link_names = ["link1", "link2", "link3", "link4", "link5", "end_effector"]
+
+    for link_name in link_names:
+        try:
+            link_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, link_name)
+            link_pos = data.xpos[link_id]  # 连杆中心位置
+
+            # 检测与每个障碍物的距离
+            for obs in config.obstacle_list:
+                obs_pos = np.array(obs[:3])
+                obs_radius = obs[3]
+
+                # 计算连杆到障碍物的距离（减去安全裕度）
+                dist = np.linalg.norm(link_pos - obs_pos) - (obs_radius + config.safety_margin)
+
+                if dist < 0:
+                    collision = True
+                    logger.warning(f"碰撞风险：{link_name} 与障碍物 {obs_pos} 距离 {dist:.4f}m")
+
+                if return_min_dist:
+                    min_dist = min(min_dist, dist)
+        except Exception as e:
+            logger.error(f"碰撞检测失败：{link_name} - {e}")
+            continue
+
+    global collision_warning
+    collision_warning = collision
+
+    if return_min_dist:
+        return collision, min_dist
+    return collision
+
+
+# ====================== 3. 核心增强：轨迹平滑滤波（修复阶数不匹配） ======================
+def smooth_cartesian_traj(traj_points, smooth_factor=0.2):
+    """
+    贝塞尔曲线平滑笛卡尔轨迹（修复点数不足问题）
+    :param traj_points: 原始轨迹点列表
+    :param smooth_factor: 平滑系数（0-1，越大越平滑）
+    :return: 平滑后的轨迹点
+    """
+    traj_array = np.array(traj_points)
+    n_points = len(traj_array)
+
+    # 处理点数不足的情况：
+    # - 1个点：直接返回
+    # - 2-3个点：使用线性插值/低阶样条
+    # - 4个及以上点：使用3阶样条
+    if n_points <= 1:
+        return traj_points
+    elif n_points <= 3:
+        k = n_points - 1  # 适配点数的阶数（2点→1阶，3点→2阶）
+    else:
+        k = 3  # 4点及以上用3阶样条
+
+    # 生成插值参数
+    t = np.linspace(0, 1, n_points)
+    smoothed_traj = np.zeros_like(traj_array)
+
+    for dim in range(3):  # x/y/z三个维度
+        try:
+            # 生成适配阶数的样条曲线（核心修复）
+            spline = interpolate.make_interp_spline(t, traj_array[:, dim], k=k)
+            # 生成更密集的插值点（提升平滑度）
+            t_smooth = np.linspace(0, 1, max(10, n_points * 2))
+            smooth_vals = spline(t_smooth)
+
+            # 低通滤波（增加异常处理，提升稳定性）
+            try:
+                from scipy.signal import filtfilt, butter
+                b, a = butter(2, smooth_factor, btype="low")
+                smooth_vals = filtfilt(b, a, smooth_vals)
+            except:
+                # 滤波失败时直接使用插值结果
+                pass
+
+            # 采样回原点数
+            smoothed_traj[:, dim] = np.interp(t, t_smooth, smooth_vals)
+        except Exception as e:
+            # 插值失败时降级为原始轨迹
+            logger.warning(f"轨迹平滑失败（维度{dim}）：{e}，使用原始轨迹")
+            smoothed_traj = traj_array
+            break
+
+    return smoothed_traj.tolist()
+
+
+def smooth_joint_traj(joint_traj, smooth_factor=0.1):
+    """
+    平滑关节轨迹（修复点数不足+简化滤波）
+    """
+    joint_array = np.array(joint_traj)
+    n_points, n_joints = joint_array.shape
+
+    # 点数不足时直接返回
+    if n_points <= 1:
+        return joint_traj
+
+    # 简化滤波逻辑，提升兼容性
+    try:
+        from scipy.signal import filtfilt, butter
+        b, a = butter(1, smooth_factor, btype="low")  # 1阶滤波更稳定
+        smoothed_joints = np.zeros_like(joint_array)
+
+        for j in range(n_joints):
+            smoothed_joints[:, j] = filtfilt(b, a, joint_array[:, j])
+    except Exception as e:
+        logger.warning(f"关节轨迹平滑失败：{e}，使用原始轨迹")
+        smoothed_joints = joint_array
+
+    return smoothed_joints
+
+
+# ====================== 4. 核心增强：真实能耗计算 ======================
+def calculate_real_energy_consumption(model, data, config, dt):
+    """
+    计算真实能耗（考虑电机效率、摩擦损耗）
+    """
+    energy = 0.0
+
+    for joint_idx in range(6):
+        # 1. 获取MuJoCo实时输出扭矩
+        real_torque = data.qfrc_actuator[joint_idx]
+        # 2. 关节速度
+        joint_vel = data.qvel[joint_idx]
+        # 3. 摩擦损耗
+        friction_loss = config.joint_friction[joint_idx] * abs(joint_vel)
+        # 4. 实际能耗计算
+        mechanical_power = abs(real_torque * joint_vel)
+        total_power = mechanical_power + friction_loss
+        energy += total_power / config.motor_efficiency * dt
+
+    return energy
+
+
+# ====================== 5. 核心增强：可视化工具（适配新版MuJoCo API） ======================
+def draw_enhanced_visualization(viewer, model, data, config):
+    """
+    增强可视化：适配新版MuJoCo API，避免MjGeom报错
+    注：新版MuJoCo简化了用户绘制，这里改用更兼容的方式展示关键信息
+    """
+    try:
+        # 方式1：使用新版Viewer的user_scn（兼容大部分版本）
+        scene = viewer.user_scn
+        scene.ngeom = 0  # 清空原有绘制
+
+        # 1. 绘制轨迹历史（使用mjv_geom接口）
+        if len(traj_history) > 1:
+            traj_array = np.array(list(traj_history))
+
+            # 轨迹线
+            for i in range(len(traj_array) - 1):
+                # 初始化几何对象（适配新版API）
+                geom = mujoco.MjvGeom()
+                mujoco.mjv_initGeom(
+                    geom,
+                    mujoco.mjtGeom.mjGEOM_LINE,
+                    np.array([0.003, 0, 0]),  # 大小
+                    traj_array[i],  # 起始点
+                    traj_array[i + 1],  # 结束点
+                    np.array([0, 1, 0, 0.6])  # 绿色轨迹线
+                )
+                # 添加到场景
+                mujoco.mjv_addGeom(scene, model, data, geom)
+
+            # 轨迹起点（蓝色）
+            start_geom = mujoco.MjvGeom()
+            mujoco.mjv_initGeom(
+                start_geom,
+                mujoco.mjtGeom.mjGEOM_SPHERE,
+                np.array([0.015, 0, 0]),
+                traj_array[0],
+                np.array([0, 0, 0]),
+                np.array([0, 0, 1, 0.8])
+            )
+            mujoco.mjv_addGeom(scene, model, data, start_geom)
+
+            # 轨迹终点（红色）
+            end_geom = mujoco.MjvGeom()
+            mujoco.mjv_initGeom(
+                end_geom,
+                mujoco.mjtGeom.mjGEOM_SPHERE,
+                np.array([0.015, 0, 0]),
+                traj_array[-1],
+                np.array([0, 0, 0]),
+                np.array([1, 0, 0, 0.8])
+            )
+            mujoco.mjv_addGeom(scene, model, data, end_geom)
+
+        # 2. 绘制碰撞警告（红色半透明球）
+        if collision_warning:
+            ee_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "ee_site")
+            ee_pos = data.site_xpos[ee_id]
+
+            warn_geom = mujoco.MjvGeom()
+            mujoco.mjv_initGeom(
+                warn_geom,
+                mujoco.mjtGeom.mjGEOM_SPHERE,
+                np.array([0.08, 0, 0]),
+                ee_pos,
+                np.array([0, 0, 0]),
+                np.array([1, 0, 0, 0.3])
+            )
+            mujoco.mjv_addGeom(scene, model, data, warn_geom)
+
+    except Exception as e:
+        # 兼容最简化版本：禁用可视化绘制，避免影响核心功能
+        logger.warning(f"可视化绘制失败（MuJoCo版本兼容问题）：{e}")
+        logger.warning("已禁用可视化增强，核心仿真功能不受影响")
+
+
+# ====================== 6. 基础工具函数 ======================
 def get_ee_cartesian_velocity(model, data, ee_site_id):
-    """计算末端笛卡尔速度（兼容所有MuJoCo版本）"""
     jacp = np.zeros((3, model.nv))
     jacr = np.zeros((3, model.nv))
     mujoco.mj_jacSite(model, data, jacp, jacr, ee_site_id)
@@ -74,75 +337,63 @@ def get_ee_cartesian_velocity(model, data, ee_site_id):
     return ee_cart_vel
 
 
-def calculate_joint_torque(model, data, joint_idx):
-    """计算关节扭矩（能耗核心指标，适配新版MuJoCo）"""
-    # 方案1：使用预定义惯性参数（兼容所有版本）
-    inertia = JOINT_INERTIA[joint_idx]
-    gravity_comp = JOINT_GRAVITY[joint_idx]
-
-    # 方案2（备选）：从data中获取实时扭矩（新版MuJoCo推荐）
-    # torque = data.qfrc_actuator[joint_idx]  # 实际输出扭矩
-
-    # 计算关节加速度（数值微分）
-    joint_acc = np.gradient(data.qvel[joint_idx]) if data.time > 0 else 0.0
-    torque = inertia * joint_acc + gravity_comp
-    torque = np.clip(torque, -CONSTRAINTS["max_torque"][joint_idx], CONSTRAINTS["max_torque"][joint_idx])
-    return torque
-
-
-# ====================== 3. 鲁棒性避障（保留原有核心逻辑） ======================
-def check_local_optimum(ee_vel, ee_pos, target_pos):
-    """检测局部最优并生成引导目标"""
+def check_local_optimum(ee_vel, ee_pos, target_pos, config):
     global stagnant_start_time
     vel_mag = np.linalg.norm(ee_vel)
-    if vel_mag < OBSTACLE_CONFIG["stagnant_threshold"]:
+
+    if vel_mag < config.stagnant_threshold:
         if stagnant_start_time is None:
             stagnant_start_time = time.time()
-        elif time.time() - stagnant_start_time > OBSTACLE_CONFIG["stagnant_time"]:
-            print(f"\n⚠️  检测到局部最优！末端速度={vel_mag:.4f}m/s")
+        elif time.time() - stagnant_start_time > config.stagnant_time:
+            logger.warning(f"检测到局部最优！末端速度={vel_mag:.4f}m/s")
             dir_to_target = np.array(target_pos) - np.array(ee_pos)
-            dir_to_target = dir_to_target / np.linalg.norm(dir_to_target) if np.linalg.norm(
-                dir_to_target) > 1e-6 else np.array([0, 0, 0.1])
-            guide_target = np.array(ee_pos) + dir_to_target * OBSTACLE_CONFIG["guide_offset"]
+            dir_norm = np.linalg.norm(dir_to_target)
+            if dir_norm < 1e-6:
+                dir_to_target = np.array([0, 0, 0.1])
+            else:
+                dir_to_target = dir_to_target / dir_norm
+
+            guide_target = np.array(ee_pos) + dir_to_target * config.guide_offset
             stagnant_start_time = None
             return True, guide_target.tolist()
     else:
         stagnant_start_time = None
+
     return False, target_pos
 
 
-def adaptive_potential_params(ee_pos, obstacle_list):
-    """自适应势场参数"""
+def adaptive_potential_params(ee_pos, obstacle_list, config):
     obs_distances = [np.linalg.norm(np.array(ee_pos) - np.array(obs[:3])) for obs in obstacle_list]
     min_dist = min(obs_distances) if obs_distances else 1.0
-    k_rep = OBSTACLE_CONFIG["base_k_rep"] if min_dist > 0.2 else OBSTACLE_CONFIG["base_k_rep"] * 2.0
-    k_att = OBSTACLE_CONFIG["base_k_att"] if len(obstacle_list) <= 2 else OBSTACLE_CONFIG["base_k_att"] * 0.5
+
+    k_rep = config.base_k_rep if min_dist > 0.2 else config.base_k_rep * 2.0
+    k_att = config.base_k_att if len(obstacle_list) <= 2 else config.base_k_att * 0.5
     return k_att, k_rep
 
 
-def robust_artificial_potential_field(ee_pos, ee_vel, target_pos, obstacle_list):
-    """鲁棒版人工势场法"""
+def robust_artificial_potential_field(ee_pos, ee_vel, target_pos, obstacle_list, config):
     ee_pos = np.array(ee_pos)
     target_pos = np.array(target_pos)
 
     # 局部最优规避
-    is_local_opt, guide_target = check_local_optimum(ee_vel, ee_pos, target_pos)
+    is_local_opt, guide_target = check_local_optimum(ee_vel, ee_pos, target_pos, config)
     current_target = np.array(guide_target) if is_local_opt else target_pos
 
     # 自适应参数
-    k_att, k_rep = adaptive_potential_params(ee_pos, obstacle_list)
+    k_att, k_rep = adaptive_potential_params(ee_pos, obstacle_list, config)
 
     # 引力+斥力计算
     att_force = k_att * (current_target - ee_pos)
     rep_force = np.zeros(3)
+
     for obs in obstacle_list:
         obs_pos = np.array(obs[:3])
         obs_radius = obs[3]
         dist = np.linalg.norm(ee_pos - obs_pos)
-        if dist < OBSTACLE_CONFIG["rep_radius"] + obs_radius:
+
+        if dist < config.rep_radius + obs_radius:
             rep_dir = (ee_pos - obs_pos) / (dist + 1e-6)
-            rep_force += k_rep * (1 / (dist - obs_radius) - 1 / OBSTACLE_CONFIG["rep_radius"]) * (
-                        1 / dist ** 2) * rep_dir
+            rep_force += k_rep * (1 / (dist - obs_radius) - 1 / config.rep_radius) * (1 / dist ** 2) * rep_dir
 
     # 修正目标并约束
     corrected_target = ee_pos + att_force + rep_force
@@ -151,47 +402,25 @@ def robust_artificial_potential_field(ee_pos, ee_vel, target_pos, obstacle_list)
     return corrected_target.tolist()
 
 
-def collision_check_approx(ee_pos, joint_pos, obstacle_list):
-    """碰撞冗余检测"""
-    ee_collision = False
-    min_ee_dist = 100.0
-    for obs in obstacle_list:
-        obs_pos = np.array(obs[:3])
-        obs_radius = obs[3]
-        dist = np.linalg.norm(np.array(ee_pos) - obs_pos)
-        min_ee_dist = min(min_ee_dist, dist)
-        if dist < obs_radius + EFFICIENCY_CONFIG["safety_margin"]:
-            ee_collision = True
-            break
-    return ee_collision, min_ee_dist
-
-
-# ====================== 4. 效率优化核心：时间最优轨迹规划 ======================
-def time_optimal_joint_trajectory(start_joint, end_joint, seg_time):
-    """
-    时间最优关节轨迹（梯形速度曲线，满足速度/加速度约束）
-    :return: 时间最优的关节位置/速度/加速度轨迹
-    """
+# ====================== 7. 轨迹规划 ======================
+def time_optimal_joint_trajectory(start_joint, end_joint, seg_time, config):
     n_joints = 6
-    traj_points = EFFICIENCY_CONFIG["traj_interp_points"]
+    traj_points = config.traj_interp_points
     t_steps = np.linspace(0, seg_time, traj_points)
 
-    # 初始化轨迹数组
     opt_pos = np.zeros((traj_points, n_joints))
     opt_vel = np.zeros((traj_points, n_joints))
     opt_acc = np.zeros((traj_points, n_joints))
 
     for j in range(n_joints):
         delta = end_joint[j] - start_joint[j]
-        max_vel = CONSTRAINTS["max_vel"][j]
-        max_acc = CONSTRAINTS["max_acc"][j]
+        max_vel = config.max_vel[j]
+        max_acc = config.max_acc[j]
 
-        # 计算梯形速度曲线的关键时间点
-        t_acc = max_vel / max_acc  # 加速时间
-        s_acc = 0.5 * max_acc * t_acc ** 2  # 加速段位移
+        t_acc = max_vel / max_acc
+        s_acc = 0.5 * max_acc * t_acc ** 2
 
         if abs(delta) < 2 * s_acc:
-            # 三角形速度曲线（未到最大速度）
             t_joint = 2 * np.sqrt(abs(delta) / max_acc)
             for i, t in enumerate(t_steps):
                 if t <= t_joint / 2:
@@ -204,161 +433,128 @@ def time_optimal_joint_trajectory(start_joint, end_joint, seg_time):
                     opt_vel[i, j] = max_acc * t_rem * np.sign(delta)
                     opt_acc[i, j] = -max_acc * np.sign(delta)
         else:
-            # 梯形速度曲线（达到最大速度）
-            t_const = (abs(delta) - 2 * s_acc) / max_vel  # 匀速时间
+            t_const = (abs(delta) - 2 * s_acc) / max_vel
             t_joint = 2 * t_acc + t_const
             for i, t in enumerate(t_steps):
                 if t <= t_acc:
-                    # 加速段
                     opt_pos[i, j] = start_joint[j] + 0.5 * max_acc * t ** 2 * np.sign(delta)
                     opt_vel[i, j] = max_acc * t * np.sign(delta)
                     opt_acc[i, j] = max_acc * np.sign(delta)
                 elif t <= t_acc + t_const:
-                    # 匀速段
                     opt_pos[i, j] = start_joint[j] + (s_acc + max_vel * (t - t_acc)) * np.sign(delta)
                     opt_vel[i, j] = max_vel * np.sign(delta)
                     opt_acc[i, j] = 0.0
                 else:
-                    # 减速段
                     t_rem = t_joint - t
                     opt_pos[i, j] = end_joint[j] - 0.5 * max_acc * t_rem ** 2 * np.sign(delta)
                     opt_vel[i, j] = max_acc * t_rem * np.sign(delta)
                     opt_acc[i, j] = -max_acc * np.sign(delta)
 
-        # 约束速度/加速度
         opt_vel[:, j] = np.clip(opt_vel[:, j], -max_vel, max_vel)
         opt_acc[:, j] = np.clip(opt_acc[:, j], -max_acc, max_acc)
 
+    # 平滑关节轨迹（使用修复后的函数）
+    opt_pos = smooth_joint_traj(opt_pos, config.smooth_factor)
+
     return opt_pos, opt_vel, opt_acc
 
 
-# ====================== 5. 效率优化核心：能耗最优二次规划（兼容所有求解器） ======================
-def energy_optimal_trajectory(joint_waypoints, seg_time):
-    """
-    能耗最优轨迹（二次规划求解，最小化扭矩平方积分）
-    :return: 能耗最优的关节位置轨迹
-    """
+def energy_optimal_trajectory(joint_waypoints, seg_time, config):
     n_joints = 6
     n_points = len(joint_waypoints)
     t_step = seg_time / (n_points - 1)
 
-    # 定义优化变量
-    q = cp.Variable((n_joints, n_points))  # 关节位置
-    qd = cp.Variable((n_joints, n_points))  # 关节速度
-    qdd = cp.Variable((n_joints, n_points))  # 关节加速度
+    q = cp.Variable((n_joints, n_points))
+    qd = cp.Variable((n_joints, n_points))
+    qdd = cp.Variable((n_joints, n_points))
 
-    # 代价函数：最小化能耗（扭矩平方积分≈加速度平方积分）
     energy_cost = cp.sum_squares(qdd)
-    time_cost = cp.sum(cp.max(cp.abs(qd), axis=1))  # 时间代价：速度越大时间越短
-    total_cost = EFFICIENCY_CONFIG["time_weight"] * time_cost + EFFICIENCY_CONFIG["energy_weight"] * energy_cost
+    time_cost = cp.sum(cp.max(cp.abs(qd), axis=1))
+    total_cost = config.time_weight * time_cost + config.energy_weight * energy_cost
 
-    # 约束条件
     constraints = []
-    # 初始/终止条件
     constraints.append(q[:, 0] == joint_waypoints[0])
     constraints.append(q[:, -1] == joint_waypoints[-1])
     constraints.append(qd[:, 0] == 0)
     constraints.append(qd[:, -1] == 0)
-    # 速度/加速度约束
+
     for j in range(n_joints):
-        constraints.append(qd[j, :] <= CONSTRAINTS["max_vel"][j])
-        constraints.append(qd[j, :] >= -CONSTRAINTS["max_vel"][j])
-        constraints.append(qdd[j, :] <= CONSTRAINTS["max_acc"][j])
-        constraints.append(qdd[j, :] >= -CONSTRAINTS["max_acc"][j])
-    # 动力学约束（差分）
+        constraints.append(qd[j, :] <= config.max_vel[j])
+        constraints.append(qd[j, :] >= -config.max_vel[j])
+        constraints.append(qdd[j, :] <= config.max_acc[j])
+        constraints.append(qdd[j, :] >= -config.max_acc[j])
+
     for i in range(n_points - 1):
         constraints.append(qd[:, i + 1] == (q[:, i + 1] - q[:, i]) / t_step)
         constraints.append(qdd[:, i + 1] == (qd[:, i + 1] - qd[:, i]) / t_step)
 
-    # 求解二次规划（自动选择可用求解器，增加容错）
     prob = cp.Problem(cp.Minimize(total_cost), constraints)
     try:
-        # 优先尝试ECOS求解器
         prob.solve(solver=cp.ECOS, verbose=False)
     except:
         try:
-            # 备选：OSQP求解器（CVXPY默认推荐）
             prob.solve(solver=cp.OSQP, verbose=False)
         except:
-            # 最后：使用CVXPY自动选择的求解器
             prob.solve(verbose=False)
 
     if prob.status != cp.OPTIMAL:
-        print("⚠️  能耗优化求解失败，降级为时间最优轨迹")
+        logger.warning("能耗优化求解失败，降级为时间最优轨迹")
         return None
+
     return q.value.T
 
 
-# ====================== 6. 效率+鲁棒融合：避障轨迹的双优优化（修复索引越界） ======================
-def optimize_obstacle_traj_with_efficiency(model, data, ee_pos, target_pos, obstacle_list):
-    """
-    融合避障鲁棒性+时间/能耗最优的轨迹规划
-    :return: 优化后的关节目标、当前段能耗
-    """
+def optimize_obstacle_traj_with_efficiency(model, data, ee_pos, target_pos, config):
     global total_motion_time, total_energy_consume
 
-    # 步骤1：鲁棒避障修正笛卡尔目标
+    # 1. 鲁棒避障修正目标
     ee_vel = get_ee_cartesian_velocity(model, data, mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "ee_site"))
-    corrected_cart_target = robust_artificial_potential_field(ee_pos, ee_vel, target_pos, obstacle_list)
+    corrected_cart_target = robust_artificial_potential_field(ee_pos, ee_vel, target_pos, config.obstacle_list, config)
 
-    # 步骤2：逆解得到关节目标
+    # 2. 平滑笛卡尔目标轨迹（使用修复后的函数）
+    corrected_cart_target = smooth_cartesian_traj([ee_pos, corrected_cart_target], config.smooth_factor)[-1]
+
+    # 3. 逆解得到关节目标
     ee_site_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "ee_site")
     data.site_xpos[ee_site_id] = corrected_cart_target
     mujoco.mj_inverse(model, data)
     end_joint = data.qpos[:6].copy()
-    start_joint = data.qpos[:6].copy()  # 当前关节位置为起点
+    start_joint = data.qpos[:6].copy()
 
-    # 步骤3：时间最优轨迹初值
-    seg_time = 2.0  # 初始段时间
-    time_opt_pos, time_opt_vel, time_opt_acc = time_optimal_joint_trajectory(start_joint, end_joint, seg_time)
+    # 4. 时间最优轨迹
+    seg_time = 2.0
+    time_opt_pos, time_opt_vel, time_opt_acc = time_optimal_joint_trajectory(start_joint, end_joint, seg_time, config)
 
-    # 步骤4：能耗最优优化（二次规划）
-    energy_opt_pos = energy_optimal_trajectory(time_opt_pos, seg_time)
+    # 5. 能耗最优优化
+    energy_opt_pos = energy_optimal_trajectory(time_opt_pos, seg_time, config)
     if energy_opt_pos is None:
         final_joint_traj = time_opt_pos
     else:
         final_joint_traj = energy_opt_pos
 
-    # 步骤5：计算当前段能耗（扭矩平方积分，修复索引越界错误）
+    # 6. 计算真实能耗
+    dt = seg_time / len(final_joint_traj)
     seg_energy = 0.0
-    # 遍历每个轨迹点
+
     for traj_idx in range(len(final_joint_traj)):
-        # 跳过第一个点（无加速度）
         if traj_idx == 0:
             continue
 
-        # 遍历每个关节计算扭矩
-        for joint_idx in range(6):
-            # 获取当前轨迹点和上一个轨迹点的关节角度
-            curr_angle = final_joint_traj[traj_idx, joint_idx]
-            prev_angle = final_joint_traj[traj_idx - 1, joint_idx]
-
-            # 计算关节速度（差分）
-            dt = seg_time / len(final_joint_traj)
-            joint_vel = (curr_angle - prev_angle) / dt
-
-            # 计算关节加速度（差分，使用前一个速度）
-            if traj_idx == 1:
-                joint_acc = joint_vel / dt
-            else:
-                prev_vel = (final_joint_traj[traj_idx - 1, joint_idx] - final_joint_traj[traj_idx - 2, joint_idx]) / dt
-                joint_acc = (joint_vel - prev_vel) / dt
-
-            # 计算扭矩和能耗（积分）
-            torque = JOINT_INERTIA[joint_idx] * joint_acc + JOINT_GRAVITY[joint_idx]
-            seg_energy += np.square(torque) * dt
+        # 使用真实扭矩计算能耗
+        seg_energy += calculate_real_energy_consumption(model, data, config, dt)
 
     # 更新全局统计
     total_motion_time += seg_time
     total_energy_consume += seg_energy
 
-    # 返回当前时刻的关节目标（取第一个插值点）
+    # 记录轨迹历史
+    traj_history.append(corrected_cart_target)
+
     return final_joint_traj[0], corrected_cart_target, seg_energy
 
 
-# ====================== 7. 机械臂模型（修复XML语法错误） ======================
-def get_arm_xml_with_obstacles():
-    """生成带障碍的机械臂XML模型（修复inertial标签错误）"""
+# ====================== 8. 机械臂模型 ======================
+def get_arm_xml_with_obstacles(config):
     arm_xml = """
 <mujoco model="6dof_arm_efficiency_optimized">
   <compiler angle="radian" inertiafromgeom="true"/>
@@ -372,37 +568,30 @@ def get_arm_xml_with_obstacles():
   </asset>
   <worldbody>
     <geom name="floor" type="plane" size="5 5 0.1" pos="0 0 0" material="gray"/>
-    <!-- 基座（包含inertial标签） -->
     <body name="base" pos="0 0 0">
       <inertial pos="0 0 0" mass="5.0" diaginertia="0.01 0.01 0.01"/>
       <geom name="base_geom" type="cylinder" size="0.15 0.1" pos="0 0 0" material="gray"/>
       <joint name="joint0" type="hinge" axis="0 0 1" pos="0 0 0.1"/>
-      <!-- 连杆1 -->
       <body name="link1" pos="0 0 0.1">
         <inertial pos="0 0 0.15" mass="1.2" diaginertia="0.02 0.02 0.02"/>
         <geom name="link1_geom" type="capsule" size="0.05" fromto="0 0 0 0 0 0.3" material="blue"/>
         <joint name="joint1" type="hinge" axis="0 1 0" pos="0 0 0.3"/>
-        <!-- 连杆2 -->
         <body name="link2" pos="0 0 0.3">
           <inertial pos="0.2 0 0" mass="1.0" diaginertia="0.015 0.015 0.015"/>
           <geom name="link2_geom" type="capsule" size="0.05" fromto="0 0 0 0.4 0 0" material="blue"/>
           <joint name="joint2" type="hinge" axis="0 1 0" pos="0.4 0 0"/>
-          <!-- 连杆3 -->
           <body name="link3" pos="0.4 0 0">
             <inertial pos="0.175 0 0" mass="0.8" diaginertia="0.01 0.01 0.01"/>
             <geom name="link3_geom" type="capsule" size="0.04" fromto="0 0 0 0.35 0 0" material="blue"/>
             <joint name="joint3" type="hinge" axis="1 0 0" pos="0.35 0 0"/>
-            <!-- 连杆4 -->
             <body name="link4" pos="0.35 0 0">
               <inertial pos="0 0 0.125" mass="0.6" diaginertia="0.008 0.008 0.008"/>
               <geom name="link4_geom" type="capsule" size="0.04" fromto="0 0 0 0 0 0.25" material="blue"/>
               <joint name="joint4" type="hinge" axis="0 1 0" pos="0 0 0.25"/>
-              <!-- 连杆5 -->
               <body name="link5" pos="0 0 0.25">
                 <inertial pos="0 0 0.1" mass="0.4" diaginertia="0.008 0.008 0.008"/>
                 <geom name="link5_geom" type="capsule" size="0.03" fromto="0 0 0 0 0 0.2" material="blue"/>
                 <joint name="joint5" type="hinge" axis="1 0 0" pos="0 0 0.2"/>
-                <!-- 末端执行器 -->
                 <body name="end_effector" pos="0 0 0.2">
                   <inertial pos="0 0 0" mass="0.2" diaginertia="0.005 0.005 0.005"/>
                   <geom name="ee_geom" type="box" size="0.08 0.08 0.08" pos="0 0 0" material="red"/>
@@ -414,15 +603,16 @@ def get_arm_xml_with_obstacles():
         </body>
       </body>
     </body>
-    <!-- 障碍物 -->
     """
-    # 添加障碍
-    for i, obs in enumerate(OBSTACLE_CONFIG["obstacle_list"]):
+
+    # 添加障碍物
+    for i, obs in enumerate(config.obstacle_list):
         x, y, z, r = obs
         material = "critical_obstacle" if i == 0 else "obstacle"
         arm_xml += f"""
     <geom name="obstacle_{i}" type="sphere" size="{r}" pos="{x} {y} {z}" material="{material}"/>
         """
+
     arm_xml += """
   </worldbody>
   <actuator>
@@ -438,11 +628,12 @@ def get_arm_xml_with_obstacles():
     return arm_xml
 
 
-# ====================== 8. 仿真主逻辑 ======================
-def run_efficiency_optimized_simulation():
-    """运行效率+鲁棒双优化的仿真"""
+# ====================== 9. 仿真主逻辑 ======================
+def run_enhanced_simulation():
     global total_motion_time, total_energy_consume
-    arm_xml = get_arm_xml_with_obstacles()
+
+    # 生成模型XML
+    arm_xml = get_arm_xml_with_obstacles(config)
 
     with tempfile.NamedTemporaryFile(mode='w', suffix='.xml', delete=False) as f:
         f.write(arm_xml)
@@ -451,60 +642,66 @@ def run_efficiency_optimized_simulation():
     try:
         model = mujoco.MjModel.from_xml_path(xml_path)
         data = mujoco.MjData(model)
-        print("✅ 效率+鲁棒双优化机械臂模型加载成功！")
-        print(f"🔧 效率配置：时间权重={EFFICIENCY_CONFIG['time_weight']}, 能耗权重={EFFICIENCY_CONFIG['energy_weight']}")
-        print(f"🔧 鲁棒配置：局部最优规避 + 自适应势场 + 碰撞冗余检测")
+        logger.info("✅ 增强版机械臂仿真模型加载成功！")
+        logger.info(
+            f"🔧 配置：轨迹点数={config.traj_interp_points}, 时间权重={config.time_weight}, 平滑系数={config.smooth_factor}")
 
-        # 预计算笛卡尔轨迹对应的关节起点
+        # 预计算关节起点
         ee_site_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "ee_site")
         joint_waypoints = []
-        for cart_pos in CART_WAYPOINTS:
+
+        for cart_pos in config.cart_waypoints:
             mujoco.mj_resetData(model, data)
             data.site_xpos[ee_site_id] = cart_pos
             mujoco.mj_inverse(model, data)
             joint_waypoints.append(data.qpos[:6].copy())
 
+        # 启动仿真视图
         with mujoco.viewer.launch_passive(model, data) as viewer:
-            print("\n🎮 效率+鲁棒双优化仿真启动！")
-            print("💡 核心：避障安全 + 时间/能耗最优（工业级需求）")
-            print("💡 按 Ctrl+C 退出\n")
+            logger.info("\n🎮 增强版机械臂仿真启动！")
+            logger.info("💡 核心增强：全链路碰撞检测 + 轨迹平滑 + 真实能耗计算 + 可视化增强")
+            logger.info("💡 按 Ctrl+C 退出\n")
 
-            traj_idx = 0
             current_waypoint = 0
             last_print_time = 0.0
+            pause_simulation = False
 
             while viewer.is_running():
+                if pause_simulation:
+                    time.sleep(0.1)
+                    continue
+
                 t_total = data.time
                 ee_pos = data.site_xpos[ee_site_id].tolist()
 
-                # 切换笛卡尔目标点
-                if current_waypoint < len(CART_WAYPOINTS):
-                    target_cart = CART_WAYPOINTS[current_waypoint]
-                    # 距离目标点<0.01m时切换下一个
+                # 切换目标点
+                if current_waypoint < len(config.cart_waypoints):
+                    target_cart = config.cart_waypoints[current_waypoint]
                     if np.linalg.norm(np.array(ee_pos) - np.array(target_cart)) < 0.01:
-                        current_waypoint = (current_waypoint + 1) % len(CART_WAYPOINTS)
-                        print(f"\n🔄 切换到目标点 {current_waypoint}: {np.round(target_cart, 3)}")
+                        current_waypoint = (current_waypoint + 1) % len(config.cart_waypoints)
+                        logger.info(f"\n🔄 切换到目标点 {current_waypoint}: {np.round(target_cart, 3)}")
                 else:
-                    target_cart = CART_WAYPOINTS[-1]
+                    target_cart = config.cart_waypoints[-1]
 
-                # 融合避障+效率优化的轨迹规划
+                # 轨迹优化
                 target_joints, corrected_cart, seg_energy = optimize_obstacle_traj_with_efficiency(
-                    model, data, ee_pos, target_cart, OBSTACLE_CONFIG["obstacle_list"]
+                    model, data, ee_pos, target_cart, config
                 )
 
-                # 碰撞检测与紧急避障
-                is_collision, min_obs_dist = collision_check_approx(ee_pos, target_joints,
-                                                                    OBSTACLE_CONFIG["obstacle_list"])
+                # 全链路碰撞检测
+                is_collision, min_obs_dist = full_arm_collision_check(model, data, config)
+
+                # 紧急避障
                 if is_collision:
-                    emergency_rep = np.array(ee_pos) - np.array(OBSTACLE_CONFIG["obstacle_list"][0][:3])
+                    logger.warning("🆘 检测到碰撞风险，执行紧急避障！")
+                    emergency_rep = np.array(ee_pos) - np.array(config.obstacle_list[0][:3])
                     emergency_rep = emergency_rep / np.linalg.norm(emergency_rep) * 0.05
                     corrected_cart = np.array(corrected_cart) + emergency_rep
                     data.site_xpos[ee_site_id] = corrected_cart
                     mujoco.mj_inverse(model, data)
                     target_joints = data.qpos[:6].copy()
-                    print(f"🆘 紧急避障：修正目标={np.round(corrected_cart, 3)}")
 
-                # 闭环PD控制（带扭矩约束）
+                # PD控制
                 ctrl_signals = []
                 for i in range(6):
                     k_p = 8.0
@@ -514,47 +711,54 @@ def run_efficiency_optimized_simulation():
                     pos_error = target_joints[i] - current_pos
                     vel_error = -current_vel
                     ctrl = k_p * pos_error + k_d * vel_error
-                    # 扭矩约束（转换为控制量约束）
-                    max_ctrl = CONSTRAINTS["max_torque"][i] / 100.0  # gear=100，直接计算
+                    max_ctrl = config.max_torque[i] / 100.0
                     ctrl = np.clip(ctrl, -max_ctrl, max_ctrl)
                     ctrl_signals.append(ctrl)
+
                 data.ctrl[:6] = ctrl_signals
 
-                # 打印效率统计（每2秒）
+                # 打印统计信息
                 if t_total - last_print_time > 2.0 and t_total > 0:
                     ee_vel = get_ee_cartesian_velocity(model, data, ee_site_id)
                     avg_vel = np.linalg.norm(ee_vel)
                     avg_energy = total_energy_consume / t_total if t_total > 0 else 0.0
-                    print(f"\n⏱️  时间：{t_total:.2f}s | 累计运动时间：{total_motion_time:.2f}s")
-                    print(f"   末端位置：{np.round(ee_pos, 3)} | 目标位置：{np.round(corrected_cart, 3)}")
-                    print(f"   末端速度：{avg_vel:.4f}m/s | 最近障碍：{min_obs_dist:.3f}m")
-                    print(f"   累计能耗：{total_energy_consume:.2f}J | 平均能耗：{avg_energy:.2f}J/s")
-                    print(f"   碰撞风险：{'是' if is_collision else '否'}")
+
+                    logger.info(f"\n⏱️  仿真时间：{t_total:.2f}s | 累计运动时间：{total_motion_time:.2f}s")
+                    logger.info(f"   末端位置：{np.round(ee_pos, 3)} | 目标位置：{np.round(corrected_cart, 3)}")
+                    logger.info(f"   末端速度：{avg_vel:.4f}m/s | 最近障碍距离：{min_obs_dist:.3f}m")
+                    logger.info(f"   累计能耗：{total_energy_consume:.2f}J | 平均能耗：{avg_energy:.2f}J/s")
+                    logger.info(f"   碰撞风险：{'⚠️  高' if is_collision else '✅  低'}")
                     last_print_time = t_total
 
-                # 仿真步运行
+                # 增强可视化（适配新版API）
+                draw_enhanced_visualization(viewer, model, data, config)
+
+                # 运行仿真步
                 mujoco.mj_step(model, data)
                 viewer.sync()
+
                 try:
                     mujoco.utils.mju_sleep(1 / 60)
                 except:
                     time.sleep(1 / 60)
 
+    except KeyboardInterrupt:
+        logger.info("\n🛑 用户终止仿真")
     except Exception as e:
-        print(f"❌ 仿真出错：{e}")
+        logger.error(f"❌ 仿真出错：{e}")
         import traceback
         traceback.print_exc()
     finally:
         os.unlink(xml_path)
-        # 打印最终效率统计
-        print(f"\n📊 仿真结束 - 效率统计")
-        print(f"   总运动时间：{total_motion_time:.2f}s")
-        print(f"   总能耗：{total_energy_consume:.2f}J")
-        print(
-            f"   时间/能耗综合得分：{total_motion_time * EFFICIENCY_CONFIG['time_weight'] + total_energy_consume * EFFICIENCY_CONFIG['energy_weight']:.2f}")
+        # 最终统计
+        logger.info(f"\n📊 仿真结束 - 最终统计")
+        logger.info(f"   总运动时间：{total_motion_time:.2f}s")
+        logger.info(f"   总能耗：{total_energy_consume:.2f}J")
+        logger.info(
+            f"   综合得分：{total_motion_time * config.time_weight + total_energy_consume * config.energy_weight:.2f}")
 
 
 if __name__ == "__main__":
-    # 安装依赖（首次运行需执行）
-    # pip install cvxpy scipy ecos osqp
-    run_efficiency_optimized_simulation()
\ No newline at end of file
+    # 安装依赖
+    # pip install cvxpy scipy ecos osqp mujoco
+    run_enhanced_simulation()
\ No newline at end of file

From 2ad4af1a9642f66a4289e4aaeead06d3d8f5b301 Mon Sep 17 00:00:00 2001
From: XYTX0427 <481177497@qq.com>
Date: Thu, 25 Dec 2025 20:48:17 +0800
Subject: [PATCH 17/21] =?UTF-8?q?=E4=BC=98=E5=8C=96=E6=9C=BA=E6=A2=B0?=
 =?UTF-8?q?=E8=87=82=E7=9A=84=E6=98=93=E7=94=A8=E6=80=A7?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/Robotic_arm/main.py | 1409 +++++++++++++++++++++------------------
 1 file changed, 748 insertions(+), 661 deletions(-)

diff --git a/src/Robotic_arm/main.py b/src/Robotic_arm/main.py
index fdadc83717..238d01c137 100644
--- a/src/Robotic_arm/main.py
+++ b/src/Robotic_arm/main.py
@@ -1,764 +1,851 @@
-import mujoco
-import mujoco.viewer
-import numpy as np
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+机械臂仿真完整单文件版本（最终完美修复版）
+核心修复：
+1. 统一关节数为5个（匹配XML模型）
+2. 所有数组维度改为5维
+3. 移除所有硬编码的6关节逻辑
+4. 确保所有运算维度匹配
+"""
+
+import sys
 import os
-import tempfile
 import time
-from scipy import interpolate
-import cvxpy as cp
-import warnings
+import logging
 import argparse
-from dataclasses import dataclass
+from dataclasses import dataclass, field
+from typing import List, Tuple, Optional, Dict, Deque
 from collections import deque
-import logging
 
-# ====================== 0. 初始化配置与日志 ======================
-warnings.filterwarnings("ignore")
+# ====================== 核心配置：统一关节数 ======================
+JOINT_COUNT = 5  # 关键：改为5个关节（匹配XML模型）
 
-# 日志配置（结构化输出）
-logging.basicConfig(
-    level=logging.INFO,
-    format="%(asctime)s - %(module)s - %(levelname)s - %(message)s",
-    handlers=[logging.StreamHandler()]
-)
-logger = logging.getLogger("robotic_arm")
+# ====================== mujoco 版本兼容处理 ======================
+try:
+    import numpy as np
+    import mujoco
 
+    try:
+        from mujoco import viewer
 
-# 解析命令行参数
-def parse_args():
-    parser = argparse.ArgumentParser(description="机械臂效率+鲁棒双优化仿真（核心增强版）")
-    parser.add_argument("--traj-points", type=int, default=20, help="轨迹插值点数（平衡精度与速度）")
-    parser.add_argument("--time-weight", type=float, default=0.6, help="时间权重（0-1）")
-    parser.add_argument("--energy-weight", type=float, default=0.4, help="能耗权重（0-1）")
-    parser.add_argument("--smooth-factor", type=float, default=0.2, help="轨迹平滑系数（0-1）")
-    return parser.parse_args()
+        MUJOCO_VIEWER_MODE = "new"
+    except ImportError:
+        if hasattr(mujoco, 'viewer'):
+            viewer = mujoco.viewer
+            MUJOCO_VIEWER_MODE = "old"
+        else:
+            raise ImportError("请安装最新版mujoco：pip install mujoco>=2.3.0")
 
+    from scipy import interpolate
+    from scipy.signal import filtfilt, butter
+    import cvxpy as cp
+except ImportError as e:
+    print(f"❌ 缺少依赖库：{e.name}")
+    print("🔧 请运行：pip install mujoco>=2.3.0 numpy scipy cvxpy ecos osqp")
+    sys.exit(1)
 
-args = parse_args()
+
+# ====================== 1. 配置管理模块（改为5关节） ======================
+@dataclass
+class PhysicsConfig:
+    # 改为5个关节的限制参数
+    max_vel: List[float] = field(default_factory=lambda: [1.0, 0.8, 0.8, 1.2, 0.9])
+    max_acc: List[float] = field(default_factory=lambda: [0.5, 0.4, 0.4, 0.6, 0.5])
+    max_jerk: List[float] = field(default_factory=lambda: [0.3, 0.2, 0.2, 0.4, 0.3])
+    max_torque: List[float] = field(default_factory=lambda: [15.0, 15.0, 10.0, 5.0, 5.0])
+    ctrl_limit: Tuple[float, float] = (-10.0, 10.0)
 
 
-# ====================== 1. 配置参数（结构化管理） ======================
 @dataclass
-class RobotConfig:
-    # 物理约束（UR5工业级参数）
-    max_vel: list = None
-    max_acc: list = None
-    max_jerk: list = None
-    max_torque: list = None
-    ctrl_limit: tuple = (-10.0, 10.0)
-
-    # 避障鲁棒性参数
+class ObstacleConfig:
     base_k_att: float = 0.8
     base_k_rep: float = 0.6
     rep_radius: float = 0.3
     stagnant_threshold: float = 0.01
     stagnant_time: float = 1.0
     guide_offset: float = 0.1
-    obstacle_list: list = None
-
-    # 效率优化参数
-    time_weight: float = args.time_weight
-    energy_weight: float = args.energy_weight
-    traj_interp_points: int = args.traj_points
+    obstacle_list: List[List[float]] = field(
+        default_factory=lambda: [[0.6, 0.1, 0.5, 0.1], [0.55, 0.05, 0.55, 0.08], [0.4, -0.1, 0.6, 0.08]])
     safety_margin: float = 0.05
-    opt_horizon: float = 1.0
-    smooth_factor: float = args.smooth_factor
-
-    # 能耗计算参数（工业级电机参数）
-    motor_efficiency: float = 0.85  # 电机效率
-    joint_friction: list = None  # 关节摩擦系数
-
-    # 笛卡尔轨迹关键点
-    cart_waypoints: list = None
-
-
-# 初始化配置
-config = RobotConfig(
-    max_vel=[1.0, 0.8, 0.8, 1.2, 0.9, 1.2],
-    max_acc=[0.5, 0.4, 0.4, 0.6, 0.5, 0.6],
-    max_jerk=[0.3, 0.2, 0.2, 0.4, 0.3, 0.4],
-    max_torque=[15.0, 15.0, 10.0, 5.0, 5.0, 3.0],
-    obstacle_list=[
-        [0.6, 0.1, 0.5, 0.1],
-        [0.55, 0.05, 0.55, 0.08],
-        [0.4, -0.1, 0.6, 0.08]
-    ],
-    joint_friction=[0.001, 0.002, 0.0015, 0.001, 0.0008, 0.0005],
-    cart_waypoints=[
-        [0.5, 0.0, 0.6],
-        [0.6, 0.0, 0.58],
-        [0.8, 0.1, 0.8],
-        [0.6, 0.0, 0.58],
-        [0.5, 0.0, 0.6]
-    ]
-)
-
-# 全局变量
-stagnant_start_time = None
-total_motion_time = 0.0
-total_energy_consume = 0.0
-traj_history = deque(maxlen=50)  # 轨迹历史（用于可视化）
-collision_warning = False
-
-# 预定义关节惯性参数
-JOINT_INERTIA = [0.01, 0.02, 0.015, 0.01, 0.008, 0.005]
-JOINT_GRAVITY = [0.5, 0.8, 0.6, 0.3, 0.2, 0.1]
-
-
-# ====================== 2. 核心增强：全链路碰撞检测 ======================
-def full_arm_collision_check(model, data, config, return_min_dist=True):
-    """
-    检测所有连杆与障碍物的碰撞（核心增强）
-    :param return_min_dist: 是否返回最近距离
-    :return: 是否碰撞, 最近距离（可选）
-    """
-    collision = False
-    min_dist = float("inf")
 
-    # 所有需要检测的连杆/末端
-    link_names = ["link1", "link2", "link3", "link4", "link5", "end_effector"]
 
-    for link_name in link_names:
-        try:
-            link_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, link_name)
-            link_pos = data.xpos[link_id]  # 连杆中心位置
+@dataclass
+class EfficiencyConfig:
+    time_weight: float = 0.6
+    energy_weight: float = 0.4
+    traj_interp_points: int = 20
+    opt_horizon: float = 1.0
+    smooth_factor: float = 0.2
+    motor_efficiency: float = 0.85
+    # 改为5个关节的摩擦系数
+    joint_friction: List[float] = field(default_factory=lambda: [0.001, 0.002, 0.0015, 0.001, 0.0008])
 
-            # 检测与每个障碍物的距离
-            for obs in config.obstacle_list:
-                obs_pos = np.array(obs[:3])
-                obs_radius = obs[3]
 
-                # 计算连杆到障碍物的距离（减去安全裕度）
-                dist = np.linalg.norm(link_pos - obs_pos) - (obs_radius + config.safety_margin)
+@dataclass
+class TrajectoryConfig:
+    cart_waypoints: List[List[float]] = field(
+        default_factory=lambda: [[0.5, 0.0, 0.6], [0.6, 0.0, 0.58], [0.8, 0.1, 0.8], [0.6, 0.0, 0.58], [0.5, 0.0, 0.6]])
 
-                if dist < 0:
-                    collision = True
-                    logger.warning(f"碰撞风险：{link_name} 与障碍物 {obs_pos} 距离 {dist:.4f}m")
 
-                if return_min_dist:
-                    min_dist = min(min_dist, dist)
-        except Exception as e:
-            logger.error(f"碰撞检测失败：{link_name} - {e}")
-            continue
+@dataclass
+class SimulationConfig:
+    timestep: float = 0.005
+    fps: int = 60
+    log_level: str = "INFO"
+    enable_interaction: bool = False
 
-    global collision_warning
-    collision_warning = collision
 
-    if return_min_dist:
-        return collision, min_dist
-    return collision
+@dataclass
+class RobotConfig:
+    physics: PhysicsConfig = field(default_factory=PhysicsConfig)
+    obstacle: ObstacleConfig = field(default_factory=ObstacleConfig)
+    efficiency: EfficiencyConfig = field(default_factory=EfficiencyConfig)
+    trajectory: TrajectoryConfig = field(default_factory=TrajectoryConfig)
+    simulation: SimulationConfig = field(default_factory=SimulationConfig)
+
+    def validate(self):
+        """校验并自动修复配置参数"""
+        logger = logging.getLogger(__name__)
+        if self.simulation.fps < 1 or self.simulation.fps > 120:
+            logger.warning(f"⚠️ FPS {self.simulation.fps} 超出范围，自动调整为30")
+            self.simulation.fps = 30
+        if self.efficiency.traj_interp_points < 5 or self.efficiency.traj_interp_points > 100:
+            logger.warning(f"⚠️ 插值点数 {self.efficiency.traj_interp_points} 超出范围，自动调整为20")
+            self.efficiency.traj_interp_points = 20
+        weight_sum = self.efficiency.time_weight + self.efficiency.energy_weight
+        if not abs(weight_sum - 1.0) < 1e-6:
+            logger.warning(f"⚠️ 时间+能耗权重和为 {weight_sum}（应为1），自动归一化")
+            self.efficiency.time_weight /= weight_sum
+            self.efficiency.energy_weight /= weight_sum
+
+
+# 全局配置实例
+_global_config: Optional[RobotConfig] = None
+
+
+def get_config() -> RobotConfig:
+    """获取全局配置（单例+参数校验）"""
+    global _global_config
+    if _global_config is None:
+        _global_config = RobotConfig()
+
+        # 应用命令行参数
+        parser = argparse.ArgumentParser(description="机械臂仿真配置", add_help=False)
+        parser.add_argument("--fps", type=int, help="仿真帧率（1-120）")
+        parser.add_argument("--traj-points", type=int, dest="traj_interp_points", help="轨迹插值点数（5-100）")
+        parser.add_argument("--smooth-factor", type=float, help="轨迹平滑系数（0.01-1.0）")
+        parser.add_argument("--time-weight", type=float, help="时间权重（0-1）")
+        parser.add_argument("--energy-weight", type=float, help="能耗权重（0-1）")
+        parser.add_argument("--log-level", choices=["DEBUG", "INFO", "WARNING", "ERROR"], help="日志级别")
+        parser.add_argument("-h", "--help", action="store_true", help="显示帮助信息")
+
+        args, _ = parser.parse_known_args()
+
+        # 应用参数到配置
+        if args.fps:
+            _global_config.simulation.fps = args.fps
+        if args.traj_interp_points:
+            _global_config.efficiency.traj_interp_points = args.traj_interp_points
+        if args.smooth_factor:
+            _global_config.efficiency.smooth_factor = args.smooth_factor
+        if args.time_weight:
+            _global_config.efficiency.time_weight = args.time_weight
+        if args.energy_weight:
+            _global_config.efficiency.energy_weight = args.energy_weight
+        if args.log_level:
+            _global_config.simulation.log_level = args.log_level
+
+        # 校验配置
+        _global_config.validate()
+
+        # 显示帮助信息
+        if args.help:
+            print("""
+🤖 机械臂仿真使用帮助：
+命令行参数：
+  --fps N           设置仿真帧率（1-120），默认60
+  --traj-points N   设置轨迹插值点数（5-100），默认20
+  --smooth-factor F 设置轨迹平滑系数（0.01-1.0），默认0.2
+  --time-weight F   设置时间权重（0-1），默认0.6
+  --energy-weight F 设置能耗权重（0-1），默认0.4
+  --log-level LEVEL 设置日志级别（DEBUG/INFO/WARNING/ERROR），默认INFO
+  -h/--help         显示此帮助信息
+            """)
+            sys.exit(0)
+
+    return _global_config
+
+
+# ====================== 2. 性能优化缓存 ======================
+_TRAJ_CACHE = {
+    "butter_coeffs": {},
+    "joint_limits": None
+}
+
+_COLLISION_CACHE = {
+    "link_ids": {},
+    "obstacle_grid": None,
+    "obstacle_array": None,
+    "safety_margin": None
+}
+
+_ENERGY_CACHE = {
+    "friction": None,
+    "motor_eff": None
+}
+
+
+def init_global_caches():
+    """初始化全局缓存（只执行一次）"""
+    config = get_config()
+
+    if _TRAJ_CACHE["joint_limits"] is None:
+        # 改为5个关节的限制参数
+        _TRAJ_CACHE["joint_limits"] = {
+            "max_vel": np.array(config.physics.max_vel, dtype=np.float64),
+            "max_acc": np.array(config.physics.max_acc, dtype=np.float64),
+            "max_torque": np.array(config.physics.max_torque, dtype=np.float64)
+        }
+
+    if not _COLLISION_CACHE["link_ids"]:
+        _COLLISION_CACHE["obstacle_array"] = np.array(config.obstacle.obstacle_list, dtype=np.float64)
+        _COLLISION_CACHE["safety_margin"] = config.obstacle.safety_margin
+        obs_pos = _COLLISION_CACHE["obstacle_array"][:, :3]
+        min_coords = np.min(obs_pos, axis=0) - 0.5
+        max_coords = np.max(obs_pos, axis=0) + 0.5
+        _COLLISION_CACHE["obstacle_grid"] = (min_coords, max_coords)
+
+    if _ENERGY_CACHE["friction"] is None:
+        _ENERGY_CACHE["friction"] = np.array(config.efficiency.joint_friction, dtype=np.float64)
+        _ENERGY_CACHE["motor_eff"] = config.efficiency.motor_efficiency
+
+
+# ====================== 3. 核心算法模块（改为5关节） ======================
+def smooth_cartesian_traj(traj_points: List[List[float]], smooth_factor: float = None) -> List[List[float]]:
+    """笛卡尔轨迹平滑"""
+    config = get_config()
+    smooth_factor = smooth_factor or config.efficiency.smooth_factor
+    traj_array = np.asarray(traj_points, dtype=np.float64)
+
+    if traj_array.size == 0 or len(traj_array) <= 1:
+        return traj_points
 
+    key = round(smooth_factor, 3)
+    if key not in _TRAJ_CACHE["butter_coeffs"]:
+        b, a = butter(1, smooth_factor, btype="low")
+        _TRAJ_CACHE["butter_coeffs"][key] = (b.astype(np.float64), a.astype(np.float64))
+    b, a = _TRAJ_CACHE["butter_coeffs"][key]
 
-# ====================== 3. 核心增强：轨迹平滑滤波（修复阶数不匹配） ======================
-def smooth_cartesian_traj(traj_points, smooth_factor=0.2):
-    """
-    贝塞尔曲线平滑笛卡尔轨迹（修复点数不足问题）
-    :param traj_points: 原始轨迹点列表
-    :param smooth_factor: 平滑系数（0-1，越大越平滑）
-    :return: 平滑后的轨迹点
-    """
-    traj_array = np.array(traj_points)
-    n_points = len(traj_array)
-
-    # 处理点数不足的情况：
-    # - 1个点：直接返回
-    # - 2-3个点：使用线性插值/低阶样条
-    # - 4个及以上点：使用3阶样条
-    if n_points <= 1:
+    k = min(3, len(traj_array) - 1)
+    t = np.linspace(0, 1, len(traj_array), dtype=np.float64)
+    t_smooth = np.linspace(0, 1, max(10, len(traj_array) * 2), dtype=np.float64)
+
+    try:
+        spline = interpolate.make_interp_spline(t, traj_array, k=k, axis=0)
+        smooth_vals = spline(t_smooth)
+        smooth_vals = filtfilt(b, a, smooth_vals, axis=0)
+
+        smoothed_traj = np.empty_like(traj_array)
+        for dim in range(3):
+            smoothed_traj[:, dim] = np.interp(t, t_smooth, smooth_vals[:, dim])
+        return smoothed_traj.tolist()
+    except Exception:
         return traj_points
-    elif n_points <= 3:
-        k = n_points - 1  # 适配点数的阶数（2点→1阶，3点→2阶）
-    else:
-        k = 3  # 4点及以上用3阶样条
 
-    # 生成插值参数
-    t = np.linspace(0, 1, n_points)
-    smoothed_traj = np.zeros_like(traj_array)
 
-    for dim in range(3):  # x/y/z三个维度
-        try:
-            # 生成适配阶数的样条曲线（核心修复）
-            spline = interpolate.make_interp_spline(t, traj_array[:, dim], k=k)
-            # 生成更密集的插值点（提升平滑度）
-            t_smooth = np.linspace(0, 1, max(10, n_points * 2))
-            smooth_vals = spline(t_smooth)
+def time_optimal_joint_trajectory(
+        start_joint: np.ndarray,
+        end_joint: np.ndarray,
+        seg_time: float
+) -> Tuple[np.ndarray, np.ndarray, np.ndarray]:
+    """时间最优关节轨迹（改为5关节）"""
+    limits = _TRAJ_CACHE["joint_limits"]
+    max_vel = limits["max_vel"]
+    max_acc = limits["max_acc"]
+
+    config = get_config()
+    traj_points = config.efficiency.traj_interp_points
+
+    t_steps = np.linspace(0, seg_time, traj_points, dtype=np.float64)
+    # 改为5列（5个关节）
+    opt_pos = np.empty((traj_points, JOINT_COUNT), dtype=np.float64)
+    opt_vel = np.empty_like(opt_pos)
+    opt_acc = np.empty_like(opt_pos)
+
+    delta = end_joint - start_joint
+    delta_abs = np.abs(delta)
+    sign = np.sign(delta)
+
+    t_acc = max_vel / max_acc
+    s_acc = 0.5 * max_acc * t_acc ** 2
+    t_joint = np.where(
+        delta_abs < 2 * s_acc,
+        2 * np.sqrt(delta_abs / max_acc),
+        2 * t_acc + (delta_abs - 2 * s_acc) / max_vel
+    )
+
+    # 遍历5个关节
+    for i, t in enumerate(t_steps):
+        for j in range(JOINT_COUNT):
+            if delta_abs[j] < 2 * s_acc[j]:
+                if t <= t_joint[j] / 2:
+                    opt_pos[i, j] = start_joint[j] + 0.5 * max_acc[j] * t ** 2 * sign[j]
+                    opt_vel[i, j] = max_acc[j] * t * sign[j]
+                    opt_acc[i, j] = max_acc[j] * sign[j]
+                else:
+                    t_rem = t_joint[j] - t
+                    opt_pos[i, j] = end_joint[j] - 0.5 * max_acc[j] * t_rem ** 2 * sign[j]
+                    opt_vel[i, j] = max_acc[j] * t_rem * sign[j]
+                    opt_acc[i, j] = -max_acc[j] * sign[j]
+            else:
+                if t <= t_acc[j]:
+                    opt_pos[i, j] = start_joint[j] + 0.5 * max_acc[j] * t ** 2 * sign[j]
+                    opt_vel[i, j] = max_acc[j] * t * sign[j]
+                    opt_acc[i, j] = max_acc[j] * sign[j]
+                elif t <= t_acc[j] + (delta_abs[j] - 2 * s_acc[j]) / max_vel[j]:
+                    opt_pos[i, j] = start_joint[j] + (s_acc[j] + max_vel[j] * (t - t_acc[j])) * sign[j]
+                    opt_vel[i, j] = max_vel[j] * sign[j]
+                    opt_acc[i, j] = 0.0
+                else:
+                    t_rem = t_joint[j] - t
+                    opt_pos[i, j] = end_joint[j] - 0.5 * max_acc[j] * t_rem ** 2 * sign[j]
+                    opt_vel[i, j] = max_acc[j] * t_rem * sign[j]
+                    opt_acc[i, j] = -max_acc[j] * sign[j]
 
-            # 低通滤波（增加异常处理，提升稳定性）
-            try:
-                from scipy.signal import filtfilt, butter
-                b, a = butter(2, smooth_factor, btype="low")
-                smooth_vals = filtfilt(b, a, smooth_vals)
-            except:
-                # 滤波失败时直接使用插值结果
-                pass
+        opt_vel[i] = np.clip(opt_vel[i], -max_vel, max_vel)
+        opt_acc[i] = np.clip(opt_acc[i], -max_acc, max_acc)
 
-            # 采样回原点数
-            smoothed_traj[:, dim] = np.interp(t, t_smooth, smooth_vals)
-        except Exception as e:
-            # 插值失败时降级为原始轨迹
-            logger.warning(f"轨迹平滑失败（维度{dim}）：{e}，使用原始轨迹")
-            smoothed_traj = traj_array
-            break
+    return opt_pos, opt_vel, opt_acc
 
-    return smoothed_traj.tolist()
 
+def full_arm_collision_check(
+        model,
+        data,
+        return_min_dist: bool = True
+) -> Tuple[bool, float] | bool:
+    """全链路碰撞检测"""
+    if not _COLLISION_CACHE["link_ids"]:
+        # 5个关节对应的连杆
+        link_names = ["link1", "link2", "link3", "link4", "link5", "end_effector"]
+        for name in link_names:
+            _COLLISION_CACHE["link_ids"][name] = mujoco.mj_name2id(
+                model, mujoco.mjtObj.mjOBJ_BODY, name
+            )
 
-def smooth_joint_traj(joint_traj, smooth_factor=0.1):
-    """
-    平滑关节轨迹（修复点数不足+简化滤波）
-    """
-    joint_array = np.array(joint_traj)
-    n_points, n_joints = joint_array.shape
+    collision = False
+    min_dist = float("inf")
+    obstacle_array = _COLLISION_CACHE["obstacle_array"]
+    safety_margin = _COLLISION_CACHE["safety_margin"]
+    grid_min, grid_max = _COLLISION_CACHE["obstacle_grid"]
 
-    # 点数不足时直接返回
-    if n_points <= 1:
-        return joint_traj
+    for link_name, link_id in _COLLISION_CACHE["link_ids"].items():
+        try:
+            link_pos = data.xpos[link_id].astype(np.float64)
 
-    # 简化滤波逻辑，提升兼容性
-    try:
-        from scipy.signal import filtfilt, butter
-        b, a = butter(1, smooth_factor, btype="low")  # 1阶滤波更稳定
-        smoothed_joints = np.zeros_like(joint_array)
+            if np.any(link_pos < grid_min) or np.any(link_pos > grid_max):
+                continue
 
-        for j in range(n_joints):
-            smoothed_joints[:, j] = filtfilt(b, a, joint_array[:, j])
-    except Exception as e:
-        logger.warning(f"关节轨迹平滑失败：{e}，使用原始轨迹")
-        smoothed_joints = joint_array
+            obs_pos = obstacle_array[:, :3]
+            obs_radius = obstacle_array[:, 3]
+            distances = np.linalg.norm(link_pos - obs_pos, axis=1) - (obs_radius + safety_margin)
 
-    return smoothed_joints
+            if np.any(distances < 0):
+                collision = True
+                if not return_min_dist:
+                    return True
 
+            if return_min_dist:
+                min_dist = min(min_dist, np.min(distances))
+        except Exception:
+            continue
 
-# ====================== 4. 核心增强：真实能耗计算 ======================
-def calculate_real_energy_consumption(model, data, config, dt):
-    """
-    计算真实能耗（考虑电机效率、摩擦损耗）
-    """
-    energy = 0.0
+    if return_min_dist:
+        return collision, min_dist
+    return collision
 
-    for joint_idx in range(6):
-        # 1. 获取MuJoCo实时输出扭矩
-        real_torque = data.qfrc_actuator[joint_idx]
-        # 2. 关节速度
-        joint_vel = data.qvel[joint_idx]
-        # 3. 摩擦损耗
-        friction_loss = config.joint_friction[joint_idx] * abs(joint_vel)
-        # 4. 实际能耗计算
-        mechanical_power = abs(real_torque * joint_vel)
-        total_power = mechanical_power + friction_loss
-        energy += total_power / config.motor_efficiency * dt
 
-    return energy
+def calculate_real_energy_consumption(model, data, dt: float) -> float:
+    """真实能耗计算（改为5关节）"""
+    friction = _ENERGY_CACHE["friction"]
+    motor_eff = _ENERGY_CACHE["motor_eff"]
 
+    # 只取前5个关节的数据
+    torques = data.qfrc_actuator[:JOINT_COUNT].astype(np.float64)
+    velocities = data.qvel[:JOINT_COUNT].astype(np.float64)
 
-# ====================== 5. 核心增强：可视化工具（适配新版MuJoCo API） ======================
-def draw_enhanced_visualization(viewer, model, data, config):
-    """
-    增强可视化：适配新版MuJoCo API，避免MjGeom报错
-    注：新版MuJoCo简化了用户绘制，这里改用更兼容的方式展示关键信息
-    """
+    friction_loss = np.sum(friction * np.abs(velocities))
+    mechanical_power = np.sum(np.abs(torques * velocities))
+    total_energy = (mechanical_power + friction_loss) * dt / motor_eff
+
+    return float(total_energy)
+
+
+# ====================== 4. 可视化模块 ======================
+def draw_enhanced_visualization(
+        viewer_inst,
+        model,
+        data,
+        traj_history: Deque[list],
+        collision_warning: bool
+):
+    """增强可视化"""
     try:
-        # 方式1：使用新版Viewer的user_scn（兼容大部分版本）
-        scene = viewer.user_scn
-        scene.ngeom = 0  # 清空原有绘制
+        scene = viewer_inst.user_scn
+        scene.ngeom = 0
 
-        # 1. 绘制轨迹历史（使用mjv_geom接口）
         if len(traj_history) > 1:
-            traj_array = np.array(list(traj_history))
+            traj_array = np.array(traj_history, dtype=np.float64)
 
-            # 轨迹线
             for i in range(len(traj_array) - 1):
-                # 初始化几何对象（适配新版API）
                 geom = mujoco.MjvGeom()
                 mujoco.mjv_initGeom(
                     geom,
                     mujoco.mjtGeom.mjGEOM_LINE,
-                    np.array([0.003, 0, 0]),  # 大小
-                    traj_array[i],  # 起始点
-                    traj_array[i + 1],  # 结束点
-                    np.array([0, 1, 0, 0.6])  # 绿色轨迹线
+                    np.array([0.003, 0, 0], dtype=np.float64),
+                    traj_array[i],
+                    traj_array[i + 1],
+                    np.array([0, 1, 0, 0.6], dtype=np.float64)
                 )
-                # 添加到场景
                 mujoco.mjv_addGeom(scene, model, data, geom)
 
-            # 轨迹起点（蓝色）
-            start_geom = mujoco.MjvGeom()
-            mujoco.mjv_initGeom(
-                start_geom,
-                mujoco.mjtGeom.mjGEOM_SPHERE,
-                np.array([0.015, 0, 0]),
-                traj_array[0],
-                np.array([0, 0, 0]),
-                np.array([0, 0, 1, 0.8])
-            )
-            mujoco.mjv_addGeom(scene, model, data, start_geom)
-
-            # 轨迹终点（红色）
-            end_geom = mujoco.MjvGeom()
-            mujoco.mjv_initGeom(
-                end_geom,
-                mujoco.mjtGeom.mjGEOM_SPHERE,
-                np.array([0.015, 0, 0]),
-                traj_array[-1],
-                np.array([0, 0, 0]),
-                np.array([1, 0, 0, 0.8])
-            )
-            mujoco.mjv_addGeom(scene, model, data, end_geom)
+            def draw_sphere(pos, rgba, size):
+                geom = mujoco.MjvGeom()
+                mujoco.mjv_initGeom(
+                    geom,
+                    mujoco.mjtGeom.mjGEOM_SPHERE,
+                    np.array([size, 0, 0], dtype=np.float64),
+                    pos,
+                    np.array([0, 0, 0], dtype=np.float64),
+                    np.array(rgba, dtype=np.float64)
+                )
+                mujoco.mjv_addGeom(scene, model, data, geom)
+
+            draw_sphere(traj_array[0], [0, 0, 1, 0.8], 0.015)
+            draw_sphere(traj_array[-1], [1, 0, 0, 0.8], 0.015)
 
-        # 2. 绘制碰撞警告（红色半透明球）
         if collision_warning:
             ee_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "ee_site")
             ee_pos = data.site_xpos[ee_id]
-
-            warn_geom = mujoco.MjvGeom()
-            mujoco.mjv_initGeom(
-                warn_geom,
-                mujoco.mjtGeom.mjGEOM_SPHERE,
-                np.array([0.08, 0, 0]),
-                ee_pos,
-                np.array([0, 0, 0]),
-                np.array([1, 0, 0, 0.3])
-            )
-            mujoco.mjv_addGeom(scene, model, data, warn_geom)
+            draw_sphere(ee_pos, [1, 0, 0, 0.3], 0.08)
 
     except Exception as e:
-        # 兼容最简化版本：禁用可视化绘制，避免影响核心功能
-        logger.warning(f"可视化绘制失败（MuJoCo版本兼容问题）：{e}")
-        logger.warning("已禁用可视化增强，核心仿真功能不受影响")
-
-
-# ====================== 6. 基础工具函数 ======================
-def get_ee_cartesian_velocity(model, data, ee_site_id):
-    jacp = np.zeros((3, model.nv))
-    jacr = np.zeros((3, model.nv))
-    mujoco.mj_jacSite(model, data, jacp, jacr, ee_site_id)
-    joint_vel = data.qvel[:6]
-    ee_cart_vel = jacp @ joint_vel
-    return ee_cart_vel
-
-
-def check_local_optimum(ee_vel, ee_pos, target_pos, config):
-    global stagnant_start_time
-    vel_mag = np.linalg.norm(ee_vel)
-
-    if vel_mag < config.stagnant_threshold:
-        if stagnant_start_time is None:
-            stagnant_start_time = time.time()
-        elif time.time() - stagnant_start_time > config.stagnant_time:
-            logger.warning(f"检测到局部最优！末端速度={vel_mag:.4f}m/s")
-            dir_to_target = np.array(target_pos) - np.array(ee_pos)
-            dir_norm = np.linalg.norm(dir_to_target)
-            if dir_norm < 1e-6:
-                dir_to_target = np.array([0, 0, 0.1])
-            else:
-                dir_to_target = dir_to_target / dir_norm
-
-            guide_target = np.array(ee_pos) + dir_to_target * config.guide_offset
-            stagnant_start_time = None
-            return True, guide_target.tolist()
-    else:
-        stagnant_start_time = None
-
-    return False, target_pos
+        logging.warning(f"可视化绘制失败：{e}")
 
 
-def adaptive_potential_params(ee_pos, obstacle_list, config):
-    obs_distances = [np.linalg.norm(np.array(ee_pos) - np.array(obs[:3])) for obs in obstacle_list]
-    min_dist = min(obs_distances) if obs_distances else 1.0
-
-    k_rep = config.base_k_rep if min_dist > 0.2 else config.base_k_rep * 2.0
-    k_att = config.base_k_att if len(obstacle_list) <= 2 else config.base_k_att * 0.5
-    return k_att, k_rep
-
-
-def robust_artificial_potential_field(ee_pos, ee_vel, target_pos, obstacle_list, config):
-    ee_pos = np.array(ee_pos)
-    target_pos = np.array(target_pos)
-
-    # 局部最优规避
-    is_local_opt, guide_target = check_local_optimum(ee_vel, ee_pos, target_pos, config)
-    current_target = np.array(guide_target) if is_local_opt else target_pos
-
-    # 自适应参数
-    k_att, k_rep = adaptive_potential_params(ee_pos, obstacle_list, config)
-
-    # 引力+斥力计算
-    att_force = k_att * (current_target - ee_pos)
-    rep_force = np.zeros(3)
-
-    for obs in obstacle_list:
-        obs_pos = np.array(obs[:3])
-        obs_radius = obs[3]
-        dist = np.linalg.norm(ee_pos - obs_pos)
-
-        if dist < config.rep_radius + obs_radius:
-            rep_dir = (ee_pos - obs_pos) / (dist + 1e-6)
-            rep_force += k_rep * (1 / (dist - obs_radius) - 1 / config.rep_radius) * (1 / dist ** 2) * rep_dir
-
-    # 修正目标并约束
-    corrected_target = ee_pos + att_force + rep_force
-    corrected_target = np.clip(corrected_target, [0.3, -0.4, 0.2], [0.9, 0.4, 1.0])
-
-    return corrected_target.tolist()
-
-
-# ====================== 7. 轨迹规划 ======================
-def time_optimal_joint_trajectory(start_joint, end_joint, seg_time, config):
-    n_joints = 6
-    traj_points = config.traj_interp_points
-    t_steps = np.linspace(0, seg_time, traj_points)
-
-    opt_pos = np.zeros((traj_points, n_joints))
-    opt_vel = np.zeros((traj_points, n_joints))
-    opt_acc = np.zeros((traj_points, n_joints))
-
-    for j in range(n_joints):
-        delta = end_joint[j] - start_joint[j]
-        max_vel = config.max_vel[j]
-        max_acc = config.max_acc[j]
-
-        t_acc = max_vel / max_acc
-        s_acc = 0.5 * max_acc * t_acc ** 2
-
-        if abs(delta) < 2 * s_acc:
-            t_joint = 2 * np.sqrt(abs(delta) / max_acc)
-            for i, t in enumerate(t_steps):
-                if t <= t_joint / 2:
-                    opt_pos[i, j] = start_joint[j] + 0.5 * max_acc * t ** 2 * np.sign(delta)
-                    opt_vel[i, j] = max_acc * t * np.sign(delta)
-                    opt_acc[i, j] = max_acc * np.sign(delta)
-                else:
-                    t_rem = t_joint - t
-                    opt_pos[i, j] = end_joint[j] - 0.5 * max_acc * t_rem ** 2 * np.sign(delta)
-                    opt_vel[i, j] = max_acc * t_rem * np.sign(delta)
-                    opt_acc[i, j] = -max_acc * np.sign(delta)
-        else:
-            t_const = (abs(delta) - 2 * s_acc) / max_vel
-            t_joint = 2 * t_acc + t_const
-            for i, t in enumerate(t_steps):
-                if t <= t_acc:
-                    opt_pos[i, j] = start_joint[j] + 0.5 * max_acc * t ** 2 * np.sign(delta)
-                    opt_vel[i, j] = max_acc * t * np.sign(delta)
-                    opt_acc[i, j] = max_acc * np.sign(delta)
-                elif t <= t_acc + t_const:
-                    opt_pos[i, j] = start_joint[j] + (s_acc + max_vel * (t - t_acc)) * np.sign(delta)
-                    opt_vel[i, j] = max_vel * np.sign(delta)
-                    opt_acc[i, j] = 0.0
-                else:
-                    t_rem = t_joint - t
-                    opt_pos[i, j] = end_joint[j] - 0.5 * max_acc * t_rem ** 2 * np.sign(delta)
-                    opt_vel[i, j] = max_acc * t_rem * np.sign(delta)
-                    opt_acc[i, j] = -max_acc * np.sign(delta)
-
-        opt_vel[:, j] = np.clip(opt_vel[:, j], -max_vel, max_vel)
-        opt_acc[:, j] = np.clip(opt_acc[:, j], -max_acc, max_acc)
-
-    # 平滑关节轨迹（使用修复后的函数）
-    opt_pos = smooth_joint_traj(opt_pos, config.smooth_factor)
-
-    return opt_pos, opt_vel, opt_acc
-
-
-def energy_optimal_trajectory(joint_waypoints, seg_time, config):
-    n_joints = 6
-    n_points = len(joint_waypoints)
-    t_step = seg_time / (n_points - 1)
-
-    q = cp.Variable((n_joints, n_points))
-    qd = cp.Variable((n_joints, n_points))
-    qdd = cp.Variable((n_joints, n_points))
-
-    energy_cost = cp.sum_squares(qdd)
-    time_cost = cp.sum(cp.max(cp.abs(qd), axis=1))
-    total_cost = config.time_weight * time_cost + config.energy_weight * energy_cost
-
-    constraints = []
-    constraints.append(q[:, 0] == joint_waypoints[0])
-    constraints.append(q[:, -1] == joint_waypoints[-1])
-    constraints.append(qd[:, 0] == 0)
-    constraints.append(qd[:, -1] == 0)
-
-    for j in range(n_joints):
-        constraints.append(qd[j, :] <= config.max_vel[j])
-        constraints.append(qd[j, :] >= -config.max_vel[j])
-        constraints.append(qdd[j, :] <= config.max_acc[j])
-        constraints.append(qdd[j, :] >= -config.max_acc[j])
-
-    for i in range(n_points - 1):
-        constraints.append(qd[:, i + 1] == (q[:, i + 1] - q[:, i]) / t_step)
-        constraints.append(qdd[:, i + 1] == (qd[:, i + 1] - qd[:, i]) / t_step)
-
-    prob = cp.Problem(cp.Minimize(total_cost), constraints)
-    try:
-        prob.solve(solver=cp.ECOS, verbose=False)
-    except:
-        try:
-            prob.solve(solver=cp.OSQP, verbose=False)
-        except:
-            prob.solve(verbose=False)
-
-    if prob.status != cp.OPTIMAL:
-        logger.warning("能耗优化求解失败，降级为时间最优轨迹")
-        return None
-
-    return q.value.T
-
-
-def optimize_obstacle_traj_with_efficiency(model, data, ee_pos, target_pos, config):
-    global total_motion_time, total_energy_consume
-
-    # 1. 鲁棒避障修正目标
-    ee_vel = get_ee_cartesian_velocity(model, data, mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "ee_site"))
-    corrected_cart_target = robust_artificial_potential_field(ee_pos, ee_vel, target_pos, config.obstacle_list, config)
-
-    # 2. 平滑笛卡尔目标轨迹（使用修复后的函数）
-    corrected_cart_target = smooth_cartesian_traj([ee_pos, corrected_cart_target], config.smooth_factor)[-1]
-
-    # 3. 逆解得到关节目标
-    ee_site_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "ee_site")
-    data.site_xpos[ee_site_id] = corrected_cart_target
-    mujoco.mj_inverse(model, data)
-    end_joint = data.qpos[:6].copy()
-    start_joint = data.qpos[:6].copy()
-
-    # 4. 时间最优轨迹
-    seg_time = 2.0
-    time_opt_pos, time_opt_vel, time_opt_acc = time_optimal_joint_trajectory(start_joint, end_joint, seg_time, config)
-
-    # 5. 能耗最优优化
-    energy_opt_pos = energy_optimal_trajectory(time_opt_pos, seg_time, config)
-    if energy_opt_pos is None:
-        final_joint_traj = time_opt_pos
-    else:
-        final_joint_traj = energy_opt_pos
-
-    # 6. 计算真实能耗
-    dt = seg_time / len(final_joint_traj)
-    seg_energy = 0.0
-
-    for traj_idx in range(len(final_joint_traj)):
-        if traj_idx == 0:
-            continue
+# ====================== 5. 机械臂模型构建（5关节） ======================
+def get_arm_xml_with_obstacles(config: RobotConfig) -> str:
+    """生成机械臂XML模型（5关节）"""
+    obstacles_xml = ""
+    for i, obs in enumerate(config.obstacle.obstacle_list):
+        x, y, z, r = obs
+        obstacles_xml += f"""
+    <body name="obstacle_{i}" pos="{x} {y} {z}">
+        <geom name="obs_geom_{i}" type="sphere" size="{r}" rgba="1 0 0 0.5"/>
+    </body>
+        """
 
-        # 使用真实扭矩计算能耗
-        seg_energy += calculate_real_energy_consumption(model, data, config, dt)
-
-    # 更新全局统计
-    total_motion_time += seg_time
-    total_energy_consume += seg_energy
-
-    # 记录轨迹历史
-    traj_history.append(corrected_cart_target)
-
-    return final_joint_traj[0], corrected_cart_target, seg_energy
-
-
-# ====================== 8. 机械臂模型 ======================
-def get_arm_xml_with_obstacles(config):
-    arm_xml = """
-<mujoco model="6dof_arm_efficiency_optimized">
-  <compiler angle="radian" inertiafromgeom="true"/>
-  <option timestep="0.005" gravity="0 0 -9.81"/>
-  <asset>
-    <material name="gray" rgba="0.7 0.7 0.7 1"/>
-    <material name="blue" rgba="0.2 0.4 0.8 1"/>
-    <material name="red" rgba="0.8 0.2 0.2 1"/>
-    <material name="obstacle" rgba="1 0 0 0.5"/>
-    <material name="critical_obstacle" rgba="1 0 0 0.7"/>
-  </asset>
-  <worldbody>
-    <geom name="floor" type="plane" size="5 5 0.1" pos="0 0 0" material="gray"/>
-    <body name="base" pos="0 0 0">
-      <inertial pos="0 0 0" mass="5.0" diaginertia="0.01 0.01 0.01"/>
-      <geom name="base_geom" type="cylinder" size="0.15 0.1" pos="0 0 0" material="gray"/>
-      <joint name="joint0" type="hinge" axis="0 0 1" pos="0 0 0.1"/>
-      <body name="link1" pos="0 0 0.1">
-        <inertial pos="0 0 0.15" mass="1.2" diaginertia="0.02 0.02 0.02"/>
-        <geom name="link1_geom" type="capsule" size="0.05" fromto="0 0 0 0 0 0.3" material="blue"/>
-        <joint name="joint1" type="hinge" axis="0 1 0" pos="0 0 0.3"/>
-        <body name="link2" pos="0 0 0.3">
-          <inertial pos="0.2 0 0" mass="1.0" diaginertia="0.015 0.015 0.015"/>
-          <geom name="link2_geom" type="capsule" size="0.05" fromto="0 0 0 0.4 0 0" material="blue"/>
-          <joint name="joint2" type="hinge" axis="0 1 0" pos="0.4 0 0"/>
-          <body name="link3" pos="0.4 0 0">
-            <inertial pos="0.175 0 0" mass="0.8" diaginertia="0.01 0.01 0.01"/>
-            <geom name="link3_geom" type="capsule" size="0.04" fromto="0 0 0 0.35 0 0" material="blue"/>
-            <joint name="joint3" type="hinge" axis="1 0 0" pos="0.35 0 0"/>
-            <body name="link4" pos="0.35 0 0">
-              <inertial pos="0 0 0.125" mass="0.6" diaginertia="0.008 0.008 0.008"/>
-              <geom name="link4_geom" type="capsule" size="0.04" fromto="0 0 0 0 0 0.25" material="blue"/>
-              <joint name="joint4" type="hinge" axis="0 1 0" pos="0 0 0.25"/>
-              <body name="link5" pos="0 0 0.25">
-                <inertial pos="0 0 0.1" mass="0.4" diaginertia="0.008 0.008 0.008"/>
-                <geom name="link5_geom" type="capsule" size="0.03" fromto="0 0 0 0 0 0.2" material="blue"/>
-                <joint name="joint5" type="hinge" axis="1 0 0" pos="0 0 0.2"/>
-                <body name="end_effector" pos="0 0 0.2">
-                  <inertial pos="0 0 0" mass="0.2" diaginertia="0.005 0.005 0.005"/>
-                  <geom name="ee_geom" type="box" size="0.08 0.08 0.08" pos="0 0 0" material="red"/>
-                  <site name="ee_site" pos="0 0 0" type="sphere" size="0.01" rgba="1 0 0 1"/>
+    xml = f"""
+<mujoco model="robotic_arm">
+    <compiler angle="radian" inertiafromgeom="true"/>
+    <option timestep="{config.simulation.timestep}" gravity="0 0 -9.81"/>
+
+    <worldbody>
+        <!-- 地面 -->
+        <geom name="floor" type="plane" size="5 5 0.1" pos="0 0 0" rgba="0.8 0.8 0.8 1"/>
+
+        <!-- 机械臂基座 -->
+        <body name="base" pos="0 0 0">
+            <geom name="base_geom" type="cylinder" size="0.1 0.1" rgba="0.2 0.2 0.8 1"/>
+
+            <!-- 关节1 -->
+            <body name="link1" pos="0 0 0.1">
+                <joint name="joint1" type="hinge" axis="0 0 1" range="-3.14 3.14"/>
+                <geom name="link1_geom" type="cylinder" size="0.05 0.2" rgba="0.2 0.8 0.2 1"/>
+
+                <!-- 关节2 -->
+                <body name="link2" pos="0 0 0.2">
+                    <joint name="joint2" type="hinge" axis="0 1 0" range="-3.14 3.14"/>
+                    <geom name="link2_geom" type="cylinder" size="0.05 0.2" rgba="0.2 0.8 0.2 1"/>
+
+                    <!-- 关节3 -->
+                    <body name="link3" pos="0 0 0.2">
+                        <joint name="joint3" type="hinge" axis="0 1 0" range="-3.14 3.14"/>
+                        <geom name="link3_geom" type="cylinder" size="0.05 0.2" rgba="0.2 0.8 0.2 1"/>
+
+                        <!-- 关节4 -->
+                        <body name="link4" pos="0 0 0.2">
+                            <joint name="joint4" type="hinge" axis="0 1 0" range="-3.14 3.14"/>
+                            <geom name="link4_geom" type="cylinder" size="0.05 0.2" rgba="0.2 0.8 0.2 1"/>
+
+                            <!-- 关节5 -->
+                            <body name="link5" pos="0 0 0.2">
+                                <joint name="joint5" type="hinge" axis="0 1 0" range="-3.14 3.14"/>
+                                <geom name="link5_geom" type="cylinder" size="0.05 0.1" rgba="0.2 0.8 0.2 1"/>
+
+                                <!-- 末端执行器 -->
+                                <body name="end_effector" pos="0 0 0.1">
+                                    <site name="ee_site" pos="0 0 0" size="0.01"/>
+                                    <geom name="ee_geom" type="sphere" size="0.05" rgba="0.8 0.2 0.2 1"/>
+                                </body>
+                            </body>
+                        </body>
+                    </body>
                 </body>
-              </body>
             </body>
-          </body>
         </body>
-      </body>
-    </body>
-    """
 
-    # 添加障碍物
-    for i, obs in enumerate(config.obstacle_list):
-        x, y, z, r = obs
-        material = "critical_obstacle" if i == 0 else "obstacle"
-        arm_xml += f"""
-    <geom name="obstacle_{i}" type="sphere" size="{r}" pos="{x} {y} {z}" material="{material}"/>
-        """
-
-    arm_xml += """
-  </worldbody>
-  <actuator>
-    <motor name="motor0" joint="joint0" ctrlrange="-3.14 3.14" gear="100"/>
-    <motor name="motor1" joint="joint1" ctrlrange="-1.57 1.57" gear="100"/>
-    <motor name="motor2" joint="joint2" ctrlrange="-1.57 1.57" gear="100"/>
-    <motor name="motor3" joint="joint3" ctrlrange="-3.14 3.14" gear="100"/>
-    <motor name="motor4" joint="joint4" ctrlrange="-1.57 1.57" gear="100"/>
-    <motor name="motor5" joint="joint5" ctrlrange="-3.14 3.14" gear="100"/>
-  </actuator>
+        <!-- 障碍物 -->
+        {obstacles_xml}
+    </worldbody>
+
+    <!-- 控制器（5个电机） -->
+    <actuator>
+        <motor name="motor1" joint="joint1" ctrlrange="-1 1" gear="100"/>
+        <motor name="motor2" joint="joint2" ctrlrange="-1 1" gear="100"/>
+        <motor name="motor3" joint="joint3" ctrlrange="-1 1" gear="100"/>
+        <motor name="motor4" joint="joint4" ctrlrange="-1 1" gear="100"/>
+        <motor name="motor5" joint="joint5" ctrlrange="-1 1" gear="100"/>
+    </actuator>
 </mujoco>
     """
-    return arm_xml
+    return xml
 
 
-# ====================== 9. 仿真主逻辑 ======================
-def run_enhanced_simulation():
-    global total_motion_time, total_energy_consume
+# ====================== 6. 仿真器主类（5关节） ======================
+class ArmSimulator:
+    def __init__(self):
+        self.config = get_config()
+        init_global_caches()
 
-    # 生成模型XML
-    arm_xml = get_arm_xml_with_obstacles(config)
+        # 配置日志
+        self._setup_logging()
 
-    with tempfile.NamedTemporaryFile(mode='w', suffix='.xml', delete=False) as f:
-        f.write(arm_xml)
-        xml_path = f.name
+        # 初始化仿真环境
+        self._init_simulation()
 
-    try:
-        model = mujoco.MjModel.from_xml_path(xml_path)
-        data = mujoco.MjData(model)
-        logger.info("✅ 增强版机械臂仿真模型加载成功！")
-        logger.info(
-            f"🔧 配置：轨迹点数={config.traj_interp_points}, 时间权重={config.time_weight}, 平滑系数={config.smooth_factor}")
+        # 状态管理
+        self.total_motion_time = 0.0
+        self.total_energy_consume = 0.0
+        self.traj_history: Deque[list] = deque(maxlen=50)
+        self.collision_warning = False
+        self.stagnant_start_time: Optional[float] = None
 
         # 预计算关节起点
-        ee_site_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "ee_site")
+        self.ee_site_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_SITE, "ee_site")
+        self.joint_waypoints = self._precompute_joint_waypoints()
+
+        self.logger.info("✅ 机械臂仿真器初始化完成")
+        self.logger.info(f"🔧 使用mujoco viewer模式：{MUJOCO_VIEWER_MODE}")
+        self.logger.info(f"🔧 机械臂关节数：{JOINT_COUNT}")
+
+    def _setup_logging(self):
+        """配置日志系统"""
+        log_level = getattr(logging, self.config.simulation.log_level.upper())
+        logging.basicConfig(
+            level=log_level,
+            format="%(asctime)s - %(name)s - %(levelname)s - %(message)s",
+            handlers=[logging.StreamHandler()]
+        )
+        self.logger = logging.getLogger("ArmSimulator")
+
+    def _init_simulation(self):
+        """初始化仿真环境"""
+        arm_xml = get_arm_xml_with_obstacles(self.config)
+
+        # 创建临时文件
+        import tempfile
+        self.temp_file = tempfile.NamedTemporaryFile(mode='w', suffix='.xml', delete=False)
+        self.temp_file.write(arm_xml)
+        self.temp_file.close()
+
+        # 加载模型
+        self.model = mujoco.MjModel.from_xml_path(self.temp_file.name)
+        self.model.opt.timestep = self.config.simulation.timestep
+        self.data = mujoco.MjData(self.model)
+
+    def _precompute_joint_waypoints(self) -> list:
+        """预计算关节起点（5关节）"""
         joint_waypoints = []
-
-        for cart_pos in config.cart_waypoints:
-            mujoco.mj_resetData(model, data)
-            data.site_xpos[ee_site_id] = cart_pos
-            mujoco.mj_inverse(model, data)
-            joint_waypoints.append(data.qpos[:6].copy())
-
-        # 启动仿真视图
-        with mujoco.viewer.launch_passive(model, data) as viewer:
-            logger.info("\n🎮 增强版机械臂仿真启动！")
-            logger.info("💡 核心增强：全链路碰撞检测 + 轨迹平滑 + 真实能耗计算 + 可视化增强")
-            logger.info("💡 按 Ctrl+C 退出\n")
-
-            current_waypoint = 0
-            last_print_time = 0.0
-            pause_simulation = False
-
-            while viewer.is_running():
-                if pause_simulation:
-                    time.sleep(0.1)
-                    continue
-
-                t_total = data.time
-                ee_pos = data.site_xpos[ee_site_id].tolist()
-
-                # 切换目标点
-                if current_waypoint < len(config.cart_waypoints):
-                    target_cart = config.cart_waypoints[current_waypoint]
-                    if np.linalg.norm(np.array(ee_pos) - np.array(target_cart)) < 0.01:
-                        current_waypoint = (current_waypoint + 1) % len(config.cart_waypoints)
-                        logger.info(f"\n🔄 切换到目标点 {current_waypoint}: {np.round(target_cart, 3)}")
+        for cart_pos in self.config.trajectory.cart_waypoints:
+            mujoco.mj_resetData(self.model, self.data)
+            self.data.site_xpos[self.ee_site_id] = cart_pos
+            mujoco.mj_inverse(self.model, self.data)
+            # 只取前5个关节
+            joint_waypoints.append(self.data.qpos[:JOINT_COUNT].copy())
+        return joint_waypoints
+
+    def _get_ee_cartesian_velocity(self) -> np.ndarray:
+        """获取末端笛卡尔速度"""
+        jacp = np.zeros((3, self.model.nv), dtype=np.float64)
+        jacr = np.zeros((3, self.model.nv), dtype=np.float64)
+
+        mujoco.mj_jacSite(self.model, self.data, jacp, jacr, self.ee_site_id)
+        ee_vel = jacp @ self.data.qvel
+        return ee_vel
+
+    def _check_local_optimum(self, ee_vel: np.ndarray, ee_pos: list, target_pos: list) -> tuple:
+        """检测局部最优"""
+        vel_mag = np.linalg.norm(ee_vel)
+        if vel_mag < self.config.obstacle.stagnant_threshold:
+            if self.stagnant_start_time is None:
+                self.stagnant_start_time = time.time()
+            elif time.time() - self.stagnant_start_time > self.config.obstacle.stagnant_time:
+                self.logger.warning(f"检测到局部最优！末端速度={vel_mag:.4f}m/s")
+                dir_to_target = np.array(target_pos) - np.array(ee_pos, dtype=np.float64)
+                dir_norm = np.linalg.norm(dir_to_target)
+                if dir_norm < 1e-6:
+                    dir_to_target = np.array([0, 0, 0.1], dtype=np.float64)
                 else:
-                    target_cart = config.cart_waypoints[-1]
+                    dir_to_target = dir_to_target / dir_norm
 
+                guide_target = np.array(ee_pos, dtype=np.float64) + dir_to_target * self.config.obstacle.guide_offset
+                self.stagnant_start_time = None
+                return True, guide_target.tolist()
+        else:
+            self.stagnant_start_time = None
+        return False, target_pos
+
+    def _robust_artificial_potential_field(self, ee_pos: list, target_pos: list) -> list:
+        """人工势场法避障"""
+        ee_pos = np.array(ee_pos, dtype=np.float64)
+        target_pos = np.array(target_pos, dtype=np.float64)
+
+        ee_vel = self._get_ee_cartesian_velocity()
+        is_local_opt, guide_target = self._check_local_optimum(ee_vel, ee_pos.tolist(), target_pos.tolist())
+        current_target = np.array(guide_target, dtype=np.float64) if is_local_opt else target_pos
+
+        # 自适应参数
+        obs_distances = [np.linalg.norm(ee_pos - np.array(obs[:3], dtype=np.float64))
+                         for obs in self.config.obstacle.obstacle_list]
+        min_dist = min(obs_distances) if obs_distances else 1.0
+        k_rep = self.config.obstacle.base_k_rep if min_dist > 0.2 else self.config.obstacle.base_k_rep * 2.0
+        k_att = self.config.obstacle.base_k_att if len(
+            self.config.obstacle.obstacle_list) <= 2 else self.config.obstacle.base_k_att * 0.5
+
+        # 引力+斥力
+        att_force = k_att * (current_target - ee_pos)
+        rep_force = np.zeros(3, dtype=np.float64)
+
+        for obs in self.config.obstacle.obstacle_list:
+            obs_pos = np.array(obs[:3], dtype=np.float64)
+            obs_radius = obs[3]
+            dist = np.linalg.norm(ee_pos - obs_pos)
+
+            if dist < self.config.obstacle.rep_radius + obs_radius:
+                rep_dir = (ee_pos - obs_pos) / (dist + 1e-6)
+                rep_force += k_rep * (1 / (dist - obs_radius) - 1 / self.config.obstacle.rep_radius) * (
+                            1 / dist ** 2) * rep_dir
+
+        corrected_target = ee_pos + att_force + rep_force
+        corrected_target = np.clip(corrected_target, [0.3, -0.4, 0.2], [0.9, 0.4, 1.0])
+        return corrected_target.tolist()
+
+    def _energy_optimal_trajectory(self, joint_waypoints: np.ndarray, seg_time: float) -> Optional[np.ndarray]:
+        """能耗最优轨迹（5关节）"""
+        n_joints = JOINT_COUNT
+        n_points = len(joint_waypoints)
+        t_step = seg_time / (n_points - 1)
+
+        q = cp.Variable((n_joints, n_points))
+        qd = cp.Variable((n_joints, n_points))
+        qdd = cp.Variable((n_joints, n_points))
+
+        energy_cost = cp.sum_squares(qdd)
+        time_cost = cp.sum(cp.max(cp.abs(qd), axis=1))
+        total_cost = self.config.efficiency.time_weight * time_cost + self.config.efficiency.energy_weight * energy_cost
+
+        constraints = [
+            q[:, 0] == joint_waypoints[0],
+            q[:, -1] == joint_waypoints[-1],
+            qd[:, 0] == 0,
+            qd[:, -1] == 0
+        ]
+
+        max_vel = self.config.physics.max_vel
+        max_acc = self.config.physics.max_acc
+        for j in range(n_joints):
+            constraints.extend([
+                qd[j, :] <= max_vel[j],
+                qd[j, :] >= -max_vel[j],
+                qdd[j, :] <= max_acc[j],
+                qdd[j, :] >= -max_acc[j]
+            ])
+
+        for i in range(n_points - 1):
+            constraints.extend([
+                qd[:, i + 1] == (q[:, i + 1] - q[:, i]) / t_step,
+                qdd[:, i + 1] == (qd[:, i + 1] - qd[:, i]) / t_step
+            ])
+
+        prob = cp.Problem(cp.Minimize(total_cost), constraints)
+        try:
+            prob.solve(solver=cp.ECOS, verbose=False, warm_start=True)
+        except:
+            try:
+                prob.solve(solver=cp.OSQP, verbose=False, warm_start=True)
+            except:
+                prob.solve(verbose=False)
+
+        if prob.status != cp.OPTIMAL:
+            self.logger.warning("能耗优化求解失败，降级为时间最优轨迹")
+            return None
+
+        return q.value.T
+
+    def _optimize_obstacle_traj_with_efficiency(self, ee_pos: list, target_pos: list) -> tuple:
+        """轨迹优化主逻辑"""
+        # 避障修正
+        corrected_cart_target = self._robust_artificial_potential_field(ee_pos, target_pos)
+
+        # 平滑轨迹
+        corrected_cart_target = smooth_cartesian_traj([ee_pos, corrected_cart_target])[-1]
+
+        # 逆解
+        self.data.site_xpos[self.ee_site_id] = corrected_cart_target
+        mujoco.mj_inverse(self.model, self.data)
+        # 只取前5个关节
+        end_joint = self.data.qpos[:JOINT_COUNT].copy()
+        start_joint = self.data.qpos[:JOINT_COUNT].copy()
+
+        # 时间最优轨迹
+        seg_time = 2.0
+        time_opt_pos, _, _ = time_optimal_joint_trajectory(start_joint, end_joint, seg_time)
+
+        # 能耗最优
+        energy_opt_pos = self._energy_optimal_trajectory(time_opt_pos, seg_time)
+        final_joint_traj = energy_opt_pos if energy_opt_pos is not None else time_opt_pos
+
+        # 能耗计算
+        dt = seg_time / len(final_joint_traj)
+        seg_energy = sum([calculate_real_energy_consumption(self.model, self.data, dt)
+                          for _ in range(1, len(final_joint_traj))])
+
+        # 更新状态
+        self.total_motion_time += seg_time
+        self.total_energy_consume += seg_energy
+        self.traj_history.append(corrected_cart_target)
+
+        return final_joint_traj[0], corrected_cart_target, seg_energy
+
+    def _run_simulation_loop(self, viewer_inst):
+        """通用仿真循环"""
+        self.logger.info("🎮 机械臂仿真启动！")
+
+        config = self.config
+        fps = config.simulation.fps
+        sleep_time = 1.0 / fps
+        print_interval = 2.0
+        waypoints = np.array(config.trajectory.cart_waypoints, dtype=np.float64)
+        n_waypoints = len(waypoints)
+
+        current_waypoint = 0
+        last_print_time = 0.0
+        last_step_time = time.time()
+
+        while viewer_inst.is_running():
+            # 固定步长控制
+            current_time = time.time()
+            if current_time - last_step_time < sleep_time:
+                continue
+            last_step_time = current_time
+
+            # 获取当前状态
+            t_total = self.data.time
+            ee_pos = self.data.site_xpos[self.ee_site_id].tolist()
+
+            # 切换目标点
+            target_cart = waypoints[current_waypoint].tolist()
+            if np.linalg.norm(np.array(ee_pos, dtype=np.float64) - np.array(target_cart, dtype=np.float64)) < 0.01:
+                current_waypoint = (current_waypoint + 1) % n_waypoints
+                self.logger.info(f"🔄 切换到目标点 {current_waypoint}: {np.round(target_cart, 3)}")
+
+            try:
                 # 轨迹优化
-                target_joints, corrected_cart, seg_energy = optimize_obstacle_traj_with_efficiency(
-                    model, data, ee_pos, target_cart, config
-                )
+                target_joints, corrected_cart, _ = self._optimize_obstacle_traj_with_efficiency(ee_pos, target_cart)
+                target_joints = np.array(target_joints, dtype=np.float64)
 
-                # 全链路碰撞检测
-                is_collision, min_obs_dist = full_arm_collision_check(model, data, config)
+                # 碰撞检测
+                is_collision, min_obs_dist = full_arm_collision_check(self.model, self.data)
+                self.collision_warning = is_collision
 
                 # 紧急避障
                 if is_collision:
-                    logger.warning("🆘 检测到碰撞风险，执行紧急避障！")
-                    emergency_rep = np.array(ee_pos) - np.array(config.obstacle_list[0][:3])
+                    self.logger.warning("🆘 检测到碰撞风险，执行紧急避障！")
+                    emergency_rep = np.array(ee_pos, dtype=np.float64) - np.array(config.obstacle.obstacle_list[0][:3],
+                                                                                  dtype=np.float64)
                     emergency_rep = emergency_rep / np.linalg.norm(emergency_rep) * 0.05
-                    corrected_cart = np.array(corrected_cart) + emergency_rep
-                    data.site_xpos[ee_site_id] = corrected_cart
-                    mujoco.mj_inverse(model, data)
-                    target_joints = data.qpos[:6].copy()
-
-                # PD控制
-                ctrl_signals = []
-                for i in range(6):
-                    k_p = 8.0
-                    k_d = 0.2
-                    current_pos = data.qpos[i]
-                    current_vel = data.qvel[i]
-                    pos_error = target_joints[i] - current_pos
-                    vel_error = -current_vel
-                    ctrl = k_p * pos_error + k_d * vel_error
-                    max_ctrl = config.max_torque[i] / 100.0
-                    ctrl = np.clip(ctrl, -max_ctrl, max_ctrl)
-                    ctrl_signals.append(ctrl)
-
-                data.ctrl[:6] = ctrl_signals
+                    corrected_cart = np.array(corrected_cart, dtype=np.float64) + emergency_rep
+                    self.data.site_xpos[self.ee_site_id] = corrected_cart
+                    mujoco.mj_inverse(self.model, self.data)
+                    target_joints = self.data.qpos[:JOINT_COUNT].copy()
+
+                # PD控制（5关节）
+                max_torque = np.array(config.physics.max_torque, dtype=np.float64) / 100.0
+                pos_error = target_joints - self.data.qpos[:JOINT_COUNT]
+                vel_error = -self.data.qvel[:JOINT_COUNT]
+                ctrl_signals = 8.0 * pos_error + 0.2 * vel_error
+                ctrl_signals = np.clip(ctrl_signals, -max_torque, max_torque)
+                self.data.ctrl[:JOINT_COUNT] = ctrl_signals
 
                 # 打印统计信息
-                if t_total - last_print_time > 2.0 and t_total > 0:
-                    ee_vel = get_ee_cartesian_velocity(model, data, ee_site_id)
+                if t_total - last_print_time > print_interval and t_total > 0:
+                    ee_vel = self._get_ee_cartesian_velocity()
                     avg_vel = np.linalg.norm(ee_vel)
-                    avg_energy = total_energy_consume / t_total if t_total > 0 else 0.0
-
-                    logger.info(f"\n⏱️  仿真时间：{t_total:.2f}s | 累计运动时间：{total_motion_time:.2f}s")
-                    logger.info(f"   末端位置：{np.round(ee_pos, 3)} | 目标位置：{np.round(corrected_cart, 3)}")
-                    logger.info(f"   末端速度：{avg_vel:.4f}m/s | 最近障碍距离：{min_obs_dist:.3f}m")
-                    logger.info(f"   累计能耗：{total_energy_consume:.2f}J | 平均能耗：{avg_energy:.2f}J/s")
-                    logger.info(f"   碰撞风险：{'⚠️  高' if is_collision else '✅  低'}")
+                    avg_energy = self.total_energy_consume / t_total if t_total > 0 else 0.0
+
+                    self.logger.info(
+                        f"\n⏱️ 仿真时间：{t_total:.2f}s | 累计运动时间：{self.total_motion_time:.2f}s\n"
+                        f"   末端位置：{np.round(ee_pos, 3)} | 目标位置：{np.round(corrected_cart, 3)}\n"
+                        f"   末端速度：{avg_vel:.4f}m/s | 最近障碍距离：{min_obs_dist:.3f}m\n"
+                        f"   累计能耗：{self.total_energy_consume:.2f}J | 平均能耗：{avg_energy:.2f}J/s\n"
+                        f"   碰撞风险：{'⚠️ 高' if is_collision else '✅ 低'}"
+                    )
                     last_print_time = t_total
 
-                # 增强可视化（适配新版API）
-                draw_enhanced_visualization(viewer, model, data, config)
+                # 可视化
+                draw_enhanced_visualization(viewer_inst, self.model, self.data,
+                                            self.traj_history, self.collision_warning)
 
-                # 运行仿真步
-                mujoco.mj_step(model, data)
-                viewer.sync()
+            except Exception as e:
+                self.logger.error(f"仿真步执行失败：{e}", exc_info=False)
+                continue
 
-                try:
-                    mujoco.utils.mju_sleep(1 / 60)
-                except:
-                    time.sleep(1 / 60)
+            # 执行仿真步
+            mujoco.mj_step(self.model, self.data)
+            viewer_inst.sync()
 
-    except KeyboardInterrupt:
-        logger.info("\n🛑 用户终止仿真")
+    def run(self):
+        """运行仿真主循环"""
+        try:
+            with viewer.launch_passive(self.model, self.data) as viewer_inst:
+                self._run_simulation_loop(viewer_inst)
+
+        except KeyboardInterrupt:
+            self.logger.info("\n🛑 用户终止仿真")
+        except Exception as e:
+            self.logger.error(f"❌ 仿真出错：{e}", exc_info=True)
+        finally:
+            # 清理资源
+            if hasattr(self, 'temp_file'):
+                os.unlink(self.temp_file.name)
+            self.logger.info(f"\n📊 仿真结束 - 最终统计")
+            self.logger.info(f"   总运动时间：{self.total_motion_time:.2f}s")
+            self.logger.info(f"   总能耗：{self.total_energy_consume:.2f}J")
+            self.logger.info(
+                f"   综合得分：{self.total_motion_time * self.config.efficiency.time_weight + self.total_energy_consume * self.config.efficiency.energy_weight:.2f}")
+
+
+# ====================== 7. 主入口 ======================
+def main():
+    """程序主入口"""
+    try:
+        simulator = ArmSimulator()
+        simulator.run()
     except Exception as e:
-        logger.error(f"❌ 仿真出错：{e}")
-        import traceback
-        traceback.print_exc()
-    finally:
-        os.unlink(xml_path)
-        # 最终统计
-        logger.info(f"\n📊 仿真结束 - 最终统计")
-        logger.info(f"   总运动时间：{total_motion_time:.2f}s")
-        logger.info(f"   总能耗：{total_energy_consume:.2f}J")
-        logger.info(
-            f"   综合得分：{total_motion_time * config.time_weight + total_energy_consume * config.energy_weight:.2f}")
+        print(f"❌ 程序运行失败：{e}")
+        sys.exit(1)
 
 
 if __name__ == "__main__":
-    # 安装依赖
-    # pip install cvxpy scipy ecos osqp mujoco
-    run_enhanced_simulation()
\ No newline at end of file
+    main()
\ No newline at end of file

From 4aba4f2abe0e2e166564657179c20e8dd352530d Mon Sep 17 00:00:00 2001
From: XYTX0427 <481177497@qq.com>
Date: Sat, 27 Dec 2025 11:15:00 +0800
Subject: [PATCH 18/21] =?UTF-8?q?=E6=8E=A7=E5=88=B6=E6=9C=BA=E6=A2=B0?=
 =?UTF-8?q?=E8=87=82=E5=85=B3=E8=8A=82=E5=BA=A6?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/Robotic_arm/main.py | 1118 ++++++++++++---------------------------
 1 file changed, 348 insertions(+), 770 deletions(-)

diff --git a/src/Robotic_arm/main.py b/src/Robotic_arm/main.py
index 238d01c137..d6c12a5dac 100644
--- a/src/Robotic_arm/main.py
+++ b/src/Robotic_arm/main.py
@@ -1,489 +1,163 @@
 #!/usr/bin/env python3
 # -*- coding: utf-8 -*-
 """
-机械臂仿真完整单文件版本（最终完美修复版）
-核心修复：
-1. 统一关节数为5个（匹配XML模型）
-2. 所有数组维度改为5维
-3. 移除所有硬编码的6关节逻辑
-4. 确保所有运算维度匹配
+机械臂关节控制器 - 最终兼容版
+核心优化：
+1. 修复Mujoco mj_name2id API类型错误（兼容所有版本）
+2. 移除所有不兼容属性和依赖
+3. 纯原生实现，无Numba/特殊依赖
+4. Windows深度适配+优雅退出
 """
 
 import sys
 import os
 import time
-import logging
-import argparse
-from dataclasses import dataclass, field
-from typing import List, Tuple, Optional, Dict, Deque
-from collections import deque
-
-# ====================== 核心配置：统一关节数 ======================
-JOINT_COUNT = 5  # 关键：改为5个关节（匹配XML模型）
-
-# ====================== mujoco 版本兼容处理 ======================
-try:
-    import numpy as np
-    import mujoco
-
+import signal
+import ctypes
+import threading
+import numpy as np
+import mujoco
+
+# ====================== 全局配置 ======================
+# 系统适配（Windows优先）
+if os.name == 'nt':
     try:
-        from mujoco import viewer
-
-        MUJOCO_VIEWER_MODE = "new"
-    except ImportError:
-        if hasattr(mujoco, 'viewer'):
-            viewer = mujoco.viewer
-            MUJOCO_VIEWER_MODE = "old"
-        else:
-            raise ImportError("请安装最新版mujoco：pip install mujoco>=2.3.0")
-
-    from scipy import interpolate
-    from scipy.signal import filtfilt, butter
-    import cvxpy as cp
-except ImportError as e:
-    print(f"❌ 缺少依赖库：{e.name}")
-    print("🔧 请运行：pip install mujoco>=2.3.0 numpy scipy cvxpy ecos osqp")
-    sys.exit(1)
-
-
-# ====================== 1. 配置管理模块（改为5关节） ======================
-@dataclass
-class PhysicsConfig:
-    # 改为5个关节的限制参数
-    max_vel: List[float] = field(default_factory=lambda: [1.0, 0.8, 0.8, 1.2, 0.9])
-    max_acc: List[float] = field(default_factory=lambda: [0.5, 0.4, 0.4, 0.6, 0.5])
-    max_jerk: List[float] = field(default_factory=lambda: [0.3, 0.2, 0.2, 0.4, 0.3])
-    max_torque: List[float] = field(default_factory=lambda: [15.0, 15.0, 10.0, 5.0, 5.0])
-    ctrl_limit: Tuple[float, float] = (-10.0, 10.0)
-
-
-@dataclass
-class ObstacleConfig:
-    base_k_att: float = 0.8
-    base_k_rep: float = 0.6
-    rep_radius: float = 0.3
-    stagnant_threshold: float = 0.01
-    stagnant_time: float = 1.0
-    guide_offset: float = 0.1
-    obstacle_list: List[List[float]] = field(
-        default_factory=lambda: [[0.6, 0.1, 0.5, 0.1], [0.55, 0.05, 0.55, 0.08], [0.4, -0.1, 0.6, 0.08]])
-    safety_margin: float = 0.05
-
-
-@dataclass
-class EfficiencyConfig:
-    time_weight: float = 0.6
-    energy_weight: float = 0.4
-    traj_interp_points: int = 20
-    opt_horizon: float = 1.0
-    smooth_factor: float = 0.2
-    motor_efficiency: float = 0.85
-    # 改为5个关节的摩擦系数
-    joint_friction: List[float] = field(default_factory=lambda: [0.001, 0.002, 0.0015, 0.001, 0.0008])
-
-
-@dataclass
-class TrajectoryConfig:
-    cart_waypoints: List[List[float]] = field(
-        default_factory=lambda: [[0.5, 0.0, 0.6], [0.6, 0.0, 0.58], [0.8, 0.1, 0.8], [0.6, 0.0, 0.58], [0.5, 0.0, 0.6]])
-
-
-@dataclass
-class SimulationConfig:
-    timestep: float = 0.005
-    fps: int = 60
-    log_level: str = "INFO"
-    enable_interaction: bool = False
-
-
-@dataclass
-class RobotConfig:
-    physics: PhysicsConfig = field(default_factory=PhysicsConfig)
-    obstacle: ObstacleConfig = field(default_factory=ObstacleConfig)
-    efficiency: EfficiencyConfig = field(default_factory=EfficiencyConfig)
-    trajectory: TrajectoryConfig = field(default_factory=TrajectoryConfig)
-    simulation: SimulationConfig = field(default_factory=SimulationConfig)
-
-    def validate(self):
-        """校验并自动修复配置参数"""
-        logger = logging.getLogger(__name__)
-        if self.simulation.fps < 1 or self.simulation.fps > 120:
-            logger.warning(f"⚠️ FPS {self.simulation.fps} 超出范围，自动调整为30")
-            self.simulation.fps = 30
-        if self.efficiency.traj_interp_points < 5 or self.efficiency.traj_interp_points > 100:
-            logger.warning(f"⚠️ 插值点数 {self.efficiency.traj_interp_points} 超出范围，自动调整为20")
-            self.efficiency.traj_interp_points = 20
-        weight_sum = self.efficiency.time_weight + self.efficiency.energy_weight
-        if not abs(weight_sum - 1.0) < 1e-6:
-            logger.warning(f"⚠️ 时间+能耗权重和为 {weight_sum}（应为1），自动归一化")
-            self.efficiency.time_weight /= weight_sum
-            self.efficiency.energy_weight /= weight_sum
-
-
-# 全局配置实例
-_global_config: Optional[RobotConfig] = None
-
-
-def get_config() -> RobotConfig:
-    """获取全局配置（单例+参数校验）"""
-    global _global_config
-    if _global_config is None:
-        _global_config = RobotConfig()
-
-        # 应用命令行参数
-        parser = argparse.ArgumentParser(description="机械臂仿真配置", add_help=False)
-        parser.add_argument("--fps", type=int, help="仿真帧率（1-120）")
-        parser.add_argument("--traj-points", type=int, dest="traj_interp_points", help="轨迹插值点数（5-100）")
-        parser.add_argument("--smooth-factor", type=float, help="轨迹平滑系数（0.01-1.0）")
-        parser.add_argument("--time-weight", type=float, help="时间权重（0-1）")
-        parser.add_argument("--energy-weight", type=float, help="能耗权重（0-1）")
-        parser.add_argument("--log-level", choices=["DEBUG", "INFO", "WARNING", "ERROR"], help="日志级别")
-        parser.add_argument("-h", "--help", action="store_true", help="显示帮助信息")
-
-        args, _ = parser.parse_known_args()
-
-        # 应用参数到配置
-        if args.fps:
-            _global_config.simulation.fps = args.fps
-        if args.traj_interp_points:
-            _global_config.efficiency.traj_interp_points = args.traj_interp_points
-        if args.smooth_factor:
-            _global_config.efficiency.smooth_factor = args.smooth_factor
-        if args.time_weight:
-            _global_config.efficiency.time_weight = args.time_weight
-        if args.energy_weight:
-            _global_config.efficiency.energy_weight = args.energy_weight
-        if args.log_level:
-            _global_config.simulation.log_level = args.log_level
-
-        # 校验配置
-        _global_config.validate()
-
-        # 显示帮助信息
-        if args.help:
-            print("""
-🤖 机械臂仿真使用帮助：
-命令行参数：
-  --fps N           设置仿真帧率（1-120），默认60
-  --traj-points N   设置轨迹插值点数（5-100），默认20
-  --smooth-factor F 设置轨迹平滑系数（0.01-1.0），默认0.2
-  --time-weight F   设置时间权重（0-1），默认0.6
-  --energy-weight F 设置能耗权重（0-1），默认0.4
-  --log-level LEVEL 设置日志级别（DEBUG/INFO/WARNING/ERROR），默认INFO
-  -h/--help         显示此帮助信息
-            """)
-            sys.exit(0)
-
-    return _global_config
-
-
-# ====================== 2. 性能优化缓存 ======================
-_TRAJ_CACHE = {
-    "butter_coeffs": {},
-    "joint_limits": None
-}
-
-_COLLISION_CACHE = {
-    "link_ids": {},
-    "obstacle_grid": None,
-    "obstacle_array": None,
-    "safety_margin": None
-}
-
-_ENERGY_CACHE = {
-    "friction": None,
-    "motor_eff": None
+        kernel32 = ctypes.windll.kernel32
+        kernel32.SetConsoleMode(kernel32.GetStdHandle(-11), 7)
+        os.system('chcp 65001 >nul 2>&1')
+        kernel32.SetThreadPriority(kernel32.GetCurrentThread(), 1)
+    except:
+        pass
+    os.environ['OMP_NUM_THREADS'] = '1'
+    os.environ['MKL_NUM_THREADS'] = '1'
+
+# Mujoco Viewer兼容
+try:
+    from mujoco import viewer
+
+    MUJOCO_NEW_VIEWER = True
+except ImportError:
+    import mujoco.viewer as viewer
+
+    MUJOCO_NEW_VIEWER = False
+
+# 核心参数配置
+JOINT_COUNT = 5
+JOINT_NAMES = ["joint1", "joint2", "joint3", "joint4", "joint5"]
+JOINT_LIMITS = np.array([
+    [-np.pi, np.pi],  # joint1 (Z轴)
+    [-np.pi / 2, np.pi / 2],  # joint2 (Y轴)
+    [-np.pi / 2, np.pi / 2],  # joint3 (Y轴)
+    [-np.pi / 2, np.pi / 2],  # joint4 (Y轴)
+    [-np.pi / 2, np.pi / 2],  # joint5 (Y轴)
+], dtype=np.float64)
+JOINT_MAX_VELOCITY = np.array([1.0, 0.8, 0.8, 0.6, 0.6], dtype=np.float64)
+
+# 仿真参数
+SIMULATION_TIMESTEP = 0.005
+CONTROL_FREQUENCY = 200
+CONTROL_TIMESTEP = 1.0 / CONTROL_FREQUENCY
+FPS = 60
+SLEEP_TIME = 1.0 / FPS
+EPS = 1e-8
+RUNNING = True
+
+# PD控制参数
+KP = 80.0
+KD = 5.0
+
+
+# ====================== 信号处理（优雅退出） ======================
+def signal_handler(sig, frame):
+    global RUNNING
+    print("\n⚠️ 收到退出信号，正在优雅退出...")
+    RUNNING = False
+
+
+signal.signal(signal.SIGINT, signal_handler)
+
+# ====================== 预分配内存 ======================
+WORK_ARRAYS = {
+    'current_angles': np.zeros(JOINT_COUNT, dtype=np.float64),
+    'target_angles': np.zeros(JOINT_COUNT, dtype=np.float64),
+    'joint_velocities': np.zeros(JOINT_COUNT, dtype=np.float64),
+    'control_signals': np.zeros(JOINT_COUNT, dtype=np.float64),
+    'ee_position': np.zeros(3, dtype=np.float64),
+    'angle_error': np.zeros(JOINT_COUNT, dtype=np.float64),
+    'desired_vel': np.zeros(JOINT_COUNT, dtype=np.float64)
 }
 
 
-def init_global_caches():
-    """初始化全局缓存（只执行一次）"""
-    config = get_config()
-
-    if _TRAJ_CACHE["joint_limits"] is None:
-        # 改为5个关节的限制参数
-        _TRAJ_CACHE["joint_limits"] = {
-            "max_vel": np.array(config.physics.max_vel, dtype=np.float64),
-            "max_acc": np.array(config.physics.max_acc, dtype=np.float64),
-            "max_torque": np.array(config.physics.max_torque, dtype=np.float64)
+# ====================== 兼容型Mujoco ID查询函数 ======================
+def get_mujoco_id(model, obj_type, name):
+    """
+    兼容所有Mujoco版本的ID查询函数
+    :param model: MjModel对象
+    :param obj_type: 对象类型（字符串或枚举）
+    :param name: 对象名称
+    :return: 对象ID
+    """
+    # 处理类型转换（关键修复）
+    if isinstance(obj_type, str):
+        # 字符串类型映射
+        type_map = {
+            'joint': mujoco.mjtObj.mjOBJ_JOINT,
+            'actuator': mujoco.mjtObj.mjOBJ_ACTUATOR,
+            'site': mujoco.mjtObj.mjOBJ_SITE
         }
+        obj_type_int = type_map.get(obj_type, mujoco.mjtObj.mjOBJ_JOINT)
+    else:
+        # 枚举类型转为整数（核心修复）
+        obj_type_int = int(obj_type)
 
-    if not _COLLISION_CACHE["link_ids"]:
-        _COLLISION_CACHE["obstacle_array"] = np.array(config.obstacle.obstacle_list, dtype=np.float64)
-        _COLLISION_CACHE["safety_margin"] = config.obstacle.safety_margin
-        obs_pos = _COLLISION_CACHE["obstacle_array"][:, :3]
-        min_coords = np.min(obs_pos, axis=0) - 0.5
-        max_coords = np.max(obs_pos, axis=0) + 0.5
-        _COLLISION_CACHE["obstacle_grid"] = (min_coords, max_coords)
-
-    if _ENERGY_CACHE["friction"] is None:
-        _ENERGY_CACHE["friction"] = np.array(config.efficiency.joint_friction, dtype=np.float64)
-        _ENERGY_CACHE["motor_eff"] = config.efficiency.motor_efficiency
-
-
-# ====================== 3. 核心算法模块（改为5关节） ======================
-def smooth_cartesian_traj(traj_points: List[List[float]], smooth_factor: float = None) -> List[List[float]]:
-    """笛卡尔轨迹平滑"""
-    config = get_config()
-    smooth_factor = smooth_factor or config.efficiency.smooth_factor
-    traj_array = np.asarray(traj_points, dtype=np.float64)
-
-    if traj_array.size == 0 or len(traj_array) <= 1:
-        return traj_points
-
-    key = round(smooth_factor, 3)
-    if key not in _TRAJ_CACHE["butter_coeffs"]:
-        b, a = butter(1, smooth_factor, btype="low")
-        _TRAJ_CACHE["butter_coeffs"][key] = (b.astype(np.float64), a.astype(np.float64))
-    b, a = _TRAJ_CACHE["butter_coeffs"][key]
-
-    k = min(3, len(traj_array) - 1)
-    t = np.linspace(0, 1, len(traj_array), dtype=np.float64)
-    t_smooth = np.linspace(0, 1, max(10, len(traj_array) * 2), dtype=np.float64)
-
+    # 兼容不同版本的mj_name2id调用方式
     try:
-        spline = interpolate.make_interp_spline(t, traj_array, k=k, axis=0)
-        smooth_vals = spline(t_smooth)
-        smooth_vals = filtfilt(b, a, smooth_vals, axis=0)
-
-        smoothed_traj = np.empty_like(traj_array)
-        for dim in range(3):
-            smoothed_traj[:, dim] = np.interp(t, t_smooth, smooth_vals[:, dim])
-        return smoothed_traj.tolist()
-    except Exception:
-        return traj_points
-
-
-def time_optimal_joint_trajectory(
-        start_joint: np.ndarray,
-        end_joint: np.ndarray,
-        seg_time: float
-) -> Tuple[np.ndarray, np.ndarray, np.ndarray]:
-    """时间最优关节轨迹（改为5关节）"""
-    limits = _TRAJ_CACHE["joint_limits"]
-    max_vel = limits["max_vel"]
-    max_acc = limits["max_acc"]
-
-    config = get_config()
-    traj_points = config.efficiency.traj_interp_points
-
-    t_steps = np.linspace(0, seg_time, traj_points, dtype=np.float64)
-    # 改为5列（5个关节）
-    opt_pos = np.empty((traj_points, JOINT_COUNT), dtype=np.float64)
-    opt_vel = np.empty_like(opt_pos)
-    opt_acc = np.empty_like(opt_pos)
-
-    delta = end_joint - start_joint
-    delta_abs = np.abs(delta)
-    sign = np.sign(delta)
-
-    t_acc = max_vel / max_acc
-    s_acc = 0.5 * max_acc * t_acc ** 2
-    t_joint = np.where(
-        delta_abs < 2 * s_acc,
-        2 * np.sqrt(delta_abs / max_acc),
-        2 * t_acc + (delta_abs - 2 * s_acc) / max_vel
-    )
-
-    # 遍历5个关节
-    for i, t in enumerate(t_steps):
-        for j in range(JOINT_COUNT):
-            if delta_abs[j] < 2 * s_acc[j]:
-                if t <= t_joint[j] / 2:
-                    opt_pos[i, j] = start_joint[j] + 0.5 * max_acc[j] * t ** 2 * sign[j]
-                    opt_vel[i, j] = max_acc[j] * t * sign[j]
-                    opt_acc[i, j] = max_acc[j] * sign[j]
-                else:
-                    t_rem = t_joint[j] - t
-                    opt_pos[i, j] = end_joint[j] - 0.5 * max_acc[j] * t_rem ** 2 * sign[j]
-                    opt_vel[i, j] = max_acc[j] * t_rem * sign[j]
-                    opt_acc[i, j] = -max_acc[j] * sign[j]
-            else:
-                if t <= t_acc[j]:
-                    opt_pos[i, j] = start_joint[j] + 0.5 * max_acc[j] * t ** 2 * sign[j]
-                    opt_vel[i, j] = max_acc[j] * t * sign[j]
-                    opt_acc[i, j] = max_acc[j] * sign[j]
-                elif t <= t_acc[j] + (delta_abs[j] - 2 * s_acc[j]) / max_vel[j]:
-                    opt_pos[i, j] = start_joint[j] + (s_acc[j] + max_vel[j] * (t - t_acc[j])) * sign[j]
-                    opt_vel[i, j] = max_vel[j] * sign[j]
-                    opt_acc[i, j] = 0.0
-                else:
-                    t_rem = t_joint[j] - t
-                    opt_pos[i, j] = end_joint[j] - 0.5 * max_acc[j] * t_rem ** 2 * sign[j]
-                    opt_vel[i, j] = max_acc[j] * t_rem * sign[j]
-                    opt_acc[i, j] = -max_acc[j] * sign[j]
-
-        opt_vel[i] = np.clip(opt_vel[i], -max_vel, max_vel)
-        opt_acc[i] = np.clip(opt_acc[i], -max_acc, max_acc)
-
-    return opt_pos, opt_vel, opt_acc
-
-
-def full_arm_collision_check(
-        model,
-        data,
-        return_min_dist: bool = True
-) -> Tuple[bool, float] | bool:
-    """全链路碰撞检测"""
-    if not _COLLISION_CACHE["link_ids"]:
-        # 5个关节对应的连杆
-        link_names = ["link1", "link2", "link3", "link4", "link5", "end_effector"]
-        for name in link_names:
-            _COLLISION_CACHE["link_ids"][name] = mujoco.mj_name2id(
-                model, mujoco.mjtObj.mjOBJ_BODY, name
-            )
-
-    collision = False
-    min_dist = float("inf")
-    obstacle_array = _COLLISION_CACHE["obstacle_array"]
-    safety_margin = _COLLISION_CACHE["safety_margin"]
-    grid_min, grid_max = _COLLISION_CACHE["obstacle_grid"]
-
-    for link_name, link_id in _COLLISION_CACHE["link_ids"].items():
-        try:
-            link_pos = data.xpos[link_id].astype(np.float64)
-
-            if np.any(link_pos < grid_min) or np.any(link_pos > grid_max):
-                continue
-
-            obs_pos = obstacle_array[:, :3]
-            obs_radius = obstacle_array[:, 3]
-            distances = np.linalg.norm(link_pos - obs_pos, axis=1) - (obs_radius + safety_margin)
-
-            if np.any(distances < 0):
-                collision = True
-                if not return_min_dist:
-                    return True
-
-            if return_min_dist:
-                min_dist = min(min_dist, np.min(distances))
-        except Exception:
-            continue
-
-    if return_min_dist:
-        return collision, min_dist
-    return collision
-
-
-def calculate_real_energy_consumption(model, data, dt: float) -> float:
-    """真实能耗计算（改为5关节）"""
-    friction = _ENERGY_CACHE["friction"]
-    motor_eff = _ENERGY_CACHE["motor_eff"]
-
-    # 只取前5个关节的数据
-    torques = data.qfrc_actuator[:JOINT_COUNT].astype(np.float64)
-    velocities = data.qvel[:JOINT_COUNT].astype(np.float64)
-
-    friction_loss = np.sum(friction * np.abs(velocities))
-    mechanical_power = np.sum(np.abs(torques * velocities))
-    total_energy = (mechanical_power + friction_loss) * dt / motor_eff
-
-    return float(total_energy)
-
+        # 新版本调用方式
+        return mujoco.mj_name2id(model, obj_type_int, name)
+    except:
+        # 旧版本兼容
+        return mujoco.mj_name2id(model, obj_type, name)
 
-# ====================== 4. 可视化模块 ======================
-def draw_enhanced_visualization(
-        viewer_inst,
-        model,
-        data,
-        traj_history: Deque[list],
-        collision_warning: bool
-):
-    """增强可视化"""
-    try:
-        scene = viewer_inst.user_scn
-        scene.ngeom = 0
-
-        if len(traj_history) > 1:
-            traj_array = np.array(traj_history, dtype=np.float64)
-
-            for i in range(len(traj_array) - 1):
-                geom = mujoco.MjvGeom()
-                mujoco.mjv_initGeom(
-                    geom,
-                    mujoco.mjtGeom.mjGEOM_LINE,
-                    np.array([0.003, 0, 0], dtype=np.float64),
-                    traj_array[i],
-                    traj_array[i + 1],
-                    np.array([0, 1, 0, 0.6], dtype=np.float64)
-                )
-                mujoco.mjv_addGeom(scene, model, data, geom)
-
-            def draw_sphere(pos, rgba, size):
-                geom = mujoco.MjvGeom()
-                mujoco.mjv_initGeom(
-                    geom,
-                    mujoco.mjtGeom.mjGEOM_SPHERE,
-                    np.array([size, 0, 0], dtype=np.float64),
-                    pos,
-                    np.array([0, 0, 0], dtype=np.float64),
-                    np.array(rgba, dtype=np.float64)
-                )
-                mujoco.mjv_addGeom(scene, model, data, geom)
-
-            draw_sphere(traj_array[0], [0, 0, 1, 0.8], 0.015)
-            draw_sphere(traj_array[-1], [1, 0, 0, 0.8], 0.015)
-
-        if collision_warning:
-            ee_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "ee_site")
-            ee_pos = data.site_xpos[ee_id]
-            draw_sphere(ee_pos, [1, 0, 0, 0.3], 0.08)
-
-    except Exception as e:
-        logging.warning(f"可视化绘制失败：{e}")
-
-
-# ====================== 5. 机械臂模型构建（5关节） ======================
-def get_arm_xml_with_obstacles(config: RobotConfig) -> str:
-    """生成机械臂XML模型（5关节）"""
-    obstacles_xml = ""
-    for i, obs in enumerate(config.obstacle.obstacle_list):
-        x, y, z, r = obs
-        obstacles_xml += f"""
-    <body name="obstacle_{i}" pos="{x} {y} {z}">
-        <geom name="obs_geom_{i}" type="sphere" size="{r}" rgba="1 0 0 0.5"/>
-    </body>
-        """
 
+# ====================== 机械臂模型生成 ======================
+def create_arm_model():
+    """生成极简兼容版XML模型"""
     xml = f"""
-<mujoco model="robotic_arm">
+<mujoco model="controllable_arm">
     <compiler angle="radian" inertiafromgeom="true"/>
-    <option timestep="{config.simulation.timestep}" gravity="0 0 -9.81"/>
+    <option timestep="{SIMULATION_TIMESTEP}" gravity="0 0 -9.81"/>
+
+    <default>
+        <joint type="hinge" armature="0.1" damping="0.1"/>
+        <motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="50"/>
+    </default>
 
     <worldbody>
-        <!-- 地面 -->
         <geom name="floor" type="plane" size="5 5 0.1" pos="0 0 0" rgba="0.8 0.8 0.8 1"/>
 
-        <!-- 机械臂基座 -->
         <body name="base" pos="0 0 0">
             <geom name="base_geom" type="cylinder" size="0.1 0.1" rgba="0.2 0.2 0.8 1"/>
 
-            <!-- 关节1 -->
+            <joint name="joint1" type="hinge" axis="0 0 1" pos="0 0 0.1"/>
             <body name="link1" pos="0 0 0.1">
-                <joint name="joint1" type="hinge" axis="0 0 1" range="-3.14 3.14"/>
                 <geom name="link1_geom" type="cylinder" size="0.05 0.2" rgba="0.2 0.8 0.2 1"/>
 
-                <!-- 关节2 -->
+                <joint name="joint2" type="hinge" axis="0 1 0" pos="0 0 0.2"/>
                 <body name="link2" pos="0 0 0.2">
-                    <joint name="joint2" type="hinge" axis="0 1 0" range="-3.14 3.14"/>
                     <geom name="link2_geom" type="cylinder" size="0.05 0.2" rgba="0.2 0.8 0.2 1"/>
 
-                    <!-- 关节3 -->
+                    <joint name="joint3" type="hinge" axis="0 1 0" pos="0 0 0.2"/>
                     <body name="link3" pos="0 0 0.2">
-                        <joint name="joint3" type="hinge" axis="0 1 0" range="-3.14 3.14"/>
                         <geom name="link3_geom" type="cylinder" size="0.05 0.2" rgba="0.2 0.8 0.2 1"/>
 
-                        <!-- 关节4 -->
+                        <joint name="joint4" type="hinge" axis="0 1 0" pos="0 0 0.2"/>
                         <body name="link4" pos="0 0 0.2">
-                            <joint name="joint4" type="hinge" axis="0 1 0" range="-3.14 3.14"/>
                             <geom name="link4_geom" type="cylinder" size="0.05 0.2" rgba="0.2 0.8 0.2 1"/>
 
-                            <!-- 关节5 -->
+                            <joint name="joint5" type="hinge" axis="0 1 0" pos="0 0 0.2"/>
                             <body name="link5" pos="0 0 0.2">
-                                <joint name="joint5" type="hinge" axis="0 1 0" range="-3.14 3.14"/>
-                                <geom name="link5_geom" type="cylinder" size="0.05 0.1" rgba="0.2 0.8 0.2 1"/>
+                                <geom name="link5_geom" type="cylinder" size="0.05 0.1" rgba="0.8 0.2 0.2 1"/>
 
-                                <!-- 末端执行器 -->
                                 <body name="end_effector" pos="0 0 0.1">
                                     <site name="ee_site" pos="0 0 0" size="0.01"/>
                                     <geom name="ee_geom" type="sphere" size="0.05" rgba="0.8 0.2 0.2 1"/>
@@ -494,12 +168,8 @@ def get_arm_xml_with_obstacles(config: RobotConfig) -> str:
                 </body>
             </body>
         </body>
-
-        <!-- 障碍物 -->
-        {obstacles_xml}
     </worldbody>
 
-    <!-- 控制器（5个电机） -->
     <actuator>
         <motor name="motor1" joint="joint1" ctrlrange="-1 1" gear="100"/>
         <motor name="motor2" joint="joint2" ctrlrange="-1 1" gear="100"/>
@@ -512,340 +182,248 @@ def get_arm_xml_with_obstacles(config: RobotConfig) -> str:
     return xml
 
 
-# ====================== 6. 仿真器主类（5关节） ======================
-class ArmSimulator:
+# ====================== 核心控制器类 ======================
+class ArmJointController:
     def __init__(self):
-        self.config = get_config()
-        init_global_caches()
-
-        # 配置日志
-        self._setup_logging()
-
-        # 初始化仿真环境
-        self._init_simulation()
-
-        # 状态管理
-        self.total_motion_time = 0.0
-        self.total_energy_consume = 0.0
-        self.traj_history: Deque[list] = deque(maxlen=50)
-        self.collision_warning = False
-        self.stagnant_start_time: Optional[float] = None
-
-        # 预计算关节起点
-        self.ee_site_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_SITE, "ee_site")
-        self.joint_waypoints = self._precompute_joint_waypoints()
-
-        self.logger.info("✅ 机械臂仿真器初始化完成")
-        self.logger.info(f"🔧 使用mujoco viewer模式：{MUJOCO_VIEWER_MODE}")
-        self.logger.info(f"🔧 机械臂关节数：{JOINT_COUNT}")
-
-    def _setup_logging(self):
-        """配置日志系统"""
-        log_level = getattr(logging, self.config.simulation.log_level.upper())
-        logging.basicConfig(
-            level=log_level,
-            format="%(asctime)s - %(name)s - %(levelname)s - %(message)s",
-            handlers=[logging.StreamHandler()]
-        )
-        self.logger = logging.getLogger("ArmSimulator")
-
-    def _init_simulation(self):
-        """初始化仿真环境"""
-        arm_xml = get_arm_xml_with_obstacles(self.config)
-
-        # 创建临时文件
-        import tempfile
-        self.temp_file = tempfile.NamedTemporaryFile(mode='w', suffix='.xml', delete=False)
-        self.temp_file.write(arm_xml)
-        self.temp_file.close()
-
-        # 加载模型
-        self.model = mujoco.MjModel.from_xml_path(self.temp_file.name)
-        self.model.opt.timestep = self.config.simulation.timestep
+        # 初始化模型和数据
+        self.model = mujoco.MjModel.from_xml_string(create_arm_model())
         self.data = mujoco.MjData(self.model)
 
-    def _precompute_joint_waypoints(self) -> list:
-        """预计算关节起点（5关节）"""
-        joint_waypoints = []
-        for cart_pos in self.config.trajectory.cart_waypoints:
-            mujoco.mj_resetData(self.model, self.data)
-            self.data.site_xpos[self.ee_site_id] = cart_pos
-            mujoco.mj_inverse(self.model, self.data)
-            # 只取前5个关节
-            joint_waypoints.append(self.data.qpos[:JOINT_COUNT].copy())
-        return joint_waypoints
-
-    def _get_ee_cartesian_velocity(self) -> np.ndarray:
-        """获取末端笛卡尔速度"""
-        jacp = np.zeros((3, self.model.nv), dtype=np.float64)
-        jacr = np.zeros((3, self.model.nv), dtype=np.float64)
-
-        mujoco.mj_jacSite(self.model, self.data, jacp, jacr, self.ee_site_id)
-        ee_vel = jacp @ self.data.qvel
-        return ee_vel
-
-    def _check_local_optimum(self, ee_vel: np.ndarray, ee_pos: list, target_pos: list) -> tuple:
-        """检测局部最优"""
-        vel_mag = np.linalg.norm(ee_vel)
-        if vel_mag < self.config.obstacle.stagnant_threshold:
-            if self.stagnant_start_time is None:
-                self.stagnant_start_time = time.time()
-            elif time.time() - self.stagnant_start_time > self.config.obstacle.stagnant_time:
-                self.logger.warning(f"检测到局部最优！末端速度={vel_mag:.4f}m/s")
-                dir_to_target = np.array(target_pos) - np.array(ee_pos, dtype=np.float64)
-                dir_norm = np.linalg.norm(dir_to_target)
-                if dir_norm < 1e-6:
-                    dir_to_target = np.array([0, 0, 0.1], dtype=np.float64)
-                else:
-                    dir_to_target = dir_to_target / dir_norm
-
-                guide_target = np.array(ee_pos, dtype=np.float64) + dir_to_target * self.config.obstacle.guide_offset
-                self.stagnant_start_time = None
-                return True, guide_target.tolist()
-        else:
-            self.stagnant_start_time = None
-        return False, target_pos
-
-    def _robust_artificial_potential_field(self, ee_pos: list, target_pos: list) -> list:
-        """人工势场法避障"""
-        ee_pos = np.array(ee_pos, dtype=np.float64)
-        target_pos = np.array(target_pos, dtype=np.float64)
-
-        ee_vel = self._get_ee_cartesian_velocity()
-        is_local_opt, guide_target = self._check_local_optimum(ee_vel, ee_pos.tolist(), target_pos.tolist())
-        current_target = np.array(guide_target, dtype=np.float64) if is_local_opt else target_pos
-
-        # 自适应参数
-        obs_distances = [np.linalg.norm(ee_pos - np.array(obs[:3], dtype=np.float64))
-                         for obs in self.config.obstacle.obstacle_list]
-        min_dist = min(obs_distances) if obs_distances else 1.0
-        k_rep = self.config.obstacle.base_k_rep if min_dist > 0.2 else self.config.obstacle.base_k_rep * 2.0
-        k_att = self.config.obstacle.base_k_att if len(
-            self.config.obstacle.obstacle_list) <= 2 else self.config.obstacle.base_k_att * 0.5
-
-        # 引力+斥力
-        att_force = k_att * (current_target - ee_pos)
-        rep_force = np.zeros(3, dtype=np.float64)
-
-        for obs in self.config.obstacle.obstacle_list:
-            obs_pos = np.array(obs[:3], dtype=np.float64)
-            obs_radius = obs[3]
-            dist = np.linalg.norm(ee_pos - obs_pos)
-
-            if dist < self.config.obstacle.rep_radius + obs_radius:
-                rep_dir = (ee_pos - obs_pos) / (dist + 1e-6)
-                rep_force += k_rep * (1 / (dist - obs_radius) - 1 / self.config.obstacle.rep_radius) * (
-                            1 / dist ** 2) * rep_dir
-
-        corrected_target = ee_pos + att_force + rep_force
-        corrected_target = np.clip(corrected_target, [0.3, -0.4, 0.2], [0.9, 0.4, 1.0])
-        return corrected_target.tolist()
-
-    def _energy_optimal_trajectory(self, joint_waypoints: np.ndarray, seg_time: float) -> Optional[np.ndarray]:
-        """能耗最优轨迹（5关节）"""
-        n_joints = JOINT_COUNT
-        n_points = len(joint_waypoints)
-        t_step = seg_time / (n_points - 1)
-
-        q = cp.Variable((n_joints, n_points))
-        qd = cp.Variable((n_joints, n_points))
-        qdd = cp.Variable((n_joints, n_points))
-
-        energy_cost = cp.sum_squares(qdd)
-        time_cost = cp.sum(cp.max(cp.abs(qd), axis=1))
-        total_cost = self.config.efficiency.time_weight * time_cost + self.config.efficiency.energy_weight * energy_cost
-
-        constraints = [
-            q[:, 0] == joint_waypoints[0],
-            q[:, -1] == joint_waypoints[-1],
-            qd[:, 0] == 0,
-            qd[:, -1] == 0
-        ]
-
-        max_vel = self.config.physics.max_vel
-        max_acc = self.config.physics.max_acc
-        for j in range(n_joints):
-            constraints.extend([
-                qd[j, :] <= max_vel[j],
-                qd[j, :] >= -max_vel[j],
-                qdd[j, :] <= max_acc[j],
-                qdd[j, :] >= -max_acc[j]
-            ])
-
-        for i in range(n_points - 1):
-            constraints.extend([
-                qd[:, i + 1] == (q[:, i + 1] - q[:, i]) / t_step,
-                qdd[:, i + 1] == (qd[:, i + 1] - qd[:, i]) / t_step
-            ])
-
-        prob = cp.Problem(cp.Minimize(total_cost), constraints)
+        # 获取ID（使用兼容型函数，核心修复）
+        self.joint_ids = []
+        for name in JOINT_NAMES:
+            # 关键修复：使用字符串类型+整数转换
+            jid = get_mujoco_id(self.model, 'joint', name)
+            self.joint_ids.append(jid)
+
+        self.motor_ids = []
+        for i in range(JOINT_COUNT):
+            mid = get_mujoco_id(self.model, 'actuator', f"motor{i + 1}")
+            self.motor_ids.append(mid)
+
+        self.ee_site_id = get_mujoco_id(self.model, 'site', "ee_site")
+
+        # 状态变量
+        self.viewer_inst = None
+        self.viewer_ready = False
+        self.last_control_time = time.time()
+        self.last_print_time = time.time()
+        self.fps_counter = 0
+
+        # 初始化目标角度为零位
+        self.set_joint_angles(np.zeros(JOINT_COUNT), smooth=False)
+
+    def get_current_joint_angles(self):
+        """获取当前关节角度"""
+        for i, jid in enumerate(self.joint_ids):
+            if jid >= 0:  # 安全检查
+                WORK_ARRAYS['current_angles'][i] = self.data.qpos[jid]
+        return WORK_ARRAYS['current_angles'].copy()
+
+    def get_joint_velocities(self):
+        """获取关节速度"""
+        for i, jid in enumerate(self.joint_ids):
+            if jid >= 0:
+                WORK_ARRAYS['joint_velocities'][i] = self.data.qvel[jid]
+        return WORK_ARRAYS['joint_velocities'].copy()
+
+    def get_ee_position(self):
+        """获取末端位置"""
+        if self.ee_site_id >= 0:
+            WORK_ARRAYS['ee_position'][:] = self.data.site_xpos[self.ee_site_id]
+        return WORK_ARRAYS['ee_position'].copy()
+
+    def clamp_joint_angles(self, angles):
+        """关节限位保护"""
+        return np.clip(angles, JOINT_LIMITS[:, 0], JOINT_LIMITS[:, 1])
+
+    def set_joint_angles(self, target_angles, smooth=True):
+        """设置关节目标角度"""
+        if len(target_angles) != JOINT_COUNT:
+            raise ValueError(f"目标角度数量必须为{JOINT_COUNT}")
+
+        # 限位保护
+        target_angles = np.array(target_angles, dtype=np.float64)
+        WORK_ARRAYS['target_angles'][:] = self.clamp_joint_angles(target_angles)
+
+        # 立即设置（无平滑）
+        if not smooth:
+            for i, jid in enumerate(self.joint_ids):
+                if jid >= 0:
+                    self.data.qpos[jid] = WORK_ARRAYS['target_angles'][i]
+                    self.data.qvel[jid] = 0.0
+            mujoco.mj_forward(self.model, self.data)
+
+    def move_joint(self, joint_idx, angle, smooth=True):
+        """单独控制单个关节"""
+        if joint_idx < 0 or joint_idx >= JOINT_COUNT:
+            raise ValueError(f"关节索引必须在0-{JOINT_COUNT - 1}之间")
+
+        current_angles = self.get_current_joint_angles()
+        current_angles[joint_idx] = angle
+        self.set_joint_angles(current_angles, smooth)
+
+    def pd_control_loop(self):
+        """PD控制核心逻辑"""
+        # 获取当前状态
+        current_angles = self.get_current_joint_angles()
+        current_vels = self.get_joint_velocities()
+
+        # 计算角度误差
+        WORK_ARRAYS['angle_error'][:] = WORK_ARRAYS['target_angles'] - current_angles
+
+        # 计算期望速度（带速度限制）
+        WORK_ARRAYS['desired_vel'][:] = np.clip(WORK_ARRAYS['angle_error'] * KP, -JOINT_MAX_VELOCITY,
+                                                JOINT_MAX_VELOCITY)
+
+        # PD控制计算
+        WORK_ARRAYS['control_signals'][:] = KP * WORK_ARRAYS['angle_error'] + KD * (
+                    WORK_ARRAYS['desired_vel'] - current_vels)
+
+        # 设置控制信号到电机
+        for i, mid in enumerate(self.motor_ids):
+            if mid >= 0:
+                self.data.ctrl[mid] = WORK_ARRAYS['control_signals'][i]
+
+    def init_viewer(self):
+        """初始化Viewer"""
         try:
-            prob.solve(solver=cp.ECOS, verbose=False, warm_start=True)
-        except:
-            try:
-                prob.solve(solver=cp.OSQP, verbose=False, warm_start=True)
-            except:
-                prob.solve(verbose=False)
-
-        if prob.status != cp.OPTIMAL:
-            self.logger.warning("能耗优化求解失败，降级为时间最优轨迹")
-            return None
-
-        return q.value.T
-
-    def _optimize_obstacle_traj_with_efficiency(self, ee_pos: list, target_pos: list) -> tuple:
-        """轨迹优化主逻辑"""
-        # 避障修正
-        corrected_cart_target = self._robust_artificial_potential_field(ee_pos, target_pos)
-
-        # 平滑轨迹
-        corrected_cart_target = smooth_cartesian_traj([ee_pos, corrected_cart_target])[-1]
-
-        # 逆解
-        self.data.site_xpos[self.ee_site_id] = corrected_cart_target
-        mujoco.mj_inverse(self.model, self.data)
-        # 只取前5个关节
-        end_joint = self.data.qpos[:JOINT_COUNT].copy()
-        start_joint = self.data.qpos[:JOINT_COUNT].copy()
-
-        # 时间最优轨迹
-        seg_time = 2.0
-        time_opt_pos, _, _ = time_optimal_joint_trajectory(start_joint, end_joint, seg_time)
-
-        # 能耗最优
-        energy_opt_pos = self._energy_optimal_trajectory(time_opt_pos, seg_time)
-        final_joint_traj = energy_opt_pos if energy_opt_pos is not None else time_opt_pos
-
-        # 能耗计算
-        dt = seg_time / len(final_joint_traj)
-        seg_energy = sum([calculate_real_energy_consumption(self.model, self.data, dt)
-                          for _ in range(1, len(final_joint_traj))])
-
-        # 更新状态
-        self.total_motion_time += seg_time
-        self.total_energy_consume += seg_energy
-        self.traj_history.append(corrected_cart_target)
-
-        return final_joint_traj[0], corrected_cart_target, seg_energy
-
-    def _run_simulation_loop(self, viewer_inst):
-        """通用仿真循环"""
-        self.logger.info("🎮 机械臂仿真启动！")
-
-        config = self.config
-        fps = config.simulation.fps
-        sleep_time = 1.0 / fps
-        print_interval = 2.0
-        waypoints = np.array(config.trajectory.cart_waypoints, dtype=np.float64)
-        n_waypoints = len(waypoints)
-
-        current_waypoint = 0
-        last_print_time = 0.0
-        last_step_time = time.time()
-
-        while viewer_inst.is_running():
-            # 固定步长控制
-            current_time = time.time()
-            if current_time - last_step_time < sleep_time:
-                continue
-            last_step_time = current_time
+            if MUJOCO_NEW_VIEWER:
+                self.viewer_inst = viewer.launch_passive(self.model, self.data)
+            else:
+                self.viewer_inst = viewer.Viewer(self.model, self.data)
+            self.viewer_ready = True
+            return True
+        except Exception as e:
+            print(f"❌ Viewer初始化失败: {e}")
+            return False
+
+    def print_status(self):
+        """打印实时状态"""
+        current_time = time.time()
+        if current_time - self.last_print_time >= 1.0:
+            angles = self.get_current_joint_angles()
+            ee_pos = self.get_ee_position()
+            fps = self.fps_counter / (current_time - self.last_print_time)
 
-            # 获取当前状态
-            t_total = self.data.time
-            ee_pos = self.data.site_xpos[self.ee_site_id].tolist()
+            print(f"\n📊 实时状态 | FPS: {fps:5.1f}")
+            print(f"🔧 关节角度 (弧度): {np.round(angles, 3)}")
+            print(f"🎯 末端位置 (m): {np.round(ee_pos, 3)}")
 
-            # 切换目标点
-            target_cart = waypoints[current_waypoint].tolist()
-            if np.linalg.norm(np.array(ee_pos, dtype=np.float64) - np.array(target_cart, dtype=np.float64)) < 0.01:
-                current_waypoint = (current_waypoint + 1) % n_waypoints
-                self.logger.info(f"🔄 切换到目标点 {current_waypoint}: {np.round(target_cart, 3)}")
+            self.last_print_time = current_time
+            self.fps_counter = 0
 
+    def run(self):
+        """运行完整仿真"""
+        global RUNNING
+
+        # 初始化Viewer
+        if not self.init_viewer():
+            RUNNING = False
+            return
+
+        # 启动信息
+        print("=" * 60)
+        print("🚀 机械臂关节控制器 - 最终兼容版")
+        print(f"✅ MJ_NAME2ID API错误已修复")
+        print(f"✅ 全Mujoco版本兼容")
+        print(f"💻 Windows优化已启用")
+        print("📝 控制指令:")
+        print("   - 单关节控制: controller.move_joint(0, np.pi/4)")
+        print("   - 多关节控制: controller.set_joint_angles([0, π/4, π/6, 0, 0])")
+        print("   - 按 Ctrl+C 退出")
+        print("=" * 60)
+
+        # 主循环
+        while RUNNING:
             try:
-                # 轨迹优化
-                target_joints, corrected_cart, _ = self._optimize_obstacle_traj_with_efficiency(ee_pos, target_cart)
-                target_joints = np.array(target_joints, dtype=np.float64)
-
-                # 碰撞检测
-                is_collision, min_obs_dist = full_arm_collision_check(self.model, self.data)
-                self.collision_warning = is_collision
-
-                # 紧急避障
-                if is_collision:
-                    self.logger.warning("🆘 检测到碰撞风险，执行紧急避障！")
-                    emergency_rep = np.array(ee_pos, dtype=np.float64) - np.array(config.obstacle.obstacle_list[0][:3],
-                                                                                  dtype=np.float64)
-                    emergency_rep = emergency_rep / np.linalg.norm(emergency_rep) * 0.05
-                    corrected_cart = np.array(corrected_cart, dtype=np.float64) + emergency_rep
-                    self.data.site_xpos[self.ee_site_id] = corrected_cart
-                    mujoco.mj_inverse(self.model, self.data)
-                    target_joints = self.data.qpos[:JOINT_COUNT].copy()
-
-                # PD控制（5关节）
-                max_torque = np.array(config.physics.max_torque, dtype=np.float64) / 100.0
-                pos_error = target_joints - self.data.qpos[:JOINT_COUNT]
-                vel_error = -self.data.qvel[:JOINT_COUNT]
-                ctrl_signals = 8.0 * pos_error + 0.2 * vel_error
-                ctrl_signals = np.clip(ctrl_signals, -max_torque, max_torque)
-                self.data.ctrl[:JOINT_COUNT] = ctrl_signals
-
-                # 打印统计信息
-                if t_total - last_print_time > print_interval and t_total > 0:
-                    ee_vel = self._get_ee_cartesian_velocity()
-                    avg_vel = np.linalg.norm(ee_vel)
-                    avg_energy = self.total_energy_consume / t_total if t_total > 0 else 0.0
-
-                    self.logger.info(
-                        f"\n⏱️ 仿真时间：{t_total:.2f}s | 累计运动时间：{self.total_motion_time:.2f}s\n"
-                        f"   末端位置：{np.round(ee_pos, 3)} | 目标位置：{np.round(corrected_cart, 3)}\n"
-                        f"   末端速度：{avg_vel:.4f}m/s | 最近障碍距离：{min_obs_dist:.3f}m\n"
-                        f"   累计能耗：{self.total_energy_consume:.2f}J | 平均能耗：{avg_energy:.2f}J/s\n"
-                        f"   碰撞风险：{'⚠️ 高' if is_collision else '✅ 低'}"
-                    )
-                    last_print_time = t_total
-
-                # 可视化
-                draw_enhanced_visualization(viewer_inst, self.model, self.data,
-                                            self.traj_history, self.collision_warning)
+                current_time = time.time()
+                self.fps_counter += 1
+
+                # 控制频率执行PD控制
+                if current_time - self.last_control_time >= CONTROL_TIMESTEP:
+                    self.pd_control_loop()
+                    self.last_control_time = current_time
+
+                # 执行仿真步
+                mujoco.mj_step(self.model, self.data)
+
+                # 同步Viewer
+                if self.viewer_ready:
+                    self.viewer_inst.sync()
+
+                # 打印状态
+                self.print_status()
+
+                # Windows睡眠优化
+                time_diff = current_time - self.last_control_time
+                if time_diff < SLEEP_TIME:
+                    time.sleep(max(0.00001, SLEEP_TIME - time_diff))
 
             except Exception as e:
-                self.logger.error(f"仿真步执行失败：{e}", exc_info=False)
+                print(f"⚠️ 仿真步异常: {e}")
                 continue
 
-            # 执行仿真步
-            mujoco.mj_step(self.model, self.data)
-            viewer_inst.sync()
+        # 清理资源
+        self.cleanup()
+        print("\n✅ 控制器已优雅退出")
 
-    def run(self):
-        """运行仿真主循环"""
-        try:
-            with viewer.launch_passive(self.model, self.data) as viewer_inst:
-                self._run_simulation_loop(viewer_inst)
+    def cleanup(self):
+        """资源清理"""
+        if self.viewer_ready and self.viewer_inst:
+            try:
+                self.viewer_inst.close()
+            except:
+                pass
+        for arr in WORK_ARRAYS.values():
+            arr.fill(0)
 
-        except KeyboardInterrupt:
-            self.logger.info("\n🛑 用户终止仿真")
-        except Exception as e:
-            self.logger.error(f"❌ 仿真出错：{e}", exc_info=True)
-        finally:
-            # 清理资源
-            if hasattr(self, 'temp_file'):
-                os.unlink(self.temp_file.name)
-            self.logger.info(f"\n📊 仿真结束 - 最终统计")
-            self.logger.info(f"   总运动时间：{self.total_motion_time:.2f}s")
-            self.logger.info(f"   总能耗：{self.total_energy_consume:.2f}J")
-            self.logger.info(
-                f"   综合得分：{self.total_motion_time * self.config.efficiency.time_weight + self.total_energy_consume * self.config.efficiency.energy_weight:.2f}")
-
-
-# ====================== 7. 主入口 ======================
-def main():
-    """程序主入口"""
-    try:
-        simulator = ArmSimulator()
-        simulator.run()
-    except Exception as e:
-        print(f"❌ 程序运行失败：{e}")
-        sys.exit(1)
 
+# ====================== 演示函数 ======================
+def demo_movements(controller):
+    """预设演示动作"""
+
+    def demo():
+        time.sleep(2)
+
+        print("\n🎬 演示1：所有关节归位")
+        controller.set_joint_angles([0, 0, 0, 0, 0])
+        time.sleep(3)
+
+        print("\n🎬 演示2：关节1旋转45度")
+        controller.move_joint(0, np.pi / 4)
+        time.sleep(2)
+
+        print("\n🎬 演示3：关节2抬起30度")
+        controller.move_joint(1, np.pi / 6)
+        time.sleep(2)
 
+        print("\n🎬 演示4：组合关节运动")
+        controller.set_joint_angles([np.pi / 4, np.pi / 6, np.pi / 8, np.pi / 10, np.pi / 12])
+        time.sleep(3)
+
+        print("\n🎬 演示5：回到零位")
+        controller.set_joint_angles([0, 0, 0, 0, 0])
+        time.sleep(2)
+
+        global RUNNING
+        RUNNING = False
+
+    demo_thread = threading.Thread(target=demo)
+    demo_thread.daemon = True
+    demo_thread.start()
+
+
+# ====================== 主入口 ======================
 if __name__ == "__main__":
-    main()
\ No newline at end of file
+    # 禁用NumPy警告
+    np.seterr(all='ignore')
+
+    # 创建控制器（现在可正常初始化）
+    controller = ArmJointController()
+
+    # 运行预设演示
+    demo_movements(controller)
+
+    # 启动控制器
+    controller.run()
\ No newline at end of file

From 23242d019ae928214fca1902f1c4f86d171d6b73 Mon Sep 17 00:00:00 2001
From: XYTX0427 <481177497@qq.com>
Date: Sat, 27 Dec 2025 15:26:33 +0800
Subject: [PATCH 19/21] =?UTF-8?q?=E6=9C=BA=E6=A2=B0=E8=87=82=E5=85=B3?=
 =?UTF-8?q?=E8=8A=82=E8=B4=9F=E8=BD=BD=E6=8F=90=E5=8D=87?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/Robotic_arm/main.py | 701 +++++++++++++++++++++++++++-------------
 1 file changed, 485 insertions(+), 216 deletions(-)

diff --git a/src/Robotic_arm/main.py b/src/Robotic_arm/main.py
index d6c12a5dac..27a685095c 100644
--- a/src/Robotic_arm/main.py
+++ b/src/Robotic_arm/main.py
@@ -1,12 +1,15 @@
 #!/usr/bin/env python3
 # -*- coding: utf-8 -*-
 """
-机械臂关节控制器 - 最终兼容版
+机械臂关节负载性能优化版（终极全XML错误修复）
 核心优化：
-1. 修复Mujoco mj_name2id API类型错误（兼容所有版本）
-2. 移除所有不兼容属性和依赖
-3. 纯原生实现，无Numba/特殊依赖
-4. Windows深度适配+优雅退出
+1.  彻底修复所有XML Schema错误（body/mass/forcelimit 均移除违规配置）
+2.  精准关节负载建模（末端负载/关节负载可配置）
+3.  负载自适应PD控制（抗干扰、无超调、响应快）
+4.  物理仿真优化（减少负载下的计算冗余，提升实时性）
+5.  负载状态监控（实时显示负载大小、关节受力、控制误差）
+6.  软件层面过载保护（替代forcelimit，兼容所有Mujoco版本）
+7.  全Mujoco版本兼容（支持新旧版本，无任何语法隐患）
 """
 
 import sys
@@ -18,149 +21,222 @@
 import numpy as np
 import mujoco
 
-# ====================== 全局配置 ======================
-# 系统适配（Windows优先）
+# ====================== 全局配置（负载优化专用） ======================
+# 系统适配（Windows优先，极致CPU优化）
 if os.name == 'nt':
     try:
         kernel32 = ctypes.windll.kernel32
         kernel32.SetConsoleMode(kernel32.GetStdHandle(-11), 7)
         os.system('chcp 65001 >nul 2>&1')
         kernel32.SetThreadPriority(kernel32.GetCurrentThread(), 1)
-    except:
-        pass
+    except Exception as e:
+        print(f"⚠️ Windows系统优化失败（不影响核心功能）: {e}")
+    # 强制单线程，避免负载下多线程竞争导致卡顿
     os.environ['OMP_NUM_THREADS'] = '1'
     os.environ['MKL_NUM_THREADS'] = '1'
+    os.environ['NUMEXPR_NUM_THREADS'] = '1'
 
 # Mujoco Viewer兼容
+MUJOCO_NEW_VIEWER = False
 try:
     from mujoco import viewer
 
     MUJOCO_NEW_VIEWER = True
 except ImportError:
-    import mujoco.viewer as viewer
-
-    MUJOCO_NEW_VIEWER = False
+    try:
+        import mujoco.viewer as viewer
+    except ImportError as e:
+        print(f"⚠️ Mujoco Viewer导入失败（无法可视化）: {e}")
 
 # 核心参数配置
+# 关节基础配置
 JOINT_COUNT = 5
 JOINT_NAMES = ["joint1", "joint2", "joint3", "joint4", "joint5"]
-JOINT_LIMITS = np.array([
+JOINT_LIMITS_RAD = np.array([
     [-np.pi, np.pi],  # joint1 (Z轴)
     [-np.pi / 2, np.pi / 2],  # joint2 (Y轴)
     [-np.pi / 2, np.pi / 2],  # joint3 (Y轴)
     [-np.pi / 2, np.pi / 2],  # joint4 (Y轴)
     [-np.pi / 2, np.pi / 2],  # joint5 (Y轴)
 ], dtype=np.float64)
-JOINT_MAX_VELOCITY = np.array([1.0, 0.8, 0.8, 0.6, 0.6], dtype=np.float64)
+JOINT_MAX_VELOCITY_RAD = np.array([1.0, 0.8, 0.8, 0.6, 0.6], dtype=np.float64)
+JOINT_MAX_TORQUE = np.array([15.0, 12.0, 10.0, 8.0, 5.0], dtype=np.float64)  # 关节最大扭矩（软件过载保护）
 
-# 仿真参数
-SIMULATION_TIMESTEP = 0.005
-CONTROL_FREQUENCY = 200
+# 仿真配置（负载下实时性优化）
+SIMULATION_TIMESTEP = 0.002  # 更小步长，提升负载下控制精度
+CONTROL_FREQUENCY = 500  # 更高控制频率，应对负载下响应滞后
 CONTROL_TIMESTEP = 1.0 / CONTROL_FREQUENCY
 FPS = 60
 SLEEP_TIME = 1.0 / FPS
 EPS = 1e-8
 RUNNING = True
+SIMULATION_START_TIME = None
+
+# PD控制参数（负载自适应配置）
+PD_PARAMS = {
+    'kp_base': 80.0,
+    'kd_base': 5.0,
+    'kp_load_gain': 1.5,  # 负载下比例增益放大系数
+    'kd_load_gain': 1.2,  # 负载下微分增益放大系数
+    'max_vel': JOINT_MAX_VELOCITY_RAD.copy()
+}
 
-# PD控制参数
-KP = 80.0
-KD = 5.0
+# 负载配置（可动态调整）
+LOAD_PARAMS = {
+    'end_effector_mass': 0.5,  # 末端负载质量（kg），默认0.5kg
+    'joint_loads': np.zeros(JOINT_COUNT),  # 各关节附加负载（N·m）
+    'max_allowed_load': 2.0,  # 最大允许末端负载（过载保护）
+    'load_smoothing_factor': 0.1  # 负载检测平滑系数，避免抖动
+}
 
 
-# ====================== 信号处理（优雅退出） ======================
+# ====================== 信号处理（负载下优雅退出） ======================
 def signal_handler(sig, frame):
     global RUNNING
-    print("\n⚠️ 收到退出信号，正在优雅退出...")
+    if not RUNNING:
+        sys.exit(0)
+    print("\n⚠️ 收到退出信号，正在优雅退出（清理负载相关资源）...")
     RUNNING = False
 
 
 signal.signal(signal.SIGINT, signal_handler)
-
-# ====================== 预分配内存 ======================
-WORK_ARRAYS = {
-    'current_angles': np.zeros(JOINT_COUNT, dtype=np.float64),
-    'target_angles': np.zeros(JOINT_COUNT, dtype=np.float64),
-    'joint_velocities': np.zeros(JOINT_COUNT, dtype=np.float64),
-    'control_signals': np.zeros(JOINT_COUNT, dtype=np.float64),
-    'ee_position': np.zeros(3, dtype=np.float64),
-    'angle_error': np.zeros(JOINT_COUNT, dtype=np.float64),
-    'desired_vel': np.zeros(JOINT_COUNT, dtype=np.float64)
-}
+signal.signal(signal.SIGTERM, signal_handler)
 
 
-# ====================== 兼容型Mujoco ID查询函数 ======================
+# ====================== 工具函数（负载优化专用） ======================
 def get_mujoco_id(model, obj_type, name):
+    """兼容所有Mujoco版本的ID查询函数（容错增强）"""
+    if model is None:
+        return -1
+    type_map = {
+        'joint': mujoco.mjtObj.mjOBJ_JOINT,
+        'actuator': mujoco.mjtObj.mjOBJ_ACTUATOR,
+        'site': mujoco.mjtObj.mjOBJ_SITE,
+        'body': mujoco.mjtObj.mjOBJ_BODY,
+        'geom': mujoco.mjtObj.mjOBJ_GEOM
+    }
+    obj_type_int = type_map.get(obj_type, mujoco.mjtObj.mjOBJ_JOINT)
+    try:
+        obj_id = mujoco.mj_name2id(model, int(obj_type_int), str(name))
+        return obj_id if obj_id >= 0 else -1
+    except Exception as e:
+        print(f"⚠️ 查询{obj_type} {name} ID失败: {e}")
+        return -1
+
+
+def deg2rad(degrees):
+    """角度值（度）转弧度（容错增强）"""
+    try:
+        degrees = np.array(degrees, dtype=np.float64)
+        return np.deg2rad(degrees)
+    except Exception as e:
+        print(f"⚠️ 角度转换失败: {e}")
+        return 0.0 if np.isscalar(degrees) else np.zeros(JOINT_COUNT, dtype=np.float64)
+
+
+def rad2deg(radians):
+    """弧度转角度值（度）（容错增强）"""
+    try:
+        radians = np.array(radians, dtype=np.float64)
+        return np.rad2deg(radians)
+    except Exception as e:
+        print(f"⚠️ 弧度转换失败: {e}")
+        return 0.0 if np.isscalar(radians) else np.zeros(JOINT_COUNT, dtype=np.float64)
+
+
+def calculate_load_adaptive_gains(current_load):
     """
-    兼容所有Mujoco版本的ID查询函数
-    :param model: MjModel对象
-    :param obj_type: 对象类型（字符串或枚举）
-    :param name: 对象名称
-    :return: 对象ID
+    负载自适应增益计算（核心优化）
+    根据当前末端负载，动态调整PD增益，抵消负载干扰
+    :param current_load: 当前末端负载（kg）
+    :return: 自适应kp, kd
     """
-    # 处理类型转换（关键修复）
-    if isinstance(obj_type, str):
-        # 字符串类型映射
-        type_map = {
-            'joint': mujoco.mjtObj.mjOBJ_JOINT,
-            'actuator': mujoco.mjtObj.mjOBJ_ACTUATOR,
-            'site': mujoco.mjtObj.mjOBJ_SITE
-        }
-        obj_type_int = type_map.get(obj_type, mujoco.mjtObj.mjOBJ_JOINT)
-    else:
-        # 枚举类型转为整数（核心修复）
-        obj_type_int = int(obj_type)
+    # 负载归一化（0 ~ 1）
+    normalized_load = min(current_load / LOAD_PARAMS['max_allowed_load'], 1.0)
+    # 动态调整增益（负载越大，增益越高，保证响应性）
+    adaptive_kp = PD_PARAMS['kp_base'] * (1 + normalized_load * (PD_PARAMS['kp_load_gain'] - 1))
+    adaptive_kd = PD_PARAMS['kd_base'] * (1 + normalized_load * (PD_PARAMS['kd_load_gain'] - 1))
+    return adaptive_kp, adaptive_kd
 
-    # 兼容不同版本的mj_name2id调用方式
-    try:
-        # 新版本调用方式
-        return mujoco.mj_name2id(model, obj_type_int, name)
-    except:
-        # 旧版本兼容
-        return mujoco.mj_name2id(model, obj_type, name)
 
+# ====================== 机械臂模型生成（终极全XML错误修复） ======================
+def create_arm_model_with_load():
+    """
+    生成带负载建模的机械臂XML模型（终极全修复+全版本兼容）
+    1.  彻底修复所有XML Schema错误：
+        - 移除body的mass属性（改用geom定义质量）
+        - 移除motor的forcelimit属性（改用软件层面过载保护）
+    2.  末端负载通过geom的mass属性配置，保留可动态调整功能
+    3.  关节添加阻尼和惯量，模拟真实负载特性
+    4.  简化非核心几何，提升负载下仿真速度
+    5.  依赖compiler的inertiafromgeom="true"，自动由geom质量推导body惯性属性
+    6.  无任何违规配置，兼容所有Mujoco版本
+    """
+    end_effector_mass = LOAD_PARAMS['end_effector_mass']
+    # 连杆geom质量（对应原body质量，通过geom定义，兼容所有Mujoco版本）
+    link1_geom_mass = 0.8
+    link2_geom_mass = 0.6
+    link3_geom_mass = 0.6
+    link4_geom_mass = 0.4
+    link5_geom_mass = 0.2
 
-# ====================== 机械臂模型生成 ======================
-def create_arm_model():
-    """生成极简兼容版XML模型"""
     xml = f"""
-<mujoco model="controllable_arm">
-    <compiler angle="radian" inertiafromgeom="true"/>
-    <option timestep="{SIMULATION_TIMESTEP}" gravity="0 0 -9.81"/>
+<mujoco model="arm_with_load">
+    <compiler angle="radian" inertiafromgeom="true" autolimits="true"/>
+    <option timestep="{SIMULATION_TIMESTEP}" gravity="0 0 -9.81" iterations="30" tolerance="1e-6"/>
 
+    <!-- 仅保留Mujoco基础支持的元素，无任何版本专属配置（全版本兼容） -->
     <default>
-        <joint type="hinge" armature="0.1" damping="0.1"/>
-        <motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="50"/>
+        <joint type="hinge" armature="0.2" damping="0.2" limited="true" margin="0.01"/>
+        <motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="80"/>
+        <geom contype="1" conaffinity="1" rgba="0.2 0.8 0.2 1"/>
     </default>
 
+    <!-- 负载相关配置：末端负载可视化材质 -->
+    <asset>
+        <material name="load_material" rgba="1.0 0.0 0.0 0.8"/> <!-- 红色标记负载 -->
+    </asset>
+
     <worldbody>
-        <geom name="floor" type="plane" size="5 5 0.1" pos="0 0 0" rgba="0.8 0.8 0.8 1"/>
+        <!-- 地面（简化尺寸，减少渲染开销） -->
+        <geom name="floor" type="plane" size="3 3 0.1" pos="0 0 0" rgba="0.8 0.8 0.8 1"/>
 
+        <!-- 机械臂基座（无mass属性，符合Schema规范） -->
         <body name="base" pos="0 0 0">
             <geom name="base_geom" type="cylinder" size="0.1 0.1" rgba="0.2 0.2 0.8 1"/>
 
-            <joint name="joint1" type="hinge" axis="0 0 1" pos="0 0 0.1"/>
+            <!-- 关节1 -->
+            <joint name="joint1" type="hinge" axis="0 0 1" pos="0 0 0.1" range="{JOINT_LIMITS_RAD[0, 0]} {JOINT_LIMITS_RAD[0, 1]}"/>
+            <!-- 连杆1：无body mass，质量通过geom定义（兼容所有Mujoco版本） -->
             <body name="link1" pos="0 0 0.1">
-                <geom name="link1_geom" type="cylinder" size="0.05 0.2" rgba="0.2 0.8 0.2 1"/>
-
-                <joint name="joint2" type="hinge" axis="0 1 0" pos="0 0 0.2"/>
-                <body name="link2" pos="0 0 0.2">
-                    <geom name="link2_geom" type="cylinder" size="0.05 0.2" rgba="0.2 0.8 0.2 1"/>
-
-                    <joint name="joint3" type="hinge" axis="0 1 0" pos="0 0 0.2"/>
-                    <body name="link3" pos="0 0 0.2">
-                        <geom name="link3_geom" type="cylinder" size="0.05 0.2" rgba="0.2 0.8 0.2 1"/>
-
-                        <joint name="joint4" type="hinge" axis="0 1 0" pos="0 0 0.2"/>
-                        <body name="link4" pos="0 0 0.2">
-                            <geom name="link4_geom" type="cylinder" size="0.05 0.2" rgba="0.2 0.8 0.2 1"/>
-
-                            <joint name="joint5" type="hinge" axis="0 1 0" pos="0 0 0.2"/>
-                            <body name="link5" pos="0 0 0.2">
-                                <geom name="link5_geom" type="cylinder" size="0.05 0.1" rgba="0.8 0.2 0.2 1"/>
-
-                                <body name="end_effector" pos="0 0 0.1">
+                <geom name="link1_geom" type="cylinder" size="0.04 0.18" mass="{link1_geom_mass}"/>
+
+                <!-- 关节2 -->
+                <joint name="joint2" type="hinge" axis="0 1 0" pos="0 0 0.18" range="{JOINT_LIMITS_RAD[1, 0]} {JOINT_LIMITS_RAD[1, 1]}"/>
+                <body name="link2" pos="0 0 0.18">
+                    <geom name="link2_geom" type="cylinder" size="0.04 0.18" mass="{link2_geom_mass}"/>
+
+                    <!-- 关节3 -->
+                    <joint name="joint3" type="hinge" axis="0 1 0" pos="0 0 0.18" range="{JOINT_LIMITS_RAD[2, 0]} {JOINT_LIMITS_RAD[2, 1]}"/>
+                    <body name="link3" pos="0 0 0.18">
+                        <geom name="link3_geom" type="cylinder" size="0.04 0.18" mass="{link3_geom_mass}"/>
+
+                        <!-- 关节4 -->
+                        <joint name="joint4" type="hinge" axis="0 1 0" pos="0 0 0.18" range="{JOINT_LIMITS_RAD[3, 0]} {JOINT_LIMITS_RAD[3, 1]}"/>
+                        <body name="link4" pos="0 0 0.18">
+                            <geom name="link4_geom" type="cylinder" size="0.04 0.18" mass="{link4_geom_mass}"/>
+
+                            <!-- 关节5 -->
+                            <joint name="joint5" type="hinge" axis="0 1 0" pos="0 0 0.18" range="{JOINT_LIMITS_RAD[4, 0]} {JOINT_LIMITS_RAD[4, 1]}"/>
+                            <body name="link5" pos="0 0 0.18">
+                                <geom name="link5_geom" type="cylinder" size="0.03 0.09" mass="{link5_geom_mass}" rgba="0.8 0.2 0.2 1"/>
+
+                                <!-- 末端执行器（负载通过geom mass定义，无body mass属性，兼容所有版本） -->
+                                <body name="end_effector" pos="0 0 0.09">
                                     <site name="ee_site" pos="0 0 0" size="0.01"/>
-                                    <geom name="ee_geom" type="sphere" size="0.05" rgba="0.8 0.2 0.2 1"/>
+                                    <!-- 末端负载：通过geom的mass属性配置，实现可动态调整 -->
+                                    <geom name="load_geom" type="sphere" size="0.04" mass="{end_effector_mass}" 
+                                          rgba="1.0 0.0 0.0 0.8" material="load_material"/>
                                 </body>
                             </body>
                         </body>
@@ -170,6 +246,7 @@ def create_arm_model():
         </body>
     </worldbody>
 
+    <!-- 关节电机（移除forcelimit属性，改用软件层面过载保护，兼容所有Mujoco版本） -->
     <actuator>
         <motor name="motor1" joint="joint1" ctrlrange="-1 1" gear="100"/>
         <motor name="motor2" joint="joint2" ctrlrange="-1 1" gear="100"/>
@@ -182,230 +259,414 @@ def create_arm_model():
     return xml
 
 
-# ====================== 核心控制器类 ======================
-class ArmJointController:
+# ====================== 核心控制器类（关节负载性能优化+软件过载保护） ======================
+class ArmJointLoadOptimizedController:
     def __init__(self):
-        # 初始化模型和数据
-        self.model = mujoco.MjModel.from_xml_string(create_arm_model())
-        self.data = mujoco.MjData(self.model)
-
-        # 获取ID（使用兼容型函数，核心修复）
-        self.joint_ids = []
-        for name in JOINT_NAMES:
-            # 关键修复：使用字符串类型+整数转换
-            jid = get_mujoco_id(self.model, 'joint', name)
-            self.joint_ids.append(jid)
-
-        self.motor_ids = []
-        for i in range(JOINT_COUNT):
-            mid = get_mujoco_id(self.model, 'actuator', f"motor{i + 1}")
-            self.motor_ids.append(mid)
+        # 初始化模型和数据（负载下容错增强）
+        self.model = None
+        self.data = None
+        try:
+            self.model = mujoco.MjModel.from_xml_string(create_arm_model_with_load())
+            self.data = mujoco.MjData(self.model)
+        except Exception as e:
+            print(f"❌ 带负载模型初始化失败: {e}")
+            global RUNNING
+            RUNNING = False
+            return
 
+        # 获取ID
+        self.joint_ids = [get_mujoco_id(self.model, 'joint', name) for name in JOINT_NAMES]
+        self.motor_ids = [get_mujoco_id(self.model, 'actuator', f"motor{i + 1}") for i in range(JOINT_COUNT)]
         self.ee_site_id = get_mujoco_id(self.model, 'site', "ee_site")
+        self.ee_body_id = get_mujoco_id(self.model, 'body', "end_effector")
+        self.load_geom_id = get_mujoco_id(self.model, 'geom', "load_geom")  # 负载geom ID，用于更新质量
 
-        # 状态变量
+        # 状态变量（负载监控专用）
         self.viewer_inst = None
         self.viewer_ready = False
         self.last_control_time = time.time()
         self.last_print_time = time.time()
         self.fps_counter = 0
+        self.step_count = 0
+        self.total_simulation_time = 0.0
 
-        # 初始化目标角度为零位
-        self.set_joint_angles(np.zeros(JOINT_COUNT), smooth=False)
-
-    def get_current_joint_angles(self):
-        """获取当前关节角度"""
-        for i, jid in enumerate(self.joint_ids):
-            if jid >= 0:  # 安全检查
-                WORK_ARRAYS['current_angles'][i] = self.data.qpos[jid]
-        return WORK_ARRAYS['current_angles'].copy()
+        # 负载相关状态
+        self.current_end_load = LOAD_PARAMS['end_effector_mass']
+        self.smoothed_joint_forces = np.zeros(JOINT_COUNT, dtype=np.float64)
+        self.angle_error_history = np.zeros(JOINT_COUNT, dtype=np.float64)  # 控制误差监控
+        self.overload_warning_flag = False
 
-    def get_joint_velocities(self):
-        """获取关节速度"""
+        # 初始化目标角度为零位（提前定义，避免属性不存在错误）
+        self.target_angles_rad = np.zeros(JOINT_COUNT, dtype=np.float64)
+        try:
+            self.set_joint_angles(np.zeros(JOINT_COUNT), smooth=False, use_deg=False)
+        except Exception as e:
+            print(f"⚠️ 初始化关节角度失败: {e}")
+
+        # 全局仿真开始时间
+        global SIMULATION_START_TIME
+        SIMULATION_START_TIME = time.time()
+
+    def get_current_joint_angles(self, use_deg=True):
+        """获取当前关节角度（负载下按需转换，减少冗余）"""
+        if self.data is None:
+            return np.zeros(JOINT_COUNT, dtype=np.float64)
+        current_rad = np.array([self.data.qpos[jid] if jid >= 0 else 0 for jid in self.joint_ids], dtype=np.float64)
+        if use_deg:
+            return rad2deg(current_rad)
+        return current_rad
+
+    def get_joint_forces(self):
+        """
+        获取关节实时受力（负载监控核心）
+        返回各关节扭矩（N·m），反映负载大小
+        """
+        if self.data is None:
+            return np.zeros(JOINT_COUNT, dtype=np.float64)
+        joint_forces = np.zeros(JOINT_COUNT, dtype=np.float64)
         for i, jid in enumerate(self.joint_ids):
             if jid >= 0:
-                WORK_ARRAYS['joint_velocities'][i] = self.data.qvel[jid]
-        return WORK_ARRAYS['joint_velocities'].copy()
+                # 读取关节受力，并平滑处理，避免抖动
+                raw_force = abs(self.data.qfrc_actuator[jid])
+                self.smoothed_joint_forces[i] = (1 - LOAD_PARAMS['load_smoothing_factor']) * self.smoothed_joint_forces[
+                    i] + \
+                                                LOAD_PARAMS['load_smoothing_factor'] * raw_force
+                joint_forces[i] = self.smoothed_joint_forces[i]
+        return joint_forces
 
     def get_ee_position(self):
-        """获取末端位置"""
-        if self.ee_site_id >= 0:
-            WORK_ARRAYS['ee_position'][:] = self.data.site_xpos[self.ee_site_id]
-        return WORK_ARRAYS['ee_position'].copy()
+        """获取末端位置（容错增强）"""
+        if self.data is None or self.ee_site_id < 0:
+            return np.zeros(3, dtype=np.float64)
+        return self.data.site_xpos[self.ee_site_id].copy()
+
+    def clamp_joint_angles(self, angles, use_deg=True):
+        """关节限位保护（负载下更严格的限位校验）"""
+        angles = np.array(angles, dtype=np.float64)
+        if use_deg:
+            angles_rad = deg2rad(angles)
+        else:
+            angles_rad = angles.copy()
+        # 负载下缩小限位余量，防止冲击
+        limit_margin = 0.05  # 5%余量
+        limits_rad_margin = JOINT_LIMITS_RAD.copy()
+        limits_rad_margin[:, 0] += limit_margin
+        limits_rad_margin[:, 1] -= limit_margin
+        clamped_rad = np.clip(angles_rad, limits_rad_margin[:, 0], limits_rad_margin[:, 1])
+        if use_deg:
+            return rad2deg(clamped_rad)
+        return clamped_rad
+
+    def set_end_effector_load(self, mass):
+        """
+        动态设置末端负载（核心功能，兼容所有Mujoco版本）
+        :param mass: 末端负载质量（kg），需≤max_allowed_load
+        """
+        if mass < 0 or mass > LOAD_PARAMS['max_allowed_load']:
+            self.overload_warning_flag = True
+            print(f"⚠️ 末端负载超出限制（0 ~ {LOAD_PARAMS['max_allowed_load']}kg），当前设置：{mass}kg")
+            return
+        self.overload_warning_flag = False
+
+        # 方案1：直接更新负载geom的质量（无需重新初始化模型，更高效）
+        if self.model is not None and self.load_geom_id >= 0:
+            try:
+                # 直接修改geom的mass属性，实时生效
+                self.model.geom_mass[self.load_geom_id] = mass
+                # 更新内部状态
+                self.current_end_load = mass
+                LOAD_PARAMS['end_effector_mass'] = mass
+                print(f"✅ 末端负载已更新为 {mass}kg（直接修改geom质量，无需重启模型）")
+                return
+            except Exception as e:
+                print(f"⚠️ 直接更新负载失败，将重新初始化模型: {e}")
 
-    def clamp_joint_angles(self, angles):
-        """关节限位保护"""
-        return np.clip(angles, JOINT_LIMITS[:, 0], JOINT_LIMITS[:, 1])
+        # 方案2：降级方案 - 重新初始化模型（兼容特殊场景）
+        try:
+            LOAD_PARAMS['end_effector_mass'] = mass
+            self.current_end_load = mass
+            # 重新初始化模型
+            self.model = mujoco.MjModel.from_xml_string(create_arm_model_with_load())
+            self.data = mujoco.MjData(self.model)
+            # 重新获取所有ID
+            self.joint_ids = [get_mujoco_id(self.model, 'joint', name) for name in JOINT_NAMES]
+            self.motor_ids = [get_mujoco_id(self.model, 'actuator', f"motor{i + 1}") for i in range(JOINT_COUNT)]
+            self.ee_site_id = get_mujoco_id(self.model, 'site', "ee_site")
+            self.ee_body_id = get_mujoco_id(self.model, 'body', "end_effector")
+            self.load_geom_id = get_mujoco_id(self.model, 'geom', "load_geom")
+            # 保留当前目标角度
+            current_target = self.target_angles_rad.copy()
+            self.target_angles_rad = current_target
+            self.set_joint_angles(current_target, smooth=False, use_deg=False)
+            print(f"✅ 末端负载已更新为 {mass}kg（重新初始化模型生效）")
+        except Exception as e:
+            print(f"❌ 更新末端负载失败: {e}")
 
-    def set_joint_angles(self, target_angles, smooth=True):
-        """设置关节目标角度"""
+    def set_joint_angles(self, target_angles, smooth=True, use_deg=True):
+        """设置关节目标角度（负载下参数校验增强）"""
+        if self.data is None:
+            raise Exception("模型未初始化，无法设置关节角度")
         if len(target_angles) != JOINT_COUNT:
-            raise ValueError(f"目标角度数量必须为{JOINT_COUNT}")
+            raise ValueError(f"目标角度数量必须为{JOINT_COUNT}，当前为{len(target_angles)}")
 
-        # 限位保护
-        target_angles = np.array(target_angles, dtype=np.float64)
-        WORK_ARRAYS['target_angles'][:] = self.clamp_joint_angles(target_angles)
+        target_angles_rad = self.clamp_joint_angles(target_angles, use_deg=use_deg)
 
-        # 立即设置（无平滑）
         if not smooth:
             for i, jid in enumerate(self.joint_ids):
                 if jid >= 0:
-                    self.data.qpos[jid] = WORK_ARRAYS['target_angles'][i]
+                    self.data.qpos[jid] = target_angles_rad[i]
                     self.data.qvel[jid] = 0.0
-            mujoco.mj_forward(self.model, self.data)
+            try:
+                mujoco.mj_forward(self.model, self.data)
+            except Exception as e:
+                print(f"⚠️ 更新模型状态失败: {e}")
 
-    def move_joint(self, joint_idx, angle, smooth=True):
-        """单独控制单个关节"""
+        # 平滑控制时，记录目标角度（供PD控制使用）
+        self.target_angles_rad = target_angles_rad.copy()
+
+    def move_joint(self, joint_idx, angle, smooth=True, use_deg=True):
+        """单独控制单个关节（负载下容错增强）"""
         if joint_idx < 0 or joint_idx >= JOINT_COUNT:
-            raise ValueError(f"关节索引必须在0-{JOINT_COUNT - 1}之间")
+            raise ValueError(f"关节索引必须在0-{JOINT_COUNT - 1}之间，当前为{joint_idx}")
 
-        current_angles = self.get_current_joint_angles()
+        current_angles = self.get_current_joint_angles(use_deg=use_deg)
         current_angles[joint_idx] = angle
-        self.set_joint_angles(current_angles, smooth)
+        self.set_joint_angles(current_angles, smooth=smooth, use_deg=use_deg)
+
+    def load_adaptive_pd_control(self):
+        """
+        负载自适应PD控制（核心性能优化）+ 软件层面过载保护（替代forcelimit）
+        1.  动态调整PD增益，抵消负载干扰
+        2.  软件扭矩限制，防止关节过载（兼容所有Mujoco版本）
+        3.  误差反馈平滑，减少负载下抖动
+        4.  过载时自动降低控制输出，保护关节
+        """
+        if self.data is None:
+            return
 
-    def pd_control_loop(self):
-        """PD控制核心逻辑"""
-        # 获取当前状态
-        current_angles = self.get_current_joint_angles()
-        current_vels = self.get_joint_velocities()
+        # 1. 获取当前状态
+        current_angles_rad = self.get_current_joint_angles(use_deg=False)
+        current_vels_rad = np.array([self.data.qvel[jid] if jid >= 0 else 0 for jid in self.joint_ids],
+                                    dtype=np.float64)
+        joint_forces = self.get_joint_forces()
 
-        # 计算角度误差
-        WORK_ARRAYS['angle_error'][:] = WORK_ARRAYS['target_angles'] - current_angles
+        # 2. 计算负载自适应PD增益
+        adaptive_kp, adaptive_kd = calculate_load_adaptive_gains(self.current_end_load)
 
-        # 计算期望速度（带速度限制）
-        WORK_ARRAYS['desired_vel'][:] = np.clip(WORK_ARRAYS['angle_error'] * KP, -JOINT_MAX_VELOCITY,
-                                                JOINT_MAX_VELOCITY)
+        # 3. 计算控制误差（平滑处理）
+        angle_error_rad = self.target_angles_rad - current_angles_rad
+        self.angle_error_history = (1 - LOAD_PARAMS['load_smoothing_factor']) * self.angle_error_history + \
+                                   LOAD_PARAMS['load_smoothing_factor'] * angle_error_rad
 
-        # PD控制计算
-        WORK_ARRAYS['control_signals'][:] = KP * WORK_ARRAYS['angle_error'] + KD * (
-                    WORK_ARRAYS['desired_vel'] - current_vels)
+        # 4. 计算期望速度（带速度限制）
+        desired_vel_rad = np.clip(self.angle_error_history * adaptive_kp, -PD_PARAMS['max_vel'], PD_PARAMS['max_vel'])
+
+        # 5. PD控制输出计算
+        control_signals = adaptive_kp * self.angle_error_history + adaptive_kd * (desired_vel_rad - current_vels_rad)
+
+        # 6. 软件层面过载保护（替代motor的forcelimit，兼容所有Mujoco版本）
+        for i in range(JOINT_COUNT):
+            # 判断关节是否接近过载（受力达到90%最大扭矩阈值）
+            if joint_forces[i] > JOINT_MAX_TORQUE[i] * 0.9:
+                control_signals[i] *= 0.5  # 降低50%控制输出，防止过载损坏
+                self.overload_warning_flag = True  # 置位过载警告标志
+            else:
+                # 过载解除后，清除警告标志
+                if self.overload_warning_flag:
+                    self.overload_warning_flag = False
 
-        # 设置控制信号到电机
+        # 7. 设置控制信号
         for i, mid in enumerate(self.motor_ids):
             if mid >= 0:
-                self.data.ctrl[mid] = WORK_ARRAYS['control_signals'][i]
+                self.data.ctrl[mid] = control_signals[i]
 
     def init_viewer(self):
-        """初始化Viewer"""
+        """初始化Viewer（负载下延迟加载，提升启动速度）"""
+        if self.model is None or self.data is None:
+            return False
+        if self.viewer_ready:
+            return True
         try:
             if MUJOCO_NEW_VIEWER:
                 self.viewer_inst = viewer.launch_passive(self.model, self.data)
             else:
                 self.viewer_inst = viewer.Viewer(self.model, self.data)
             self.viewer_ready = True
+            print("✅ Viewer初始化成功")
             return True
         except Exception as e:
             print(f"❌ Viewer初始化失败: {e}")
             return False
 
-    def print_status(self):
-        """打印实时状态"""
+    def print_load_status(self):
+        """打印负载相关状态（核心监控功能）"""
         current_time = time.time()
-        if current_time - self.last_print_time >= 1.0:
-            angles = self.get_current_joint_angles()
-            ee_pos = self.get_ee_position()
-            fps = self.fps_counter / (current_time - self.last_print_time)
+        if current_time - self.last_print_time < 1.0:
+            return
 
-            print(f"\n📊 实时状态 | FPS: {fps:5.1f}")
-            print(f"🔧 关节角度 (弧度): {np.round(angles, 3)}")
-            print(f"🎯 末端位置 (m): {np.round(ee_pos, 3)}")
+        # 计算统计信息
+        fps = self.fps_counter / (current_time - self.last_print_time)
+        joint_angles_deg = self.get_current_joint_angles(use_deg=True)
+        ee_pos = self.get_ee_position()
+        joint_forces = self.get_joint_forces()
+        angle_errors_deg = rad2deg(self.angle_error_history)
+        self.total_simulation_time = current_time - (SIMULATION_START_TIME or current_time)
+        adaptive_kp, adaptive_kd = calculate_load_adaptive_gains(self.current_end_load)
+
+        # 格式化打印（负载信息突出显示）
+        print("-" * 100)
+        print(f"📊 仿真统计 | 耗时: {self.total_simulation_time:.2f}s | 步数: {self.step_count:,} | FPS: {fps:5.1f}")
+        print(f"🔧 关节角度 (度): {np.round(joint_angles_deg, 1)} | 控制误差 (度): {np.round(abs(angle_errors_deg), 3)}")
+        print(f"🎯 末端位置 (m): {np.round(ee_pos, 3)} | 当前末端负载 (kg): {self.current_end_load:.2f}")
+        print(f"⚙️ 关节受力 (N·m): {np.round(joint_forces, 2)} | 最大扭矩 (N·m): {np.round(JOINT_MAX_TORQUE, 1)}")
+        print(f"📈 自适应PD参数 | kp: {adaptive_kp:.1f} | kd: {adaptive_kd:.1f}")
+        if self.overload_warning_flag:
+            print("⚠️  警告：关节接近过载状态，已自动降低控制输出！")
+        print("-" * 100)
+
+        # 重置计数器
+        self.last_print_time = current_time
+        self.fps_counter = 0
 
-            self.last_print_time = current_time
-            self.fps_counter = 0
+    def preset_pose(self, pose_name):
+        """预设常用姿态（负载下更平稳的姿态切换）"""
+        pose_map = {
+            'zero': [0, 0, 0, 0, 0],  # 零位
+            'up': [0, 30, 20, 10, 0],  # 抬起姿态
+            'grasp': [0, 45, 30, 20, 10]  # 抓取姿态
+        }
+        if pose_name not in pose_map:
+            print(f"⚠️ 无效姿态名称，支持：{list(pose_map.keys())}")
+            return
+        self.set_joint_angles(pose_map[pose_name], smooth=True, use_deg=True)
+        print(f"✅ 切换到{pose_name}姿态（负载自适应控制已启用）")
 
     def run(self):
-        """运行完整仿真"""
+        """运行完整仿真（负载下循环逻辑优化）"""
         global RUNNING
 
-        # 初始化Viewer
         if not self.init_viewer():
             RUNNING = False
             return
 
         # 启动信息
-        print("=" * 60)
-        print("🚀 机械臂关节控制器 - 最终兼容版")
-        print(f"✅ MJ_NAME2ID API错误已修复")
-        print(f"✅ 全Mujoco版本兼容")
-        print(f"💻 Windows优化已启用")
-        print("📝 控制指令:")
-        print("   - 单关节控制: controller.move_joint(0, np.pi/4)")
-        print("   - 多关节控制: controller.set_joint_angles([0, π/4, π/6, 0, 0])")
-        print("   - 按 Ctrl+C 退出")
-        print("=" * 60)
-
-        # 主循环
+        print("=" * 100)
+        print("🚀 机械臂关节负载性能优化控制器 - 启动成功")
+        print(f"✅ 模型信息 | 关节数量: {JOINT_COUNT} | 初始末端负载: {self.current_end_load:.2f}kg")
+        print(f"✅ 仿真配置 | 控制频率: {CONTROL_FREQUENCY}Hz | 仿真步长: {SIMULATION_TIMESTEP:.3f}s")
+        print(
+            f"✅ 保护配置 | 最大末端负载: {LOAD_PARAMS['max_allowed_load']}kg | 关节最大扭矩: {np.max(JOINT_MAX_TORQUE)}N·m")
+        print("📝 快捷指令:")
+        print("   - 设置末端负载: controller.set_end_effector_load(1.0) （设置1kg负载）")
+        print("   - 单关节控制: controller.move_joint(0, 90) （关节1旋转90度）")
+        print("   - 预设姿态: controller.preset_pose('up') （切换抬起姿态）")
+        print("   - 按 Ctrl+C 优雅退出")
+        print("=" * 100)
+
+        # 主循环（负载下极致效率优化）
         while RUNNING:
             try:
                 current_time = time.time()
                 self.fps_counter += 1
+                self.step_count += 1
 
-                # 控制频率执行PD控制
+                # 按高控制频率执行负载自适应PD控制
                 if current_time - self.last_control_time >= CONTROL_TIMESTEP:
-                    self.pd_control_loop()
+                    self.load_adaptive_pd_control()
                     self.last_control_time = current_time
 
-                # 执行仿真步
-                mujoco.mj_step(self.model, self.data)
+                # 执行仿真步（负载下容错增强）
+                if self.model is not None and self.data is not None:
+                    mujoco.mj_step(self.model, self.data)
 
                 # 同步Viewer
                 if self.viewer_ready:
                     self.viewer_inst.sync()
 
-                # 打印状态
-                self.print_status()
+                # 打印负载状态
+                self.print_load_status()
 
-                # Windows睡眠优化
+                # 动态睡眠优化，减少负载下CPU空转
                 time_diff = current_time - self.last_control_time
                 if time_diff < SLEEP_TIME:
-                    time.sleep(max(0.00001, SLEEP_TIME - time_diff))
+                    sleep_duration = max(0.00001, SLEEP_TIME - time_diff)
+                    time.sleep(sleep_duration)
 
             except Exception as e:
-                print(f"⚠️ 仿真步异常: {e}")
+                print(f"⚠️ 仿真步异常（步数：{self.step_count}）: {e}")
                 continue
 
         # 清理资源
         self.cleanup()
-        print("\n✅ 控制器已优雅退出")
+        # 最终统计
+        print("\n" + "=" * 100)
+        print("✅ 控制器已优雅退出 - 负载仿真最终统计")
+        print(
+            f"📈 总仿真时间: {self.total_simulation_time:.2f}s | 总步数: {self.step_count:,} | 平均FPS: {self.step_count / max(1, self.total_simulation_time):.1f}")
+        print(
+            f"🎯 最终末端负载 (kg): {self.current_end_load:.2f} | 最终关节受力 (N·m): {np.round(self.get_joint_forces(), 2)}")
+        print(f"🎯 最终关节角度 (度): {np.round(self.get_current_joint_angles(), 1)}")
+        print("=" * 100)
 
     def cleanup(self):
-        """资源清理"""
+        """资源清理（负载下完整释放，避免内存泄漏）"""
         if self.viewer_ready and self.viewer_inst:
             try:
                 self.viewer_inst.close()
-            except:
-                pass
-        for arr in WORK_ARRAYS.values():
-            arr.fill(0)
+            except Exception as e:
+                print(f"⚠️ Viewer关闭失败: {e}")
+            self.viewer_inst = None
+            self.viewer_ready = False
+        self.model = None
+        self.data = None
+        global RUNNING, SIMULATION_START_TIME
+        RUNNING = False
+        SIMULATION_START_TIME = None
 
 
-# ====================== 演示函数 ======================
-def demo_movements(controller):
-    """预设演示动作"""
+# ====================== 负载演示函数（验证优化效果） ======================
+def load_demo(controller):
+    """负载变化演示，验证自适应控制效果"""
 
     def demo():
         time.sleep(2)
 
-        print("\n🎬 演示1：所有关节归位")
-        controller.set_joint_angles([0, 0, 0, 0, 0])
+        # 演示1：初始零位（0.5kg负载）
+        print("\n🎬 演示1：切换到零位姿态（初始负载0.5kg）")
+        controller.preset_pose('zero')
         time.sleep(3)
 
-        print("\n🎬 演示2：关节1旋转45度")
-        controller.move_joint(0, np.pi / 4)
-        time.sleep(2)
+        # 演示2：切换抬起姿态（0.5kg负载）
+        print("\n🎬 演示2：切换到抬起姿态（0.5kg负载）")
+        controller.preset_pose('up')
+        time.sleep(3)
 
-        print("\n🎬 演示3：关节2抬起30度")
-        controller.move_joint(1, np.pi / 6)
+        # 演示3：增加末端负载到1.5kg
+        print("\n🎬 演示3：设置末端负载为1.5kg（自适应PD控制自动生效）")
+        controller.set_end_effector_load(1.5)
         time.sleep(2)
 
-        print("\n🎬 演示4：组合关节运动")
-        controller.set_joint_angles([np.pi / 4, np.pi / 6, np.pi / 8, np.pi / 10, np.pi / 12])
+        # 演示4：负载下旋转关节1（90度）
+        print("\n🎬 演示4：1.5kg负载下，关节1旋转90度（抗干扰控制，无超调）")
+        controller.move_joint(0, 90, smooth=True, use_deg=True)
+        time.sleep(3)
+
+        # 演示5：切换抓取姿态（1.5kg负载）
+        print("\n🎬 演示5：1.5kg负载下，切换到抓取姿态")
+        controller.preset_pose('grasp')
         time.sleep(3)
 
-        print("\n🎬 演示5：回到零位")
-        controller.set_joint_angles([0, 0, 0, 0, 0])
+        # 演示6：降低负载到0.2kg
+        print("\n🎬 演示6：降低末端负载为0.2kg（PD增益自动回落）")
+        controller.set_end_effector_load(0.2)
         time.sleep(2)
 
+        # 演示7：回到零位
+        print("\n🎬 演示7：切换回零位姿态")
+        controller.preset_pose('zero')
+        time.sleep(2)
+
+        # 结束演示
         global RUNNING
         RUNNING = False
 
@@ -416,14 +677,22 @@ def demo():
 
 # ====================== 主入口 ======================
 if __name__ == "__main__":
-    # 禁用NumPy警告
     np.seterr(all='ignore')
 
-    # 创建控制器（现在可正常初始化）
-    controller = ArmJointController()
+    # 创建负载优化控制器
+    controller = None
+    try:
+        controller = ArmJointLoadOptimizedController()
+    except Exception as e:
+        print(f"❌ 控制器创建失败: {e}")
+        sys.exit(1)
 
-    # 运行预设演示
-    demo_movements(controller)
+    # 运行负载演示
+    if controller is not None:
+        load_demo(controller)
 
     # 启动控制器
-    controller.run()
\ No newline at end of file
+    if controller is not None:
+        controller.run()
+
+    sys.exit(0)
\ No newline at end of file

From 88b61ed1d67b28cb4b48a27b94e38310fd2634f2 Mon Sep 17 00:00:00 2001
From: XYTX0427 <481177497@qq.com>
Date: Sat, 27 Dec 2025 17:29:50 +0800
Subject: [PATCH 20/21] =?UTF-8?q?=E6=9C=BA=E6=A2=B0=E8=87=82=E5=85=B3?=
 =?UTF-8?q?=E8=8A=82=E5=88=9A=E5=BA=A6=E4=B8=8E=E5=8F=AF=E9=9D=A0=E6=80=A7?=
 =?UTF-8?q?=E4=BC=98=E5=8C=96?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/Robotic_arm/main.py | 697 +++++++++++++++++++++++++---------------
 1 file changed, 438 insertions(+), 259 deletions(-)

diff --git a/src/Robotic_arm/main.py b/src/Robotic_arm/main.py
index 27a685095c..27818121d4 100644
--- a/src/Robotic_arm/main.py
+++ b/src/Robotic_arm/main.py
@@ -1,15 +1,13 @@
 #!/usr/bin/env python3
 # -*- coding: utf-8 -*-
 """
-机械臂关节负载性能优化版（终极全XML错误修复）
+机械臂关节刚度与可靠性优化控制器
 核心优化：
-1.  彻底修复所有XML Schema错误（body/mass/forcelimit 均移除违规配置）
-2.  精准关节负载建模（末端负载/关节负载可配置）
-3.  负载自适应PD控制（抗干扰、无超调、响应快）
-4.  物理仿真优化（减少负载下的计算冗余，提升实时性）
-5.  负载状态监控（实时显示负载大小、关节受力、控制误差）
-6.  软件层面过载保护（替代forcelimit，兼容所有Mujoco版本）
-7.  全Mujoco版本兼容（支持新旧版本，无任何语法隐患）
+1.  关节刚度分层配置+自适应调节（负载/误差驱动）
+2.  全方位可靠性保障（卡死检测/过载保护/异常复位/容错处理）
+3.  刚度-阻尼-惯量匹配优化，降低振动与干扰
+4.  全状态监控与日志记录，便于故障追溯
+5.  兼容新旧Mujoco版本，无XML语法错误
 """
 
 import sys
@@ -20,8 +18,9 @@
 import threading
 import numpy as np
 import mujoco
+from datetime import datetime
 
-# ====================== 全局配置（负载优化专用） ======================
+# ====================== 全局配置（刚度+可靠性专用） ======================
 # 系统适配（Windows优先，极致CPU优化）
 if os.name == 'nt':
     try:
@@ -31,7 +30,7 @@
         kernel32.SetThreadPriority(kernel32.GetCurrentThread(), 1)
     except Exception as e:
         print(f"⚠️ Windows系统优化失败（不影响核心功能）: {e}")
-    # 强制单线程，避免负载下多线程竞争导致卡顿
+    # 强制单线程，避免多线程竞争导致的控制不稳定
     os.environ['OMP_NUM_THREADS'] = '1'
     os.environ['MKL_NUM_THREADS'] = '1'
     os.environ['NUMEXPR_NUM_THREADS'] = '1'
@@ -49,22 +48,39 @@
         print(f"⚠️ Mujoco Viewer导入失败（无法可视化）: {e}")
 
 # 核心参数配置
-# 关节基础配置
+# 关节基础配置（按重要性分层：1>2>3>4>5）
 JOINT_COUNT = 5
 JOINT_NAMES = ["joint1", "joint2", "joint3", "joint4", "joint5"]
 JOINT_LIMITS_RAD = np.array([
-    [-np.pi, np.pi],  # joint1 (Z轴)
-    [-np.pi / 2, np.pi / 2],  # joint2 (Y轴)
-    [-np.pi / 2, np.pi / 2],  # joint3 (Y轴)
-    [-np.pi / 2, np.pi / 2],  # joint4 (Y轴)
-    [-np.pi / 2, np.pi / 2],  # joint5 (Y轴)
+    [-np.pi, np.pi],  # joint1（基座，最高刚度）
+    [-np.pi / 2, np.pi / 2],  # joint2（大臂，高刚度）
+    [-np.pi / 2, np.pi / 2],  # joint3（中臂，中高刚度）
+    [-np.pi / 2, np.pi / 2],  # joint4（小臂，中刚度）
+    [-np.pi / 2, np.pi / 2],  # joint5（末端，低刚度）
 ], dtype=np.float64)
 JOINT_MAX_VELOCITY_RAD = np.array([1.0, 0.8, 0.8, 0.6, 0.6], dtype=np.float64)
-JOINT_MAX_TORQUE = np.array([15.0, 12.0, 10.0, 8.0, 5.0], dtype=np.float64)  # 关节最大扭矩（软件过载保护）
+JOINT_MAX_TORQUE = np.array([15.0, 12.0, 10.0, 8.0, 5.0], dtype=np.float64)  # 最大扭矩（可靠性保护）
+
+# 关节刚度分层配置（核心优化：按关节层级设定基准刚度）
+STIFFNESS_PARAMS = {
+    'base_stiffness': np.array([200.0, 180.0, 150.0, 120.0, 80.0]),  # 各关节基准刚度
+    'load_stiffness_gain': 1.8,  # 负载下刚度放大系数
+    'error_stiffness_gain': 1.5,  # 大误差下刚度放大系数
+    'min_stiffness': np.array([100.0, 90.0, 75.0, 60.0, 40.0]),  # 最小允许刚度
+    'max_stiffness': np.array([300.0, 270.0, 225.0, 180.0, 120.0]),  # 最大允许刚度
+    'stiffness_smoothing': 0.05,  # 刚度变化平滑系数，防止突变
+}
+
+# 阻尼与惯量匹配配置（刚度配套优化，提升可靠性）
+DAMPING_INERTIA_PARAMS = {
+    'base_damping': np.array([8.0, 7.0, 6.0, 5.0, 3.0]),  # 基准阻尼（与刚度匹配）
+    'damping_stiffness_ratio': 0.04,  # 阻尼-刚度匹配比，保证运动平稳
+    'armature_inertia': np.array([0.5, 0.4, 0.3, 0.2, 0.1]),  # 关节惯量补偿
+}
 
-# 仿真配置（负载下实时性优化）
-SIMULATION_TIMESTEP = 0.002  # 更小步长，提升负载下控制精度
-CONTROL_FREQUENCY = 500  # 更高控制频率，应对负载下响应滞后
+# 仿真配置（可靠性优化：小步长提升控制稳定性）
+SIMULATION_TIMESTEP = 0.001  # 更小步长，降低控制误差
+CONTROL_FREQUENCY = 1000  # 更高控制频率，提升响应可靠性
 CONTROL_TIMESTEP = 1.0 / CONTROL_FREQUENCY
 FPS = 60
 SLEEP_TIME = 1.0 / FPS
@@ -72,30 +88,41 @@
 RUNNING = True
 SIMULATION_START_TIME = None
 
-# PD控制参数（负载自适应配置）
+# PD控制参数（与刚度/阻尼联动）
 PD_PARAMS = {
     'kp_base': 80.0,
     'kd_base': 5.0,
-    'kp_load_gain': 1.5,  # 负载下比例增益放大系数
-    'kd_load_gain': 1.2,  # 负载下微分增益放大系数
+    'kp_load_gain': 1.5,
+    'kd_load_gain': 1.2,
     'max_vel': JOINT_MAX_VELOCITY_RAD.copy()
 }
 
-# 负载配置（可动态调整）
+# 负载配置（与刚度联动优化）
 LOAD_PARAMS = {
-    'end_effector_mass': 0.5,  # 末端负载质量（kg），默认0.5kg
-    'joint_loads': np.zeros(JOINT_COUNT),  # 各关节附加负载（N·m）
-    'max_allowed_load': 2.0,  # 最大允许末端负载（过载保护）
-    'load_smoothing_factor': 0.1  # 负载检测平滑系数，避免抖动
+    'end_effector_mass': 0.5,
+    'joint_loads': np.zeros(JOINT_COUNT),
+    'max_allowed_load': 2.0,
+    'load_smoothing_factor': 0.1
+}
+
+# 可靠性保护配置（核心：卡死/过载/异常检测参数）
+RELIABILITY_PARAMS = {
+    'stall_detection_threshold': 0.01,  # 关节卡死判定阈值（速度<此值且扭矩>90%）
+    'stall_duration_threshold': 1.0,  # 卡死持续时间（秒），触发复位
+    'overload_duration_threshold': 2.0,  # 过载持续时间，触发保护
+    'max_angle_error': np.deg2rad(10.0),  # 最大允许角度误差，触发异常报警
+    'auto_reset_on_error': True,  # 是否自动复位异常关节
+    'log_reliability_data': True,  # 是否记录可靠性日志
+    'log_path': 'arm_reliability_log.txt'  # 日志保存路径
 }
 
 
-# ====================== 信号处理（负载下优雅退出） ======================
+# ====================== 信号处理（可靠性优化：优雅退出） ======================
 def signal_handler(sig, frame):
     global RUNNING
     if not RUNNING:
         sys.exit(0)
-    print("\n⚠️ 收到退出信号，正在优雅退出（清理负载相关资源）...")
+    print("\n⚠️ 收到退出信号，正在优雅退出（保存可靠性日志+清理资源）...")
     RUNNING = False
 
 
@@ -103,9 +130,9 @@ def signal_handler(sig, frame):
 signal.signal(signal.SIGTERM, signal_handler)
 
 
-# ====================== 工具函数（负载优化专用） ======================
+# ====================== 工具函数（刚度+可靠性专用） ======================
 def get_mujoco_id(model, obj_type, name):
-    """兼容所有Mujoco版本的ID查询函数（容错增强）"""
+    """兼容所有Mujoco版本的ID查询（容错增强，提升可靠性）"""
     if model is None:
         return -1
     type_map = {
@@ -125,7 +152,7 @@ def get_mujoco_id(model, obj_type, name):
 
 
 def deg2rad(degrees):
-    """角度值（度）转弧度（容错增强）"""
+    """角度转弧度（容错增强，可靠性保障）"""
     try:
         degrees = np.array(degrees, dtype=np.float64)
         return np.deg2rad(degrees)
@@ -135,7 +162,7 @@ def deg2rad(degrees):
 
 
 def rad2deg(radians):
-    """弧度转角度值（度）（容错增强）"""
+    """弧度转角度（容错增强，可靠性保障）"""
     try:
         radians = np.array(radians, dtype=np.float64)
         return np.rad2deg(radians)
@@ -144,97 +171,107 @@ def rad2deg(radians):
         return 0.0 if np.isscalar(radians) else np.zeros(JOINT_COUNT, dtype=np.float64)
 
 
-def calculate_load_adaptive_gains(current_load):
-    """
-    负载自适应增益计算（核心优化）
-    根据当前末端负载，动态调整PD增益，抵消负载干扰
-    :param current_load: 当前末端负载（kg）
-    :return: 自适应kp, kd
-    """
-    # 负载归一化（0 ~ 1）
-    normalized_load = min(current_load / LOAD_PARAMS['max_allowed_load'], 1.0)
-    # 动态调整增益（负载越大，增益越高，保证响应性）
-    adaptive_kp = PD_PARAMS['kp_base'] * (1 + normalized_load * (PD_PARAMS['kp_load_gain'] - 1))
-    adaptive_kd = PD_PARAMS['kd_base'] * (1 + normalized_load * (PD_PARAMS['kd_load_gain'] - 1))
-    return adaptive_kp, adaptive_kd
+def write_reliability_log(content, log_path=RELIABILITY_PARAMS['log_path']):
+    """写入可靠性日志（核心：记录异常状态，便于追溯）"""
+    if not RELIABILITY_PARAMS['log_reliability_data']:
+        return
+    try:
+        with open(log_path, 'a', encoding='utf-8') as f:
+            timestamp = datetime.now().strftime('%Y-%m-%d %H:%M:%S')
+            f.write(f"[{timestamp}] {content}\n")
+    except Exception as e:
+        print(f"⚠️ 写入可靠性日志失败: {e}")
 
 
-# ====================== 机械臂模型生成（终极全XML错误修复） ======================
-def create_arm_model_with_load():
+# ====================== 机械臂模型生成（刚度配置+无XML错误） ======================
+def create_arm_model_with_stiffness():
     """
-    生成带负载建模的机械臂XML模型（终极全修复+全版本兼容）
-    1.  彻底修复所有XML Schema错误：
-        - 移除body的mass属性（改用geom定义质量）
-        - 移除motor的forcelimit属性（改用软件层面过载保护）
-    2.  末端负载通过geom的mass属性配置，保留可动态调整功能
-    3.  关节添加阻尼和惯量，模拟真实负载特性
-    4.  简化非核心几何，提升负载下仿真速度
-    5.  依赖compiler的inertiafromgeom="true"，自动由geom质量推导body惯性属性
-    6.  无任何违规配置，兼容所有Mujoco版本
+    生成带关节刚度配置的机械臂XML模型（兼容所有Mujoco版本）
+    1.  按关节层级配置基准刚度、阻尼、惯量，实现刚度分层
+    2.  移除所有违规XML属性，保证无语法错误
+    3.  几何与质量配置优化，提升仿真可靠性
     """
     end_effector_mass = LOAD_PARAMS['end_effector_mass']
-    # 连杆geom质量（对应原body质量，通过geom定义，兼容所有Mujoco版本）
+    # 连杆geom质量（兼容新旧Mujoco版本）
     link1_geom_mass = 0.8
     link2_geom_mass = 0.6
     link3_geom_mass = 0.6
     link4_geom_mass = 0.4
     link5_geom_mass = 0.2
 
+    # 从配置中提取关节参数（刚度/阻尼/惯量）
+    base_stiffness = STIFFNESS_PARAMS['base_stiffness']
+    base_damping = DAMPING_INERTIA_PARAMS['base_damping']
+    armature_inertia = DAMPING_INERTIA_PARAMS['armature_inertia']
+
     xml = f"""
-<mujoco model="arm_with_load">
+<mujoco model="arm_with_stiffness_reliability">
     <compiler angle="radian" inertiafromgeom="true" autolimits="true"/>
-    <option timestep="{SIMULATION_TIMESTEP}" gravity="0 0 -9.81" iterations="30" tolerance="1e-6"/>
+    <option timestep="{SIMULATION_TIMESTEP}" gravity="0 0 -9.81" iterations="50" tolerance="1e-7"/>
 
-    <!-- 仅保留Mujoco基础支持的元素，无任何版本专属配置（全版本兼容） -->
+    <!-- 关节刚度+阻尼+惯量基础配置（分层设定，提升可靠性） -->
     <default>
-        <joint type="hinge" armature="0.2" damping="0.2" limited="true" margin="0.01"/>
-        <motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="80"/>
+        <joint type="hinge" armature="{armature_inertia[0]}" damping="{base_damping[0]}" limited="true" margin="0.01"/>
+        <motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="100"/>
         <geom contype="1" conaffinity="1" rgba="0.2 0.8 0.2 1"/>
     </default>
 
-    <!-- 负载相关配置：末端负载可视化材质 -->
+    <!-- 负载与刚度可视化材质 -->
     <asset>
-        <material name="load_material" rgba="1.0 0.0 0.0 0.8"/> <!-- 红色标记负载 -->
+        <material name="load_material" rgba="1.0 0.0 0.0 0.8"/>
+        <material name="high_stiffness_material" rgba="0.0 0.8 0.0 0.8"/>
+        <material name="low_stiffness_material" rgba="0.8 0.0 0.0 0.8"/>
     </asset>
 
     <worldbody>
-        <!-- 地面（简化尺寸，减少渲染开销） -->
+        <!-- 地面（简化几何，提升仿真效率） -->
         <geom name="floor" type="plane" size="3 3 0.1" pos="0 0 0" rgba="0.8 0.8 0.8 1"/>
 
-        <!-- 机械臂基座（无mass属性，符合Schema规范） -->
+        <!-- 机械臂基座（joint1：最高刚度） -->
         <body name="base" pos="0 0 0">
             <geom name="base_geom" type="cylinder" size="0.1 0.1" rgba="0.2 0.2 0.8 1"/>
 
-            <!-- 关节1 -->
-            <joint name="joint1" type="hinge" axis="0 0 1" pos="0 0 0.1" range="{JOINT_LIMITS_RAD[0, 0]} {JOINT_LIMITS_RAD[0, 1]}"/>
-            <!-- 连杆1：无body mass，质量通过geom定义（兼容所有Mujoco版本） -->
+            <!-- 关节1（基座关节，最高刚度+惯量） -->
+            <joint name="joint1" type="hinge" axis="0 0 1" pos="0 0 0.1" 
+                   range="{JOINT_LIMITS_RAD[0, 0]} {JOINT_LIMITS_RAD[0, 1]}" 
+                   armature="{armature_inertia[0]}" damping="{base_damping[0]}"/>
             <body name="link1" pos="0 0 0.1">
-                <geom name="link1_geom" type="cylinder" size="0.04 0.18" mass="{link1_geom_mass}"/>
+                <geom name="link1_geom" type="cylinder" size="0.04 0.18" mass="{link1_geom_mass}"
+                      material="high_stiffness_material"/>
 
-                <!-- 关节2 -->
-                <joint name="joint2" type="hinge" axis="0 1 0" pos="0 0 0.18" range="{JOINT_LIMITS_RAD[1, 0]} {JOINT_LIMITS_RAD[1, 1]}"/>
+                <!-- 关节2（大臂关节，高刚度） -->
+                <joint name="joint2" type="hinge" axis="0 1 0" pos="0 0 0.18" 
+                       range="{JOINT_LIMITS_RAD[1, 0]} {JOINT_LIMITS_RAD[1, 1]}" 
+                       armature="{armature_inertia[1]}" damping="{base_damping[1]}"/>
                 <body name="link2" pos="0 0 0.18">
-                    <geom name="link2_geom" type="cylinder" size="0.04 0.18" mass="{link2_geom_mass}"/>
+                    <geom name="link2_geom" type="cylinder" size="0.04 0.18" mass="{link2_geom_mass}"
+                          material="high_stiffness_material"/>
 
-                    <!-- 关节3 -->
-                    <joint name="joint3" type="hinge" axis="0 1 0" pos="0 0 0.18" range="{JOINT_LIMITS_RAD[2, 0]} {JOINT_LIMITS_RAD[2, 1]}"/>
+                    <!-- 关节3（中臂关节，中高刚度） -->
+                    <joint name="joint3" type="hinge" axis="0 1 0" pos="0 0 0.18" 
+                           range="{JOINT_LIMITS_RAD[2, 0]} {JOINT_LIMITS_RAD[2, 1]}" 
+                           armature="{armature_inertia[2]}" damping="{base_damping[2]}"/>
                     <body name="link3" pos="0 0 0.18">
                         <geom name="link3_geom" type="cylinder" size="0.04 0.18" mass="{link3_geom_mass}"/>
 
-                        <!-- 关节4 -->
-                        <joint name="joint4" type="hinge" axis="0 1 0" pos="0 0 0.18" range="{JOINT_LIMITS_RAD[3, 0]} {JOINT_LIMITS_RAD[3, 1]}"/>
+                        <!-- 关节4（小臂关节，中刚度） -->
+                        <joint name="joint4" type="hinge" axis="0 1 0" pos="0 0 0.18" 
+                               range="{JOINT_LIMITS_RAD[3, 0]} {JOINT_LIMITS_RAD[3, 1]}" 
+                               armature="{armature_inertia[3]}" damping="{base_damping[3]}"/>
                         <body name="link4" pos="0 0 0.18">
                             <geom name="link4_geom" type="cylinder" size="0.04 0.18" mass="{link4_geom_mass}"/>
 
-                            <!-- 关节5 -->
-                            <joint name="joint5" type="hinge" axis="0 1 0" pos="0 0 0.18" range="{JOINT_LIMITS_RAD[4, 0]} {JOINT_LIMITS_RAD[4, 1]}"/>
+                            <!-- 关节5（末端关节，低刚度） -->
+                            <joint name="joint5" type="hinge" axis="0 1 0" pos="0 0 0.18" 
+                                   range="{JOINT_LIMITS_RAD[4, 0]} {JOINT_LIMITS_RAD[4, 1]}" 
+                                   armature="{armature_inertia[4]}" damping="{base_damping[4]}"/>
                             <body name="link5" pos="0 0 0.18">
-                                <geom name="link5_geom" type="cylinder" size="0.03 0.09" mass="{link5_geom_mass}" rgba="0.8 0.2 0.2 1"/>
+                                <geom name="link5_geom" type="cylinder" size="0.03 0.09" mass="{link5_geom_mass}"
+                                      material="low_stiffness_material" rgba="0.8 0.2 0.2 1"/>
 
-                                <!-- 末端执行器（负载通过geom mass定义，无body mass属性，兼容所有版本） -->
+                                <!-- 末端执行器（带负载，兼容动态调整） -->
                                 <body name="end_effector" pos="0 0 0.09">
                                     <site name="ee_site" pos="0 0 0" size="0.01"/>
-                                    <!-- 末端负载：通过geom的mass属性配置，实现可动态调整 -->
                                     <geom name="load_geom" type="sphere" size="0.04" mass="{end_effector_mass}" 
                                           rgba="1.0 0.0 0.0 0.8" material="load_material"/>
                                 </body>
@@ -246,7 +283,7 @@ def create_arm_model_with_load():
         </body>
     </worldbody>
 
-    <!-- 关节电机（移除forcelimit属性，改用软件层面过载保护，兼容所有Mujoco版本） -->
+    <!-- 关节电机（无违规属性，兼容所有Mujoco版本） -->
     <actuator>
         <motor name="motor1" joint="joint1" ctrlrange="-1 1" gear="100"/>
         <motor name="motor2" joint="joint2" ctrlrange="-1 1" gear="100"/>
@@ -259,29 +296,31 @@ def create_arm_model_with_load():
     return xml
 
 
-# ====================== 核心控制器类（关节负载性能优化+软件过载保护） ======================
-class ArmJointLoadOptimizedController:
+# ====================== 核心控制器类（刚度+可靠性优化） ======================
+class ArmStiffnessReliabilityController:
     def __init__(self):
-        # 初始化模型和数据（负载下容错增强）
+        # 模型与数据初始化（容错增强，提升可靠性）
         self.model = None
         self.data = None
         try:
-            self.model = mujoco.MjModel.from_xml_string(create_arm_model_with_load())
+            self.model = mujoco.MjModel.from_xml_string(create_arm_model_with_stiffness())
             self.data = mujoco.MjData(self.model)
+            write_reliability_log("模型初始化成功，刚度与可靠性配置加载完成")
         except Exception as e:
-            print(f"❌ 带负载模型初始化失败: {e}")
+            error_msg = f"带刚度配置模型初始化失败: {e}"
+            print(f"❌ {error_msg}")
+            write_reliability_log(error_msg)
             global RUNNING
             RUNNING = False
             return
 
-        # 获取ID
+        # 获取各类ID（容错增强）
         self.joint_ids = [get_mujoco_id(self.model, 'joint', name) for name in JOINT_NAMES]
         self.motor_ids = [get_mujoco_id(self.model, 'actuator', f"motor{i + 1}") for i in range(JOINT_COUNT)]
         self.ee_site_id = get_mujoco_id(self.model, 'site', "ee_site")
-        self.ee_body_id = get_mujoco_id(self.model, 'body', "end_effector")
-        self.load_geom_id = get_mujoco_id(self.model, 'geom', "load_geom")  # 负载geom ID，用于更新质量
+        self.load_geom_id = get_mujoco_id(self.model, 'geom', "load_geom")
 
-        # 状态变量（负载监控专用）
+        # 状态变量初始化
         self.viewer_inst = None
         self.viewer_ready = False
         self.last_control_time = time.time()
@@ -290,25 +329,39 @@ def __init__(self):
         self.step_count = 0
         self.total_simulation_time = 0.0
 
-        # 负载相关状态
+        # 刚度相关状态（核心：平滑刚度记录）
+        self.current_stiffness = STIFFNESS_PARAMS['base_stiffness'].copy()
+        self.current_damping = DAMPING_INERTIA_PARAMS['base_damping'].copy()
+        self.target_angles_rad = np.zeros(JOINT_COUNT, dtype=np.float64)
+
+        # 负载与受力状态
         self.current_end_load = LOAD_PARAMS['end_effector_mass']
         self.smoothed_joint_forces = np.zeros(JOINT_COUNT, dtype=np.float64)
-        self.angle_error_history = np.zeros(JOINT_COUNT, dtype=np.float64)  # 控制误差监控
+        self.angle_error_history = np.zeros(JOINT_COUNT, dtype=np.float64)
+
+        # 可靠性保护状态（核心：卡死/过载/异常检测）
         self.overload_warning_flag = False
+        self.stall_detection_flag = np.zeros(JOINT_COUNT, dtype=bool)
+        self.stall_duration = np.zeros(JOINT_COUNT, dtype=np.float64)
+        self.overload_duration = np.zeros(JOINT_COUNT, dtype=np.float64)
+        self.error_reset_count = 0  # 异常复位计数
 
-        # 初始化目标角度为零位（提前定义，避免属性不存在错误）
-        self.target_angles_rad = np.zeros(JOINT_COUNT, dtype=np.float64)
+        # 初始化关节角度
         try:
             self.set_joint_angles(np.zeros(JOINT_COUNT), smooth=False, use_deg=False)
+            write_reliability_log("关节角度初始化成功，零位校准完成")
         except Exception as e:
-            print(f"⚠️ 初始化关节角度失败: {e}")
+            error_msg = f"初始化关节角度失败: {e}"
+            print(f"⚠️ {error_msg}")
+            write_reliability_log(error_msg)
 
         # 全局仿真开始时间
         global SIMULATION_START_TIME
         SIMULATION_START_TIME = time.time()
+        write_reliability_log(f"仿真启动，控制频率：{CONTROL_FREQUENCY}Hz，步长：{SIMULATION_TIMESTEP}s")
 
     def get_current_joint_angles(self, use_deg=True):
-        """获取当前关节角度（负载下按需转换，减少冗余）"""
+        """获取当前关节角度（容错增强）"""
         if self.data is None:
             return np.zeros(JOINT_COUNT, dtype=np.float64)
         current_rad = np.array([self.data.qpos[jid] if jid >= 0 else 0 for jid in self.joint_ids], dtype=np.float64)
@@ -317,93 +370,191 @@ def get_current_joint_angles(self, use_deg=True):
         return current_rad
 
     def get_joint_forces(self):
-        """
-        获取关节实时受力（负载监控核心）
-        返回各关节扭矩（N·m），反映负载大小
-        """
+        """获取关节实时受力（可靠性监控核心）"""
         if self.data is None:
             return np.zeros(JOINT_COUNT, dtype=np.float64)
         joint_forces = np.zeros(JOINT_COUNT, dtype=np.float64)
         for i, jid in enumerate(self.joint_ids):
             if jid >= 0:
-                # 读取关节受力，并平滑处理，避免抖动
                 raw_force = abs(self.data.qfrc_actuator[jid])
+                # 平滑受力，避免抖动影响检测
                 self.smoothed_joint_forces[i] = (1 - LOAD_PARAMS['load_smoothing_factor']) * self.smoothed_joint_forces[
                     i] + \
                                                 LOAD_PARAMS['load_smoothing_factor'] * raw_force
                 joint_forces[i] = self.smoothed_joint_forces[i]
         return joint_forces
 
-    def get_ee_position(self):
-        """获取末端位置（容错增强）"""
-        if self.data is None or self.ee_site_id < 0:
-            return np.zeros(3, dtype=np.float64)
-        return self.data.site_xpos[self.ee_site_id].copy()
+    def calculate_adaptive_stiffness(self):
+        """
+        核心优化：计算自适应关节刚度
+        1.  根据负载大小动态调整刚度
+        2.  根据角度误差动态调整刚度
+        3.  刚度限幅+平滑处理，保证可靠性
+        4.  阻尼与刚度匹配，降低振动
+        """
+        # 1. 负载归一化
+        normalized_load = min(self.current_end_load / LOAD_PARAMS['max_allowed_load'], 1.0)
+
+        # 2. 角度误差归一化
+        current_angles = self.get_current_joint_angles(use_deg=False)
+        angle_error_rad = np.abs(self.target_angles_rad - current_angles)
+        normalized_error = np.clip(angle_error_rad / RELIABILITY_PARAMS['max_angle_error'], 0.0, 1.0)
+
+        # 3. 计算目标刚度（负载+误差双驱动）
+        target_stiffness = STIFFNESS_PARAMS['base_stiffness'] * \
+                           (1 + normalized_load * (STIFFNESS_PARAMS['load_stiffness_gain'] - 1)) * \
+                           (1 + normalized_error * (STIFFNESS_PARAMS['error_stiffness_gain'] - 1))
+
+        # 4. 刚度限幅（防止超出合理范围）
+        target_stiffness = np.clip(target_stiffness,
+                                   STIFFNESS_PARAMS['min_stiffness'],
+                                   STIFFNESS_PARAMS['max_stiffness'])
+
+        # 5. 刚度平滑更新（防止突变，提升可靠性）
+        self.current_stiffness = (1 - STIFFNESS_PARAMS['stiffness_smoothing']) * self.current_stiffness + \
+                                 STIFFNESS_PARAMS['stiffness_smoothing'] * target_stiffness
+
+        # 6. 阻尼与刚度匹配更新（保证运动平稳）
+        target_damping = self.current_stiffness * DAMPING_INERTIA_PARAMS['damping_stiffness_ratio']
+        self.current_damping = np.clip(target_damping,
+                                       DAMPING_INERTIA_PARAMS['base_damping'] * 0.5,
+                                       DAMPING_INERTIA_PARAMS['base_damping'] * 1.5)
+
+        # 7. 更新模型阻尼（实时生效）
+        for i, jid in enumerate(self.joint_ids):
+            if jid >= 0 and self.model is not None:
+                self.model.jnt_damping[jid] = self.current_damping[i]
 
-    def clamp_joint_angles(self, angles, use_deg=True):
-        """关节限位保护（负载下更严格的限位校验）"""
-        angles = np.array(angles, dtype=np.float64)
-        if use_deg:
-            angles_rad = deg2rad(angles)
-        else:
-            angles_rad = angles.copy()
-        # 负载下缩小限位余量，防止冲击
-        limit_margin = 0.05  # 5%余量
-        limits_rad_margin = JOINT_LIMITS_RAD.copy()
-        limits_rad_margin[:, 0] += limit_margin
-        limits_rad_margin[:, 1] -= limit_margin
-        clamped_rad = np.clip(angles_rad, limits_rad_margin[:, 0], limits_rad_margin[:, 1])
-        if use_deg:
-            return rad2deg(clamped_rad)
-        return clamped_rad
+        return self.current_stiffness, self.current_damping
 
-    def set_end_effector_load(self, mass):
+    def reliability_detection(self):
         """
-        动态设置末端负载（核心功能，兼容所有Mujoco版本）
-        :param mass: 末端负载质量（kg），需≤max_allowed_load
+        核心可靠性功能：关节卡死+过载+异常检测
+        1.  卡死检测：速度极低且扭矩接近最大值
+        2.  过载检测：受力持续超过阈值
+        3.  异常复位：满足条件时自动复位关节
         """
+        if self.data is None:
+            return
+
+        # 1. 获取当前状态
+        current_forces = self.get_joint_forces()
+        current_vels = np.array([self.data.qvel[jid] if jid >= 0 else 0 for jid in self.joint_ids], dtype=np.float64)
+        current_angles = self.get_current_joint_angles(use_deg=False)
+        angle_error = np.abs(self.target_angles_rad - current_angles)
+
+        # 2. 卡死检测（速度<阈值 且 受力>90%最大扭矩）
+        current_time = time.time()
+        for i in range(JOINT_COUNT):
+            vel_abs = abs(current_vels[i])
+            force_ratio = current_forces[i] / JOINT_MAX_TORQUE[i] if JOINT_MAX_TORQUE[i] > 0 else 0
+
+            # 判定卡死条件
+            if vel_abs < RELIABILITY_PARAMS['stall_detection_threshold'] and force_ratio > 0.9:
+                self.stall_duration[i] += current_time - self.last_control_time
+                if self.stall_duration[i] >= RELIABILITY_PARAMS['stall_duration_threshold']:
+                    self.stall_detection_flag[i] = True
+                    error_msg = f"关节{JOINT_NAMES[i]}卡死检测触发，速度：{vel_abs:.4f}，受力：{current_forces[i]:.2f}N·m"
+                    print(f"⚠️ {error_msg}")
+                    write_reliability_log(error_msg)
+            else:
+                self.stall_duration[i] = 0.0
+                self.stall_detection_flag[i] = False
+
+            # 3. 过载检测（受力>90%最大扭矩 且 持续超时）
+            if force_ratio > 0.9:
+                self.overload_duration[i] += current_time - self.last_control_time
+                if self.overload_duration[i] >= RELIABILITY_PARAMS['overload_duration_threshold']:
+                    self.overload_warning_flag = True
+                    error_msg = f"关节{JOINT_NAMES[i]}过载持续触发，受力：{current_forces[i]:.2f}N·m，持续时间：{self.overload_duration[i]:.2f}s"
+                    print(f"⚠️ {error_msg}")
+                    write_reliability_log(error_msg)
+            else:
+                self.overload_duration[i] = 0.0
+
+        # 4. 大误差检测（角度误差超出阈值）
+        large_error_joints = np.where(angle_error > RELIABILITY_PARAMS['max_angle_error'])[0]
+        if len(large_error_joints) > 0:
+            joint_names = [JOINT_NAMES[i] for i in large_error_joints]
+            error_msg = f"大角度误差触发，关节：{joint_names}，最大误差：{np.max(angle_error):.2f}rad"
+            print(f"⚠️ {error_msg}")
+            write_reliability_log(error_msg)
+
+        # 5. 自动异常复位（可靠性核心功能）
+        if RELIABILITY_PARAMS['auto_reset_on_error'] and (
+                np.any(self.stall_detection_flag) or self.overload_warning_flag or len(large_error_joints) > 0):
+            self.auto_reset_joints()
+            self.error_reset_count += 1
+            write_reliability_log(f"异常自动复位触发，复位次数：{self.error_reset_count}")
+
+    def auto_reset_joints(self):
+        """自动复位异常关节（可靠性保护：恢复零位，降低负载）"""
+        print("\n🔧 执行关节自动复位，恢复零位并降低末端负载...")
+        # 1. 降低末端负载到安全值
+        self.set_end_effector_load(0.1)
+        # 2. 复位关节到零位
+        self.set_joint_angles(np.zeros(JOINT_COUNT), smooth=False, use_deg=False)
+        # 3. 重置可靠性状态标志
+        self.overload_warning_flag = False
+        self.stall_detection_flag = np.zeros(JOINT_COUNT, dtype=bool)
+        self.stall_duration = np.zeros(JOINT_COUNT, dtype=np.float64)
+        self.overload_duration = np.zeros(JOINT_COUNT, dtype=np.float64)
+        # 4. 重置刚度到基准值
+        self.current_stiffness = STIFFNESS_PARAMS['base_stiffness'].copy()
+        self.current_damping = DAMPING_INERTIA_PARAMS['base_damping'].copy()
+        time.sleep(0.5)  # 复位后延迟，保证稳定
+        print("✅ 关节自动复位完成，恢复安全状态")
+
+    def set_end_effector_load(self, mass):
+        """动态设置末端负载（与刚度联动）"""
         if mass < 0 or mass > LOAD_PARAMS['max_allowed_load']:
             self.overload_warning_flag = True
-            print(f"⚠️ 末端负载超出限制（0 ~ {LOAD_PARAMS['max_allowed_load']}kg），当前设置：{mass}kg")
+            warning_msg = f"末端负载超出限制（0 ~ {LOAD_PARAMS['max_allowed_load']}kg），当前设置：{mass}kg"
+            print(f"⚠️ {warning_msg}")
+            write_reliability_log(warning_msg)
             return
         self.overload_warning_flag = False
 
-        # 方案1：直接更新负载geom的质量（无需重新初始化模型，更高效）
+        # 优先直接更新负载geom质量（高效）
         if self.model is not None and self.load_geom_id >= 0:
             try:
-                # 直接修改geom的mass属性，实时生效
                 self.model.geom_mass[self.load_geom_id] = mass
-                # 更新内部状态
                 self.current_end_load = mass
                 LOAD_PARAMS['end_effector_mass'] = mass
-                print(f"✅ 末端负载已更新为 {mass}kg（直接修改geom质量，无需重启模型）")
+                info_msg = f"末端负载更新为 {mass}kg（直接修改geom质量）"
+                print(f"✅ {info_msg}")
+                write_reliability_log(info_msg)
                 return
             except Exception as e:
-                print(f"⚠️ 直接更新负载失败，将重新初始化模型: {e}")
+                error_msg = f"直接更新负载失败，将重新初始化模型: {e}"
+                print(f"⚠️ {error_msg}")
+                write_reliability_log(error_msg)
 
-        # 方案2：降级方案 - 重新初始化模型（兼容特殊场景）
+        # 降级方案：重新初始化模型
         try:
             LOAD_PARAMS['end_effector_mass'] = mass
             self.current_end_load = mass
-            # 重新初始化模型
-            self.model = mujoco.MjModel.from_xml_string(create_arm_model_with_load())
+            self.model = mujoco.MjModel.from_xml_string(create_arm_model_with_stiffness())
             self.data = mujoco.MjData(self.model)
-            # 重新获取所有ID
+            # 重新获取ID
             self.joint_ids = [get_mujoco_id(self.model, 'joint', name) for name in JOINT_NAMES]
             self.motor_ids = [get_mujoco_id(self.model, 'actuator', f"motor{i + 1}") for i in range(JOINT_COUNT)]
             self.ee_site_id = get_mujoco_id(self.model, 'site', "ee_site")
-            self.ee_body_id = get_mujoco_id(self.model, 'body', "end_effector")
             self.load_geom_id = get_mujoco_id(self.model, 'geom', "load_geom")
-            # 保留当前目标角度
+            # 保留目标角度
             current_target = self.target_angles_rad.copy()
             self.target_angles_rad = current_target
             self.set_joint_angles(current_target, smooth=False, use_deg=False)
-            print(f"✅ 末端负载已更新为 {mass}kg（重新初始化模型生效）")
+            info_msg = f"末端负载更新为 {mass}kg（重新初始化模型生效）"
+            print(f"✅ {info_msg}")
+            write_reliability_log(info_msg)
         except Exception as e:
-            print(f"❌ 更新末端负载失败: {e}")
+            error_msg = f"更新末端负载失败: {e}"
+            print(f"❌ {error_msg}")
+            write_reliability_log(error_msg)
 
     def set_joint_angles(self, target_angles, smooth=True, use_deg=True):
-        """设置关节目标角度（负载下参数校验增强）"""
+        """设置关节目标角度（容错增强）"""
         if self.data is None:
             raise Exception("模型未初始化，无法设置关节角度")
         if len(target_angles) != JOINT_COUNT:
@@ -419,61 +570,64 @@ def set_joint_angles(self, target_angles, smooth=True, use_deg=True):
             try:
                 mujoco.mj_forward(self.model, self.data)
             except Exception as e:
-                print(f"⚠️ 更新模型状态失败: {e}")
+                error_msg = f"更新模型状态失败: {e}"
+                print(f"⚠️ {error_msg}")
+                write_reliability_log(error_msg)
 
-        # 平滑控制时，记录目标角度（供PD控制使用）
         self.target_angles_rad = target_angles_rad.copy()
 
-    def move_joint(self, joint_idx, angle, smooth=True, use_deg=True):
-        """单独控制单个关节（负载下容错增强）"""
-        if joint_idx < 0 or joint_idx >= JOINT_COUNT:
-            raise ValueError(f"关节索引必须在0-{JOINT_COUNT - 1}之间，当前为{joint_idx}")
-
-        current_angles = self.get_current_joint_angles(use_deg=use_deg)
-        current_angles[joint_idx] = angle
-        self.set_joint_angles(current_angles, smooth=smooth, use_deg=use_deg)
+    def clamp_joint_angles(self, angles, use_deg=True):
+        """关节限位保护（可靠性优化：缩小余量，防止冲击）"""
+        angles = np.array(angles, dtype=np.float64)
+        if use_deg:
+            angles_rad = deg2rad(angles)
+        else:
+            angles_rad = angles.copy()
+        # 安全余量：5%，防止关节撞击限位
+        limit_margin = 0.05
+        limits_rad_margin = JOINT_LIMITS_RAD.copy()
+        limits_rad_margin[:, 0] += limit_margin
+        limits_rad_margin[:, 1] -= limit_margin
+        clamped_rad = np.clip(angles_rad, limits_rad_margin[:, 0], limits_rad_margin[:, 1])
+        if use_deg:
+            return rad2deg(clamped_rad)
+        return clamped_rad
 
-    def load_adaptive_pd_control(self):
+    def stiffness_adaptive_pd_control(self):
         """
-        负载自适应PD控制（核心性能优化）+ 软件层面过载保护（替代forcelimit）
-        1.  动态调整PD增益，抵消负载干扰
-        2.  软件扭矩限制，防止关节过载（兼容所有Mujoco版本）
-        3.  误差反馈平滑，减少负载下抖动
-        4.  过载时自动降低控制输出，保护关节
+        刚度自适应PD控制（核心：刚度与PD参数联动，提升精度与可靠性）
         """
         if self.data is None:
             return
 
-        # 1. 获取当前状态
-        current_angles_rad = self.get_current_joint_angles(use_deg=False)
-        current_vels_rad = np.array([self.data.qvel[jid] if jid >= 0 else 0 for jid in self.joint_ids],
-                                    dtype=np.float64)
-        joint_forces = self.get_joint_forces()
+        # 1. 自适应刚度与阻尼更新
+        current_stiffness, current_damping = self.calculate_adaptive_stiffness()
 
-        # 2. 计算负载自适应PD增益
-        adaptive_kp, adaptive_kd = calculate_load_adaptive_gains(self.current_end_load)
+        # 2. 获取当前状态
+        current_angles = self.get_current_joint_angles(use_deg=False)
+        current_vels = np.array([self.data.qvel[jid] if jid >= 0 else 0 for jid in self.joint_ids], dtype=np.float64)
+        joint_forces = self.get_joint_forces()
+        angle_error = self.target_angles_rad - current_angles
 
-        # 3. 计算控制误差（平滑处理）
-        angle_error_rad = self.target_angles_rad - current_angles_rad
+        # 3. 误差平滑
         self.angle_error_history = (1 - LOAD_PARAMS['load_smoothing_factor']) * self.angle_error_history + \
-                                   LOAD_PARAMS['load_smoothing_factor'] * angle_error_rad
+                                   LOAD_PARAMS['load_smoothing_factor'] * angle_error
 
-        # 4. 计算期望速度（带速度限制）
-        desired_vel_rad = np.clip(self.angle_error_history * adaptive_kp, -PD_PARAMS['max_vel'], PD_PARAMS['max_vel'])
+        # 4. PD参数与刚度联动
+        kp = current_stiffness / 2.5  # 刚度-P比例联动
+        kd = current_damping / 1.6  # 阻尼-D比例联动
 
-        # 5. PD控制输出计算
-        control_signals = adaptive_kp * self.angle_error_history + adaptive_kd * (desired_vel_rad - current_vels_rad)
+        # 5. 期望速度与控制信号计算
+        desired_vel = np.clip(self.angle_error_history * kp, -JOINT_MAX_VELOCITY_RAD, JOINT_MAX_VELOCITY_RAD)
+        control_signals = kp * self.angle_error_history + kd * (desired_vel - current_vels)
 
-        # 6. 软件层面过载保护（替代motor的forcelimit，兼容所有Mujoco版本）
+        # 6. 软件过载保护（可靠性核心）
         for i in range(JOINT_COUNT):
-            # 判断关节是否接近过载（受力达到90%最大扭矩阈值）
-            if joint_forces[i] > JOINT_MAX_TORQUE[i] * 0.9:
-                control_signals[i] *= 0.5  # 降低50%控制输出，防止过载损坏
-                self.overload_warning_flag = True  # 置位过载警告标志
-            else:
-                # 过载解除后，清除警告标志
-                if self.overload_warning_flag:
-                    self.overload_warning_flag = False
+            force_ratio = joint_forces[i] / JOINT_MAX_TORQUE[i] if JOINT_MAX_TORQUE[i] > 0 else 0
+            if force_ratio > 0.9:
+                control_signals[i] *= 0.4  # 降低60%输出，防止过载
+            elif force_ratio > 0.7:
+                control_signals[i] *= 0.7  # 降低30%输出，预警保护
 
         # 7. 设置控制信号
         for i, mid in enumerate(self.motor_ids):
@@ -481,7 +635,7 @@ def load_adaptive_pd_control(self):
                 self.data.ctrl[mid] = control_signals[i]
 
     def init_viewer(self):
-        """初始化Viewer（负载下延迟加载，提升启动速度）"""
+        """初始化Viewer（延迟加载，提升可靠性）"""
         if self.model is None or self.data is None:
             return False
         if self.viewer_ready:
@@ -492,57 +646,67 @@ def init_viewer(self):
             else:
                 self.viewer_inst = viewer.Viewer(self.model, self.data)
             self.viewer_ready = True
+            write_reliability_log("Viewer初始化成功，可视化启用")
             print("✅ Viewer初始化成功")
             return True
         except Exception as e:
-            print(f"❌ Viewer初始化失败: {e}")
+            error_msg = f"Viewer初始化失败: {e}"
+            print(f"❌ {error_msg}")
+            write_reliability_log(error_msg)
             return False
 
-    def print_load_status(self):
-        """打印负载相关状态（核心监控功能）"""
+    def print_stiffness_reliability_status(self):
+        """打印刚度与可靠性状态（实时监控）"""
         current_time = time.time()
         if current_time - self.last_print_time < 1.0:
             return
 
-        # 计算统计信息
+        # 统计信息
         fps = self.fps_counter / (current_time - self.last_print_time)
-        joint_angles_deg = self.get_current_joint_angles(use_deg=True)
-        ee_pos = self.get_ee_position()
+        joint_angles = self.get_current_joint_angles(use_deg=True)
         joint_forces = self.get_joint_forces()
-        angle_errors_deg = rad2deg(self.angle_error_history)
+        current_stiffness, current_damping = self.calculate_adaptive_stiffness()
+        angle_errors = rad2deg(self.angle_error_history)
         self.total_simulation_time = current_time - (SIMULATION_START_TIME or current_time)
-        adaptive_kp, adaptive_kd = calculate_load_adaptive_gains(self.current_end_load)
-
-        # 格式化打印（负载信息突出显示）
-        print("-" * 100)
-        print(f"📊 仿真统计 | 耗时: {self.total_simulation_time:.2f}s | 步数: {self.step_count:,} | FPS: {fps:5.1f}")
-        print(f"🔧 关节角度 (度): {np.round(joint_angles_deg, 1)} | 控制误差 (度): {np.round(abs(angle_errors_deg), 3)}")
-        print(f"🎯 末端位置 (m): {np.round(ee_pos, 3)} | 当前末端负载 (kg): {self.current_end_load:.2f}")
-        print(f"⚙️ 关节受力 (N·m): {np.round(joint_forces, 2)} | 最大扭矩 (N·m): {np.round(JOINT_MAX_TORQUE, 1)}")
-        print(f"📈 自适应PD参数 | kp: {adaptive_kp:.1f} | kd: {adaptive_kd:.1f}")
+
+        # 格式化打印
+        print("-" * 120)
+        print(
+            f"📊 仿真统计 | 耗时: {self.total_simulation_time:.2f}s | 步数: {self.step_count:,} | FPS: {fps:5.1f} | 复位次数: {self.error_reset_count}")
+        print(f"🔧 关节角度 (度): {np.round(joint_angles, 1)} | 控制误差 (度): {np.round(abs(angle_errors), 3)}")
+        print(
+            f"🏋️  末端负载 (kg): {self.current_end_load:.2f} | 关节受力 (N·m): {np.round(joint_forces, 2)} | 最大扭矩 (N·m): {np.round(JOINT_MAX_TORQUE, 1)}")
+        print(f"🔩 关节刚度: {np.round(current_stiffness, 1)} | 关节阻尼: {np.round(current_damping, 1)}")
         if self.overload_warning_flag:
-            print("⚠️  警告：关节接近过载状态，已自动降低控制输出！")
-        print("-" * 100)
+            print("⚠️  警告：关节过载，已启用输出限制！")
+        if np.any(self.stall_detection_flag):
+            stall_joints = [JOINT_NAMES[i] for i in range(JOINT_COUNT) if self.stall_detection_flag[i]]
+            print(f"⚠️  警告：关节{stall_joints}卡死风险，即将触发自动复位！")
+        print("-" * 120)
 
         # 重置计数器
         self.last_print_time = current_time
         self.fps_counter = 0
 
     def preset_pose(self, pose_name):
-        """预设常用姿态（负载下更平稳的姿态切换）"""
+        """预设姿态（可靠性优化：平稳切换）"""
         pose_map = {
-            'zero': [0, 0, 0, 0, 0],  # 零位
+            'zero': [0, 0, 0, 0, 0],  # 零位（安全姿态）
             'up': [0, 30, 20, 10, 0],  # 抬起姿态
             'grasp': [0, 45, 30, 20, 10]  # 抓取姿态
         }
         if pose_name not in pose_map:
-            print(f"⚠️ 无效姿态名称，支持：{list(pose_map.keys())}")
+            warning_msg = f"无效姿态名称，支持：{list(pose_map.keys())}"
+            print(f"⚠️ {warning_msg}")
+            write_reliability_log(warning_msg)
             return
         self.set_joint_angles(pose_map[pose_name], smooth=True, use_deg=True)
-        print(f"✅ 切换到{pose_name}姿态（负载自适应控制已启用）")
+        info_msg = f"切换到{pose_name}姿态，刚度自适应控制已启用"
+        print(f"✅ {info_msg}")
+        write_reliability_log(info_msg)
 
     def run(self):
-        """运行完整仿真（负载下循环逻辑优化）"""
+        """运行完整仿真（刚度+可靠性核心循环）"""
         global RUNNING
 
         if not self.init_viewer():
@@ -550,71 +714,76 @@ def run(self):
             return
 
         # 启动信息
-        print("=" * 100)
-        print("🚀 机械臂关节负载性能优化控制器 - 启动成功")
+        print("=" * 120)
+        print("🚀 机械臂关节刚度与可靠性优化控制器 - 启动成功")
         print(f"✅ 模型信息 | 关节数量: {JOINT_COUNT} | 初始末端负载: {self.current_end_load:.2f}kg")
-        print(f"✅ 仿真配置 | 控制频率: {CONTROL_FREQUENCY}Hz | 仿真步长: {SIMULATION_TIMESTEP:.3f}s")
         print(
-            f"✅ 保护配置 | 最大末端负载: {LOAD_PARAMS['max_allowed_load']}kg | 关节最大扭矩: {np.max(JOINT_MAX_TORQUE)}N·m")
+            f"✅ 刚度配置 | 基座最大刚度: {STIFFNESS_PARAMS['max_stiffness'][0]:.1f} | 末端最小刚度: {STIFFNESS_PARAMS['min_stiffness'][-1]:.1f}")
+        print(f"✅ 可靠性配置 | 控制频率: {CONTROL_FREQUENCY}Hz | 最大允许负载: {LOAD_PARAMS['max_allowed_load']}kg")
         print("📝 快捷指令:")
-        print("   - 设置末端负载: controller.set_end_effector_load(1.0) （设置1kg负载）")
-        print("   - 单关节控制: controller.move_joint(0, 90) （关节1旋转90度）")
-        print("   - 预设姿态: controller.preset_pose('up') （切换抬起姿态）")
+        print("   - 设置末端负载: controller.set_end_effector_load(1.0)")
+        print("   - 单关节控制: controller.move_joint(0, 90)")
+        print("   - 预设姿态: controller.preset_pose('up')")
         print("   - 按 Ctrl+C 优雅退出")
-        print("=" * 100)
+        print("=" * 120)
 
-        # 主循环（负载下极致效率优化）
+        # 主循环（可靠性优化：容错增强）
         while RUNNING:
             try:
                 current_time = time.time()
                 self.fps_counter += 1
                 self.step_count += 1
 
-                # 按高控制频率执行负载自适应PD控制
+                # 高频率控制更新
                 if current_time - self.last_control_time >= CONTROL_TIMESTEP:
-                    self.load_adaptive_pd_control()
+                    self.stiffness_adaptive_pd_control()  # 刚度自适应控制
+                    self.reliability_detection()  # 可靠性检测
                     self.last_control_time = current_time
 
-                # 执行仿真步（负载下容错增强）
+                # 仿真步执行
                 if self.model is not None and self.data is not None:
                     mujoco.mj_step(self.model, self.data)
 
-                # 同步Viewer
+                # 可视化同步
                 if self.viewer_ready:
                     self.viewer_inst.sync()
 
-                # 打印负载状态
-                self.print_load_status()
+                # 状态打印
+                self.print_stiffness_reliability_status()
 
-                # 动态睡眠优化，减少负载下CPU空转
+                # 动态睡眠，降低CPU占用
                 time_diff = current_time - self.last_control_time
                 if time_diff < SLEEP_TIME:
                     sleep_duration = max(0.00001, SLEEP_TIME - time_diff)
                     time.sleep(sleep_duration)
 
             except Exception as e:
-                print(f"⚠️ 仿真步异常（步数：{self.step_count}）: {e}")
+                error_msg = f"仿真步异常（步数：{self.step_count}）: {e}"
+                print(f"⚠️ {error_msg}")
+                write_reliability_log(error_msg)
                 continue
 
-        # 清理资源
+        # 资源清理
         self.cleanup()
         # 最终统计
-        print("\n" + "=" * 100)
-        print("✅ 控制器已优雅退出 - 负载仿真最终统计")
-        print(
-            f"📈 总仿真时间: {self.total_simulation_time:.2f}s | 总步数: {self.step_count:,} | 平均FPS: {self.step_count / max(1, self.total_simulation_time):.1f}")
-        print(
-            f"🎯 最终末端负载 (kg): {self.current_end_load:.2f} | 最终关节受力 (N·m): {np.round(self.get_joint_forces(), 2)}")
-        print(f"🎯 最终关节角度 (度): {np.round(self.get_current_joint_angles(), 1)}")
-        print("=" * 100)
+        final_msg = f"仿真结束 | 总耗时: {self.total_simulation_time:.2f}s | 总步数: {self.step_count:,} | 复位次数: {self.error_reset_count}"
+        print("\n" + "=" * 120)
+        print("✅ 控制器已优雅退出 - 刚度与可靠性仿真最终统计")
+        print(f"📈 {final_msg}")
+        print(f"🎯 最终末端负载 (kg): {self.current_end_load:.2f} | 最终关节刚度: {np.round(self.current_stiffness, 1)}")
+        print("=" * 120)
+        write_reliability_log(final_msg)
 
     def cleanup(self):
-        """资源清理（负载下完整释放，避免内存泄漏）"""
+        """资源清理（可靠性优化：完整释放，避免内存泄漏）"""
         if self.viewer_ready and self.viewer_inst:
             try:
                 self.viewer_inst.close()
+                write_reliability_log("Viewer资源清理完成")
             except Exception as e:
-                print(f"⚠️ Viewer关闭失败: {e}")
+                error_msg = f"Viewer关闭失败: {e}"
+                print(f"⚠️ {error_msg}")
+                write_reliability_log(error_msg)
             self.viewer_inst = None
             self.viewer_ready = False
         self.model = None
@@ -622,47 +791,57 @@ def cleanup(self):
         global RUNNING, SIMULATION_START_TIME
         RUNNING = False
         SIMULATION_START_TIME = None
+        write_reliability_log("控制器资源清理完成，仿真正常退出")
+
+    def move_joint(self, joint_idx, angle, smooth=True, use_deg=True):
+        """单独控制单个关节（容错增强）"""
+        if joint_idx < 0 or joint_idx >= JOINT_COUNT:
+            raise ValueError(f"关节索引必须在0-{JOINT_COUNT - 1}之间，当前为{joint_idx}")
+
+        current_angles = self.get_current_joint_angles(use_deg=use_deg)
+        current_angles[joint_idx] = angle
+        self.set_joint_angles(current_angles, smooth=smooth, use_deg=use_deg)
 
 
-# ====================== 负载演示函数（验证优化效果） ======================
-def load_demo(controller):
-    """负载变化演示，验证自适应控制效果"""
+# ====================== 刚度与可靠性演示函数 ======================
+def stiffness_reliability_demo(controller):
+    """演示刚度自适应与可靠性保护功能"""
 
     def demo():
         time.sleep(2)
 
-        # 演示1：初始零位（0.5kg负载）
-        print("\n🎬 演示1：切换到零位姿态（初始负载0.5kg）")
+        # 演示1：零位姿态（基准刚度）
+        print("\n🎬 演示1：切换到零位姿态，使用基准刚度")
         controller.preset_pose('zero')
         time.sleep(3)
 
-        # 演示2：切换抬起姿态（0.5kg负载）
-        print("\n🎬 演示2：切换到抬起姿态（0.5kg负载）")
+        # 演示2：抬起姿态（刚度自适应调整）
+        print("\n🎬 演示2：切换到抬起姿态，刚度随姿态自动调整")
         controller.preset_pose('up')
         time.sleep(3)
 
-        # 演示3：增加末端负载到1.5kg
-        print("\n🎬 演示3：设置末端负载为1.5kg（自适应PD控制自动生效）")
-        controller.set_end_effector_load(1.5)
-        time.sleep(2)
+        # 演示3：增加负载（刚度放大，可靠性保护启用）
+        print("\n🎬 演示3：设置末端负载为1.8kg（接近最大值，刚度自动放大）")
+        controller.set_end_effector_load(1.8)
+        time.sleep(3)
 
-        # 演示4：负载下旋转关节1（90度）
-        print("\n🎬 演示4：1.5kg负载下，关节1旋转90度（抗干扰控制，无超调）")
+        # 演示4：大角度运动（大误差下刚度进一步提升）
+        print("\n🎬 演示4：关节1旋转90度（大误差，刚度与阻尼联动优化）")
         controller.move_joint(0, 90, smooth=True, use_deg=True)
-        time.sleep(3)
+        time.sleep(4)
 
-        # 演示5：切换抓取姿态（1.5kg负载）
-        print("\n🎬 演示5：1.5kg负载下，切换到抓取姿态")
+        # 演示5：抓取姿态（全关节刚度匹配）
+        print("\n🎬 演示5：切换到抓取姿态，全关节刚度分层生效")
         controller.preset_pose('grasp')
         time.sleep(3)
 
-        # 演示6：降低负载到0.2kg
-        print("\n🎬 演示6：降低末端负载为0.2kg（PD增益自动回落）")
+        # 演示6：降低负载（刚度回落，恢复平稳）
+        print("\n🎬 演示6：降低末端负载为0.2kg（刚度回落，运动更平稳）")
         controller.set_end_effector_load(0.2)
         time.sleep(2)
 
-        # 演示7：回到零位
-        print("\n🎬 演示7：切换回零位姿态")
+        # 演示7：复位零位（可靠性演示）
+        print("\n🎬 演示7：切换回零位姿态，完成刚度与可靠性演示")
         controller.preset_pose('zero')
         time.sleep(2)
 
@@ -679,17 +858,17 @@ def demo():
 if __name__ == "__main__":
     np.seterr(all='ignore')
 
-    # 创建负载优化控制器
+    # 创建刚度与可靠性控制器
     controller = None
     try:
-        controller = ArmJointLoadOptimizedController()
+        controller = ArmStiffnessReliabilityController()
     except Exception as e:
         print(f"❌ 控制器创建失败: {e}")
         sys.exit(1)
 
-    # 运行负载演示
+    # 运行演示
     if controller is not None:
-        load_demo(controller)
+        stiffness_reliability_demo(controller)
 
     # 启动控制器
     if controller is not None:

From 931fc2c410ed032f08647f912d799c8cc756e94d Mon Sep 17 00:00:00 2001
From: XYTX0427 <481177497@qq.com>
Date: Sat, 27 Dec 2025 18:10:01 +0800
Subject: [PATCH 21/21] =?UTF-8?q?=E6=9C=BA=E6=A2=B0=E8=87=82=E5=85=B3?=
 =?UTF-8?q?=E8=8A=82=E7=B2=BE=E5=BA=A6=E6=80=A7=E8=83=BD=E4=BC=98=E5=8C=96?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/Robotic_arm/main.py | 902 ++++++++++++++++++++++++++--------------
 1 file changed, 595 insertions(+), 307 deletions(-)

diff --git a/src/Robotic_arm/main.py b/src/Robotic_arm/main.py
index 27818121d4..5146c30326 100644
--- a/src/Robotic_arm/main.py
+++ b/src/Robotic_arm/main.py
@@ -1,13 +1,8 @@
 #!/usr/bin/env python3
 # -*- coding: utf-8 -*-
 """
-机械臂关节刚度与可靠性优化控制器
-核心优化：
-1.  关节刚度分层配置+自适应调节（负载/误差驱动）
-2.  全方位可靠性保障（卡死检测/过载保护/异常复位/容错处理）
-3.  刚度-阻尼-惯量匹配优化，降低振动与干扰
-4.  全状态监控与日志记录，便于故障追溯
-5.  兼容新旧Mujoco版本，无XML语法错误
+机械臂关节精度性能优化控制器（修复geom标签viscous属性错误版）
+核心修复：移除geom标签无效viscous属性，迁移至joint标签damping属性，保证XML Schema合规
 """
 
 import sys
@@ -20,8 +15,8 @@
 import mujoco
 from datetime import datetime
 
-# ====================== 全局配置（刚度+可靠性专用） ======================
-# 系统适配（Windows优先，极致CPU优化）
+# ====================== 全局配置（精度优化专用） ======================
+# 系统适配（Windows优先，降低系统干扰影响精度）
 if os.name == 'nt':
     try:
         kernel32 = ctypes.windll.kernel32
@@ -30,7 +25,7 @@
         kernel32.SetThreadPriority(kernel32.GetCurrentThread(), 1)
     except Exception as e:
         print(f"⚠️ Windows系统优化失败（不影响核心功能）: {e}")
-    # 强制单线程，避免多线程竞争导致的控制不稳定
+    # 强制单线程，避免多线程竞争导致控制延迟，影响精度
     os.environ['OMP_NUM_THREADS'] = '1'
     os.environ['MKL_NUM_THREADS'] = '1'
     os.environ['NUMEXPR_NUM_THREADS'] = '1'
@@ -48,81 +43,109 @@
         print(f"⚠️ Mujoco Viewer导入失败（无法可视化）: {e}")
 
 # 核心参数配置
-# 关节基础配置（按重要性分层：1>2>3>4>5）
 JOINT_COUNT = 5
 JOINT_NAMES = ["joint1", "joint2", "joint3", "joint4", "joint5"]
 JOINT_LIMITS_RAD = np.array([
-    [-np.pi, np.pi],  # joint1（基座，最高刚度）
-    [-np.pi / 2, np.pi / 2],  # joint2（大臂，高刚度）
-    [-np.pi / 2, np.pi / 2],  # joint3（中臂，中高刚度）
-    [-np.pi / 2, np.pi / 2],  # joint4（小臂，中刚度）
-    [-np.pi / 2, np.pi / 2],  # joint5（末端，低刚度）
+    [-np.pi, np.pi],  # joint1（基座）
+    [-np.pi / 2, np.pi / 2],  # joint2（大臂）
+    [-np.pi / 2, np.pi / 2],  # joint3（中臂）
+    [-np.pi / 2, np.pi / 2],  # joint4（小臂）
+    [-np.pi / 2, np.pi / 2],  # joint5（末端）
 ], dtype=np.float64)
 JOINT_MAX_VELOCITY_RAD = np.array([1.0, 0.8, 0.8, 0.6, 0.6], dtype=np.float64)
-JOINT_MAX_TORQUE = np.array([15.0, 12.0, 10.0, 8.0, 5.0], dtype=np.float64)  # 最大扭矩（可靠性保护）
+JOINT_MAX_ACCEL_RAD = np.array([2.0, 1.6, 1.6, 1.2, 1.2], dtype=np.float64)  # 最大加速度（精度优化：限制加减速避免超调）
+JOINT_MAX_TORQUE = np.array([15.0, 12.0, 10.0, 8.0, 5.0], dtype=np.float64)
 
-# 关节刚度分层配置（核心优化：按关节层级设定基准刚度）
+# 刚度配置（兼容之前的优化，不影响精度）
 STIFFNESS_PARAMS = {
-    'base_stiffness': np.array([200.0, 180.0, 150.0, 120.0, 80.0]),  # 各关节基准刚度
-    'load_stiffness_gain': 1.8,  # 负载下刚度放大系数
-    'error_stiffness_gain': 1.5,  # 大误差下刚度放大系数
-    'min_stiffness': np.array([100.0, 90.0, 75.0, 60.0, 40.0]),  # 最小允许刚度
-    'max_stiffness': np.array([300.0, 270.0, 225.0, 180.0, 120.0]),  # 最大允许刚度
-    'stiffness_smoothing': 0.05,  # 刚度变化平滑系数，防止突变
+    'base_stiffness': np.array([200.0, 180.0, 150.0, 120.0, 80.0]),
+    'load_stiffness_gain': 1.8,
+    'error_stiffness_gain': 1.5,
+    'min_stiffness': np.array([100.0, 90.0, 75.0, 60.0, 40.0]),
+    'max_stiffness': np.array([300.0, 270.0, 225.0, 180.0, 120.0]),
+    'stiffness_smoothing': 0.05,
 }
 
-# 阻尼与惯量匹配配置（刚度配套优化，提升可靠性）
+# 阻尼与惯量配置（优化：将粘性摩擦参数整合至joint damping）
 DAMPING_INERTIA_PARAMS = {
-    'base_damping': np.array([8.0, 7.0, 6.0, 5.0, 3.0]),  # 基准阻尼（与刚度匹配）
-    'damping_stiffness_ratio': 0.04,  # 阻尼-刚度匹配比，保证运动平稳
-    'armature_inertia': np.array([0.5, 0.4, 0.3, 0.2, 0.1]),  # 关节惯量补偿
+    'base_damping': np.array([8.0, 7.0, 6.0, 5.0, 3.0]),  # 基础阻尼（对应原粘性摩擦需求）
+    'viscous_damping_gain': np.array([1.2, 1.1, 1.1, 1.0, 1.0]),  # 粘性阻尼增益，补充原有viscous效果
+    'damping_stiffness_ratio': 0.04,
+    'armature_inertia': np.array([0.5, 0.4, 0.3, 0.2, 0.1]),
 }
 
-# 仿真配置（可靠性优化：小步长提升控制稳定性）
-SIMULATION_TIMESTEP = 0.001  # 更小步长，降低控制误差
-CONTROL_FREQUENCY = 1000  # 更高控制频率，提升响应可靠性
+# 仿真配置（精度优化：更小步长+更高控制频率，提升控制分辨率）
+SIMULATION_TIMESTEP = 0.0005  # 微步长，降低离散化误差
+CONTROL_FREQUENCY = 2000  # 高频控制，提升响应精度
 CONTROL_TIMESTEP = 1.0 / CONTROL_FREQUENCY
 FPS = 60
 SLEEP_TIME = 1.0 / FPS
-EPS = 1e-8
+EPS = 1e-9  # 更小误差阈值，提升精度判断准确性
 RUNNING = True
 SIMULATION_START_TIME = None
 
-# PD控制参数（与刚度/阻尼联动）
-PD_PARAMS = {
-    'kp_base': 80.0,
-    'kd_base': 5.0,
-    'kp_load_gain': 1.5,
-    'kd_load_gain': 1.2,
-    'max_vel': JOINT_MAX_VELOCITY_RAD.copy()
+# 高精度PD+前馈控制参数（核心精度优化）
+PRECISION_PD_PARAMS = {
+    'kp_base': 120.0,  # 更高比例增益，提升静态定位精度
+    'kd_base': 8.0,  # 优化阻尼增益，抑制振动超调
+    'kp_load_gain': 1.8,  # 负载下增益放大，维持精度
+    'kd_load_gain': 1.5,  # 负载下阻尼优化，防止震荡
+    'ff_gain': 0.7,  # 前馈增益，补偿动态误差
+    'max_vel': JOINT_MAX_VELOCITY_RAD.copy(),
+    'max_accel': JOINT_MAX_ACCEL_RAD.copy()
 }
 
-# 负载配置（与刚度联动优化）
+# 负载配置
 LOAD_PARAMS = {
     'end_effector_mass': 0.5,
     'joint_loads': np.zeros(JOINT_COUNT),
     'max_allowed_load': 2.0,
-    'load_smoothing_factor': 0.1
+    'load_smoothing_factor': 0.05  # 更小平滑系数，提升负载检测精度
 }
 
-# 可靠性保护配置（核心：卡死/过载/异常检测参数）
+# 误差补偿配置（核心精度优化：移除geom的viscous配置，保留摩擦系数用于误差计算）
+ERROR_COMPENSATION_PARAMS = {
+    'backlash_error': np.array([0.001, 0.001, 0.002, 0.002, 0.003]),  # 关节间隙误差（rad）
+    'friction_coeff': np.array([0.1, 0.08, 0.08, 0.06, 0.06]),  # 静摩擦力系数（仅用于误差补偿计算）
+    'gravity_compensation': True,  # 是否启用重力误差补偿
+    'comp_smoothing': 0.02,  # 误差补偿平滑系数，避免突变
+}
+
+# 轨迹规划配置（精度优化：梯形速度规划参数）
+TRAJECTORY_PLANNING_PARAMS = {
+    'traj_type': 'trapezoidal',  # 梯形速度规划，无超调
+    'acceleration_time': 0.2,  # 加速时间
+    'deceleration_time': 0.2,  # 减速时间
+    'position_tol': 1e-5,  # 位置公差（rad），高精度定位判定
+    'velocity_tol': 1e-4  # 速度公差（rad/s），平稳停止判定
+}
+
+# 精度监测配置
+PRECISION_MONITOR_PARAMS = {
+    'log_precision_data': True,
+    'log_path': 'arm_joint_precision_log.txt',
+    'max_allowed_position_error': np.deg2rad(0.1),  # 最大允许定位误差（0.1度）
+    'max_allowed_trajectory_error': np.deg2rad(0.2)  # 最大允许轨迹跟踪误差（0.2度）
+}
+
+# 可靠性配置（兼容之前的优化）
 RELIABILITY_PARAMS = {
-    'stall_detection_threshold': 0.01,  # 关节卡死判定阈值（速度<此值且扭矩>90%）
-    'stall_duration_threshold': 1.0,  # 卡死持续时间（秒），触发复位
-    'overload_duration_threshold': 2.0,  # 过载持续时间，触发保护
-    'max_angle_error': np.deg2rad(10.0),  # 最大允许角度误差，触发异常报警
-    'auto_reset_on_error': True,  # 是否自动复位异常关节
-    'log_reliability_data': True,  # 是否记录可靠性日志
-    'log_path': 'arm_reliability_log.txt'  # 日志保存路径
+    'stall_detection_threshold': 0.005,  # 更高灵敏度，提升异常检测精度
+    'stall_duration_threshold': 1.0,
+    'overload_duration_threshold': 2.0,
+    'max_angle_error': np.deg2rad(10.0),
+    'auto_reset_on_error': True,
+    'log_reliability_data': True,
+    'reliability_log_path': 'arm_reliability_log.txt'
 }
 
 
-# ====================== 信号处理（可靠性优化：优雅退出） ======================
+# ====================== 信号处理（优雅退出，避免精度数据丢失） ======================
 def signal_handler(sig, frame):
     global RUNNING
     if not RUNNING:
         sys.exit(0)
-    print("\n⚠️ 收到退出信号，正在优雅退出（保存可靠性日志+清理资源）...")
+    print("\n⚠️ 收到退出信号，正在优雅退出（保存精度日志+清理资源）...")
     RUNNING = False
 
 
@@ -130,9 +153,9 @@ def signal_handler(sig, frame):
 signal.signal(signal.SIGTERM, signal_handler)
 
 
-# ====================== 工具函数（刚度+可靠性专用） ======================
+# ====================== 工具函数（精度优化专用） ======================
 def get_mujoco_id(model, obj_type, name):
-    """兼容所有Mujoco版本的ID查询（容错增强，提升可靠性）"""
+    """兼容所有Mujoco版本的ID查询（容错增强，提升精度稳定性）"""
     if model is None:
         return -1
     type_map = {
@@ -152,7 +175,7 @@ def get_mujoco_id(model, obj_type, name):
 
 
 def deg2rad(degrees):
-    """角度转弧度（容错增强，可靠性保障）"""
+    """角度转弧度（高精度转换，容错增强）"""
     try:
         degrees = np.array(degrees, dtype=np.float64)
         return np.deg2rad(degrees)
@@ -162,7 +185,7 @@ def deg2rad(degrees):
 
 
 def rad2deg(radians):
-    """弧度转角度（容错增强，可靠性保障）"""
+    """弧度转角度（高精度转换，容错增强）"""
     try:
         radians = np.array(radians, dtype=np.float64)
         return np.rad2deg(radians)
@@ -171,8 +194,20 @@ def rad2deg(radians):
         return 0.0 if np.isscalar(radians) else np.zeros(JOINT_COUNT, dtype=np.float64)
 
 
-def write_reliability_log(content, log_path=RELIABILITY_PARAMS['log_path']):
-    """写入可靠性日志（核心：记录异常状态，便于追溯）"""
+def write_precision_log(content, log_path=PRECISION_MONITOR_PARAMS['log_path']):
+    """写入精度日志（记录误差数据，便于精度分析与优化）"""
+    if not PRECISION_MONITOR_PARAMS['log_precision_data']:
+        return
+    try:
+        with open(log_path, 'a', encoding='utf-8') as f:
+            timestamp = datetime.now().strftime('%Y-%m-%d %H:%M:%S.%f')[:-3]  # 毫秒级时间戳，提升日志精度
+            f.write(f"[{timestamp}] {content}\n")
+    except Exception as e:
+        print(f"⚠️ 写入精度日志失败: {e}")
+
+
+def write_reliability_log(content, log_path=RELIABILITY_PARAMS['reliability_log_path']):
+    """写入可靠性日志（兼容之前的优化）"""
     if not RELIABILITY_PARAMS['log_reliability_data']:
         return
     try:
@@ -183,95 +218,189 @@ def write_reliability_log(content, log_path=RELIABILITY_PARAMS['log_path']):
         print(f"⚠️ 写入可靠性日志失败: {e}")
 
 
-# ====================== 机械臂模型生成（刚度配置+无XML错误） ======================
-def create_arm_model_with_stiffness():
+def trapezoidal_velocity_planner(start_pos, target_pos, max_vel, max_accel, dt):
+    """
+    梯形速度规划（精度优化核心：无超调平滑轨迹生成）
+    :param start_pos: 起始位置（rad）
+    :param target_pos: 目标位置（rad）
+    :param max_vel: 最大速度（rad/s）
+    :param max_accel: 最大加速度（rad/s²）
+    :param dt: 时间步长（s）
+    :return: 规划的位置序列、速度序列
+    """
+    pos_error = target_pos - start_pos
+    total_distance = abs(pos_error)
+    if total_distance < TRAJECTORY_PLANNING_PARAMS['position_tol']:
+        return np.array([target_pos]), np.array([0.0])
+
+    # 计算梯形速度规划关键参数
+    accel_phase_vel = max_vel
+    accel_phase_dist = (accel_phase_vel ** 2) / (2 * max_accel)
+    total_accel_decel_dist = 2 * accel_phase_dist
+
+    # 判定运动阶段（是否存在匀速阶段）
+    pos_list = []
+    vel_list = []
+    current_pos = start_pos
+    current_vel = 0.0
+    direction = np.sign(pos_error)
+
+    if total_distance <= total_accel_decel_dist:
+        # 无匀速阶段：加速到最大速度前即开始减速
+        max_reached_vel = np.sqrt(total_distance * max_accel)
+        accel_time = max_reached_vel / max_accel
+        total_time = 2 * accel_time
+
+        t = 0.0
+        while t < total_time + dt:
+            if t <= accel_time:
+                # 加速阶段
+                current_vel = max_accel * t * direction
+                current_pos = start_pos + 0.5 * max_accel * (t ** 2) * direction
+            else:
+                # 减速阶段
+                delta_t = t - accel_time
+                current_vel = (max_reached_vel - max_accel * delta_t) * direction
+                current_pos = start_pos + (max_reached_vel * accel_time - 0.5 * max_accel * (delta_t ** 2)) * direction
+            pos_list.append(current_pos)
+            vel_list.append(current_vel)
+            t += dt
+    else:
+        # 有匀速阶段：加速→匀速→减速
+        accel_time = max_vel / max_accel
+        uniform_dist = total_distance - total_accel_decel_dist
+        uniform_time = uniform_dist / max_vel
+        total_time = 2 * accel_time + uniform_time
+
+        t = 0.0
+        while t < total_time + dt:
+            if t <= accel_time:
+                # 加速阶段
+                current_vel = max_accel * t * direction
+                current_pos = start_pos + 0.5 * max_accel * (t ** 2) * direction
+            elif t <= accel_time + uniform_time:
+                # 匀速阶段
+                current_vel = max_vel * direction
+                delta_t = t - accel_time
+                current_pos = start_pos + (accel_phase_dist + max_vel * delta_t) * direction
+            else:
+                # 减速阶段
+                delta_t = t - (accel_time + uniform_time)
+                current_vel = (max_vel - max_accel * delta_t) * direction
+                delta_pos = accel_phase_dist - 0.5 * max_accel * (delta_t ** 2)
+                current_pos = start_pos + (total_distance - delta_pos) * direction
+            pos_list.append(current_pos)
+            vel_list.append(current_vel)
+            t += dt
+
+    # 最后强制设置为目标位置，消除累积误差
+    pos_list[-1] = target_pos
+    vel_list[-1] = 0.0
+    return np.array(pos_list), np.array(vel_list)
+
+
+# ====================== 机械臂模型生成（修复geom标签viscous属性，高精度配置） ======================
+def create_arm_model_with_precision():
     """
-    生成带关节刚度配置的机械臂XML模型（兼容所有Mujoco版本）
-    1.  按关节层级配置基准刚度、阻尼、惯量，实现刚度分层
-    2.  移除所有违规XML属性，保证无语法错误
-    3.  几何与质量配置优化，提升仿真可靠性
+    生成高精度机械臂XML模型（彻底修复Schema违规错误，兼容所有Mujoco版本）
+    核心修复：
+    1.  移除所有geom标签的viscous属性（该属性不被geom支持，消除Schema违规）
+    2.  保留geom标签的friction属性（3个值，合法支持静摩擦功能）
+    3.  将粘性摩擦需求迁移至joint标签的damping属性（合法归属），通过粘性阻尼增益补充效果
+    4.  优化joint标签的damping参数，确保与原有粘性摩擦需求一致
     """
     end_effector_mass = LOAD_PARAMS['end_effector_mass']
-    # 连杆geom质量（兼容新旧Mujoco版本）
     link1_geom_mass = 0.8
     link2_geom_mass = 0.6
     link3_geom_mass = 0.6
     link4_geom_mass = 0.4
     link5_geom_mass = 0.2
 
-    # 从配置中提取关节参数（刚度/阻尼/惯量）
     base_stiffness = STIFFNESS_PARAMS['base_stiffness']
     base_damping = DAMPING_INERTIA_PARAMS['base_damping']
+    viscous_damping_gain = DAMPING_INERTIA_PARAMS['viscous_damping_gain']
     armature_inertia = DAMPING_INERTIA_PARAMS['armature_inertia']
+    friction_coeffs = ERROR_COMPENSATION_PARAMS['friction_coeff']
+
+    # 计算最终关节阻尼（基础阻尼 + 粘性阻尼增益，等效原有viscous效果）
+    joint_damping = base_damping * viscous_damping_gain
 
     xml = f"""
-<mujoco model="arm_with_stiffness_reliability">
+<mujoco model="arm_with_precision_optimization">
+    <!-- 修复1：compiler标签仅保留合法属性 -->
     <compiler angle="radian" inertiafromgeom="true" autolimits="true"/>
-    <option timestep="{SIMULATION_TIMESTEP}" gravity="0 0 -9.81" iterations="50" tolerance="1e-7"/>
+    <!-- tolerance属性合法存放于option标签 -->
+    <option timestep="{SIMULATION_TIMESTEP}" gravity="0 0 -9.81" iterations="100" tolerance="1e-9"/>
 
-    <!-- 关节刚度+阻尼+惯量基础配置（分层设定，提升可靠性） -->
+    <!-- 高精度默认配置：修复2：移除geom的viscous，保留friction；优化joint的damping -->
     <default>
-        <joint type="hinge" armature="{armature_inertia[0]}" damping="{base_damping[0]}" limited="true" margin="0.01"/>
+        <!-- joint标签：配置合法属性，damping整合基础阻尼+粘性阻尼效果 -->
+        <joint type="hinge" armature="{armature_inertia[0]}" damping="{joint_damping[0]}" 
+               limited="true" margin="0.001"/> <!-- 更小间隙，提升精度 -->
         <motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="100"/>
-        <geom contype="1" conaffinity="1" rgba="0.2 0.8 0.2 1"/>
+        <!-- geom标签：仅保留合法属性，移除viscous，保留friction（3个值） -->
+        <geom contype="1" conaffinity="1" rgba="0.2 0.8 0.2 1" solref="0.01 1" solimp="0.9 0.95 0.001"
+              friction="{friction_coeffs[0]} {friction_coeffs[0]} {friction_coeffs[0]}"/> <!-- 高精度接触与静摩擦参数 -->
     </default>
 
-    <!-- 负载与刚度可视化材质 -->
+    <!-- 材质配置 -->
     <asset>
         <material name="load_material" rgba="1.0 0.0 0.0 0.8"/>
-        <material name="high_stiffness_material" rgba="0.0 0.8 0.0 0.8"/>
-        <material name="low_stiffness_material" rgba="0.8 0.0 0.0 0.8"/>
+        <material name="high_precision_material" rgba="0.0 0.8 0.0 0.8"/>
+        <material name="end_effector_material" rgba="0.8 0.2 0.2 1"/>
     </asset>
 
     <worldbody>
-        <!-- 地面（简化几何，提升仿真效率） -->
-        <geom name="floor" type="plane" size="3 3 0.1" pos="0 0 0" rgba="0.8 0.8 0.8 1"/>
+        <!-- 地面（高精度几何，降低接触误差） -->
+        <geom name="floor" type="plane" size="3 3 0.1" pos="0 0 0" rgba="0.8 0.8 0.8 1" solref="0.01 1"/>
 
-        <!-- 机械臂基座（joint1：最高刚度） -->
+        <!-- 机械臂基座（joint1） -->
         <body name="base" pos="0 0 0">
             <geom name="base_geom" type="cylinder" size="0.1 0.1" rgba="0.2 0.2 0.8 1"/>
 
-            <!-- 关节1（基座关节，最高刚度+惯量） -->
+            <!-- 修复3：joint标签配置优化后的damping（整合粘性阻尼效果），无违规属性 -->
             <joint name="joint1" type="hinge" axis="0 0 1" pos="0 0 0.1" 
                    range="{JOINT_LIMITS_RAD[0, 0]} {JOINT_LIMITS_RAD[0, 1]}" 
-                   armature="{armature_inertia[0]}" damping="{base_damping[0]}"/>
+                   armature="{armature_inertia[0]}" damping="{joint_damping[0]}"/>
             <body name="link1" pos="0 0 0.1">
+                <!-- 修复4：geom标签移除viscous，仅保留friction（3个值），合法合规 -->
                 <geom name="link1_geom" type="cylinder" size="0.04 0.18" mass="{link1_geom_mass}"
-                      material="high_stiffness_material"/>
+                      material="high_precision_material"
+                      friction="{friction_coeffs[1]} {friction_coeffs[1]} {friction_coeffs[1]}"/>
 
-                <!-- 关节2（大臂关节，高刚度） -->
                 <joint name="joint2" type="hinge" axis="0 1 0" pos="0 0 0.18" 
                        range="{JOINT_LIMITS_RAD[1, 0]} {JOINT_LIMITS_RAD[1, 1]}" 
-                       armature="{armature_inertia[1]}" damping="{base_damping[1]}"/>
+                       armature="{armature_inertia[1]}" damping="{joint_damping[1]}"/>
                 <body name="link2" pos="0 0 0.18">
                     <geom name="link2_geom" type="cylinder" size="0.04 0.18" mass="{link2_geom_mass}"
-                          material="high_stiffness_material"/>
+                          material="high_precision_material"
+                          friction="{friction_coeffs[2]} {friction_coeffs[2]} {friction_coeffs[2]}"/>
 
-                    <!-- 关节3（中臂关节，中高刚度） -->
                     <joint name="joint3" type="hinge" axis="0 1 0" pos="0 0 0.18" 
                            range="{JOINT_LIMITS_RAD[2, 0]} {JOINT_LIMITS_RAD[2, 1]}" 
-                           armature="{armature_inertia[2]}" damping="{base_damping[2]}"/>
+                           armature="{armature_inertia[2]}" damping="{joint_damping[2]}"/>
                     <body name="link3" pos="0 0 0.18">
-                        <geom name="link3_geom" type="cylinder" size="0.04 0.18" mass="{link3_geom_mass}"/>
+                        <geom name="link3_geom" type="cylinder" size="0.04 0.18" mass="{link3_geom_mass}"
+                              friction="{friction_coeffs[3]} {friction_coeffs[3]} {friction_coeffs[3]}"/>
 
-                        <!-- 关节4（小臂关节，中刚度） -->
                         <joint name="joint4" type="hinge" axis="0 1 0" pos="0 0 0.18" 
                                range="{JOINT_LIMITS_RAD[3, 0]} {JOINT_LIMITS_RAD[3, 1]}" 
-                               armature="{armature_inertia[3]}" damping="{base_damping[3]}"/>
+                               armature="{armature_inertia[3]}" damping="{joint_damping[3]}"/>
                         <body name="link4" pos="0 0 0.18">
-                            <geom name="link4_geom" type="cylinder" size="0.04 0.18" mass="{link4_geom_mass}"/>
+                            <geom name="link4_geom" type="cylinder" size="0.04 0.18" mass="{link4_geom_mass}"
+                                  friction="{friction_coeffs[3]} {friction_coeffs[3]} {friction_coeffs[3]}"/>
 
-                            <!-- 关节5（末端关节，低刚度） -->
                             <joint name="joint5" type="hinge" axis="0 1 0" pos="0 0 0.18" 
                                    range="{JOINT_LIMITS_RAD[4, 0]} {JOINT_LIMITS_RAD[4, 1]}" 
-                                   armature="{armature_inertia[4]}" damping="{base_damping[4]}"/>
+                                   armature="{armature_inertia[4]}" damping="{joint_damping[4]}"/>
                             <body name="link5" pos="0 0 0.18">
                                 <geom name="link5_geom" type="cylinder" size="0.03 0.09" mass="{link5_geom_mass}"
-                                      material="low_stiffness_material" rgba="0.8 0.2 0.2 1"/>
+                                      material="end_effector_material"
+                                      friction="{friction_coeffs[4]} {friction_coeffs[4]} {friction_coeffs[4]}"/>
 
-                                <!-- 末端执行器（带负载，兼容动态调整） -->
+                                <!-- 末端执行器（高精度负载配置） -->
                                 <body name="end_effector" pos="0 0 0.09">
-                                    <site name="ee_site" pos="0 0 0" size="0.01"/>
+                                    <site name="ee_site" pos="0 0 0" size="0.005"/> <!-- 更小站点，提升定位精度 -->
                                     <geom name="load_geom" type="sphere" size="0.04" mass="{end_effector_mass}" 
                                           rgba="1.0 0.0 0.0 0.8" material="load_material"/>
                                 </body>
@@ -296,19 +425,21 @@ def create_arm_model_with_stiffness():
     return xml
 
 
-# ====================== 核心控制器类（刚度+可靠性优化） ======================
-class ArmStiffnessReliabilityController:
+# ====================== 核心控制器类（关节精度性能优化） ======================
+class ArmJointPrecisionOptimizationController:
     def __init__(self):
-        # 模型与数据初始化（容错增强，提升可靠性）
+        # 模型与数据初始化（高精度配置）
         self.model = None
         self.data = None
         try:
-            self.model = mujoco.MjModel.from_xml_string(create_arm_model_with_stiffness())
+            self.model = mujoco.MjModel.from_xml_string(create_arm_model_with_precision())
             self.data = mujoco.MjData(self.model)
-            write_reliability_log("模型初始化成功，刚度与可靠性配置加载完成")
+            write_precision_log("高精度模型初始化成功，geom viscous属性修复完成，精度优化配置加载完毕")
+            write_reliability_log("高精度模型初始化成功，geom viscous属性修复完成，精度优化配置加载完毕")
         except Exception as e:
-            error_msg = f"带刚度配置模型初始化失败: {e}"
+            error_msg = f"高精度模型初始化失败: {e}"
             print(f"❌ {error_msg}")
+            write_precision_log(error_msg)
             write_reliability_log(error_msg)
             global RUNNING
             RUNNING = False
@@ -329,39 +460,55 @@ def __init__(self):
         self.step_count = 0
         self.total_simulation_time = 0.0
 
-        # 刚度相关状态（核心：平滑刚度记录）
+        # 精度相关核心状态
         self.current_stiffness = STIFFNESS_PARAMS['base_stiffness'].copy()
-        self.current_damping = DAMPING_INERTIA_PARAMS['base_damping'].copy()
+        self.current_damping = DAMPING_INERTIA_PARAMS['base_damping'].copy() * DAMPING_INERTIA_PARAMS[
+            'viscous_damping_gain']
         self.target_angles_rad = np.zeros(JOINT_COUNT, dtype=np.float64)
+        self.planned_positions = np.zeros((1, JOINT_COUNT), dtype=np.float64)  # 规划位置序列
+        self.planned_velocities = np.zeros((1, JOINT_COUNT), dtype=np.float64)  # 规划速度序列
+        self.traj_step_idx = 0  # 轨迹步骤索引
+        self.position_error = np.zeros(JOINT_COUNT, dtype=np.float64)  # 当前定位误差
+        self.trajectory_error = np.zeros(JOINT_COUNT, dtype=np.float64)  # 当前轨迹跟踪误差
+        self.max_position_error = np.zeros(JOINT_COUNT, dtype=np.float64)  # 最大定位误差
+        self.max_trajectory_error = np.zeros(JOINT_COUNT, dtype=np.float64)  # 最大轨迹跟踪误差
 
         # 负载与受力状态
         self.current_end_load = LOAD_PARAMS['end_effector_mass']
         self.smoothed_joint_forces = np.zeros(JOINT_COUNT, dtype=np.float64)
         self.angle_error_history = np.zeros(JOINT_COUNT, dtype=np.float64)
 
-        # 可靠性保护状态（核心：卡死/过载/异常检测）
+        # 可靠性状态（兼容之前的优化）
         self.overload_warning_flag = False
         self.stall_detection_flag = np.zeros(JOINT_COUNT, dtype=bool)
         self.stall_duration = np.zeros(JOINT_COUNT, dtype=np.float64)
         self.overload_duration = np.zeros(JOINT_COUNT, dtype=np.float64)
-        self.error_reset_count = 0  # 异常复位计数
+        self.error_reset_count = 0
 
-        # 初始化关节角度
+        # 误差补偿状态
+        self.compensated_error = np.zeros(JOINT_COUNT, dtype=np.float64)
+        self.gravity_compensation_torque = np.zeros(JOINT_COUNT, dtype=np.float64)
+
+        # 初始化关节角度与轨迹
         try:
             self.set_joint_angles(np.zeros(JOINT_COUNT), smooth=False, use_deg=False)
-            write_reliability_log("关节角度初始化成功，零位校准完成")
+            self.plan_trajectory(np.zeros(JOINT_COUNT), np.zeros(JOINT_COUNT))
+            write_precision_log("关节零位校准完成，初始轨迹规划成功")
+            write_reliability_log("关节零位校准完成，初始轨迹规划成功")
         except Exception as e:
-            error_msg = f"初始化关节角度失败: {e}"
+            error_msg = f"初始化关节角度或轨迹失败: {e}"
             print(f"⚠️ {error_msg}")
+            write_precision_log(error_msg)
             write_reliability_log(error_msg)
 
         # 全局仿真开始时间
         global SIMULATION_START_TIME
         SIMULATION_START_TIME = time.time()
-        write_reliability_log(f"仿真启动，控制频率：{CONTROL_FREQUENCY}Hz，步长：{SIMULATION_TIMESTEP}s")
+        write_precision_log(f"高精度仿真启动，控制频率：{CONTROL_FREQUENCY}Hz，步长：{SIMULATION_TIMESTEP}s")
+        write_reliability_log(f"高精度仿真启动，控制频率：{CONTROL_FREQUENCY}Hz，步长：{SIMULATION_TIMESTEP}s")
 
     def get_current_joint_angles(self, use_deg=True):
-        """获取当前关节角度（容错增强）"""
+        """获取当前关节角度（高精度采集，容错增强）"""
         if self.data is None:
             return np.zeros(JOINT_COUNT, dtype=np.float64)
         current_rad = np.array([self.data.qpos[jid] if jid >= 0 else 0 for jid in self.joint_ids], dtype=np.float64)
@@ -369,87 +516,248 @@ def get_current_joint_angles(self, use_deg=True):
             return rad2deg(current_rad)
         return current_rad
 
+    def get_current_joint_velocities(self, use_deg=True):
+        """获取当前关节速度（高精度采集，用于速度闭环控制）"""
+        if self.data is None:
+            return np.zeros(JOINT_COUNT, dtype=np.float64)
+        current_vel_rad = np.array([self.data.qvel[jid] if jid >= 0 else 0 for jid in self.joint_ids], dtype=np.float64)
+        if use_deg:
+            return rad2deg(current_vel_rad)
+        return current_vel_rad
+
     def get_joint_forces(self):
-        """获取关节实时受力（可靠性监控核心）"""
+        """获取关节实时受力（高精度平滑，避免抖动影响精度）"""
         if self.data is None:
             return np.zeros(JOINT_COUNT, dtype=np.float64)
         joint_forces = np.zeros(JOINT_COUNT, dtype=np.float64)
         for i, jid in enumerate(self.joint_ids):
             if jid >= 0:
                 raw_force = abs(self.data.qfrc_actuator[jid])
-                # 平滑受力，避免抖动影响检测
                 self.smoothed_joint_forces[i] = (1 - LOAD_PARAMS['load_smoothing_factor']) * self.smoothed_joint_forces[
                     i] + \
                                                 LOAD_PARAMS['load_smoothing_factor'] * raw_force
                 joint_forces[i] = self.smoothed_joint_forces[i]
         return joint_forces
 
-    def calculate_adaptive_stiffness(self):
+    def calculate_error_compensation(self):
+        """
+        核心精度优化：多维度误差补偿计算
+        1.  关节间隙误差补偿
+        2.  摩擦力误差补偿（静摩擦，基于friction_coeff）
+        3.  重力误差补偿
+        """
+        current_angles = self.get_current_joint_angles(use_deg=False)
+        current_vels = self.get_current_joint_velocities(use_deg=False)
+        current_forces = self.get_joint_forces()
+
+        # 1. 关节间隙误差补偿（根据运动方向补偿间隙）
+        backlash_comp = np.zeros(JOINT_COUNT, dtype=np.float64)
+        for i in range(JOINT_COUNT):
+            if abs(current_vels[i]) > TRAJECTORY_PLANNING_PARAMS['velocity_tol']:
+                # 运动时，根据速度方向补偿间隙
+                backlash_comp[i] = ERROR_COMPENSATION_PARAMS['backlash_error'][i] * np.sign(current_vels[i])
+            else:
+                # 静止时，补偿当前误差方向的间隙
+                backlash_comp[i] = ERROR_COMPENSATION_PARAMS['backlash_error'][i] * np.sign(self.position_error[i])
+
+        # 2. 摩擦力误差补偿（仅静摩擦，基于合法的friction_coeff）
+        friction_comp = np.zeros(JOINT_COUNT, dtype=np.float64)
+        for i in range(JOINT_COUNT):
+            # 静摩擦力补偿（速度为零时）
+            if abs(current_vels[i]) < TRAJECTORY_PLANNING_PARAMS['velocity_tol']:
+                friction_comp[i] = ERROR_COMPENSATION_PARAMS['friction_coeff'][i] * np.sign(self.position_error[i])
+
+        # 3. 重力误差补偿（简化版，根据关节角度补偿重力扭矩）
+        gravity_comp = np.zeros(JOINT_COUNT, dtype=np.float64)
+        if ERROR_COMPENSATION_PARAMS['gravity_compensation']:
+            for i in range(JOINT_COUNT):
+                gravity_comp[i] = 0.5 * np.sin(current_angles[i]) * self.current_end_load  # 简化重力补偿模型
+
+        # 总误差补偿（平滑处理，避免突变）
+        total_comp = backlash_comp + friction_comp + gravity_comp
+        self.compensated_error = (1 - ERROR_COMPENSATION_PARAMS['comp_smoothing']) * self.compensated_error + \
+                                 ERROR_COMPENSATION_PARAMS['comp_smoothing'] * total_comp
+
+        # 重力补偿扭矩（直接用于控制信号补偿）
+        self.gravity_compensation_torque = gravity_comp * 0.8  # 重力扭矩补偿系数
+
+        return self.compensated_error, self.gravity_compensation_torque
+
+    def plan_trajectory(self, start_angles, target_angles, use_deg=True):
         """
-        核心优化：计算自适应关节刚度
-        1.  根据负载大小动态调整刚度
-        2.  根据角度误差动态调整刚度
-        3.  刚度限幅+平滑处理，保证可靠性
-        4.  阻尼与刚度匹配，降低振动
+        精度优化：规划高精度平滑轨迹（梯形速度规划）
+        :param start_angles: 起始角度
+        :param target_angles: 目标角度
+        :param use_deg: 是否为角度单位
         """
-        # 1. 负载归一化
+        start_angles_rad = self.clamp_joint_angles(start_angles, use_deg=use_deg)
+        target_angles_rad = self.clamp_joint_angles(target_angles, use_deg=use_deg)
+
+        # 为每个关节规划梯形速度轨迹
+        joint_planned_pos = []
+        joint_planned_vel = []
+        max_traj_length = 0
+        for i in range(JOINT_COUNT):
+            pos_traj, vel_traj = trapezoidal_velocity_planner(
+                start_angles_rad[i],
+                target_angles_rad[i],
+                PRECISION_PD_PARAMS['max_vel'][i],
+                PRECISION_PD_PARAMS['max_accel'][i],
+                CONTROL_TIMESTEP
+            )
+            joint_planned_pos.append(pos_traj)
+            joint_planned_vel.append(vel_traj)
+            if len(pos_traj) > max_traj_length:
+                max_traj_length = len(pos_traj)
+
+        # 统一轨迹长度（补零）
+        for i in range(JOINT_COUNT):
+            if len(joint_planned_pos[i]) < max_traj_length:
+                pad_length = max_traj_length - len(joint_planned_pos[i])
+                joint_planned_pos[i] = np.pad(joint_planned_pos[i], (0, pad_length), 'constant',
+                                              constant_values=target_angles_rad[i])
+                joint_planned_vel[i] = np.pad(joint_planned_vel[i], (0, pad_length), 'constant', constant_values=0.0)
+
+        # 转换为二维数组
+        self.planned_positions = np.array(joint_planned_pos).T
+        self.planned_velocities = np.array(joint_planned_vel).T
+        self.traj_step_idx = 0
+        self.target_angles_rad = target_angles_rad.copy()
+
+        info_msg = f"轨迹规划完成：从{np.round(rad2deg(start_angles_rad), 2)}度到{np.round(rad2deg(target_angles_rad), 2)}度，轨迹长度：{max_traj_length}步"
+        print(f"✅ {info_msg}")
+        write_precision_log(info_msg)
+
+    def precision_adaptive_pd_control(self):
+        """
+        核心精度优化：高精度PD+前馈控制（位置-速度双闭环）
+        1.  自适应PD参数，根据负载与误差调整
+        2.  误差前馈补偿，提升动态响应精度
+        3.  重力扭矩补偿，抵消静态误差
+        4.  输出限幅，防止超调与过载
+        """
+        if self.data is None or self.planned_positions.shape[0] == 0:
+            return
+
+        # 1. 获取当前状态与误差补偿
+        current_angles = self.get_current_joint_angles(use_deg=False)
+        current_vels = self.get_current_joint_velocities(use_deg=False)
+        compensated_error, gravity_comp_torque = self.calculate_error_compensation()
+
+        # 2. 获取规划轨迹点（防止索引越界）
+        if self.traj_step_idx < self.planned_positions.shape[0]:
+            target_pos = self.planned_positions[self.traj_step_idx]
+            target_vel = self.planned_velocities[self.traj_step_idx]
+            self.traj_step_idx += 1
+        else:
+            target_pos = self.target_angles_rad
+            target_vel = np.zeros(JOINT_COUNT, dtype=np.float64)
+
+        # 3. 计算定位误差与轨迹跟踪误差
+        self.position_error = target_pos - current_angles
+        self.trajectory_error = target_pos - current_angles + (target_vel - current_vels) * CONTROL_TIMESTEP
+
+        # 更新最大误差
+        self.max_position_error = np.maximum(self.max_position_error, np.abs(self.position_error))
+        self.max_trajectory_error = np.maximum(self.max_trajectory_error, np.abs(self.trajectory_error))
+
+        # 4. 自适应PD参数计算（根据负载调整）
         normalized_load = min(self.current_end_load / LOAD_PARAMS['max_allowed_load'], 1.0)
+        kp = PRECISION_PD_PARAMS['kp_base'] * (1 + normalized_load * (PRECISION_PD_PARAMS['kp_load_gain'] - 1))
+        kd = PRECISION_PD_PARAMS['kd_base'] * (1 + normalized_load * (PRECISION_PD_PARAMS['kd_load_gain'] - 1))
 
-        # 2. 角度误差归一化
+        # 5. PD控制信号计算（位置-速度双闭环）
+        pd_control = kp * self.position_error + kd * (target_vel - current_vels)
+
+        # 6. 前馈补偿与重力补偿
+        ff_control = PRECISION_PD_PARAMS['ff_gain'] * target_vel  # 速度前馈
+        total_control = pd_control + ff_control + gravity_comp_torque + compensated_error
+
+        # 7. 输出限幅（防止超调与过载）
+        for i in range(JOINT_COUNT):
+            total_control[i] = np.clip(total_control[i], -JOINT_MAX_TORQUE[i], JOINT_MAX_TORQUE[i])
+
+        # 8. 更新关节阻尼（与刚度匹配，提升控制精度）
+        self.calculate_adaptive_stiffness()
+        for i, jid in enumerate(self.joint_ids):
+            if jid >= 0 and self.model is not None:
+                self.model.jnt_damping[jid] = self.current_damping[i]
+
+        # 9. 设置控制信号
+        for i, mid in enumerate(self.motor_ids):
+            if mid >= 0:
+                self.data.ctrl[mid] = total_control[i]
+
+    def calculate_adaptive_stiffness(self):
+        """自适应刚度计算（兼容之前的优化，辅助提升精度）"""
+        normalized_load = min(self.current_end_load / LOAD_PARAMS['max_allowed_load'], 1.0)
         current_angles = self.get_current_joint_angles(use_deg=False)
         angle_error_rad = np.abs(self.target_angles_rad - current_angles)
         normalized_error = np.clip(angle_error_rad / RELIABILITY_PARAMS['max_angle_error'], 0.0, 1.0)
 
-        # 3. 计算目标刚度（负载+误差双驱动）
+        # 目标刚度计算
         target_stiffness = STIFFNESS_PARAMS['base_stiffness'] * \
                            (1 + normalized_load * (STIFFNESS_PARAMS['load_stiffness_gain'] - 1)) * \
                            (1 + normalized_error * (STIFFNESS_PARAMS['error_stiffness_gain'] - 1))
-
-        # 4. 刚度限幅（防止超出合理范围）
-        target_stiffness = np.clip(target_stiffness,
-                                   STIFFNESS_PARAMS['min_stiffness'],
+        target_stiffness = np.clip(target_stiffness, STIFFNESS_PARAMS['min_stiffness'],
                                    STIFFNESS_PARAMS['max_stiffness'])
 
-        # 5. 刚度平滑更新（防止突变，提升可靠性）
+        # 刚度平滑更新
         self.current_stiffness = (1 - STIFFNESS_PARAMS['stiffness_smoothing']) * self.current_stiffness + \
                                  STIFFNESS_PARAMS['stiffness_smoothing'] * target_stiffness
 
-        # 6. 阻尼与刚度匹配更新（保证运动平稳）
+        # 阻尼与刚度匹配（整合粘性阻尼增益）
         target_damping = self.current_stiffness * DAMPING_INERTIA_PARAMS['damping_stiffness_ratio']
+        target_damping = target_damping * DAMPING_INERTIA_PARAMS['viscous_damping_gain']
         self.current_damping = np.clip(target_damping,
                                        DAMPING_INERTIA_PARAMS['base_damping'] * 0.5,
-                                       DAMPING_INERTIA_PARAMS['base_damping'] * 1.5)
-
-        # 7. 更新模型阻尼（实时生效）
-        for i, jid in enumerate(self.joint_ids):
-            if jid >= 0 and self.model is not None:
-                self.model.jnt_damping[jid] = self.current_damping[i]
+                                       DAMPING_INERTIA_PARAMS['base_damping'] * 2.0)
 
         return self.current_stiffness, self.current_damping
 
+    def monitor_precision(self):
+        """精度实时监测与评估，量化精度性能"""
+        # 判定是否超出允许误差
+        position_error_over_limit = \
+        np.where(np.abs(self.position_error) > PRECISION_MONITOR_PARAMS['max_allowed_position_error'])[0]
+        trajectory_error_over_limit = \
+        np.where(np.abs(self.trajectory_error) > PRECISION_MONITOR_PARAMS['max_allowed_trajectory_error'])[0]
+
+        # 记录超限信息
+        if len(position_error_over_limit) > 0:
+            joint_names = [JOINT_NAMES[i] for i in position_error_over_limit]
+            error_values = np.round(rad2deg(self.position_error[position_error_over_limit]), 4)
+            warning_msg = f"定位误差超限：关节{joint_names}，误差：{error_values}度（最大允许：{rad2deg(PRECISION_MONITOR_PARAMS['max_allowed_position_error']):.2f}度）"
+            print(f"⚠️ {warning_msg}")
+            write_precision_log(warning_msg)
+
+        if len(trajectory_error_over_limit) > 0:
+            joint_names = [JOINT_NAMES[i] for i in trajectory_error_over_limit]
+            error_values = np.round(rad2deg(self.trajectory_error[trajectory_error_over_limit]), 4)
+            warning_msg = f"轨迹跟踪误差超限：关节{joint_names}，误差：{error_values}度（最大允许：{rad2deg(PRECISION_MONITOR_PARAMS['max_allowed_trajectory_error']):.2f}度）"
+            print(f"⚠️ {warning_msg}")
+            write_precision_log(warning_msg)
+
+        # 记录精度统计信息
+        precision_stats = f"精度统计：当前定位误差（度）：{np.round(rad2deg(np.abs(self.position_error)), 4)}，最大定位误差（度）：{np.round(rad2deg(self.max_position_error), 4)}；当前轨迹误差（度）：{np.round(rad2deg(np.abs(self.trajectory_error)), 4)}，最大轨迹误差（度）：{np.round(rad2deg(self.max_trajectory_error), 4)}"
+        write_precision_log(precision_stats)
+
     def reliability_detection(self):
-        """
-        核心可靠性功能：关节卡死+过载+异常检测
-        1.  卡死检测：速度极低且扭矩接近最大值
-        2.  过载检测：受力持续超过阈值
-        3.  异常复位：满足条件时自动复位关节
-        """
+        """可靠性检测（兼容之前的优化，为精度提供保障）"""
         if self.data is None:
             return
 
-        # 1. 获取当前状态
         current_forces = self.get_joint_forces()
-        current_vels = np.array([self.data.qvel[jid] if jid >= 0 else 0 for jid in self.joint_ids], dtype=np.float64)
+        current_vels = self.get_current_joint_velocities(use_deg=False)
         current_angles = self.get_current_joint_angles(use_deg=False)
         angle_error = np.abs(self.target_angles_rad - current_angles)
-
-        # 2. 卡死检测（速度<阈值 且 受力>90%最大扭矩）
         current_time = time.time()
+
+        # 卡死检测
         for i in range(JOINT_COUNT):
             vel_abs = abs(current_vels[i])
             force_ratio = current_forces[i] / JOINT_MAX_TORQUE[i] if JOINT_MAX_TORQUE[i] > 0 else 0
 
-            # 判定卡死条件
             if vel_abs < RELIABILITY_PARAMS['stall_detection_threshold'] and force_ratio > 0.9:
                 self.stall_duration[i] += current_time - self.last_control_time
                 if self.stall_duration[i] >= RELIABILITY_PARAMS['stall_duration_threshold']:
@@ -457,11 +765,12 @@ def reliability_detection(self):
                     error_msg = f"关节{JOINT_NAMES[i]}卡死检测触发，速度：{vel_abs:.4f}，受力：{current_forces[i]:.2f}N·m"
                     print(f"⚠️ {error_msg}")
                     write_reliability_log(error_msg)
+                    write_precision_log(f"卡死异常影响精度：{error_msg}")
             else:
                 self.stall_duration[i] = 0.0
                 self.stall_detection_flag[i] = False
 
-            # 3. 过载检测（受力>90%最大扭矩 且 持续超时）
+            # 过载检测
             if force_ratio > 0.9:
                 self.overload_duration[i] += current_time - self.last_control_time
                 if self.overload_duration[i] >= RELIABILITY_PARAMS['overload_duration_threshold']:
@@ -469,92 +778,101 @@ def reliability_detection(self):
                     error_msg = f"关节{JOINT_NAMES[i]}过载持续触发，受力：{current_forces[i]:.2f}N·m，持续时间：{self.overload_duration[i]:.2f}s"
                     print(f"⚠️ {error_msg}")
                     write_reliability_log(error_msg)
+                    write_precision_log(f"过载异常影响精度：{error_msg}")
             else:
                 self.overload_duration[i] = 0.0
 
-        # 4. 大误差检测（角度误差超出阈值）
+        # 大误差检测
         large_error_joints = np.where(angle_error > RELIABILITY_PARAMS['max_angle_error'])[0]
         if len(large_error_joints) > 0:
             joint_names = [JOINT_NAMES[i] for i in large_error_joints]
             error_msg = f"大角度误差触发，关节：{joint_names}，最大误差：{np.max(angle_error):.2f}rad"
             print(f"⚠️ {error_msg}")
             write_reliability_log(error_msg)
+            write_precision_log(f"大误差异常：{error_msg}")
 
-        # 5. 自动异常复位（可靠性核心功能）
+        # 自动复位
         if RELIABILITY_PARAMS['auto_reset_on_error'] and (
                 np.any(self.stall_detection_flag) or self.overload_warning_flag or len(large_error_joints) > 0):
             self.auto_reset_joints()
             self.error_reset_count += 1
             write_reliability_log(f"异常自动复位触发，复位次数：{self.error_reset_count}")
+            write_precision_log(f"异常复位恢复精度：复位次数{self.error_reset_count}")
 
     def auto_reset_joints(self):
-        """自动复位异常关节（可靠性保护：恢复零位，降低负载）"""
-        print("\n🔧 执行关节自动复位，恢复零位并降低末端负载...")
-        # 1. 降低末端负载到安全值
+        """自动复位异常关节（恢复安全状态，保障后续精度）"""
+        print("\n🔧 执行关节自动复位，恢复零位并降低负载，保障精度...")
         self.set_end_effector_load(0.1)
-        # 2. 复位关节到零位
         self.set_joint_angles(np.zeros(JOINT_COUNT), smooth=False, use_deg=False)
-        # 3. 重置可靠性状态标志
+        self.plan_trajectory(np.zeros(JOINT_COUNT), np.zeros(JOINT_COUNT))
         self.overload_warning_flag = False
         self.stall_detection_flag = np.zeros(JOINT_COUNT, dtype=bool)
         self.stall_duration = np.zeros(JOINT_COUNT, dtype=np.float64)
         self.overload_duration = np.zeros(JOINT_COUNT, dtype=np.float64)
-        # 4. 重置刚度到基准值
         self.current_stiffness = STIFFNESS_PARAMS['base_stiffness'].copy()
-        self.current_damping = DAMPING_INERTIA_PARAMS['base_damping'].copy()
-        time.sleep(0.5)  # 复位后延迟，保证稳定
-        print("✅ 关节自动复位完成，恢复安全状态")
+        self.current_damping = DAMPING_INERTIA_PARAMS['base_damping'].copy() * DAMPING_INERTIA_PARAMS[
+            'viscous_damping_gain']
+        # 重置精度相关状态
+        self.position_error = np.zeros(JOINT_COUNT, dtype=np.float64)
+        self.trajectory_error = np.zeros(JOINT_COUNT, dtype=np.float64)
+        self.traj_step_idx = 0
+        time.sleep(0.5)
+        print("✅ 关节自动复位完成，恢复高精度安全状态")
 
     def set_end_effector_load(self, mass):
-        """动态设置末端负载（与刚度联动）"""
+        """动态设置末端负载（高精度更新，兼容刚度优化）"""
         if mass < 0 or mass > LOAD_PARAMS['max_allowed_load']:
             self.overload_warning_flag = True
             warning_msg = f"末端负载超出限制（0 ~ {LOAD_PARAMS['max_allowed_load']}kg），当前设置：{mass}kg"
             print(f"⚠️ {warning_msg}")
+            write_precision_log(warning_msg)
             write_reliability_log(warning_msg)
             return
         self.overload_warning_flag = False
 
-        # 优先直接更新负载geom质量（高效）
+        # 优先直接更新
         if self.model is not None and self.load_geom_id >= 0:
             try:
                 self.model.geom_mass[self.load_geom_id] = mass
                 self.current_end_load = mass
                 LOAD_PARAMS['end_effector_mass'] = mass
-                info_msg = f"末端负载更新为 {mass}kg（直接修改geom质量）"
+                info_msg = f"末端负载更新为 {mass}kg（直接修改geom质量，不影响精度）"
                 print(f"✅ {info_msg}")
+                write_precision_log(info_msg)
                 write_reliability_log(info_msg)
                 return
             except Exception as e:
                 error_msg = f"直接更新负载失败，将重新初始化模型: {e}"
                 print(f"⚠️ {error_msg}")
+                write_precision_log(error_msg)
                 write_reliability_log(error_msg)
 
-        # 降级方案：重新初始化模型
+        # 降级方案
         try:
             LOAD_PARAMS['end_effector_mass'] = mass
             self.current_end_load = mass
-            self.model = mujoco.MjModel.from_xml_string(create_arm_model_with_stiffness())
+            self.model = mujoco.MjModel.from_xml_string(create_arm_model_with_precision())
             self.data = mujoco.MjData(self.model)
-            # 重新获取ID
             self.joint_ids = [get_mujoco_id(self.model, 'joint', name) for name in JOINT_NAMES]
             self.motor_ids = [get_mujoco_id(self.model, 'actuator', f"motor{i + 1}") for i in range(JOINT_COUNT)]
             self.ee_site_id = get_mujoco_id(self.model, 'site', "ee_site")
             self.load_geom_id = get_mujoco_id(self.model, 'geom', "load_geom")
-            # 保留目标角度
             current_target = self.target_angles_rad.copy()
             self.target_angles_rad = current_target
             self.set_joint_angles(current_target, smooth=False, use_deg=False)
-            info_msg = f"末端负载更新为 {mass}kg（重新初始化模型生效）"
+            self.plan_trajectory(current_target, current_target)
+            info_msg = f"末端负载更新为 {mass}kg（重新初始化模型生效，精度恢复）"
             print(f"✅ {info_msg}")
+            write_precision_log(info_msg)
             write_reliability_log(info_msg)
         except Exception as e:
             error_msg = f"更新末端负载失败: {e}"
             print(f"❌ {error_msg}")
+            write_precision_log(error_msg)
             write_reliability_log(error_msg)
 
     def set_joint_angles(self, target_angles, smooth=True, use_deg=True):
-        """设置关节目标角度（容错增强）"""
+        """设置关节目标角度（高精度限位，避免超程影响精度）"""
         if self.data is None:
             raise Exception("模型未初始化，无法设置关节角度")
         if len(target_angles) != JOINT_COUNT:
@@ -572,19 +890,25 @@ def set_joint_angles(self, target_angles, smooth=True, use_deg=True):
             except Exception as e:
                 error_msg = f"更新模型状态失败: {e}"
                 print(f"⚠️ {error_msg}")
+                write_precision_log(error_msg)
                 write_reliability_log(error_msg)
 
+        # 若平滑模式，规划轨迹
+        if smooth:
+            start_angles = self.get_current_joint_angles(use_deg=use_deg)
+            self.plan_trajectory(start_angles, target_angles, use_deg=use_deg)
+
         self.target_angles_rad = target_angles_rad.copy()
 
     def clamp_joint_angles(self, angles, use_deg=True):
-        """关节限位保护（可靠性优化：缩小余量，防止冲击）"""
+        """关节高精度限位（更小余量，提升定位精度）"""
         angles = np.array(angles, dtype=np.float64)
         if use_deg:
             angles_rad = deg2rad(angles)
         else:
             angles_rad = angles.copy()
-        # 安全余量：5%，防止关节撞击限位
-        limit_margin = 0.05
+        # 极小安全余量：1%，防止关节撞击限位，保证定位精度
+        limit_margin = 0.01
         limits_rad_margin = JOINT_LIMITS_RAD.copy()
         limits_rad_margin[:, 0] += limit_margin
         limits_rad_margin[:, 1] -= limit_margin
@@ -593,120 +917,68 @@ def clamp_joint_angles(self, angles, use_deg=True):
             return rad2deg(clamped_rad)
         return clamped_rad
 
-    def stiffness_adaptive_pd_control(self):
-        """
-        刚度自适应PD控制（核心：刚度与PD参数联动，提升精度与可靠性）
-        """
-        if self.data is None:
-            return
-
-        # 1. 自适应刚度与阻尼更新
-        current_stiffness, current_damping = self.calculate_adaptive_stiffness()
-
-        # 2. 获取当前状态
-        current_angles = self.get_current_joint_angles(use_deg=False)
-        current_vels = np.array([self.data.qvel[jid] if jid >= 0 else 0 for jid in self.joint_ids], dtype=np.float64)
-        joint_forces = self.get_joint_forces()
-        angle_error = self.target_angles_rad - current_angles
-
-        # 3. 误差平滑
-        self.angle_error_history = (1 - LOAD_PARAMS['load_smoothing_factor']) * self.angle_error_history + \
-                                   LOAD_PARAMS['load_smoothing_factor'] * angle_error
-
-        # 4. PD参数与刚度联动
-        kp = current_stiffness / 2.5  # 刚度-P比例联动
-        kd = current_damping / 1.6  # 阻尼-D比例联动
-
-        # 5. 期望速度与控制信号计算
-        desired_vel = np.clip(self.angle_error_history * kp, -JOINT_MAX_VELOCITY_RAD, JOINT_MAX_VELOCITY_RAD)
-        control_signals = kp * self.angle_error_history + kd * (desired_vel - current_vels)
-
-        # 6. 软件过载保护（可靠性核心）
-        for i in range(JOINT_COUNT):
-            force_ratio = joint_forces[i] / JOINT_MAX_TORQUE[i] if JOINT_MAX_TORQUE[i] > 0 else 0
-            if force_ratio > 0.9:
-                control_signals[i] *= 0.4  # 降低60%输出，防止过载
-            elif force_ratio > 0.7:
-                control_signals[i] *= 0.7  # 降低30%输出，预警保护
-
-        # 7. 设置控制信号
-        for i, mid in enumerate(self.motor_ids):
-            if mid >= 0:
-                self.data.ctrl[mid] = control_signals[i]
-
-    def init_viewer(self):
-        """初始化Viewer（延迟加载，提升可靠性）"""
-        if self.model is None or self.data is None:
-            return False
-        if self.viewer_ready:
-            return True
-        try:
-            if MUJOCO_NEW_VIEWER:
-                self.viewer_inst = viewer.launch_passive(self.model, self.data)
-            else:
-                self.viewer_inst = viewer.Viewer(self.model, self.data)
-            self.viewer_ready = True
-            write_reliability_log("Viewer初始化成功，可视化启用")
-            print("✅ Viewer初始化成功")
-            return True
-        except Exception as e:
-            error_msg = f"Viewer初始化失败: {e}"
-            print(f"❌ {error_msg}")
-            write_reliability_log(error_msg)
-            return False
-
-    def print_stiffness_reliability_status(self):
-        """打印刚度与可靠性状态（实时监控）"""
+    def print_precision_status(self):
+        """打印精度与系统状态（实时监控）"""
         current_time = time.time()
         if current_time - self.last_print_time < 1.0:
             return
 
-        # 统计信息
         fps = self.fps_counter / (current_time - self.last_print_time)
         joint_angles = self.get_current_joint_angles(use_deg=True)
+        joint_vels = self.get_current_joint_velocities(use_deg=True)
         joint_forces = self.get_joint_forces()
         current_stiffness, current_damping = self.calculate_adaptive_stiffness()
-        angle_errors = rad2deg(self.angle_error_history)
+        position_error_deg = rad2deg(self.position_error)
+        trajectory_error_deg = rad2deg(self.trajectory_error)
+        max_position_error_deg = rad2deg(self.max_position_error)
+        max_trajectory_error_deg = rad2deg(self.max_trajectory_error)
         self.total_simulation_time = current_time - (SIMULATION_START_TIME or current_time)
 
         # 格式化打印
-        print("-" * 120)
+        print("-" * 150)
+        print(
+            f"📊 高精度仿真统计 | 耗时: {self.total_simulation_time:.2f}s | 步数: {self.step_count:,} | FPS: {fps:5.1f} | 复位次数: {self.error_reset_count}")
+        print(
+            f"🔧 关节状态 | 角度 (度): {np.round(joint_angles, 2)} | 速度 (度/s): {np.round(joint_vels, 3)} | 受力 (N·m): {np.round(joint_forces, 2)}")
         print(
-            f"📊 仿真统计 | 耗时: {self.total_simulation_time:.2f}s | 步数: {self.step_count:,} | FPS: {fps:5.1f} | 复位次数: {self.error_reset_count}")
-        print(f"🔧 关节角度 (度): {np.round(joint_angles, 1)} | 控制误差 (度): {np.round(abs(angle_errors), 3)}")
+            f"🎯 精度指标 | 当前定位误差 (度): {np.round(np.abs(position_error_deg), 4)} | 最大定位误差 (度): {np.round(max_position_error_deg, 4)}")
         print(
-            f"🏋️  末端负载 (kg): {self.current_end_load:.2f} | 关节受力 (N·m): {np.round(joint_forces, 2)} | 最大扭矩 (N·m): {np.round(JOINT_MAX_TORQUE, 1)}")
-        print(f"🔩 关节刚度: {np.round(current_stiffness, 1)} | 关节阻尼: {np.round(current_damping, 1)}")
+            f"🎯 精度指标 | 当前轨迹误差 (度): {np.round(np.abs(trajectory_error_deg), 4)} | 最大轨迹误差 (度): {np.round(max_trajectory_error_deg, 4)}")
+        print(f"🔩 刚度阻尼 | 关节刚度: {np.round(current_stiffness, 1)} | 关节阻尼: {np.round(current_damping, 1)}")
+        print(
+            f"🏋️  负载状态 | 末端负载 (kg): {self.current_end_load:.2f} | 负载限制 (kg): {LOAD_PARAMS['max_allowed_load']}")
         if self.overload_warning_flag:
-            print("⚠️  警告：关节过载，已启用输出限制！")
+            print("⚠️  警告：关节过载，已启用输出限制，精度可能受影响！")
         if np.any(self.stall_detection_flag):
             stall_joints = [JOINT_NAMES[i] for i in range(JOINT_COUNT) if self.stall_detection_flag[i]]
-            print(f"⚠️  警告：关节{stall_joints}卡死风险，即将触发自动复位！")
-        print("-" * 120)
+            print(f"⚠️  警告：关节{stall_joints}卡死风险，即将触发自动复位，精度将临时下降！")
+        print("-" * 150)
 
-        # 重置计数器
         self.last_print_time = current_time
         self.fps_counter = 0
 
     def preset_pose(self, pose_name):
-        """预设姿态（可靠性优化：平稳切换）"""
+        """预设高精度姿态（平滑切换，无超调）"""
         pose_map = {
-            'zero': [0, 0, 0, 0, 0],  # 零位（安全姿态）
+            'zero': [0, 0, 0, 0, 0],  # 零位（高精度基准姿态）
             'up': [0, 30, 20, 10, 0],  # 抬起姿态
-            'grasp': [0, 45, 30, 20, 10]  # 抓取姿态
+            'grasp': [0, 45, 30, 20, 10],  # 抓取姿态
+            'precision_test': [10, 20, 15, 5, 8]  # 精度测试姿态
         }
         if pose_name not in pose_map:
             warning_msg = f"无效姿态名称，支持：{list(pose_map.keys())}"
             print(f"⚠️ {warning_msg}")
+            write_precision_log(warning_msg)
             write_reliability_log(warning_msg)
             return
         self.set_joint_angles(pose_map[pose_name], smooth=True, use_deg=True)
-        info_msg = f"切换到{pose_name}姿态，刚度自适应控制已启用"
+        info_msg = f"切换到{pose_name}高精度姿态，轨迹规划与误差补偿已启用"
         print(f"✅ {info_msg}")
+        write_precision_log(info_msg)
         write_reliability_log(info_msg)
 
     def run(self):
-        """运行完整仿真（刚度+可靠性核心循环）"""
+        """运行高精度仿真主循环"""
         global RUNNING
 
         if not self.init_viewer():
@@ -714,29 +986,31 @@ def run(self):
             return
 
         # 启动信息
-        print("=" * 120)
-        print("🚀 机械臂关节刚度与可靠性优化控制器 - 启动成功")
+        print("=" * 150)
+        print("🚀 机械臂关节精度性能优化控制器 - 启动成功（geom viscous属性修复完成）")
         print(f"✅ 模型信息 | 关节数量: {JOINT_COUNT} | 初始末端负载: {self.current_end_load:.2f}kg")
+        print(
+            f"✅ 精度配置 | 控制频率: {CONTROL_FREQUENCY}Hz | 仿真步长: {SIMULATION_TIMESTEP}s | 定位公差: {rad2deg(TRAJECTORY_PLANNING_PARAMS['position_tol']):.4f}度")
         print(
             f"✅ 刚度配置 | 基座最大刚度: {STIFFNESS_PARAMS['max_stiffness'][0]:.1f} | 末端最小刚度: {STIFFNESS_PARAMS['min_stiffness'][-1]:.1f}")
-        print(f"✅ 可靠性配置 | 控制频率: {CONTROL_FREQUENCY}Hz | 最大允许负载: {LOAD_PARAMS['max_allowed_load']}kg")
         print("📝 快捷指令:")
         print("   - 设置末端负载: controller.set_end_effector_load(1.0)")
         print("   - 单关节控制: controller.move_joint(0, 90)")
-        print("   - 预设姿态: controller.preset_pose('up')")
+        print("   - 预设姿态: controller.preset_pose('precision_test')")
         print("   - 按 Ctrl+C 优雅退出")
-        print("=" * 120)
+        print("=" * 150)
 
-        # 主循环（可靠性优化：容错增强）
+        # 主循环
         while RUNNING:
             try:
                 current_time = time.time()
                 self.fps_counter += 1
                 self.step_count += 1
 
-                # 高频率控制更新
+                # 高频控制更新
                 if current_time - self.last_control_time >= CONTROL_TIMESTEP:
-                    self.stiffness_adaptive_pd_control()  # 刚度自适应控制
+                    self.precision_adaptive_pd_control()  # 高精度控制
+                    self.monitor_precision()  # 精度监测
                     self.reliability_detection()  # 可靠性检测
                     self.last_control_time = current_time
 
@@ -749,9 +1023,9 @@ def run(self):
                     self.viewer_inst.sync()
 
                 # 状态打印
-                self.print_stiffness_reliability_status()
+                self.print_precision_status()
 
-                # 动态睡眠，降低CPU占用
+                # 动态睡眠
                 time_diff = current_time - self.last_control_time
                 if time_diff < SLEEP_TIME:
                     sleep_duration = max(0.00001, SLEEP_TIME - time_diff)
@@ -760,29 +1034,55 @@ def run(self):
             except Exception as e:
                 error_msg = f"仿真步异常（步数：{self.step_count}）: {e}"
                 print(f"⚠️ {error_msg}")
+                write_precision_log(error_msg)
                 write_reliability_log(error_msg)
                 continue
 
         # 资源清理
         self.cleanup()
-        # 最终统计
-        final_msg = f"仿真结束 | 总耗时: {self.total_simulation_time:.2f}s | 总步数: {self.step_count:,} | 复位次数: {self.error_reset_count}"
-        print("\n" + "=" * 120)
-        print("✅ 控制器已优雅退出 - 刚度与可靠性仿真最终统计")
+        # 最终精度统计
+        final_msg = f"高精度仿真结束 | 总耗时: {self.total_simulation_time:.2f}s | 总步数: {self.step_count:,} | 复位次数: {self.error_reset_count} | 最大定位误差: {np.round(rad2deg(np.max(self.max_position_error)), 4)}度 | 最大轨迹误差: {np.round(rad2deg(np.max(self.max_trajectory_error)), 4)}度"
+        print("\n" + "=" * 150)
+        print("✅ 控制器已优雅退出 - 关节精度性能仿真最终统计")
         print(f"📈 {final_msg}")
-        print(f"🎯 最终末端负载 (kg): {self.current_end_load:.2f} | 最终关节刚度: {np.round(self.current_stiffness, 1)}")
-        print("=" * 120)
+        print("=" * 150)
+        write_precision_log(final_msg)
         write_reliability_log(final_msg)
 
+    def init_viewer(self):
+        """初始化Viewer（延迟加载，不影响精度）"""
+        if self.model is None or self.data is None:
+            return False
+        if self.viewer_ready:
+            return True
+        try:
+            if MUJOCO_NEW_VIEWER:
+                self.viewer_inst = viewer.launch_passive(self.model, self.data)
+            else:
+                self.viewer_inst = viewer.Viewer(self.model, self.data)
+            self.viewer_ready = True
+            write_precision_log("Viewer初始化成功，可视化启用（不影响高精度控制）")
+            write_reliability_log("Viewer初始化成功，可视化启用")
+            print("✅ Viewer初始化成功")
+            return True
+        except Exception as e:
+            error_msg = f"Viewer初始化失败: {e}"
+            print(f"❌ {error_msg}")
+            write_precision_log(error_msg)
+            write_reliability_log(error_msg)
+            return False
+
     def cleanup(self):
-        """资源清理（可靠性优化：完整释放，避免内存泄漏）"""
+        """资源清理（完整释放，避免内存泄漏影响后续精度测试）"""
         if self.viewer_ready and self.viewer_inst:
             try:
                 self.viewer_inst.close()
+                write_precision_log("Viewer资源清理完成")
                 write_reliability_log("Viewer资源清理完成")
             except Exception as e:
                 error_msg = f"Viewer关闭失败: {e}"
                 print(f"⚠️ {error_msg}")
+                write_precision_log(error_msg)
                 write_reliability_log(error_msg)
             self.viewer_inst = None
             self.viewer_ready = False
@@ -791,10 +1091,11 @@ def cleanup(self):
         global RUNNING, SIMULATION_START_TIME
         RUNNING = False
         SIMULATION_START_TIME = None
-        write_reliability_log("控制器资源清理完成，仿真正常退出")
+        write_precision_log("高精度控制器资源清理完成，仿真正常退出")
+        write_reliability_log("高精度控制器资源清理完成，仿真正常退出")
 
     def move_joint(self, joint_idx, angle, smooth=True, use_deg=True):
-        """单独控制单个关节（容错增强）"""
+        """单独控制单个关节（高精度平滑切换）"""
         if joint_idx < 0 or joint_idx >= JOINT_COUNT:
             raise ValueError(f"关节索引必须在0-{JOINT_COUNT - 1}之间，当前为{joint_idx}")
 
@@ -803,45 +1104,45 @@ def move_joint(self, joint_idx, angle, smooth=True, use_deg=True):
         self.set_joint_angles(current_angles, smooth=smooth, use_deg=use_deg)
 
 
-# ====================== 刚度与可靠性演示函数 ======================
-def stiffness_reliability_demo(controller):
-    """演示刚度自适应与可靠性保护功能"""
+# ====================== 精度优化演示函数 ======================
+def precision_optimization_demo(controller):
+    """演示关节精度优化功能"""
 
     def demo():
         time.sleep(2)
 
-        # 演示1：零位姿态（基准刚度）
-        print("\n🎬 演示1：切换到零位姿态，使用基准刚度")
+        # 演示1：零位姿态（基准精度测试）
+        print("\n🎬 演示1：切换到零位姿态，进行基准精度校准")
         controller.preset_pose('zero')
         time.sleep(3)
 
-        # 演示2：抬起姿态（刚度自适应调整）
-        print("\n🎬 演示2：切换到抬起姿态，刚度随姿态自动调整")
-        controller.preset_pose('up')
-        time.sleep(3)
+        # 演示2：精度测试姿态（多关节协同，验证轨迹精度）
+        print("\n🎬 演示2：切换到精度测试姿态，验证多关节轨迹跟踪精度")
+        controller.preset_pose('precision_test')
+        time.sleep(4)
 
-        # 演示3：增加负载（刚度放大，可靠性保护启用）
-        print("\n🎬 演示3：设置末端负载为1.8kg（接近最大值，刚度自动放大）")
-        controller.set_end_effector_load(1.8)
-        time.sleep(3)
+        # 演示3：增加负载（验证抗干扰精度维持）
+        print("\n🎬 演示3：设置末端负载为1.5kg，验证负载下精度稳定性")
+        controller.set_end_effector_load(1.5)
+        time.sleep(4)
 
-        # 演示4：大角度运动（大误差下刚度进一步提升）
-        print("\n🎬 演示4：关节1旋转90度（大误差，刚度与阻尼联动优化）")
-        controller.move_joint(0, 90, smooth=True, use_deg=True)
+        # 演示4：单关节大角度运动（验证定位精度，无超调）
+        print("\n🎬 演示4：关节1旋转45度，验证单关节高精度定位（无超调）")
+        controller.move_joint(0, 45, smooth=True, use_deg=True)
         time.sleep(4)
 
-        # 演示5：抓取姿态（全关节刚度匹配）
-        print("\n🎬 演示5：切换到抓取姿态，全关节刚度分层生效")
+        # 演示5：抓取姿态（验证全关节精度匹配）
+        print("\n🎬 演示5：切换到抓取姿态，验证全关节协同精度")
         controller.preset_pose('grasp')
         time.sleep(3)
 
-        # 演示6：降低负载（刚度回落，恢复平稳）
-        print("\n🎬 演示6：降低末端负载为0.2kg（刚度回落，运动更平稳）")
+        # 演示6：降低负载（验证精度恢复能力）
+        print("\n🎬 演示6：降低末端负载为0.2kg，验证精度恢复特性")
         controller.set_end_effector_load(0.2)
-        time.sleep(2)
+        time.sleep(3)
 
-        # 演示7：复位零位（可靠性演示）
-        print("\n🎬 演示7：切换回零位姿态，完成刚度与可靠性演示")
+        # 演示7：复位零位（验证精度复位能力）
+        print("\n🎬 演示7：切换回零位姿态，完成精度优化演示")
         controller.preset_pose('zero')
         time.sleep(2)
 
@@ -856,22 +1157,9 @@ def demo():
 
 # ====================== 主入口 ======================
 if __name__ == "__main__":
-    np.seterr(all='ignore')
-
-    # 创建刚度与可靠性控制器
-    controller = None
-    try:
-        controller = ArmStiffnessReliabilityController()
-    except Exception as e:
-        print(f"❌ 控制器创建失败: {e}")
-        sys.exit(1)
-
-    # 运行演示
-    if controller is not None:
-        stiffness_reliability_demo(controller)
-
-    # 启动控制器
-    if controller is not None:
-        controller.run()
-
-    sys.exit(0)
\ No newline at end of file
+    # 补充完整：设置numpy输出格式，便于查看高精度关节数据
+    np.set_printoptions(precision=4, suppress=True, linewidth=150)
+    # 初始化控制器并运行
+    controller = ArmJointPrecisionOptimizationController()
+    precision_optimization_demo(controller)
+    controller.run()
\ No newline at end of file