release: v0.3

docs/DDA_KNOWLEDGE.md

@@ -0,0 +1,122 @@
+# DDA 知识点：Directional Distance Avoidance
+
+## 一、从「离散射线」到「深度图」的转变
+
+| 原方案 | 新方案 |
+|--------|--------|
+| mj_ray 离散射线 | 深度图 / 点云（带距离的视场） |
+| N 条射线直接给距离 | 从深度图按方位做 1D 距离扇区 |
+| 每条射线独立 | 每个扇区取**最近障碍距离**（鲁棒统计） |
+
+**核心**：深感相机给你的是 **Depth / PointCloud**，不需要 mj_ray 这种离散射线；先把深度转成扇区最近距离，再做避障。
+
+---
+
+## 二、DDA 核心思想
+
+1. **输入**：`dist[sector]` —— 每个方向上的最近障碍距离（米）
+2. **判断**：当前期望运动方向 `move_theta = atan2(vy_cmd, vx_cmd)` 是否过近
+3. **动作**：
+   - 若过近：**禁止/衰减**朝该方向的速度分量
+   - 同时生成**侧向/偏航**的「脱离」控制，朝开阔方向
+
+---
+
+## 三、深度 → 扇区最近距离（depth_to_sectors）
+
+- 视场水平角 `HFOV`（如 RealSense D435 ≈ 86°）
+- 扇区数 `N`（如 31、61）
+- 对每个扇区：在深度图对应列范围取**鲁棒最小值**：
+  - `percentile(depth, 5%)` 或
+  - 剔除 0/无效 + 中值滤波
+
+得到：
+- `angles[i]`：扇区角（车体坐标系，左正右负）
+- `d[i]`：该方向最近障碍距离（米）
+
+---
+
+## 四、DDA 避障控制律
+
+1. 计算期望运动方向角：`move_theta = atan2(vy_cmd, vx_cmd)`
+2. 在 `move_theta` 附近取窗口内最小距离 `dmin_dir`
+3. 阈值判断：
+   - `dmin_dir >= safe_dist`：正常走
+   - `dmin_dir < stop_dist`：禁止向前 + 强制侧移/转向
+   - 中间区间：按比例衰减 + 轻微偏航
+4. 选择「最开阔方向」`best_theta`，作为绕行/偏航目标
+
+---
+
+## 五、ROS1 + 深感相机 落地要点
+
+### 5.1 深度图输入
+
+- **Depth Image**：`sensor_msgs/Image`，订阅 `/camera/depth/image_rect` 等
+- **PointCloud**：`sensor_msgs/PointCloud2`，转为深度图或直接投影到扇区
+
+### 5.2 常用深感相机参数
+
+| 相机 | HFOV | 分辨率 |
+|------|------|--------|
+| RealSense D435 | ~87° | 640×480 |
+| Orbbec Astra | ~60° | 640×480 |
+| ZED | 可调 | 多种 |
+| OAK-D | ~73° | 640×400 |
+
+### 5.3 推荐两行增强（仿真中已启用）
+
+1. **速度自适应安全距离**：跑得越快看得越远
+   ```py
+   safe_dist = base_safe + k * speed
+   stop_dist = base_stop + 0.5 * k * speed
+   ```
+
+2. **前方扇区限速**：只用前方 ±20° 的最小距离限速
+   ```py
+   front = (np.abs(angles) < np.deg2rad(20))
+   d_front = np.min(dists[front])
+   speed_limit = np.clip((d_front - stop_dist) / (safe_dist - stop_dist), 0, 1) * MAX_LINEAR
+   ```
+
+---
+
+## 六、ROS1 深度输入对接（待实现）
+
+### 6.1 Depth Image 订阅
+
+```python
+# 伪代码：ROS1 订阅
+import rospy
+from sensor_msgs.msg import Image
+from cv_bridge import CvBridge
+
+def depth_cb(msg):
+    depth = bridge.imgmsg_to_cv2(msg, desired_encoding='passthrough')
+    depth = depth.astype(np.float32) / 1000.0  # mm -> m
+    dists, angles = depth_to_sectors(depth, hfov_rad, num_sectors, ...)
+    # 存入共享变量，供控制回调使用
+
+rospy.Subscriber('/camera/depth/image_rect', Image, depth_cb)
+```
+
+### 6.2 PointCloud 订阅
+
+从 `sensor_msgs/PointCloud2` 投影到极坐标扇区，取每扇区最近距离。
+
+---
+
+## 七、控制流程对接
+
+**仿真**：`mj_ray` 网格 → 仿真深度图 `(1, num_sectors)` → `depth_to_sectors()`  
+**实机**：深感相机 depth/pointcloud → `depth_to_sectors()`  
+
+```
+depth (H×W) 或 pointcloud
+    → depth_to_sectors() → dists, angles
+    → dda_avoidance_from_depth(dists, angles, vx_g, vy_g)
+    → vx_a, vy_a, wz_a
+    → blend_and_clamp(vx_a, vy_a, wz_a, vx_g, vy_g, wz_g)
+```
+
+融合后继续做：滤波 → 车体系→世界系 → 执行器。

