You Only Plan Once

原始论文：You Only Plan Once: A Learning-Based One-Stage Planner With Guidance Learning

改进与应用：YOPOv2-Tracker: An End-to-End Agile Tracking and Navigation Framework from Perception to Action

论文视频：YouTube，bilibili

一些真实世界实验：YouTube，bilibili

更快且更简单： 代码已经用 Python/PyTorch 进行了大幅简化和重构。我们还将模拟器替换为基于 CUDA 加速的随机环境，它更快、更轻量且没有边界。若需要与论文一致的稳定版本，请参考 main 分支。

硬件：

由 @Mioulo 设计的无人机也已开源。硬件组件列在 hardware_list.pdf 中，碳纤维机架的 SolidWorks 文件可以在 /hardware 中找到（完整装配文件包含在 Release 中）。

简介：

我们提出了一种用于障碍密集环境中自主导航的学习型规划器，它将经典方法中的 (i) 感知与建图、(ii) 前端路径搜索，以及 (iii) 后端优化整合到单个网络中。

学习型规划器： 考虑到导航问题的多模态特性，并为了避免陷入初始值附近的局部最小值，我们的方法采用一组运动基元作为锚点来覆盖搜索空间，并预测这些基元的偏移量和评分以进一步改进（类似于单阶段目标检测器 YOLO）。

训练策略： 与在模仿学习中使用专家示范作为标签，或在强化学习中通过试错探索相比，我们直接将轨迹代价（例如来自 ESDF）的梯度反向传播到网络权重。这种方式简单、直接、准确，并且与序列无关（无需在线模拟器交互或渲染）。


基元锚点	预测轨迹和评分	学习方法

安装

本项目已在 Ubuntu 20.04 和 Jetson Orin/Xavier NX 上测试。我们假设你已经安装了 CUDA、ROS 和 Conda 等必要依赖。

1. 克隆代码

git clone --depth 1 git@github.com:TJU-Aerial-Robotics/YOPO.git

2. 创建虚拟环境

我指定了当前使用的版本号，以避免未来版本变化带来的影响；你也可以移除这些版本号。

conda create --name yopo python=3.10 -y
conda activate yopo
cd YOPO
pip install -r requirements.txt

3. 构建模拟器

构建控制器和动力学模拟器

conda deactivate
cd Controller
catkin_make

构建环境和传感器模拟器（如果出现 CUDA 错误，请参考 Simulator_Introduction）

conda deactivate
cd Simulator
catkin_make

测试策略

你可以使用我们在 YOPO/saved/YOPO_1/epoch50.pth 提供的预训练权重来测试策略。

1. 启动控制器和动力学模拟器

关于控制器的详细介绍，请参考 Controller_Introduction

cd Controller
source devel/setup.bash
roslaunch so3_quadrotor_simulator simulator_attitude_control.launch

2. 启动环境和传感器模拟器

关于模拟器的详细介绍，请参考 Simulator_Introduction。随机森林示例可见 random_forest.png

cd Simulator
source devel/setup.bash
rosrun sensor_simulator sensor_simulator_cuda

你可以参考 config.yaml 来修改传感器（例如相机和 LiDAR 参数）以及环境（例如场景类型和障碍物密度）。

3. 启动 YOPO 规划器

你可以参考 traj_opt.yaml 来修改飞行速度（给定权重在 6 m/s 下预训练，并且在 0 - 6 m/s 之间表现平滑；更多预训练模型可在 Releases 获取）。

cd YOPO
conda activate yopo
python test_yopo_ros.py --trial=1 --epoch=50

4. 可视化

启动 RVIZ 以可视化图像和轨迹。

cd YOPO
rviz -d yopo.rviz

左：随机森林（maze_type=5）；右：3D Perlin（maze_type=1）。

new_env

你可以在 RVIZ 中点击 2D Nav Goal 作为目标点（地图是无限的，因此目标点可以自由选择），就像下面的 GIF（Flightmare Simulator）所示。

