PX4+Gazebo仿真降落.zip

# 识别位姿自主降落(四旋翼无人机) 该算法包基于Ubuntu20.04 ROS Noetic下运行，输出相机位姿数据，寻找降落目标，实现无人机的自主降落，[演示视频](https://www.bilibili.com/video/BV1hB4y1A75t)。 该包分为三部分： 1. 普通电脑Ubuntu运行相机识别算法 2. 电脑Ubuntu运行Gazebo仿真，实现无人机识别降落目标并自主降落。 3. 真实实验实现无人机自主降落 ## 硬件参数 1. 相机：[锐尔威视](http://www.rervision.cn/featured_products.html)，帧率：260FPS，像素：640*360 2. 标记点图片： <img src="./imgs/mark.png" alt="Mark Points" style="zoom:40%;" /> 3. 四旋翼无人机（真实飞行实验）：[DJI F450](https://www.dji.com/cn/flame-wheel-arf/feature)机架、[Pixhawk2 Hex Cube](https://docs.px4.io/v1.9.0/en/flight_controller/pixhawk-2.html)飞控 4. 板载计算机（真实飞行实验）：[Nanopi M4](http://wiki.friendlyarm.com/wiki/index.php/NanoPi_M4/zh)（FridenlyDesktop LUbuntu18.04） 5. 无人机定位设备（真实飞行实验）：[OpTitrack](http://optitrack.com.cn/) --- ## 安装过程 安装过程分为3个部分，每个部分有，其中无人机部分仿真可以不用任何硬件设备。 --- ### 1.普通电脑运行相机识别算法 1. 安装[Ros](http://wiki.ros.org/noetic/Installation/Ubuntu) 2. 安装g2o库依赖（优化PnP估计位姿数据） ~~~sh sudo apt-get install libsuitesparse-dev qtdeclarative5-dev qt5-qmake libqglviewer-headers git clone https://github.com/koide3/g2o.git cd g2o git checkout hdl_graph_slam mkdir build && cd build cmake .. -DCMAKE_BUILD_TYPE=RELEASE make -j8 sudo make install ~~~ 3. 安装Sophus库（李群与李代数，描述位姿SO3空间） ~~~shell git clone https://github.com/strasdat/Sophus.git cd Sophus/ mkdir build cd build cmake .. make sudo make install ~~~ 4. 安装uvc camera驱动 ~~~shell sudo apt install libuvc-dev ~~~ 5. 创建工作空间，下载源码 ~~~sh cd ~/ mkdir -p ~/quadrotor_ws/src cd ~/quadrotor_ws/src git clone ~~~ 6. 编译libuvc_ros，并修改相机串口号 - 编译： ~~~sh # Noetic安装python3的catkin编译工具 sudo apt install python3-catkin-tools python3-osrf-pycommon catkin build libuvc_ros ~~~ - 设置相机串口： ~~~shell lsusb # Bus 004 Device 050: ID 32e4:4689 ~~~ 其中Vendor号为0x32e4，Product号为4689，如果要运行自己的相机需要修改成自己相机的ID号 编辑/etc/udev/rules.d/99-uvc.rules文件 ~~~shell sudo gedit /etc/udev/rules.d/99-uvc.rules ~~~ ~~~txt # UVC cameras SUBSYSTEMS=="usb", ENV{DEVTYPE}=="usb_device", ATTRS{idVendor}=="32e4", ATTRS{idProduct}=="4689", MODE="0666" Change the vendor and product IDs to match your camera. ~~~ 修改libuvc_ros/libuvc_camera/launch/libuvc_camera.launch ~~~ <param name="vendor" value="0x32e4"/> <param name="product" value="0x4689"/> ~~~ 保存后，重新加载udev规则 ~~~sh sudo udevadm control --reload-rules sudo udevadm trigger ~~~ 7. 标定自己的相机参数：详见http://wiki.ros.org/camera_calibration/Tutorials/MonocularCalibration。并在~/quadrotor_ws/src/detect_points/src/detect_points.cpp替换成自己的相机参数。 8. 编译其余包 ~~~shell catkin build detect_points rviz_simulation ~~~ 9. 运行 ~~~shell # 开启相机 roslaunch libuvc_camera libuvc_camera.launch # 开启检测位姿节点 rosrun detect_points detect_points # 仿真 rosrun rviz_simulation rviz_simulation rviz ~~~ 10. 效果 <img src="./imgs/位姿识别运行.png" style="zoom:60%;" /> --- ### 2.运行Gazebo仿真，实现无人机识别降落目标并自主降落 1. 安装PX4固件 - 安装依赖 安装mavros以及相关的 geographiclib dataset ~~~sh sudo apt install ros-noetic-mavros ros-noetic-mavros-extras wget https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh chmod +x install_geographiclib_datasets.sh sudo ./install_geographiclib_datasets.sh ~~~ - 下载PX4/Autopilot（已经使用了国内源，下载速度会快许多），这里把Firmware克隆至~/ ~~~sh git clone https://github.com.cnpmjs.org/PX4/Firmware.git --recursive ~~~ - 在下载子模块的时候速度很慢，停止。同样使用国内源，打开~/Firmware/.git/config，找到子模块下载网址，将github.com全部改为github.com.cnpmjs.org，然后继续下载。 ~~~shell git submodule update --init --recursive ~~~ - 下载完成后切换版本并编译px4_sitl_default ~~~sh git checkout v1.11.0-beta1 # 完成环境配置 source ~/Firmware/Tools/setup/ubuntu.sh sudo apt install libgstreamer-plugins-base1.0-dev make distclean make px4_sitl_default ~~~ - 编译Gazebo 在`~/Firmware/Tools/sitl_gazebo`路径下执行: ~~~sh mkdir Build ### 将以下所有的[]都替换成Firmware所在的路径 ### 设置插件的路径以便 Gazebo 能找到模型文件 export GAZEBO_PLUGIN_PATH=${GAZEBO_PLUGIN_PATH}:~/Firmware/Tools/sitl_gazebo/Build ### 设置模型路径以便 Gazebo 能找到机型 export GAZEBO_MODEL_PATH=${GAZEBO_MODEL_PATH}:~/Firmware/Tools/sitl_gazebo/models ### 禁用在线模型查找 export GAZEBO_MODEL_DATABASE_URI="" ### 设置 sitl_gazebo 文件夹的路径 export SITL_GAZEBO_PATH=~/Firmware/Tools/sitl_gazebo ### 编译gazebo仿真 cd Build cmake .. make ~~~ - 启动仿真 ~~~sh cd ~/Firmware make px4_sitl_default source ~/Firmware/Tools/setup_gazebo.bash ~/Firmware ~/Firmware/build/px4_sitl_default export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:~/Firmware export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:~/Firmware/Tools/sitl_gazebo roslaunch px4 mavros_posix_sitl.launch ~~~ 为了方便在~/.bashrc中添加环境变量 ~~~sh source ~/Firmware/Tools/setup_gazebo.bash ~/Firmware ~/Firmware/build/px4_sitl_default export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:~/Firmware export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:~/Firmware/Tools/sitl_gazebo ~~~ 2. 将地标图片和环境导入至Gazebo中 - 将models文件夹下的全部文件全部复制到`~/.gazebo/medels/`中 - 将world/land文件复制至`/home/jh/Firmware/Tools/sitl_gazebo/worlds` - 修改`~/Firmware/launch/mavros_posix_sitl.launch`文件 ~~~txt # 在14行下面增加一句： <arg name="cam" default="iris_fpv_cam"/> # 注释原文件该行 # <arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/empty.world"/> # 替换成该行 <arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/land"/> # 注释原文件该行 # <arg name="sdf" default="$(find mavlink_sitl_gazebo)/models/$(arg vehicle)/$(arg vehicle).sdf"/> # 替换成该行 <arg name="sdf" default="$(find mavlink_sitl_gazebo)/models/$(arg cam)/$(arg cam).sdf"/> ~~~ - 修改fpv camera摄像头朝向 默认情况下摄像头朝向正前方，修改为正视向下。 打开`~/Firmware/Tools/sitl_gazebo/models/iris_fpv_cam/iris_fpv_cam.sdf`修改pose ~~~ <pose>0 0 0 0 1.57 0</pose> ~~~ 3. 标定仿真相机 - 由于iris_fpv_cam相机的内参矩阵没有，需要自行标定。model文件夹下的pattern为标定棋盘的sdf模型，将文件world/calibration复制到~/Firmware/Tools/sitl_gazebo/worlds下。修改~/Firmware/launch/mavros_posix_sitl.launch的第14行，将world改为calibration（即刚复制的文件名）。 - 编译运行节点offboard： ~~~sh