路威实验指导书

路威实验指导书

第一章 硬件的认识

1 硬件框架

jujonrobot ROS机器人硬件采用Jetson NANO和 STM32 驱动板组成双系统通信框架，具体组成参考下图：

2 控制器

2.1自研驱动板

底层驱动板为巨匠机器人自主研发的机器人驱动板，跟上层主控采用 ROSSERIAL 协议通信， 整体通信协议基于标准 ROSSERIAL 协议，调用库函数 API，用户可通过自定义消息来实现数据通信。驱动板 MCU 采用 STM32F103RCT6 芯片，提供四路电机控制，提供四路编码器支持，提供四路舵机控制，提供2路5V电源支持，提供9轴IMU支持，提供蜂鸣器支持，可外接OLED显示器，可外接蓝牙，提供两路串口输出还引出芯片富余 IO 口可供用户二次开发使用。

​

2.2 Jetson NANO

​ NVIDIA® Jetson Nano™ 开发者套件是一种功能强大的小型计算机，您可借助其并行运行多个神经网络，从而实现图像分类、目标检测、分割和语音处理等应用。该平台易于使 用，可完成所有工作，且运行功率仅为 5 瓦。

​ Jetson Nano 模组仅有 70 x 45 毫米，是体积非常小巧的 Jetson 设备。 为多个行业（从智慧城市到机器人）的边缘设备部署 AI 时，此生产就绪型模组系统 (SOM) 可以提供强大支持。

3 执行器

3.1 直流电机

​ 路威采用了4个12V直流供电减速电机，电机减速比为90，电机额定转速是：48X150=7200rpm,电机转一周，编码器旋转一周产生11个脉冲。过减速箱后，输出轴转1圈产生的脉冲数是:11X150=1650个脉冲。也就是说目前的轮子转一周产生1650个脉冲。

​ 轮子转一周，机器人行进里程是 0.484米，机器人的额定最大速度（米/秒）=(额定速度rpm/减速比)/60轮子的周长，S=7200/90/600.484=0.6453米/秒，如何通过编码器的数据反映出实际的行进里程和速度呢？

​ 轮子转一周产生1650个脉冲，4驱的驱动器内部做了4倍频，所以数据接口上传的数据上，变化1650X4=6600个脉冲值，标示轮子转了一周。产生了0.484米的位移。

4 传感器

4.1 激光雷达

​ 思岚 A1 雷达，部分 ROS 机器人搭载思岚 A1 激光雷达,可实现 12M 测距范围, 8000 次/ 秒测量频率,适合室内 SLAM 建图导航。

采用思岚 A1 激光雷达

测距范围	米	待定	0.15-12	待定	基于白色高反光物体检测
扫描角度	度	不适用	0-360	不适用
测距分辨率	毫米	不适用	<0.5 <实际距离*1%	不适用	测量物体在1.5米以内 全部量程范围内
角度分辨率	度	不适用	≤1	不适用	5.5hz扫描时
单次测距时间	毫秒	不适用	0.5	不适用
测量频率	赫兹	2000	≥4000	8000
扫描频率	赫兹	1	5.5	10	扫描一周的频率 典型值为一次扫描恰好400个采样点的情况

4.2 结构光相机

​ 乐视体感摄像头，RGBD 深度相机，包含普通摄像头的功能和能获取深度数据，二合一这对于 ROS 开发来说非常友好。另外摄像模组和奥比中光 Astra 模组驱动兼容，ROS 开发资料完全可以利用奥比中光 Astra ROS 驱动包。

乐视体感摄像头，奥比中光astra pro。

产品名称	Astra Pro/LeMTC-520
深度	3D技术	ORBBEC单目结构光
工作范围	0.6-8m
精度	1m: ±3mm
视场角(FOV)	H58.4° * V45.7°
分辨率@帧率	1280x1024@7fps 640x480@30fps 320x240@30fps 160x120@30fps
深度处理芯片	MX400
近距离保护	支持
RGB	视场角	H66.10° x V40.2°
分辨率@帧率	1280x720@7fps 640x480@30fps 320x240@30fps
UVC	UVC
其他	支持操作系统	Android/Linux/Windows
工作环境	室内
数据接口	USB2.0
尺寸	164x43.3x56.5mm
麦克风	双声道立体声
功耗	<2.5w
工作温度	10°C-40°C
安全性	Class1 激光

4.3 惯性测量单元(IMU)

九轴模块(三轴陀螺仪+三轴加速度+三轴磁场)

型号	GY-85
使用芯片	ITG3205+ADXL345+HMC5883L
供电电源	3-5v
通信方式	IIC通信协议(完全兼容3-5v系统,含LLC电路)
尺寸	2.2cm*1.7cm

4.4 电机编码器

电机编码器和电机绑定，内置在电机中，用来测量电机转速。

5 能源

5.1 锂电池

​

• 12V聚合物锂电池10000MAH

• 外型尺寸: 2063100（mm）

• 充电电流: 0.5~5（A）

• 支持过充保护

• 支持过放保护

• 支持过压保护

• 支持过流保护

​

第二章 SSH连接路威

1 SSH 简介

SSH（Secure Shell）是一套协议标准，可以用来实现两台机器之间的安全登录以及安全的数据传送，其保证数据安全的原理是非对称加密。

传统的对称加密使用的是一套秘钥，数据的加密以及解密用的都是这一套秘钥，可想而知所有的客户端以及服务端都需要保存这套秘钥，泄露的风险很高，而一旦秘钥便泄露便保证不了数据安全。

非对称加密解决的就是这个问题，它包含两套秘钥 - 公钥以及 私钥，其中公钥用来加密，私钥用来解密，并且通过公钥计算不出私钥，因此私钥谨慎保存在服务端，而公钥可以随便传递，即使泄露也无风险。

保证SSH安全性的方法，简单来说就是客户端和服务端各自生成一套私钥和公钥，并且互相交换公钥，这样每一条发出的数据都可以用对方的公钥来加密，对方收到后再用自己的私钥来解密。

通过SSH工具可以使用PC机直接连接打开一个路威套件上Ubuntu的终端，可在此终端操作路威套件。

2 路威连接WI-FI

根据不同的软件版本

在路威套件上若用户名为 nvidia 则密码为 nvidia

若用户名为 jubot 则密码为 jubot

连接WIFI

在进入系统以后，点击右上角的WIFI图标。

在more networks中选择你要连接的WI-FI。

在弹出的对话框中输入WIFI的密码。

使用飞鼠右键桌面，单击Open Terminal 在打开的终端中输入 ifconfig

3 使用SSH工具连接路威套件

推荐使用 Xshell 和 Xmanager 工具。 Xmanager 可以传输部分GUI信息。

第一次使用SSH连接时，首先要使用飞鼠在路威套件上连接上WI-FI ，并且确保所使用的PC与路威套件处在同一个局域网内。

安装并且打开 Xsheel

在此处点击新建，并且输入在实验4.4.2中查看到的IP，在弹出的对话框中输入用户名和密码

按照以上步骤通过SSH连接到路威套件

此时可以使用ls命令查看当前目录

