机器人定位技术：robot_localization_人工智能

robot_localization是基于卡尔曼滤波在ROS系统上比较成熟、应用比较广泛的一个机器人动态定位软件包。robot_localization软件包中使用的定位算法并不是最时新最优秀的，但是它具备几个不可替代的优势：

它有专门的逻辑融合GPS定位信息，可以支持户外定位
它能够融合多种传感器数据，支持3D空间定位
与ROS系统的集成由来已久，深得人心，普及率挺好。

由于这些原因，robot_localization软件包迄今为止从代码到文档都得到了很好的维护，对ROS乃至ROS2的各个发布版本都有专门的支持。

robot_localization实现了几个机器人状态估计（State Estimation)节点。每个节点(Node，ROS术语，一个Node是一个独立的进程空间，具备自己的上下文与生命周期)都是非线性状态估计器的一种实现，用于在3D空间中移动的机器人。它包括两个状态估计节点ekf_localization_node和ukf_localization_node。另外，robot_localization提供navsat_transform_node，它实现对GPS数据的集成。

robot_localization软件包的特征

robot_localization具备如下的一些特征：

经过泛化并通用的状态估计软件包。
融合任意数量的输入数据源。节点不限制传感器的数量。例如，如果您的机器人具有多个IMU或里程计信息，则robot_localization中的状态估计节点可以支持所有传感器。
支持多种ROS消息类型。robot_localization中的所有状态估计节点都可以接收的消息类型包括：
nav_msgs/Odometry，
sensor_msgs/Imu，
geometry_msgs/PoseWithCovarianceStamped，
geometry_msgs/TwistWithCovarianceStamped。
自定义每个传感器的输入。如果给定的传感器消息包含您不希望包含在状态估计中的数据，则robot_localization中的状态估计节点允许您排除该数据。
持续估计。robot_localization中的每个状态估计节点在收到一次测量结果后便开始估算机器人的状态。如果传感器数据中有间歇（即很长一段时间，没有收到任何数据），则滤波器将继续通过内部运动模型来估算机器人的状态。这个对于规则性运动具备很好的维持性，这个也符合机器人、无人机等短时间内的运动预期。

robot_localization使用15维向量来表示机器人的运动状态：

每3个数据一组，分别表示：

x-y-z坐标系的坐标（机器人位置）、
绕x/y/z轴的角度（机器人方向）、
沿x/y/z轴的速度、
绕x/y/z轴的角速度、
沿x/y/z轴的加速度。

细致请看下面的例子：

有人似乎会问，既然都计算了沿坐标轴的线加速度，那么是不是应该再加第六组数据，记录绕坐标轴的角加速度呢？不要这么问哈，没人性了哈，用15个数据足可以描述一个机器人的状态，并且，角加速度的误差较大，目前也还没有太多的传感器可以直接输出角加速度的，因此用较大的协方差去差这个输入值，没有太大的意义。

正是由于这些特征，robot_localization常常被用在两种典型的场景：

融合连续的传感器数据（里程计和IMU）创建局部精确的状态估计
融合连续的传感器数据及全局位姿估计来提供精确而完整的全局状态估计

这两句干干巴巴的，有点难以理解，换成人话就是：

适合应用于使用多种位置、方向的传感器融合的场合，可以做出精确的局部位姿估计
如果再加上一些全局state的话（来自于其他的全局传感器或数据）可以实现对全局的状态估计。

robot_localization的典型用法应该是配合机器人导航模块，实现各种sensor的融合以及精确的路线导航。

如上图所示，图的半部分是robot_localization的逻辑，包括*kf_localization_node(指ekf_localization_node或ukf_localizationnode）和navstat_transform_node.

*kf_localization_node节点融合多种传感器数据，这些数据可以来自于真实的传感器，也可以来自于其他的定位模块，例如amcl、gmapping、cartographer等等，如果想要融合GPS数据，则需要navstat_transform_node节点做中转。

图的右半部分就是大名鼎鼎的ROS Navigation模块的逻辑图，若想做完美的路线导航，Navigation模块需要获得精美的定位信息（坐标系转换矩阵/tf，和机器人行程信息odom)，而这正是robot_localization 包的强项。

这些精美的输出，下图可以说明一二：

这个图上，列出了3个坐标系：

