之前博客已經(jīng)介紹過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中的狀態(tài)估計節(jié)點開始之前，用戶必須確保其傳感器數(shù)據(jù)格式正確。

其實對于在ROS中采用的代碼，其傳遞的狀態(tài)信息的數(shù)據(jù)，本身就應該跟ROS相匹配。下面來看看

ROS中標準測量單位和坐標約定

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

單元和約定的不一致是開發(fā)人員集成問題的常見來源，也可能導致軟件錯誤。由于數(shù)據(jù)轉換，它也會產(chǎn)生不必要的計算。

基本的單位

衍生單位

坐標幀的轉換

• 采用右手定則
• 旋轉表示法（四元數(shù)）
• 協(xié)方差的表示

?

ROS中移動平臺坐標系

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

這文檔指定與ROS一起使用的移動平臺坐標系的命名約定和語義含義

坐標幀主要有以下幾種

base_link

?base_link 坐標系與移動機器人底座剛性連接。?base_link可以在任何位置或方向附著在底座上。對于每個硬件平臺，在?base上都會有一個不同的位置，提供一個明顯的參考點

odom

odom是應該世界固定框架（?world-fixed frame）。在odom frame中，移動平臺的姿態(tài)可以隨時間漂移，沒有任何邊界。這一飄逸導致odom frame在長期的全局參考中沒有意義。然而，機器人在odom frame中的位姿可以保證是連續(xù)的，意味著odom frame中的移動平臺的位姿總是以平滑的方式進化，沒有離散的跳躍。

典型的odom幀的設置是基于里程計源計算，如車輪里程計、可視里程計或慣性測量裝置。

odom作為一個精確的、短期的局部參考是有用的，但是漂移使它成為一個不適合長期參考的框架

map

map坐標幀是世界坐標系。Z軸垂直向上。移動平臺的位姿與map幀相關，不應隨著時間顯著飄移。map幀不是連續(xù)的，以為著移動平臺在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包中包含了兩個狀態(tài)估計節(jié)點

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

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

robot_localization包支持任意數(shù)量的傳感器數(shù)據(jù)融合。節(jié)點不限制輸入傳感器的數(shù)量，比如機器人具有多個IMU或機器人里程計信息，robot_localization中的狀態(tài)估計節(jié)點可以支持所有的傳感器。

robot_localization包支持多種ROS msg格式。所有的狀態(tài)估計節(jié)點可以支持以下多種msg

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

robot_localization包使用15維向量來表示機器人的運動狀態(tài)：

每3個數(shù)據(jù)一組，分別表示：

• x-y-z坐標系的坐標（機器人位置）
• 繞x/y/z軸的角度（機器人方向）
• 沿x/y/z軸的速度
• 繞x/y/z軸的角速度
• 沿x/y/z軸的加速度

robot_localization常常被用在兩種典型的場景:

• 融合連續(xù)的傳感器數(shù)據(jù)（里程計和IMU）創(chuàng)建局部精確的狀態(tài)估計。也即適合應用于使用多種位置、方向的傳感器融合的場合，可以做出精確的局部位姿估計
• 融合連續(xù)的傳感器數(shù)據(jù)及全局位姿估計來提供精確而完整的全局狀態(tài)估計。如果再加上一些全局state的話（來自于其他的全局傳感器或數(shù)據(jù)）可以實現(xiàn)對全局的狀態(tài)估計。

robot_localization的典型用法應該是配合機器人導航模塊，實現(xiàn)各種sensor的融合以及精確的路線導航 。

如果某個傳感器信息包含有希望忽略的估計數(shù)據(jù)，robot_localization允許對單個傳感器輸入數(shù)據(jù)定制處理。

續(xù)估計。每個狀態(tài)估計節(jié)點在接收到機器人一個測試數(shù)據(jù)時就開始估計機器人狀態(tài)。當存在間歇接受的傳感數(shù)據(jù)時（一段周期沒有接受數(shù)據(jù)），機器人會通過內部模型繼續(xù)狀態(tài)估計。

?

robot_localization包的使用

Coordinate Frames and Transforming Sensor Data

robot_localization包的狀態(tài)估計節(jié)點會產(chǎn)生一個狀態(tài)的估計，該估計的位姿在map或者odom frame中，而該狀態(tài)的速度（velocity）由base_link frame給出。所有輸入的數(shù)據(jù)在狀態(tài)融合之前，會被轉換成這些坐標系中的一個。每個消息類型的數(shù)據(jù)將會轉換成如下所示：

• 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

里程計

大多數(shù)的機器人平臺都會有一個輪式的編碼器（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.

協(xié)方差（Covariance）的值對robot_localization非常重要。如果一個sensor沒有產(chǎn)生某個變量，那么可以讓?robot_pose_ekf忽視那個值就是通過將其方差寫得非常大（1e3）從而有效地忽略了所討論的變量.但這種做法既不必要，甚至不利于robot_localization包。例外情況是，您有第二個輸入源來測量所討論的變量，在這種情況下，膨脹的協(xié)方差將起作用。

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

?

總結

以上是生活随笔為你收集整理的ROS学习笔记之——robot_localization包的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內容還不錯，歡迎將生活随笔推薦給好友。