ros2 Navigation 学习笔记 第三章（the construct 网站）

第三章 如何在环境中给机器人定位

序言

本章包括：

机器人定位的意义（即需要知道它对于环境中的位置和方向）

如何在ROS2中用AMCL定位

如何设置机器人的初始位置（手动、自动与使用ROS API）

如何global localization

什么是AMCL

ROS有鲁棒性极强的定位算法AMCL(自适应蒙特卡洛定位)，这是一个二维机器人的概率定位系统。

它实现了自适应蒙特卡洛定位方法，该方法使用粒子过滤器根据已知地图跟踪机器人的姿态。

当某人发布/map帧和/odom帧之间的转换时，ROS机器人被定位

这意味着机器人的/odom知道其相对于/map的相对位置，因此机器人知道它在地图上的位置，因为它的/base_link直接连接到/odom。

当一切正确，AMCL将发布该转换。现在学习如何配置AMCL以发布该转换。

如何在ROS2中定位

需要三个节点 map_server提供地图。localization提供算法。和上一章提到的life cycle manager管理各个节点的生命周期。

1、启动map_server节点

以下字段是你需要为你的包提供的启动节点的字段

map_server节点由nav2_map_server提供

所需要的参数 use_sim_time（true）和yaml_filename（都是上一章提到过的）

            package='nav2_map_server',
            executable='map_server',
            name='map_server',
            output='screen',
            parameters=[{'use_sim_time': True}, 
                        {'yaml_filename':map_file} 
                       ]),

2、启动amcl节点

需要以下字段：

amcl节点由nav2_amcl包提供

可执行文件名amcl，需要的参数是yaml文件包含所有配置参数。

            package='nav2_amcl',
            executable='amcl',
            name='amcl',
            output='screen',
            parameters=[nav2_yaml]

3、启动lifecycle_manager节点

由nav2_lifecycle_manager提供

可执行文件叫做lifecycle_manager

参数：use_sim_time. autostart. node_name（同上一章）

练习 3.1

建立一个launch文件能够在你上一节建立的地图上启动定位系统

1、创建一个叫做localization_server的包

2、创建config和launch路径

3、在launch路径内建立一个叫localization.launch.py的文件能够启动之前提到的三个节点

4、在config路径内建立一个叫amcl_config.yaml的yaml文件，文件内容如下

后来经过实际测试，yaml文件按照下面写会报错，原因如下

Description

The fully-qualified type of the plugin class. Options are “nav2_amcl::DifferentialMotionModel” and “nav2_amcl::OmniMotionModel”. Users can also provide their own custom motion model plugin type.

Note for users of galactic and earlier

The models are selectable by string key (valid options: “differential”, “omnidirectional”) rather than plugins.

amcl:
  ros__parameters:
    use_sim_time: True
    alpha1: 0.2
    alpha2: 0.2
    alpha3: 0.2
    alpha4: 0.2
    alpha5: 0.2
    base_frame_id: "base_footprint"
    beam_skip_distance: 0.5
    beam_skip_error_threshold: 0.9
    beam_skip_threshold: 0.3
    do_beamskip: false
    global_frame_id: "map"
    lambda_short: 0.1
    laser_likelihood_max_dist: 2.0
    laser_max_range: 100.0
    laser_min_range: -1.0
    laser_model_type: "likelihood_field"
    max_beams: 60
    max_particles: 8000
    min_particles: 200
    odom_frame_id: "odom"
    pf_err: 0.05
    pf_z: 0.99
    recovery_alpha_fast: 0.0
    recovery_alpha_slow: 0.0
    resample_interval: 1
    robot_model_type: "differential"
    save_pose_rate: 0.5
    sigma_hit: 0.2
    tf_broadcast: true
    transform_tolerance: 1.0
    update_min_a: 0.2
    update_min_d: 0.25
    z_hit: 0.5
    z_max: 0.05
    z_rand: 0.5
    z_short: 0.05

备注

在编译之前需要完成：

