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

什么是機(jī)器人定位robot_localization

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

特性

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

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

所有狀態(tài)估計(jì)節(jié)點(diǎn)跟蹤機(jī)器人15個(gè)狀態(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)估計(jì)節(jié)點(diǎn)

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

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

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

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

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

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

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

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

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

• ~transform_time_offset - 坐標(biāo)變換時(shí)間不長(zhǎng)-一些軟件包需要用一個(gè)少量的時(shí)間補(bǔ)償控制坐標(biāo)變換在將來(lái)的時(shí)間，參數(shù)的值將加入map->odom 或 odom->base_link坐標(biāo)變換的時(shí)間戳。

• ~odomN, twistN, imuN, poseN** - 對(duì)于每一個(gè)傳感器，使用者需要根據(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"/>
  每個(gè)參數(shù)名的索引是基于0開(kāi)始的：(e.g., odom0 , odom1 , etc.) 同時(shí)必須連續(xù)定義 (e.g., 不要在沒(méi)有pose1情況下使用pose0 和pose2). 每個(gè)參數(shù)的值是傳感器的主題名。

• ~odomN_config, ~twistN_config, ~imuN_config, ~poseN_config** - 每個(gè)傳感器選擇相應(yīng)的參數(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. 在這個(gè)例子中，我們將融合 X 和 Y position, yaw, X velocity, 和 yaw velocity. 注意這個(gè)分類在傳感器坐標(biāo)系中完成而不再世界坐標(biāo)系或者機(jī)器人坐標(biāo)系中。參見(jiàn) Sensor Integration tutorial

• ~odomN_queue_size, ~twistN_queue_size, ~imuN_queue_size, poseN_queue_size** -使用這些參數(shù)調(diào)整每個(gè)傳感器callback隊(duì)列長(zhǎng)度，這在當(dāng)頻率參數(shù)比傳感頻率低很多時(shí)非常有用，它允許濾波器吸收周期類的所有傳感數(shù)據(jù)。

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

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

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

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

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

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

• ~debug_out_file -當(dāng)debug是 true, 寫入log的文件。

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

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

4.3 節(jié)點(diǎn)特有參數(shù)
標(biāo)準(zhǔn)和擴(kuò)展的參數(shù)可以用于所以robot_localization節(jié)點(diǎn)，這個(gè)參數(shù)用于特定的節(jié)點(diǎn)。
4.3.1 ukf_localization_node
ukf_localization_node 依照original paper 和wiki article的語(yǔ)法

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

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

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

5. 發(fā)布主題

• odometry/filtered (nav_msgs/Odometry)

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

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

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

其他節(jié)點(diǎn)

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

1.1 Parameters參數(shù)

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

• ~delay - 等待GPS坐標(biāo)系到機(jī)器人坐標(biāo)系的延時(shí)

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

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

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

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

1.2 訂閱的主題

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

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

• gps/fix - 包含GPS坐標(biāo)的 sensor_msgs/NavSatFix 消息

1.3 Published Topics

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

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

1.4 Published Transforms

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