robot_localization

1. 狀態估計節點

1.1 ekf定位節點

ekf_localization_node是一個擴展的卡爾曼濾波器的實現。該方法利用全向運動模型及時預測狀態，並利用感知到的傳感器數據對預測結果進行修正。

1.2 ukf定位節點

ukf_localization_node是一個無跡卡爾曼濾波器的實現。它使用一組精心挑選的西格瑪點，通過在EKF中使用的相同的運動模型來投射狀態，然後使用這些投射的西格瑪點來恢復狀態估計和協方差。這消除了雅可比矩陣的使用，使濾波器更加穩定。但是，它在計算上也比ekf_localization_node更費力。

1.3 參數

ekf_localization_node和ukf_localization_node共享它們的大部分參數，因爲大多數參數控制數據在與核心過濾器融合之前的處理方式。

狀態估計節點可用的參數相對較多，因此啓動和配置文件是啓動任何節點的首選方法。該包包含模板啓動和配置文件，以幫助用戶啓動。

（1）ekf_localization_node和ukf_localization_node的共享參數

[標準參數]

頻率

濾波器產生狀態估計的實值頻率，單位爲Hz。

注意：直到從輸入之一接收到至少一條消息，過濾器纔會開始計算。

傳感器超時

實值週期（以秒爲單位），之後我們認爲任何傳感器都已超時。在這種情況下，我們將對EKF進行預測週期，而無需對其進行校正。可以將此參數視爲濾波器將生成新輸出的最小頻率的倒數。

two_d_mode

如果您的機器人在平面環境中運行，並且您願意忽略地面的細微變化（如IMU所報告），則將其設置爲true。它將爲所有3D變量（Z，側傾，俯仰以及它們各自的速度和加速度）融合0值。這樣可以確保這些值的協方差不會爆炸，同時確保機器人的狀態估算值仍固定在XY平面上。

座標系frame

具體參數

[地圖座標系]
[里程計座標系]
[機器人本體base_link座標系]
[機器人本體輸出座標系]
[世界座標系]

這些參數定義了機器人定位robot_localization的操作“模式”。REP-105指定三個主要座標系：地圖，里程計算和base_link。base_link是固定在機器人上的座標系。機器人在odom框架中的位置會隨着時間而漂移，但是在短期內是準確的，應該是連續的。的地圖幀，像奧多姆幀，是世界固定座標幀，雖然它包含用於你的機器人全球最準確的位置估計，它是受離散跳躍，例如由於GPS數據的融合

。這是使用這些參數的方法：

設置map_frame，odom_frame和base_link_frame參數去對應您的系統中相應的座標框架名稱。

注意：如果您的系統沒有map_frame，則將其刪除，並確保world_frame將其設置爲的值odom_frame。

注意：如果您正在運行多個EKF實例，並且想“覆蓋”輸出轉換和消息以使其具有此框架child_frame_id，則可以進行設置。該base_link_output_frame是可選的，默認爲base_link_frame。當運行多個EKF實例時，這有助於啓用斷開連接的TF樹。當計算最終狀態時，我們“覆蓋”輸出轉換和消息以使其具有該幀child_frame_id。

如果你只融合如車輪編碼器測距，視覺里程，或IMU數據集的連續位置數據world_frame到你的odom_frame價值。這是中狀態估計節點的默認行爲robot_localization，也是最常見的用法。
如果你是全球融合的絕對位置數據，是受離散跳躍（例如，GPS或具有里程碑意義的觀察位置更新），則：

[1] 設定您world_frame的map_frame的值
[2] 確保其他東西正在生成odom->base_link轉換。這甚至可以是robot_localization狀態估計節點的另一個實例。然而，該實例應該不是保險絲全局數據。

map_frame、odom_frame和base_link_frame的默認值分別是map、odom和base_link。base_link_output_frame參數默認爲base_link_frame的值。world_frame參數默認爲odom_frame的值。

transform_time_offset

有些包要求您的轉換以一個小的時間偏移量爲未來日期。該參數的值將被添加到robot_localization中狀態估計節點生成的map->odom或odom->base_link變換的時間戳中。

transform_timeout

robot_localization包使用tf2的lookupTransform方法來請求轉換。此參數指定如果轉換還不可用，我們希望等待多長時間。如果沒有設置，默認值爲0。0意味着我們只獲得最新可用的(參見tf2實現)轉換，因此我們不會阻塞過濾器。指定一個非零的transform_timeout會影響過濾器的計時，因爲它會等待transform_timeout的最大值以使轉換可用。這直接意味着在大多數情況下，指定的期望輸出速率沒有得到滿足，因爲更新時過濾器必須等待轉換。

傳感器

對於每個傳感器，用戶需要根據消息類型定義此參數。例如，如果我們定義一個Imu消息源和兩個測程消息源，配置將如下所示:

<param name="imu0" value="robot/imu/data"/>
<param name="odom0" value="wheel_encoder/odometry"/>
<param name="odom1" value="visual_odometry/odometry"/>

每個參數名的索引都是基於0的(例如，odom0、odom1等)，並且必須按順序定義(例如，如果沒有定義pose1，就不要使用pose0和pose2)。每個參數的值都是該傳感器的主題名稱。

傳感器配置

具體參數：

[odomN_config]
[twistN_config]
[imuN_config]
[poseN_config]

對於上述定義的每個傳感器消息，用戶必須指定應該將這些消息的哪些變量融合到最終狀態估計中。里程測量配置的一個例子可能是這樣的:

<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˙、Y˙、Z˙、roll˙、pitch˙、yaw˙、X¨、Y¨、Z¨。在這個例子中，我們融合了X和Y的位置，偏航角，X˙，和偏航角˙。

注意：規範是在傳感器的frame_id中完成的，而不是在world_frame或base_link_frame中。有關更多信息，請參閱配置教程。

