当前位置：首页 > news >正文

无人机避障——感知篇（Orin nx采用zed2双目相机进行Vins-Fusion定位，再通过位姿和深度图建图完成实时感知）

news 来源：原创 2025/6/14 9:03:28

通过ZED2双目视觉相机进行Vins FusionSLAM定位之后，需要获取其中的信息完成无人机的实时感知部分，感知需要的是栅格地图话题，因为EGO-planner高飞老师就是通过Vins Fusion做感知的部分，所以其中可以借鉴一下建图方法。

通过ZED2双目视觉相机进行Vins FusionSLAM定位看我的前一篇涉及到双目视觉标定、IMU标定以及感知定位实际测试的文章：

无人机避障——感知篇（在Ubuntu20.04的Orin nx上基于ZED2实现Vins Fusion）-CSDN博客

结果展示：

借鉴Ego-Planner中的plan_env代码：

发现别人已经改好了EGO中相关建图环境部分的代码：

https://github.com/maxibooksiyi/ego_grid_map.git

下载以后进行编译安装：

这部分基本操作不赘述。

然后修改话题：

这是我已经修改好的，就两个话题进行修改就可以。

原来的代码：

 //depth_sub_.reset(new message_filters::Subscriber<sensor_msgs::Image>(node_, "/grid_map/depth", 50));depth_sub_.reset(new message_filters::Subscriber<sensor_msgs::Image>(node_, "/camera/aligned_depth_to_color/image_raw", 50));if (mp_.pose_type_ == POSE_STAMPED){pose_sub_.reset(new message_filters::Subscriber<geometry_msgs::PoseStamped>(node_, "/grid_map/pose", 25));sync_image_pose_.reset(new message_filters::Synchronizer<SyncPolicyImagePose>(SyncPolicyImagePose(100), *depth_sub_, *pose_sub_));sync_image_pose_->registerCallback(boost::bind(&GridMap::depthPoseCallback, this, _1, _2));}else if (mp_.pose_type_ == ODOMETRY){//odom_sub_.reset(new message_filters::Subscriber<nav_msgs::Odometry>(node_, "/grid_map/odom", 100));odom_sub_.reset(new message_filters::Subscriber<nav_msgs::Odometry>(node_, "/vins_estimator/imu_propagate", 100));sync_image_odom_.reset(new message_filters::Synchronizer<SyncPolicyImageOdom>(SyncPolicyImageOdom(100), *depth_sub_, *odom_sub_));sync_image_odom_->registerCallback(boost::bind(&GridMap::depthOdomCallback, this, _1, _2));}

修改之后的代码：

//depth_sub_.reset(new message_filters::Subscriber<sensor_msgs::Image>(node_, "/grid_map/depth", 50));depth_sub_.reset(new message_filters::Subscriber<sensor_msgs::Image>(node_, "/zed2/zed_node/depth/depth_registered", 50));if (mp_.pose_type_ == POSE_STAMPED){pose_sub_.reset(new message_filters::Subscriber<geometry_msgs::PoseStamped>(node_, "/grid_map/pose", 25));sync_image_pose_.reset(new message_filters::Synchronizer<SyncPolicyImagePose>(SyncPolicyImagePose(100), *depth_sub_, *pose_sub_));sync_image_pose_->registerCallback(boost::bind(&GridMap::depthPoseCallback, this, _1, _2));}else if (mp_.pose_type_ == ODOMETRY){//odom_sub_.reset(new message_filters::Subscriber<nav_msgs::Odometry>(node_, "/grid_map/odom", 100));odom_sub_.reset(new message_filters::Subscriber<nav_msgs::Odometry>(node_, "/vins_estimator/imu_propagate", 100));sync_image_odom_.reset(new message_filters::Synchronizer<SyncPolicyImageOdom>(SyncPolicyImageOdom(100), *depth_sub_, *odom_sub_));sync_image_odom_->registerCallback(boost::bind(&GridMap::depthOdomCallback, this, _1, _2));}

需要修改话题：

双目相机的深度图话题：

<sensor_msgs::Image>(node_, "/camera/aligned_depth_to_color/image_raw", 50)

替换成ZED2双目相机的深度图话题：

<sensor_msgs::Image>(node_, "/zed2/zed_node/depth/depth_registered", 50)

Vins Fusion的位姿话题：

<nav_msgs::Odometry>(node_, "/vins_estimator/imu_propagate", 100)

无需修改，都是用Vins Fusion进行获取。

然后在我的前一个博客的bash文件的基础上，加上此处建图的一键启动命令：

# run.sh文件#!/bin/bash# Start roscore
gnome-terminal -- bash -c "roscore"
# Start RViz
# gnome-terminal -- bash -c "cd vins_ws && source devel/setup.bash && roslaunch vins vins_rviz.launch"# Start VINS-Fusion node
sleep 5
gnome-terminal -- bash -c "cd vins_ws && source devel/setup.bash && rosrun vins vins_node src/VINS-Fusion/config/zed/zed2_stereo_config.yaml"#回环检测
sleep 5
gnome-terminal -- bash -c "cd vins_ws && source devel/setup.bash && rosrun loop_fusion loop_fusion_node src/config/zed/zed2_stereo_config.yaml"## 实时相机
sleep 5
gnome-terminal -- bash -c "cd vins_ws && source devel/setup.bash && source /home/nvidia/zed_ws/devel/setup.bash && roslaunch zed_wrapper zed2.launch"## 实时建栅格地图
sleep 5
gnome-terminal -- bash -c "cd vins_ws && source devel/setup.bash && source /home/nvidia/ego_planner_grid/devel/setup.bash && roslaunch plan_env grid_map.launch"
# Play rosbag
# sleep 5
# gnome-terminal -- bash -c "source devel/setup.bash && rosbag play /home/nvidia/data_set/MH_01_easy.bag"# Keep the terminal open until you manually close it
echo "Press Enter to close the terminals"
read