01-ROS导航功能包源码阅读（一）功能介绍、ROS导航基本概念.md

原始 Markdown文档、Visio流程图、XMind思维导图见：https://github.com/LiZhengXiao99/Navigation-Learning

[TOC]

一、介绍

1、概述

ROS 中导航功能包为机器人导航提供了一套通用的实现，开发者不再需要关注于导航算法、硬件交互等偏复杂、偏底层的实现，这些实现都由更专业的研发人员管理、迭代和维护；可以更专注于上层功能，而对于导航功能的调用，只需要根据自身机器人相关参数合理设置各模块的配置文件即可，当然，如果有必要，也可以基于现有的功能包二次开发实现一些定制化需求，这样可以大大提高研发效率，缩短产品落地时间。

功能简单来说，就是根据输入的里程计等传感器的信息流和机器人的全局位置，通过导航算法，计算得出安全可靠的机器人速度控制指令。

目前常用的主要有 kinetic-devel 和 melodic-devel 两种分支

广泛用在一些对可靠性要求没那么高的自主导航机器人场景中，比如扫地机器人、物流机器人等；自动驾驶一般不会用这个导航功能包，甚至大部分都不用 ROS。

ROS wiki 中对这个功能包又非常详细的使用介绍，

用 cloc 对整个包文件进行代码量统计，结果如下：

大概只有两万行代码，不算多，这个包对我来说学习价值很高，我打算全部看一遍，写一套源码阅读笔记，好了解里面的技术细节，方便以后对代码进行拓展。

2、导航包工作原理

实现导航的关键在于得知道“我在哪”、“我要去哪”和“我要怎么去”，这也就对应着导航的重点：机器人定位、获取目标点和路径规划。涉及的关键技术有以下五点：

• 全局地图：在现实生活中，当我们需要实现导航时，可能会首先参考一张全局性质的地图，然后根据地图来确定自身的位置、目的地位置，并且也会根据地图显示来规划一条大致的路线...对于机器人导航而言，也是如此，在机器人导航中地图是一个重要的组成元素，当然如果要使用地图，首先需要绘制地图。**SLAM（simultaneous localization and mapping）**是最常用的机器人导航建图方法。
• 自身定位：导航伊始和导航过程中，机器人都需要确定当前自身的位置，如果在室外，那么GNSS 是一个不错的选择，而如果室内、隧道、地下或一些特殊的屏蔽GPS信号的区域，由于 GNSS 信号弱化甚至完全不可用，那么就必须另辟蹊径了，比如前面的 SLAM 就可以实现自身定位。ROS中还提供了一个用于定位的功能包：**amcl（adaptiveMonteCarloLocalization）**自适应的蒙特卡洛定位，是用于2D移动机器人的概率定位系统。它实现了自适应（或 KLD 采样）蒙特卡洛定位方法，该方法使用粒子过滤器根据已知地图跟踪机器人的姿态。
• 环境感知：感知周围环境信息，为其他模块提供支持。比如：摄像头、激光雷达、编码器...，摄像头、激光雷达可以用于感知外界环境的深度信息，编码器可以感知电机的转速信息，进而可以获取速度信息并生成里程计信息。
• 路径规划：包括*全局路径规划（gloable_planner）根据给定的目标点和全局地图实现总体的路径规划，使用 Dijkstra 或 A 算法进行全局路径规划，计算最优路线，作为全局路线；和局部路径规划（local_planner）**在实际导航过程中，机器人可能无法按照给定的全局最优路线运行，比如：机器人在运行中，可能会随时出现一定的障碍物...本地规划的作用就是使用一定算法（Dynamic Window Approaches） 来实现障碍物的规避，并选取当前最优路径以尽量符合全局最优路径。
• 运动控制：有了路径规划之后，想让机器人真正执行，得按照一定的格式，把命令发给移动基座，让电机执行。

对应的节点图如下：

