ROS

week48 utm、diagnostics、robot_loc

2021-04-10  本文已影响0人  吃醋不吃辣的雷儿

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软件包的特征

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消息类型的规范：

REP-105指定了四个主要坐标系：base_link，odom，map和Earth(ROS系统中一般叫做World)。base_link坐标系牢固地固定在机器人上。map和odom是固定的世界坐标系，其原点通常与机器人的起始位置对齐。Earth坐标系用于为多个map坐标系（例如，分布在较大区域的机器人）提供公共参考坐标系。
robot_localization的状态估计节点会生成状态估计，其状态在map或odom坐标系中给出，其速度在base_link坐标系中给出。在与状态融合之前，所有传入的数据都将转换为这些坐标系之一。每种消息类型中的数据如下转换：

如果里程计同时提供位置（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坐标系的介入。

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

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

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

上一篇下一篇

猜你喜欢

热点阅读