静止坐标系map
半动不动坐标系odom
动态坐标系base_link

定位的复杂度都是来自于不动不动的odom坐标系。这个世界如果是精确的，机器人得到的数据都是高度一致的话，是不需要odom这个坐标系的存在的，动则动，静则静，很分明也很容易处理。然而由于误差的存在，基本的数学公式是不能简单地套用的，必须把误差抽象进数学运算中，这本身又变成了一个概率问题，更说不清道不明了。

从上图的运算公式可以看出，各种sensor的输入数据被误差和方差（，）所包裹。因此为了简化机器人的运动学的表示，抽象出来odom坐标系，转门对输入误差（主要为初始位置的误差和sensor数据的误差）进行表示。

重新回到上面的坐标系的转换图。由于odom坐标系的引入，解决定位的问题，就转变成了求解odom到base_link或者map到odom的空间关系。因为我们只需要知道两个空间关系的任何一个，都可以推导出来机器人（通过base_link标识）在地图上的准确位姿。

图中3个坐标系之间的实线箭头表示其空间关系可以直接由robot_localization包来求解，他们之间的虚线箭头表示它们之间的空间关系也可以求得，但是需要通过TF转换间接获得。

图上同时表达了对输入数据的不同处理用途，pose data（包含odom数据）会被用来对接map坐标系和odom坐标系的校准上，而Twist data以及IMU data被用来对接base_link也即机器人那本体的方向校准上，这些校准是用来换算定位准确度的基础。

那么，map–>Odom转换和odom–>baselink转换在感官上有什么不同吗？这个可以从设置不同的fixed_frame加以感受。选择以odom坐标系作为基准的时候，可以看到机器人的运动轨迹相当平顺，已经滤除了噪音的干扰（如下图）。

选择以map坐标系作为基准的时候，则需要多经过一次TF转换，因此在运动轨迹上会有一定的扰动，这个与误差的大小以及TF转换的及时性有一定的关系（如下图）。请注意蓝色箭头是odom的Transformer，即坐标转换的即时值。

由此，又引出在传感器数据融合过程中，正确地设置协方差，是多么的重要。按照卡尔曼滤波算法，在sensor融合的时候，对数据的精度也进行了平均，因此如果协方差不能很好地配置地话，就会得到适得其反的效果。

robot_localization对输入数据的格式化需求

在使用robot_localization中的状态估计节点开始之前，用户必须确保其传感器数据格式正确，这一点很重要。每种类型的传感器数据都有各种注意事项，因此在使用之前需要尽量满足这些注意事项。

robot_localization要求输入数据符合ROS的一些标准：

REP-103（标准计量单位和坐标约定）
REP-105（坐标系约定）

正如前所述，robot_localization所支持的ROS消息类型的规范：

REP-105指定了四个主要坐标系：base_link，odom，map和Earth(ROS系统中一般叫做World)。base_link坐标系牢固地固定在机器人上。map和odom是固定的世界坐标系，其原点通常与机器人的起始位置对齐。Earth坐标系用于为多个map坐标系（例如，分布在较大区域的机器人）提供公共参考坐标系。earth坐标系与本教程无关。

robot_localization的状态估计节点会生成状态估计，其状态在map或odom坐标系中给出，其速度在base_link坐标系中给出。在与状态融合之前，所有传入的数据都将转换为这些坐标系之一。每种消息类型中的数据如下转换：

nav_msgs/Odometry：所有位姿数据（位置和方向）都从消息头的frame_id转换为world_frame参数指定的坐标系（通常为map或odom）。在消息本身中，这特别是指pose属性中包含的所有内容。所有twist数据（线速度和角速度）都将从消息的child_frame_id转换为base_link_frame参数（通常为base_link）指定的坐标系。
geometry_msgs/PoseWithCovarianceStamped：以与Odometry消息中的pose数据相同的方式处理。
geometry_msgs/TwistWithCovarianceStamped：以与Odometry消息中的twist数据相同的方式处理。
sensor_msgs/Imu：尽管ROS社区正在解决IMU消息，但目前存在一些歧义。大多数IMU在固定的世界坐标系中报告方向数据，该坐标系的X和Z轴分别由指向磁北和地球中心的向量定义，Y轴朝东（与磁北向量偏移90度）。此坐标系通常称为NED（北，东，下）。但是，REP-103为室外导航指定了ENU（东，北，上）坐标系。在撰写本文时，robot_localization假定所有IMU数据都使用ENU坐标系，并且不适用于NED坐标系数据。将来可能会有所改变，但就目前而言，用户应确保将数据转换为ENU框架后，再将其与robot_localization中的任何节点一起使用。