此章节帮助理解SSH基本知识

学会查看linux ip地址

学会使用SSH工具连接路威套件

第三章 路威控制功能的实现

1 使用键盘控制路威运动

检查机器人底盘能否正常运动

打开新终端，输入

roslaunch jujonrobot bringup2.launch

进行底盘的连接

可以打开新终端输入rqt_graph来查看当前的节点以及消息之间的关系，得到下图：

（图中椭圆内代表节点，方框内代表节点之家传递的消息）

下面打开一个新终端，输入

rosrun teleop_twist_keyboard teleop_twist_keyboard.py

打开键盘控制节点：

这时候就可以按照终端上的指示通过键盘上的 u i o j k l m ，. 这些按键来控制小车移动了。

其中i是向前;k是停车; ,是向后;

这时候可以再

使用 rqt_graph 查看会得到以下关系：

可以看出和上面的区别在于/teleop_twist_keyboard 节点发送给了/arduino_serial_node 这一节点一个名为/cmd_vel

的消息，说明我们的按键指令实际上就是在/teleop_twist_keyboard 节点内通过翻译转化成为/arduino_serial_node 驱动节点可以识别的/cmd_vel 消息，那么对于手柄来说我们可以大胆假设，可能也是通过一个节点将指令转换成/cmd_vel 消息发送给/zoo_driver 驱动节点。

2 使用手柄控制路威套件

待发布 敬请期待~

第四章 路威上查看雷达数据

1 激光雷达的测距原理

在激光雷达技术领域中，目前主要通过三角测距法与 TOF 方法来进行测距。在路威套件中我们所用的思岚激光雷达所采用测距法的是三角测距法，以下简单介绍：

如图所示，由发射装置和接收装置所获得的时间差，以及两装置之间的距离就可以计算出障碍点到激光雷达的具体距离了。

激光雷达工作时会先在当前位置发出激光并接收反射光束，解析得到距离信息，而后激光发射器会转过一个角度分辨率对应的角度再次重复这个过程。限于物理及机械方面的限制，激光雷达通常会有一部分“盲区”。使用激光雷达返回的数据通常可以描绘出一幅极坐标图，极点位于雷达扫描中心，0-360°整周圆由扫描区域及盲区组成。

在扫描区域中激光雷达在每个角度分辨率对应位置解析出的距离值会被依次连接起来，这样，通过极坐标表示就能非常直观地看到周围物体的轮廓，激光雷达扫描范围如下图所示:

目前，移动机器人的研究中已经大量使用激光雷达辅助机器人的避障导航，通常激光雷达都会提供ROS 驱动，将消息/LaserScan 发布到话题/Scan 中。

2 LaserScan 消息解析

在ROS上正确地发布从传感器获取的数据对导航功能包集的安全运行很重要。如果导航功能包集无法从机器人的传感器接收到任何信息，那么它就会盲目行事，最有可能的是发生碰撞。

有许多传感器可用于为导航功能包集提供信息：激光、摄像头、声纳、红外线、碰撞传感器等等。然而，目前导航功能包集只接受使用sensor_msgs/LaserScan或sensor_msgs/PointCloud消息类型发布的传感器数据。

此处以LaserScan 为例，LaserScan的消息结构如下所示

(此内容文件包含在/opt/ros/melodic/include/sensor_msgs/LaserScan.h)

其中：

其中：
angle-min：扫描起始角度
angle-max：扫描终止角度
angle-increment：两次测量的角度差
time-increment： 两次测量的时间间隔
scan-time ：完成一次扫描的时间
range-min：测距的最小值
range-max：测距的最大值
ranges：转一周的测量数据，一共 360 个
intensities：随影的 ranges 数据的置信度，同样 360 个

3 雷达的使用

在我们实际使用激光雷达的时候，激光雷达一般放置在机器人一个平面最上方

首先打开路威的激光雷达数据：

开启终端输入：

roslaunch jujonrobot view_rplidar.launch

打开激光雷达数据

得到路威周围的障碍信息。

观察 rviz：

点击 topic 后的发现新增一个下拉菜单，选择/scan ,得到以下结果：

可以得到 /scan得到的结果，后续我们实现 slam，避障等功能的时候，都会使用这些数据。

第五章 线速度与角速度的标定

1 线速度的标定

通过SSH连接到路威套件，关于如何通过SSH连接到路威套件，详细内容请参考《路威实验指导书》SSH连接章节。

连接成功后如下图所示：

运行机器人启动的launch文件

roslaunch jujonrobot bringup.launch

启动成功后如下图所示：

之后运行线速度标定的python文件，输入

rosrun jujonrobot_nav calibrate_linear.py

得到如下结果：

根据提示我们可知，此时需要启动rqt_reconfigure

打开一个新的终端，输入

rosrun rqt_reconfigure rqt_reconfigure

(若使用的SSH工具不能传输GUI信息，请使用飞鼠在路威的屏幕上操作)

得到如下结果：

参数解释：

test_distance : 需要让路威套件要移动的距离

speed ：测试时路威套件的速度

tolerance：可以容忍的误差，这个值越大则误差越大，但是如果很小，超过电机的精准范围，则电机有可能发生反向转动。

odom_linear_scale_correction : 标定的数值，即在此处矫正路威的linear_scale。调整此数值，使得路威能够按照test_distance设定的距离进行移动。

之后在你使用的launch 文件中修改如下图所示参数

将此处value值修改成 odom_linear_scale_correctio 调整准确的数值

2 角速度的标定

角速度的标定与线速度的标定逻辑上是一样的只不过启动的标定python文件不同

rosrun rqt_reconfigure rqt_reconfigure

rosrun jujonrobot_nav  calibrate_angular.py

rosrun rqt_reconfigure rqt_reconfigure

在对应的launch文件里面修改对应参数

<param name="angular_scale" value="1.0" />

3 代码分析

calibrate_angular.py为标定角速度的文件，calibrate_linear.py为标定线速度的文件 calibrate_angular.py如下所示：

#!/usr/bin/env python

""" calibrate_angular.py - Version 1.1 2013-12-20
    Rotate the robot 360 degrees to check the odometry parameters of the base controller.
    Created for the Pi Robot Project: http://www.pirobot.org
    Copyright (c) 2012 Patrick Goebel.  All rights reserved.
    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.5

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details at:

    http://www.gnu.org/licenses/gpl.html

"""

import rospy
from geometry_msgs.msg import Twist, Quaternion
from nav_msgs.msg import Odometry
from dynamic_reconfigure.server import Server
import dynamic_reconfigure.client
from calibration.cfg import CalibrateAngularConfig
import tf
from math import radians, copysign
import PyKDL
from math import pi

def quat_to_angle(quat):
    rot = PyKDL.Rotation.Quaternion(quat.x, quat.y, quat.z, quat.w)
    return rot.GetRPY()[2]

def normalize_angle(angle):
    res = angle
    while res > pi:
        res -= 2.0 * pi
    while res < -pi:
        res += 2.0 * pi
    return res

