基于px4的无人机自主导航

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

ROS导航框架介绍

无人机导航运动控制系统大致分为五个层次的架构，从高到低依次为：给定目标位置->建图定位->路径规划->底层控制->无人机转子速度。总结起来如下图所示：
基于px4的无人机自主导航将小车导航中用到的激光雷达移植到无人机上，通过激光雷达的扫描信息进行建图导航，无人机大致的模型如下图所示，其中蓝色的光束即代表无人机激光雷达扫描发射的光束。

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 quadrotor ros_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

最终的效果如下动图所示，实现了无人机的路径规划以及规避障碍
基于px4的无人机自主导航可参考最终节点图如下：
大家如有遇到任何问题可以在古月居论坛的无人机版块与笔者一起探讨。