项目主题：基于深度相机的无人机自主搜索方案 来源：第三届 3D 视觉创新应用竞赛 | 周家乐 | 杭州电子科技大学 相机型号：Astra+ 支持设备：Jetson Nano、无人机 源代码链接：RobotV_UAV-exploration
1. 项目一眼看懂：让无人机在复杂室内“自己找路”🧭
火灾、坍塌、化工事故等灾害现场常常存在视线受阻、通道狭窄、障碍物密集等问题。救援人员贸然进入，风险很高；传统地面机器人又可能被楼梯、碎石、门槛和杂物“卡脖子”。这时，小型无人机就很适合作为先遣侦察员：飞得快、进得去、能带传感器，还不怕在复杂空间里多绕两圈。
本项目面向室内灾情勘测任务，基于 Astra+ 深度相机、Jetson Nano 与无人机平台，构建了一套“感知-建图-规划-避障-控制”的自主搜索方案。它的目标不是让无人机炫技，而是让无人机在建筑物内部穿越障碍、构建环境信息，并导航到指定位置执行任务。简单说，就是让无人机在室内复杂地形里少一点“我是谁、我在哪、我往哪飞”的迷茫。
2. 为什么要做这个系统？🔥
消防救援和灾害勘测场景具有明显的高风险特征：建筑内部结构复杂、烟尘或弱光会影响视觉判断，障碍物会阻断人工巡查路径。无人机如果能够自主探索，就可以在人员进入前快速获取现场信息，为救援决策提供依据。
| 需求场景 | 传统方式痛点 | 本项目对应能力 |
|---|---|---|
| 室内灾情勘测 | 人员进入风险高 | 无人机先行探测 |
| 障碍物密集空间 | 地面机器人易受阻 | 空中平台更灵活 |
| 弱光或纹理不足 | 纯视觉识别不稳定 | 深度相机提供距离信息 |
| 未知环境导航 | 预设地图不可用 | SLAM 建图与在线规划 |
3. 系统组成：不是一架无人机，是一支“空中侦察小队”🛠️
系统由硬件平台、感知建图模块、路径规划模块、局部避障模块和飞行控制模块组成。整体逻辑可以理解为：Astra+ 负责“看见距离”，ORB-SLAM3 负责“我在哪里”，Informed RRT* 负责“往哪走”，3D VFH+ 负责“别撞墙”，控制节点负责“真的飞过去”。
| 模块 | 主要文件/资料 | 作用 |
|---|---|---|
| 深度转扫描 | depthimage_to_laserscan |
将深度图转为 LaserScan |
| 视觉 SLAM | orb_slam3-master |
RGB-D 建图与定位 |
| 路径规划 | Informed_RRT_planning.py |
全局路径搜索 |
| 局部避障 | VFH.py、Valleys.py |
直方图避障与候选通道选择 |
| 飞控接口 | tello_interface.py、controller.py |
起飞、降落、速度发布与状态控制 |
4. 工作流程：从一张深度图到一次安全飞行🚦
项目的核心流程可以概括为五步：
- Astra+ 获取 RGB-D 数据，深度图提供像素级距离信息。
- ORB-SLAM3 使用 RGB-D 输入完成无人机位姿估计与环境地图构建。
depthimage_to_laserscan将深度图压缩成类似二维激光雷达的扫描数据。
- Informed RRT* 在全局层面规划从起点到目标点的可行路径。
- 3D VFH+ 根据实时障碍物分布选择局部安全方向，并通过 MAVROS 发布速度控制。
5. 关键技术一：基于 ORB-SLAM3 的 RGB-D 室内建图📍
室内无人机探索的第一件事，是回答“我在哪里”。项目采用 ORB-SLAM3 的 RGB-D 能力，将 Astra+ 的彩色图和深度图作为输入，进行实时位姿估计和地图构建。
ORB-SLAM3 的价值在于它不仅关注单帧图像，还通过关键帧、地图点、回环检测和多地图机制维护较稳定的空间理解。对无人机而言，这相当于边飞边记路：飞过的地方会变成地图，当前相机姿态会成为后续导航的坐标基础。
| 技术环节 | 作用 | 对无人机的意义 |
|---|---|---|
| RGB-D 输入 | 同时获得纹理与深度 | 弱纹理环境更稳 |
| 关键帧选择 | 保留代表性视角 | 降低计算负担 |
| 地图点维护 | 形成空间结构 | 支撑路径规划 |
| 位姿估计 | 输出相机/机体位置 | 控制飞行方向 |
项目资料中提到，团队通过 ROS 系统连接 Astra+ 相机和 SLAM 功能包，并修改订阅参数，使建图节点能够读取相机输入数据。这一点很关键：算法再强，也得先把话题接对，不然 SLAM 就像等外卖却填错地址。
6. 关键技术二：深度图转 LaserScan，让相机“临时客串激光雷达”📡
无人机局部避障需要实时知道前方哪里有障碍。项目使用 depthimage_to_laserscan 将深度图转换为 sensor_msgs/LaserScan，从而让基于扫描数据的避障算法可以直接工作。
深度图的每个像素点可按针孔相机模型恢复到三维空间：
X = (u - cx) * Z / fx
Y = (v - cy) * Z / fy
Z = depth(u, v)在工程实现中，转换节点会选取图像中心附近若干行，将像素投影成射线，再映射到角度槽中。若多个点落入同一个角度槽，则保留更近的距离值，因为离无人机更近的障碍更需要优先处理。
来自 DepthImageToLaserScan.h 的核心逻辑可以概括为：
const float center_x = cam_model.cx();
const double unit_scaling = depthimage_to_laserscan::DepthTraits<T>::toMeters(T(1));
const float constant_x = unit_scaling / cam_model.fx();
const double th = -atan2((double)(u - center_x) * constant_x, unit_scaling);
const int index = (th - scan_msg->angle_min) / scan_msg->angle_increment;
double x = (u - center_x) * depth * constant_x;
double z = depthimage_to_laserscan::DepthTraits<T>::toMeters(depth);
double r = hypot(x, z);这段代码展示了一个非常实用的工程思想：不用真的加装激光雷达，也能把深度相机数据整理成“像激光雷达一样好用”的输入格式。对小型无人机来说，这能减少硬件重量和集成复杂度。
**7. 关键技术三：Informed RRT* 全局路径规划，先画一条“能走的大路线”🗺️**
在未知或半未知环境中，系统需要从起点到目标点规划一条避开障碍的路径。项目中的 Informed_RRT_planning.py 实现了 RRT、RRT 与 Informed RRT 相关逻辑，并在示例中设置了若干矩形障碍物。
普通 RRT 像是在地图里不断“试探伸枝”；RRT 在此基础上会不断优化路径；Informed RRT 则更聪明：一旦找到初始路径，就把采样空间收缩到一个椭圆区域中，把更多计算资源放在可能改进路径的区域。通俗讲，就是少做无意义乱逛，多做有希望的尝试。
项目代码中的核心思路如下：
cMin = math.sqrt(pow(self.start.x - self.goal.x, 2)
+ pow(self.start.y - self.goal.y, 2))
xCenter = np.array([[(self.start.x + self.goal.x) / 2.0],
[(self.start.y + self.goal.y) / 2.0], [0]])
for i in range(self.max_iter):
rnd = self.informed_sample(cBest, cMin, xCenter, C)
n_ind = self.get_nearest_list_index(self.node_list, rnd)
nearestNode = self.node_list[n_ind]
theta = limit_angle(math.atan2(rnd[1] - nearestNode.y,
rnd[0] - nearestNode.x))同时，代码还对障碍物进行膨胀处理：
def dilateObstacle(self, bigger_size):
for i in range(int(self.num_obstacle)):
self.points_for_obstacle[i*self.side, 0] -= bigger_size
self.points_for_obstacle[i*self.side, 1] += bigger_size障碍膨胀的意义很直接：无人机不是数学上的一个点，它有机身尺寸、旋翼范围和控制误差。提前把障碍物“变胖一点”，能给真实飞行留出安全余量。
8. 关键技术四：3D VFH+ 局部避障，面对障碍不硬刚🧱
全局路径告诉无人机“总体往哪里去”，但飞行过程中会遇到局部障碍和噪声，这时就需要实时避障。项目使用 3D VFH+ 思想，将障碍分布转换为扇区直方图，并从可通行的“谷区”中选出较优方向。
tello_interface.py 中设置了局部避障参数：
SAFE_DISTANCE = 0.2
WINDOW_RADIUS = 2
ROBOT_RADIUS = 0.2
VEL = 0.6
ANGULAR_VEL = 2.25
NUM_SECTOR = 150
CELL_SIZE = 0.2
T_HIGH = 270
T_LOW = 250
WIDE_THRESH = 9这些参数共同决定了无人机“看多远”“多保守”“飞多快”和“如何划分方向”。例如 SAFE_DISTANCE 与 ROBOT_RADIUS 共同构成安全半径，NUM_SECTOR 决定直方图角度分辨率，T_HIGH/T_LOW 用于判断某个方向是否被障碍占据。
候选通道选择的代价函数来自 Valleys.py：
def gfunction(self, selected_sector, current_sector, target_sector, previous_sector):
firstpart = self.miu1 * self.min_between_sector(selected_sector, target_sector)
secondpart = self.miu2 * self.min_between_sector(selected_sector, current_sector)
thirdpart = self.miu3 * self.min_between_sector(selected_sector, previous_sector)
return firstpart + secondpart + thirdpart这个代价函数同时考虑三件事：
| 评价项 | 含义 | 效果 |
|---|---|---|
| 靠近目标方向 | 少绕路 | 提高任务效率 |
| 靠近当前方向 | 少急转 | 飞行更平滑 |
| 靠近上一方向 | 少抖动 | 避免左右横跳 |
所以，3D VFH+ 不是见缝就钻，而是在安全、目标、平滑性之间做折中。说得生活化一点：它会绕路，但不会乱扭。
9. 控制层：从“规划结果”到“无人机真的动起来”🎮
项目的控制层主要由 controller.py 和 tello_interface.py 负责。controller.py 负责状态机逻辑，例如等待、起飞、导航、到达目标后降落；tello_interface.py 则通过 MAVROS 与 Gazebo 状态话题获取无人机姿态，并向速度控制话题发布 Twist 指令。
简化后的控制流程如下：
if flight_state == WAITING:
publish("takeoff")
if takeoff_finished:
flight_state = NAVIGATING
if flight_state == NAVIGATING:
publish("Start VFH")
if distance_to_goal < threshold:
publish("Stop VFH")
publish("land")在 tello_interface.py 中，局部避障会输出速度指令：
Twist1 = Twist()
Twist1.linear.x = velocity
Twist1.linear.y = 0
Twist1.angular.z = angle_velocity * sin(select_angle)
if self.t_wu_[2] > 1.8:
Twist1.linear.z = -0.1
elif self.t_wu_[2] < 1.25:
Twist1.linear.z = 0.05
self.uav_target_speed_local_pub_.publish(Twist1)这里既有水平避障，也有高度约束：无人机太高就往下压一点，太低就往上提一点。这个小逻辑很朴素，但对室内飞行很有用，因为天花板和地面都不太欢迎旋翼来“亲密接触”。
10. 测试方案：先在仿真里练会，再考虑现实里飞🧪
项目采用虚拟仿真环境和现实模拟场景结合的思路。Gazebo 用于构建室内障碍环境，ROS 负责消息通信，MAVROS 连接无人机飞控接口，Astra+ 提供深度信息。仿真测试可以降低调试风险，也便于复现实验。
| 测试内容 | 验证目标 | 观察指标 |
|---|---|---|
| RGB-D 建图 | 是否能形成室内地图 | 位姿连续性、地图稳定性 |
| 深度转扫描 | 是否能输出可用障碍距离 | 扫描范围、噪声、空洞 |
| 全局规划 | 是否能生成可行路径 | 路径长度、碰撞检测 |
| 局部避障 | 是否能绕过近距离障碍 | 转向平滑性、安全距离 |
| 飞行控制 | 是否能到达目标点 | 到达误差、是否稳定降落 |
11. 难点与创新：这套方案真正难在哪里？🧠
| 类别 | 具体内容 | 项目处理思路 |
|---|---|---|
| 光照变化 | 火场、室内弱光影响视觉 | 引入深度相机补充距离感知 |
| 机体抖动 | 飞行导致深度图噪声增大 | 通过局部窗口和阈值过滤缓解 |
| 算力限制 | Jetson Nano 资源有限 | 关键帧选择与局部规划降低负担 |
| 窄通道避障 | 室内障碍密集 | 3D VFH+ 选择可通行谷区 |
| 路径平滑 | 四旋翼不能像鼠标指针一样瞬移 | 代价函数约束转向变化 |
项目的创新点可以概括为：基于 Astra+ 深度感知，将 VSLAM、局部地图、全局路径规划和实时避障整合到无人机室内搜索任务中，使无人机能够在复杂障碍环境下完成自主探索闭环。
12. 项目价值：灾害现场里的“空中探路员”🚨
这套方案的意义不只是“无人机能飞”，而是让无人机具备一定自主环境理解能力。在灾害勘测、室内巡检、仓库盘点、危险区域侦察等场景中，它都具有扩展潜力。
| 应用方向 | 可迁移能力 |
|---|---|
| 消防搜救 | 室内快速探查、辅助决策 |
| 工业巡检 | 狭窄空间自主巡航 |
| 仓储物流 | 复杂货架间路径规划 |
| 应急侦察 | 人员进入前风险评估 |
| 科研教学 | ROS、SLAM、规划与控制综合实验 |
13. 总结：会看路、会绕路、还能稳住心态的无人机✅
基于深度相机的无人机自主搜索方案，将 Astra+ 深度感知、ORB-SLAM3 RGB-D 建图、depthimage_to_laserscan 数据转换、Informed RRT* 全局路径规划和 3D VFH+ 局部避障整合在一起，形成了一条较完整的室内自主探索技术链路。
从学术角度看，它覆盖了机器人领域中感知、定位、建图、路径规划、运动控制等关键问题；从应用角度看，它面向灾情勘测这一真实需求，强调复杂环境中的安全性、实时性和可部署性。换句话说，这不是一架只会“起飞拍照”的无人机，而是一套正在学习如何在室内复杂环境里认真找路的自主系统。