class CalibrateAngular():
    def __init__(self):
        # Give the node a name
        rospy.init_node('calibrate_angular', anonymous=False)

        # Set rospy to execute a shutdown function when terminating the script
        rospy.on_shutdown(self.shutdown)

        # How fast will we check the odometry values?
        self.rate = rospy.get_param('~rate', 20)
        r = rospy.Rate(self.rate)

        # The test angle is 360 degrees
        self.test_angle = radians(rospy.get_param('~test_angle', 360.0))

        self.speed = rospy.get_param('~speed', 0.5) # radians per second
        self.tolerance = radians(rospy.get_param('tolerance', 1)) # degrees converted to radians
        self.odom_angular_scale_correction = rospy.get_param('~odom_angular_scale_correction', 1.0)
        self.start_test = rospy.get_param('~start_test', True)

        # Publisher to control the robot's speed
        self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5)

        # Fire up the dynamic_reconfigure server
        dyn_server = Server(CalibrateAngularConfig, self.dynamic_reconfigure_callback)

        # Connect to the dynamic_reconfigure server
        dyn_client = dynamic_reconfigure.client.Client("calibrate_angular", timeout=60)

        # The base frame is usually base_link or base_footprint
        self.base_frame = rospy.get_param('~base_frame', '/base_link')

        # The odom frame is usually just /odom
        self.odom_frame = rospy.get_param('~odom_frame', '/odom')

        # Initialize the tf listener
        self.tf_listener = tf.TransformListener()

        # Give tf some time to fill its buffer
        rospy.sleep(2)

        # Make sure we see the odom and base frames
        self.tf_listener.waitForTransform(self.odom_frame, self.base_frame, rospy.Time(), rospy.Duration(60.0))

        rospy.loginfo("Bring up rqt_reconfigure to control the test.")

        reverse = 1

        while not rospy.is_shutdown():
            if self.start_test:
                # Get the current rotation angle from tf
                self.odom_angle = self.get_odom_angle()

                last_angle = self.odom_angle
                turn_angle = 0
                self.test_angle *= reverse
                error = self.test_angle - turn_angle

                # Alternate directions between tests
                reverse = -reverse

                while abs(error) > self.tolerance and self.start_test:
                    if rospy.is_shutdown():
                        return

                    # Rotate the robot to reduce the error
                    move_cmd = Twist()
                    move_cmd.angular.z = copysign(self.speed, error)
                    self.cmd_vel.publish(move_cmd)
                    r.sleep()

                    # Compute how far we have gone since the last measurement
                    delta_angle = self.odom_angular_scale_correction * normalize_angle(self.odom_angle - last_angle)

                    # Add to our total angle so far
                    turn_angle += delta_angle

                    # Compute the new error
                    error = self.test_angle - turn_angle

                    # Store the current angle for the next comparison
                    last_angle = self.odom_angle

                # Stop the robot
                self.cmd_vel.publish(Twist())

                # Update the status flag
                self.start_test = False
                params = {'start_test': False}
                dyn_client.update_configuration(params)

            rospy.sleep(0.5)

        # Stop the robot
        self.cmd_vel.publish(Twist())

    def get_odom_angle(self):
        # Get the current transform between the odom and base frames
        try:
            (trans, rot)  = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
        except (tf.Exception, tf.ConnectivityException, tf.LookupException):
            rospy.loginfo("TF Exception")
            return

        # Convert the rotation from a quaternion to an Euler angle
        return quat_to_angle(Quaternion(*rot))

    def dynamic_reconfigure_callback(self, config, level):
        self.test_angle =  radians(config['test_angle'])
        self.speed = config['speed']
        self.tolerance = radians(config['tolerance'])
        self.odom_angular_scale_correction = config['odom_angular_scale_correction']
        self.start_test = config['start_test']

        return config

    def shutdown(self):
        # Always stop the robot when shutting down the node
        rospy.loginfo("Stopping the robot...")
        self.cmd_vel.publish(Twist())
        rospy.sleep(1)

if __name__ == '__main__':
    try:
        CalibrateAngular()
    except:
        rospy.loginfo("Calibration terminated.")

calibrate_linear.py文件如下所示：

#!/usr/bin/env python

""" calibrate_linear.py - Version 1.1 2013-12-20
    Move the robot 1.0 meter to check on the PID parameters of the base controller.
    Created for the Pi Robot Project: http://www.pirobot.org
    Copyright (c) 2012 Patrick Goebel.  All rights reserved.
    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.5

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details at:

    http://www.gnu.org/licenses/gpl.html

"""

import rospy
from geometry_msgs.msg import Twist, Point
from math import copysign, sqrt, pow
from dynamic_reconfigure.server import Server
import dynamic_reconfigure.client
from calibration.cfg import CalibrateLinearConfig
import tf

class CalibrateLinear():
    def __init__(self):
        # Give the node a name
        rospy.init_node('calibrate_linear', anonymous=False)

        # Set rospy to execute a shutdown function when terminating the script
        rospy.on_shutdown(self.shutdown)

        # How fast will we check the odometry values?
        self.rate = rospy.get_param('~rate', 20)
        r = rospy.Rate(self.rate)

        # Set the distance to travel
        self.test_distance = rospy.get_param('~test_distance', 1.0) # meters
        self.speed = rospy.get_param('~speed', 0.15) # meters per second
        self.tolerance = rospy.get_param('~tolerance', 0.01) # meters
        self.odom_linear_scale_correction = rospy.get_param('~odom_linear_scale_correction', 1.0)
        self.start_test = rospy.get_param('~start_test', True)

        # Publisher to control the robot's speed
        self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5)

        # Fire up the dynamic_reconfigure server
        dyn_server = Server(CalibrateLinearConfig, self.dynamic_reconfigure_callback)

        # Connect to the dynamic_reconfigure server
        dyn_client = dynamic_reconfigure.client.Client("calibrate_linear", timeout=60)

        # The base frame is base_footprint for the TurtleBot but base_link for Pi Robot
        self.base_frame = rospy.get_param('~base_frame', '/base_link')