[sensor]_queue_size

具體參數：

- [~odomN_config] 
- [~twistN_config]
- [~imuN_config]
- [~poseN_config]

[sensor]_differential

具體參數：

- [~odomN_differential] 
- [~imuN_differential]
- [~poseN_differential]

對於上面定義的包含姿態信息的每個傳感器消息，用戶可以指定是否應該對姿態變量進行不同的集成。如果給定的值設置爲true，那麼對於有問題的傳感器在t時刻的測量，我們首先減去t−1時刻的測量，然後將結果值轉換爲速度。如果您的機器人有兩個絕對姿態信息來源，例如來自里程計的偏航測量和IMU，則此設置特別有用。在這種情況下，如果輸入源上的方差配置不正確，這些測量值可能彼此不同步，並在過濾器中引起振盪，但是通過對其中一個或兩個進行不同的積分，我們可以避免這種情況。

用戶在使用這個參數進行方向數據時應該小心，因爲轉換爲速度意味着方向狀態變量的協方差將會無限制地增長(除非另一個絕對方向數據的來源正在被融合)。如果你只是想讓所有的姿態變量都從0開始，那麼請使用_relative參數。

注意:如果您通過navsat_transform_node或utm_transform_node融合GPS信息，您應該確保_差分設置爲假。

[sensor]_relative

具體參數：

- [~odomN_relative] 
- [~imuN_relative]
- [~poseN_relative]

如果該參數設置爲true，則來自該傳感器的任何測量值都將與從該傳感器接收到的第一個測量值相融合。如果您希望狀態估計總是從(0,0,0)開始，並且滾、俯仰和偏航值都是(0,0,0)，那麼這是非常有用的。它類似於_微分參數，但是我們總是在0時刻去掉測量值，而不是在t−1時刻去掉測量值，而且測量值沒有轉換爲速度。

imuN_remove_gravitational_acceleration（imu去除重力加速度）

如果融合來自IMUs的加速度計數據，該參數將決定在融合之前是否將重力加速度從加速度測量中移除。

注意：這裏假設提供加速度數據的IMU也產生一個絕對方向。正確消除重力加速度需要方向數據。

gravitational_acceleration

如果imun_remove_gravity/al_acceleration設置爲true，則該參數決定從IMU的線性加速度數據中刪除的重力加速度Z。是9默認9.80665 (m/s^2)

初始狀態

以指定的狀態啓動篩選器。狀態是一個15維的雙精度向量，其順序與傳感器配置相同。例如，要在(5.0,4.0,3.0)的位置啓動機器人，偏航爲1.57，線速度爲(0.1,0.2,0.3)，您將使用:

<rosparam param="initial_state">[5.0,  4.0,  3.0,
                                 0.0,  0.0,  1.57,
                                 0.1,  0.2,  0.3,
                                 0.0,  0.0,  0.0,
                                 0.0,  0.0,  0.0]</rosparam>

發佈tf

如果爲真，狀態估計節點將把轉換從world_frame參數指定的幀發佈到base_link_frame參數指定的幀。默認值爲true。

發佈加速度

如果爲真，狀態估計節點將發佈線性加速狀態。默認值爲false。

print_diagnostics

如果爲真，狀態估計節點將向/diagnostics主題發佈診斷消息。這對於調試配置和傳感器數據非常有用。

高級參數

use_control

如果爲真，則狀態估計節點將偵聽geometry_msgs/Twist消息的cmd_vel主題，並使用該主題生成一個加速項。這一項將用於機器人的狀態預測。這在某些情況下特別有用，即使在給定狀態變量的收斂上有很小的延遲，也會導致應用程序出現問題(例如，在旋轉過程中激光雷達發生移位)。默認值爲false。

注意：來自IMU的線性加速度數據的存在和包含將當前“覆蓋”預測的線性加速度值。

stamped_control

如果爲true，use_control並且也爲true，則查找geometry_msgs / TwistStamped消息，而不是geometry_msgs / Twist消息。

control_timeout

如果use_control設置爲true且在此時間內沒有收到控制命令（以秒爲單位），則基於控制的加速項將停止應用。

control_config

控制cmd_vel消息中的哪些變量用於狀態預測。這些值的順序是X˙、Y˙、Z˙、橫滾˙、俯仰˙、偏航˙。僅在use_control設置爲true時使用。

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

acceleration_limits

你的機器人在每個維度上的加速速度。匹配control_config中的參數順序。僅在use_control設置爲true時使用。

rosparam param="acceleration_limits">[1.3, 0.0, 0.0,
                                       0.0, 0.0, 3.2]</rosparam>

deceleration_limits

您的機器人在每個尺寸上減速的速度。匹配中的參數順序control_config。僅當use_control設置爲true時使用。

acceleration_gains

如果您的機器人無法立即達到其加速度極限，則可以通過這些增益來控制允許的變化。僅當use_control設置爲true時使用。

<rosparam  param = “ acceleration_limits” > [ 0.8，0.0，0.0，0.0，0.0，0.9 
                                       ] </ rosparam>

deceleration_gains

如果您的機器人無法立即達到其減速極限，則可以通過這些增益來控制允許的變化。僅當use_control設置爲true時使用。

smooth_lagged_data

如果您的任何傳感器產生的時間戳數據都比最新的過濾器更新早（更明確地說，如果您有滯後的傳感器數據源），則將此參數設置爲true將在接收到滯後的數據後啓用過濾器恢復到滯後測量之前的最後狀態，然後處理所有測量直到當前時間。這對於來自需要大量CPU使用量以生成姿態估計值的節點（例如，激光掃描匹配器）進行的測量特別有用，因爲它們經常滯後於當前時間。