click_in_rviz

训练策略

1. 数据采集

为了提高效率，我们通过随机重置无人机状态（位置和姿态）来主动采集数据集（图像、状态和地图）。采集 100,000 个样本只需要 1-2 分钟，并且只需要采集一次。

cd Simulator
source devel/setup.bash
rosrun sensor_simulator dataset_generator

数据将保存在 ./dataset：

YOPO/
├── YOPO/
├── Simulator/
├── Controller/
├── dataset/

你可以参考 config.yaml 来修改采样状态、传感器和环境。此外，我们使用随机 vel/acc/goal 进行数据增强，其分布可在 state_samples 中查看。

2. 训练策略

cd YOPO/
conda activate yopo
python train_yopo.py

在 RTX 3080 GPU 和 i9-12900K CPU 上，使用 100,000 个样本训练 50 个 epoch 的耗时少于 1 小时。此外，如果你的 CPU 采用带 P 核和 E 核的混合架构，我们强烈建议通过 taskset -c 1,2,3,4 python train_yopo.py 将进程绑定到 P 核。如果一切顺利，训练日志如下：

cd YOPO/saved
conda activate yopo
tensorboard --logdir=./

train_log

此外，你可以参考 traj_opt.yaml 来修改轨迹优化参数（例如速度和惩罚项）。

TensorRT 部署

在真实世界飞行时，我们强烈推荐使用 TensorRT 加速。在 NVIDIA Orin NX 上推理仅需 1ms（使用 ResNet-14 Backbone）到 5ms（使用 ResNet-18 Backbone）。

1. 准备

conda activate yopo
pip install -U nvidia-tensorrt --index-url https://pypi.ngc.nvidia.com

git clone https://github.com/NVIDIA-AI-IOT/torch2trt
cd torch2trt
python setup.py install

2. PyTorch 模型转 TensorRT

cd YOPO
conda activate yopo
python yopo_trt_transfer.py --trial=1 --epoch=50

3. TensorRT 推理

cd YOPO
conda activate yopo
python test_yopo_ros.py --use_tensorrt=1

4. 适配你的平台

你需要将 test_yopo_ros.py 末尾的 env: simulation 改为 env: 435（这会影响深度图像的单位），并将里程计修改为你自己的话题（NWU 坐标系）。
配置你的深度相机，使其与训练配置一致（预训练权重使用 16:9 分辨率和 90° FOV；对于 RealSense，可以在 ROS 驱动文件中将分辨率设置为 480×270）。
在真实飞行中，你可能希望像传统规划器一样使用位置控制器，以便与你的控制器兼容。你应该将 test_yopo_ros.py 末尾的 plan_from_reference: False 改为 True。你可以在仿真中使用位置控制器测试这些修改：roslaunch so3_quadrotor_simulator simulator_position_control.launch

5. 泛化能力

我们使用随机训练场景、图像和状态来增强泛化能力。使用真实深度图像训练的策略可以零样本迁移到双目相机和未见过的场景：

sim2real

RKNN 部署

在 RK3566 芯片（仅 1 TOPS NPU）上，使用 RKNN 和 INT8 量化部署后，推理仅需约 20 ms（backbone：ResNet-14）。RK3566 或 RK3588 上的部署更新即将推出。

最后

我们仍在持续改进和重构代码，以提升可读性、可靠性和效率。如有任何技术问题，请随时联系我（lqzx1998@tju.edu.cn）😀 我们非常开放，也很乐意合作！

如果你觉得这项工作有用或有趣，请给我们一个 star ⭐；如果我们的仓库支持了你的学术项目，请引用我们的论文。谢谢！

@article{YOPO,
  title={You Only Plan Once: A Learning-based One-stage Planner with Guidance Learning},
  author={Lu, Junjie and Zhang, Xuewei and Shen, Hongming and Xu, Liwen and Tian, Bailing},
  journal={IEEE Robotics and Automation Letters},
  year={2024},
  publisher={IEEE}
}