# The odom frame is usually just /odom
        self.odom_frame = rospy.get_param('~odom_frame', '/odom')

        # Initialize the tf listener
        self.tf_listener = tf.TransformListener()

        # Give tf some time to fill its buffer
        rospy.sleep(2)

        # Make sure we see the odom and base frames
        self.tf_listener.waitForTransform(self.odom_frame, self.base_frame, rospy.Time(), rospy.Duration(60.0))

        rospy.loginfo("Bring up rqt_reconfigure to control the test.")

        self.position = Point()

        # Get the starting position from the tf transform between the odom and base frames
        self.position = self.get_position()

        x_start = self.position.x
        y_start = self.position.y

        move_cmd = Twist()

        while not rospy.is_shutdown():
            # Stop the robot by default
            move_cmd = Twist()

            if self.start_test:
                # Get the current position from the tf transform between the odom and base frames
                self.position = self.get_position()

                # Compute the Euclidean distance from the target point
                distance = sqrt(pow((self.position.x - x_start), 2) +
                                pow((self.position.y - y_start), 2))

                # Correct the estimated distance by the correction factor
                distance *= self.odom_linear_scale_correction

                # How close are we?
                error =  distance - self.test_distance

                # Are we close enough?
                if not self.start_test or abs(error) <  self.tolerance:
                    self.start_test = False
                    params = {'start_test': False}
                    rospy.loginfo(params)
                    dyn_client.update_configuration(params)
                else:
                    # If not, move in the appropriate direction
                    move_cmd.linear.x = copysign(self.speed, -1 * error)
            else:
                self.position = self.get_position()
                x_start = self.position.x
                y_start = self.position.y

            self.cmd_vel.publish(move_cmd)
            r.sleep()

        # Stop the robot
        self.cmd_vel.publish(Twist())

    def dynamic_reconfigure_callback(self, config, level):
        self.test_distance = config['test_distance']
        self.speed = config['speed']
        self.tolerance = config['tolerance']
        self.odom_linear_scale_correction = config['odom_linear_scale_correction']
        self.start_test = config['start_test']

        return config

    def get_position(self):
        # Get the current transform between the odom and base frames
        try:
            (trans, rot)  = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
        except (tf.Exception, tf.ConnectivityException, tf.LookupException):
            rospy.loginfo("TF Exception")
            return

        return Point(*trans)

    def shutdown(self):
        # Always stop the robot when shutting down the node
        rospy.loginfo("Stopping the robot...")
        self.cmd_vel.publish(Twist())
        rospy.sleep(1)

if __name__ == '__main__':
    try:
        CalibrateLinear()
        rospy.spin()
    except:
        rospy.loginfo("Calibration terminated.")

第六章 SLAM

1 栅格地图的概念

占据栅格地图是一种在 SLAM 地图中相较而言创建和维护较简单的地图表达形式，使用栅格地图更容易实现导航功能。

占据栅格地图在目前二维 SLAM 中应用较为广泛，其基本原理是将机器人所在的空间划分为若干个大小相同且相互独立的栅格块，如图所示。

而对于每一个栅格块，我们给予（0，1）之间的数来表示在这个栅格内有多大的概率存在障碍。

对栅格的划分越细，其地图的精度自然也就越高。

但同时栅格越多，计算机的计算量也就越大，这是栅格地图主要的缺点，栅格地图大多使用在空间面积较小的环境中。

栅格地图也是对 SLAM 建图支持度相当高的一种地图模式。

2 建图原理

2D 激光 SLAM 的建图的输入一般是里程计数据、IMU 数据、2D 激光雷达数据，得到的输出为覆盖栅格地图以及机器人的运动轨迹。目前的算法主要由两个部分执行：一部分通过数据融合、特征处理等方式将提取得的传感器数据转化为可用的数据模型；另一部分则通过概率算法等手段来对这些模型进行处理，生成环境地图。但一般来说这种直接根据传感器数据得出的地图会在某些情况下不可取。因为但凡传感器都是有误差的，而在建图的时候上一帧数据对下一帧的处理影响非常大，这就导致一旦出现误差就会在建图过程中一直累积下去。如果环境非常大，到最后就会导致所获得的的地图完全不可用。因此，回环检测是 SLAM 建图算法中非常重要的一部分。回环检测的基本原理就是将获得的一帧数据以及自己的位置信息与之前某一帧数据或是已计算出的地图进行匹配，找到能够成功匹配的帧，建立位姿约束关系，并以此约束关系为基准再去调整其他的数据，从而起到减小累积误差的效果。

在目前的算法中，回环检测也不仅仅是只有帧间配对，scan-to-map 以及map-to-map 才是用的比较多的方式。他们分别表示用当前帧数据去匹配之前某些帧建立的地图和用当前某些帧建立的地图去匹配之前建立的地图。相较而言 map-to-map 的匹配方式更加准确但计算量更大，同时匹配难度也更大。所以对于同的应用场景来说，选择哪种算法应当根据当前场景的特点来确定。路威上使用的 gmapping 包是基于滤波 SLAM 框架的常用开源算法，它基于 RBpf 粒子滤波算法，即将定位和建图分离，先进行定位再建图，Gmapping 在 RBpf 算法上做了两个主要的改进：改进提议分布和选择性重采样。

但是它只适用于小环境下的建图定位，计算量小且精度较高。因为这种算法有效的利用了里程计的数据，所以并没有使用回环检测。在室内小环境下，即便相比目前开源算法中效果最好的Cartographer 精度也没有下降太多，反而计算量由于没有回环检测而大大减小。不过随着场景增大，在没有回环检测的情况下回环闭合时就可能会导致地图错位，故路威不适用于特别大的环境场景。

3 SLAM

本节内容主要介绍导航的完整性实现，旨在掌握机器人导航的基本流程，该章涉及的主要内容如下:

• SLAM建图(选用较为常见的gmapping)

• 地图服务(可以保存和重现地图)

• 机器人定位

• 路径规划

• 上述流程介绍完毕，还会对功能进一步集成实现探索式的SLAM建图。

准备工作

请先安装相关的ROS功能包:

• 安装 gmapping 包(用于构建地图):sudo apt install ros-<ROS版本>-gmapping

• 安装地图服务包(用于保存与读取地图):sudo apt install ros-<ROS版本>-map-server

• 安装 navigation 包(用于定位以及路径规划):sudo apt install ros-<ROS版本>-navigation

新建功能包，并导入依赖: gmapping map_server amcl move_base

SLAM算法有多种，当前我们选用gmapping，后续会再介绍其他几种常用的SLAM实现。

3.1 gmapping 简介

gmapping 是ROS开源社区中较为常用且比较成熟的SLAM算法之一，gmapping可以根据移动机器人里程计数据和激光雷达数据来绘制二维的栅格地图，对应的，gmapping对硬件也有一定的要求:

• 该移动机器人可以发布里程计消息

• 机器人需要发布雷达消息(该消息可以通过水平固定安装的雷达发布，或者也可以将深度相机消息转换成雷达消息)

关于里程计与雷达数据，仿真环境中可以正常获取的，不再赘述，栅格地图如案例所示。

gmapping 安装前面也有介绍，命令如下:

sudo apt install ros-<ROS版本>-gmapping

3.2 gmapping节点说明

gmapping 功能包中的核心节点是:slam_gmapping。为了方便调用，需要先了解该节点订阅的话题、发布的话题、服务以及相关参数。

3.2.1订阅的Topic

tf (tf/tfMessage)

• 用于雷达、底盘与里程计之间的坐标变换消息。

scan(sensor_msgs/LaserScan)

• SLAM所需的雷达信息。

3.2.2 发布的Topic

map_metadata(nav_msgs/MapMetaData)

• 地图元数据，包括地图的宽度、高度、分辨率等，该消息会固定更新。

map(nav_msgs/OccupancyGrid)

• 地图栅格数据，一般会在rviz中以图形化的方式显示。

~entropy(std_msgs/Float64)

• 机器人姿态分布熵估计(值越大，不确定性越大)。

3.2.3 服务

dynamic_map(nav_msgs/GetMap)

• 用于获取地图数据。

3.2.4 参数

~base_frame(string, default:"base_link")

