基于px4的无人机自主导航

2104
5
2020年6月30日 12时12分

基于px4的无人机自主导航

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

ROS导航框架介绍

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

基于px4的无人机自主导航插图

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

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
  1. 创建工作空间
    mkdir -p ~/catkin_ws/src
    cd ~/catkin_ws/src/
    catkin_init_workspace
    
  2. 下载编译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
    
  3. 添加相应的环境变量信息
    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的无人机自主导航插图(2)

 

 

可参考最终节点图如下:
基于px4的无人机自主导航插图(3)

 

大家如有遇到任何问题可以在古月居论坛的无人机版块与笔者一起探讨。

发表评论

后才能评论

评论列表(5条)

  •          2020年11月21日 上午11:20

    请问GeographicLib并不支持Ubuntu18.04 melodic, 您是如何安装的?

  • 臻臻臻 2020年11月5日 下午3:51

    题主你好,请问一下,我的rviz中不出现地图,map中有警告No map received,可能是什么原因呢?雷达发布的topic是scan

  • 2020年10月20日 下午6:12

    题主你好,想问问你的launch文件都是在哪儿新建的,是在 ~/catkin_ws/Firmware/launch 文件下吗?还有ros_nav_quadrotor.cpp 这个节点文件是放在哪儿呢?

    • Judez 回复 2020年10月31日 下午1:12

      launch文件可以放在任意的功能包里面,只需要在启动的时候修改launch文件前面的功能包名字;类似的cpp文件你可以新建一个功能包,将cpp放进去,想启用这个cpp的话,只需要在launch中启用这个节点;文件放在哪里在ros中不会有很大的关系,很多时候我们为了方便起见,建立不同的功能包加以区分,建议你学一下ros中launch文件的使用以及如何在ros中运行cpp文件。

  • (σ゚∀゚)σ 2020年7月8日 下午6:52

    运行make px4_sitl_default gazebo的时候出现这个错误,怎么办啊

    make: Warning: File ‘Makefile’ has modification time 22533 s in the future
    — PX4 version: v1.11.0-beta1
    — PX4 config file: /headless/Desktop/catkin_ws/Firmware/boards/px4/sitl/default.cmake
    — PX4 config: px4_sitl_default
    — PX4 platform: posix
    — PX4 lockstep: enabled
    — cmake build type: RelWithDebInfo
    — The CXX compiler identification is GNU 7.5.0
    — The C compiler identification is GNU 7.5.0
    — The ASM compiler identification is GNU
    — Found assembler: /usr/bin/cc
    — Check for working CXX compiler: /usr/bin/c++
    — Check for working CXX compiler: /usr/bin/c++ — works
    — Detecting CXX compiler ABI info
    — Detecting CXX compiler ABI info – done
    — Detecting CXX compile features
    — Detecting CXX compile features – done
    — Check for working C compiler: /usr/bin/cc
    — Check for working C compiler: /usr/bin/cc — works
    — Detecting C compiler ABI info
    — Detecting C compiler ABI info – done
    — Detecting C compile features
    — Detecting C compile features – done
    — Building for code coverage
    — ccache enabled (export CCACHE_DISABLE=1 to disable)
    — Found PythonInterp: /usr/bin/python3 (found suitable version “3.6.9”, minimum required is “3”)
    CMake Error at src/lib/CMakeLists.txt:48 (add_subdirectory):
    The source directory

    /headless/Desktop/catkin_ws/Firmware/src/lib/ecl

    does not contain a CMakeLists.txt file.

    CMake Error at cmake/px4_add_module.cmake:205 (get_target_property):
    get_target_property() called with non-existent target “ecl_airdata”.
    Call Stack (most recent call first):
    src/modules/airspeed_selector/CMakeLists.txt:33 (px4_add_module)

    CMake Error at cmake/px4_add_module.cmake:205 (get_target_property):
    get_target_property() called with non-existent target “ecl_geo_lookup”.
    Call Stack (most recent call first):
    src/modules/attitude_estimator_q/CMakeLists.txt:34 (px4_add_module)

    CMake Error at cmake/px4_add_module.cmake:205 (get_target_property):
    get_target_property() called with non-existent target “ecl_geo”.
    Call Stack (most recent call first):
    src/modules/commander/CMakeLists.txt:37 (px4_add_module)

    CMake Error at cmake/px4_add_module.cmake:205 (get_target_property):
    get_target_property() called with non-existent target “ecl_EKF”.
    Call Stack (most recent call first):
    src/modules/ekf2/CMakeLists.txt:36 (px4_add_module)

    CMake Error at cmake/px4_add_module.cmake:205 (get_target_property):
    get_target_property() called with non-existent target “ecl_geo”.
    Call Stack (most recent call first):
    src/modules/ekf2/CMakeLists.txt:36 (px4_add_module)

    CMake Error at cmake/px4_add_module.cmake:205 (get_target_property):
    get_target_property() called with non-existent target “ecl_l1”.
    Call Stack (most recent call first):
    src/modules/fw_pos_control_l1/CMakeLists.txt:37 (px4_add_module)

    CMake Error at cmake/px4_add_module.cmake:205 (get_target_property):
    get_target_property() called with non-existent target “ecl_tecs”.
    Call Stack (most recent call first):
    src/modules/fw_pos_control_l1/CMakeLists.txt:37 (px4_add_module)

    CMake Error at cmake/px4_add_module.cmake:205 (get_target_property):
    get_target_property() called with non-existent target “ecl_geo”.
    Call Stack (most recent call first):
    src/modules/local_position_estimator/CMakeLists.txt:33 (px4_add_module)

    CMake Error at cmake/px4_add_module.cmake:205 (get_target_property):
    get_target_property() called with non-existent target “ecl_geo”.
    Call Stack (most recent call first):
    src/modules/mavlink/CMakeLists.txt:36 (px4_add_module)

    CMake Error at cmake/px4_add_module.cmake:205 (get_target_property):
    get_target_property() called with non-existent target “ecl_geo”.
    Call Stack (most recent call first):
    src/modules/mc_pos_control/CMakeLists.txt:37 (px4_add_module)

    CMake Error at cmake/px4_add_module.cmake:205 (get_target_property):
    get_target_property() called with non-existent target “ecl_geo”.
    Call Stack (most recent call first):
    src/modules/navigator/CMakeLists.txt:34 (px4_add_module)

    CMake Error at cmake/px4_add_module.cmake:205 (get_target_property):
    get_target_property() called with non-existent target “ecl_l1”.
    Call Stack (most recent call first):
    src/modules/rover_pos_control/CMakeLists.txt:33 (px4_add_module)

    CMake Error at cmake/px4_add_module.cmake:205 (get_target_property):
    get_target_property() called with non-existent target “ecl_validation”.
    Call Stack (most recent call first):
    src/modules/sensors/CMakeLists.txt:41 (px4_add_module)

    CMake Error at cmake/px4_add_module.cmake:205 (get_target_property):
    get_target_property() called with non-existent target “ecl_geo”.
    Call Stack (most recent call first):
    src/modules/simulator/CMakeLists.txt:56 (px4_add_module)

    CMake Error at cmake/px4_add_module.cmake:205 (get_target_property):
    get_target_property() called with non-existent target “ecl_geo”.
    Call Stack (most recent call first):
    src/modules/vmount/CMakeLists.txt:33 (px4_add_module)

    CMake Error at cmake/px4_add_module.cmake:205 (get_target_property):
    get_target_property() called with non-existent target “ecl_geo_lookup”.
    Call Stack (most recent call first):
    src/systemcmds/tests/CMakeLists.txt:88 (px4_add_module)

    CMake Error at /usr/share/cmake-3.10/Modules/ExternalProject.cmake:2474 (message):
    No download info given for ‘sitl_gazebo’ and its source directory:

    /headless/Desktop/catkin_ws/Firmware/Tools/sitl_gazebo

    is not an existing non-empty directory. Please specify one of:

    * SOURCE_DIR with an existing non-empty directory
    * DOWNLOAD_COMMAND
    * URL
    * GIT_REPOSITORY
    * SVN_REPOSITORY
    * HG_REPOSITORY
    * CVS_REPOSITORY and CVS_MODULE
    Call Stack (most recent call first):
    /usr/share/cmake-3.10/Modules/ExternalProject.cmake:3029 (_ep_add_download_command)
    platforms/posix/cmake/sitl_target.cmake:33 (ExternalProject_Add)
    platforms/posix/CMakeLists.txt:115 (include)

    — Configuring incomplete, errors occurred!
    See also “/headless/Desktop/catkin_ws/Firmware/build/px4_sitl_default/CMakeFiles/CMakeOutput.log”.
    Error: /headless/Desktop/catkin_ws/Firmware/build/px4_sitl_default is not a directory
    Makefile:198: recipe for target ‘px4_sitl_default’ failed
    make: *** [px4_sitl_default] Error 1