摘自：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的安装源

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

2 . 加入秘钥

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

3 . 更新

  sudo apt-get update

4 . 安装ros

  sudo apt-get install ros-melodic-desktop

5 . Source ROS

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

6 . 安装Gazebo

  sudo apt install ros-melodic-gazebo9*

7 . 初始化rosdep

  rosdep init rosdep update

8 . 安装catkin工具

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

9 . 安装mavros

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

10 . 安装geographiclib dataset

  #下载脚本 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仿真工具安装说明

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

  #下载脚本 wget https://raw.githubusercontent.com/PX4/Firmware/master/Tools/setup/ubuntu.sh wget https://raw.githubusercontent.com/PX4/Firmware/master/Tools/setup/requirements.txt #这俩个文件下载不下来可以试试“xx上网” #执行脚本： source ubuntu.sh 创建工作空间   mkdir -p ~/catkin_ws/src cd ~/catkin_ws/src/ catkin_init_workspace 下载编译px4   #下载代码 cd ~/catkin_ws/ git clone https://github.com/PX4/Firmware #然后更新submodule切换固件并编译 cd Firmware git submodule update --init --recursive git checkout v1.11.0-beta1 make distclean #在具体编译前还需要安装相关的工具 sudo apt-get install python-jinja2 sudo pip install numpy toml #开始编译 make px4_sitl_default gazebo 添加相应的环境变量信息   cd ~/catkin_ws/ catkin build #添加工作空间source echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc #添加gazebo模型路径 echo "export GAZEBO_MODEL_PATH=:~/catkin_ws/models" >> ~/.bashrc #添加px4路径 echo "source ~/catkin_ws/Firmware/Tools/setup_gazebo.bash ~/catkin_ws/Firmware ~/catkin_ws/Firmware/build/px4_sitl_default" >> ~/.bashrc echo "export ROS_PACKAGE_PATH=\$ROS_PACKAGE_PATH:~/catkin_ws/Firmware" >> ~/.bashrc echo "export ROS_PACKAGE_PATH=\$ROS_PACKAGE_PATH:~/catkin_ws/Firmware/Tools/sitl_gazebo" >> ~/.bashrc source ~/.bashrc

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

