ROS学习笔记之——robot_localization包

之前博客已经介绍过robot_pose_ekf功能包以及（extended）kalman滤波的原理《 ROS学习笔记之——EKF （Extended Kalman Filter） node 扩展卡尔曼滤波》本博文看看robot_localization包中的EKF

更多关于卡尔曼滤波，可以参考下面资料：

http://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/

以及博客《学习笔记之——卡尔曼滤波》

包的源码

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

 

目录

遵守ROS标准

ROS中标准测量单位和坐标约定

ROS中移动平台坐标系

base_link

odom

map

Map Conventions

robot_localization包的基本介绍

robot_localization包的使用

Coordinate Frames and Transforming Sensor Data

Considerations for Each Sensor Message Type

里程计

IMU

参考资料

---

 

遵守ROS标准

在使用robot_localization中的状态估计节点开始之前，用户必须确保其传感器数据格式正确。

其实对于在ROS中采用的代码，其传递的状态信息的数据，本身就应该跟ROS相匹配。下面来看看

ROS中标准测量单位和坐标约定

 https://www.ros.org/reps/rep-0103.html

单元和约定的不一致是开发人员集成问题的常见来源，也可能导致软件错误。由于数据转换，它也会产生不必要的计算。

基本的单位

衍生单位

坐标帧的转换

• 采用右手定则
• 旋转表示法（四元数）
• 协方差的表示

 

ROS中移动平台坐标系

 https://www.ros.org/reps/rep-0105.html

这文档指定与ROS一起使用的移动平台坐标系的命名约定和语义含义

坐标帧主要有以下几种

base_link

 base_link 坐标系与移动机器人底座刚性连接。 base_link可以在任何位置或方向附着在底座上。对于每个硬件平台，在 base上都会有一个不同的位置，提供一个明显的参考点

odom

odom是应该世界固定框架（ world-fixed frame）。在odom frame中，移动平台的姿态可以随时间漂移，没有任何边界。这一飘逸导致odom frame在长期的全局参考中没有意义。然而，机器人在odom frame中的位姿可以保证是连续的，意味着odom frame中的移动平台的位姿总是以平滑的方式进化，没有离散的跳跃。

典型的odom帧的设置是基于里程计源计算，如车轮里程计、可视里程计或惯性测量装置。

odom作为一个精确的、短期的局部参考是有用的，但是漂移使它成为一个不适合长期参考的框架

map

map坐标帧是世界坐标系。Z轴垂直向上。移动平台的位姿与map帧相关，不应随着时间显著飘移。map帧不是连续的，以为着移动平台在map帧上的的位姿将会在任何时间离散的改变。

在典型的设置中，定位的组件会跟踪传感器的观测（sensor observation）不断的计算机器人在map frame中的位姿。因此消除了漂移，但当新的传感器信息到达时会导致离散跳变。

地图框架作为长期的全局参考是有用的，但是位置估计器中的离散跳跃使得它成为局部感知和行动的一个很差的参考框架。

 

Map Conventions

当建立一个全局坐标系时，应该The default should be to align the x-axis east, y-axis north, and the z-axis up at the origin of the coordinate frame.

如果是多机器人，则应该为

一般base_link固定在机器人上。map与odom frames则是world-fixed帧，其原点一般是机器人的起始坐标。而earth frame提供作为一个共同的参考给多个地图帧

 

robot_localization包的基本介绍

在robot_localization包中包含了两个状态估计节点

• ekf_localization_node 是一个扩展卡尔曼估计器，它使用一个三维测量模型随着时间生成状态，同时利用感知数据校正已经监测过的估计。

• ukf_localization_node 是一个无迹卡尔曼滤波估计器，它使用一系列sigma点通过非线性变换生成状态，并使用这些估计过的sigma点覆盖状态估计点和协方差，这个估计使用雅克比矩阵并使得估计器更加稳定。然而缺点是比ekf_localization_node耗费更大的计算量。

robot_localization包支持任意数量的传感器数据融合。节点不限制输入传感器的数量，比如机器人具有多个IMU或机器人里程计信息，robot_localization中的状态估计节点可以支持所有的传感器。

robot_localization包支持多种ROS msg格式。所有的状态估计节点可以支持以下多种msg