创建ament_python的python包，打开其中的setup.py，在其中加入路径

        (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')),
        (os.path.join('share', package_name, 'config'), glob('config/*')),

以及import

import os
from glob import glob

另外记住永远在整个ws下编译包。

colcon build --packages-select localization_server
source ~/ros2_ws/install/setup.bash

在此之后，为了启动定位系统。在第一个终端中

ros2 launch localization_server localization.launch.py

如果正确启动，那么你将看到

[amcl-2] [WARN] [1632242660.692356589] [amcl]: ACML cannot publish a pose or update the transform. Please set the initial pose...

注意：有warn是正常的，因为定位系统在等待初始位置，不然不知道把机器人放在哪里。

若要手动提供初始位置：

在另一个终端中启动RVIZ

rviz2

在RVIZ的display中加入：

map：展示当前的地图

TF:展示机器人的框架，需要在topic中指定发布的模型是Turtlebot3_Burger_robot

        具体做法：在Description_Topic中，写下/robot_description，这会由仿真一起启动的robot_state_publisher节点发布。

Laser ScanL显示机器人位置估计与实际地图的匹配程度。

Pose with covariance：显示定位状态

Particle Cloud：查看定位粒子分布

将现在的RVIZ配置放到src下，名为localizer_rviz_config

现在只要用第一章说过的2D pose Estimate就可以启动仿真，整个系统随之启动

如果在RVIZ中去掉所有的帧，除了map、odom和base_footprint。你可以看到从机器人中心到地图的箭头。这是AMCL发布的从odom到mp的转换。

还会看到激光数据与地图或多或少相匹配，掉膘这定位的正确。

然后用键盘移动机器人，观察定位如何适应新的机器人的位置。你应该看到离自己中在最可能的机器人的位置

在第一个终端中执行

ros2 run teleop_twist_keyboard teleop_twist_keyboard

再一次保存rviz的设置以供以后使用。

localization.launch.py的参考内容

import os

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    
    nav2_yaml = os.path.join(get_package_share_directory('localization_server'), 'config', 'amcl_config.yaml')
    map_file = os.path.join(get_package_share_directory('map_server'), 'config', 'turtlebot_area.yaml')

    return LaunchDescription([
        Node(
            package='nav2_map_server',
            executable='map_server',
            name='map_server',
            output='screen',
            parameters=[{'use_sim_time': True}, 
                        {'yaml_filename':map_file}]
        ),
            
        Node(
            package='nav2_amcl',
            executable='amcl',
            name='amcl',
            output='screen',
            parameters=[nav2_yaml]
        ),

        Node(
            package='nav2_lifecycle_manager',
            executable='lifecycle_manager',
            name='lifecycle_manager_localization',
            output='screen',
            parameters=[{'use_sim_time': True},
                        {'autostart': True},
                        {'node_names': ['map_server', 'amcl']}]
        )
    ])

关于机器人定位

当你知道机器人在地图中的xyθ时就定位了。

AMCL包使用静态映射，再用AMCL方法计算xyθ完成定位。

AMCL方法使用粒子来定位机器人，这些粒子有自己的坐标和方向值，并且具有给定的权重。权重的定义是机器人实际的姿势和该特定粒子的预测姿势之间的绝对差。粒子的权重越大，它定义的机器人姿势越准确。

每当机器人在环境中移动时，都会对粒子进行重新采样，每次采样时权重较轻的粒子会消亡，权重较高的粒子会存活下来。在多次迭代后，粒子将收敛并评估机器人自带的近似值。因此该算法基于传感器的输入来估计机器人的方向和位置。

为了在环境中准确定位机器人，需要配置良好的AMCL节点。下面是对于AMCL的一些参数解释。

AMCL的配置参数

有三类ROS参数可用于配置AMCL节点

总滤波器参数

激光模型参数

里程计模型参数。

总滤波器参数

min_particles：整形，默认500，允许最小的粒子数

max_particles：整形，默认2000，允许最大的粒子数

update_min_d：double形，默认0.25米，执行过滤器更新之前需要的平移移动

update_min_a：double形，默认0.2radians。执行过滤器更新之前需要进行旋转移动

resample_interval：整形，默认1。重新采样前所需的过滤器更新数

transform_tolerance：double，默认1s。发布转换的日期的时间，表示此转换在将来有效

recovery_alpha_slow：double，默认0.慢速平均权重滤波器的指数衰减率用于决定何时通过添加随机姿态来恢复。一个好的值可能是0.001。

recovery_alpha_fast：double，默认是0。快速平均权重滤波器的指数衰减率用于决定何时通过添加随机姿态来恢复。较好的值可能是0.1。

set_initial_pose：布尔，默认false。是否更具initial_pose参数设置初始姿态，而不是等待initial_pose消息。

initial_pose：{0，0，0}.初始坐标

always_reset_initial_pose：double，默认false。在重置时通过topic或initial_pose参数向AMCL提供初始姿势。

save_pose_rate：double，默认0.5HZ.在~initial  pose和~initial cov中将最后估计的姿态和协方差存储到参数服务器的最大速率。此保持的子哦太将用于后续运行以初始化过滤器。-1.0表示禁用

激光器模型参数

laser_min_range：double，-1。要考虑的最小扫描范围，用-1将导致使用激光器报告的最小范围

laser_max_range：double，100.最大扫描范围，用-1即用激光器报告的最大扫描范围

max_beams：int，60.更新过滤器时，每次扫描将使用多少均匀间隔的光束。

z_hit：double,0.5。模型z_hit部分的混合权重

z_short：doube，0.05。 模型z_short部分的混合权重

z_max：double,0.05.模型的z_max部分的混合权重

z_rand：doubel,0.5.模型z——rand部分的混合权重

sigama_hit：double，0.1。模型z_hit部分使用的高斯模型的标准偏差。

lambda_short：double,0.1。模型z_short部分的指数衰减参数

laser_likelihood_max_dist：double，2。在地图上进行障碍物充气的最大距离，用于likelihood_field模型。

laser_model_type：string，likelihood_field：要用的模型是beam还是likelihood_field还是laiklihood_field_prob。

里程计模型参数

robot_model_type：string,differential。用哪个模型，differential或者omnidirectional

alpha1：double，0.2. 更具机器人运动的旋转分量指定里程计旋转估计中的预期噪声

alpha2：double，0.2.根据机器人运动的平移分量指定里程计旋转估计中的预期噪声

alpha3：double，0.2.根据机器人运动的平移分量指定里程计平移估计中的预期噪声

alpha4：double，0.2.根据机器人运动的旋转分量指定里程计平移估计中的预期噪声

alpha5：double，0.2.平移相关噪声参数，尽在模型为omni时使用

odom_grame_id：string，odom。用于里程计的框架

base_frame_id：string，base_footprint，用于机器人底座的框架。

global_frame_id：string，map。定位系统发布的坐标系的名称

tf_broadcast：bool，true。将此设置为false以防止AMCL发布全局帧和里程计帧之间的变换。

如何从配置文件中设置robot初始位置

当你的机器人开始工作时，必需要指出其初始位置。在没有rviz的时候也能定位需要以下内容。

练习3.2

在本练习中学习在启动期间设置机器人的初始姿态。

1、关闭之前的启动程序

2、重新按下relaunch图标，把机器人置于本地位置。

3、使用之前的配置启动rviz和localization_server

4、执行2D姿态估计以初始定位机器人

5、现在去启动rviz程序的终端，可以看到哪里出现一些坐标，这些事地图最初放置机器人的坐标，记录它们

1、复制你的amcl_config.yaml文件，调用复制的 amcl_config_initialized.yaml

2、将允许初始化的参数添加到新的配置文件中，并设置从rviz获得的正确值。

    set_initial_pose: true
    initial_pose: 
        x: -4.44264
        y: 2.32243
        yaw: 0.328028

1、修改localization.launch.py文件以启动新的配置文件

2、重新编译

3、启动localization_server和rviz。机器人应位于指定位置。

注释

在开始练习前终止每一个节点。在包中更改任何文件后都需要再次colcon build一下。

amcl_config_initialized.yaml的参考答案

amcl:
  ros__parameters:
    use_sim_time: True
    alpha1: 0.2
    alpha2: 0.2
    alpha3: 0.2
    alpha4: 0.2
    alpha5: 0.2
    base_frame_id: "base_footprint"
    beam_skip_distance: 0.5
    beam_skip_error_threshold: 0.9
    beam_skip_threshold: 0.3
    do_beamskip: false
    global_frame_id: "map"
    lambda_short: 0.1
    laser_likelihood_max_dist: 2.0
    laser_max_range: 100.0
    laser_min_range: -1.0
    laser_model_type: "likelihood_field"
    max_beams: 60
    max_particles: 8000
    min_particles: 200
    odom_frame_id: "odom"
    pf_err: 0.05
    pf_z: 0.99
    recovery_alpha_fast: 0.0
    recovery_alpha_slow: 0.0
    resample_interval: 1
    robot_model_type: "differential"
    save_pose_rate: 0.5
    sigma_hit: 0.2
    tf_broadcast: true
    transform_tolerance: 1.0
    update_min_a: 0.2
    update_min_d: 0.25
    z_hit: 0.5
    z_max: 0.05
    z_rand: 0.5
    z_short: 0.05
    set_initial_pose: true
    initial_pose: 
        x: -4.44264
        y: 2.32243
        yaw: 0.328028

如何用命令行设置机器人的初始位置

AMCLnode提供了一个topic，可以将所需要的机器人初始姿态发布到该主题，主题名为/initialpose

练习3.3

在本练习中，学习如何从终端设置机器人的初始位置。

（a）通过发布点从RVIZ获取点坐标

（b）通过以下命令将这些坐标发布到topic/initipose

在终端1中输入

ros2 topic pub -1 /initialpose geometry_msgs/msg/PoseWithCovarianceStamped "{header: {stamp: {sec: 0}, frame_id: 'map'}, pose: {pose: {position: {x: 0.2, y: 0.0, z: 0.0}, orientation: {w: 1.0}}}}"

注释

请记住你可以使用如下格式在终端中对任何话题发布值

ros2 topics pub   

如何以编程的方式设置机器人的初始位置

如果你想构建自主机器人，你可能不希望每次都人为初始化机器人。为了防止这种情况，你可以创建一个ROS程序，再/initialpose主题中自行发布初始姿势。

练习3.4

在本联系中，将从脚本设置机器人的初始位置。

1、再localizer_server包中创建一个新的ROS2程序叫做initial_pose_pub.py

2、此节点必须有订阅者才能获取/clicked_point主题的数据

3、它还必须有一个发布者，讲X和Y坐标发布到/initial_pose主题。

4、创建一个launch文件叫做init_robot.launch.py，来启动这个节点。

5、启动上述节点并使用Publish Point发布一个点从RVIZ

本联系是从交互式屏幕向AMCL系统提供初始姿态的示例，也可以调整成其他接口，但结构是相同的。

注释

记得将Python脚本放在localization_server/localization_server中

打开包setup.py，在entry_points字段的console_scripts括号内添加以下行

entry_points={
        'console_scripts': [
                'initial_pose_pub = .:main',
        ],
},

在练习中，需要用包名替换，文件名替换

通过colcon build编译包

colcon build --packages-select localization_server

initial_pose_pub.py参考代码

import sys
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import PoseWithCovarianceStamped
from geometry_msgs.msg import PointStamped


class Publisher(Node):

    def __init__(self):
        super().__init__('initial_pose_pub_node')
        self.publisher_ = self.create_publisher(PoseWithCovarianceStamped, '/initialpose', 1)
        self.subscriber_ = self.create_subscription(PointStamped, '/clicked_point', self.callback, 1)
        self.subscriber_  # prevent unused variable warning
    
    def callback(self, msg):
        self.get_logger().info('Recieved Data:\n X : %f \n Y : %f \n Z : %f' % (msg.point.x, msg.point.y, msg.point.z))
        self.publish(msg.point.x, msg.point.y)

    def publish(self,x,y):
        msg = PoseWithCovarianceStamped()
        msg.header.frame_id = '/map'
        msg.pose.pose.position.x = x
        msg.pose.pose.position.y = y
        self.get_logger().info('Publishing  Initial Position  \n X= %f \n Y= %f '% (msg.pose.pose.position.x, msg.pose.pose.position.y))
        self.publisher_.publish(msg)

def main(args=None):
    rclpy.init(args=args)
    publisher = Publisher()
    rclpy.spin(publisher)
    publisher.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

init_robot.launch.py参考代码

from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        Node(
            package='localization_server',
            executable='initial_pose_pub',
            output='screen'),
    ])

