UTM
UTM（Universal Transverse Mercator Grid System，通用横墨卡托格网系统）坐标是一种平面直角坐标，这种坐标格网系统及其所依据的投影已经广泛用于地形图，作为卫星影像和自然资源数据库的参考格网以及要求精确定位的其他应用
投影参数
UTM投影为椭圆柱横正轴割地球椭球体，椭圆柱的中心线位于椭球体赤道面上，且通过椭球体质点。从而将椭球体上的点投影到椭圆柱上。两条割线圆在UTM投影图上长度无变，即2条标准经线圆。两条割线圆之正中间为中央经线圆，中央经线投影后的长度为其投影前的0.9996倍，比例因子k=投影后的长度/投影前的实际长度。则标准割线和中央经线的经度差为1.6206°，即1°37′14.244″
椭圆椭球投影
坐标表示格式
UTM 坐标的表示格式为：经度区纬度区以东以北，其中以东表示从经度区的中心子午线的投影距离，而以北表示距离赤道的投影距离。这个两个值的单位均为米。举例来说，使用 UTM 表示经/纬度坐标 61.44，25.40 的结果就是35V 414668 6812844；而经/纬度坐标 -47.04，-73.48 的表示结果为18G 615471 4789269
坐标表示
更精确的MGRS
MGRS 是北约（NATO）军事组织使用的标准坐标系统。MGRS以 UTM 为基础并进一步将每个区划分为 100 km × 100 km 的小方块。这些方块使用两个相连的字母标识：第一个字母表示经度区的东西位置，而第二个字母表示南北位置。
例如，UTM 点35V 414668 6812844 等价于MGRS点35VMJ1466812844。该MGRS点精度为米，使用15个字符表示，其中最后10个字符表示指定网格中的以东和以北的值。可以使用15个字符表示MGRS值（如前例），也可表示为13、11、9或7个字符；因此，所表示的值的精度分别为1米、10米、100米、1,000米或10,000米。
MGRS
ROS中的diagnostics模块
diagnostics意为诊断，该模块旨在从系统中收集各种状态信息和硬件信息，以进行分析，故障排除和记录。 工具链中包含用于收集，发布，分析和查看diagnostics数据的工具。主要内容有：
diagnostics的消息
诊断工具链围绕/diagnostics主题构建。 在此主题上，系统通过diagnostic_msgs/DiagnosticArray类型的消息（定义如下）发布设备的各种状态，每个状态都关联相应的诊断任务。
Header header #for timestamp
DiagnosticStatus[] status # an array of components being reported on
其中DiagnosticStatus具体信息包括：
byte level # level of operation enumerated above
string name # a description of the test/component reporting
string message # a description of the status
string hardware_id # 硬件ID
KeyValue[] values # an array of values associated with the status
level 的值设定：
byte OK=0
byte WARN=1
byte ERROR=2
byte STALE=3
消息发布 diagnostic_updater
diagnostic_updater是diagnostics模块的核心，用于：
在设备驱动程序上发布传感器数据主题的状态
报告硬件设备已关闭
当值超出范围时报告错误，例如温度
diagnostics数据的分析与整合
diagnostic_aggregator包含一个ros节点aggregator_node，主要用于在运行时对诊断消息进行分类和分析。该节点的订阅发布消息如下：
Subscribed Topics
/diagnostics (diagnostic_msgs/DiagnosticArray)
Published Topics
/diagnostics_agg(diagnostic_msgs/DiagnosticArray)
/diagnostics_toplevel_state (diagnostic_msgs/DiagnosticStatus)
robot_localization
参考：https://zhuanlan.zhihu.com/p/121622661
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软件包的特征
-
经过泛化并通用的状态估计软件包。
-
融合任意数量的输入数据源。节点不限制传感器的数量。例如，如果您的机器人具有多个IMU或里程计信息，则
robot_localization中的状态估计节点可以支持所有传感器。 -
支持多种ROS消息类型。
robot_localization中的所有状态估计节点都可以接收的消息类型包括： -
自定义每个传感器的输入。如果给定的传感器消息包含您不希望包含在状态估计中的数据，则
robot_localization中的状态估计节点允许您排除该数据。 -
持续估计。
robot_localization中的每个状态估计节点在收到一次测量结果后便开始估算机器人的状态。如果传感器数据中有间歇（即很长一段时间，没有收到任何数据），则滤波器将继续通过内部运动模型来估算机器人的状态。 这个对于规则性运动具备很好的维持性，这个也符合机器人、无人机等短时间内的运动预期。
robot_localization使用15维向量来表示机器人的运动状态：
每3个数据一组，分别表示：
x-y-z坐标系的坐标（机器人位置）、
绕x/y/z轴的角度（机器人方向）、
沿x/y/z轴的速度、
绕x/y/z轴的角速度、
沿x/y/z轴的加速度。
正是由于这些特征，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 包的强项。
静止坐标系map
半动不动坐标系odom
动态坐标系base_link
定位的复杂度都是来自于不动不动的odom坐标系。这个世界如果是精确的，机器人得到的数据都是高度一致的话，是不需要odom这个坐标系的存在的，动则动，静则静，很分明也很容易处理。然而由于误差的存在，基本的数学公式是不能简单地套用的，必须把误差抽象进数学运算中。
robot_localization对输入数据的格式化需求
在使用
robot_localization中的状态估计节点开始之前，用户必须确保其传感器数据格式正确，这一点很重要。每种类型的传感器数据都有各种注意事项，因此在使用之前需要尽量满足这些注意事项。
robot_localization要求输入数据符合ROS的一些标准：
正如前所述，robot_localization所支持的ROS消息类型的规范：
- nav_msgs/Odometry
- geometry_msgs/PoseWithCovarianceStamped
- geometry_msgs/TwistWithCovarianceStamped
- sensor_msgs/Imu
REP-105指定了四个主要坐标系：base_link，odom，map和Earth(ROS系统中一般叫做World)。base_link坐标系牢固地固定在机器人上。map和odom是固定的世界坐标系，其原点通常与机器人的起始位置对齐。Earth坐标系用于为多个map坐标系（例如，分布在较大区域的机器人）提供公共参考坐标系。
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中的任何节点一起使用。
最常见的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包在现代机器人系统中有着不可或缺的地方，值得大家深入研究。
网友评论