• 机器人基坐标系。

~map_frame(string, default:"map")

• 地图坐标系。

~odom_frame(string, default:"odom")

• 里程计坐标系。

~map_update_interval(float, default: 5.0)

• 地图更新频率，根据指定的值设计更新间隔。

~maxUrange(float, default: 80.0)

• 激光探测的最大可用范围(超出此阈值，被截断)。

~maxRange(float)

• 激光探测的最大范围。

.... 参数较多，上述是几个较为常用的参数，其他参数介绍可参考官网。

3.2.5 所需的坐标变换

雷达坐标系→基坐标系

• 一般由 robot_state_publisher 或 static_transform_publisher 发布。

基坐标系→里程计坐标系

• 一般由里程计节点发布。

3.2.6 发布的坐标变换

地图坐标系→里程计坐标系

• 地图到里程计坐标系之间的变换。

3.3 gmapping 使用

3.3.1编写 gmapping 节点相关launch文件

launch文件编写可以参考 github 的 PR2 机器人gmapping demo的 launch文件：

https://github.com/ros-perception/slam_gmapping/blob/melodic-devel/gmapping/launch/slam_gmapping_pr2.launch

复制并修改如下:

<launch>
  <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
    <param name="base_frame" value="/base_link" />
    <param name="odom_frame" value="/odom" />
    <param name="map_update_interval" value="15.0"/>
    <param name="maxUrange" value="5.0"/>
    <param name="minRange" value="-0.5"/>
    <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="100"/>
    <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.7"/>
    <param name="angularUpdate" value="0.7"/>
    <param name="temporalUpdate" value="-0.5"/>
    <param name="resampleThreshold" value="0.5"/>
    <param name="particles" value="50"/>
    <param name="xmin" value="-50.0"/>
    <param name="ymin" value="-50.0"/>
    <param name="xmax" value="50.0"/>
    <param name="ymax" value="50.0"/>
    <param name="delta" value="0.05"/>
    <param name="llsamplerange" value="0.05"/>
    <param name="llsamplestep" value="0.05"/>
    <param name="lasamplerange" value="0.005"/>
    <param name="lasamplestep" value="0.005"/>
    <param name="transform_publish_period" value="0.1"/>
  </node>
</launch>

关键代码解释：

<param name="base_frame" value="base_footprint"/><!--底盘坐标系-->
<param name="odom_frame" value="odom"/> <!--里程计坐标系-->

这些都是 gmapping 中的一些参数，下面将针对其中的部分参数作简单介绍：

map_update_interval:是 gmapping 过程中每隔几秒更新一次地图，该值变小的话表明更新地图的频率

加快，会增加消耗更多当前系统的 CPU 计算资源。同时地图更新也受 scanmach 的影响，如果 scanmatch

没有成功的话，不会更新地图。

maxUrange**：**是雷达可用的最大有效测距值莫的，maxRange 。

是雷达的理论最大测距值，一般情况下设置 maxUrange < 雷达的现实实际测距值 <= maxRange。

下面这些参数一般不用修改保持默认值即可：

sigma (float, default: 0.05)，endpoint 匹配标准差。

kernelSize (int, default: 1)，用于查找对应的 kernel size。

lstep (float, default: 0.05)，平移优化步长。

astep (float, default: 0.05)，旋转优化步长。

iterations (int, default: 5)，扫描匹配迭代步数。

lsigma (float, default: 0.075)，用于扫描匹配概率的激光标准差。

ogain (float, default: 3.0)，似然估计为平滑重采样影响使用的 gain。

lskip (int, default: 0)，每次扫描跳过的光束数。

minimumScore: 最小匹配得分，这个参数很重要，它决定了对激光的一个置信度，越高说明对激光匹

配算法的要求越高，激光的匹配也越容易失败而转去使用里程计数据，而设的太低又会使地图中出现大量

噪声，所以需要权衡调整。

srr,srt,str,stt 四个参数是运动模型的噪声参数，一般设置为默认值不用修改。

linearUpdate:机器人移动多远距离，进行一次 scanmatch 匹配。

angularUpdate:机器人旋转多少弧度，进行一次 scanmatch 匹配。

temporalUpdate:如果最新扫描处理比更新慢，则处理 1 次扫描，该值为负数时候关闭基于时间的更

新，该参数很重要，需要重点关注。

resampleThreshold:基于重采样门限的 Neff

particles:gmapping 算法中的粒子数，因为 gmapping 使用的是粒子滤波算法，粒子在不断地迭代更新，

所以选取一个合适的粒子数可以让算法在保证比较准确的同时有较高的速度。

xmin,ymin,xmax,ymax:初始化的地图大小。

delta:创建的地图分辨率，默认是 0.05m。

llsamplerange:线速度移动多长距离进行似然估计。

llsamplestep:线速度移动多少步长进行似然估计。

lasamplerange:每转动多少弧度用于似然估计。

lasamplestep:用于似然估计的弧度采样步长是多少。

transform_publish_period:多长时间发布一次 tf 转换（map->odom 转换），单位是秒。

occ_thresh:在 gmapping 过程中占有栅格地图的阈值，只有当前单元格的占有率超过该值才认为是被占有。如果设置该值为 1 的话就会导致任何障碍物都不会认为是占有。

3.3.2执行

1.先临界并启动好路威套件

2.然后再启动地图绘制的 launch 文件:

roslaunch jujonrobot bringup2.launch

启动成功如下图所示

3.启动键盘键盘控制节点，用于控制机器人运动建图:

rosrun teleop_twist_keyboard teleop_twist_keyboard.py

启动成功如下图所示：

打开新终端输入：

roslaunch robot_slam view_mapping.launch

在 Rviz 下查看建图效果：

此时通过键盘或者手柄控制，控制机器人移动完成建图。

3.4 地图服务

上一节我们已经实现通过gmapping的构建地图并在rviz中显示了地图，不过，上一节中地图数据是保存在内存中的，当节点关闭时，数据也会被一并释放，我们需要将栅格地图序列化到的硬盘以持久化存储，后期还要通过反序列化读取硬盘的地图数据再执行后续操作。在ROS中，地图数据的序列化与反序列化可以通过 map_server 功能包实现。

3.4.1 map_server简介

map_server功能包中提供了两个节点: map_saver 和 map_server，前者用于将栅格地图保存到硬盘，后者读取硬盘的栅格地图并以服务的方式提供出去。

map_server安装前面也有介绍，命令如下:

sudo apt install ros-<ROS版本>-map-server

3.4.2 map_server使用之地图保存节点(map_saver)

map_saver节点说明

订阅的topic:

map(nav_msgs/OccupancyGrid)

• 订阅此话题用于生成地图文件。

地图保存launch文件

地图保存的语法比较简单，编写一个launch文件，内容如下:

<launch> 
        <node pkg="map_server" type="map_saver" name="map_saver1" args="-f        /home/robot/catkin_car/src/my_robot/project/jujonrobot/maps/0114test">
        </node>
</launch>

其中 args的参数是指地图的保存路径以及保存的文件名称。