HISTORY_LENGTH
如果smooth_lagged_data設置爲true，則此參數指定過濾器將保留其狀態和測量歷史記錄的秒數。該值應至少等於滯後測量值與當前時間之間的時間增量。
[sensor]_nodelay

具體參數：

[~odomN_nodelay]
[~twistN_nodelay]
[~imuN_nodelay]
[~poseN_nodelay]

如果爲true，則設置tcpNoDelay 傳輸提示。有證據表明，Nagle的算法與及時接收大消息類型（例如nav_msgs / Odometry消息）有關。將輸入設置爲true會禁用該訂戶的Nagle算法。默認爲false。

[sensor]_threshold（傳感器閾值）

具體參數：

[~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]

如果您的數據存在異常值，請使用這些閾值設置（表示爲馬哈拉諾比斯距離）來控制允許傳感器測量距當前車輛狀態多遠。numeric_limits::max()如果未指定，則每個默認爲。

debug(調試)

布爾標誌，指定是否在調試模式下運行。警告：將其設置爲true將生成大量數據。數據被寫入debug_out_file參數的值。默認爲false。

debug_out_file

如果debug爲true，則將調試輸出寫入其中的文件。

process_noise_covariance

過程噪聲協方差（通常表示爲Q）用於對濾波算法預測階段的不確定性建模。調整可能很困難，並且已作爲參數公開以方便自定義。可以單獨保留此參數，但是通過調整它可以取得更好的結果。通常，相對於輸入消息中給定變量的方差，Q值越大，濾波器將收斂到測量值的速度就越快。

dynamic_process_noise_covariance

如果爲true，將process_noise_covariance根據機器人的速度動態縮放。這很有用，例如，當您希望在機器人靜止時機器人的估計誤差協方差停止增長時。默認爲false。

initial_estimate_covariance

[ 0 ，0 ]X想要將絕對姿態變量的初始協方差值設置爲大數。這是因爲那些誤差將無限制地增長（由於缺少絕對姿態測量來減小誤差），並且以大的值開始將不會使狀態估計受益。

reset_on_time_jump

如果設置爲true且ros::Time::isSimTime()爲true，則當在主題上檢測到時間跳回時，過濾器將重置爲其未初始化狀態。這在處理bag數據時很有用，因爲可以在不重新啓動節點的情況下重新啓動bag。

predict_to_current_time

如果設置爲真，過濾預測並糾正了最新的測量（默認）的時間，但現在也可以預測到當前時間步長。

disabled_at_startup

如果設置爲true，則不會在啓動時運行過濾器。

節點的具體參數

標準和高級參數對於中的所有狀態估計節點都是通用的robot_localization。本節詳細介紹了各自狀態估計節點所特有的參數。

ukf_localization_node

這些參數ukf_localization_node遵循原始論文和Wiki文章的命名法。

〜alpha -控制sigma點的傳播。除非你是熟悉無味卡爾曼濾波器，它可能是最適合這種設置保留其默認值（0.001）。
〜kappa -也控制的西格瑪點的價差。除非您熟悉無味的卡爾曼濾波器，否則最好將此設置保持爲默認值（0）。
〜beta -涉及所述狀態向量的分佈。默認值爲2表示分佈是高斯分佈。像其他參數一樣，除非用戶熟悉無味的卡爾曼濾波器，否則該參數應保持不變。

發佈主題

odometry/filtered（nav_msgs / Odometry）

accel/filtered（geometry_msgs/AccelWithCovarianceStamped）（如果啓用）

發佈的轉換

如果用戶的world_frame參數設置爲的值，則將odom_frame轉換從odom_frame參數給定的幀發佈到參數給定的幀base_link_frame。

如果用戶的world_frame參數設置爲的值，則將map_frame轉換從map_frame參數給定的幀發佈到參數給定的幀odom_frame。

注意：此模式假定另一個節點正在廣播從odom_frame參數給定的幀到參數給定的幀的轉換base_link_frame。這可以是robot_localization狀態估計節點的另一個實例。

服務

set_pose-通過向主題發出geometry_msgs / PoseWithCovarianceStamped消息set_pose，用戶可以手動設置過濾器的狀態。這對於在測試過程中重置過濾器很有用，並允許與進行交互rviz。可選地，狀態估計節點發布SetPose服務，其類型爲robot_localization / SetPose。

2. navsat轉換節點

navsat_transform_node輸入nav_msgs / Odometry消息（通常是ekf_localization_node或的輸出ukf_localization_node），包含對機器人航向的準確估計的sensor_msgs / Imu以及包含GPS數據的sensor_msgs / NavSatFix消息作爲輸入。它會在與您的機器人世界框架一致的座標中生成測距消息。該值可以直接融合到您的狀態估計中。

注意：如果將此節點的輸出與中的任何狀態估計節點融合robot_localization，則應確保該輸入的odomN_differential設置爲false。

2.1 參數

〜頻率

設置爲true時，以Hz爲單位的實值頻率，用於navsat_transform_node檢查新的sensor_msgs / NavSatFix消息，併發布經過過濾的sensor_msgs / NavSatFix。publish_filtered_gps

〜延遲

計算從GPS座標到機器人的世界座標的轉換之前要等待的時間（以秒爲單位）。

〜magnetic_declination_radians

輸入您所在位置的磁偏角。如果您不知道，請訪問http://www.ngdc.noaa.gov/geomag-web（確保將值轉換爲弧度）。如果您的IMU相對於磁北偏向，則需要此參數。

〜yaw_offset

當您向東時，IMU的偏航讀數應爲0。如果不是，請在此處輸入偏移量（desired_value =偏移量+ sensor_raw_value）。例如，如果您的IMU朝北時報告0，就像大多數情況一樣，則此參數爲pi/2（〜1.5707963）。此參數的版本已更改2.2.1。以前，navsat_transform_node假設IMU面向北時讀爲0，所以相應地使用yaw_offset。

