参考：http://wiki.ros.org/robot_localization

什么是机器人定位robot_localization

robot_localization是一系列的机器人状态估计节点集合，其中每一个都是用于三维平面的机器人非线性状态估计，它包括两个机器人状态估计节点ekf_localization_node和ukf_localization_node。此外也提供了 navsat_transform_node节点用于整合GPS数据。
更详尽的细节可以查看robot_localization的参考文献

特性

所有的状态估计节点有相似的特性，例如：

• 任意数量的传感器数据融合。节点不限制输入传感器的数量，比如机器人具有多个IMU或机器人里程计信息，robot_localization中的状态估计节点可以支持所有的传感器。
• 支持多种ROS msg格式。所有的状态估计节点可以支持nav_msgs/Odometry, sensor_msgs/Imu, geometry_msgs/PoseWithCovarianceStamped, geometry_msgs/TwistWithCovarianceStamped 等msg。
• 单个传感器的输入定制。如果某个传感器信息包含有希望忽略的估计数据，robot_localization允许对单个传感器输入数据定制处理。
• 持续估计。每个状态估计节点在接收到机器人一个测试数据时就开始估计机器人状态。当存在间歇接受的传感数据时（一段周期没有接受数据），机器人会通过内部模型继续状态估计。

所有状态估计节点跟踪机器人15个状态维度 (x, y, z, roll, pitch, yaw, 他们各自的速度和线加速度)。

robot_localization安装

1. ubuntu软件源安装

    sudo apt-get install ros-indigo-robot-localization
   

2. git 安装

    source /opt/ros/indigo/setup.bash
    cd catkin_ws/src
    git clone https://github.com/cra-ros-pkg/robot_localization --branch indigo-devel
    cd ..
    catkin_make -DCMAKE_BUILD_TYPE=Release
    source catkin_ws/devel/setup.bash
   

状态估计节点

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

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

3. 使用robot_localization的状态估计节点
   在robot_localization在的launch file中每个节点有大量的参数需要配置，软件包中包含了示例launch文件帮助入门。

4. 参数
   robot_localization的状态估计节点具有大量参数可以用于估计计算，参数如下所示，另外，我们提供了示例的sample ekf_template.launch和 ukf_template.launch文件，文件的注释非常清晰。
   4.1 标准参数

• ~frequency - 滤波器生成状态估计的频率，单位hz，注意只有滤波器收到至少一个输入数据时滤波器才开始计算。

• ~sensor_timeout - 传感超时，实测传感器周期，当传感器接受数据超过此数据则认为超时。 这个参数可以认为是滤波器会有输出结果的最小输入频率。

• ~two_d_mode - 如果机器人在平面运行，忽略运行时的细微变化，这是可以设置参数为true。此时不会融合所有的三维变量 (Z, roll, pitch, 他们各自的速度和线加速度)。 这样保证了这些参数的协方差不被影响，从而确保了机器人的状态估计保持在xy平面内。

• ~map_frame, ~odom_frame, ~base_link_frame, ~world_frame - 参数定义了机器人的操作模式，制定了三个基本坐标系： map, odom, 和 base_link。其中base_link 是机器人本体联接的参照坐标系。机器人传感坐标系odom将随着时间漂移然而在短期是准确同时连续的。 map 坐标系是整个地图的参考坐标系，它包含了机器人人全局的准确位置估计，比如GPS数据，它是离散不连续的。下面介绍如何使用这些坐标系：

  1. 设置 map_frame, odom_frame，和 base_link 对应机器人本体坐标系名称的各自参数。
     1.1 当你的系统中没有地图坐标系，则移出它，同时确保世界坐标系由odom坐标系设置。
  2. 如果只融合连续的位置数据（编码器、视觉传感器、IMU）将世界坐标系与传感坐标系设为一个。这是状态估计节点中普遍的用法。
  3. 当融合有不连续的全局的绝对坐标数据（GPS，地标观测坐标）时：
     3.1 将世界坐标系设置为地图坐标系。
     3.2 确保有其他数据生成odom->base_link 坐标变换.注意这个甚至可以是其他估计示例的节点，然而示例不能融合全局数据。
     默认的地图、传感、本体坐标系名称为 map, odom, 和 base_link, 世界坐标系的参数默认为odom。

• ~transform_time_offset - 坐标变换时间不长-一些软件包需要用一个少量的时间补偿控制坐标变换在将来的时间，参数的值将加入map->odom 或 odom->base_link坐标变换的时间戳。