SLAM得到满意的地图后，行该launch文件即可。

或者直接在新终端内通过以下指令保存地图：

roslaunch jujonrobot save_map.launch

地图位置如下：

catkin_car/src/my_robot/project/jujonrobot/maps

第七章 navigation

机器人是如何实现导航的呢？或换言之，机器人是如何从 A 点移动到 B 点呢？ROS 官方为了提供了一张导航功能包集的图示,该图中囊括了 ROS 导航的一些关键技术:

假定我们已经以特定方式配置机器人，导航功能包集将使其可以运动。上图概述了这种配置方式。白色的部分是必须且已实现的组件，灰色的部分是可选且已实现的组件，蓝色的部分是必须为每一个机器人平台创建的组件。

1 技术要点

主要涉及的关键技术有如下五点:

1. 全局地图

2. 自身定位

3. 路径规划

4. 运动控制

5. 环境感知

机器人导航实现与无人驾驶类似，关键技术也是由上述五点组成，只是无人驾驶是基于室外的，而我们当前介绍的机器人导航更多是基于室内的。

1.1 全局地图

在现实生活中，当我们需要实现导航时，会首先参考一张全局性质的地图，根据这张地图来确定自身的位置、目的地的位置，也会根据地图显示来规划一条大致的路线。

对于机器人导航而言，如此这般，在机器人导航中地图是一个重要的组成元素。

要使用地图，首先需要绘制地图。关于地图建模技术不断涌现，这其中有一门称之为 SLAM 的理论脱颖而出:

1. SLAM(simultaneous localization and mapping),也称为CML (Concurrent Mapping and Localization), 即时定位与地图构建，或并发建图与定位。SLAM问题可以描述为: 机器人在未知环境中从一个未知位置开始移动,在移动过程中根据位置估计和地图进行自身定位，同时在自身定位的基础上建造增量式地图，以绘制出外部环境的完全地图。

2. 在 ROS 中，较为常用的 SLAM 实现也比较多，比如: gmapping、hector_slam、cartographer、rgbdslam、ORB_SLAM ....

3. 当然如果要完成 SLAM ，机器人必须要具备感知外界环境的能力，尤其是要具备获取周围环境深度信息的能力。感知的实现需要依赖于传感器，比如: 激光雷达、摄像头、RGB-D摄像头...

4. SLAM 可以用于地图生成，而生成的地图还需要被保存以待后续使用，在 ROS 中保存地图的功能包是 map_server

另外注意: SLAM 虽然是机器人导航的重要技术之一，但是 二者并不等价，确切的讲，SLAM 只是实现地图构建和即时定位。

关于SLAM的详细内容已在第九章中详细解释过，此处不再赘述。

1.2 自身定位

导航开始和导航过程中，机器人都需要确定当前自身的位置，如果在室外，可以使用GPS 。

但在室内、隧道、地下或一些特殊的区域，无法收到 GPS 的信号， GPS 信号被弱化甚至完全不可用，就要使用其他方法来定位，如 SLAM 就可以实现自身定位，除此之外，ROS 中还提供了一个用于定位的功能包: amcl

amcl (adaptiveMonteCarloLocalization)自适应的蒙特卡洛定位,是用于2D移动机器人的概率定位系统。

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

1.3 路径规划

导航就是机器人从A点运动至B点的过程，在这一过程中，机器人需要根据目标位置计算全局运动路线，并且在运动过程中，还需要实时根据出现的一些动态障碍物调整运动路线，直至到达目标点，该过程就称之为路径规划。

ROS 提供了 move_base 包来实现路径规划,该功能包主要由两大规划器组成:

1. 全局路径规划(gloable_planner)

   根据给定的目标点和全局地图实现总体的路径规划，使用 Dijkstra 或 A* 算法进行全局路径规划，计算最优路线，作为全局路线。

2. 本地时时规划(local_planner)

   在实际导航过程中，机器人可能无法按照给定的最优全局路线运行，比如:机器人在运行中，可能会随时出现一些障碍物。

   本地规划的作用就是使用一定算法(Dynamic Window Approaches) 来实现障碍物的规避，并选取当前最优路径以尽量符合全局最优路径

全局路径规划与本地路径规划是相对的，全局路径规划侧重于全局、宏观实现，而本地路径规划侧重与当前、微观实现。

1.4 运动控制

导航功能包集假定它可以通过话题"cmd_vel"发布geometry_msgs/Twist类型的消息，这个消息基于机器人的底盘坐标系，它传递的是运动命令。

这意味着需要有一个节点订阅"cmd_vel"话题， 将该话题上的速度命令转换为电机命令并发送给下位机进行转换。

1.5 环境感知

感知周围环境信息，比如: 摄像头、激光雷达、编码器等，摄像头、激光雷达可以用于感知外界环境的深度信息，编码器可以感知电机的转速信息，进而可以获取速度信息并生成里程计信息。

在导航功能包集中，环境感知也是一重要模块实现，它为其他模块提供了支持。其他模块 SLAM、amcl、move_base 都需要依赖于环境感知。

2 定位

定位是推算机器人自身在全局地图中的位置，SLAM中也包含定位算法实现，但SLAM的定位是用于构建全局地图的，是属于导航开始之前的阶段。

而当前定位是用于导航中，在导航中机器人需要按照设定的路线运动，通过定位可以判断机器人的实际轨迹是否符合预期。

在ROS的导航功能包集navigation中提供了 amcl 功能包，用于实现导航中的机器人定位。

2.1 amcl简介

AMCL(adaptive Monte Carlo Localization) 是用于2D移动机器人的概率定位系统，它实现了自适应（或KLD采样）蒙特卡洛定位方法。

可以根据已有地图使用粒子滤波器推算机器人位置。

amcl已经被集成到了navigation包，navigation安装前面也有介绍，命令如下:

 sudo apt install ros-<ROS版本>-navigation

2.2 amcl节点说明

amcl 功能包中的核心节点是:amcl。

为了方便调用，需要先了解该节点订阅的话题、发布的话题、服务以及相关参数。

2.2.1 订阅的Topic

scan(sensor_msgs/LaserScan)

• 激光雷达数据。

tf(tf/tfMessage)

• 坐标变换消息。

initialpose(geometry_msgs/PoseWithCovarianceStamped)

• 用来初始化粒子滤波器的均值和协方差。

map(nav_msgs/OccupancyGrid)

• 获取地图数据。

2.2.2 发布的Topic

amcl_pose(geometry_msgs/PoseWithCovarianceStamped)

• 机器人在地图中的位姿估计。

particlecloud(geometry_msgs/PoseArray)

• 位姿估计集合，rviz中可以被 PoseArray 订阅然后图形化显示机器人的位姿估计集合。

tf(tf/tfMessage)

• 发布从 odom 到 map 的转换。

2.2.3 服务

global_localization(std_srvs/Empty)

• 初始化全局定位的服务。

request_nomotion_update(std_srvs/Empty)

• 手动执行更新和发布更新的粒子的服务。

set_map(nav_msgs/SetMap)

• 手动设置新地图和姿态的服务。

2.2.4 调用的服务