全球定位

当人和机器人都不知道机器人在地图上的位置时，使用全局定位。然后机器人必须尝试定位自己。通过全局定位，机器人将尝试在地图上识别其位置。

要使用全局本地化，需要两个步骤：

在贴图行分布过滤器的所有粒子，此步骤通过调用名为/reinitialize_globlal_localization的服务实现。

移动机器人，直到它检测到它所在的位置。这一步可以通过在避开障碍物的同时随机移动机器人来实现。如果环境简单，也可也通过使机器人在其位置旋转来实现。

练习3.5

在本练习中，你将看到reinitialize_global_localization服务

（a）打开配置文件amcl_config并更新粒子的值

max_particles:80000,min_particles:2000

记得重新编译

（b）运行localization

（c）启动rviz并天机显示以可视化ParticleCloud

（d）给出2D_pose_estimate并观察机器人粒子

（e）现在调用reinitialize_global_localization服务并观察粒子的扩散

在终端中运行

ros2 service call /reinitialize_global_localization std_srvs/srv/Empty

（e）现在，使用键盘移动机器人，看看粒子使如何开始累积的。在某一点上，粒子应该全部位于机器人的位置。

注释

首先注意粒子的数量增加了。进行全局本地化时这是必要的。由于需要在地图上散布粒子，因此需要更多的粒子。

通过调用该服务，请注意粒子沿着地图分散，机器人已随机定位。当你不知道机器人在环境中的估计位置时，这将有所帮助。通过这项服务，机器人可以在环境中定位，并尝试借助传感器数据进行定位。当它获得更多数据时，机器人将尝试定位自己。当它定位时，所有粒子都会聚集在机器人周围。

将在下一单元的仿真中看到这一点。