123 lines
3.7 KiB
Markdown
123 lines
3.7 KiB
Markdown
# 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)
|
||
```
|
||
|
||
融合后继续做：滤波 → 车体系→世界系 → 执行器。
|