1 . 安装必要的导航包

  sudo apt-get install ros-melodic-navigation sudo apt-get install ros-melodic-gmapping sudo apt-get install ros-melodic-ar-track-alvar* sudo apt-get install ros-melodic-moveit*

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

  <launch> <arg name="world_path" default="$(find simulation)/worlds/cloister.world" /> <!-- 启动带有激光雷达的无人机模型--> <include file="$(find simulation)/launch/px4/2Dlidar_px4.launch"> <arg name="world" value="$(arg world_path)" /> </include> <!—参数说明--> <param name="/mavros/local_position/tf/send" type="bool" value="true" /> <param name="/mavros/local_position/frame_id" type="str" value="base_link" /> <param name="/mavros/local_position/tf/frame_id" type="str" value="odom" /> <!-- 启动建图--> <include file="$(find ros_slam)/launch/gmapping.launch"> </include> <!-- 启动导航--> <include file="$(find ros_navigation)/launch/nav_px4.launch"> </include> <!-- 启用导航输出转mavros节点--> <include file="$(find px4_control)/launch/ros_2DNav.launch"> <arg name="desire_posz_" value="2" /> </include> <!—启用键盘控制--> <node pkg="simulation" type="keyboard_control_px4.py" name="keyboard_control_px4" output="screen" launch-prefix="gnome-terminal --tab -e"> </node> </launch>

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

  <launch> <node pkg="tf" name="tf_2Dlidar" type="static_transform_publisher" args="0 0 0 3.1415926 0 0 base_link 2Dlidar_link 100"/> <!-- vehicle pose --> <arg name="x" default="0"/> <arg name="y" default="0"/> <arg name="z" default="0"/> <arg name="R" default="0"/> <arg name="P" default="0"/> <arg name="Y" default="0"/> <arg name="world" default="$(find simulation)/worlds/empty.world" /> <arg name="sdf" default="$(find simulation)/models/iris_2Dlidar/iris_2Dlidar.sdf" /> <arg name="verbose" default="false"/> <arg name="debug" default="false"/>  <include file="$(find px4)/launch/mavros_posix_sitl.launch" > <arg name="x" value="$(arg x)"/> <arg name="y" value="$(arg y)"/> <arg name="z" value="$(arg z)"/> <arg name="R" value="$(arg R)"/> <arg name="P" value="$(arg P)"/> <arg name="Y" value="$(arg Y)"/> <arg name="sdf" value="$(arg sdf)" /> <arg name="verbose" value="$(arg verbose)" /> <arg name="debug" value="$(arg debug)" /> <arg name="world" value="$(arg world)" /> </include> </launch>

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

  <launch> <arg name="scan_topic" default="/lidar2Dscan" /> <arg name="base_frame" default="base_link"/> <arg name="odom_frame" default="odom"/>  <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen"> <param name="base_frame" value="$(arg base_frame)"/> <!--底盘坐标系--> <param name="odom_frame" value="$(arg odom_frame)"/> <!--里程计坐标系--> <param name="map_update_interval" value="1.0"/> <!--更新时间(s)，每多久更新一次地图，不是频率--> <param name="maxUrange" value="7"/> <!--激光雷达最大可用距离，在此之外的数据截断不用--> <param name="maxRange" value="10"/> <!--激光雷达最大距离--> <param name="/use_sim_time" value="true" /> <param name="sigma" value="0.05"/> <param name="kernelSize" value="1"/> <param name="lstep" value="0.05"/> <param name="astep" value="0.05"/> <param name="iterations" value="5"/> <param name="lsigma" value="0.075"/> <param name="ogain" value="3.0"/> <param name="lskip" value="0"/> <param name="minimumScore" value="200"/> <param name="srr" value="0.01"/> <param name="srt" value="0.02"/> <param name="str" value="0.01"/> <param name="stt" value="0.02"/> <param name="linearUpdate" value="0.5"/> <param name="angularUpdate" value="0.436"/> <param name="temporalUpdate" value="-1.0"/> <param name="resampleThreshold" value="0.5"/> <param name="particles" value="80"/> <param name="xmin" value="-25.0"/> <param name="ymin" value="-25.0"/> <param name="xmax" value="25.0"/> <param name="ymax" value="25.0"/> <param name="delta" value="0.05"/> <param name="llsamplerange" value="0.01"/> <param name="llsamplestep" value="0.01"/> <param name="lasamplerange" value="0.005"/> <param name="lasamplestep" value="0.005"/> <remap from="scan" to="$(arg scan_topic)"/> </node> </launch>

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

  <launch> <arg name="open_rviz" default="true"/> <arg name="move_forward_only" default="false"/> <arg name="cmd_vel_topic" default="/px4_vel" /> <arg name="odom_topic" default="mavros/local_position/odom" /> <!-- frame_id: "odom" child_frame_id: "base_link" --> <!-- move_base --> <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen"> <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" /> <rosparam file="$(find ros_navigation)/param/costmap_common_params.yaml" command="load" ns="global_costmap" /> <rosparam file="$(find ros_navigation)/param/costmap_common_params.yaml" command="load" ns="local_costmap" /> <rosparam file="$(find ros_navigation)/param/local_costmap_params.yaml" command="load" /> <rosparam file="$(find ros_navigation)/param/global_costmap_params.yaml" command="load" /> <rosparam file="$(find ros_navigation)/param/move_base_params.yaml" command="load" /> <rosparam file="$(find ros_navigation)/param/dwa_local_planner_params.yaml" command="load" /> <remap from="cmd_vel" to="$(arg cmd_vel_topic)"/> <remap from="odom" to="$(arg odom_topic)"/> <param name="DWAPlannerROS/min_vel_x" value="0.0" if="$(arg move_forward_only)" /> </node> <!-- rviz --> <group if="$(arg open_rviz)"> <node pkg="rviz" type="rviz" name="rviz" required="true" args="-d $(find ros_navigation)/rviz/ros_navigation.rviz"/> </group> </launch>

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

  <launch> <!-- desire_posz_：期望高度--> <arg name="desire_posz_" default="1" /> <node pkg="px4_control" type="ros_nav_quadrotor_node" name="ros_nav_quadrotor_node" output="screen"> <param name="desire_posz_" value = "$(arg desire_posz_)"/> </node> </launch>

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

  #include "ros_nav_quadrotor.h" using namespace std; using namespace Eigen; PX4RosNav::PX4RosNav(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private): nh_(nh), nh_private_(nh_private) { initialize(); cmdloop_timer_ = nh_.createTimer(ros::Duration(0.1), &PX4RosNav::CmdLoopCallback, this); // Define timer for constant loop rate  cmd_vel_sub_ = nh_private_.subscribe("/px4_vel", 1, &PX4RosNav::CmdVelCallback, this,ros::TransportHints().tcpNoDelay());  }  PX4RosNav::~PX4RosNav() { //Destructor } void PX4RosNav::CmdLoopCallback(const ros::TimerEvent& event) { PublishVelControl(); }  void PX4RosNav::PublishVelControl(){  OffboardControl_.send_velxy_posz_setpoint(px4_vel_,desire_posz_); // cout << "px4_vel[0]"<<px4_vel_[0] <<endl; // cout << "px4_vel[1]"<<px4_vel_[1] <<endl; }  void PX4RosNav::CmdVelCallback(const geometry_msgs::Twist &msg){ px4_vel_[0] = msg.linear.x; px4_vel_[1] = msg.linear.y;  } void PX4RosNav::initialize() { px4_vel_[0] = 0; px4_vel_[1] = 0; //读取offboard模式下飞机的期望高度 nh_.param<float>("desire_posz_", desire_posz_, 1.0); } int main(int argc, char** argv) { ros::init(argc,argv,"ros_nav_quadrotor"); ros::NodeHandle nh(""); ros::NodeHandle nh_private("~"); PX4RosNav PX4RosNav(nh, nh_private); ros::spin(); return 0; }

Gazebo无人机导航仿真实现

编译运行

  roslaunch simulation ros_2Dnav_demo_px4.launch

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

可参考最终节点图如下：