基于先验地图的无人机全局定位与在线地图增量更新
1. 前言
在室内无人机应用中，通常可以提前获得建筑物的二维平面图，例如房间、走廊以及墙体等结构信息。但这类地图往往只描述建筑本身的固定结构，并不包含实际环境中的桌椅、设备等室内物体。因此，如果无人机仅依靠自身传感器进行 SLAM，只能逐渐建立一个以初始位置为参考的局部地图，很难直接确定自己在已有建筑地图中的绝对位置。基于这一问题，本项目尝试利用已有的二维建筑地图作为先验信息，结合无人机搭载的 Mid360 激光雷达和 IMU，实现无人机在先验地图坐标系下的全局定位，并利用飞行过程中实时获取的点云信息不断补充原有地图中缺失的环境信息，从而实现先验地图辅助的全局定位与在线地图增量更新。
tips: 本文所有实验基于已经配置好的XTDrone环境，想要完整复现请确保已经配置好了环境。
下图为设计的整体技术路线，后续所有工作基本围绕这条链路展开。首先让无人机建立自己的局部定位坐标系，然后将该坐标系与已有先验地图进行对齐，最后利用实时点云不断补充先验地图中不存在的环境信息。
2. 先验地图构建
本文采用的室内地图为~/PX4_Firmware/launch/indoor2.launch,可以先启动gazebo移除里面的桌椅后保存得到室内结构信息的world。为了得到二维先验地图，简单起见采用hector得到了map.pgm和对应的yaml文件（注意hector_mapping_xtdrone.launch文件需要调整地图位置和大小参数，否则在部分地图无法显示；还有无人机不要飞的太高，否则会有噪声）。下面是对应的原始世界，移除障碍物后的世界以及生成的map.pgm：
对应的yaml文件内容：
点击查看代码
image: /home/chime/my_map.pgm # 写绝对路径，pgm是灰度格式
resolution: 0.050000 # 1个像素代表现实世界5cm，越小精度越高
origin: [-10.024999, -10.024999, 0.000000] # [x,y,yaw],地图左下角像素在map坐标系下的坐标
negate: 0 # 不反转黑白像素，黑色是障碍物
occupied_thresh: 0.65 # 归一化灰度后大于此值则视为障碍物
free_thresh: 0.196 # 归一化灰度后小于此值则视为空闲
点击查看代码
<!-- Map size / start point -->
<param name="map_resolution" value="0.050"/>
<param name="map_size" value="400"/>
<param name="map_start_x" value="0.5"/> #这两个0.5代表了将机器人初始位置放在当前地图的中心，范围[0,1]
<param name="map_start_y" value="0.5" />
<param name="map_multi_res_levels" value="2" />
3. 无人机传感器配置
仿真实验核心感知传感器为 Livox Mid360 激光雷达与 IMU，相机预留后续扩展使用。由于 Mid360 采用非重复式扫描机制，拥有专属的自定义点云消息格式，Gazebo 原生激光插件无法模拟该雷达的输出特性。因此仿真环境同样需要配置 Livox 官方驱动，用于仿真端生成与真实硬件一致的点云数据与消息类型，保证 FAST‑LIO 算法在仿真、实机两套场景下代码通用，降低仿真到实物迁移的适配成本。由于 FAST-LIO 只接受输入话题类型为/CustomMsg，因此直接采用livox_ros_driver驱动即可。
点击查看代码
cd ~/catkin_ws/src
git clone https://github.com/Luchuanzhao/Livox_simulation_customMsg.git
# 先安装驱动的地层依赖库，没有SDK，ROS驱动无法编译
git clone https://github.com/Livox-SDK/Livox-SDK.git
cd Livox-SDK
cd build && cmake ..
make
sudo make install
cd ~/catkin_ws/src
git clone https://github.com/Livox-SDK/livox_ros_driver.git
mv Livox_simulation_customMsg livox_laser_simulation
cd ~/catkin_ws/src/livox_laser_simulation/src
vim livox_points_plugin.cpp
# 将std::string file_name这一行改成下面内容
std::string file_name = sdf->Get<std::string>("csv_file_name");
在~/PX4_Firmware/Tools/sitl_gazebo/models文件夹下新建Mid360文件夹，然后按照下图创建文件夹，将红框圈出的Mid360配置文件拷贝到对应的文件夹中。
Mid360.sdf激光雷达模型文件：
点击查看代码
<sdf version='1.6'>
<model name='Mid360'>
<link name='link'>
<pose frame=''>0 0 0 0 -0 0</pose>
<inertial>
<pose frame=''>0 0 0 0 -0 0</pose>
<mass>0.2</mass>
<inertia>
<ixx>0.02</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.02</iyy>
<iyz>0</iyz>
<izz>0.02</izz>
</inertia>
</inertial>
<collision name='mid360_base_collision'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<uri>model://Mid360/meshes/mid360_rules.dae</uri>
</mesh>
</geometry>
</collision>
<visual name='mid360_base_visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<uri>model://Mid360/meshes/mid360.dae</uri>
</mesh>
</geometry>
<material>
<script>
<name>Gazebo/Grey</name>
<uri>file://media/materials/scripts/gazebo.material</uri>
</script>
</material>
</visual>
<gravity>1</gravity>
<velocity_decay/>
<sensor name='laser_mid360' type='ray'>
<visualize>0</visualize>
<update_rate>10</update_rate>
<always_on>1</always_on>
<plugin name='gazebo_ros_laser_controller' filename='liblivox_laser_simulation.so'>
<ray>
<scan>
<horizontal>
<samples>100</samples>
<resolution>1</resolution>
<min_angle>0</min_angle>
<max_angle>6.28</max_angle>
</horizontal>
<vertical>
<samples>360</samples>
<resolution>1</resolution>
<min_angle>-0.122173</min_angle>
<max_angle>0.907571</max_angle>
</vertical>
</scan>
<range>
<min>0.1</min>
<max>8</max>
<resolution>0.02</resolution>
</range>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.0</stddev>
</noise>
</ray>
<visualize>1</visualize>
<samples>24000</samples>
<downsample>1</downsample>
<csv_file_name>/home/chime/PX4_Firmware/Tools/sitl_gazebo/models/Mid360/scan_mode/mid360.csv</csv_file_name>
<ros_topic>/scan</ros_topic>
</plugin>
<pose frame=''>-0.005 0.005 0.047 0 -0 0</pose>
</sensor>
</link>
</model>
</sdf>
然后需要在无人机机体中加入这个激光雷达模型,在相同路径下创建iris_mid360文件夹，在文件夹中创建iris_mid360.sdf模型文件，然后将~/PX4_Firmware/Tools/sitl_gazebo/models/iris_3d_lidar/iris_3d_lidar.sdf内容完全拷贝进去，替换model://3d_lidar和stereo_camera::link为以下内容，其余不变：
点击查看代码
<include>
<uri>model://Mid360</uri>
<pose>0 0 0.05 0 0 0</pose>
</include>
<joint name="livox_joint" type="fixed">
<child>Mid360::link</child>
<parent>base_link</parent>
<axis>
<xyz>0 0 1</xyz>
<limit>
<upper>0</upper>
<lower>0</lower>
</limit>
</axis>
</joint>
<include>
<uri>model://realsense_camera</uri>
<pose>0.1 0 0 0 0 0</pose>
</include>
<joint name="realsense_camera_joint" type="fixed">
<child>realsense_camera::link</child>
<parent>base_link</parent>
<axis>
<xyz>0 0 1</xyz>
<limit>
<upper>0</upper>
<lower>0</lower>
</limit>
</axis>
</joint>
最后修改~/PX4_Firmware/launch/indoor2.launch中的sdf为iris_mid360，重新打开roslaunch px4 indoor2.launch得到：
4. FAST_LIO配置
FAST_LIO是一款紧耦合激光惯性里程计算法，目的是融合激光雷达与 IMU 的数据，解算无人机的运动位姿，同时输出配准后的点云地图，为后续建图、探索规划模块提供位姿与点云输入。参考以下步骤进行配置：
点击查看代码
cd ~/catkin_ws/src
git clone https://github.com/gaoxiang12/faster-lio.git
# 在target_link_libraries中增加`tbbmalloc` `tbbmalloc_proxy`
vim ~/catkin_ws/src/faster-lio/src/CMakeLists.txt
cd ~/catkin_ws
catkin build
需要修改~/catkin_ws/src/FAST_LIO/config/mid360.yaml中输入激光雷达和 IMU 的话题分别为/scan和/iris_0/imu_gazebo。其次由于Fast-LIO（激光里程计）属于外部视觉+里程计输入，一定要确定KEF修改过来了，~/PX4_Firmware/build/px4_sitl_default/etc/init.d-posix/rcS:
由于 Fast-lio 输出为 /Odometry 话题，其类型为nav_msgs/Odometry，而 PX4 固件期望接收特定格式的里程计消息，因此需要通过mavros进行格式转换，把LIO解算得到的机体位姿与速度信息转发给PX4，供飞控实现视觉 / 激光里程计辅助定位。进入~/XTDrone/sensing/slam/laser_slam/script文件夹，保存下面文件为laser_transfer_copy.py：
点击查看代码
import rospy
from geometry_msgs.msg import PoseStamped
from tf2_ros import TransformListener, Buffer
import sys
from sensor_msgs.msg import LaserScan
from nav_msgs.msg import Odometry
from gazebo_msgs.msg import ModelStates
from nav_msgs.msg import Path
vehicle_type = sys.argv[1]
vehicle_id = sys.argv[2]
laser_slam_type = sys.argv[3]
rospy.init_node(vehicle_type+vehicle_id+'_'+laser_slam_type+'_laser_transfer')
pose_pub = rospy.Publisher(vehicle_type+'_'+ vehicle_id+"/mavros/vision_pose/pose", PoseStamped, queue_size=1)
tfBuffer = Buffer()
tflistener = TransformListener(tfBuffer)
local_pose = PoseStamped()
local_pose.header.frame_id = 'map'
hector = PoseStamped()
height = 0
# 新增：用于 faster_lio 的回调
def odom_callback(odom_msg):
pose_msg = PoseStamped()
pose_msg.header = odom_msg.header
pose_msg.pose = odom_msg.pose.pose
pose_pub.publish(pose_msg)
def hector_callback(data):
global hector
hector = data
def height_distance_callback(msg):
global height
height = msg.ranges[0]
if(height==float("inf")):
height = 0
def hector_slam():
global local_pose, height
pose2d_sub = rospy.Subscriber(vehicle_type+'_'+ vehicle_id+"/pose", PoseStamped, hector_callback,queue_size=1)
rate = rospy.Rate(100)
while True:
local_pose = hector
local_pose.pose.position.z = height
pose_pub.publish(local_pose)
rate.sleep()
def cartographer():
global local_pose, height
rate = rospy.Rate(30)
while not rospy.is_shutdown():
try:
tfstamped = tfBuffer.lookup_transform('map', 'base_link', rospy.Time(0))
except:
continue
local_pose.header.stamp = rospy.Time().now()
local_pose.pose.position = tfstamped.transform.translation
local_pose.pose.position.z = height
local_pose.pose.orientation = tfstamped.transform.rotation
pose_pub.publish(local_pose)
rate.sleep()
def aloam():
global local_pose
rate = rospy.Rate(30)
while not rospy.is_shutdown():
try:
tfstamped = tfBuffer.lookup_transform('camera_init', 'aft_mapped', rospy.Time(0))
except:
continue
local_pose.header.stamp = rospy.Time().now()
local_pose.pose.position = tfstamped.transform.translation
local_pose.pose.orientation = tfstamped.transform.rotation
pose_pub.publish(local_pose)
rate.sleep()
# 新增：faster_lio 转换函数（直接订阅 /Odometry）
def faster_lio():
rospy.loginfo("Starting faster_lio converter, subscribing to /Odometry")
# 初始化路径
path = Path()
path.header.frame_id = "camera_init"
# 创建 Path 发布器
path_pub = rospy.Publisher('/iris_0/path', Path, queue_size=10)
global_path_pub = rospy.Publisher('/path', Path, queue_size=10)
last_odom = None
def odom_callback(odom_msg):
nonlocal last_odom
last_odom = odom_msg
# 转发给 PX4
pose_msg = PoseStamped()
pose_msg.header = odom_msg.header
pose_msg.pose = odom_msg.pose.pose
pose_pub.publish(pose_msg)
# 更新并发布路径
pose = PoseStamped()
pose.header = odom_msg.header
pose.pose = odom_msg.pose.pose
path.poses.append(pose)
path.header.stamp = rospy.Time.now()
if len(path.poses) > 10000:
path.poses.pop(0)
path_pub.publish(path)
global_path_pub.publish(path)
rospy.Subscriber("/Odometry", Odometry, odom_callback)
# 保持节点运行并定期发布路径
rate = rospy.Rate(10)
while not rospy.is_shutdown():
if last_odom is not None:
path_pub.publish(path)
global_path_pub.publish(path)
rate.sleep()
# 新增：用于 Gazebo 真值（Ground Truth）的回调
def gazebo_gt_callback(model_states_msg):
# 根据无人机名称提取位姿
if vehicle_type+'_'+vehicle_id not in model_states_msg.name:
return
idx = model_states_msg.name.index(vehicle_type+'_'+vehicle_id)
pose_msg = PoseStamped()
# 手动设置时间戳和坐标系
pose_msg.header.stamp = rospy.Time.now()
pose_msg.header.frame_id = "map"
pose_msg.pose = model_states_msg.pose[idx]
pose_pub.publish(pose_msg)
def publish_gazebo_gt():
rospy.loginfo("Publishing Gazebo ground truth for %s_%s to vision_pose/pose", vehicle_type, vehicle_id)
rospy.Subscriber("/gazebo/model_states", ModelStates, gazebo_gt_callback)
rospy.spin()
if __name__ == '__main__':
if laser_slam_type == 'hector':
height_distance_sub = rospy.Subscriber(vehicle_type+'_'+ vehicle_id+"/distance", LaserScan, height_distance_callback,queue_size=1)
hector_slam()
elif laser_slam_type == 'cartographer':
height_distance_sub = rospy.Subscriber(vehicle_type+'_'+ vehicle_id+"/distance", LaserScan, height_distance_callback,queue_size=1)
cartographer()
elif laser_slam_type == 'aloam':
aloam()
elif laser_slam_type == 'faster_lio':
faster_lio()
elif laser_slam_type == 'ground_truth': # 新增分支
publish_gazebo_gt()
else:
print('input error')
运行：
点击查看代码
# 终端1
roslaunch px4 indoor2.launch
# 终端2
roslaunch fast_lio mapping_mid360.launch
# 终端3
cd ~/XTDrone/sensing/slam/laser_slam/script
python3 laser_transfer_copy.py iris 0 faster_lio
# 终端4
cd ~/XTDrone/communication
python3 multirotor_communication.py iris 0
# 终端5
cd ~/XTDrone/control/keyboard
python3 multirotor_keyboard_control.py iris 1 vel
移动无人机会输出/path和/cloud_registered话题。其中/path代表了无人机相对于camera_init（初始出发位置）的运动轨迹；cloud_registered为在camera_init坐标系下配准后的三维点云地图，如下图所示。
RViz 中红色代表 Gazebo 仿真的激光扫描射线，紫色为配准后的三维点云地图，蓝色为相对路径。
5. 三维体素地图转二维占据栅格地图
前面通过 FAST-LIO 已经可以获得无人机的实时位姿以及环境三维点云。但是，本项目最初获得的先验地图是一张二维建筑平面图，最终需要解决的是“先验二维地图 + 无人机实时感知环境”的融合问题。因此，直接使用三维点云与二维先验地图进行处理并不方便，需要先将 FAST-LIO 建立的三维环境地图转换为二维占据栅格地图。
这里采用 OctoMap 完成这一过程。整体流程比较简单：
FAST-LIO --> /cloud_registered --> OctoMap --> 三维八叉树占据地图 --> 二维投影 --> /projected_map
OctoMap 是一种基于八叉树（Octree）的三维占据栅格地图表示方法。它将三维空间递归划分为大小不同的立方体体素，并通过概率模型记录每个体素处于“占据”“空闲”或“未知”状态。相比直接保存大量三维点云，OctoMap更关注空间是否被占据，因此可以将点云转换成一种更适合环境建模的地图表示。最终得到的二维地图通过 ROS 的 nav_msgs/OccupancyGrid 类型发布，可以直接在 RViz 中显示。需要注意的是，OctoMap本身并不负责无人机的定位。FAST-LIO负责根据激光雷达和 IMU 估计无人机的运动并建立点云地图，而 OctoMap主要负责对这些点云进行占据空间建模以及二维投影。在整个系统中，它属于“地图表达与转换”这一环节。
为了避免直接修改 FAST-LIO 或 OctoMap 原有程序，本文单独建立了一个 fastlio_octomap 功能包，用于完成二者之间的数据连接。实际上，这一步并不需要自己重新实现 OctoMap 算法。octomap_server已经提供了现成的 ROS 节点，只需要将 FAST-LIO 输出的 PointCloud2 数据重新映射到 OctoMap 的输入话题，并根据实际场景配置相关参数即可。执行下面命令：
点击查看代码
cd ~/catkin_ws/src
catkin_create_pkg fastlio_octomap nav_msgs octomap_ros octomap_server roscpp rospy sensor_msgs tf
cd fastlio_octomap
mkdir launch
cd ~/catkin_ws
catkin build fastlio_octomap
source devel/setup.bash
点击查看代码
<launch>
<!--
FAST-LIO + OctoMap
PointCloud2 -> OctoMap -> 2D OccupancyGrid
-->
<node pkg="octomap_server"
type="octomap_server_node"
name="octomap_server"
output="screen">
<!-- OctoMap resolution -->
<param name="resolution" value="0.05"/>
<!-- Global coordinate frame -->
<param name="frame_id" value="camera_init"/>
<!-- Maximum LiDAR integration range -->
<param name="sensor_model/max_range" value="20.0"/>
<!-- Point cloud height filtering -->
<param name="pointcloud_min_z" value="-2.0"/>
<param name="pointcloud_max_z" value="3.0"/>
<!-- Height range used for 2D projection -->
<param name="occupancy_min_z" value="0.0"/>
<param name="occupancy_max_z" value="2.0"/>
<!-- Ground filtering -->
<param name="filter_ground" value="false"/>
<!-- Update 2D map incrementally -->
<param name="incremental_2D_projection" value="true"/>
<!-- FAST-LIO point cloud -->
<remap from="cloud_in"
to="/cloud_registered"/>
</node>
</launch>
这里设置 OctoMap 的体素分辨率为0.05m，和前面建立的先验二维地图保持一致。这里将 OctoMap 使用的全局坐标系设置为camera_init，因此，FAST-LIO 输出的/cloud_registered点云以及 OctoMap 生成的地图都处于同一个局部坐标系中。此时系统的关系可以表示为：
camera_init
├── FAST-LIO
└── OctoMap
而之前建立的建筑先验地图使用的是map坐标系。因此，后续需要通过：map --> camera_init --> body 建立两套坐标系之间的联系。这样才能最终得到无人机在先验地图中的全局位置。
第三个参数是激光雷达的最大融合距离，这里设置为20m，这个参数在实际调试过程中比较重要。最初如果允许激光点云在很大的范围内进行地图更新，OctoMap生成的地图范围可能会非常大。尤其是在室内环境中，如果雷达的有效探测距离设置过大，就容易产生一个远大于实际建筑区域的/projected_map。
pointcloud_min_z和pointcloud_max_z两个参数用于限制进入 OctoMap 的点云高度范围。这里设置为：-2 m ≤ z ≤ 3 m，主要目的是过滤掉明显不需要参与地图构建的异常高度点，同时避免一些远距离或异常点影响地图范围。需要区分这一组参数和下面的二维投影高度范围。前者决定“哪些点进入 OctoMap”，后者决定“哪些高度范围参与二维地图投影”，二者并不是同一个概念。
二维投影使用：occupancy_min_z和occupancy_max_z，也就是只将：0 m ≤ z ≤ 2 m 范围内的占据体素投影到二维平面。对于室内无人机环境来说，这样处理比较符合任务需求，因为最终关注的是无人机周围具有实际碰撞意义的墙体、桌椅等障碍物，而不是完整保留所有高度方向的信息。
地面过滤暂时关闭，因为当前阶段的主要目标是先完成“FAST-LIO 三维点云 --> OctoMap --> 二维地图”的完整数据链路，而不是对地面进行复杂的分割。如果后续需要进一步提高二维障碍物地图的质量，可以再考虑地面分割或者更加精细的高度过滤。
incremental_2D_projection是开启二维地图的增量更新。这一参数与本项目的任务目标直接相关。无人机不是一次性获得整个环境地图，而是在飞行过程中不断获取新的激光点云。因此，OctoMap也需要随着新的点云数据到来不断更新。这样生成的/projected_map就可以作为后续在线地图更新的一部分。
最后通过 remap 将 FAST-LIO 的点云输入到octomap_server，<remap from="cloud_in" to="/cloud_registered"/>这里的/cloud_registered是FAST-LIO 输出的已经经过位姿估计和坐标变换后的点云。因此，整个 ROS 数据流实际上非常简单：
OctoMap会输出\projected_map话题，代表实时转换的二维地图。在 RViz 中添加 Map 类型的显示，并将 Topic 设置为：\projected_map即可看到由 FAST-LIO 实时点云投影得到的二维在线地图。
至此，就完成了从 FAST-LIO 三维点云到二维占据栅格地图的转换。后面需要解决的核心问题就变成了：如何将这个以 camera_init 为坐标系的在线地图，与已有的 map 先验地图进行空间对齐，并最终在同一个全局坐标系下进行定位和地图更新。
6. 先验地图与在线地图的坐标对齐
目前我们得到了两张二维地图，一张是提前建立好的建筑先验地图/map，另一张是无人机飞行过程中实时生成的/projected_map。虽然两张地图描述的是同一个室内环境，但它们不能直接进行叠加。原因在于两张地图使用的坐标系不同。先验地图位于全局map坐标系下，而FAST-LIO输出的在线地图位于camera_init坐标系下。因此，即使两张地图中的墙体、房间边界等结构完全对应，如果不知道map与camera_init之间的旋转和平移关系，直接在Rviz中显示时仍然无法重合。
因此，本项目在地图融合之前加入了先验地图的全局定位模块，该模块同时接收先验地图/prior_map和在线地图/projected_map，通过提取两张地图中的结构点，并搜索两者之间的二维刚体变换，自动估计在线地图在先验地图坐标系中的位置和朝向。定位成功后，将得到的变换作为map-->camera_init持续发布，从而使FAST-LIO后续输出的无人机位姿转换到全局map坐标系中。
6.1 导入先验地图
在前面的建图过程中，我们已经获得了建筑环境的二维先验地图，并保存为PGM图像和YAML配置文件。为了让后续的定位模块能够直接使用这张地图，首先编写prior_map_loader节点，将PGM/YAML格式的地图转换为ROS中标准的nav_msgs/OccupancyGrid消息。
请执行以下命令：
点击查看代码
cd ~/catkin_ws/src
git clone https://gitcode.com/gh_mirrors/ya/yaml-cpp
cd yaml-cpp
cmake .
sudo make install
cd ~/catkin_ws/src
catkin_create_pkg prior_map_loader roscpp nav_msgs sensor_msgs yaml-cpp
cd prior_map_loader
mkdir -p src
gedit /src/prior_map_loader.cpp
写入以下内容，注意路径按自己环境写：
点击查看代码
#include <ros/ros.h>
#include <ros/package.h>
#include <nav_msgs/OccupancyGrid.h>
#include <yaml-cpp/yaml.h>
#include <opencv2/opencv.hpp>
#include <string>
#include <vector>
#include <cmath>
class PriorMapLoader
{
public:
PriorMapLoader(ros::NodeHandle& nh, ros::NodeHandle& pnh)
{
pnh.param<std::string>("yaml_file", yaml_file_, "/home/chime/my_map.yaml");
pnh.param<std::string>("frame_id", frame_id_, "camera_init");
pnh.param<std::string>("map_topic", map_topic_, "/prior_map");
pnh.param<double>("publish_rate", publish_rate_, 1.0);
loadMap();
map_pub_ = nh.advertise<nav_msgs::OccupancyGrid>(map_topic_, 1, true);
timer_ = nh.createTimer(
ros::Duration(1.0 / publish_rate_),
&PriorMapLoader::publishMap,
this
);
ROS_INFO("========================================");
ROS_INFO(" Prior Map Loader");
ROS_INFO(" YAML : %s", yaml_file_.c_str());
ROS_INFO(" Frame : %s", frame_id_.c_str());
ROS_INFO(" Topic : %s", map_topic_.c_str());
ROS_INFO(" Map size : %d x %d", map_.info.width, map_.info.height);
ROS_INFO(" Resolution: %.3f m", map_.info.resolution);
ROS_INFO(" Origin : [%.3f, %.3f, %.3f]",
map_.info.origin.position.x,
map_.info.origin.position.y,
map_.info.origin.position.z);
ROS_INFO("========================================");
}
private:
void loadMap()
{
try
{
YAML::Node config = YAML::LoadFile(yaml_file_);
// -----------------------------
// 读取 YAML 参数
// -----------------------------
std::string image_file = config["image"].as<std::string>();
resolution_ = config["resolution"].as<double>();
origin_x_ = config["origin"][0].as<double>();
origin_y_ = config["origin"][1].as<double>();
origin_yaw_ = 0.0;
if (config["origin"].size() >= 3)
{
origin_yaw_ = config["origin"][2].as<double>();
}
negate_ = 0;
if (config["negate"])
negate_ = config["negate"].as<int>();
occupied_thresh_ = 0.65;
if (config["occupied_thresh"])
occupied_thresh_ = config["occupied_thresh"].as<double>();
free_thresh_ = 0.196;
if (config["free_thresh"])
free_thresh_ = config["free_thresh"].as<double>();
// -----------------------------
// 处理 image 路径
// -----------------------------
std::string map_dir = ros::package::getPath("prior_map_loader"); // 推荐方式
if (map_dir.empty())
map_dir = ".";
// 如果 image 是绝对路径或已经在 / 目录下，保留原样
if (image_file.empty() || image_file[0] == '/' || image_file.find(":") != std::string::npos)
{
// 保持原样
}
else
{
// 拼接当前工作目录（map_server 常见用法）
image_file = map_dir + "/" + image_file;
}
cv::Mat image = cv::imread(image_file, cv::IMREAD_GRAYSCALE);
if (image.empty())
{
ROS_ERROR("Cannot load map image: %s", image_file.c_str());
ros::shutdown();
return;
}
const int width = image.cols;
const int height = image.rows;
// -----------------------------
// OccupancyGrid 基本信息
// -----------------------------
map_.header.frame_id = frame_id_;
map_.info.map_load_time = ros::Time::now();
map_.info.resolution = resolution_;
map_.info.width = width;
map_.info.height = height;
map_.info.origin.position.x = origin_x_;
map_.info.origin.position.y = origin_y_;
map_.info.origin.position.z = 0.0;
double half_yaw = origin_yaw_ * 0.5;
map_.info.origin.orientation.x = 0.0;
map_.info.origin.orientation.y = 0.0;
map_.info.origin.orientation.z = std::sin(half_yaw);
map_.info.origin.orientation.w = std::cos(half_yaw);
map_.data.resize(width * height);
// -----------------------------
// PGM -> OccupancyGrid 转换（含上下翻转）
// -----------------------------
for (int y = 0; y < height; ++y)
{
for (int x = 0; x < width; ++x)
{
int image_y = height - 1 - y;
unsigned char pixel = image.at<unsigned char>(image_y, x);
double value = static_cast<double>(pixel) / 255.0;
if (negate_)
value = 1.0 - value;
int index = y * width + x;
if (value >= occupied_thresh_)
map_.data[index] = 0;
else if (value <= free_thresh_)
map_.data[index] = 100;
else
map_.data[index] = -1;
}
}
ROS_INFO("Map image loaded successfully from: %s", image_file.c_str());
}
catch (const YAML::Exception& e)
{
ROS_ERROR("YAML error: %s", e.what());
ros::shutdown();
}
catch (const cv::Exception& e)
{
ROS_ERROR("OpenCV error: %s", e.what());
ros::shutdown();
}
}
void publishMap(const ros::TimerEvent&)
{
map_.header.stamp = ros::Time::now();
map_pub_.publish(map_);
}
private:
ros::Publisher map_pub_;
ros::Timer timer_;
nav_msgs::OccupancyGrid map_;
std::string yaml_file_;
std::string frame_id_;
std::string map_topic_;
double publish_rate_;
double resolution_;
double origin_x_;
double origin_y_;
double origin_yaw_;
int negate_;
double occupied_thresh_;
double free_thresh_;
};
int main(int argc, char** argv)
{
ros::init(argc, argv, "prior_map_loader");
ros::NodeHandle nh;
ros::NodeHandle pnh("~");
PriorMapLoader loader(nh, pnh);
ros::spin();
return 0;
}
点击查看代码
gedit ~/catkin_ws/src/prior_map_loader/CMakeLists.txt
# 补充下面文件
find_package(catkin REQUIRED COMPONENTS
nav_msgs
roscpp
roslib
sensor_msgs
tf2_ros
)
find_package(OpenCV REQUIRED)
find_package(yaml-cpp REQUIRED)
include_directories(
${catkin_INCLUDE_DIRS}
${OpenCV_INCLUDE_DIRS}
)
add_executable(prior_map_loader src/prior_map_loader.cpp)
target_link_libraries(prior_map_loader
${catkin_LIBRARIES}
${OpenCV_LIBS}
yaml-cpp::yaml-cpp
)
gedit ~/catkin_ws/src/prior_map_loader/package.xml
# 补充下面内容
<build_depend>yaml-cpp</build_depend>
<build_depend>opencv</build_depend>
<build_depend>roslib</build_depend>
<build_export_depend>yaml-cpp</build_export_depend>
<build_export_depend>opencv</build_export_depend>
<exec_depend>yaml-cpp</exec_depend>
<exec_depend>opencv</exec_depend>
cd catkin_ws
catkin build prior_map_loader -j1
source devel/setup.bash
程序首先读取YAML文件中的地图参数，包括地图图像路径、分辨率、原点位置以及占用和空闲区域的阈值。随后使用OpenCV读取PGM灰度图，并根据地图分辨率将图像转换为栅格地图。由于图像坐标系和 ROS OccupancyGrid 的坐标方向存在差异，程序在转换过程中对图像 Y 方向进行了翻转，是生成的栅格地图能够与 ROS 世界坐标系保持一致。转换完成后，程序将地图发布到prior_map，并将坐标系设置为map。同时，地图采用latched publisher发布，使得后续启动的节点也能够直接获得已经加载完成的先验地图。
下图是启动rosrun prior_map_loader prior_map_loader后Rviz中订阅prior_map得到的结果，注意frame还是camera_init:
再启动roslaunch fastlio_octomap octomap_mapping.launch在Rviz中加入在线地图如下：
6.2 全局定位
获得了/prior_map和/projected_map后，下一步需要确定在线地图在先验地图中的具体位置和朝向。这里没有直接动手设置一个固定的平移量，而是采用地图匹配的方式自动搜索两张地图之间的二维刚体变换。程序会同时订阅prior_map和projected_map。只有当两张地图都接收到之后，程序才开始执行一次全局定位。定位完成后，不再重复执行，从而避免无人机运动过程中在线地图不断变化导致全局坐标系发生漂移。整个定位过程可以分为“结构点提取、距离地图构建、粗搜索、精搜索和坐标变换发布”几个步骤。
6.2.1 坐标变换与地图匹配
假设在线地图当前的位姿为\(T=(t_x,t_y,\theta)\)，将在线地图按照这个位姿变换到先验地图坐标系中，如果变换后的建筑结构与先验地图中的建筑结构能够较好的重合，那么这个位姿就更可能是无人机当前所在的全局位置。因此，对于在线地图中的一个障碍点\(p=(x,y)\)，在给定候选位姿\(T\)后，可以计算其在先验地图坐标系中的位置：
\[\begin{aligned}
x_{map}&=\cos\theta x-\sin\theta y+t_x\\
y_{map}&=\sin\theta x+\cos\theta y+t_y
\end{aligned}
\]
其中，\(\theta\)表示在线地图相对于先验地图的旋转角度，\(t_x\)和\(t_y\)表示平移量。经过该变换后，在线地图中的每一个结构点都可以映射到先验地图坐标系中，从而判断当前候选位姿下两张地图的匹配程度。
本文采用距离变换（Distance Transform）来描述先验地图的空间结构，以此来判断在线地图点与先验地图中的建筑结构是否匹配。距离变换的基本思想是：对于先验地图中的每一个栅格，计算它到最近建筑结构栅格的距离。因此，得到的距离变换图可以理解为一张“距离场”：
\[D(x,y)=\min_{(x_o,y_o)\in O}
\sqrt{(x-x_o)^2+(y-y_o)^2}
\]
其中\(O\)表示先验地图中的建筑结构区域，\(D(x,y)\)表示位置\((x,y)\)到最近建筑结构的距离。例如，如果一个点正好落在墙体上，那么它到最近墙体的距离接近0；如果它位于距离墙体5 cm的位置，那么对应的距离值约为0.05 m；如果它距离所有建筑结构都很远，则距离值较大。这样，地图匹配就不再是简单的“重合/不重合”判断，而变成了“距离建筑结构有多近”的连续评价。
对于每一个候选位姿，程序将在线地图中的结构点变换到map坐标系，并查询这些点在距离变换图中的距离。距离越小，说明当前在线地图中的结构越接近先验地图中的建筑结构，因此该候选位姿的匹配程度越高。对于距离超过设定阈值的点，则认为其与先验建筑结构没有足够的对应关系，不参与当前匹配得分的计算。最终，可以将一个候选位姿下所有有效结构点的匹配程度进行累计，得到该位姿的匹配得分。
6.2.2 搜索策略
为了寻找得分最高的候选位姿，本文采用“粗搜索+精搜索”的两阶段策略。第一阶段为粗搜索，在较大的范围内同时搜索平移和旋转参数。本文默认将旋转角搜索范围设置为\((-15^\circ\sim15^\circ)\)，步长为\((3^\circ)\)；X、Y平移范围分别为±3 m，步长为0.2m。程序遍历这些候选位姿，并保留匹配分数最高的结果作为初始估计。
得到粗略结果后，程序进一步进行精搜索。精搜索以粗搜索得到的(x,y,yaw)为中心，将搜索范围缩小到±0.30 m 和 ±3°，同时将平移步长缩小到0.05m、旋转步长减小到0.5°，从而在较小范围内进一步提高配准精度。
因此，整个自动定位过程可以概括为：
最终，程序会对匹配结果进行质量判断。当最终匹配得分低于设定阈值时，会认为当前定位结果不可靠，不会继续发布全局坐标变换；只有匹配得分达到要求后，才将定位结果正式作为全局定位结果。定位成功后，程序首先通过/map_pose发布无人机局部地图坐标系原点在全局map坐标系中的位置和朝向，其中：
\[x=t_x,\qquad y=t_y,\qquad yaw=\theta
\]
同时，以map为父坐标系、camera_init为子坐标系，持续广播：map-->camera_init-->body。前面箭头是通过先验地图与在线地图匹配得到的固定变换，而后面箭头是有FAST-LIO根据无人机运动实时更新。因此，二者结合后，就可以得到无人机在建筑全局地图中的实时位姿：
\[T_{body}^{map} = T_{map}^{camera\_init} \, T_{camera\_init}^{body}
\]
\[P_{map}=T_{body}^{map}\,P_{body}
\]
也就是说，FAST-LIO负责提供无人机连续、实时的局部运动估计，而先验地图定位模块负责确定这个局部坐标系在建筑全局地图中的位置。这样就将FAST-LIO的局部定位结构转换成了具有实际建筑地图意义的全局定位结果。
完整的配置过程如下：
点击查看代码
gedit /src/prior_map_localization.cpp
#include <ros/ros.h>
#include <nav_msgs/OccupancyGrid.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2/LinearMath/Quaternion.h>
#include <opencv2/opencv.hpp>
#include <cmath>
#include <vector>
#include <limits>
#include <algorithm>
#include <memory>
struct Point2D
{
double x;
double y;
};
class PriorMapLocalization
{
public:
PriorMapLocalization()
: prior_received_(false),
online_received_(false),
localization_done_(false),
best_x_(0.0),
best_y_(0.0),
best_yaw_(0.0),
best_score_(0.0)
{
ros::NodeHandle pnh("~");
pnh.param("coarse_yaw_min_deg", coarse_yaw_min_deg_, -15.0);
pnh.param("coarse_yaw_max_deg", coarse_yaw_max_deg_, 15.0);
pnh.param("coarse_yaw_step_deg", coarse_yaw_step_deg_, 3.0);
pnh.param("coarse_xy_range", coarse_xy_range_, 3.0);
pnh.param("coarse_xy_step", coarse_xy_step_, 0.20);
pnh.param("fine_yaw_range_deg", fine_yaw_range_deg_, 3.0);
pnh.param("fine_yaw_step_deg", fine_yaw_step_deg_, 0.5);
pnh.param("fine_xy_range", fine_xy_range_, 0.30);
pnh.param("fine_xy_step", fine_xy_step_, 0.05);
pnh.param("distance_threshold", distance_threshold_, 0.20);
pnh.param("max_points", max_points_, 2500);
pnh.param("min_score", min_score_, 0.10);
prior_sub_ = nh_.subscribe("/prior_map", 1, &PriorMapLocalization::priorMapCallback, this);
online_sub_ = nh_.subscribe("/projected_map", 1, &PriorMapLocalization::onlineMapCallback, this);
pose_pub_ = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("/map_pose", 1, true);
tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>();
tf_timer_ = nh_.createTimer(ros::Duration(0.05), &PriorMapLocalization::broadcastTF, this);
ROS_INFO("==========================================");
ROS_INFO("Prior Map Global Localization Node");
ROS_INFO("==========================================");
ROS_INFO("Prior map frame : map");
ROS_INFO("Online map frame : camera_init");
ROS_INFO("Coarse yaw : %.1f ~ %.1f deg", coarse_yaw_min_deg_, coarse_yaw_max_deg_);
ROS_INFO("Coarse yaw step : %.1f deg", coarse_yaw_step_deg_);
ROS_INFO("Coarse XY range : +/- %.2f m", coarse_xy_range_);
ROS_INFO("Coarse XY step : %.2f m", coarse_xy_step_);
ROS_INFO("Fine yaw range : +/- %.1f deg", fine_yaw_range_deg_);
ROS_INFO("Fine yaw step : %.1f deg", fine_yaw_step_deg_);
ROS_INFO("Fine XY range : +/- %.2f m", fine_xy_range_);
ROS_INFO("Fine XY step : %.2f m", fine_xy_step_);
ROS_INFO("Distance threshold : %.2f m", distance_threshold_);
ROS_INFO("Max points : %d", max_points_);
ROS_INFO("Min score : %.3f", min_score_);
ROS_INFO("Waiting for /prior_map and /projected_map...");
}
private:
ros::NodeHandle nh_;
ros::Subscriber prior_sub_;
ros::Subscriber online_sub_;
ros::Publisher pose_pub_;
ros::Timer tf_timer_;
std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
nav_msgs::OccupancyGrid prior_map_;
nav_msgs::OccupancyGrid online_map_;
bool prior_received_;
bool online_received_;
bool localization_done_;
double coarse_yaw_min_deg_;
double coarse_yaw_max_deg_;
double coarse_yaw_step_deg_;
double coarse_xy_range_;
double coarse_xy_step_;
double fine_yaw_range_deg_;
double fine_yaw_step_deg_;
double fine_xy_range_;
double fine_xy_step_;
double distance_threshold_;
int max_points_;
double min_score_;
double best_x_;
double best_y_;
double best_yaw_;
double best_score_;
double getMapYaw(const nav_msgs::OccupancyGrid& map)
{
double qx = map.info.origin.orientation.x;
double qy = map.info.origin.orientation.y;
double qz = map.info.origin.orientation.z;
double qw = map.info.origin.orientation.w;
double norm = std::sqrt(qx * qx + qy * qy + qz * qz + qw * qw);
if (norm < 1e-6) { return 0.0; }
qx /= norm; qy /= norm; qz /= norm; qw /= norm;
return std::atan2(2.0 * (qw * qz + qx * qy), 1.0 - 2.0 * (qy * qy + qz * qz));
}
Point2D cellToWorld(const nav_msgs::OccupancyGrid& map, int x, int y)
{
Point2D p;
double yaw = getMapYaw(map);
double local_x = (static_cast<double>(x) + 0.5) * map.info.resolution;
double local_y = (static_cast<double>(y) + 0.5) * map.info.resolution;
double c = std::cos(yaw);
double s = std::sin(yaw);
p.x = map.info.origin.position.x + c * local_x - s * local_y;
p.y = map.info.origin.position.y + s * local_x + c * local_y;
return p;
}
bool worldToCell(const nav_msgs::OccupancyGrid& map, double wx, double wy, int& mx, int& my)
{
// 获取地图origin的朝向角度
double yaw = getMapYaw(map);
// 计算世界点相对于地图origin原点的偏移（世界坐标系下）
double dx = wx - map.info.origin.position.x;
double dy = wy - map.info.origin.position.y;
double c = std::cos(yaw);
double s = std::sin(yaw);
// 坐标旋转变换：世界坐标系下的偏移量转换为地图坐标系下的偏移量
double local_x = c * dx + s * dy;
double local_y = -s * dx + c * dy;
// 将地图坐标系下的偏移量转换为栅格索引
mx = static_cast<int>(std::floor(local_x / map.info.resolution));
my = static_cast<int>(std::floor(local_y / map.info.resolution));
// 边界检查，如果坐标超出地图范围，则返回false
if (mx < 0 || my < 0 || mx >= static_cast<int>(map.info.width) || my >= static_cast<int>(map.info.height)) { return false; }
return true;
}
std::vector<Point2D> extractOccupiedPoints(const nav_msgs::OccupancyGrid& map)
{
std::vector<Point2D> points;
for (unsigned int y = 0; y < map.info.height; ++y)
{
for (unsigned int x = 0; x < map.info.width; ++x)
{
int index = y * map.info.width + x;
if (index >= static_cast<int>(map.data.size())) { continue; }
if (map.data[index] < 65) { continue; }
// 栅格(x,y)转世界坐标，存入points
points.push_back(cellToWorld(map, x, y));
}
}
return points;
}
std::vector<Point2D> samplePoints(const std::vector<Point2D>& input)
{
// 如果输入点数小于等于max_points_，则直接返回原始点集
if (static_cast<int>(input.size()) <= max_points_) { return input; }
std::vector<Point2D> result;
result.reserve(max_points_); // 预分配内存，避免多次动态分配
// 计算采样步长，确保均匀采样
double step = static_cast<double>(input.size()) / static_cast<double>(max_points_);
// 循环采样点，使用floor确保索引为整数
for (int i = 0; i < max_points_; ++i)
{
int index = static_cast<int>(i * step);
// 边界检查，确保索引不超过输入点集的大小
if (index >= static_cast<int>(input.size())) { index = input.size() - 1; }
result.push_back(input[index]);
}
return result;
}
// 输入ROS占据栅格地图，输出OpenCV距离变换图像（CV_32F类型）
// 图中每个像素的值表示该点到最近障碍物的距离（单位：像素）
cv::Mat buildDistanceMap(const nav_msgs::OccupancyGrid& map)
{
// 创建一个与地图大小相同的二值图像，初始化为255（表示自由空间）
cv::Mat binary(map.info.height, map.info.width, CV_8UC1, cv::Scalar(255));
// 遍历地图数据，将占据栅格（值 >= 65）标记为0（表示障碍物）
for (unsigned int y = 0; y < map.info.height; ++y)
{
for (unsigned int x = 0; x < map.info.width; ++x)
{
int index = y * map.info.width + x;
if (index >= static_cast<int>(map.data.size())) { continue; }
if (map.data[index] >= 65) { binary.at<unsigned char>(y, x) = 0; }
}
}
// 使用OpenCV的距离变换函数计算每个像素到最近障碍物的距离
cv::Mat distance;
cv::distanceTransform(binary, distance, cv::DIST_L2, 3);
return distance;
}
double calculateScore(const std::vector<Point2D>& online_points, const cv::Mat& distance_map, double dx, double dy, double yaw)
{
double c = std::cos(yaw);
double s = std::sin(yaw);
double score = 0.0;
int valid_points = 0;
for (const auto& p : online_points)
{
double tx = c * p.x - s * p.y + dx;
double ty = s * p.x + c * p.y + dy;
int mx;
int my;
if (!worldToCell(prior_map_, tx, ty, mx, my)) { continue; }
float distance_pixels = distance_map.at<float>(my, mx);
double distance = distance_pixels * prior_map_.info.resolution;
if (distance > distance_threshold_) { continue; }
double local_score = 1.0 - distance / distance_threshold_;
score += local_score;
valid_points++;
}
if (valid_points == 0) { return 0.0; }
return score / static_cast<double>(valid_points);
}
void coarseSearch(const std::vector<Point2D>& online_points, const cv::Mat& distance_map)
{
// best_score初始化为负无穷：保证第一个score一定能覆盖它
double best_score = -std::numeric_limits<double>::max();
double best_dx = 0.0;
double best_dy = 0.0;
double best_yaw = 0.0;
long count = 0;
//角度起始，做取模处理，防止输入大于360或者负数
double start_yaw_deg = fmod(coarse_yaw_min_deg_, 360.0);
// 三层循环暴力遍历所有候选位姿
// 第一层循环：遍历yaw角度（单位：度）
for (double yaw_deg = start_yaw_deg; yaw_deg <= coarse_yaw_max_deg_ + 1e-6; yaw_deg += coarse_yaw_step_deg_)
{
double yaw = yaw_deg * M_PI / 180.0; // 角度-->弧度转换
// 第二层循环：遍历x方向平移
for (double dx = -coarse_xy_range_; dx <= coarse_xy_range_ + 1e-6; dx += coarse_xy_step_)
{
// 第三层循环：遍历y方向平移
for (double dy = -coarse_xy_range_; dy <= coarse_xy_range_ + 1e-6; dy += coarse_xy_step_)
{
// [核心] 给当前候选位姿（dx, dy, yaw）计算匹配分数
double score = calculateScore(online_points, distance_map, dx, dy, yaw);
count++; // 统计一共评估了多少个候选位姿
// 如果当前分数比历史最佳分数更高，则更新最佳位姿和最佳分数
if (score > best_score)
{
best_score = score;
best_dx = dx;
best_dy = dy;
best_yaw = yaw;
}
}
}
}
ROS_INFO("Coarse candidates: %ld", count);
best_x_ = best_dx;
best_y_ = best_dy;
best_yaw_ = best_yaw;
best_score_ = best_score;
ROS_INFO("Coarse result: x=%.3f y=%.3f yaw=%.3f score=%.4f", best_x_, best_y_, best_yaw_ * 180.0 / M_PI, best_score_);
}
void fineSearch(const std::vector<Point2D>& online_points, const cv::Mat& distance_map)
{
// 以粗搜索的最佳位姿为中心，在更小的范围内进行精细搜索
double center_x = best_x_;
double center_y = best_y_;
// 角度归一化到 [-pi, pi] 范围内
double center_yaw = fmod(best_yaw_, 2.0 * M_PI) - M_PI;
double best_score = -std::numeric_limits<double>::max();
double best_dx = center_x;
double best_dy = center_y;
double best_yaw = center_yaw;
long count = 0;
// 三层循环：遍历相对于中心位姿的偏移量 offset
for (double yaw_offset_deg = -fine_yaw_range_deg_; yaw_offset_deg <= fine_yaw_range_deg_ + 1e-6; yaw_offset_deg += fine_yaw_step_deg_)
{
double yaw = center_yaw + yaw_offset_deg * M_PI / 180.0;
for (double dx_offset = -fine_xy_range_; dx_offset <= fine_xy_range_ + 1e-6; dx_offset += fine_xy_step_)
{
for (double dy_offset = -fine_xy_range_; dy_offset <= fine_xy_range_ + 1e-6; dy_offset += fine_xy_step_)
{
// 真实候选位姿 = 粗搜索中心点 + 当前偏移量
double dx = center_x + dx_offset;
double dy = center_y + dy_offset;
double score = calculateScore(online_points, distance_map, dx, dy, yaw);
count++;
if (score > best_score)
{
best_score = score;
best_dx = dx;
best_dy = dy;
best_yaw = yaw;
}
}
}
}
ROS_INFO("Fine candidates: %ld", count);
best_x_ = best_dx;
best_y_ = best_dy;
best_yaw_ = fmod(best_yaw, 2.0 * M_PI) - M_PI;
best_score_ = best_score;
ROS_INFO("Fine result: x=%.3f y=%.3f yaw=%.3f score=%.4f", best_x_, best_y_, best_yaw_ * 180.0 / M_PI, best_score_);
}
void performLocalization()
{
if (!prior_received_ || !online_received_) { return; }
ROS_INFO("==========================================");
ROS_INFO("Starting one-shot global localization...");
ROS_INFO("Prior map frame: [%s]", prior_map_.header.frame_id.c_str());
ROS_INFO("Online map frame: [%s]", online_map_.header.frame_id.c_str());
std::vector<Point2D> online_points = extractOccupiedPoints(online_map_);
if (online_points.empty())
{
ROS_WARN("No occupied points in /projected_map.");
return;
}
ROS_INFO("Online occupied points: %lu", online_points.size());
online_points = samplePoints(online_points);
ROS_INFO("Points used for localization: %lu", online_points.size());
cv::Mat distance_map = buildDistanceMap(prior_map_);
coarseSearch(online_points, distance_map);
fineSearch(online_points, distance_map);
ROS_INFO("==========================================");
ROS_INFO("Final localization result");
ROS_INFO("==========================================");
ROS_INFO("x = %.3f m", best_x_);
ROS_INFO("y = %.3f m", best_y_);
ROS_INFO("yaw = %.3f deg", best_yaw_ * 180.0 / M_PI);
ROS_INFO("score = %.4f", best_score_);
if (best_score_ < min_score_)
{
ROS_WARN("Localization score too low: %.4f < %.4f", best_score_, min_score_);
ROS_WARN("Global localization is rejected.");
return;
}
localization_done_ = true;
publishPose();
ROS_INFO("==========================================");
ROS_INFO("Global localization completed.");
ROS_INFO("T_map_camera_init:");
ROS_INFO(" x = %.3f m", best_x_);
ROS_INFO(" y = %.3f m", best_y_);
ROS_INFO(" yaw = %.3f deg", best_yaw_ * 180.0 / M_PI);
ROS_INFO(" score = %.4f", best_score_);
ROS_INFO("TF will be continuously broadcast:");
ROS_INFO(" map -> camera_init");
ROS_INFO("==========================================");
}
void publishPose()
{
geometry_msgs::PoseWithCovarianceStamped pose;
pose.header.stamp = ros::Time::now();
pose.header.frame_id = "map"; // 位姿参考坐标系：map
// 2D位姿信息：x, y, yaw
pose.pose.pose.position.x = best_x_;
pose.pose.pose.position.y = best_y_;
pose.pose.pose.position.z = 0.0;
// yaw欧拉角转换为四元数，ROS TF标准
tf2::Quaternion q;
q.setRPY(0.0, 0.0, best_yaw_);
pose.pose.pose.orientation.x = q.x();
pose.pose.pose.orientation.y = q.y();
pose.pose.pose.orientation.z = q.z();
pose.pose.pose.orientation.w = q.w();
// 协方差计算（不确定性）
// 这里简单地将协方差设置为与分数相关的值，分数越高，协方差越小
double variance = std::max(0.01, 1.0 - best_score_);
// x，y，yaw角度方向方差填入协方差矩阵的对角线位置
pose.pose.covariance[0] = variance;
pose.pose.covariance[7] = variance;
pose.pose.covariance[35] = variance;
pose_pub_.publish(pose);
}
void broadcastTF(const ros::TimerEvent&)
{
if (!localization_done_) { return; }
geometry_msgs::TransformStamped tf_msg;
tf_msg.header.stamp = ros::Time::now();
tf_msg.header.frame_id = "map";
tf_msg.child_frame_id = "camera_init";
// 发布位姿变换：map -> camera_init
tf_msg.transform.translation.x = best_x_;
tf_msg.transform.translation.y = best_y_;
tf_msg.transform.translation.z = 0.0;
tf2::Quaternion q;
q.setRPY(0.0, 0.0, best_yaw_);
tf_msg.transform.rotation.x = q.x();
tf_msg.transform.rotation.y = q.y();
tf_msg.transform.rotation.z = q.z();
tf_msg.transform.rotation.w = q.w();
tf_broadcaster_->sendTransform(tf_msg);
}
void priorMapCallback(const nav_msgs::OccupancyGrid::ConstPtr& msg)
{
prior_map_ = *msg;
prior_received_ = true;
ROS_INFO_THROTTLE(5.0, "Received /prior_map");
tryLocalization();
}
void onlineMapCallback(const nav_msgs::OccupancyGrid::ConstPtr& msg)
{
online_map_ = *msg;
online_received_ = true;
ROS_INFO_THROTTLE(5.0, "Received /projected_map");
tryLocalization();
}
void tryLocalization()
{
if (localization_done_) { return; }
if (!prior_received_ || !online_received_) { return; }
performLocalization();
}
};
int main(int argc, char** argv)
{
ros::init(argc, argv, "prior_map_localization_node");
PriorMapLocalization node;
ros::spin();
return 0;
}
点击查看代码
gedit ~/catkin_ws/src/prior_map_loader/CMakeLists.txt
# 补充下面文件
find_package(catkin REQUIRED COMPONENTS
nav_msgs
roscpp
roslib
sensor_msgs
sensor_msgs
geometry_msgs
tf2_ros
)
add_executable(prior_map_localization src/prior_map_localization.cpp)
target_link_libraries(prior_map_localization
${catkin_LIBRARIES}
${OpenCV_LIBS}
yaml-cpp::yaml-cpp
)
gedit ~/catkin_ws/src/prior_map_loader/package.xml
# 补充下面内容
<build_depend>roslib</build_depend>
<build_depend>tf2_ros</build_depend>
<build_export_depend>opencv</build_export_depend>
<build_export_depend>tf2_ros</build_export_depend>
<exec_depend>tf2_ros</exec_depend>
<exec_depend>roslib</exec_depend>
cd catkin_ws
catkin build prior_map_loader -j1
source devel/setup.bash
运行rosrun tf tf_echo map camera_init(相当于rostopic echo /map_pose -n 1)可以发现稳定的map → camera_init变换，这是固定变换，后续不会改变。
使用rosrun tf tf_echo map body观察无人机的全局位姿。初始位姿为：
完整的tf树如下：
7.地图融合与更新
经过前面的处理，系统已经完成了两个关键步骤：一方面，利用FAST-LIO和OctoMap获取无人机当前环境的在线二维占据地图/projected_map；另一方面，通过先验地图匹配获得了先验地图坐标系map与FAST-LIO局部坐标系camera_init之间的坐标变换，使在线地图能够与先验地图建立空间对应关系。
但是先验地图本身只描述建筑物的固定结构，并不包含室内桌椅、设备等动态或后期增加的物体。因此，如果系统只进行定位而不更新地图，那么得到的地图仍然是不完整的。本节在此基础上进一步实现在线地图与先验地图的增量融合。其基本思想是：保留先验地图中的已有建筑结构，同时从在线地图中提取先验地图原本不存在的新障碍物，并对这些候选障碍物进行空间和时间上的可靠性筛选，最终将确认后的障碍物写入更新地图/updated_map中。
7.1 障碍物检测
地图融合的第一步不是直接比较两个栅格数组，而是首先将在线地图中的栅格转换到世界坐标系，再映射到先验地图对应的栅格。这是因为两个地图虽然已经通过前面的全局定位建立了坐标关系，但是它们的地图尺寸、原点以及栅格索引并不一定完全相同。在完成坐标转换后，会遍历在线地图中的所有栅格，根据占据阈值判断在线地图中的当前栅格是否为障碍物，占据概率较高的栅格会被视为障碍物候选，随后将在线地图中的障碍物栅格转换到先验地图中，并读取先验地图对应位置，如果先验地图中该位置已经存在障碍物则认为是建筑结构，不需要作为新的障碍物重复添加，不存在则将该栅格加入新障碍物候选集合。因此整个判断流程为：
7.2 连通域筛选与时间一致性判断
直接使用在线地图检测出的所有新障碍物容易受到激光测量噪声的影响。例如，单个或少量孤立栅格可能只是传感器误差，并不代表真实物体。因此，本文使用BFS对候选栅格进行八邻域连通域分析，并根据连通区域的最小面积和最小栅格数进行筛选，仅保留具有一定空间连续性的候选区域。在进行连通域筛选后，部分候选区域可能仍然存在边缘散乱的问题，因此会对当前连通区域进行邻域扩张，再利用扩张结果筛选具有完整邻域支撑的核心栅格，从而进一步抑制边缘噪声。
空间上的连续区域并不一定就是真实障碍物。例如，无人机经过某些区域时，激光雷达可能因为遮挡、反射或者局部点云误差产生短暂的错误占据。因此这里要求新障碍物在连续观测中保持一定程度的一致性，通过跨帧累计栅格的持续出现次数，只有持续出现并达到设定次数的障碍物才被认为是可靠的新障碍物。
完整配置过程如下：
点击查看代码
cd catkin_ws/src/prior_map_loader
gedit /src/map_fusion.cpp
# 填入以下内容
#include <ros/ros.h>
#include <nav_msgs/OccupancyGrid.h>
#include <cmath>
#include <vector>
#include <string>
#include <algorithm>
#include <stdexcept>
#include <ros/package.h>
#include <queue>
#include <climits>
class MapFusionNode
{
public:
MapFusionNode(ros::NodeHandle& nh, ros::NodeHandle& pnh)
{
pnh.param<std::string>("prior_map_topic", prior_map_topic_, "/prior_map");
pnh.param<std::string>("online_map_topic", online_map_topic_, "/projected_map");
pnh.param<std::string>("new_obstacles_topic", new_obstacles_topic_, "/new_obstacles");
pnh.param<std::string>("updated_map_topic", updated_map_topic_, "/updated_map");
pnh.param<double>("occupied_threshold", occupied_threshold_, 50.0);
pnh.param<double>("new_obstacle_min_area", new_obstacle_min_area_, 0.05);
pnh.param<int>("min_obstacle_cells", min_obstacle_cells_, 4);
pnh.param<int>("obstacle_persistence_count", obstacle_persistence_count_, 2);
pnh.param<bool>("keep_prior_obstacles", keep_prior_obstacles_, true);
pnh.param<bool>("keep_online_free_space", keep_online_free_space_, false);
pnh.param<double>("prior_inflation_radius", prior_inflation_radius_m_, 0.01);
// =========================================================
// Subscribers & Publishers
// =========================================================
prior_map_sub_ = nh.subscribe(prior_map_topic_, 1, &MapFusionNode::priorMapCallback, this);
online_map_sub_ = nh.subscribe(online_map_topic_, 1, &MapFusionNode::onlineMapCallback, this);
new_obstacles_pub_ = nh.advertise<nav_msgs::OccupancyGrid>(new_obstacles_topic_, 1, true);
updated_map_pub_ = nh.advertise<nav_msgs::OccupancyGrid>(updated_map_topic_, 1, true);
ROS_INFO("==============================================");
ROS_INFO(" Map Fusion Node");
ROS_INFO("==============================================");
ROS_INFO("Prior map : %s", prior_map_topic_.c_str());
ROS_INFO("Online map : %s", online_map_topic_.c_str());
ROS_INFO("New obstacle: %s", new_obstacles_topic_.c_str());
ROS_INFO("Updated map : %s", updated_map_topic_.c_str());
ROS_INFO("Occupied threshold: %.1f", occupied_threshold_);
ROS_INFO("Minimum obstacle area: %.3f m^2", new_obstacle_min_area_);
ROS_INFO("Minimum obstacle cells: %d", min_obstacle_cells_);
ROS_INFO("Obstacle persistence: %d frames", obstacle_persistence_count_);
ROS_INFO("Prior inflation radius: %.3f m", prior_inflation_radius_m_);
ROS_INFO("==============================================");
}
private:
// =============================================================
// 回调
// =============================================================
void priorMapCallback(const nav_msgs::OccupancyGrid::ConstPtr& msg)
{
prior_map_ = *msg;
prior_map_received_ = true;
ROS_INFO("Received prior map: %d x %d, resolution %.3f",
msg->info.width, msg->info.height, msg->info.resolution);
// 初始化或合并到已存在的 updated_map_
if (!updated_map_initialized_)
{
updated_map_ = prior_map_;
// 初始化为未知
std::fill(updated_map_.data.begin(), updated_map_.data.end(), -1);
// 将 prior 中的已占据值保留到 updated_map_
for (size_t i = 0; i < prior_map_.data.size(); ++i)
{
if (prior_map_.data[i] >= occupied_threshold_)
updated_map_.data[i] = 100;
}
persistence_counts_.assign(prior_map_.data.size(), 0);
updated_map_initialized_ = true;
}
else
{
// 如果收到新的 prior_map，则把 prior 中的占据单元合并到 updated_map_
if (prior_map_.data.size() != updated_map_.data.size())
{
// 尺寸改变则重置
updated_map_ = prior_map_;
std::fill(updated_map_.data.begin(), updated_map_.data.end(), -1);
persistence_counts_.assign(prior_map_.data.size(), 0);
}
for (size_t i = 0; i < prior_map_.data.size() && i < updated_map_.data.size(); ++i)
{
if (prior_map_.data[i] >= occupied_threshold_)
updated_map_.data[i] = 100;
}
}
tryFuse();
}
void onlineMapCallback(const nav_msgs::OccupancyGrid::ConstPtr& msg)
{
online_map_ = *msg;
online_map_received_ = true;
ROS_INFO_THROTTLE(5.0, "Received online map: %d x %d, resolution %.3f",
msg->info.width, msg->info.height, msg->info.resolution);
tryFuse();
}
void tryFuse()
{
if (!prior_map_received_ || !online_map_received_) return;
if (prior_map_.header.frame_id != online_map_.header.frame_id)
{
ROS_ERROR_THROTTLE(5.0, "Frame mismatch! Prior = [%s], online = [%s]",
prior_map_.header.frame_id.c_str(), online_map_.header.frame_id.c_str());
return;
}
fuseMaps();
}
// =============================================================
// 坐标转换（保持不变）
// =============================================================
bool worldToMap(const nav_msgs::OccupancyGrid& map, double wx, double wy, int& mx, int& my)
{
const double resolution = map.info.resolution;
const double origin_x = map.info.origin.position.x;
const double origin_y = map.info.origin.position.y;
const double qx = map.info.origin.orientation.x;
const double qy = map.info.origin.orientation.y;
const double qz = map.info.origin.orientation.z;
const double qw = map.info.origin.orientation.w;
double yaw = std::atan2(2.0 * (qw * qz + qx * qy), 1.0 - 2.0 * (qy * qy + qz * qz));
double dx = wx - origin_x;
double dy = wy - origin_y;
double local_x = std::cos(yaw) * dx + std::sin(yaw) * dy;
double local_y = -std::sin(yaw) * dx + std::cos(yaw) * dy;
mx = static_cast<int>(std::floor(local_x / resolution));
my = static_cast<int>(std::floor(local_y / resolution));
if (mx < 0 || my < 0 || mx >= static_cast<int>(map.info.width) || my >= static_cast<int>(map.info.height))
return false;
return true;
}
void mapToWorld(const nav_msgs::OccupancyGrid& map, int mx, int my, double& wx, double& wy)
{
const double resolution = map.info.resolution;
double local_x = (static_cast<double>(mx) + 0.5) * resolution;
double local_y = (static_cast<double>(my) + 0.5) * resolution;
const double qx = map.info.origin.orientation.x;
const double qy = map.info.origin.orientation.y;
const double qz = map.info.origin.orientation.z;
const double qw = map.info.origin.orientation.w;
double yaw = std::atan2(2.0 * (qw * qz + qx * qy), 1.0 - 2.0 * (qy * qy + qz * qz));
wx = map.info.origin.position.x + std::cos(yaw) * local_x - std::sin(yaw) * local_y;
wy = map.info.origin.position.y + std::sin(yaw) * local_x + std::cos(yaw) * local_y;
}
// =============================================================
// ==================== 核心融合函数 ====================
// =============================================================
void fuseMaps()
{
// 保证已初始化（prior_map 已经收到并用于初始化 updated_map_）
if (!updated_map_initialized_)
{
ROS_WARN_THROTTLE(5.0, "Updated map not initialized yet. Waiting for prior map.");
return;
}
// 新的障碍发布地图初始化
new_obstacles_map_ = prior_map_;
std::fill(new_obstacles_map_.data.begin(), new_obstacles_map_.data.end(), -1);
const size_t N = prior_map_.data.size();
// ====================== 生成膨胀后的 prior mask ======================
std::vector<unsigned char> inflated_mask(N, 0);
int inflate_cells = 0;
if (prior_inflation_radius_m_ > 0.0 && prior_map_.info.resolution > 0.0)
inflate_cells = static_cast<int>(std::ceil(prior_inflation_radius_m_ / prior_map_.info.resolution));
if (inflate_cells <= 0)
{
for (size_t i = 0; i < N; ++i)
if (prior_map_.data[i] >= occupied_threshold_) inflated_mask[i] = 1;
}
else
{
const int W = prior_map_.info.width;
const int H = prior_map_.info.height;
for (size_t i = 0; i < N; ++i)
{
if (prior_map_.data[i] < occupied_threshold_) continue;
int x = static_cast<int>(i % W);
int y = static_cast<int>(i / W);
for (int dy = -inflate_cells; dy <= inflate_cells; ++dy)
{
for (int dx = -inflate_cells; dx <= inflate_cells; ++dx)
{
int nx = x + dx;
int ny = y + dy;
if (nx >= 0 && nx < W && ny >= 0 && ny < H)
{
inflated_mask[ny * W + nx] = 1;
}
}
}
}
}
// ====================== per-frame 候选标记 + 持久化计数累加 ======================
std::vector<unsigned char> candidate_mark(N, 0);
const int online_width = online_map_.info.width;
const int online_height = online_map_.info.height;
for (int oy = 0; oy < online_height; ++oy)
{
for (int ox = 0; ox < online_width; ++ox)
{
const int online_index = oy * online_width + ox;
if (online_index < 0 || online_index >= static_cast<int>(online_map_.data.size())) continue;
int online_value = online_map_.data[online_index];
if (online_value < occupied_threshold_) continue;
double wx, wy;
mapToWorld(online_map_, ox, oy, wx, wy);
int px, py;
if (!worldToMap(prior_map_, wx, wy, px, py)) continue;
const int prior_index = py * prior_map_.info.width + px;
if (prior_index < 0 || prior_index >= static_cast<int>(N)) continue;
// 如果在膨胀后的 prior 区域内，则视为已知墙体/障碍，直接合并到 updated_map_
if (inflated_mask[prior_index])
{
if (keep_prior_obstacles_)
updated_map_.data[prior_index] = 100;
continue;
}
// 新候选
if (!candidate_mark[prior_index])
{
candidate_mark[prior_index] = 1;
// 增加持久化计数（每帧一次）
if (prior_index >= 0 && prior_index < static_cast<int>(persistence_counts_.size()))
{
if (persistence_counts_[prior_index] < INT_MAX) persistence_counts_[prior_index]++;
}
}
}
}
// ====================== 连通域过滤 + 消噪（膨胀+腐蚀） ======================
std::vector<int> connected_candidates;
std::vector<unsigned char> visited(N, 0);
const double cell_area = prior_map_.info.resolution * prior_map_.info.resolution;
int min_cells_by_area = static_cast<int>(std::ceil(new_obstacle_min_area_ / cell_area));
int min_cells = std::max(min_obstacle_cells_, min_cells_by_area);
const int W = prior_map_.info.width;
const int H = prior_map_.info.height;
for (size_t i = 0; i < N; ++i)
{
if (!candidate_mark[i] || visited[i]) continue;
std::vector<int> component;
std::queue<int> q;
q.push(static_cast<int>(i));
visited[i] = 1;
while (!q.empty())
{
int current = q.front(); q.pop();
component.push_back(current);
int x = current % W;
int y = current / W;
for (int dy = -1; dy <= 1; ++dy)
{
for (int dx = -1; dx <= 1; ++dx)
{
if (dx == 0 && dy == 0) continue;
int nx = x + dx;
int ny = y + dy;
if (nx >= 0 && nx < W && ny >= 0 && ny < H)
{
int nidx = ny * W + nx;
if (!visited[nidx] && candidate_mark[nidx])
{
visited[nidx] = 1;
q.push(nidx);
}
}
}
}
}
int area = static_cast<int>(component.size());
if (area < min_cells) continue;
// 膨胀（扩展）然后腐蚀（核心筛选）
std::vector<unsigned char> expanded(N, 0);
for (int c : component)
{
int x = c % W;
int y = c / W;
for (int dy = -1; dy <= 1; ++dy)
{
for (int dx = -1; dx <= 1; ++dx)
{
int nx = x + dx;
int ny = y + dy;
if (nx >= 0 && nx < W && ny >= 0 && ny < H)
expanded[ny * W + nx] = 1;
}
}
}
std::vector<int> denoised;
for (int c : component)
{
int x = c % W;
int y = c / W;
bool is_core = true;
for (int dy = -1; dy <= 1 && is_core; ++dy)
{
for (int dx = -1; dx <= 1; ++dx)
{
if (dx == 0 && dy == 0) continue;
int nx = x + dx;
int ny = y + dy;
if (nx >= 0 && nx < W && ny >= 0 && ny < H)
{
if (!expanded[ny * W + nx])
{
is_core = false;
break;
}
}
}
}
if (is_core) denoised.push_back(c);
}
for (int c : denoised)
{
connected_candidates.push_back(c);
}
}
// ====================== 持久化判断：使用跨帧 persistence_counts_ ======================
std::vector<int> final_new_cells;
for (int idx : connected_candidates)
{
if (idx >= 0 && idx < static_cast<int>(persistence_counts_.size()) &&
persistence_counts_[idx] >= obstacle_persistence_count_)
{
final_new_cells.push_back(idx);
}
}
// ====================== 更新地图数据（保留历史 updated_map_） ======================
for (int index : final_new_cells)
{
new_obstacles_map_.data[index] = 100;
updated_map_.data[index] = 100;
}
// ====================== 发布 ======================
ros::Time stamp = ros::Time::now();
updated_map_.header.stamp = stamp;
new_obstacles_map_.header.stamp = stamp;
updated_map_.header.frame_id = prior_map_.header.frame_id;
new_obstacles_map_.header.frame_id = prior_map_.header.frame_id;
new_obstacles_pub_.publish(new_obstacles_map_);
updated_map_pub_.publish(updated_map_);
ROS_INFO_THROTTLE(5.0, "Map fusion: %lu final new obstacle cells (after connectivity + denoising + persistence)",
final_new_cells.size());
}
private:
// =============================================================
// ROS 接口
// =============================================================
ros::Subscriber prior_map_sub_;
ros::Subscriber online_map_sub_;
ros::Publisher new_obstacles_pub_;
ros::Publisher updated_map_pub_;
// =============================================================
// 地图数据
// =============================================================
nav_msgs::OccupancyGrid prior_map_;
nav_msgs::OccupancyGrid online_map_;
nav_msgs::OccupancyGrid updated_map_;
nav_msgs::OccupancyGrid new_obstacles_map_;
bool prior_map_received_ = false;
bool online_map_received_ = false;
// =============================================================
// 参数
// =============================================================
std::string prior_map_topic_;
std::string online_map_topic_;
std::string new_obstacles_topic_;
std::string updated_map_topic_;
double occupied_threshold_;
double new_obstacle_min_area_;
int min_obstacle_cells_;
int obstacle_persistence_count_;
bool keep_prior_obstacles_;
bool keep_online_free_space_;
double prior_inflation_radius_m_ = 0.1;
// 持久化计数（跨帧保存），以及 updated_map 初始化标志
std::vector<int> persistence_counts_;
bool updated_map_initialized_ = false;
};
int main(int argc, char** argv)
{
ros::init(argc, argv, "map_fusion_node");
ros::NodeHandle nh;
ros::NodeHandle pnh("~");
MapFusionNode node(nh, pnh);
ros::spin();
return 0;
}
点击查看代码
gedit ~/catkin_ws/src/prior_map_loader/CMakeLists.txt
# 补充下面文件
add_executable(map_fusion src/map_fusion.cpp)
target_link_libraries(map_fusion
${catkin_LIBRARIES}
${OpenCV_LIBS}
yaml-cpp::yaml-cpp
)
cd catkin_ws
catkin build prior_map_loader -j1
source devel/setup.bash
点击查看代码
<launch>
<!-- 加载先验地图 -->
<node pkg="prior_map_loader" type="prior_map_loader" name="prior_map_loader" output="screen"/>
<!-- 先验地图定位节点 -->
<node pkg="prior_map_loader" type="prior_map_localization" name="prior_map_localization" output="screen"/>
<!-- 地图融合节点 -->
<node pkg="prior_map_loader" type="map_fusion" name="map_fusion" output="screen"/>
</launch>
下面是先验地图，同时这里展示了FAST-LIO得到的三维地图，障碍物的轮廓结构已经很清晰。
下面是在线地图，可以发现在3D地图向2D平面投影时，在地面上出现了较多的噪点，同时障碍物的轮廓也并不是特别规整。
下面是实时更新的地图，将经过筛选并确认的新障碍物增量添加到新地图中。经过本文算法的连通域筛选、邻域去噪以及时间一致性判断后，几乎所有障碍物都被识别到了，同时障碍物的边缘较为规整。需要注意的是，由于无人机进入密闭狭小的空间时激光雷达捕捉到的特征点会很少，即会失去定位信息，因此最后那个房间就没有进入扫描，遗漏了一个垃圾桶。
8. 总结
8.1 整体框架
经过前面的实验，整个系统已经完成了从先验地图加载、无人机定位到在线地图更新的完整流程。首先，根据已有的室内建筑平面图生成二维先验地图，并利用Mid360激光雷达和IMU通过FAST-LIO建立无人机自身的局部坐标系和实时三维地图。随后，将FAST-LIO生成的三维点云通过OctoMap投影为二维占据地图，并通过先验地图与在线地图之间的匹配估计无人机在全局地图中的位置，从而建立全局坐标系与局部坐标系之间的对应关系。在此基础上，进一步将在线地图中的障碍物转换到先验地图坐标系中，通过先验结构排除、连通域筛选、邻域去噪以及跨帧一致性的判断，提取先验地图中原本不存在的新障碍物，并将其持续累计到更新地图中。
8.2 展望
目前系统的定位稳定性仍然比较依赖环境中的几何特征。在特征丰富的区域，FAST-LIO能够获得比较稳定的局部位姿，但是当无人机进入大面积空旷区域、结构单一的墙面附近或者特征点较少的房间时，激光雷达能够提供的有效几何约束会明显减少，此时系统更容易产生位姿漂移。特别是在无人机起飞阶段，由于初始运动过程中有效环境约束较少，同时无人机自身存在一定的姿态变化，初始定位也比较容易出现漂移。因此，后续可以进一步考虑利用先验地图对局部里程计进行持续约束，而不是只在初始化阶段进行一次全局定位。
现在地图融合主要是在二维占据栅格层面进行判断，虽然能够完成地图更新，但对于障碍物的类别和语义信息并没有进一步的利用。比如，系统无法判断检测到的障碍物到底是桌子还是柜子。后续加入视觉语义感知模块，就可以进一步利用相机获取的图像进行目标检测和语义分割，并将视觉识别的结果与激光雷达的空间信息进行融合，形成有语义标签的二维地图。为了获得更加完整的“感知-定位-建图-规划-控制”的完整自主导航系统，后续可以继续加入路径规划、避障和飞行控制等模块，此时先验地图不仅可以用于全局定位和地图更新，还可以为无人机提供全局路径规划的环境约束，而实时激光雷达和视觉感知则负责检测先验地图中没有记录的临时障碍物，并在飞行过程中动态调整局部路径。
浙公网安备 33010602011771号