IMU也可以在机器人的“中性”位置以外的其他位置定向。例如，用户可以将IMU安装在其侧面，或者旋转IMU使其面对机器人正面以外的方向。通常通过从base_link_frame参数到IMU消息的frame_id的静态转换来指定此偏移量。robot_localization中的状态估计节点将自动校正传感器的方向，以使其数据与base_link_frame参数指定的坐标系对齐。

对于这4中类型的消息，这里简短地过一下融合时的注意事项：

最常见的odometry数据来自于轮子的编码器。许多机器人平台都配备了提供瞬时平移和旋转速度的车轮编码器。目前也有许多整合了odometry而生成位置估计的算法跟实现。如果要对此类数据（位姿、速度等）融合处理，最佳做法通常是：

如果里程计同时提供位置（Position)和线速度(Linear Velocities)，就将线速度信息融合进来。
如果里程计同时提供方向(Orentation)和角速度(Angular Velocties)，就将方向信息融合进来。

也就是说，对于平移的定位，更多倾向于动态的速度信息，而对于旋转的定位，更多地倾向于较为静态的方向信息。

而更为复杂的，如果有两个来源提供方向数据，则需要特别注意。如果两个方向都具有精确的协方差矩阵，则可以安全地融合方向。但是，如果其中一个或两个都未报告其协方差，则应仅融合来自更精确传感器的方向数据。对于另一个传感器，请使用角速度（如果已提供），或继续融合绝对方向数据，但是要为该传感器打开_differential模式。

对于IMU类传感器数据的处理，除了需要遵循ROS的相关接口规范之外，同样要注意协方差矩阵的准确定义与配置。IMU数据在处理加速度是，需要格外注意处理地球重力加速度天然存在的事实，做好相关坐标轴上的消减考量。而这种考量是要结合IMU传感器具体的安装方位做出应对的。

GPS数据融合

最后再来说一下navsat_transform_node。

如前所述，navsat_transform_node这个节点的存在就是为了能够融合GPS数据而存在的。

如上图所示，navsat_transform_node节点的输入有3个：

nav_msgs/Odometry (EKF输出，需要机器人当前的位置)
sensor_msgs/Imu (必须有陀螺仪，需要确定全局朝向)
sensor_msgs/NavSatFix (从导航卫星设备输出)

图中将*kf_localization_node节点分为map和odom两部分，主要是可以更形象地说明，navsat_transform_node节点是通过跟静止坐标系map之间的换算来达到数据融合，不需要odom坐标系的介入。

其处理过程包含如下几个步骤：

将gps数据转换成UTM坐标。UTM（Universal Transverse Mercator，通用横轴墨卡托）是一种通用直角坐标系，是用来标识地图常用的一种标准投影坐标系。某个特定的GPS数据都能对应到某个唯一的UTM坐标，这两者存在一一对应关系（严格说来，需要明确Z轴数据才会一一对应）。因此这种转换是确定的、容易的。
使用初始的UTM坐标，EKF/UKF输出和IMU生成从UTM网格到机器人世界框架的（静态）变换T。也就是说，UTM坐标原点在静态坐标系（常常为map）的方位表示。这两个坐标系都是静止的，因此T也是固定不变的。这个T的生成需要用到实时的机器人位姿信息odometry以及动态连续的IMU数据来提供对机器人全局位姿信息。
使用T变换所有测量的gps数据。
将数据发给EKF/UKF。

总结，robot_localization包脱胎于robot_pose_ekf，并在它的基础上做了一些完善。目前robot_pose_ekf已经被移出了Navigation模块，代之以AMCL来提供2D环境下的定位功能。然而，由于其特性以及比较好的融合能力，robot_localization包在现代机器人系统中有着不可或缺的地方，值得大家深入研究。

关于具体的参数配置，就不赘述了，更多资料可以查看官方文档

robot_localization wiki

以及源代码

https://github.com/cra-ros-pkg/robot_localization