PX4与仿真入门教程-dronedoc-使用 LiDAR 进行 SLAM

PX4与仿真入门教程-dronedoc-使用 LiDAR 进行 SLAM

说明：

• 介绍使用 LiDAR 进行 SLAM

SLAM ：

• 在使用 LiDAR 和 AMCL 的自我位置估计中，使用预先创建的地图执行自我位置估计，例如通过使用 Turtlebot 创建地图。
• 下面是节点和主题的示意图。 虚线是预先创建地图的工作中的节点的示例，在这种情况下，显示了使用costmap_2d的示例。
• 由于costmap_2d将根据sensor信息创建的占用网格图发布到/map主题，所以使用map_saver节点将发布的地图保存到文件中。
• map_server 节点读取以这种方式创建的现有地图并将其发布到 /map 主题。
• amcl节点根据发布的地图信息和传感器信息估计自己的位置，并广播TF。

• 请注意，虚线内的制图和虚线外的自我位置估计不是同时进行的。
• 同时执行此操作称为 SLAM（同步定位和映射）。

gmapping 和 Hector SLAM

• 这一次，我们将使用 Hector SLAM 算法生成地图并估计自身位置。

• 使用Gmapping生成地图的gmapping也是SLAM算法之一。

• 虽然 gmapping 需要机器人里程计，但 Hector SLAM 是一种无里程计的 SLAM 技术。

• 在使用无人机时，往往很难获得里程计，因此Hector SLAM是一种适合无人机的SLAM方法。

• 对于使用作为 ROS 包提供的 LiDAR 的 SLAM 算法的比较，请参阅 ROS 的 LidarSLAM Summary-Qiita。

• 它使用 hector_slam 包，所以让我们使用以下命令安装它

sudo apt install ros-melodic-hector-slam

测试

• 启动 PX4 SITL 模拟器
• 启动 SITL 模拟器的 Launch 文件mymodel_sitl_hector.launch如下。

<launch>

    <node pkg="tf" name="base2lidar" type="static_transform_publisher" args="0 0 0.1 0 0 0 base_link lidar_link 100"/>

    <arg name="vehicle" default="iris_2d_lidar"/>
    <arg name="world" default="$(find px4_sim_pkg)/worlds/willow_garage.world" />
    <arg name="sdf" default="$(find px4_sim_pkg)/models/iris_2d_lidar/model.sdf" />
    <arg name="verbose" default="false"/>
    <arg name="debug" default="false"/>

    <include file="$(find px4)/launch/mavros_posix_sitl.launch" >
        <arg name="sdf" value="$(arg sdf)" />
        <arg name="vehicle" value="$(arg vehicle)" />
        <arg name="verbose" value="$(arg verbose)" />
        <arg name="debug" value="$(arg debug)" />
        <arg name="world" value="$(arg world)" />
    </include>

    <param name="/mavros/setpoint_velocity/mav_frame"  type="str" value="BODY_NED" />
</launch>

• 和mymodel_sitl_amcl.launch几乎一样，但是我去掉了参数设置部分，因为我不需要将tf从odom发布到base_link。
• 启动 hector_mapping 节点
• 我们将根据 hector_mapping 包中的 mapping_default.launch 进行更改，因此使用 roscp 命令复制文件。

roscp hector_mapping mapping_default.launch ~/dronedoc_ws/src/px4_sim_pkg/launch/

• 只需要更改以下三个值：
• 每个如下，因此请根据您的机器人进行设置。

基本框架/base_frame
机器人底座坐标系（框架）

odom_frame
机器人的里程计坐标系（框架）

scan_topic
已发表的激光扫描主题

• 另外，如果机器人的里程计不可用，则使base_frame和odom_frame的值相同。
• 有关在使用 hector_mapping 包时设置 TF 的信息，请参阅如何为您的机器人设置 hector_slam。
• 最终launch文件mapping_default.launch如下：

<?xml version="1.0"?>

<launch>
  <arg name="tf_map_scanmatch_transform_frame_name" default="scanmatcher_frame"/>
  <arg name="base_frame" default="base_link"/>
  <arg name="odom_frame" default="base_link"/>
  <arg name="pub_map_odom_transform" default="true"/>
  <arg name="scan_subscriber_queue_size" default="5"/>
  <arg name="scan_topic" default="/laser/scan"/>
  <arg name="map_size" default="2048"/>

  <node pkg="hector_mapping" type="hector_mapping" name="hector_mapping" output="screen">

    <!-- Frame names -->
    <param name="map_frame" value="map" />
    <param name="base_frame" value="$(arg base_frame)" />
    <param name="odom_frame" value="$(arg odom_frame)" />

    <!-- Tf use -->
    <param name="use_tf_scan_transformation" value="true"/>
    <param name="use_tf_pose_start_estimate" value="false"/>
    <param name="pub_map_odom_transform" value="$(arg pub_map_odom_transform)"/>

    <!-- Map size / start point -->
    <param name="map_resolution" value="0.050"/>
    <param name="map_size" value="$(arg map_size)"/>
    <param name="map_start_x" value="0.5"/>
    <param name="map_start_y" value="0.5" />
    <param name="map_multi_res_levels" value="2" />

    <!-- Map update parameters -->
    <param name="update_factor_free" value="0.4"/>
    <param name="update_factor_occupied" value="0.9" />
    <param name="map_update_distance_thresh" value="0.4"/>
    <param name="map_update_angle_thresh" value="0.06" />
    <param name="laser_z_min_value" value = "-1.0" />
    <param name="laser_z_max_value" value = "1.0" />

    <!-- Advertising config -->
    <param name="advertise_map_service" value="true"/>

    <param name="scan_subscriber_queue_size" value="$(arg scan_subscriber_queue_size)"/>
    <param name="scan_topic" value="$(arg scan_topic)"/>

    <!-- Debug parameters -->
    <!--
      <param name="output_timing" value="false"/>
      <param name="pub_drawings" value="true"/>
      <param name="pub_debug_output" value="true"/>
    -->
    <param name="tf_map_scanmatch_transform_frame_name" value="$(arg tf_map_scanmatch_transform_frame_name)" />
  </node>

  <!--<node pkg="tf" type="static_transform_publisher" name="map_nav_broadcaster" args="0 0 0 0 0 0 map nav 100"/>-->
</launch>

测试：

• 新开终端，启动PX4 SITL模拟器

cd ~/tools/dronedoc/
source load_environment.sh
roslaunch px4_sim_pkg mymodel_sitl_hector.launch

• 新开终端，我们将使用游戏手柄操作无人机，因此启动 mavteleop 节点

cd ~/tools/dronedoc/
source load_environment.sh
roslaunch px4_sim_pkg mavros_teleop.launch

• 在启动仿真的终端下，启动起飞

commander takeoff

• 新开终端，让我们切换到 OFFBOARD 模式并驾驶无人机

rosrun mavros mavsys mode -c OFFBOARD

• 新开终端，启动 hector_mapping 节点

cd ~/tools/dronedoc/
source load_environment.sh
roslaunch px4_sim_pkg mapping_default.launch

• 新开终端，启动Rviz

rviz

• 效果如图：