• 首先，导航功能包需要采集机器人的传感器信息，以达到实时避障的效果，其中信息包括二维激光信息(sensor_msgs/LaserScan）或者三维点云信息(sensor_msgs/PointCloud)。

• 其次，导航功能包需要机器人发布 nav_msgs/Odometry 格式的里程计信息，同时需要发布相应的 TF 变换。

• 最后，导航功能包输出的是 geometry_msgs/Twist 格式的控制指令，通过解析这些指令控制机器人完成相应的运动。

3、导航包节点

16 个文件夹对应 16 个结点：

节点名	功能
amcl	根据机器人自身的里程数值以及地图特征，利用粒子滤波修正机器人在已知的地图内的位置
fack_localization	一般用于定位仿真
map_server	称为地图服务器，用于地图的保存与导入
move_base	机器人移动导航框架，实现的是整个导航的流程，主要管理全局路径规划、局部路径规划和 recovery
nav_core	路径规划接口，包括 base_local_planner、base_global_planner 和 recivery_behavior 三个接口
base_local_planner	实现了 Trajectory Rollout 和 DWA 两种局部规划算法
dwa_local_planner	实现了DWA局部规划算法，可以看作是base_local_planner的改进版本
parrot_planner	实现了较简单的全局规划算法
navfn	实现了 Dijkstra 和 A* 全局规划算法
global_planner	重新实现了 Dijkstra 和 A* 全局规划算法<可以看作navfn的改进版
clear_costmap_recovery	实现了清除代价地图的恢复行为
rotate_recovery	实现了旋转的恢复行为
move_slow_and_clear	实现了缓慢移动的恢复行为
costmap_2d	主要用于产生二维代价地图，以及提供各种相关的函数
voxel_grid	三维代价地图
robot_pose_ekf	扩展卡尔曼滤波器，输入里程计、IMU、VO中的任意两个或者三个信息后输出一个融合之后的位姿。

4、如何看懂一个 ROS 节点

• 节点功能，文件结构
• 节点输入输出，发布了哪些话题，接受哪些消息
• 仔细看里面的回调函数

二、使用方式

翻译自 ROS wiki 的 Setup and Configuration of the Navigation Stack on a Robot 部分

1、机器人设置

1. 配置 TF 转换

2. 传感器数据源

3. 里程计数据源

4. 移动底盘

5. 建图

导航功能包不强制需要有建图，但为了本教程的目的，我们假设你有一个地图

2、导航包配置

1. 创建 Package

本教程的第一步是创建一个包，用于存储导航栈的所有配置和启动文件。该软件包将依赖于用于满足上述 "机器人设置 "部分要求的任何软件包，以及包含导航栈高级接口的 move_base 软件包。因此，请为软件包选择一个位置，然后运行以下命令：

catkin_create_pkg my_robot_name_2dnav move_base my_tf_configuration_dep my_odom_configuration_dep my_sensor_configuration_dep

该命令将创建一个包含必要依赖项的软件包，以便在机器人上运行导航功能包。

2. 创建机器人配置 Lunch 文件

现在，我们已经有了一个可以存放所有配置和启动文件的工作区，我们将创建一个 roslaunch 文件，用于调用机器人所需的所有硬件和变换发布。启动你最喜欢的编辑器，将下面的代码段粘贴到名为 my_robot_configuration.launch 的文件中。当然，你可以随意将 "my_robot "替换为实际机器人的名称。我们还需要对启动文件进行类似的修改，请务必阅读本节的其余部分。

<launch>

   <node pkg="sensor_node_pkg" type="sensor_node_type" name="sensor_node_name" output="screen">
    <param name="sensor_param" value="param_value" />
 </node>
 <node pkg="odom_node_pkg" type="odom_node_type" name="odom_node" output="screen">
    <param name="odom_param" value="param_value" />
 </node>
 <node pkg="transform_configuration_pkg" type="transform_configuration_type" name="transform_configuration_name" output="screen">
    <param name="transform_configuration_param" value="param_value" />
 </node>

</launch> 

好了，现在我们有了一个启动文件的模板，但还需要针对我们的机器人填写。下面我们将逐一介绍每个部分需要做的修改。

<launch>

   <node pkg="sensor_node_pkg" type="sensor_node_type" name="sensor_node_name" output="screen">

在本节中，我们将调出机器人用于导航的所有传感器。将 "sensor_node_pkg "替换为传感器的 ROS 驱动程序软件包名称，将 "sensor_node_type "替换为传感器的驱动程序类型，将 "sensor_node_name "替换为传感器节点的理想名称，将 "sensor_param "替换为节点可能需要的任何参数。请注意，如果您有多个传感器要用于向导航堆栈发送信息，则应在此处启动所有传感器。

</node>
 <node pkg="odom_node_pkg" type="odom_node_type" name="odom_node" output="screen">
    <param name="odom_param" value="param_value" />
 </node>

在本节中，我们将启动基地的里程计。您需要再次将 pkg、type、name 和 param 等参数替换为与实际启动节点相关的参数。

    <param name="transform_configuration_param" value="param_value" />
 </node>

在本节中，我们将启动机器人的变换配置。您需要再次将 pkg、type、name 和 param 等参数替换为与实际启动节点相关的参数。

3. 代价地图配置

导航功能包使用两个代价地图来存储世界上的障碍物信息。

• global_costmap：用于全局路径规划。
• local_costmap：用于局部路径规划和避障。

我们希望两个代价地图都遵循一些配置选项，也希望在每个地图上分别设置一些配置选项。因此，下面有三个部分用于代价地图配置：通用地图配置选项、全局地图配置选项和局部地图配置选项。

通用配置选项

导航功能包使用代价地图来存储世界上的障碍物信息。为了正确完成这项工作，我们需要将 costmaps 指向它们应该监听更新的传感器主题。让我们创建一个名为 costmap_common_params.yaml 的文件（如下所示），然后填写其中的内容：

obstacle_range: 2.5
raytrace_range: 3.0
footprint: [[x0, y0], [x1, y1], ... [xn, yn]]
#robot_radius: ir_of_robot
inflation_radius: 0.55

observation_sources: laser_scan_sensor point_cloud_sensor

laser_scan_sensor: {sensor_frame: frame_name, data_type: LaserScan, topic: topic_name, marking: true, clearing: true}

point_cloud_sensor: {sensor_frame: frame_name, data_type: PointCloud, topic: topic_name, marking: true, clearing: true}

三、ROS 机器人导航相关概念

1、ROS

1. 工程结构

• **catkin workplace：**管理和组织功能包的文件夹，以 catkin 工具编译。

• src : package 源代码（经常使用）—package1（功能包1）、package2（功能包2）。

• build : cmake&catkin 缓存和中间文件。

• **devel ：**目标文件，一般存放头文件、动态链接库、静态链接库、可执行文件。

• **package(功能包）：**ROS 软件的基本组织形式，catkin 编译的基本单元，一个 package 可以包含多个可执行文件。

• CMakeLists.txt : 规定 catkin 编译的规则，例如源文件、依赖项、目标文件。

• package.xml : 定义 package 的属性，例如包名、版本号、作者、依赖等。

• **scripts：**放置可执行文件的地方，一般为 *.sh（shell文件）和 *.cpp文件。

• **msg&srv：**放置通信格式文件的地方。

• **include：**放置头文件的地方。

• **launch：**放置 launch 文件的地方。

有三个层级：

• 工作空间（workspace）：ROS 需要在工作空间的环境下运行，工作空间简单来说是一个文件夹，里面装载了 ROS 的工程项目，其中包括三个文件夹：src、devel、bulid。catkin_make 可以编译整个工作空间。
• 功能包（package）：
• 节点（node）：每一个节点实现一个功能，每个节点对应功能包中一个可执行文件。

工作空间是项目文件的最高层级，一个工作空间包含多个功能包，一个功能包又可以包含多个节点。

2. 节点通信

在这个导航功能包里，各 ROS 节点之间通过通信来实现功能交互。

• Topic（话题）：发送/接收模式：话题消息通信中发布者或订阅者会一直工作直到被终止。
• Service（服务）：请求/响应模式：与 topic 不同，serice 是一次性的消息通信。需要提出request请求，服务端才会响应，response 返回对应消息。
• Action （动作）：类似于 service 的请求/响应通讯机制，但是还有反馈机制，用来不断向客户端反馈任务的进度，还可以中途中止运行。ROS Service 会有阻塞的效果，程序无法进行其它的工作。而 Action 的通信方式则可以让程序的非阻塞执行；弥补了 service 通信的一个不足，就是当机器人执行一个长时间的任务时，假如利用service 通信方式，那么 publisher 会很长时间收不到反馈的 eply，致使通信受阻。
  • Action server：向 ROS 系统广播指定 action 的 Node，其它 Node 可以向该 Node 发出action目标请求
  • Action client：发出 action 目标请求的 Node

想学明白一个节点，就从主函数开始看，看它订阅了哪些消息，收到消息执行哪些回调函数，然后发布了哪些消息。

3. luanch 文件

rosrun 命令每次只能运行一个节点，当运行一个机器人时要同时运行多个节点就需要用到 launch 文件了，launch 文件里面封装好了要运行的节点，写入的参数，加载的参数文件等等。通过 roslaunch 可以运行 launch 文件，指令如下：

[roslaunch][空格][功能包名][空格][launch 文件名]

运行 roslaunch 命令不需要运行 roscore，运行 roslaunch 命令时，会自动检测是否开启了 roscore，如果没有开启则自动开启 roscore。

launch 文件采用 XML 格式，语法如下：

• 必须要包含一个用 <launch> 标签定义的根元素，文件中的其他内容都必须包含在这个标签之中。

• 启动 ROS 系统的核心是启动 ROS 节点，采用 <node> 标签定义，语法如下：

  <node name="map_server_for_test" pkg="map_server" type="map_server" />

  启动一个节点至少需要三个属性：name、pkg 和 type：

  • name：属性定义节点运行的名称，可自定义
  • pkg：属性定义节点所在的功能包名称
  • type：属性定义节点的可执行文件名称，也可以是 .py 文件

• 参数设置的标签元素有两个：<param>、<arg>分别代表 parameter 和 argument：

  • param：是在 ROS 系统运行中的参数，存储在服务器中，可以给节点使用。
  • arg：是在 launch 文件中定义参数，不能提供给节点使用，只能在 launch 文件中使用。

• <inculde> 标签可以调用其它的 launch 文件，语法：

  <include file="$(文件路径)" />

4. YAML 配置文件

YAML（YAML Ain't Markup Language）是一种轻量级的数据序列化格式，可以用于配置文件、数据交换、API请求等多种场景。它是一种简单易用的数据序列化格式，使得数据可以以人类可读的方式进行存储和传输。YAML的语法非常简单，它使用缩进和符号来表示数据结构。以下是一些YAML的基本语法：

1. 字符串：用引号括起来的文本，例如："hello world"。

2. 数字：没有引号的数字，例如：42。

3. 布尔值：用 true 或 false 表示的真或假。

4. 缩进：YAML使用缩进来表示嵌套关系，每个缩进级别用空格数表示。例如，下面的代码段表示一个包含两个列表的字典：

5. 字典/对象/键值对：用短横线 - 或中括号[]表示的键值对的集合。例如：{name: John, age: 30}或- name: John age: 30。多层对象可表示为：

   key: {key1: value1, key2: value2}

   或者

   key:
     key1: value1
     key2: value2

6. 数组/列表：用短横线 - 或中括号 [] 表示的值的列表。例如：[apple, banana, orange] 或 - apple - banana - orange。复杂一点的如：

   streamers:
       - streamer:
           tag: str_gnss_rov
           output_tags: [fmt_gnss_rov]
           type: file
           path: <data-directory>/gnss_rover.bin
       - streamer:
           tag: str_gnss_ref
           output_tags: [fmt_gnss_ref]
           type: file
           path: <data-directory>/gnss_reference.bin

7. 引用：& 用来建立锚点，<< 表示合并到当前数据，* 用来引用锚点。

8. 注释：在YAML中，使用 # 表示注释。

9. YAML 需要特别注意的几个点：大小写敏感、缩进不允许使用 tab，只允许空格、缩进的空格数不重要，只要相同层级的元素左对齐即可。

10. yaml-cpp 读取 YAML 的语法：

    1. YMAL 在 C++ 中以 Node 类表示。

    2. LoadFile()：从文件中加载 YAMl 到 C++ 中 Node 对象：

       yaml_node = YAML::LoadFile(文件名); 

    3. []：Node 对象可以理解为是树形的，用中括号可以取出里面的子数，创建一个新的 Node 对象：

       YAML::Node logging_node = yaml_node["logging"];

5. ROS 常用组件

• launch：启动文件（Launch File）是ROS中一种同时启动多个节点的途径，它还可以自动启动ROS Master节点管理器，并且可以实现每个节点的各种配置，为多个节点的操作提供很大便利。
• TF 坐标变换：机器人本体和机器人的工作环境中往往存在大量的组件元素，在机器人设计和机器人应用中都会涉及不同组件的位置和姿态，TF 是一个让用户随时间跟踪多个坐标系的功能包，它使用树形数据结构，根据时间缓冲并维护多个坐标系之间的坐标变换关系，可以帮助开发者在任意时间、在坐标系间完成点、向量等坐标的变换。
• rqt 可视化工具集：为了方便可视化调试和显示，ROS 提供了一个 Qt 架构的后台图形工具套件——rqt_common_plugins，其中包含不少实用工具：日志输出工具(rqt_console)、计算图可视化工具（rqt_graph）、数据绘图工具（rqt_plot）、参数动态配置工具（rqt_reconfigure
• Rviz：rviz 是一款三维可视化工具，很好地兼容了各种基于ROS软件框架的机器人平台。在rviz中，可以使用 XML 对机器人、周围物体等任何实物进行尺寸、质量、位置、材质、关节等属性的描述，并且在界面中呈现出来。同时，rviz 还可以通过图形化方式，实时显示机器人传感器的信息、机器人的运动状态、周围环境的变化等。
• Gazebo：Gazebo 是一个功能强大的三维物理仿真平台，具备强大的物理引擎、高质量的图形渲染、方便的编程与图形接口，最重要的还有其具备开源免费的特性。虽然Gazebo中的机器人模型与 rviz 使用的模型相同，但是需要在模型中加入机器人和周围环境的物理属性，例如质量、摩擦系数、弹性系数等。机器人的传感器信息也可以通过插件的形式加入仿真环境、以可视化的方式显示。
• Navigation：也就是本套笔记所记录的内容，ROS 的二维导航功能包，功能简单来说，就是根据输入的里程计等传感器的信息流和机器人的全局位置，通过导航算法，计算得出安全可靠的机器人速度控制指令。
• Moveit：主要用来进行轨迹规划。Moveit！配置助手用来配置一些在规划中需要用到的文件，非常关键。

2、定位建图

1. 位姿

位置很简单，就是坐标，二维定位就是二维坐标，需要关注坐标的参考系（原点、轴指向、尺度），不同坐标参考系之间的转换。

姿态，指载体坐标系（b系）相对导航坐标系（n 系）的变化，有很多种表示方法，欧拉角、方向余弦阵、四元数、等效旋转矢量，各有各的应用场景：

• 欧拉角：横滚、俯仰、航向，特点是物理含义明确，作为姿态求解的结果。不好计算，同时存在着“万向锁”问题。 $$ \boldsymbol{A}=\left[\begin{array}{lll}\theta & \gamma & \psi\end{array}\right]^{\mathrm{T}} $$

• 方向余弦阵：向量的方向余弦来表示的姿态矩阵，常用于描述两个坐标系的相对姿态，比欧拉角抽象，但处理向量的投影变换非常方便。 $$ \boldsymbol{C}=\left[\begin{array}{lll}c_{x x} & c_{x y} & c_{x z} \ c_{y x} & c_{y y} & c_{y z} \ c_{z x} & c_{z y} & c_{z z}\end{array}\right] $$

• 四元数：复数可以看成是复平面上的向量，即一个二维的向量，将复数的概念再拓展可以得到四元数，用以描述三维空间中的向量。由于只有四个元素，四元数比坐标变换矩阵计算快，也避免了欧拉角中的“万向锁”问题，但理解起来更困难。 $$ \boldsymbol{Q}=q_{0}+q_{1} \boldsymbol{i}+q_{2} \boldsymbol{j}+q_{3} \boldsymbol{k}=q_{0}+\boldsymbol{q}_{v} $$

• 等效旋转矢量：定点有限旋转都可以用绕经过该固定点的一个轴的一次转动来等效实现；也是方便计算，用于表示姿态的变化，最大的特点就是可以插值。

• 李代数：同时表示位置和姿态，常用 Sophus 库来实现。

2. SLAM

3. 惯性导航 INS

INS 指惯性导航系统，IMU 指惯性测量单元

当前最常用的是捷联式惯导系统

• 零偏：陀螺仪、加速度计
• 比力：陀螺仪、加速度计

高精度的惯导解算还要考虑重力、地球自转、牵连角速度等对姿态、速度的影响。

惯导很少单独使用，因为位姿是通过递推得到的，随着递推时间增加误差不断累积： $$ \delta r_{N}=\delta r_{N, 0}+\delta v_{N, 0} \cdot t+\frac{1}{2}\left(g \cdot \delta \theta_{0}+b_{a N}\right) t^{2}+\frac{1}{6}\left(g \cdot b_{g E}\right) t^{3} $$ 从上面北向的速度误差传播公式可以看出：

• 初始位置是常值误差，
• 初始速度误差造成的位置误差是随时间一次方发散；
• 初始姿态角（俯仰角常值误差）常值误差和加速度计误差，是随时间二次方发散；
• 陀螺的零偏误差，造成的位置误差是随时间三次方发散的，这个量的影响是很大的，陀螺的零偏是衡量惯导性能的重要指标，用于对惯导分级：0.01deg/h 对应导航级，1deg/h 对应战术级。

，大多与其它导航系统组合使用：

• 与 GNSS 组合：
• 与 ODO 组合：
• 与相机、激光雷达组合：

4. 卫星导航 GNSS

• 伪距单点定位 SPP：
• 载波相位差分定位 RTK：
• 精密单点定位 PPP：

5. 轮式里程计 Odometer

里程计是一种利用从移动传感器获得的数据来估计物体位置随时间的变化而改变的方法，包括惯性里程计、视觉里程计、激光里程计、轮式里程计等。该方法被用在许多机器人系统来估计机器人相对于初始位置移动的距离。包含了两个方面的信息，一方面是位姿（位置和姿态），另一方面就是速度（前进速度和转向速度）。

轮式里程计可以利用车轮等部位的旋转运动来测量车辆运动距离和方位角，将车轮的旋转转变成张力或位移变化，再通过张力或位移的变化量来计算物体的位移和姿态角度。

编码器每转一圈会触发固定数量的 ticks 值（通常会有几百或者几千个），从而记录对应轮子转了多少圈。加上预先知道的轮子的直径和轮间距，编码器就可以把记录的数据转化轮子行驶的距离，或者用 rad 表示的轮子转动的角度：

• 前进速度： 左右轮的平均速度，这个是通过电机的编码器获取到的。编码器能记录一定时间内的转过的弧度数，根据这个看算出每个轮子的速度。
• 转向速度：根据左右轮的在给定时间内的弧度差计算得到。
• 位置的获取：根据上面的前进速度推算出位置。
• 姿态的获取：根据上面的转向速度推算出转角。

6. 相机 Cmera

• 单目相机：单目相机在环境中移动，不断获得图像帧，通过两帧之间三角化计算，将空间的点投影到相机空间，对环境进行建模。
• 双目相机：左右相机同时能得到两图像帧，直接可以获得距离。
• RGB-D相机：实现方式有多种：
  • Kinect1：使用红外光，根据返回结构光图像，计算距离。
  • Kinect2：用脉冲信号，根据收发信号时间差计算距离。

7. 激光雷达 Lidar

8. 时间同步

多个传感器因为时间不同步、采样频率不同，需要在时间上进行标定。多个传感器因为安装位置和自身旋转角度姿态不同，需要进行空间标定。

2.1、时钟硬同步 所谓的时间硬同步，就是通过唯一的时钟源给各传感器提供相同的基准时间。各传感器根据提供的基准时间校准各自的时钟时间，从硬件上实现时间同步,也就是我们说的统一时钟源，目前自动驾驶中主流时间同步是以GPS时间为基准时间 ，采用PTP/gPTP时钟同步协议来完成各传感器之间的时间同步，PTP 前提是需要交换机支持PTP协议，才能实现高精度同步。 与PTP同时出现的还有一种NTP，即网络时间协议，不同的是PTP是在硬件级实现的，NTP是在应用层级别实现。

2.2、采样频率同步 由于每种传感器的采样频率不一致，如lidar通常为10Hz，camera通常为25/30Hz，不同传感器之间的数据传输还存在一定的延迟，那么可以通过寻找相邻时间戳的方法找到最近邻帧,如果误差很大，可以采用硬同步触发，调整传感器的固有频率来达到一致性。

2.3、时间软同步 分为帧率具有整数倍数关系的传感器之间和非整数倍关系传感器之间的时间对齐，整数倍的比较好处理，非整数倍的可以用内插外推法，主要利用两个传感器帧上的时间标签，计算出时间差，然后通过包含有运动信息的目标帧与时间差结合，推算出新的帧时各个目标的位置，并于原有的两帧之间建立新的帧。

9. 标定

空间的融合标定按照实现原理分为硬件层的融合，如禾赛和Mobileye等传感器厂商，利用传感器的底层数据进行融合；数据层，利用传感器各种得到的后期数据，即每个传感器各自独立生成目标数据，再由主处理器进行融合这些特征数据来实现感知任务；任务层，先由各传感器完成感知或定位任务，如障碍物检测，车道线检测，语义分割和跟踪以及车辆自身定位等，然后添加置信度进行融合。

3.1、内参标定和外参标定 传感器标定分为单传感器的标定和多传感器之间的标定，主要是外参标定和内参标定，目的是为了保证确定不同传感器的空间关系，并统一在整车坐标系下，这样就能获取不同传感器采集的同一障碍物的信息，便于后续融合处理。 内参是决定传感器内部的映射关系，比如摄像头的焦距，偏心和像素横纵比（+畸变系数），而外参是决定传感器和外部某个坐标系的转换关系，比如姿态参数（旋转和平移6自由度）。摄像头的标定曾经是计算机视觉中3-D重建的前提。

3.2、设备标定 一般传感器安装完，需要对车辆进行整车的标定。标定分为基于标定设备的标定和基于自然场景的标定。 基于标定设备的比较容易理解，就是根据传感器的安装位置进行标定。首先要根据厂商提供的传感器尺寸和内参进行标定，把原点之间距离计算出来，这就是平移距离，这个相对简单。而旋转角度比较难于计算，需要特殊方法。 基于自然场景的标定方法，是利用外部场景中静止的路标（如树木、电线杆、路灯杆、交 通标识牌等）和清晰车道线进行标定。让移动车跑几圈，把传感器获得的数据和实际的外部场景的路标进行比较校正。

3.3、坐标系同步 空间同步,也就是不同传感器坐标系下的测量值转换到同一坐标系下，通俗理解为传感器在整车坐标系下的标定参数，其中一部分就是运动补偿，比如纯估计补偿，用括ICP(Iterative Closest Point，迭代最近点算法)以及其相关的变种(VICP)来线性补偿，但这是基于匀速运动假设基础上。

3、TF 坐标变换

TF 全称 TransForm，坐标变换，包括了位置和姿态两个方面的变换；ROS中的 TF 是一个可以让用户随时记录多个坐标系的软件包，保持缓存的树形结构中的坐标系之间的关系， 并且允许用户在任何期望的时间点在任何两个坐标系之间转换点， 矢量等。

TF（TransForm）坐标变换在ROS系统中，被定义为一个树状的数据结构，即“tf tree”。这个数据结构维护了整个机器人的坐标转换关系，包括机器人的每一个部件（例如手部、头部、关节、连杆等）的坐标系。每一个部件都有一个相对应的坐标系（frame），并且每一个frame都与一个link绑定在一起。

在机器人的运动控制中，常常需要定义两个或多个坐标系之间的关系。例如，以机器人为例，可以定义两个坐标系：一个坐标系以机器人移动平台的中心为原点，称为base_link参考系；另一个坐标系以激光雷达的中心为原点，称为base_laser参考系。tf变换树就定义了这两个坐标系之间的平移与旋转变换关系。

tf功能包提供了存储、计算不同数据在不同参考系之间变换的功能。例如，可以通过定义laser_pose.header.frame.id来确定该点所属的坐标系（比如激光坐标系），然后通过tf树将这个点转换到另一个坐标系下。

因此，TF坐标变换本质上是一种在不同坐标系之间进行转换的方法，可以用来维护和更新机器人的坐标转换关系，并且这种方法是通过树状数据结构和话题通信机制来实现的。

4、参数估计

量测总是不完全准确，载体位置也只有是一个估计值，根据已有的信息，载体最可能的位置

有本书叫《概论机器人》

1. 最小二乘

2. 卡尔曼滤波

3. 蒙特卡洛方法

• 无迹卡尔曼滤波 UKF：
• 容积卡尔曼滤波 CKF：
• 粒子滤波 PF：

4. 图优化

5、地图

自主移动机器人在未知环境中完成指定任务，则要求机器人对自身及周围环境信息进行准确感知，并以此信息构建与室内环境一致的环境地图模型，而生成反映真实环境的环境地图对机器人后续的定位、导航、避障等有着至关重要的作用。

1. 常见地图分类

• 特征地图：利用传感器将机器人所处环境中的环境信息进行采集，并通过点、直线、曲线等几何特征来描述环境信息。通过这种几何特征来描述机器人所处环境中的环境信息，使得环境地图更加简洁直观，同时特征地图占用空间小，运算量小；但是当机器人所处环境特征较为复杂时，存在难以用简单的几何特征来描述周围环境信息的问题，而且特征地图环境信息与机器人位姿估计的准确性有关，当机器人位姿估计存在一定偏差时，构建出的环境地图同样存在问题，这将可能造成后续机器人导航或避障失败。
• 拓扑地图：拓扑地图是通过一张带节点和连接线的拓扑结构图来表示的，其中，节点代表环境中特征，连接线表示各特征之间的可行路线。拓扑地图结构紧凑，空间复杂度较低，占用存储空间小，计算效率高，不需要机器人的精准定位。但当机器人所处环境中结构特征相似时，拓扑地图就很难将这些特征进行有效区分，这将对机器人的地图构建和路径规划造成影响。
• 栅格地图：栅格地图是应用最为广泛的一种环境地图。栅格地图是将机器人所处的环境分割为多个方形栅格，栅格的大小反映地图的分辨率。为了便于表示环境中的物体，栅格地图中每一个栅格都有三种状态，并通过概率来描述这三种状态，每个栅格的概率为[0，1]，1 和 0 分别代表栅格已被占用和未被占用，0.5 表示不清楚栅格是否被占用，并且在建图的一开始所有栅格的概率都为 0.5。栅格地图适合本文通过机器人上安装的激光雷达对周围环境进行感知的这种方式，而且便于后期维护，为后续的机器人自主导航与自动避障提供了良好的基础。

2. 栅格地图建图原理

在一个栅格状态被概率描述的过程中，$P(a=1)$ 代表栅格已被占用，$P(a=0)$ 表示栅格未被占用，且 $P(a=1)+P(a=0)=1$ 。定义一个 odd 函数，odd 可以表示为： $$ \operatorname{odd}(a)=\frac{P(a=1)}{P(a=0)} $$ 下一时刻，激光雷达完成一次扫描后，得到该栅格新的观测值为 $z \sim(0,1)$ 时，对栅格的状态进行更新，更新状态可以表示为： $$ \operatorname{odd}(a \mid z)=\frac{P(a=1 \mid z)}{P(a=0 \mid z)} $$ 根据贝叶斯公式，该栅格被占用和未被占用的概率可以表示为： $$ \left{\begin{array}{l} P(a=1 \mid z)=\frac{P(z \mid a=1) P(a=1)}{P(z)} \ P(a=0 \mid z)=\frac{P(z \mid a=1) P(a=0)}{P(z)} \end{array}\right. $$ 带入上上式，得到： $$ \operatorname{odd}(a \mid z)=\frac{P(a=1 \mid z)}{P(a=0 \mid z)}=\frac{\frac{P(z \mid a=1) P(a=1)}{P(z)}}{\frac{P(z \mid a=1) P(a=0)}{P(z)}}=\frac{P(z \mid a=1)}{P(z \mid a=0)} \operatorname{odd}(s) $$ 对式两边同时取对数得： $$ \log \operatorname{odd}(a \mid z)=\log \frac{P(z \mid a=1)}{P(z \mid a=0)}+\log \operatorname{odd}(a) $$ 可以看出式中只有 $\log \frac{P(z \mid a=1)}{P(z \mid a=0)}$ 唯一一个观测量，并将这个比值定义为观测量模型，记为 lomeas，将占用和未被占用两种状态定义为 looccu 和 lofree 可以表示为： $$ \left{\begin{array}{l} \text { loccu }=\log \frac{P(z=1 \mid a=1)}{P(z=1 \mid a=0)} \ \text { lofree }=\log \frac{P(z=0 \mid a=1)}{P(z=0 \mid a=0)} \end{array}\right. $$ 假设栅格 $a$ 的状态 $\log \operatorname{odd}(a \mid z)$ 用 $A$ 来表示，则模型的更新公式可以表示为： $$ A^{+}=A^{-}+\text {lomeas } $$ 式中:$A^{+}, A^{-}$一 模型更新前后的值，可以通过加法对栅格进行更新。

3. 代价地图 Cost Map

• 全局代价地图：
• 局部代价地图：

6、路径规划

1. 全局路径规划

2. 局部路径规划

7、PID 控制

1. 基本原理

• 比例 P：就是直接给偏差乘以一个比例系数。

• 积分 I：就是在一段时间内偏差的累积。

• 微分 D：偏差的变化率，反馈当前的变化速率的控制。

传感器无法做到在时间上的连续采集；微控制器（单片机）只需要满足系统控制需求即可实现不影响；一般用一阶差分代替一阶微分，用累加代替积分。

2. 编码器

编码器是一种将角位移或者直线位移转换成电信号的一种传感器，主要作用是用于测量位置、测量速度。根据读数方式可以分为增量式和绝对式，根据原理可以分为光电编码器（光学式）和机电编码器（磁式）：

如下是一个基础的原理图示：

通过霍尔元件或者光电管感应码盘的转动，输出脉冲波，

用微处理器记录脉冲波，分析得出电机转动信息：

• 位置：实际就是一直累积的脉冲数（单位为脉冲）

• 速度：实际就是把代表位置的脉冲数，进行定时读取定时清零，得到的数据就代表单位时间内转动的脉冲数，即可代表为速度（单位为脉冲数/单位时间）

记录脉冲波的方法有两种：

• 常 规：A 相计数，B 相判断方向

• 四倍频：测量 A 相和 B 相的上升沿和下降沿，提高精度。

3. 位置式 PID

1. 获取目标值，一般可以通过输入信号（比如按键控制）获取。

2. 获取测量值，单片机通过电机编码器根据脉冲数测得位置。

3. 比较目标值、测量值得到偏差，送入PID控制器计算。

4. PID控制器计算输出，根据输出值控制输出给驱动的信号。

5. 驱动放大控制信号然后输出给电机，实现电机的控制。

4. 增量式 PID

1. 获取目标值，一般可以通过输入信号（比如按键控制）获取。

2. 获取测量值，单片机通过电机编码器根据单位时间内的脉冲数等效为速度。

3. 比较目标值、测量值得到偏差，送入PID控制器计算。

4. PID控制器计算输出，根据输出值控制输出给驱动的信号。

5. 驱动放大控制信号然后输出给电机，实现电机的控制。

8、Twist 控制指令

Twist消息包含线性（linear）和角速度（angular）组件。线性组件是速度（以米/秒为单位），而角速度组件是旋转速度（以弧度/秒为单位）。Twist消息中的所有值都是相对于机器人的初始位置和方向计算的。

如果一个机器人需要以特定速度进行直线运动，那么Twist消息的线性组件会指定该速度。例如，如果需要机器人以0.1米/秒的速度直线前进，那么线性组件的值应该是[0.1, 0.0, 0.0]。

同样，如果机器人需要以特定速度进行旋转，那么Twist消息的角速度组件会指定该旋转速度。例如，如果需要机器人以0.0弧度/秒的速度旋转，那么角速度组件的值应该是[0.0, 0.0, 0.0]。

10、机器人运动学

移动机器人拥有差速运动模型、阿克曼运动模型、履带式运动模型、麦克纳姆伦运动模型等多种不同的驱动方式：

• https://mp.weixin.qq.com/s/COL_sTySeAXTb12e8wU8HA