scripts/run_simulation.py

@@ -1,10 +1,12 @@
 #!/usr/bin/env python3
 """
-MuJoCo 全向小车导航（前向射线扫描 + 势场避障 + 目标点导航）
+MuJoCo 全向小车导航（仿真深度图 + DDA 避障 + 目标点导航）
-- vx, vy, wz 全向移动（车体系输出 -> 世界系执行器）
+
-- mj_ray 扇形前向扫描
+- 输入：仿真深度图（mj_ray 网格）或可替换为真实深感相机的 depth/pointcloud
-- 避障：排斥 + 朝开阔方向绕行
+- depth_to_sectors：深度 → 1D 扇区最近距离（鲁棒统计）
-- 卡住检测：连续位移太小 -> 强制脱困策略
+- dda_avoidance_from_depth：Directional Distance Avoidance 避障
+- vx, vy, wz 全向移动（车体系 → 世界系执行器）
+- 便于 ROS1 + RealSense/Orbbec/ZED 等真实相机落地
 """
 from __future__ import annotations
 
@@ -23,13 +25,18 @@ SCENE_PATH = os.path.join(os.path.dirname(__file__), "..", "mujoco_scenes", "cyl
 # ===================== 参数配置 =====================
 @dataclass
 class NavCfg:
-    # ray scan
+    # depth sector（仿真/真实深度图统一接口）
-    num_rays: int = 19
+    hfov_deg: float = 86.0           # 水平视场角（如 RealSense D435）
-    ray_angle_span: float = np.pi  # -span/2 ~ +span/2
+    num_sectors: int = 61            # 扇区数
+    depth_z_min: float = 0.1
+    depth_z_max: float = 6.0
+    depth_percentile: float = 5.0    # 鲁棒最小值：5% 分位
 
-    # distances
+    # DDA 避障
-    obstacle_threshold: float = 0.45
+    obstacle_threshold: float = 0.45  # stop_dist
-    safe_distance: float = 0.65
+    safe_distance: float = 0.65       # safe_dist
+    dda_window: int = 2               # move_theta 附近 ±N 扇区
+    speed_adaptive_k: float = 0.15    # 速度自适应：safe += k*speed
     goal_threshold: float = 0.20
 
     # limits
@@ -41,6 +48,8 @@ class NavCfg:
     k_attract: float = 1.0
     k_repel: float = 0.6
     k_yaw_goal: float = 0.8
+    # 转向策略：大角度先转后走，小角度边走边调
+    yaw_turn_first_deg: float = 35.0   # 超过此角度：先转向，几乎不走
 
     # stuck detect
     stuck_thresh: float = 0.03
@@ -105,7 +114,137 @@ def goal_from_camera(viewer_handle) -> Optional[Tuple[float, float]]:
     return float(pt[0]), float(pt[1])
 
 
-# ===================== 射线扫描（预计算优化） =====================
+# ===================== 仿真深度图 + 扇区 + DDA =====================
+def depth_to_sectors(
+    depth: np.ndarray,
+    hfov_rad: float,
+    num_sectors: int,
+    z_min: float = 0.1,
+    z_max: float = 6.0,
+    percentile: float = 5.0,
+) -> Tuple[np.ndarray, np.ndarray]:
+    """
+    深度图 → 1D 扇区最近距离（可直接用于真实深感相机）
+    depth: H×W (m)，0/NaN 表示无效
+    """
+    H, W = depth.shape
+    angles = np.linspace(-hfov_rad / 2, hfov_rad / 2, num_sectors, dtype=np.float64)
+    cols = np.linspace(0, W, num_sectors + 1).astype(int)
+    dists = np.full(num_sectors, z_max, dtype=np.float64)
+
+    for i in range(num_sectors):
+        c0, c1 = cols[i], cols[i + 1]
+        strip = depth[:, c0:c1].reshape(-1)
+        strip = strip[np.isfinite(strip)]
+        strip = strip[(strip > z_min) & (strip < z_max)]
+        if strip.size == 0:
+            continue
+        dists[i] = float(np.percentile(strip, percentile))
+
+    dists = np.minimum(dists, np.roll(dists, 1))
+    dists = np.minimum(dists, np.roll(dists, -1))
+    return dists, angles
+
+
+class DepthSimulator:
+    """用 mj_ray 生成仿真深度图（模拟深感相机输出）"""
+
+    def __init__(self, num_sectors: int, hfov_rad: float):
+        self.num_sectors = int(num_sectors)
+        self.angles = np.linspace(-hfov_rad / 2, hfov_rad / 2, self.num_sectors)
+        c = np.cos(self.angles)
+        s = np.sin(self.angles)
+        self.ray_dirs_body = np.stack([c, s, np.zeros_like(c)], axis=1).astype(np.float64)
+        self.depth = np.full((1, self.num_sectors), 10.0, dtype=np.float64)
+        self._geomid = np.array([-1], dtype=np.int32)
+
+    def render(
+        self,
+        model: mujoco.MjModel,
+        data: mujoco.MjData,
+        body_id: int,
+        site_id: int,
+    ) -> np.ndarray:
+        site_xpos = data.site_xpos[site_id]
+        xmat = np.array(data.xmat[body_id], dtype=np.float64).reshape(3, 3)
+        ray_dirs_world = (xmat @ self.ray_dirs_body.T).T
+        norms = np.linalg.norm(ray_dirs_world, axis=1) + 1e-9
+        ray_dirs_world /= norms[:, None]
+
+        self.depth.fill(10.0)
+        for i in range(self.num_sectors):
+            d = mujoco.mj_ray(
+                model, data,
+                site_xpos, ray_dirs_world[i],
+                None, 1, body_id, self._geomid
+            )
+            if d >= 0:
+                self.depth[0, i] = float(d)
+        return self.depth
+
+
+def dda_avoidance_from_depth(
+    dists: np.ndarray,
+    angles: np.ndarray,
+    vx_cmd: float,
+    vy_cmd: float,
+    cfg: NavCfg,
+) -> Tuple[float, float, float]:
+    """
+    Directional Distance Avoidance (DDA)
+    只关心「你要去的方向」是否过近；过近则削掉该方向分量 + 朝开阔方向侧移/转向
+    """
+    speed = float(np.hypot(vx_cmd, vy_cmd))
+    if speed < 1e-6:
+        return 0.0, 0.0, 0.0
+
+    move_theta = float(np.arctan2(vy_cmd, vx_cmd))
+    N = len(dists)
+    i0 = int(np.argmin(np.abs(angles - move_theta)))
+    win = cfg.dda_window
+    idxs = [(i0 + k) % N for k in range(-win, win + 1)]
+    dmin_dir = float(np.min(dists[idxs]))
+
+    best_i = int(np.argmax(dists))
+    best_theta = float(angles[best_i])
+
+    stop_dist = cfg.obstacle_threshold
+    safe_dist = cfg.safe_distance
+    if hasattr(cfg, "speed_adaptive_k") and cfg.speed_adaptive_k > 0:
+        safe_dist = safe_dist + cfg.speed_adaptive_k * speed
+        stop_dist = stop_dist + 0.5 * cfg.speed_adaptive_k * speed
+
+    if dmin_dir >= safe_dist:
+        return 0.0, 0.0, 0.0
+
+    danger = (safe_dist - dmin_dir) / max(safe_dist - stop_dist, 1e-6)
+    danger = float(np.clip(danger, 0.0, 1.0))
+
+    vx_a = 0.0
+    vy_a = 0.0
+    wz_a = 0.0
+
+    mx = float(np.cos(move_theta))
+    my = float(np.sin(move_theta))
+    proj = vx_cmd * mx + vy_cmd * my
+    cut = (0.6 + 0.4 * danger) * proj
+    vx_a -= cut * mx
+    vy_a -= cut * my
+
+    bypass = 0.25 + 0.25 * danger
+    vx_a += bypass * np.cos(best_theta)
+    vy_a += bypass * np.sin(best_theta)
+    wz_a += (0.35 + 0.45 * danger) * best_theta
+
+    if dmin_dir < stop_dist:
+        vx_a -= vx_cmd * 0.9
+        vy_a -= vy_cmd * 0.9
+        wz_a += 0.6 * np.sign(best_theta if abs(best_theta) > 1e-3 else 1.0)
+
+    return vx_a, vy_a, wz_a
+
+
+# ===================== 射线扫描（旧接口，DDA 下可弃用） =====================
 class RayScanner:
     """
     预计算扇形扫描的 body-frame 方向（基于 forward=[1,0,0], left=[0,1,0]），
@@ -190,75 +329,57 @@ class VelocityFilter:
 
 
 def goal_attraction_vel(x: float, y: float, yaw: float, goal_x: float, goal_y: float, cfg: NavCfg):
+    """
+    大角度：先转向后前进；小角度：前进时顺带转向
+    """
     dx = goal_x - x
     dy = goal_y - y
     dist = float(np.hypot(dx, dy)) + 1e-6
 
-    # 世界系单位方向
+    target_yaw = float(np.arctan2(dy, dx))
+    yaw_err = wrap_pi(target_yaw - yaw)
+    yaw_err_abs = float(np.abs(yaw_err))
+
+    thresh_rad = np.deg2rad(cfg.yaw_turn_first_deg)
+
+    # 线速度：大角度时抑制，小角度时正常
     gx_w = dx / dist
     gy_w = dy / dist
-
-    # 转车体系（只关心 yaw）
     c = np.cos(-yaw)
     s = np.sin(-yaw)
     gx_b = c * gx_w - s * gy_w
     gy_b = s * gx_w + c * gy_w
-
-    # 线速度（随距离平滑）
     scale = float(np.tanh(dist) * 0.8 + 0.2)
-    vx = cfg.k_attract * scale * gx_b
+    vx_raw = cfg.k_attract * scale * gx_b
-    vy = cfg.k_attract * scale * gy_b
+    vy_raw = cfg.k_attract * scale * gy_b
+
+    if yaw_err_abs > thresh_rad:
+        # 大角度：先转，几乎不走（留 0.05 防止完全不动时滤波器卡死）
+        linear_scale = 0.05
+    else:
+        # 小角度：边走边调，角度越小线速越足
+        linear_scale = 1.0 - 0.85 * (yaw_err_abs / thresh_rad) ** 2
+
+    vx = vx_raw * linear_scale
+    vy = vy_raw * linear_scale
+
+    # 角速度：大角度时略加强，小角度时弱增益边走边调
+    if yaw_err_abs > thresh_rad:
+        wz = cfg.k_yaw_goal * 1.3 * float(np.tanh(yaw_err))
+    else:
+        wz = cfg.k_yaw_goal * 0.7 * float(np.tanh(yaw_err))
 
-    # 角速度（弱对准）
-    target_yaw = float(np.arctan2(dy, dx))
-    yaw_err = wrap_pi(target_yaw - yaw)
-    wz = cfg.k_yaw_goal * float(np.tanh(yaw_err))
     return vx, vy, wz
 
 
-def obstacle_avoidance_vel(distances: np.ndarray, angles: np.ndarray, cfg: NavCfg):
-    min_d = float(np.min(distances))
-    if min_d > cfg.obstacle_threshold:
-        return 0.0, 0.0, 0.0, min_d
-
-    # 最开阔方向
-    safe_i = int(np.argmax(distances))
-    best_theta = float(angles[safe_i])
-
-    vx = 0.0
-    vy = 0.0
-    wz = 0.0
-
-    # 排斥 + 轻微“扭转”朝开阔方向
-    # （这里保持你原逻辑，但数值上更稳一点）
-    for d, th in zip(distances, angles):
-        d = float(d)
-        th = float(th)
-        if d < cfg.obstacle_threshold:
-            ratio = 1.0 - d / cfg.obstacle_threshold
-            strength = cfg.k_repel * (ratio * ratio)
-            vx -= strength * np.cos(th)
-            vy -= strength * np.sin(th)
-        elif d < cfg.safe_distance:
-            ratio = (cfg.safe_distance - d) / (cfg.safe_distance - cfg.obstacle_threshold)
-            strength = cfg.k_repel * 0.15 * ratio
-            wz += strength * float(np.clip(best_theta - th, -1.0, 1.0))
-
-    # 绕行：朝最开阔方向“推一把”
-    bypass = 0.35
-    vx += bypass * np.cos(best_theta)
-    vy += bypass * np.sin(best_theta)
-    wz += 0.4 * best_theta
-
-    return vx, vy, wz, min_d
-
-
 def blend_and_clamp(
     vx_a: float, vy_a: float, wz_a: float,
     vx_g: float, vy_g: float, wz_g: float,
     dist_to_goal: float,
     stuck: bool,
-    cfg: NavCfg
+    cfg: NavCfg,
+    dists: Optional[np.ndarray] = None,
+    angles: Optional[np.ndarray] = None,
 ):
     vx = vx_a + vx_g
     vy = vy_a + vy_g
@@ -266,6 +387,22 @@ def blend_and_clamp(
 
     lin = float(np.hypot(vx, vy))
 
+    # 前方扇区限速：只用前方 ±20° 最小距离
+    if dists is not None and angles is not None and len(dists) > 0:
+        front = np.abs(angles) < np.deg2rad(20)
+        if np.any(front):
+            d_front = float(np.min(dists[front]))
+            ratio = np.clip(
+                (d_front - cfg.obstacle_threshold) / max(cfg.safe_distance - cfg.obstacle_threshold, 1e-6),
+                0.0, 1.0
+            )
+            speed_limit = ratio * cfg.max_linear
+            if lin > speed_limit + 1e-6:
+                s = speed_limit / lin
+                vx *= s
+                vy *= s
+            lin = float(np.hypot(vx, vy))
+
     # 强制最小速度：未到目标且输出太小
     if dist_to_goal > cfg.goal_threshold and lin < cfg.min_linear:
         if lin > 1e-6:
@@ -324,7 +461,8 @@ def main():
 
     hide_goal_marker()
 
-    scanner = RayScanner(CFG.num_rays, CFG.ray_angle_span)
+    hfov_rad = np.deg2rad(CFG.hfov_deg)
+    depth_sim = DepthSimulator(CFG.num_sectors, hfov_rad)
     vel_filter = VelocityFilter(alpha=CFG.ema_alpha, max_dv=CFG.max_dv, max_dw=CFG.max_dw)
 
     goals: List[Tuple[float, float]] = []
@@ -338,7 +476,7 @@ def main():
     add_goal_camera = False
 
     print("=" * 60)
-    print("MuJoCo 全向导航：射线扫描 + 避障 + 目标点")
+    print("MuJoCo 全向导航：仿真深度图 + DDA 避障 + 目标点")
     print(f"G: 车头前方 {CFG.goal_ahead_dist:.1f}m | C: 相机视线落点 | 目标边界 R<{CFG.goal_max_r:.1f}m")
     print("ESC 退出")
     print("=" * 60)
@@ -409,8 +547,12 @@ def main():
 
             set_goal_marker(goal_x, goal_y)
 
-            # ========== scan ==========
+            # ========== 仿真深度图 → 扇区 → DDA ==========
-            distances, angles = scanner.scan(model, data, body_id, site_id)
+            depth_img = depth_sim.render(model, data, body_id, site_id)
+            dists, angles = depth_to_sectors(
+                depth_img, hfov_rad, CFG.num_sectors,
+                CFG.depth_z_min, CFG.depth_z_max, CFG.depth_percentile
+            )
 
             # ========== stuck detect ==========
             moved = float(np.hypot(x - last_x, y - last_y))
@@ -422,9 +564,12 @@ def main():
             stuck = stuck_cnt > CFG.stuck_steps
 
             # ========== velocities ==========
-            vx_a, vy_a, wz_a, _min_d = obstacle_avoidance_vel(distances, angles, CFG)
             vx_g, vy_g, wz_g = goal_attraction_vel(x, y, yaw, goal_x, goal_y, CFG)
-            vx_b, vy_b, wz_b = blend_and_clamp(vx_a, vy_a, wz_a, vx_g, vy_g, wz_g, dist_to_goal, stuck, CFG)
+            vx_a, vy_a, wz_a = dda_avoidance_from_depth(dists, angles, vx_g, vy_g, CFG)
+            vx_b, vy_b, wz_b = blend_and_clamp(
+                vx_a, vy_a, wz_a, vx_g, vy_g, wz_g, dist_to_goal, stuck, CFG,
+                dists=dists, angles=angles
+            )
 
             # ========== filter ==========
             vel_filter.max_dv = CFG.max_dv_stuck if stuck else CFG.max_dv