• nav_msgs/Odometry,
• sensor_msgs/Imu
•  geometry_msgs/PoseWithCovarianceStamped
•  geometry_msgs/TwistWithCovarianceStamped 

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允许对单个传感器输入数据定制处理。

续估计。每个状态估计节点在接收到机器人一个测试数据时就开始估计机器人状态。当存在间歇接受的传感数据时（一段周期没有接受数据），机器人会通过内部模型继续状态估计。

 

robot_localization包的使用

Coordinate Frames and Transforming Sensor Data

robot_localization包的状态估计节点会产生一个状态的估计，该估计的位姿在map或者odom frame中，而该状态的速度（velocity）由base_link frame给出。所有输入的数据在状态融合之前，会被转换成这些坐标系中的一个。每个消息类型的数据将会转换成如下所示：

• nav_msgs/Odometry - All pose data (position and orientation) is transformed from the message header’s frame_id into the coordinate frame specified by the world_frame parameter (typically map or odom). In the message itself, this specifically refers to everything contained within the pose property. All twist data (linear and angular velocity) is transformed from the child_frame_id of the message into the coordinate frame specified by the base_link_frame parameter (typically base_link).
• geometry_msgs/PoseWithCovarianceStamped - Handled in the same fashion as the pose data in the Odometry message.
• geometry_msgs/TwistWithCovarianceStamped - Handled in the same fashion as the twist data in the Odometry message.
• sensor_msgs/Imu - The IMU message is currently subject to some ambiguity, though this is being addressed by the ROS community. Most IMUs natively report orientation data in a world-fixed frame whose XX and ZZ axes are defined by the vectors pointing to magnetic north and the center of the earth, respectively, with the Y axis facing east (90 degrees offset from the magnetic north vector). This frame is often referred to as NED (North, East, Down). However, REP-103 specifies an ENU (East, North, Up) coordinate frame for outdoor navigation. As of this writing, robot_localization assumes an ENU frame for all IMU data, and does not work with NED frame data. This may change in the future, but for now, users should ensure that data is transformed to the ENU frame before using it with any node in robot_localization.

Considerations for Each Sensor Message Type

里程计

大多数的机器人平台都会有一个轮式的编码器（wheel encoders），提供瞬时平动和转动速度。许多还将这些速度进行内部整合，以生成位置估计值。

对于速度与位姿： robot_localization可以整合速度与绝对位姿信息。一般来说，最佳的实践是：

• 如果里程计提供位置与线速度，则应该融合线速度
• 如果里程计提供航向角与角速度，则融合航向角

注意： If you have two sources of orientation data, then you’ll want to be careful. If both produce orientations with accurate covariance matrices, it’s safe to fuse the orientations. If, however, one or both under-reports its covariance, then you should only fuse the orientation data from the more accurate sensor. For the other sensor, use the angular velocity (if it’s provided), or continue to fuse the absolute orientation data, but turn _differential mode on for that sensor.

协方差（Covariance）的值对robot_localization非常重要。如果一个sensor没有产生某个变量，那么可以让 robot_pose_ekf忽视那个值就是通过将其方差写得非常大（1e3）从而有效地忽略了所讨论的变量.但这种做法既不必要，甚至不利于robot_localization包。例外情况是，您有第二个输入源来测量所讨论的变量，在这种情况下，膨胀的协方差将起作用。

robot_localization allow users to specify which variables from the measurement should be fused with the current state.  If your sensor reports zero for a given variable and you don’t want to fuse that value with your filter, or if the sensor is known to produce poor data for that field, then simply set its xxxx_config parameter value to false for the variable in question (see the main page for a description of this parameter).

robot_localization包的配置，可以参考：

http://docs.ros.org/en/melodic/api/robot_localization/html/configuring_robot_localization.html#configuring-robot-localization

 

IMU

 

 

 

参考资料

https://github.com/cra-ros-pkg/robot_localization/tree/melodic-devel/src

http://docs.ros.org/melodic/api/robot_localization/html/preparing_sensor_data.html

https://blog.csdn.net/Travis_X/article/details/85225904

https://vimeo.com/142624091

https://www.jianshu.com/p/415b288c8f3d

https://www.ncnynl.com/archives/201708/1910.html