static_map(nav_msgs/GetMap)

• 调用此服务获取地图数据。

2.2.5 参数

~odom_model_type(string, default:"diff")

• 里程计模型选择: "diff","omni","diff-corrected","omni-corrected" (diff 差速、omni 全向轮)

~odom_frame_id(string, default:"odom")

• 里程计坐标系。

~base_frame_id(string, default:"base_link")

• 机器人极坐标系。

~global_frame_id(string, default:"map")

• 地图坐标系。

.... 参数较多，上述是几个较为常用的参数，其他参数介绍可参考官网。

2.2.3 坐标变换

里程计本身也是可以协助机器人定位的，不过里程计存在累计误差且一些特殊情况时(如车轮打滑)会出现定位错误的情况，amcl 则可以通过估算机器人在地图坐标系下的姿态，再结合里程计提高定位准确度。

• 里程计定位:只是通过里程计数据实现 /odom_frame 与 /base_frame 之间的坐标变换。

• amcl定位: 可以提供 /map_frame 、/odom_frame 与 /base_frame 之间的坐标变换。

3 amcl案例

3.1 amcl节点的launch文件

关于launch文件的实现，在amcl功能包下的example目录已经给出了示例，可以作为参考，具体实现:

roscd amcl
ls examples

该目录下会列出两个文件: amcl_diff.launch 和 amcl_omni.launch 文件，前者适用于差分移动机器人，后者适用于全向移动机器人，可以按需选择。

此处参考前者，根据路威学习套间优化调整后如下:

<launch>
    <node pkg="amcl" type="amcl" name="amcl" output="screen">
        <param name="base_frame_id" value="base_footprint"/>
<!-- Change this if you want to change your base frame id. -->
        <param name="gui_publish_rate" value="10.0"/> 
<!-- Maximum rate (Hz) at which scans and paths are published for visualization, -1.0 to disable. -->
        <param name="kld_err" value="0.05"/>
        <param name="kld_z" value="0.99"/>
        <param name="laser_lambda_short" value="0.1"/>
        <param name="laser_likelihood_max_dist" value="2.0"/>
        <param name="laser_max_beams" value="60"/>
        <param name="laser_model_type" value="likelihood_field"/>
        <param name="laser_sigma_hit" 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="max_particles" value="2000"/>
        <param name="min_particles" value="500"/>
        <param name="odom_alpha1" value="0.25"/> 
<!-- Specifies the expected noise in odometry's rotation estimate from the rotational component of the robot's motion. -->
        <param name="odom_alpha2" value="0.25"/> 
<!-- Specifies the expected noise in odometry's rotation estimate from translational component of the robot's motion. -->
        <param name="odom_alpha3" value="0.25"/> 
<!-- Specifies the expected noise in odometry's translation estimate from the translational component of the robot's motion. -->
        <param name="odom_alpha4" value="0.25"/> 
<!-- Specifies the expected noise in odometry's translation estimate from the rotational component of the robot's motion. -->
        <param name="odom_alpha5" value="0.1"/> 
<!-- Specifies the expected noise in odometry's translation estimate from the rotational component of the robot's motion. -->
        <param name="odom_frame_id" value="odom"/>
        <param name="odom_model_type" value="diff"/>
        <param name="recovery_alpha_slow" value="0.001"/> 
<!-- Exponential decay rate for the slow average weight filter, used in deciding when to recover by adding random poses. -->
        <param name="recovery_alpha_fast" value="0.1"/> 
<!-- Exponential decay rate for the fast average weight filter, used in deciding when to recover by adding random poses. -->
        <param name="resample_interval" value="1"/> 
<!-- Number of filter updates required before resampling. -->
        <param name="transform_tolerance" value="1.25"/> 
<!-- Default 0.1; time with which to post-date the transform that is published, to indicate that this transform is valid into the future. -->
        <param name="update_min_a" value="0.2"/> 
<!-- Rotational movement required before performing a filter update. 0.1 represents 5.7 degrees  -->
        <param name="update_min_d" value="0.2"/> 
<!-- Translational movement required before performing a filter update. -->
    </node>
</launch>

3.2 测试amcl的launch文件

amcl节点是不可以单独运行的，运行 amcl 节点之前，需要先加载全局地图，然后启动 rviz 显示定位结果，上述节点可以集成进launch文件，内容示例如下:

<launch>
    <!-- 启动AMCL节点 -->
    <include file="$(find jujonrobot)/launch/test_amcl.launch" />
    <!-- 运行rviz -->
    <node pkg="rviz" type="rviz" name="rviz"/>
</launch>

当然，launch文件中地图服务节点和amcl节点中的包名、文件名需要根据自己的设置修改。

3.3执行

1.连接上路威套键并输入如下命令直接启动amcl测试launch文件

roslaunch jujonrebot run_amcl.launch

2.启动键盘控制节点：

rosrun teleop_twist_keyboard teleop_twist_keyboard.py

3.在启动的 rviz 中，添加RobotModel、Map组件，分别显示机器人模型与地图，添加 PoseArray 插件，设置topic为/particlecloud来显示 amcl 预估的当前机器人的位姿，箭头越密集的地方，说明当前机器人处于此位置的概率越高。

4.通过键盘控制机器人运动，会发现 PoseArray 也随之而改变。

4 路径规划

路径规划是导航中的核心功能之一，在ROS的导航功能包集navigation中提供了 move_base 功能包，用于实现此功能。

4.1 move_base简介

​ move_base 功能包提供了基于动作(action)的路径规划实现，move_base 可以根据给定的目标点，控制机器人底盘运动至目标位置，并且在运动过程中会连续反馈机器人自身的姿态与目标点的状态信息。如前所述move_base主要由全局路径规划与本地路径规划组成。navigation功能包中集成了move_base。

4.2 move_base节点说明

​ move_base功能包中的核心节点是:move_base。为了方便调用，需要先了解该节点action、订阅的话题、发布的话题、服务以及相关参数。

4.2.1动作订阅

move_base/goal(move_base_msgs/MoveBaseActionGoal)

• move_base 的运动规划目标。

move_base/cancel(actionlib_msgs/GoalID)

• 取消目标。

4.2.2 动作发布

move_base/feedback(move_base_msgs/MoveBaseActionFeedback)

• 连续反馈的信息，包含机器人底盘坐标。

move_base/status(actionlib_msgs/GoalStatusArray)

• 发送到move_base的目标状态信息。

move_base/result(move_base_msgs/MoveBaseActionResult)

• 操作结果(此处为空)。

4.2.3 发布的Topic

cmd_vel(geometry_msgs/Twist)

• 输出到机器人底盘的运动控制消息。

4.2.4 服务

make_plan(nav_msgs/GetPlan)

• 请求该服务，可以获取给定目标的规划路径，但是并不执行该路径规划。

clear_unknown_space(std_srvs/Empty)

• 允许用户直接清除机器人周围的未知空间。

clear_costmaps(std_srvs/Empty)

• 允许清除代价地图中的障碍物，可能会导致机器人与障碍物碰撞，请慎用。

4.3 move_base与代价地图

