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

什么是機器人定位robot_localization

robot_localization是一系列的機器人狀態(tài)估計節(jié)點集合，其中每一個都是用于三維平面的機器人非線性狀態(tài)估計，它包括兩個機器人狀態(tài)估計節(jié)點ekf_localization_node和ukf_localization_node。此外也提供了 navsat_transform_node節(jié)點用于整合GPS數(shù)據(jù)。
更詳盡的細節(jié)可以查看robot_localization的參考文獻

特性

所有的狀態(tài)估計節(jié)點有相似的特性，例如：

• 任意數(shù)量的傳感器數(shù)據(jù)融合。節(jié)點不限制輸入傳感器的數(shù)量，比如機器人具有多個IMU或機器人里程計信息，robot_localization中的狀態(tài)估計節(jié)點可以支持所有的傳感器。
• 支持多種ROS msg格式。所有的狀態(tài)估計節(jié)點可以支持nav_msgs/Odometry, sensor_msgs/Imu, geometry_msgs/PoseWithCovarianceStamped, geometry_msgs/TwistWithCovarianceStamped 等msg。
• 單個傳感器的輸入定制。如果某個傳感器信息包含有希望忽略的估計數(shù)據(jù)，robot_localization允許對單個傳感器輸入數(shù)據(jù)定制處理。
• 持續(xù)估計。每個狀態(tài)估計節(jié)點在接收到機器人一個測試數(shù)據(jù)時就開始估計機器人狀態(tài)。當存在間歇接受的傳感數(shù)據(jù)時（一段周期沒有接受數(shù)據(jù)），機器人會通過內(nèi)部模型繼續(xù)狀態(tài)估計。