zero_altitude

如果爲true，則此節點生成的nav_msgs / Odometry消息的姿態Z值設置爲0。

publish_filtered_gps

如果爲true，navsat_transform_node還將把您的機器人的世界框架（例如map）位置轉換回GPS座標，並在該主題上發佈sensor_msgs / NavSatFix消息/gps/filtered。

broadcast_utm_transform

如果爲true，navsat_transform_node則將廣播UTM網格和輸入里程錶數據的幀之間的轉換。有關更多信息，請參見下面的發佈的轉換。

use_odometry_yaw

如果爲true，navsat_transform_node則不會從IMU數據獲取標題，而是從輸入的里程錶消息獲取標題。如果您的里程錶消息具有在以地球爲參考的框架中指定的方向數據（例如，由磁力計生成的數據），則用戶應注意僅將其設置爲true。另外，如果測距法源是其中的狀態估計節點之一robot_localization，則用戶應至少將一個絕對方位數據源饋入該節點，並且將_differential和_relative參數設置爲false。

-wait_for_datum

如果爲true，navsat_transform_node將等待從以下任一位置獲取數據：

[該datum參數]
[該set_datum服務]
broadcast_utm_transform_as_parent_frame

如果爲true，navsat_transform_node將發佈utm-> world_frame轉換，而不是world_frame-> utm轉換。請注意，要發佈的轉換broadcast_utm_transform還必須設置爲true。

transform_timeout

此參數指定如果尚不可用轉換我們要等待多長時間。如果未設置，則默認爲0。值0表示我們只是獲取了最新的可用（請參閱tf2實現）轉換。

2.2 訂閱的話題

imu/data甲sensor_msgs / IMU與方位數據消息
odometry/filtered一個nav_msgs/里程計的機器人的當前位置的信息。如果您的機器人已經達到一些非零的姿態之後，第一次讀取GPS，則需要這樣做。
gps/fix一個sensor_msgs / NavSatFix包含您的機器人的GPS座標信息

2.3 發佈的話題

odometry/gps甲nav_msgs /里程計座標框架含有你的機器人的GPS座標信息，變換成它的世界。該消息可以直接融合到robot_localization的狀態估計節點中。
gps/filtered（可選）sensor_msgs / NavSatFix消息，其中包含機器人的世界框架位置，已轉換爲GPS座標

2.4 發佈的轉換

world_frame->utm（可選）-如果broadcast_utm_transform參數設置爲 true，則navsat_transform_node計算從 utm框架到frame_id輸入里程錶數據的轉換。默認情況下，使用逆變換將utm框架發佈爲里程錶框架的子代。使用該broadcast_utm_transform_as_parent_frame參數，utm框架將作爲里程錶框架的父項發佈。如果一棵TF樹中有多個機械手，這將很有用。

3.準備與robot_localization一起使用的數據

在開始使用robot_localization中的狀態估計節點之前，用戶必須確保他們的傳感器數據格式良好。每種類型的傳感器數據都有不同的注意事項，建議用戶在嘗試使用robot_localization之前完整閱讀本教程。

如需更多信息，歡迎用戶觀看ROSCon 2015的演示。

3.1 遵守ros標準

要考慮的兩個最重要的ROS REP是：

REP-103（標準計量單位和座標約定）
REP-105（座標框架約定）。

鼓勵不熟悉ROS或狀態估計的用戶閱讀兩個REP，因爲它幾乎肯定會幫助您準備傳感器數據。robot_localization嘗試儘可能地遵守這些標準。

此外，它可能會有益於用戶查看每種受支持的ROS消息類型的規範：

nav_msgs /里程錶
geometry_msgs / PoseWithCovarianceStamped
geometry_msgs / TwistWithCovarianceStamped
sensor_msgs / Imu

3.2 座標系和轉換傳感器數據

REP-105指定了四個主要的座標幀:base_link、odom、map和earth。base_link框架被剛性地附加到機器人上。map和odom幀是世界固定的幀，它們的起點通常與機器人的起始位置一致。地球框架用於爲多個地圖框架(例如，爲分佈在大範圍內的機器人)提供公共參考框架。地球框架與本教程無關。

robot_localization的狀態估計節點產生狀態估計，其位姿在map或odom幀中給出，其速度在base_link幀中給出。所有輸入的數據在與狀態融合之前都被轉換成這些座標系中的一個。每個消息類型中的數據轉換如下:

nav_msgs/Odometry—所有姿態數據(位置和方向)都從消息頭的frame_id轉換爲world_frame參數指定的座標系(通常是map或odom)。在消息本身中，這特別指的是包含在pose屬性中的所有內容。所有twist數據(線速度和角速度)都從消息的child_frame_id轉換爲base_link_frame參數(通常是base_link)指定的座標幀。
geometry_msgs/PoseWithCovarianceStamped-處理方式與測程信息中的姿態數據相同。
geometry_msgs/TwistWithCovarianceStamped-處理方式與里程數信息中的twist數據相同。
sensor_msgs/Imu -IMU的訊息目前有一些含糊不清的地方，不過ROS社區正在解決這個問題。大多數IMUs在一個固定的座標系中報告方向數據，該座標系的X軸和Z軸分別由指向地磁北極和地球中心的矢量定義，Y軸向東(與地磁北極矢量90度偏移)。這個框架通常被稱爲NED(北，東，下)。但是，REP-103爲戶外導航指定了一個ENU(東、北、上)座標系。在撰寫本文時，robot_localization假設所有IMU數據都有ENU幀，並且不使用NED幀數據。這在將來可能會改變，但是現在，用戶應該確保在將數據與robot_localization中的任何節點一起使用之前，將其轉換爲ENU幀。

