PX4+激光雷达在gazebo中仿真实现（古月居）

 

摘自：https://www.guyuehome.com/8983

基于px4的无人机自主导航

1187

1

2020年6月30日 12时12分

Judez

基于px4的无人机自主导航

在ros的学习过程中我们经常可以看到自主导航的小车，那么无人机是否也能像小车一样建图导航呢？本文即主要介绍如何在px4平台基础上进行无人机自主导航仿真实验。

ROS导航框架介绍

无人机导航运动控制系统大致分为五个层次的架构，从高到低依次为：给定目标位置->建图定位->路径规划->底层控制->无人机转子速度。总结起来如下图所示：

 

将小车导航中用到的激光雷达移植到无人机上，通过激光雷达的扫描信息进行建图导航，无人机大致的模型如下图所示，其中蓝色的光束即代表无人机激光雷达扫描发射的光束。

ROS及PX4环境搭建

本文实现的无人机自主导航是基于px4无人机仿真环境以及ROS-melodic下完成的
_ROS及部分工具安装_
1 . 加入ros的安装源

1. sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'

2 . 加入秘钥

1. sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654

3 . 更新

1. sudo apt-get update

4 . 安装ros

1. sudo apt-get install ros-melodic-desktop

5 . Source ROS

1. echo "source /opt/ros/melodic/setup.bash" >> ~/.bashrc
2. source ~/.bashrc

6 . 安装Gazebo

1. sudo apt install ros-melodic-gazebo9*

7 . 初始化rosdep

1. rosdep init
2. rosdep update

8 . 安装catkin工具

1. sudo apt-get install ros-melodic-catkin python-catkin-tools

9 . 安装mavros

1. sudo apt install ros-melodic-mavros ros-melodic-mavros-extras

10 . 安装geographiclib dataset

1. #下载脚本
2. wget https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh
3. #为脚本添加权限
4. chmod +x install_geographiclib_datasets.sh
5. #执行脚本
6. sudo ./install_geographiclib_datasets.sh

px4仿真工具安装说明

1 . 利用脚本安装必要的工具链

1. #下载脚本
2. wget https://raw.githubusercontent.com/PX4/Firmware/master/Tools/setup/ubuntu.sh
3. wget https://raw.githubusercontent.com/PX4/Firmware/master/Tools/setup/requirements.txt
4. #这俩个文件下载不下来可以试试“xx上网”
5. #执行脚本：
6. source ubuntu.sh

1. 创建工作空间
   1. mkdir -p ~/catkin_ws/src
   2. cd ~/catkin_ws/src/
   3. catkin_init_workspace
2. 下载编译px4
   1. #下载代码
   2. cd ~/catkin_ws/
   3. git clone https://github.com/PX4/Firmware
   4. #然后更新submodule切换固件并编译
   5. cd Firmware
   6. git submodule update --init --recursive
   7. git checkout v1.11.0-beta1
   8. make distclean
   9. #在具体编译前还需要安装相关的工具
   10. sudo apt-get install python-jinja2
   11. sudo pip install numpy toml
   12. #开始编译
   13. make px4_sitl_default gazebo