所有狀態(tài)估計節(jié)點跟蹤機器人15個狀態(tài)維度 (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
   

狀態(tài)估計節(jié)點

1. ekf_localization_node
   ekf_localization_node是一個擴展卡爾曼估計器，它使用一個三維測量模型隨著時間生成狀態(tài)，同時利用感知數(shù)據(jù)校正已經(jīng)監(jiān)測過的估計。

2. ukf_localization_node
   ukf_localization_node是一個無跡卡爾曼濾波估計器，它使用一系列sigma點通過非線性變換生成狀態(tài)，并使用這些估計過的sigma點覆蓋狀態(tài)估計點和協(xié)方差，這個估計使用雅克比矩陣并使得估計器更加穩(wěn)定。然而缺點是比ekf_localization_node耗費更大的計算量。

3. 使用robot_localization的狀態(tài)估計節(jié)點
   在robot_localization在的launch file中每個節(jié)點有大量的參數(shù)需要配置，軟件包中包含了示例launch文件幫助入門。

4. 參數(shù)
   robot_localization的狀態(tài)估計節(jié)點具有大量參數(shù)可以用于估計計算，參數(shù)如下所示，另外，我們提供了示例的sample ekf_template.launch和 ukf_template.launch文件，文件的注釋非常清晰。
   4.1 標準參數(shù)

• ~frequency - 濾波器生成狀態(tài)估計的頻率，單位hz，注意只有濾波器收到至少一個輸入數(shù)據(jù)時濾波器才開始計算。

• ~sensor_timeout - 傳感超時，實測傳感器周期，當傳感器接受數(shù)據(jù)超過此數(shù)據(jù)則認為超時。 這個參數(shù)可以認為是濾波器會有輸出結(jié)果的最小輸入頻率。

• ~two_d_mode - 如果機器人在平面運行，忽略運行時的細微變化，這是可以設置參數(shù)為true。此時不會融合所有的三維變量 (Z, roll, pitch, 他們各自的速度和線加速度)。 這樣保證了這些參數(shù)的協(xié)方差不被影響，從而確保了機器人的狀態(tài)估計保持在xy平面內(nèi)。

• ~map_frame, ~odom_frame, ~base_link_frame, ~world_frame - 參數(shù)定義了機器人的操作模式，制定了三個基本坐標系： map, odom, 和 base_link。其中base_link 是機器人本體聯(lián)接的參照坐標系。機器人傳感坐標系odom將隨著時間漂移然而在短期是準確同時連續(xù)的。 map 坐標系是整個地圖的參考坐標系，它包含了機器人人全局的準確位置估計，比如GPS數(shù)據(jù)，它是離散不連續(xù)的。下面介紹如何使用這些坐標系：

  1. 設置 map_frame, odom_frame，和 base_link 對應機器人本體坐標系名稱的各自參數(shù)。
     1.1 當你的系統(tǒng)中沒有地圖坐標系，則移出它，同時確保世界坐標系由odom坐標系設置。
  2. 如果只融合連續(xù)的位置數(shù)據(jù)（編碼器、視覺傳感器、IMU）將世界坐標系與傳感坐標系設為一個。這是狀態(tài)估計節(jié)點中普遍的用法。
  3. 當融合有不連續(xù)的全局的絕對坐標數(shù)據(jù)（GPS，地標觀測坐標）時：
     3.1 將世界坐標系設置為地圖坐標系。
     3.2 確保有其他數(shù)據(jù)生成odom->base_link 坐標變換.注意這個甚至可以是其他估計示例的節(jié)點，然而示例不能融合全局數(shù)據(jù)。
     默認的地圖、傳感、本體坐標系名稱為 map, odom, 和 base_link, 世界坐標系的參數(shù)默認為odom。

• ~transform_time_offset - 坐標變換時間不長-一些軟件包需要用一個少量的時間補償控制坐標變換在將來的時間，參數(shù)的值將加入map->odom 或 odom->base_link坐標變換的時間戳。

• ~odomN, twistN, imuN, poseN** - 對于每一個傳感器，使用者需要根據(jù)msg類型定義參數(shù)，例如，定義一種imu數(shù)據(jù)，兩種odom數(shù)據(jù)，配置如下所示：
  <param name="imu0" value="robot/imu/data"/>
  <param name="odom0" value="wheel_encoder/odometry"/>
  <param name="odom1" value="visual_odometry/odometry"/>
  每個參數(shù)名的索引是基于0開始的：(e.g., odom0 , odom1 , etc.) 同時必須連續(xù)定義 (e.g., 不要在沒有pose1情況下使用pose0 和pose2). 每個參數(shù)的值是傳感器的主題名。

• ~odomN_config, ~twistN_config, ~imuN_config, ~poseN_config** - 每個傳感器選擇相應的參數(shù)值用于最終的數(shù)據(jù)融合，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** -使用這些參數(shù)調(diào)整每個傳感器callback隊列長度，這在當頻率參數(shù)比傳感頻率低很多時非常有用，它允許濾波器吸收周期類的所有傳感數(shù)據(jù)。

• ~odomN_differential, ~imuN_differential, ~poseN_differential** - （當有多個傳感器采集同樣的絕對位置坐標時，設置為true防止多個估計變化的跳動。） 每一個傳感數(shù)據(jù)定義在包含pose信息之上，用戶可以選擇pose數(shù)據(jù)是否有差異的吸收，如果一個值是true這時對于在t時刻的測量，我們先減去t-1時間測試數(shù)據(jù)并將結(jié)果轉(zhuǎn)換為速度，這個設置在當機器人有兩個絕對姿態(tài)信息是非常有用，例如： e.g., yaw 是由 odometry 和 IMU測試的來. 在這個示例中，當輸入數(shù)據(jù)的差異不能正確處理，將會導致兩個測量結(jié)果不同步，同時濾波器會產(chǎn)生震蕩，但是整合兩個不同數(shù)據(jù)我們將避免這個震蕩。
  <param name="odom0_differential" value="true"/>
  在使用初始數(shù)據(jù)時用戶應當非常謹慎，因為轉(zhuǎn)換到速度意味著方向角狀態(tài)變化的協(xié)方差將會無邊界的增長 (除非另一個數(shù)據(jù)已經(jīng)融合). 如果只是希望所有姿態(tài)從0開始，可以使用相對參數(shù)（ _relative）注意 如果通過navsat_transform_node或者 utm_transform_node融合GPS數(shù)據(jù)需要確保 _differential參數(shù)是 false

• ~odomN_relative, ~imuN_relative, ~poseN_relative** - ( 理解為采用相對位置還是絕對位置 ) 當參數(shù)設置為true所有的測量數(shù)據(jù)將相對于首次測量值融合，這在適用于數(shù)據(jù)是 (0, 0, 0) 并且 roll, pitch, and yaw 也是 (0, 0, 0). 它與_differential 參數(shù)相似，但是與_differential中移除 t-1時間的測量不同，我們在time 0 時刻就移除數(shù)據(jù)，測試數(shù)據(jù)也不能轉(zhuǎn)換為速度。

• ~imuN_remove_gravitational_acceleration** -當融合IMU的加速度數(shù)據(jù)，參數(shù)決定了是否移除重力影響，注意，它假定了IMU即產(chǎn)生加速度數(shù)據(jù)也產(chǎn)生方向數(shù)據(jù)，方向數(shù)據(jù)需要去除重力影響校正。

• ~print_diagnostics -當為正時，狀態(tài)估計節(jié)點將生成診斷數(shù)據(jù)，這個可以用于debug。

4.2 擴展參數(shù)

• ~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** -當數(shù)據(jù)服從于外部傳感器，使用這些閥值設置 Mahalanobis distances控制當前狀態(tài)下傳感測量值的許可范圍，沒指定的話每一個都是tonumeric_limits<double>::max()

• ~debug - 選擇是否運行debug模式。注意為真將產(chǎn)生大量的數(shù)據(jù)，數(shù)據(jù)寫入debug_out_file里面。

• ~debug_out_file -當debug是 true, 寫入log的文件。

• ~process_noise_covariance - 噪音方差通常表示為 Q, 用于不確定模型的濾波算法預測. 它很難協(xié)調(diào)，將它設為參數(shù)用于定制化，參數(shù)可以單獨設置，調(diào)整它可以獲得較好的參數(shù)。

• ~initial_estimate_covariance - 估計方差表示為 P, 定義了當前狀態(tài)估計誤差。參數(shù)允許用戶設置矩陣初始值，這將影響濾波器收斂速度，例如，當使用者設置 position [0, 0] 到一個非常小的值（e.g., 1e-12）, 這時嘗試融合x位置，這時候濾波器將比較緩慢的采納這些測量數(shù)據(jù)，融合時間需要增加。用戶應該謹慎使用這個參數(shù)，當只融合速度數(shù)據(jù)時(e.g., 沒有絕對姿態(tài)信息), 使用者不要設置其實方差值，這是因為誤差將會無限增長無助于狀態(tài)估計。

4.3 節(jié)點特有參數(shù)
標準和擴展的參數(shù)可以用于所以robot_localization節(jié)點，這個參數(shù)用于特定的節(jié)點。
4.3.1 ukf_localization_node
ukf_localization_node 依照original paper 和wiki article的語法

• ~alpha - 控制sigma點的傳播，除非熟悉卡爾曼濾波器，否則就設置為默認值(0.001).

• ~kappa - 控制sigma點的傳播，通常用默認值(0).

• ~beta - 相對狀態(tài)矢量的分布. 默認是2代表高斯分布

5. 發(fā)布主題

• odometry/filtered (nav_msgs/Odometry)

6. 發(fā)布的坐標變換

• 當世界坐標系設置為傳感坐標系，傳感坐標系會發(fā)布一個傳感、機器人本體坐標系的坐標變換。
• 當用戶的世界坐標系設置為地圖坐標系，坐標變換是地圖坐標系發(fā)布到傳感坐標系。這個模式假定其他節(jié)點是由傳感坐標系發(fā)布到本體坐標系的。這可以成本另一個機器人定位的示例。

7. Manual State Reset手動狀態(tài)重置
   通過分配geometry_msgs/PoseWithCovarianceStamped消息到set_pose主題, 用戶可以手動設置濾波器的狀態(tài)，這在重置濾波器的過程中非常有用，可以在測試過程中重置濾波器，并且可以與rviz交互。此外同樣，狀態(tài)估計節(jié)點發(fā)布一個SetPose服務，服務類型是robot_localization/SetPose。

其他節(jié)點

1. navsat_transform_node
   navsat_transform_node輸入 nav_msgs/Odometry 消息 (通常是ekf_localization_node或ukf_localization_node的輸出), 和一個包含準確的機器人朝向估計的 sensor_msgs/Imu ，還有一個包含GPS數(shù)據(jù)的sensor_msgs/NavSatFix 消息。它生成一個世界坐標系的里程消息。 注意當將這個節(jié)點的輸出與其他節(jié)點數(shù)據(jù)融合時，應該確保odomN_differential設置是false。

1.1 Parameters參數(shù)

• magnetic_declination_radians - 機器人的磁偏角。不知道的話參見http://www.ngdc.noaa.gov/geomag-web/ (確認轉(zhuǎn)換為弧度).

• ~delay - 等待GPS坐標系到機器人坐標系的延時

• ~yaw_offset -IMU 應該在地磁北的情況下是yaw角是0. 當不是這個數(shù)據(jù)就需要設置偏移。(desired_value = offset + sensor_raw_value).

• ~zero_altitude - 為 true時nav_msgs/Odometry 消息Z pose值為0。

• ~publish_filtered_gps - 為true, navsat_transform_node會將世界坐標系坐標轉(zhuǎn)換到在GPS/gps/filtered主題生成 sensor_msgs/NavSatFix 消息。

• ~broadcast_utm_transform -為 true, navsat_transform_node將生成UTM與傳感數(shù)據(jù)的變換，詳見后面的變換發(fā)布。
  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的朝向數(shù)據(jù)，而是從里程計中獲取。只有當里程計具有類似全局坐標系中的朝向數(shù)據(jù)才可以設置為true（比如磁力計），此外，如果里程計信息是狀態(tài)估計節(jié)點中的一個，用戶至少有一個絕對的坐標的朝向，此時_differential和 _relative設置為 false。

1.2 訂閱的主題

• imu/data - 具有朝向數(shù)據(jù)的sensor_msgs/Imu 消息

• odometry/filtered - 當前位置的 nav_msgs/Odometry 消息，這需要首次GPS數(shù)據(jù)讀取之前本體先取得一些非零姿態(tài)。

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

1.3 Published Topics

• odometry/gps - 包含轉(zhuǎn)換為世界坐標系的GPS數(shù)據(jù) 的nav_msgs/Odometry消息 . 消息可以直接融合進估計節(jié)點。

• gps/filtered - 將機器人世界坐標系位置轉(zhuǎn)換為GPS坐標系中的 sensor_msgs/NavSatFix消息

1.4 Published Transforms

• world_frame->utm (可選) - 當 the broadcast_utm_transform設置為 true, navsat_transform_node計算 utm 坐標系到傳感坐標系的轉(zhuǎn)換. 然而, 通常將 utm坐標系設置為odometry的子坐標系。