IMU也可以定位在機器人的“中立”位置以外的位置。例如，用戶可以將IMU安裝在它的一側，或者旋轉它，使它朝向機器人前方以外的方向。這個偏移量通常由從base_link_frame參數到IMU消息的frame_id的靜態轉換指定。robot_localization中的狀態估計節點會自動糾正傳感器的方向，使其數據與base_link_frame參數指定的幀對齊。

3.3 處理tf_prefix

隨着從ROS Indigo 到tf2的遷移，robot_localization仍然允許使用該tf_prefix參數，但是根據tf2，所有frame_id值都將去除任何前導“ /”。

3.4 每種傳感器消息類型的注意事項

3.4.1 里程計

許多機器人平臺都配備了提供瞬時平移和旋轉速度的車輪編碼器。許多人還內部整合了這些速度以生成位置估計。如果您對此數據負責或可以對其進行編輯，請記住以下幾點：

速度/姿勢： robot_localization可以整合速度或絕對姿勢信息。通常，最佳做法是：

如果里程錶同時提供位置和線速度，請融合線速度。
如果里程錶同時提供方向和角速度，請融合方向。

注意：如果您有兩個方向數據來源，那麼您要格外小心。如果兩者都產生具有精確協方差矩陣的方向，則可以安全地融合方向。但是，如果其中一個或兩個都未報告其協方差，則應僅融合來自更精確傳感器的方向數據。對於另一個傳感器，使用角速度（如果已提供），或繼續融合絕對方向數據，但是爲該傳感器打開_differential模式。
2. frame_id：請參見上面有關座標幀和變換的部分。
3. robot_localizationžrobot_pose_ekf1個Ë 3robot_localization。例外情況是您有第二個輸入源測量有問題的變量，在這種情況下，協方差將起作用。

注意：有關更多信息，請參見配置robot_localization和從robot_pose_ekf遷移。
4. 標誌：遵守REP-103意味着您需要確保數據的標誌正確。例如，如果您有一個地面機器人，然後逆時針旋轉它，則其偏航角應增加，並且其偏航速度應爲正。如果將其向前推動，則其X位置應增加，並且其X速度應爲正。
5. 變換：在廣播奧多姆 - > * base_link *變換。當world_frame參數設置爲odom_frame配置文件中參數的值時，robot_localization的狀態估計節點既輸出nav_msgs / Odometry消息中的位置估計，也輸出從其odom_frame參數指定的幀到其base_link_frame參數的轉換。但是，某些機器人驅動程序也會將此測距信息與里程計信息一起廣播。如果用戶要robot_localization負責此轉換，則需要禁用其機器人驅動程序對該廣播的廣播。這通常作爲參數公開。

3.4.2 IMU

除以下內容外，請務必閱讀上述有關座標系和IMU數據轉換的部分。

遵守規範：與里程錶一樣，請確保您的數據符合REP-103和sensor_msgs / Imu規範。仔細檢查數據符號，並確保frame_id值正確。
協方差：遵循里程計的建議，請確保您的協方差有意義。不要使用較大的值來使過濾器忽略給定的變量。將您要忽略的變量的配置設置爲false。
加速：注意加速數據。中的狀態估計節點robot_localization假定將IMU放置在平坦表面上的其中立的右側向上位置中，它將：

Z軸是9.81米每秒的平方。
如果傳感器滾動+90度(左側向上)，Y軸的加速度應該是+9.81米/秒的平方。
如果傳感器傾斜+90度(正面朝下)，X軸的讀數應該是-9.81米/秒方。

3.4.3 PoseWithCovarianceStamped¶

請參閱里程計

3.4.4 TwistWithCovarianceStamped

請參閱里程計

3.5 常見錯誤

