DDA 知识点：Directional Distance Avoidance

一、从「离散射线」到「深度图」的转变

原方案	新方案
mj_ray 离散射线	深度图 / 点云（带距离的视场）
N 条射线直接给距离	从深度图按方位做 1D 距离扇区
每条射线独立	每个扇区取最近障碍距离（鲁棒统计）

核心：深感相机给你的是 Depth / PointCloud，不需要 mj_ray 这种离散射线；先把深度转成扇区最近距离，再做避障。

二、DDA 核心思想

输入：dist[sector] —— 每个方向上的最近障碍距离（米）
判断：当前期望运动方向 move_theta = atan2(vy_cmd, vx_cmd) 是否过近
动作：
- 若过近：禁止/衰减朝该方向的速度分量
- 同时生成侧向/偏航的「脱离」控制，朝开阔方向

三、深度 → 扇区最近距离（depth_to_sectors）

视场水平角 HFOV（如 RealSense D435 ≈ 86°）
扇区数 N（如 31、61）
对每个扇区：在深度图对应列范围取鲁棒最小值：
- percentile(depth, 5%) 或
- 剔除 0/无效 + 中值滤波

得到：

angles[i]：扇区角（车体坐标系，左正右负）
d[i]：该方向最近障碍距离（米）

四、DDA 避障控制律

计算期望运动方向角：move_theta = atan2(vy_cmd, vx_cmd)
在 move_theta 附近取窗口内最小距离 dmin_dir
阈值判断：
- dmin_dir >= safe_dist：正常走
- dmin_dir < stop_dist：禁止向前 + 强制侧移/转向
- 中间区间：按比例衰减 + 轻微偏航
选择「最开阔方向」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 推荐两行增强（仿真中已启用）

速度自适应安全距离：跑得越快看得越远

safe_dist = base_safe + k * speed
stop_dist = base_stop + 0.5 * k * speed

前方扇区限速：只用前方 ±20° 的最小距离限速

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 订阅

# 伪代码：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)

融合后继续做：滤波 → 车体系→世界系 → 执行器。

3.7 KiB Raw Permalink Blame History Unescape Escape