• ~odomN, twistN, imuN, poseN** - 对于每一个传感器，使用者需要根据msg类型定义参数，例如，定义一种imu数据，两种odom数据，配置如下所示：
  <param name="imu0" value="robot/imu/data"/>
  <param name="odom0" value="wheel_encoder/odometry"/>
  <param name="odom1" value="visual_odometry/odometry"/>
  每个参数名的索引是基于0开始的：(e.g., odom0 , odom1 , etc.) 同时必须连续定义 (e.g., 不要在没有pose1情况下使用pose0 和pose2). 每个参数的值是传感器的主题名。

• ~odomN_config, ~twistN_config, ~imuN_config, ~poseN_config** - 每个传感器选择相应的参数值用于最终的数据融合，odom的示例如下所示：

    <rosparam param="odom0_config">
      [true, true, false,  false, false, true,  true, false,
         false,  false, false, true, false, false, false]
    </rosparam>
  

其中的布尔值是X, Y, Z, roll, pitch, yaw, X velocity, Y velocity, Z velocity, roll velocity, pitch velocity, yaw velocity, X acceleration, Y acceleration, 和 Z acceleration. 在这个例子中，我们将融合 X 和 Y position, yaw, X velocity, 和 yaw velocity. 注意这个分类在传感器坐标系中完成而不再世界坐标系或者机器人坐标系中。参见 Sensor Integration tutorial

• ~odomN_queue_size, ~twistN_queue_size, ~imuN_queue_size, poseN_queue_size** -使用这些参数调整每个传感器callback队列长度，这在当频率参数比传感频率低很多时非常有用，它允许滤波器吸收周期类的所有传感数据。

• ~odomN_differential, ~imuN_differential, ~poseN_differential** - （当有多个传感器采集同样的绝对位置坐标时，设置为true防止多个估计变化的跳动。） 每一个传感数据定义在包含pose信息之上，用户可以选择pose数据是否有差异的吸收，如果一个值是true这时对于在t时刻的测量，我们先减去t-1时间测试数据并将结果转换为速度，这个设置在当机器人有两个绝对姿态信息是非常有用，例如： e.g., yaw 是由 odometry 和 IMU测试的来. 在这个示例中，当输入数据的差异不能正确处理，将会导致两个测量结果不同步，同时滤波器会产生震荡，但是整合两个不同数据我们将避免这个震荡。
  <param name="odom0_differential" value="true"/>
  在使用初始数据时用户应当非常谨慎，因为转换到速度意味着方向角状态变化的协方差将会无边界的增长 (除非另一个数据已经融合). 如果只是希望所有姿态从0开始，可以使用相对参数（ _relative）注意 如果通过navsat_transform_node或者 utm_transform_node融合GPS数据需要确保 _differential参数是 false

• ~odomN_relative, ~imuN_relative, ~poseN_relative** - ( 理解为采用相对位置还是绝对位置 ) 当参数设置为true所有的测量数据将相对于首次测量值融合，这在适用于数据是 (0, 0, 0) 并且 roll, pitch, and yaw 也是 (0, 0, 0). 它与_differential 参数相似，但是与_differential中移除 t-1时间的测量不同，我们在time 0 时刻就移除数据，测试数据也不能转换为速度。

• ~imuN_remove_gravitational_acceleration** -当融合IMU的加速度数据，参数决定了是否移除重力影响，注意，它假定了IMU即产生加速度数据也产生方向数据，方向数据需要去除重力影响校正。

• ~print_diagnostics -当为正时，状态估计节点将生成诊断数据，这个可以用于debug。

4.2 扩展参数

• ~odomN_pose_rejection_threshold, ~odomN_twist_rejection_threshold, ~poseN_rejection_threshold, ~twistN_rejection_threshold, ~imuN_pose_rejection_threshold, ~imuN_angular_velocity_rejection_threshold, ~imuN_linear_acceleration_rejection_threshold** -当数据服从于外部传感器，使用这些阀值设置 Mahalanobis distances控制当前状态下传感测量值的许可范围，没指定的话每一个都是tonumeric_limits<double>::max()

• ~debug - 选择是否运行debug模式。注意为真将产生大量的数据，数据写入debug_out_file里面。

• ~debug_out_file -当debug是 true, 写入log的文件。

• ~process_noise_covariance - 噪音方差通常表示为 Q, 用于不确定模型的滤波算法预测. 它很难协调，将它设为参数用于定制化，参数可以单独设置，调整它可以获得较好的参数。

