PX4与仿真入门教程-dronedoc-使用LiDAR和AMCL进行自我位置估计

PX4与仿真入门教程-dronedoc-使用LiDAR和AMCL进行自我位置估计

说明：

• 介绍使用 LiDAR 和自适应蒙特卡罗定位 (AMCL) 进行自我位置估计。

制作地图以进行自我位置估计：

• 需要提前准备地图以进行自我位置估计。 如果您没有提前准备好地图，请使用以下任一方法创建地图。
• 使用 Gazebo 插件创建地图，参考教程
• 使用 Gmapping 生成地图，参考教程

使用 AMCL 执行自身位置估计

• 蒙特卡罗定位是一种使用粒子滤波器进行自我位置估计的方法。
• 在该算法中，机器人位置的概率分布由粒子分布表示，并根据观测信息更新分布来估计自身位置。
• 自适应蒙特卡罗定位是一种可以动态调整蒙特卡罗位置估计中使用的粒子数量的算法。
• 这次我们将使用 ROS amcl 包。 我们还使用 map_server 包来利用上一节中创建的地图信息。

使用 map_server 发布地图信息：

• 由于amcl使用/map主题中发布的地图信息进行自身位置估计，因此需要使用map_server发布上一节创建的地图信息。
• 使用以下命令启动发布地图信息的节点。 map.yaml 是使用地图图像生成的配置文件。

rosrun map_server map_server map.yaml

• 如果您启动 Rviz 并显示/map题，您将看到如下图所示的地图。

• 如果没有显示地图，请检查Global Option的Fixed Frame是否为map。

启动 amcl 节点

• 接下来，编写一个 Launch 文件来启动 amcl 节点。
• amcl节点参数较多，调整参数影响自身位置估计的准确性。 您可能希望引用现有的 Launch 文件，例如 pr2_navigation 包中的 amcl_node.xml。
• 在这个Launch文件中，激光扫描的主题是base_scan，所以如果你要使用它，根据你使用的传感器的设置更改主题名称。
• 您还需要以与机器人配置相同的方式更改 base_frame_id 和 odom_frame_id。
• 每个 amcl 参数及其描述都可以在 amcl ROS Wiki 页面上找到，以供在调整参数时参考。
• 包含 map_server 启动的启动文件amcl.launch如下。

<launch>
    <node name="map_server" pkg="map_server" type="map_server" args="$(find px4_sim_pkg)/map/willow_garage.yaml" />

    <arg name="use_map_topic" default="true"/>

    <node pkg="amcl" type="amcl" name="amcl">
        <remap from="scan" to="/laser/scan" />
        <param name="use_map_topic" value="$(arg use_map_topic)"/>
        <!-- Publish scans from best pose at a max of 10 Hz -->
        <param name="base_frame_id" value="base_link"/>
        <param name="odom_model_type" value="omni"/>
        <param name="odom_alpha5" value="0.1"/>
        <param name="transform_tolerance" value="0.2" />
        <param name="gui_publish_rate" value="10.0"/>
        <param name="laser_max_beams" value="30"/>
        <param name="min_particles" value="500"/>
        <param name="max_particles" value="5000"/>
        <param name="kld_err" value="0.05"/>
        <param name="kld_z" value="0.99"/>
        <param name="odom_alpha1" value="0.2"/>
        <param name="odom_alpha2" value="0.2"/>
        <!-- translation std dev, m -->
        <param name="odom_alpha3" value="0.8"/>
        <param name="odom_alpha4" value="0.2"/>
        <param name="laser_z_hit" value="0.5"/>
        <param name="laser_z_short" value="0.05"/>
        <param name="laser_z_max" value="0.05"/>
        <param name="laser_z_rand" value="0.5"/>
        <param name="laser_sigma_hit" value="0.2"/>
        <param name="laser_lambda_short" value="0.1"/>
        <param name="laser_lambda_short" value="0.1"/>
        <param name="laser_model_type" value="likelihood_field"/>
        <!-- <param name="laser_model_type" value="beam"/> -->
        <param name="laser_likelihood_max_dist" value="2.0"/>
        <param name="update_min_d" value="0.2"/>
        <param name="update_min_a" value="0.5"/>
        <param name="odom_frame_id" value="odom"/>
        <param name="resample_interval" value="1"/>
        <param name="transform_tolerance" value="0.1"/>
        <param name="recovery_alpha_slow" value="0.0"/>
        <param name="recovery_alpha_fast" value="0.0"/>
    </node>
</launch>

启动 PX4 SITL 模拟器

• 最后，创建一个 Launch 文件来启动 PX4 SITL 模拟器。

• 内容与使用 GPS 自主飞行的 mymodel_sitl_tf.launch 几乎相同，但没有将 TF 从 odom 发布到地图的节点。

• amcl 节点计算从map坐标系到机器人基准坐标系的 TF（这次是 base_link），但它实际上是将 TF 从map传播到odom坐标系（这次是 odom）。这次我们将使用amcl广播的map-to-odom TF，所以我们会阻止其他节点广播map-to-odom TF。

• Odom 是根据车轮的转数计算出来的，用于自我位置估计，但由于滑动和干扰导致误差累积，因此需要使用传感器信息对其进行校正。 amcl 节点使用地图和传感器信息来估计此误差，并将其作为 TF 从map传播到 odom。

• 另外，由于本次使用安装的LiDAR数据进行自身位置估计，因此还需要将TF从base_link广播到lidar_link。

• 关系图如下：

• mymodel_sitl_amcl.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/local_position/tf/send" type="bool" value="true" />
    <param name="/mavros/local_position/frame_id" type="str" value="odom" />
    <param name="/mavros/local_position/tf/frame_id" type="str" value="odom" />
    <param name="/mavros/setpoint_velocity/mav_frame"  type="str" value="BODY_NED" />

</launch>

测试：

• 新终端，首先启动PX4模拟器。

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

• 新终端，一旦 Gazebo 启动，下一步就是启动 mavteleop 节点。

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

• 在启动PX4模拟器，启动起飞

commander takeoff

• 新终端，起飞然后更改模式，以便您可以从游戏手柄控制无人机。

rosrun mavros mavsys mode -c OFFBOARD

• 新终端，最后启动amcl节点。

roslaunch px4_sim_pkg amcl.launch

• 启动 Rviz 并查看 /map 主题。

• 此外，当您查看 /particlecloud 主题时，您将看到具有估计姿势的粒子。

• 启动 Rviz 后，选择 2D Pose Estimate 并给出初始位置和姿势，如下图所示。

• 之后，让我们使用游戏手柄移动无人机。

• 起初，姿势（红色箭头）分布如下图所示

• 随着数据的观察，姿势的方差变小。

• 在下面的视频中，您在途中撞到了一堵墙，但是您可以看到可以估计位置而没有任何影响。