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
|