• ~initial_estimate_covariance - 估计方差表示为 P, 定义了当前状态估计误差。参数允许用户设置矩阵初始值，这将影响滤波器收敛速度，例如，当使用者设置 position [0, 0] 到一个非常小的值（e.g., 1e-12）, 这时尝试融合x位置，这时候滤波器将比较缓慢的采纳这些测量数据，融合时间需要增加。用户应该谨慎使用这个参数，当只融合速度数据时(e.g., 没有绝对姿态信息), 使用者不要设置其实方差值，这是因为误差将会无限增长无助于状态估计。

4.3 节点特有参数
标准和扩展的参数可以用于所以robot_localization节点，这个参数用于特定的节点。
4.3.1 ukf_localization_node
ukf_localization_node 依照original paper 和wiki article的语法

• ~alpha - 控制sigma点的传播，除非熟悉卡尔曼滤波器，否则就设置为默认值(0.001).

• ~kappa - 控制sigma点的传播，通常用默认值(0).

• ~beta - 相对状态矢量的分布. 默认是2代表高斯分布

5. 发布主题

• odometry/filtered (nav_msgs/Odometry)

6. 发布的坐标变换

• 当世界坐标系设置为传感坐标系，传感坐标系会发布一个传感、机器人本体坐标系的坐标变换。
• 当用户的世界坐标系设置为地图坐标系，坐标变换是地图坐标系发布到传感坐标系。这个模式假定其他节点是由传感坐标系发布到本体坐标系的。这可以成本另一个机器人定位的示例。

7. Manual State Reset手动状态重置
   通过分配geometry_msgs/PoseWithCovarianceStamped消息到set_pose主题, 用户可以手动设置滤波器的状态，这在重置滤波器的过程中非常有用，可以在测试过程中重置滤波器，并且可以与rviz交互。此外同样，状态估计节点发布一个SetPose服务，服务类型是robot_localization/SetPose。

其他节点

1. navsat_transform_node
   navsat_transform_node输入 nav_msgs/Odometry 消息 (通常是ekf_localization_node或ukf_localization_node的输出), 和一个包含准确的机器人朝向估计的 sensor_msgs/Imu ，还有一个包含GPS数据的sensor_msgs/NavSatFix 消息。它生成一个世界坐标系的里程消息。 注意当将这个节点的输出与其他节点数据融合时，应该确保odomN_differential设置是false。

1.1 Parameters参数

• magnetic_declination_radians - 机器人的磁偏角。不知道的话参见http://www.ngdc.noaa.gov/geomag-web/ (确认转换为弧度).

• ~delay - 等待GPS坐标系到机器人坐标系的延时

• ~yaw_offset -IMU 应该在地磁北的情况下是yaw角是0. 当不是这个数据就需要设置偏移。(desired_value = offset + sensor_raw_value).

• ~zero_altitude - 为 true时nav_msgs/Odometry 消息Z pose值为0。

• ~publish_filtered_gps - 为true, navsat_transform_node会将世界坐标系坐标转换到在GPS/gps/filtered主题生成 sensor_msgs/NavSatFix 消息。

• ~broadcast_utm_transform -为 true, navsat_transform_node将生成UTM与传感数据的变换，详见后面的变换发布。
  will broadcast the transform between the UTM grid and the frame of the input odometry data. See Published Transforms below for more information.

• ~use_odometry_yaw -为 true, navsat_transform_node将不会取得IMU的朝向数据，而是从里程计中获取。只有当里程计具有类似全局坐标系中的朝向数据才可以设置为true（比如磁力计），此外，如果里程计信息是状态估计节点中的一个，用户至少有一个绝对的坐标的朝向，此时_differential和 _relative设置为 false。

1.2 订阅的主题

• imu/data - 具有朝向数据的sensor_msgs/Imu 消息

• odometry/filtered - 当前位置的 nav_msgs/Odometry 消息，这需要首次GPS数据读取之前本体先取得一些非零姿态。

• gps/fix - 包含GPS坐标的 sensor_msgs/NavSatFix 消息

1.3 Published Topics

• odometry/gps - 包含转换为世界坐标系的GPS数据 的nav_msgs/Odometry消息 . 消息可以直接融合进估计节点。

• gps/filtered - 将机器人世界坐标系位置转换为GPS坐标系中的 sensor_msgs/NavSatFix消息

1.4 Published Transforms

• world_frame->utm (可选) - 当 the broadcast_utm_transform设置为 true, navsat_transform_node计算 utm 坐标系到传感坐标系的转换. 然而, 通常将 utm坐标系设置为odometry的子坐标系。