ROS学习笔记之——robot_localization包
之前博客已經(jīng)介紹過(guò)robot_pose_ekf功能包以及（extended）kalman濾波的原理《?ROS學(xué)習(xí)筆記之——EKF （Extended Kalman Filter） node 擴(kuò)展卡爾曼濾波》本博文看看robot_localization包中的EKF
更多關(guān)于卡爾曼濾波，可以參考下面資料：
http://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/
以及博客《學(xué)習(xí)筆記之——卡爾曼濾波》
包的源碼
https://github.com/cra-ros-pkg/robot_localization
?
目錄
遵守ROS標(biāo)準(zhǔn)
ROS中標(biāo)準(zhǔn)測(cè)量單位和坐標(biāo)約定
ROS中移動(dòng)平臺(tái)坐標(biāo)系
base_link
odom
map
Map Conventions
robot_localization包的基本介紹
robot_localization包的使用
Coordinate Frames and Transforming Sensor Data
Considerations for Each Sensor Message Type
里程計(jì)
IMU
參考資料
?
遵守ROS標(biāo)準(zhǔn)
在使用robot_localization中的狀態(tài)估計(jì)節(jié)點(diǎn)開始之前，用戶必須確保其傳感器數(shù)據(jù)格式正確。
其實(shí)對(duì)于在ROS中采用的代碼，其傳遞的狀態(tài)信息的數(shù)據(jù)，本身就應(yīng)該跟ROS相匹配。下面來(lái)看看
ROS中標(biāo)準(zhǔn)測(cè)量單位和坐標(biāo)約定
?https://www.ros.org/reps/rep-0103.html
單元和約定的不一致是開發(fā)人員集成問(wèn)題的常見來(lái)源，也可能導(dǎo)致軟件錯(cuò)誤。由于數(shù)據(jù)轉(zhuǎn)換，它也會(huì)產(chǎn)生不必要的計(jì)算。
基本的單位
衍生單位
坐標(biāo)幀的轉(zhuǎn)換
- 采用右手定則
- 旋轉(zhuǎn)表示法（四元數(shù)）
- 協(xié)方差的表示
?
ROS中移動(dòng)平臺(tái)坐標(biāo)系
?https://www.ros.org/reps/rep-0105.html
這文檔指定與ROS一起使用的移動(dòng)平臺(tái)坐標(biāo)系的命名約定和語(yǔ)義含義
坐標(biāo)幀主要有以下幾種
base_link
?base_link 坐標(biāo)系與移動(dòng)機(jī)器人底座剛性連接。?base_link可以在任何位置或方向附著在底座上。對(duì)于每個(gè)硬件平臺(tái)，在?base上都會(huì)有一個(gè)不同的位置，提供一個(gè)明顯的參考點(diǎn)
odom
odom是應(yīng)該世界固定框架（?world-fixed frame）。在odom frame中，移動(dòng)平臺(tái)的姿態(tài)可以隨時(shí)間漂移，沒(méi)有任何邊界。這一飄逸導(dǎo)致odom frame在長(zhǎng)期的全局參考中沒(méi)有意義。然而，機(jī)器人在odom frame中的位姿可以保證是連續(xù)的，意味著odom frame中的移動(dòng)平臺(tái)的位姿總是以平滑的方式進(jìn)化，沒(méi)有離散的跳躍。
典型的odom幀的設(shè)置是基于里程計(jì)源計(jì)算，如車輪里程計(jì)、可視里程計(jì)或慣性測(cè)量裝置。
odom作為一個(gè)精確的、短期的局部參考是有用的，但是漂移使它成為一個(gè)不適合長(zhǎng)期參考的框架
map
map坐標(biāo)幀是世界坐標(biāo)系。Z軸垂直向上。移動(dòng)平臺(tái)的位姿與map幀相關(guān)，不應(yīng)隨著時(shí)間顯著飄移。map幀不是連續(xù)的，以為著移動(dòng)平臺(tái)在map幀上的的位姿將會(huì)在任何時(shí)間離散的改變。
在典型的設(shè)置中，定位的組件會(huì)跟蹤傳感器的觀測(cè)（sensor observation）不斷的計(jì)算機(jī)器人在map frame中的位姿。因此消除了漂移，但當(dāng)新的傳感器信息到達(dá)時(shí)會(huì)導(dǎo)致離散跳變。
地圖框架作為長(zhǎng)期的全局參考是有用的，但是位置估計(jì)器中的離散跳躍使得它成為局部感知和行動(dòng)的一個(gè)很差的參考框架。
?
Map Conventions
當(dāng)建立一個(gè)全局坐標(biāo)系時(shí)，應(yīng)該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.
如果是多機(jī)器人，則應(yīng)該為
一般base_link固定在機(jī)器人上。map與odom frames則是world-fixed幀，其原點(diǎn)一般是機(jī)器人的起始坐標(biāo)。而earth frame提供作為一個(gè)共同的參考給多個(gè)地圖幀
?
robot_localization包的基本介紹
在robot_localization包中包含了兩個(gè)狀態(tài)估計(jì)節(jié)點(diǎn)
- ekf_localization_node?是一個(gè)擴(kuò)展卡爾曼估計(jì)器，它使用一個(gè)三維測(cè)量模型隨著時(shí)間生成狀態(tài)，同時(shí)利用感知數(shù)據(jù)校正已經(jīng)監(jiān)測(cè)過(guò)的估計(jì)。
-
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ì)算量。
robot_localization包支持任意數(shù)量的傳感器數(shù)據(jù)融合。節(jié)點(diǎn)不限制輸入傳感器的數(shù)量，比如機(jī)器人具有多個(gè)IMU或機(jī)器人里程計(jì)信息，robot_localization中的狀態(tài)估計(jì)節(jié)點(diǎn)可以支持所有的傳感器。
robot_localization包支持多種ROS msg格式。所有的狀態(tài)估計(jì)節(jié)點(diǎn)可以支持以下多種msg
- nav_msgs/Odometry,
- sensor_msgs/Imu
- ?geometry_msgs/PoseWithCovarianceStamped
- ?geometry_msgs/TwistWithCovarianceStamped?
robot_localization包使用15維向量來(lái)表示機(jī)器人的運(yùn)動(dòng)狀態(tài)：
每3個(gè)數(shù)據(jù)一組，分別表示：
- x-y-z坐標(biāo)系的坐標(biāo)（機(jī)器人位置）
- 繞x/y/z軸的角度（機(jī)器人方向）
- 沿x/y/z軸的速度
- 繞x/y/z軸的角速度
- 沿x/y/z軸的加速度
robot_localization常常被用在兩種典型的場(chǎng)景:
- 融合連續(xù)的傳感器數(shù)據(jù)（里程計(jì)和IMU）創(chuàng)建局部精確的狀態(tài)估計(jì)。也即適合應(yīng)用于使用多種位置、方向的傳感器融合的場(chǎng)合，可以做出精確的局部位姿估計(jì)
- 融合連續(xù)的傳感器數(shù)據(jù)及全局位姿估計(jì)來(lái)提供精確而完整的全局狀態(tài)估計(jì)。如果再加上一些全局state的話（來(lái)自于其他的全局傳感器或數(shù)據(jù)）可以實(shí)現(xiàn)對(duì)全局的狀態(tài)估計(jì)。
robot_localization的典型用法應(yīng)該是配合機(jī)器人導(dǎo)航模塊，實(shí)現(xiàn)各種sensor的融合以及精確的路線導(dǎo)航 。
如果某個(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í)就開始估計(jì)機(jī)器人狀態(tài)。當(dāng)存在間歇接受的傳感數(shù)據(jù)時(shí)（一段周期沒(méi)有接受數(shù)據(jù)），機(jī)器人會(huì)通過(guò)內(nèi)部模型繼續(xù)狀態(tài)估計(jì)。
?
robot_localization包的使用
Coordinate Frames and Transforming Sensor Data
robot_localization包的狀態(tài)估計(jì)節(jié)點(diǎn)會(huì)產(chǎn)生一個(gè)狀態(tài)的估計(jì)，該估計(jì)的位姿在map或者odom frame中，而該狀態(tài)的速度（velocity）由base_link frame給出。所有輸入的數(shù)據(jù)在狀態(tài)融合之前，會(huì)被轉(zhuǎn)換成這些坐標(biāo)系中的一個(gè)。每個(gè)消息類型的數(shù)據(jù)將會(huì)轉(zhuǎn)換成如下所示：
- 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
里程計(jì)
大多數(shù)的機(jī)器人平臺(tái)都會(huì)有一個(gè)輪式的編碼器（wheel encoders），提供瞬時(shí)平動(dòng)和轉(zhuǎn)動(dòng)速度。許多還將這些速度進(jìn)行內(nèi)部整合，以生成位置估計(jì)值。
對(duì)于速度與位姿：?robot_localization可以整合速度與絕對(duì)位姿信息。一般來(lái)說(shuō)，最佳的實(shí)踐是：
- 如果里程計(jì)提供位置與線速度，則應(yīng)該融合線速度
- 如果里程計(jì)提供航向角與角速度，則融合航向角
注意：?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）的值對(duì)robot_localization非常重要。如果一個(gè)sensor沒(méi)有產(chǎn)生某個(gè)變量，那么可以讓?robot_pose_ekf忽視那個(gè)值就是通過(guò)將其方差寫得非常大（1e3）從而有效地忽略了所討論的變量.但這種做法既不必要，甚至不利于robot_localization包。例外情況是，您有第二個(gè)輸入源來(lái)測(cè)量所討論的變量，在這種情況下，膨脹的協(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
?
總結(jié)
以上是生活随笔為你收集整理的ROS学习笔记之——robot_localization包的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問(wèn)題。
- 上一篇： 微信小程序云开发云数据库_聚合操作_联表
- 下一篇： 打印机不能正常打印怎么办