129 lines
2.4 KiB
Markdown
129 lines
2.4 KiB
Markdown
|
# D_star Lite 机器人任务规划
|
|||
|
|
## 目录结构
|
|||
|
|
### 坐标离散化
|
|||
|
|
`discretize_map.py`
|
|||
|
|
|
|||
|
|
### 地图文件(选择缩放倍率)
|
|||
|
|
`map_3.pkl`
|
|||
|
|
`map_4.pkl`
|
|||
|
|
`map_5.pkl`
|
|||
|
|
|
|||
|
|
### 导航类
|
|||
|
|
`navigate.py`
|
|||
|
|
|
|||
|
|
### D_star Lite 算法实现
|
|||
|
|
`dstar_lite.py`
|
|||
|
|
|
|||
|
|
### 测试文件
|
|||
|
|
`test.py`
|
|||
|
|
|
|||
|
|
---
|
|||
|
|
|
|||
|
|
## 世界地图
|
|||
|
|
|
|||
|
|
### 实际坐标范围
|
|||
|
|
|
|||
|
|
`X: -350 ~ 600`
|
|||
|
|
|
|||
|
|
`Y: -400 ~ 1450`
|
|||
|
|
|
|||
|
|
### 5倍缩放后坐标范围
|
|||
|
|
|
|||
|
|
`X: -70 ~ 120`
|
|||
|
|
|
|||
|
|
`Y: -80 ~ 290`
|
|||
|
|
|
|||
|
|
### 网格地图
|
|||
|
|
|
|||
|
|
| Idx | Obj |
|
|||
|
|
|-----|------------------|
|
|||
|
|
| 0 | free |
|
|||
|
|
| 1 | obstacle |
|
|||
|
|
| 2 | dynamic obstacle |
|
|||
|
|
|
|||
|
|
### 代价地图
|
|||
|
|
| Cost | Obj |
|
|||
|
|
|---------|-----------------|
|
|||
|
|
| 0 | free |
|
|||
|
|
| 15-10-5 | obs周围3格 |
|
|||
|
|
| inf | obstacle |
|
|||
|
|
| 100 | dynamic obstacle |
|
|||
|
|
|
|||
|
|
|
|||
|
|
---
|
|||
|
|
|
|||
|
|
|
|||
|
|
## 参数
|
|||
|
|
`机器人步长`： 150
|
|||
|
|
|
|||
|
|
`机器人速度`： 150
|
|||
|
|
|
|||
|
|
`机器人观测范围`： 300
|
|||
|
|
|
|||
|
|
`行人半径`： 36
|
|||
|
|
|
|||
|
|
`目标判达距离`： 50
|
|||
|
|
|
|||
|
|
`机器人移动dis_limit`： 10
|
|||
|
|
|
|||
|
|
---
|
|||
|
|
|
|||
|
|
## 使用方法
|
|||
|
|
```python
|
|||
|
|
# 选择缩放合适的地图：3、4、5
|
|||
|
|
file_name = 'map_4.pkl'
|
|||
|
|
if os.path.exists(file_name):
|
|||
|
|
with open(file_name, 'rb') as file:
|
|||
|
|
map = pickle.load(file)
|
|||
|
|
|
|||
|
|
# 初始化场景
|
|||
|
|
scene.init_world(1, 11)
|
|||
|
|
scene = scene.Scene(sceneID=0)
|
|||
|
|
|
|||
|
|
# 舒适化导航类
|
|||
|
|
# (需要传入：场景、实际地图范围、离散化地图、缩放比例)
|
|||
|
|
navigator = Navigator(scene=scene, area_range=[-350, 600, -400, 1450], map=map, scale_ratio=4)
|
|||
|
|
|
|||
|
|
# 设置目标
|
|||
|
|
goal = (0, 0)
|
|||
|
|
|
|||
|
|
# 导航
|
|||
|
|
# (animation: 选择是否画出导航过程)
|
|||
|
|
navigator.navigate(goal, animation=False)
|
|||
|
|
|
|||
|
|
```
|
|||
|
|
|
|||
|
|
|
|||
|
|
---
|
|||
|
|
|
|||
|
|
## 可靠性保证
|
|||
|
|
|
|||
|
|
`目标合法性保证`：
|
|||
|
|
|
|||
|
|
- 目标在地图外：重置目标为最近的地图内位置
|
|||
|
|
- 目标在静态障碍物：从当前位置不断向外圈扩展直到找到合法位置，重置目标为该位置
|
|||
|
|
|
|||
|
|
`规划sbgoal合法性保证`：
|
|||
|
|
|
|||
|
|
- 规划subgoal被动态障碍物占据：重新规划路径
|
|||
|
|
|
|||
|
|
|
|||
|
|
`起点合法性保证`：
|
|||
|
|
|
|||
|
|
- 起点在静态障碍物：从当前位置不断向外圈扩展直到找到合法位置，重置起点为该位置
|
|||
|
|
- 起点在动态障碍物范围内：缩小动态障碍物半径，保证起点位置为空闲
|
|||
|
|
|
|||
|
|
|
|||
|
|
`机器人朝向保证`：
|
|||
|
|
|
|||
|
|
- 机器人始终朝向每一步的移动方向
|
|||
|
|
|
|||
|
|
|
|||
|
|
`规划抖动解决`：
|
|||
|
|
|
|||
|
|
- 规划路径不允许有重复点
|
|||
|
|
|
|||
|
|
`避免机器人沿障碍物行走`：
|
|||
|
|
|
|||
|
|
- 障碍物扩张：在代价地图`cost_map`中，静态障碍物周围的空位也会受到影响，并产生cost
|