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_

   1. <launch>
   2. <arg name="world_path" default="$(find simulation)/worlds/cloister.world" />
   3. <!-- 启动带有激光雷达的无人机模型-->
   4. <include file="$(find simulation)/launch/px4/2Dlidar_px4.launch">
   5. <arg name="world" value="$(arg world_path)" />
   6. </include>
   7. <!—参数说明-->
   8. <param name="/mavros/local_position/tf/send" type="bool" value="true" />
   9. <param name="/mavros/local_position/frame_id" type="str" value="base_link" />
   10. <param name="/mavros/local_position/tf/frame_id" type="str" value="odom" />
   11. <!-- 启动建图-->
   12. <include file="$(find ros_slam)/launch/gmapping.launch">
   13. </include>
   14. <!-- 启动导航-->
   15. <include file="$(find ros_navigation)/launch/nav_px4.launch">
   16. </include>
   17. <!-- 启用导航输出转mavros节点-->
   18. <include file="$(find px4_control)/launch/ros_2DNav.launch">
   19. <arg name="desire_posz_" value="2" />
   20. </include>
   21. <!—启用键盘控制-->
   22. <node pkg="simulation" type="keyboard_control_px4.py" name="keyboard_control_px4" output="screen" launch-prefix="gnome-terminal --tab -e">
   23. </node>
   24. </launch>

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

   1. <launch>
   2. <node pkg="tf" name="tf_2Dlidar" type="static_transform_publisher" args="0 0 0 3.1415926 0 0 base_link 2Dlidar_link 100"/>
   3. <!-- vehicle pose -->
   4. <arg name="x" default="0"/>
   5. <arg name="y" default="0"/>
   6. <arg name="z" default="0"/>
   7. <arg name="R" default="0"/>
   8. <arg name="P" default="0"/>
   9. <arg name="Y" default="0"/>
   10. <arg name="world" default="$(find simulation)/worlds/empty.world" />
   11. <arg name="sdf" default="$(find simulation)/models/iris_2Dlidar/iris_2Dlidar.sdf" />
   12. <arg name="verbose" default="false"/>
   13. <arg name="debug" default="false"/>
   14.  
   15. <include file="$(find px4)/launch/mavros_posix_sitl.launch" >
   16. <arg name="x" value="$(arg x)"/>
   17. <arg name="y" value="$(arg y)"/>
   18. <arg name="z" value="$(arg z)"/>
   19. <arg name="R" value="$(arg R)"/>
   20. <arg name="P" value="$(arg P)"/>
   21. <arg name="Y" value="$(arg Y)"/>
   22. <arg name="sdf" value="$(arg sdf)" />
   23. <arg name="verbose" value="$(arg verbose)" />
   24. <arg name="debug" value="$(arg debug)" />
   25. <arg name="world" value="$(arg world)" />
   26. </include>
   27. </launch>

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

   1. <launch>
   2. <arg name="scan_topic" default="/lidar2Dscan" />
   3. <arg name="base_frame" default="base_link"/>
   4. <arg name="odom_frame" default="odom"/>
   5.  
   6. <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
   7. <param name="base_frame" value="$(arg base_frame)"/> <!--底盘坐标系-->
   8. <param name="odom_frame" value="$(arg odom_frame)"/> <!--里程计坐标系-->
   9. <param name="map_update_interval" value="1.0"/> <!--更新时间(s)，每多久更新一次地图，不是频率-->
   10. <param name="maxUrange" value="7"/> <!--激光雷达最大可用距离，在此之外的数据截断不用-->
   11. <param name="maxRange" value="10"/> <!--激光雷达最大距离-->
   12. <param name="/use_sim_time" value="true" />
   13. <param name="sigma" value="0.05"/>
   14. <param name="kernelSize" value="1"/>
   15. <param name="lstep" value="0.05"/>
   16. <param name="astep" value="0.05"/>
   17. <param name="iterations" value="5"/>
   18. <param name="lsigma" value="0.075"/>
   19. <param name="ogain" value="3.0"/>
   20. <param name="lskip" value="0"/>
   21. <param name="minimumScore" value="200"/>
   22. <param name="srr" value="0.01"/>
   23. <param name="srt" value="0.02"/>
   24. <param name="str" value="0.01"/>
   25. <param name="stt" value="0.02"/>
   26. <param name="linearUpdate" value="0.5"/>
   27. <param name="angularUpdate" value="0.436"/>
   28. <param name="temporalUpdate" value="-1.0"/>
   29. <param name="resampleThreshold" value="0.5"/>
   30. <param name="particles" value="80"/>
   31. <param name="xmin" value="-25.0"/>
   32. <param name="ymin" value="-25.0"/>
   33. <param name="xmax" value="25.0"/>
   34. <param name="ymax" value="25.0"/>
   35. <param name="delta" value="0.05"/>
   36. <param name="llsamplerange" value="0.01"/>
   37. <param name="llsamplestep" value="0.01"/>
   38. <param name="lasamplerange" value="0.005"/>
   39. <param name="lasamplestep" value="0.005"/>
   40. <remap from="scan" to="$(arg scan_topic)"/>
   41. </node>
   42. </launch>

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

   1. <launch>
   2. <arg name="open_rviz" default="true"/>
   3. <arg name="move_forward_only" default="false"/>
   4. <arg name="cmd_vel_topic" default="/px4_vel" />
   5. <arg name="odom_topic" default="mavros/local_position/odom" /> <!-- frame_id: "odom" child_frame_id: "base_link" -->
   6. <!-- move_base -->
   7. <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
   8. <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" />
   9. <rosparam file="$(find ros_navigation)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
   10. <rosparam file="$(find ros_navigation)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />
   11. <rosparam file="$(find ros_navigation)/param/local_costmap_params.yaml" command="load" />
   12. <rosparam file="$(find ros_navigation)/param/global_costmap_params.yaml" command="load" />
   13. <rosparam file="$(find ros_navigation)/param/move_base_params.yaml" command="load" />
   14. <rosparam file="$(find ros_navigation)/param/dwa_local_planner_params.yaml" command="load" />
   15. <remap from="cmd_vel" to="$(arg cmd_vel_topic)"/>
   16. <remap from="odom" to="$(arg odom_topic)"/>
   17. <param name="DWAPlannerROS/min_vel_x" value="0.0" if="$(arg move_forward_only)" />
   18. </node>
   19. <!-- rviz -->
   20. <group if="$(arg open_rviz)">
   21. <node pkg="rviz" type="rviz" name="rviz" required="true"
   22. args="-d $(find ros_navigation)/rviz/ros_navigation.rviz"/>
   23. </group>
   24. </launch>

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

1. <launch>
2. <!--
3. desire_posz_：期望高度-->
4. <arg name="desire_posz_" default="1" />
5. <node pkg="px4_control" type="ros_nav_quadrotor_node" name="ros_nav_quadrotor_node" output="screen">
6. <param name="desire_posz_" value = "$(arg desire_posz_)"/>
7. </node>
8. </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]"<<px4_vel_[0] <<endl;
26. // cout << "px4_vel[1]"<<px4_vel_[1] <<endl;
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<float>("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

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

可参考最终节点图如下：