3. 添加相应的环境变量信息
   1. cd ~/catkin_ws/
   2. catkin build
   3. #添加工作空间source
   4. echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc
   5. #添加gazebo模型路径
   6. echo "export GAZEBO_MODEL_PATH=:~/catkin_ws/models" >> ~/.bashrc
   7. #添加px4路径
   8. echo "source ~/catkin_ws/Firmware/Tools/setup_gazebo.bash ~/catkin_ws/Firmware ~/catkin_ws/Firmware/build/px4_sitl_default" >> ~/.bashrc
   9. echo "export ROS_PACKAGE_PATH=\$ROS_PACKAGE_PATH:~/catkin_ws/Firmware" >> ~/.bashrc
   10. echo "export ROS_PACKAGE_PATH=\$ROS_PACKAGE_PATH:~/catkin_ws/Firmware/Tools/sitl_gazebo" >> ~/.bashrc
   11. source ~/.bashrc

   无人机导航及定位功能包配置

   1 . 安装必要的导航包

   1. sudo apt-get install ros-melodic-navigation
   2. sudo apt-get install ros-melodic-gmapping
   3. sudo apt-get install ros-melodic-ar-track-alvar*
   4. sudo apt-get install ros-melodic-moveit*

   2 . 自主导航实现
   编写launch文件如下
   _ros_2Dnav_demo_px4.launch_

   该launch的主要作用是启用多个launch文件，其中包括启动gazebo以及无人机模型、建图、路径规划、键盘控制、将路径规划输出转换成无人机飞控的节点，其中，我们比较无人机的自主导航与小车的自主导航可以知道，主要差别就在于将路径规划的输出转换成无人机飞控的输出。
   2Dlidar_px4.launch

   14.  

   该launch文件主要是启用了px4自带的无人机仿真启动文件，负责启动gazebo和无人机模型，其中我们将无人机模型换成加装了激光雷达的模型。
   gmapping.launch

   5.  

   该launch文件主要是启用建图程序，主要是用了gmapping的建图功能包，所以在运行前我们需要安装好gmapping导航包，确保程序可以正常运行。
   _nav_px4.launch_

   22. args="-d $(find ros_navigation)/rviz/ros_navigation.rviz"/>

   该launch文件启用了路径规划的节点，其中我们使用了dwa局部路径规划的方法。
_ros_2DNav.launch_

这个launch文件主要负责启动将路径规划输出转换成无人机的飞控的节点， 该节点由c++编写实现，如下所示，主要实现ros navigation中move_base速度控制输出的cmd_vel控制px4 quadrotor
ros_nav_quadrotor.cpp

1. #include "ros_nav_quadrotor.h"
2. using namespace std;
3. using namespace Eigen;
4. PX4RosNav::PX4RosNav(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private):
5. nh_(nh),
6. nh_private_(nh_private) {
7. initialize();
8. cmdloop_timer_ = nh_.createTimer(ros::Duration(0.1), &PX4RosNav::CmdLoopCallback, this); // Define timer for constant loop rate
9.  
10. cmd_vel_sub_ = nh_private_.subscribe("/px4_vel", 1, &PX4RosNav::CmdVelCallback, this,ros::TransportHints().tcpNoDelay());
11.  
12. }
13.  
14. PX4RosNav::~PX4RosNav() {
15. //Destructor
16. }
17. void PX4RosNav::CmdLoopCallback(const ros::TimerEvent& event)
18. {
19. PublishVelControl();
20. }
21.  
22. void PX4RosNav::PublishVelControl(){
23.  
24. OffboardControl_.send_velxy_posz_setpoint(px4_vel_,desire_posz_);
25. // cout << "px4_vel[0]"<
26. // cout << "px4_vel[1]"<
27. }
28.  
29. void PX4RosNav::CmdVelCallback(const geometry_msgs::Twist &msg){
30. px4_vel_[0] = msg.linear.x;
31. px4_vel_[1] = msg.linear.y;
32.  
33. }
34. void PX4RosNav::initialize()
35. {
36. px4_vel_[0] = 0;
37. px4_vel_[1] = 0;
38. //读取offboard模式下飞机的期望高度
39. nh_.param("desire_posz_", desire_posz_, 1.0);
40. }
41. int main(int argc, char** argv) {
42. ros::init(argc,argv,"ros_nav_quadrotor");
43. ros::NodeHandle nh("");
44. ros::NodeHandle nh_private("~");
45. PX4RosNav PX4RosNav(nh, nh_private);
46. ros::spin();
47. return 0;
48. }

Gazebo无人机导航仿真实现

编译运行

1. roslaunch simulation ros_2Dnav_demo_px4.launch

最终的效果如下动图所示，实现了无人机的路径规划以及规避障碍

可参考最终节点图如下：