輸入數據不符合REP-103。確保所有的值(特別是方向角)在正確的方向上增加和減少。
不正確的frame_id值。速度數據應該在base_link_frame參數給出的框架中報告，或者在速度數據的frame_id和base_link_frame之間存在轉換。
膨脹的協方差。在度量中忽略變量的首選方法是通過odomN_config參數。
失蹤的協方差。如果您已經配置了一個給定的傳感器來將一個給定的變量融合到狀態估計節點中，那麼該值的方差(即， (i,i)處的協方差矩陣值，其中i爲該變量的下標，不應爲0。如果遇到一個被融合的變量的方差爲0，狀態估計節點將在該值上增加一個小的epsilon值(1e−6)。更好的解決方案是讓用戶適當地設置協方差。

4. 配置robot_localization

將傳感器數據合併到任何robot_localization狀態估計節點的位置估計中時，重要的是要提取儘可能多的信息。本教程詳細介紹了傳感器集成的最佳實踐。

有關更多信息，建議用戶觀看ROSCon 2015的演示文稿。

4.1 傳感器配置

即使所討論的消息類型在配置向量中不包含某些變量（例如，<< MsgLink（geometry_msgs / TwistWithCovarianceStamped）>>缺少任何姿勢數據），所有傳感器的配置向量格式也相同。配置向量仍然具有姿勢變量的值）。未使用的變量將被忽略。

注意:配置向量是在輸入消息的frame_id中給出的。例如，考慮一個速度傳感器，它生成一個geometry_msgs/ twistwithcovarimessage，其frame_id爲velocity_sensor_frame。在這個例子中，我們假設存在一個從velocity_sensor_frame到您的機器人的base_link_frame(例如，base_link)的轉換，並且這個轉換會將velocity_sensor_frame中的X速度轉換爲base_link_frame中的Z速度。爲了將來自傳感器的X速度數據包含到濾波器中，配置向量應該將X速度值設爲true，而不是設爲Z˙速度值:

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

注意布爾值的順序是(X,Y,Z,roll,pitch,yaw,X˙，Y˙，Z˙，roll˙，pitch˙，yaw˙，X¨，Y¨，Z¨)。

4.2 以2D操作？

在配置傳感器時要做的第一個決定是，您的機器人是否在平面環境中工作，並且您可以忽略地平面變化的細微影響，就像IMU報告的那樣。如果是，請將two_d_mode參數設置爲true。這有效地將每個測量中的三維位姿變量歸零，並迫使它們在狀態估計中融合。

4.3 融合不可測量的變量

讓我們從一個例子開始。假設你有一個在平面環境中工作的輪式非完整機器人。你的機器人有一些輪子編碼器，用來估計瞬時X速度和絕對姿態信息。此信息在nav_msgs/里程計消息中報告。此外，你的機器人有一個IMU，可以測量轉速、車輛姿態和線性加速度。它的數據在sensor_msgs/Imu消息中報告。由於我們是在平面環境中操作的，所以我們將two_d_mode參數設置爲true。這將自動歸零所有三維變量，如Z，滾，俯仰，它們各自的速度，和Z加速度。我們從這個配置開始:

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

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

首先，平面機器人只需要考慮X, Y, X˙，Y˙，X, Y, Y, yaw, yaw˙。然而，這裏有幾點需要注意。

對於odom0，我們包括X和Y(在世界座標系中報告)，偏航，X˙(在體座標系中報告)，以及偏航˙。但是，除非您的機器人內部使用IMU，否則它很可能只是使用車輪編碼器數據來生成其測量值。因此，它的速度、航向和位置數據都來自同一來源。在這種情況下，我們不希望使用所有的值，因爲您將重複的信息輸入到過濾器中。相反，最好使用速度:

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

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

接下來，我們注意到我們沒有把Y˙融合。乍一看，這是正確的選擇，因爲我們的機器人不能立即橫向移動。但是，如果nav_msgs/Odometry message報告了一個Y˙的0值(並且Y˙的協方差沒有被誇大到很大的值)，那麼最好將這個值反饋給過濾器。在本例中，測量值爲0，表示機器人不可能向該方向移動，因此，測量值爲0是完全有效的:

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

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

你可能會覺得奇怪，爲什麼我們不把Z˙的速度融合在一起呢?答案是，當我們將two_d_mode設置爲false時，確實是這樣。如果我們不這樣做，我們可以將Z˙速度的0度量融合到這個濾波器中。
3. 最後，我們來看IMU。你可能已經注意到我們把Y’'設置爲False了。這是因爲許多系統，包括我們在這裏討論的假設系統，不會經歷瞬時Y加速度。然而，IMU可能會報告Y加速度的非零、噪聲值，這可能導致您的估計值迅速漂移。

4.4 微分和相對參數

“robot_localization”中的狀態估計節點允許用戶融合任意數量的傳感器。這允許用戶使用多個源來測量某些狀態向量變量——特別是姿態變量。例如，您的機器人可能從多個imu獲取絕對方向信息，或者它可能有多個數據源來估計其絕對位置。在這種情況下，用戶有兩個選擇:

按原樣融合所有絕對位置/方向數據，例如:

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

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

在這種情況下，用戶應該非常小心，確保每個測量的方向變量的協方差設置正確。如果每個IMU的偏航方差，例如:math: ’ 0.1 '，而IMUs的偏航測量值之間的差值是:math: ’ > 0.1 '，那麼濾波器的輸出將在每個傳感器提供的值之間來回振盪。用戶應確保每個測量周圍的噪聲分佈重疊。
2. 當然，用戶可以使用_differentiation參數。對於給定的傳感器，將其設置爲true，通過計算兩個連續時間步長的測量值的變化，將所有位姿(位置和方向)數據轉換爲速度。然後數據被融合成一個速度。但是，用戶應該再次注意:當度量完全融合時(特別是imu)，如果度量對於給定的變量有一個靜態或非遞增的方差，那麼估計的協方差矩陣中的方差將是有界的。如果將該信息轉換爲速度，那麼在每個時間步長時，估計值將獲得一些小的誤差，所討論的變量的方差將無限制地增長。對於位置(X,Y,Z)信息，這不是問題，但對於方向數據，這是一個問題。例如，一個機器人在它的環境中移動，一段時間後在X方向上積累1.5米的誤差是可以接受的。如果同一機器人在左右移動時積累了1.5弧度的偏航誤差，那麼當機器人下一次向前行駛時，它的位置誤差將會爆炸。

微分參數的一般經驗法則是，如果給定的機器人只有一個方向數據源，那麼微分參數應該設置爲false。如果有N個源，用戶可以將其中N - 1個源的_微分參數設置爲true，或者簡單地確保協方差值足夠大以消除振盪。

5. 基於robot_localizaion_ekf移植

從robot_pose_ekf遷移非常簡單。此頁面旨在突出包之間的相關差異，以促進快速轉換。

5.1 源消息中的協方差

對於robot_pose_ekf，讓過濾器忽略測量值的一種常見方法是給它一個極大的膨脹協方差，通常是10^3的數量級。然而，robot_localization中的狀態估計節點允許用戶指定測量中的哪些變量應該與當前狀態融合。如果你的傳感器報告零對於一個給定的變量和你不想融合值過濾,或者如果知道傳感器產生糟糕的數據字段,然後簡單地設置其xxxx_config參數值爲false的變量問題的主要頁面(見這個參數的描述)。

但是，用戶應該注意:有時候平臺約束會創建隱式的變量度量。例如，一個不能立即向Y方向移動的差動驅動機器人可以用一個小的協方差值安全地融合Y˙的一個0測量。

5.2 差分參數

默認情況下，robot_pose_ekf將在t時刻進行姿態測量，確定它與t−1時刻的測量之間的差異，將該差異轉換爲當前幀，然後對該測量進行集成。這巧妙地幫助了兩個傳感器測量相同的位姿變量的情況:隨着時間的推移，每個傳感器報告的值將開始出現分歧。如果這些測量值中至少有一個的協方差沒有恰當地增長，濾波器最終將開始在測量值之間振盪。通過微分積分，避免了這種情況，測量值與當前狀態一致。

如果將兩個不同的源用於相同的位姿數據(例如，兩個不同的傳感器測量Z位置)，確保這些源準確地報告它們的協方差。如果兩個源開始出現分歧，那麼它們的協方差應該反映至少一個源中出現的增長誤差。
如果可用，融合速度數據，而不是位姿數據。如果您有兩個測量相同變量的獨立數據源，則將最精確的數據源合併爲姿態數據和速度數據。
作爲(2)的替代方案，如果對於給定的姿態測量無法獲得速度數據，則爲其中一個傳感器啓用_差分參數。這將導致它作爲一個速度被區分和融合。
使用robot_localization狀態估計節點的三種不同方法可以避免這種情況:

6. 集成GPS數據

GPS數據集成是用戶的普遍要求。robot_localization包含一個節點navsat_transform_node，它將GPS數據轉換爲一個幀，該幀與機器人在其世界框架中的起始姿態(位置和方向)一致。這大大簡化了GPS數據的融合。本教程解釋瞭如何使用navsat_transform_node，並深入研究了它背後的一些數學原理。

如需更多信息，歡迎用戶觀看ROSCon 2015的演示。

6.1 關於融合GPS數據的注意事項

在開始本教程之前，用戶應確保自己熟悉REP-105。對於用戶而言，重要的是要意識到，由於GPS數據易受離散的不連續性（“跳躍”）的影響，因此使用包含GPS數據的位置估計可能不適合導航模塊使用。如果要將來自GPS的數據融合到位置估計中，一種可能的解決方案是執行以下操作：

運行robot_localization狀態估計節點的一個實例，該實例僅融合連續數據，例如里程錶和IMU數據。world_frame將此實例的參數設置爲與odom_frame參數相同的值。在此框架中執行本地路徑計劃和動作。
運行robot_localization狀態估計節點的另一個實例，該實例融合所有數據源，包括GPS。world_frame將此實例的參數設置爲與map_frame參數相同的值。

但是，這只是一個建議，用戶可以自由地將GPS數據融合到robot_localization狀態估計節點的單個實例中。

6.2 使用navsat_transform_node

6.2.1 所需輸入

navsat_transform_node需要三個信息源:機器人當前在其世界框架中的姿態估計、一個與地球相關的標題，以及一個以緯度/經度對錶示的地理座標(可選高度)。

這些數據可以通過三種不同的方式獲得:

這些數據可以完全來自機器人的傳感器和姿態估計軟件。要啓用此模式，請確保wait_for_datum參數設置爲false(其默認值)。所需訊息包括:

帶有原始GPS座標的sensor_msgs/NavSatFix消息。
帶有絕對(地球參考)標題的sensor_msgs/Imu消息。
一個nav_msgs/測程信息，包含機器人在其起始位置指定的幀中的當前位置估計(通常是robot_localization狀態估計節點的輸出)。

2.可以通過datum參數指定基準（全局框架原點）。

注意爲了使用此模式，wait_for_datum必須將參數設置爲true。

該datum參數採用以下形式：

<rosparam param="datum">[55.944904, -3.186693, 0.0, map, base_link]</rosparam>

參數順序是緯度(以十進制度表示)、經度(以十進制度表示)、標題(以弧度表示)。，爲robot_localization狀態估計節點中的world_frame參數的值)，爲機器人身體幀的frame_id(即，爲robot_本地化狀態估計節點中的base_link_frame參數的值)。當使用此模式時，機器人將假定您的機器人的世界框架原點位於指定的緯度和經度，且標題爲0(東)。
3. 可以通過set_datum服務和使用robot_localization/SetDatum服務消息手動設置數據。