4.3.1 代价地图概念

机器人导航(尤其是路径规划模块)是依赖于地图的，地图在SLAM时已经有所介绍了，ROS中的地图其实就是一张图片，这张图片有宽度、高度、分辨率等元数据，在图片中使用灰度值来表示障碍物存在的概率。不过SLAM构建的地图在导航中是不可以直接使用的，因为：

1. SLAM构建的地图是静态地图，而导航过程中，障碍物信息是可变的，可能障碍物被移走了，也可能添加了新的障碍物，导航中需要时时的获取障碍物信息；

2. 在靠近障碍物边缘时，虽然此处是空闲区域，但是机器人在进入该区域后可能由于其他一些因素，比如：惯性、或者不规则形体的机器人转弯时可能会与障碍物产生碰撞，安全起见，最好在地图的障碍物边缘设置警戒区，尽量禁止机器人进入...

所以，静态地图无法直接应用于导航，其基础之上需要添加一些辅助信息的地图，比如时时获取的障碍物数据，基于静态地图添加的膨胀区等数据。

4.3.2 代价地图组成

代价地图有两张:global_costmap(全局代价地图) 和 local_costmap(本地代价地图)，前者用于全局路径规划，后者用于本地路径规划。

两张代价地图都可以多层叠加,一般有以下层级:

• Static Map Layer：静态地图层，SLAM构建的静态地图。

• Obstacle Map Layer：障碍地图层，传感器感知的障碍物信息。

• Inflation Layer：膨胀层，在以上两层地图上进行膨胀（向外扩张），以避免机器人的外壳会撞上障碍物。

• Other Layers：自定义costmap。

多个layer可以按需自由搭配。

4.3.3 碰撞算法

在ROS中，如何计算代价值呢？请看下图:

上图中，横轴是距离机器人中心的距离，纵轴是代价地图中栅格的灰度值。

• 致命障碍:栅格值为254，此时障碍物与机器人中心重叠，必然发生碰撞；

• 内切障碍:栅格值为253，此时障碍物处于机器人的内切圆内，必然发生碰撞；

• 外切障碍:栅格值为[128,252]，此时障碍物处于其机器人的外切圆内，处于碰撞临界，不一定发生碰撞；

• 非自由空间:栅格值为(0,127]，此时机器人处于障碍物附近，属于危险警戒区，进入此区域，将来可能会发生碰撞；

• 自由区域:栅格值为0，此处机器人可以自由通过；

• 未知区域:栅格值为255，还没探明是否有障碍物。

膨胀空间的设置可以参考非自由空间。

4.4 move_base使用

​ 路径规划算法在move_base功能包的move_base节点中已经封装完毕了，但是还不可以直接调用，因为算法虽然已经封装了，但是该功能包面向的是各种类型支持ROS的机器人，不同类型机器人可能大小尺寸不同，传感器不同，速度不同，应用场景不同，最后可能会导致不同的路径规划结果，那么在调用路径规划节点之前，我们还需要配置机器人参数。

4.4.1 launch文件

关于move_base节点的调用，launch文件如下:

<launch>
  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen"  clear_params="true">
        <rosparam file="$(find jujonrobot)/param/navigation/navigation/2wd/costmap_common_params.yaml" command="load" ns="global_costmap" />
        <rosparam file="$(find jujonrobot)/param/navigation/navigation/2wd/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find jujonrobot)/param/navigation/navigation/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find jujonrobot)/param/navigation/navigation/global_costmap_params.yaml" command="load" />
        <rosparam file="$(find jujonrobot)/param/navigation/navigation/2wd/base_local_planner_params.yaml" command="load" />
    <rosparam file="$(find jujonrobot)/param/navigation/navigation/move_base_params.yaml" command="load" />
  </node>
</launch>

launch文件解释:

启动了 move_base 功能包下的 move_base 节点，respawn 为 false，意味着该节点关闭后，不会被重启；clear_params 为 true，意味着每次启动该节点都要清空私有参数然后重新载入；通过 rosparam 会载入若干 yaml 文件用于配置参数，这些yaml文件的配置以及作用详见下一小节内容。

4.4.2 yaml 配置文件

costmap_common_params.yaml

obstacle_range: 2.5
raytrace_range: 3.0
footprint: [[-0.11, -0.13], [-0.11, 0.13], [0.11, 0.13], [0.11, -0.13]]
inflation_radius: 0.55
transform_tolerance: 0.5
observation_sources: scan
scan:
  data_type: LaserScan
  topic: scan
  marking: true
  clearing: true
map_type: costmap

local_costmap_params.yaml

local_costmap:
  global_frame: odom
  robot_base_frame: base_footprint
  update_frequency: 1.0 #before 5.0
  publish_frequency: 2.0 #before 2.0
  static_map: false
  rolling_window: true
  width: 2.5
  height: 2.5
  resolution: 0.05 #increase to for higher res 0.025
  transform_tolerance: 0.5
  cost_scaling_factor: 5
  inflation_radius: 0.55

global_costmap_params.yaml

global_costmap:
  global_frame: map
  robot_base_frame: base_footprint
  update_frequency: 1.0 #before: 5.0
  publish_frequency: 0.5 #before 0.5
  static_map: true
  transform_tolerance: 0.5
  cost_scaling_factor: 10.0
  inflation_radius: 0.55

base_local_planner_params.yaml

DWAPlannerROS:
  max_trans_vel: 0.15
  min_trans_vel: 0.01
  max_vel_x: 0.15
  min_vel_x: -0.25
  max_vel_y: 0.0
  min_vel_y: 0.0
  max_rot_vel: 0.5
  min_rot_vel: 0.0
  acc_lim_x: 1.25
  acc_lim_y: 0.0
  acc_lim_theta: 5
  acc_lim_trans: 1.25

  prune_plan: false

  xy_goal_tolerance: 0.25
  yaw_goal_tolerance: 0.1
  trans_stopped_vel: 0.1
  rot_stopped_vel: 0.1
  sim_time: 3.0
  sim_granularity: 0.1
  angular_sim_granularity: 0.1
  path_distance_bias: 34.0
  goal_distance_bias: 24.0
  occdist_scale: 0.05
  twirling_scale: 0.0
  stop_time_buffer: 0.5
  oscillation_reset_dist: 0.05
  oscillation_reset_angle: 0.2
  forward_point_distance: 0.3
  scaling_speed: 0.25
  max_scaling_factor: 0.2
  vx_samples: 20
  vy_samples: 0
  vth_samples: 40

  use_dwa: true
  restore_defaults: false

move_base_params.yaml

base_global_planner: global_planner/GlobalPlanner
base_local_planner: dwa_local_planner/DWAPlannerROS

shutdown_costmaps: false

controller_frequency: 5.0 #before 5.0
controller_patience: 3.0

planner_frequency: 5
planner_patience: 5.0

oscillation_timeout: 10.0
oscillation_distance: 0.2

conservative_reset_dist: 0.1 #distance from an obstacle at which it will unstuck itself

cost_factor: 1.0
neutral_cost: 55
lethal_cost: 253

未完待续