GPS數據

請注意：navsat_transform_node的所有開發都是使用Garmin 18x GPS單元完成的，因此可能有其他單元生成的複雜數據需要處理。

優秀的nmea_navsat_driver包提供了所需的sensor_msgs/NavSatFix消息。這是我們將在本教程中使用的nmea_navsat_driver啓動文件:

<node pkg="nmea_navsat_driver" type="nmea_serial_driver" name="navsat" respawn="true">
  <param name="port" value="/dev/ttyUSB0"/>
  <param name="baud" value="19200"/>
</node>

只有當用戶沒有通過datum參數或set_datum服務手動指定原點時，此信息纔有意義。

IMU數據

注意:自2.2.1版以來，navsat_transform_node已經轉移到一個標準，其中所有的航向數據都假設從其零點朝東開始。如果您的IMU不符合這個標準，而是報告0時，面向北，您仍然可以使用yaw_offset參數來糾正這個錯誤。在這種情況下,yaw_offset將π/ 2的值(約1.5707963)。

用戶應確保其imu符合REP-105。特別是，檢查你的方向角的符號在正確的方向增加。此外，用戶應該查找機器人工作區域的磁偏角，將其轉換爲弧度，然後將該值用於magnetic_declination_radians參數。

只有當用戶沒有通過datum參數或set_datum服務手動指定原點時，此信息纔有意義。

里程計數據

這應該是用於融合GPS數據的robot_localization狀態估計節點實例的輸出。

6.2.2 配置navsat_transform_node

下面是我們將在本教程中使用的navsat_transform_node啓動文件:

<launch>

  <node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node" respawn="true">

    <param name="magnetic_declination_radians" value="0"/>

    <param name="yaw_offset" value="0"/>

    <remap from="/imu/data" to="/your/imu/topic" />
    <remap from="/gps/fix" to="/your/gps/fix/topic" />
    <remap from="/odometry/filtered" to="/your/robot_localization/output/topic" />

  </node>

</launch>

這些參數將在主頁上討論。

6.2.3 配置robot_localization

此時，與robot_localization的集成非常簡單。簡單地添加這個塊到您的狀態估計節點啓動文件:

<rosparam param="odomN_config">[true,  true,  false,
                                false, false, false,
                                false, false, false,
                                false, false, false,
                                false, false, false]</rosparam>
<param name="odomN_differential" value="false"/>

確保將odomN更改爲您的odometry輸入值(例如，odom1、odom2等)。此外，如果希望包含海拔數據，請將odomN_config的第三個值設置爲true。

注意，如果你是在2D操作沒有任何傳感器測量Z或Z速度，你可以:

將navsat_transform_node的zero_altitude參數設置爲true，然後將odomN_config的第三個值設置爲true
在robot_本地化狀態估計節點中將two_d_mode設置爲true

您應該不需要修改狀態估計節點中的_差分設置。GPS是一個絕對位置傳感器，啓用差分集成違背了使用它的目的。

6.2.4 細節

我們從一張圖片開始。考慮一個從某個經度和緯度開始並帶有某個標題的機器人。在本教程中，我們假設這個標題來自一個面向東方的IMU，它的讀數爲0，並根據ROS規範(即逆時針方向)。本教程的其餘部分將參考圖1:

REP-105建議使用四種座標系:base_link、odom、map和earth。base_link是固定在車輛上的座標系。奧多姆和地圖幀是世界固定的幀，它們通常起源於車輛的起始位置和方向。地球幀被用作多個地圖幀的公共參考幀，目前還不被navsat_transform_node支持。注意，在圖1中，機器人剛剛啓動(t = 0)，因此它的base_link、odom和map幀是對齊的。我們還可以爲UTM網格定義一個座標系，我們稱之爲UTM。出於本教程的目的，我們將把UTM網格座標系稱爲UTM。因此，我們要做的是創建一個utm->map轉換。

參考圖1，這些想法(希望如此)是清楚的。UTM原點是與機器人的GPS定位相關的UTM區域的(0UTM,0UTM)點。機器人開始於UTM區域內的某個位置(xUTM,yUTM)。上面的一些機器人的初始取向角θUTM網格的軸。因此我們變換將要求我們知道xUTM, yUTM和θ。

現在需要將緯度和經度轉換爲UTM座標。UTM網格假設x軸朝東，y軸朝北，z軸指向地面。這符合repo -105所規定的右手座標系。代表還說，偏航角爲0意味着我們正對着x軸向下，偏航是逆時針增加的。navsat_transform_node假設你的標題數據符合這個標準。但是，有兩個因素需要考慮:

IMU驅動程序可能不允許用戶應用磁偏角校正因子
IMU驅動程序朝北時可能報錯0，朝東時可能報錯0(即使它的標題正確地增加和減少)。幸運的是，navsat_transform_node提供了兩個參數來彌補IMU數據中可能存在的缺陷:magnetic_declination_radians和yaw_offset。參考圖1，對於當前正在測量imu_yaw的偏航值的IMU，

    yawimu=−ω−offsetyaw+θ

    θ=yawimu+ω+offsetyaw

我們現在有一個旋轉(xUTM yUTM)和旋轉θ,我們可以使用它來創建所需的utm - >映射變換。我們使用轉換將所有未來的GPS位置轉換爲機器人的局部座標系。如果broadcast_utm_transform參數設置爲true，則navsat_transform_node也將廣播此轉換。

如果您對本教程有任何問題，請在answers.ros.org上隨意提問。

7. 用戶輸入的教程

下面是用戶提供的robot_localization教程列表!

7.1 教程

ros-sensor-fusion-tutorial（ros傳感器融合教程）

一個全面的端到端的教程，爲傳感器融合設置robot_localization，以及運行必要的概念。(包括一個使用超聲波信標設置它的實際示例!)

Kapernikov: ROS robot_localization軟件包

一旦您瞭解了robot_localization包的工作方式，它的文檔就非常清楚了。然而，它缺少一個實際操作的教程來幫助您完成第一步。有一些關於如何設置robot_localization包的很好的例子，但是它們需要好的工作硬件。本教程將使用turtlesim包作爲虛擬機器人，試圖彌補這一不足。

HamRadioDXer666

發佈了41 篇原創文章 · 獲贊 4 · 訪問量 5420

私信關注

ros開源節點robot_localization

robot_localization

1. 狀態估計節點

1.1 ekf定位節點

1.2 ukf定位節點

1.3 參數

（1）ekf_localization_node和ukf_localization_node的共享參數

2. navsat轉換節點

2.1 參數

2.2 訂閱的話題

2.3 發佈的話題

2.4 發佈的轉換

3.準備與robot_localization一起使用的數據

3.1 遵守ros標準

3.2 座標系和轉換傳感器數據

3.3 處理tf_prefix

3.4 每種傳感器消息類型的注意事項

3.4.1 里程計

3.4.2 IMU

3.4.3 PoseWithCovarianceStamped¶

3.4.4 TwistWithCovarianceStamped

3.5 常見錯誤

4. 配置robot_localization

4.1 傳感器配置

4.2 以2D操作？

4.3 融合不可測量的變量

4.4 微分和相對參數

5. 基於robot_localizaion_ekf移植

5.1 源消息中的協方差

5.2 差分參數

6. 集成GPS數據

6.1 關於融合GPS數據的注意事項

6.2 使用navsat_transform_node

6.2.1 所需輸入

6.2.2 配置navsat_transform_node

6.2.3 配置robot_localization

6.2.4 細節

7. 用戶輸入的教程

7.1 教程