ROS2 robot-localization配置方案

來源：古月居

robot-localization

robot_localization是一個(gè)ROS的包，基于卡爾曼濾波，對多種傳感器進(jìn)行數(shù)據(jù)融合，進(jìn)而完成機(jī)器人的定位。近期確實(shí)遇到了需要實(shí)現(xiàn)更高精度定位的問題，在多方考慮下其中一部分優(yōu)化方法就是引入了robot-localization庫，這個(gè)庫的引入還是比較簡單的，但是實(shí)際上的操作方式還是和大家分享一下。

硬件平臺

此處硬件平臺其實(shí)并不局限，這里以OriginBot為例好了

OriginBot（導(dǎo)航版）

Ubuntu 20.04

使用方式

安裝robot-localization

基本有兩種方式了。

一種是使用二進(jìn)制方式下載

sudo apt install ros-foxy-robot-localization

這種方式可以直接把包拿來使用，比較方便而且也是最快上手的方式了

第二種是直接拉源碼包出來用。

這里需要說明的是，類似這種官方源碼包如果在不熟悉的情況下取改動(dòng)的話出錯(cuò)的概率還是很大的，所以推薦第一種使用方式。

下載完成之后無論如何請執(zhí)行養(yǎng)成良好的工程習(xí)慣

source /opt/ros/foxy/setup.bash

配置robot-localization

說起配置，其實(shí)是一件很簡單的事情了。說回來robot-localization，本質(zhì)上拿到的是兩組數(shù)據(jù)，一組為odom（里程計(jì)數(shù)據(jù)）、一組為IMU（陀螺儀數(shù)據(jù)），然后將兩組數(shù)據(jù)利用卡爾曼濾波的方式去預(yù)測當(dāng)前真實(shí)的位置。

如何去配置呢？一般的工科做法當(dāng)然是盡可能使用配置文件來做。所以就有了ekf.yaml文件，ekf翻譯過來就是擴(kuò)展卡爾曼濾波?！救绻凑瘴业睦斫?，擴(kuò)展卡爾曼濾波相對于卡爾曼濾波的差別就是可以進(jìn)一步處理非線性的數(shù)據(jù)】

ekf.yaml文件的內(nèi)容如下：

  ###ekf配置文件###ekf_filter_node: ros__parameters:


  # The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin


  # computation until it receives at least one message from one of the inputs. It will then run continuously at the


  # frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified.


  # 濾波器輸出位置估計(jì)的頻率，單位為Hz。請注意，過濾器在從其中一個(gè)輸入接收到至少一條消息之前不會(huì)開始計(jì)算。


  # 然后，無論是否接收到更多的測量值，它都將以此處指定的頻率連續(xù)運(yùn)行。如果未指定，則默認(rèn)為30。


      frequency: 30.0


  # The period, in seconds, after which we consider a sensor to have timed out. In this event, we carry out a predict


  # cycle on the EKF without correcting it. This parameter can be thought of as the minimum frequency with which the


  # filter will generate new output. Defaults to 1 / frequency if not specified.


  # 我們認(rèn)為傳感器超時(shí)的時(shí)間段（以秒為單位）。在這種情況下，我們在EKF上執(zhí)行一個(gè)預(yù)測周期，而不進(jìn)行校正。


  # 這個(gè)參數(shù)可以被認(rèn)為是濾波器產(chǎn)生新輸出的最小頻率。如果未指定，則默認(rèn)為1/頻率。


      sensor_timeout: 0.1


  # ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is


  # set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar


  # environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected


  # by, for example, an IMU. Defaults to false if unspecified.


  # ekf_localization_node and ukf_localization_node都使用3D全向運(yùn)動(dòng)模型。


  # 如果此參數(shù)設(shè)置為true，則不會(huì)在狀態(tài)估計(jì)中使用任何三維信息。


  # 如果您在平面環(huán)境中操作，并且希望忽略地平面中可能檢測到的微小變化的影響，請使用此選項(xiàng)


  # 例如通過IMU。如果未指定，則默認(rèn)為false。


      two_d_mode: true


  # Use this parameter to provide an offset to the transform generated by ekf_localization_node. This can be used for


  # future dating the transform, which is required for interaction with some other packages. Defaults to 0.0 if


  # unspecified.


  # 使用此參數(shù)可以為ekf_location_node生成的變換提供偏移。這可以用于未來的日期轉(zhuǎn)換，這是與其他一些包交互所必需的。


  # 如果未指定，則默認(rèn)為0.0。


      transform_time_offset: 0.0


  # Use this parameter to provide specify how long the tf listener should wait for a transform to become available.


  # Defaults to 0.0 if unspecified.


  #使用此參數(shù)可以指定tf偵聽器應(yīng)等待轉(zhuǎn)換可用的時(shí)間。#如果未指定，則默認(rèn)為0.0。 transform_timeout: 0.0


  # If you're having trouble, try setting this to true, and then echo the /diagnostics_agg topic to see if the node is


  # unhappy with any settings or data.


  #如果遇到問題，請嘗試將其設(shè)置為true，然后echo the /diagnostics_agg主題，查看節(jié)點(diǎn)是否對任何設(shè)置或數(shù)據(jù)不滿意 print_diagnostics: true


  # Debug settings. Not for the faint of heart. Outputs a ludicrous amount of information to the file specified by


  # debug_out_file. I hope you like matrices! Please note that setting this to true will have strongly deleterious


  # effects on the performance of the node. Defaults to false if unspecified.


  # 調(diào)試設(shè)置。不適合膽小的人。向debug_out_file指定的文件輸出數(shù)量驚人的信息。我希望你喜歡矩陣！


  # 請注意，將此設(shè)置為true將對節(jié)點(diǎn)的性能產(chǎn)生嚴(yán)重的有害影響。如果未指定，則默認(rèn)為false。


      debug: false


  # Defaults to "robot_localization_debug.txt" if unspecified. Please specify the full path.


  #如果未指定，則默認(rèn)為“robot_location_debug.txt”。請指定完整路徑。 debug_out_file: /path/to/debug/file.txt


  # Whether we'll allow old measurements to cause a re-publication of the updated state


  # 我們是否允許舊的測量結(jié)果導(dǎo)致更新狀態(tài)的重新發(fā)布


      permit_corrected_publication: false


  # Whether to publish the acceleration state. Defaults to false if unspecified.


  #是否發(fā)布加速狀態(tài)。如果未指定，則默認(rèn)為false。 publish_acceleration: false


  # Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified.


  #是否在/tf主題上廣播轉(zhuǎn)換。如果未指定，則默認(rèn)為true。 publish_tf: false


  # REP-105 (http://www.ros.org/reps/rep-0105.html) specifies four principal coordinate frames: base_link, odom, map, and


  # earth. base_link is the coordinate frame that is affixed to the robot. Both odom and map are world-fixed frames.


  # The robot's position in the odom frame will drift over time, but is accurate in the short term and should be


  # continuous. The odom frame is therefore the best frame for executing local motion plans. The map frame, like the odom


  # frame, is a world-fixed coordinate frame, and while it contains the most globally accurate position estimate for your


  # robot, it is subject to discrete jumps, e.g., due to the fusion of GPS data or a correction from a map-based


  # localization node. The earth frame is used to relate multiple map frames by giving them a common reference frame.


  # ekf_localization_node and ukf_localization_node are not concerned with the earth frame.


  # Here is how to use the following settings:


  # REP-105(http://www.ros.org/reps/rep-0105.html)指定四個(gè)主要坐標(biāo)系：base_link、odom、map和earth。


  # base_link是固定在機(jī)器人上的坐標(biāo)系。odom和map都是world-fixed frames。


  # 機(jī)器人在odom框架中的位置會(huì)隨著時(shí)間的推移而漂移，但在短期內(nèi)是準(zhǔn)確的，應(yīng)該是連續(xù)的。因此，odom幀是用于執(zhí)行局部運(yùn)動(dòng)計(jì)劃的最佳幀。


  # 與odom框架一樣，map框架是一個(gè)世界固定的坐標(biāo)框架，雖然它包含了機(jī)器人最全局準(zhǔn)確的位置估計(jì)，但它會(huì)受到離散跳躍的影響，


  # 例如，由于GPS數(shù)據(jù)的融合或基于地圖的定位節(jié)點(diǎn)的校正。地球坐標(biāo)系用于通過給多個(gè)地圖坐標(biāo)系提供一個(gè)公共參考坐標(biāo)系來關(guān)聯(lián)它們。


  # ekf_location_node和ukf_localization_node與map幀無關(guān)。


  #以下是如何使用以下設(shè)置：


  # 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system.


  # 1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of


  # odom_frame.


  # 1.將map_frame、odom_frame和base_link幀設(shè)置為系統(tǒng)的適當(dāng)幀名稱。


  # 1a. 如果您的系統(tǒng)沒有map_frame，只需將其刪除，并確?！皐orld_frame”設(shè)置為odom_frame的值。


  # 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set


  # "world_frame" to your odom_frame value. This is the default behavior for robot_localization's state estimation nodes.


  # 2. 如果要融合連續(xù)位置數(shù)據(jù)，如車輪編碼器里程計(jì)、視覺里程計(jì)或IMU數(shù)據(jù)，請將“world_frame”設(shè)置為odom_frame值。這是robot_location的狀態(tài)估計(jì)節(jié)點(diǎn)的默認(rèn)行為。


  # 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates


  # from landmark observations) then:


  # 3a. Set your "world_frame" to your map_frame value


  # 3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state


  # estimation node from robot_localization! However, that instance should *not* fuse the global data.


  # 3. 如果您正在融合受離散跳躍影響的全球絕對位置數(shù)據(jù)（例如，GPS或地標(biāo)的位置更新觀察），然后：


  # 3a. 將“world_frame”設(shè)置為map_frame值


  # 3b. 確保其他東西正在生成odom->base_link變換。請注意，這甚至可以是robot_location的另一個(gè)狀態(tài)估計(jì)節(jié)點(diǎn)！但是，該實(shí)例不應(yīng)該融合全局?jǐn)?shù)據(jù)。


      map_frame: map       # 如果未指定，則默認(rèn)為“map”
      odom_frame: odom      # 如果未指定，則默認(rèn)為"odom"
      base_link_frame: base_link # 如果未指定，則默認(rèn)為"base_link"
      world_frame: odom      # 如果未指定，則默認(rèn)為"odom"


  # The filter accepts an arbitrary number of inputs from each input message type (nav_msgs/Odometry,


  # geometry_msgs/PoseWithCovarianceStamped, geometry_msgs/TwistWithCovarianceStamped,


  # sensor_msgs/Imu). To add an input, simply append the next number in the sequence to its "base" name, e.g., odom0,


  # odom1, twist0, twist1, imu0, imu1, imu2, etc. The value should be the topic name. These parameters obviously have no


  # default values, and must be specified.


  # 過濾器接受來自每個(gè)輸入消息類型的任意數(shù)量的輸入（nav_msgs/Odometry、geometry_msgs/PoseWithCovarianceStamped、


  # geometry_msgs/TwistWithCovariance Stamped,sensor_msgs/Imu）。


  #要添加輸入，只需將序列中的下一個(gè)數(shù)字附加到其“base”名稱，例如odom0、odom1、twist0、twist1、imu0、imu1、imu2等。#該值應(yīng)為topic名稱。這些參數(shù)顯然沒有默認(rèn)值，必須指定。 odom0: /odom_diff


  # Each sensor reading updates some or all of the filter's state. These options give you greater control over which


  # values from each measurement are fed to the filter. For example, if you have an odometry message as input, but only


  # want to use its Z position value, then set the entire vector to false, except for the third entry. The order of the


  # values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Note that not some message types


  # do not provide some of the state variables estimated by the filter. For example, a TwistWithCovarianceStamped message


  # has no pose information, so the first six values would be meaningless in that case. Each vector defaults to all false


  # if unspecified, effectively making this parameter required for each sensor.


  # 每個(gè)傳感器讀數(shù)都會(huì)更新過濾器的部分或全部狀態(tài)。這些選項(xiàng)使您能夠更好地控制將每個(gè)測量值輸入到過濾器的值。


  # 例如，如果您有里程計(jì)消息作為輸入，但只想使用其Z位置值，則將整個(gè)矢量設(shè)置為false，第三個(gè)條目除外。


  # 值的順序?yàn)閤、y、z、roll、pitch、yaw、vx、vy、vz、vroll、vpitch、vyaw、ax、ay、az。


  # 請注意，并非某些消息類型不提供過濾器估計(jì)的某些狀態(tài)變量。例如，TwistWithCovarianceStamped消息沒有姿勢信息，


  # 因此在這種情況下，前六個(gè)值將毫無意義。如果未指定，則每個(gè)向量默認(rèn)為全假，從而有效地使每個(gè)傳感器都需要此參數(shù)。


      odom0_config: [false, false, false, #x-y-z坐標(biāo)系的坐標(biāo)（機(jī)器人位置）
              false, false, false, #繞x/y/z軸的角度（機(jī)器人方向）、
              true, false, false, #沿x/y/z軸的線速度、
              false, false, true, #繞x/y/z軸的角速度、
              false, false, false] #沿x/y/z軸的加速度。


  # If you have high-frequency data or are running with a low frequency parameter value, then you may want to increase


  # the size of the subscription queue so that more measurements are fused.


  #如果您有高頻數(shù)據(jù)或使用低頻參數(shù)值運(yùn)行，則可能需要增加訂閱隊(duì)列的大小，以便融合更多的測量值。 odom0_queue_size: 20


  # [ADVANCED] Large messages in ROS can exhibit strange behavior when they arrive at a high frequency. This is a result


  # of Nagle's algorithm. This option tells the ROS subscriber to use the tcpNoDelay option, which disables Nagle's


  # algorithm.


  # [高級]ROS中的大消息在高頻到達(dá)時(shí)會(huì)表現(xiàn)出奇怪的行為。這是Nagle算法的結(jié)果。


  # 此選項(xiàng)告訴ROS訂戶使用tcpNoDelay選項(xiàng)，該選項(xiàng)禁用Nagle的算法。


      odom0_nodelay: false


  # [ADVANCED] When measuring one pose variable with two sensors, a situation can arise in which both sensors under-


  # report their covariances. This can lead to the filter rapidly jumping back and forth between each measurement as they


  # arrive. In these cases, it often makes sense to (a) correct the measurement covariances, or (b) if velocity is also


  # measured by one of the sensors, let one sensor measure pose, and the other velocity. However, doing (a) or (b) isn't


  # always feasible, and so we expose the differential parameter. When differential mode is enabled, all absolute pose


  # data is converted to velocity data by differentiating the absolute pose measurements. These velocities are then


  # integrated as usual. NOTE: this only applies to sensors that provide pose measurements; setting differential to true


  # for twist measurements has no effect.


  # [高級]當(dāng)用兩個(gè)傳感器測量一個(gè)姿態(tài)變量時(shí)，可能會(huì)出現(xiàn)兩個(gè)傳感器都報(bào)告不足的情況。這可能導(dǎo)致濾波器在每次測量到達(dá)時(shí)快速來回跳躍。


  # 在這些情況下，（a）校正測量協(xié)變量，或者（b）如果其中一個(gè)傳感器也測量速度，則讓一個(gè)傳感器測量姿態(tài)，而另一個(gè)傳感器則測量速度，


  # 這通常是有意義的。然而，做（a）或（b）并不總是可行的，因此我們公開了微分參數(shù)。啟用差分模式時(shí)，通過對絕對姿態(tài)測量值進(jìn)行微分，


  # 將所有絕對姿態(tài)數(shù)據(jù)轉(zhuǎn)換為速度數(shù)據(jù)。然后像往常一樣對這些速度進(jìn)行積分。注：這僅適用于提供姿態(tài)測量的傳感器；


  # 將twist測量的微分設(shè)置為true沒有效果。


      odom0_differential: false


  # [ADVANCED] When the node starts, if this parameter is true, then the first measurement is treated as a "zero point"


  # for all future measurements. While you can achieve the same effect with the differential paremeter, the key


  # difference is that the relative parameter doesn't cause the measurement to be converted to a velocity before


  # integrating it. If you simply want your measurements to start at 0 for a given sensor, set this to true.


  # [ADVANCED]當(dāng)節(jié)點(diǎn)啟動(dòng)時(shí)，如果此參數(shù)為真，則第一次測量將被視為所有未來測量的“零點(diǎn)”。雖然你可以用微分參數(shù)計(jì)獲得同樣的效果，


  # 但關(guān)鍵的區(qū)別在于，相對參數(shù)不會(huì)導(dǎo)致測量在積分之前轉(zhuǎn)換為速度。如果你只是想讓給定傳感器的測量從0開始，請將其設(shè)置為true。


      odom0_relative: false


  # [ADVANCED] If your data is subject to outliers, use these threshold settings, expressed as Mahalanobis distances, to


  # control how far away from the current vehicle state a sensor measurement is permitted to be. Each defaults to


  # numeric_limits::max() if unspecified. It is strongly recommended that these parameters be removed if not


  # required. Data is specified at the level of pose and twist variables, rather than for each variable in isolation.


  # For messages that have both pose and twist data, the parameter specifies to which part of the message we are applying


  # the thresholds.


  # [ADVANCED]如果您的數(shù)據(jù)存在異常值，請使用這些閾值設(shè)置（表示為Mahalanobis距離）來控制允許傳感器測量距離當(dāng)前車輛狀態(tài)的距離。


  # 如果未指定，則每個(gè)閾值都默認(rèn)為numeric_limits＜double＞：：max（）。如果不需要，強(qiáng)烈建議刪除這些參數(shù)。


  # 數(shù)據(jù)是在姿勢和扭曲變量級別指定的，而不是單獨(dú)為每個(gè)變量指定的。對于同時(shí)具有姿勢和扭曲數(shù)據(jù)的消息，


  # 該參數(shù)指定我們將閾值應(yīng)用于消息的哪一部分。


      odom0_pose_rejection_threshold: 5.0
      odom0_twist_rejection_threshold: 1.0


  # Further input parameter examples


  # 進(jìn)一步的輸入?yún)?shù)示例


      # odom1: example/odom2
      # odom1_config: [false, false, true,
      #        false, false, false,
      #        false, false, false,
      #        false, false, true,
      #        false, false, false]
      # odom1_differential: false
      # odom1_relative: true
      # odom1_queue_size: 2
      # odom1_pose_rejection_threshold: 2.0
      # odom1_twist_rejection_threshold: 0.2
      # odom1_nodelay: false
      # pose0: example/pose
      # pose0_config: [true, true, false,
      #        false, false, false,
      #        false, false, false,
      #        false, false, false,
      #        false, false, false]
      # pose0_differential: true
      # pose0_relative: false
      # pose0_queue_size: 5
      # pose0_rejection_threshold: 2.0 # 注意參數(shù)名稱的差異
      # pose0_nodelay: false


      # twist0: example/twist
      # twist0_config: [false, false, false,
      #         false, false, false,
      #         true, true, true,
      #         false, false, false,
      #         false, false, false]
      # twist0_queue_size: 3
      # twist0_rejection_threshold: 2.0
      # twist0_nodelay: false


      imu0: /imu_data


      imu0_config: [false, false, false,  #x-y-z坐標(biāo)系的坐標(biāo)（機(jī)器人位置）
             false, false, false, #繞x/y/z軸的角度（機(jī)器人方向）
             false, false, false,  #沿x/y/z軸的線速度
             false, false, true,   #繞x/y/z軸的角速度
             true, true, false]  #沿x/y/z軸的加速度       
      imu0_nodelay: false
      imu0_differential: false
      imu0_relative: false
      imu0_queue_size: 20
      imu0_pose_rejection_threshold: 0.8         # 注意參數(shù)名稱的差異
      imu0_twist_rejection_threshold: 0.8        #
      imu0_linear_acceleration_rejection_threshold: 0.8 #


  # [ADVANCED] Some IMUs automatically remove acceleration due to gravity, and others don't. If yours doesn't, please set


  # this to true, and *make sure* your data conforms to REP-103, specifically, that the data is in ENU frame.


  # [高級]一些IMU會(huì)自動(dòng)消除重力造成的加速度，而另一些則不會(huì)。如果您的數(shù)據(jù)不符合，請將其設(shè)置為true，


  # 并*確保*您的數(shù)據(jù)符合REP-103，特別是數(shù)據(jù)在ENU框架中。


      imu0_remove_gravitational_acceleration: false


  # [ADVANCED] The EKF and UKF models follow a standard predict/correct cycle. During prediction, if there is no


  # acceleration reference, the velocity at time t+1 is simply predicted to be the same as the velocity at time t. During


  # correction, this predicted value is fused with the measured value to produce the new velocity estimate. This can be


  # problematic, as the final velocity will effectively be a weighted average of the old velocity and the new one. When


  # this velocity is the integrated into a new pose, the result can be sluggish covergence. This effect is especially


  # noticeable with LIDAR data during rotations. To get around it, users can try inflating the process_noise_covariance


  # for the velocity variable in question, or decrease the variance of the variable in question in the measurement


  # itself. In addition, users can also take advantage of the control command being issued to the robot at the time we


  # make the prediction. If control is used, it will get converted into an acceleration term, which will be used during


  # predicition. Note that if an acceleration measurement for the variable in question is available from one of the


  # inputs, the control term will be ignored.


  # Whether or not we use the control input during predicition. Defaults to false.


  # [高級]EKF和UKF模型遵循標(biāo)準(zhǔn)的預(yù)測/校正周期。在預(yù)測期間，如果沒有加速度參考，則簡單地將時(shí)間t+1處的速度預(yù)測為與時(shí)間t處的速度相同。


  # 在校正期間，將該預(yù)測值與測量值融合以產(chǎn)生新的速度估計(jì)。這可能是有問題的，因?yàn)樽罱K速度實(shí)際上是舊速度和新速度的加權(quán)平均值。


  # 當(dāng)這個(gè)速度被整合到一個(gè)新的姿勢中時(shí)，結(jié)果可能是緩慢的隱蔽。這種效果在旋轉(zhuǎn)過程中的激光雷達(dá)數(shù)據(jù)中尤為明顯。為了解決這個(gè)問題，


  # 用戶可以嘗試為有問題的速度變量增加process_noise_covariance，或者在測量本身中減少有問題的變量的方差。


  # 此外，用戶還可以利用在我們進(jìn)行預(yù)測時(shí)向機(jī)器人發(fā)出的控制命令。如果使用控制，它將被轉(zhuǎn)換為加速項(xiàng)，該加速項(xiàng)將在預(yù)測過程中使用。


  # 請注意，如果從其中一個(gè)輸入中可以獲得有關(guān)變量的加速度測量值，則控制項(xiàng)將被忽略。


  # 無論我們是否在預(yù)測期間使用控制輸入。默認(rèn)為false。


      use_control: false


  # Whether the input (assumed to be cmd_vel) is a geometry_msgs/Twist or geometry_msgs/TwistStamped message. Defaults to


  # false.


  #輸入（假定為cmd_vel）是geometry_msgs/Twist還是geometry_msgs/TwistStamped消息。默認(rèn)為false。 stamped_control: false


  # The last issued control command will be used in prediction for this period. Defaults to 0.2.


  #最后發(fā)布的控制命令將用于該時(shí)段的預(yù)測。默認(rèn)值為0.2。 control_timeout: 0.2


  # Which velocities are being controlled. Order is vx, vy, vz, vroll, vpitch, vyaw.


  #正在控制哪些速度。順序是vx、vy、vz、vroll、vpitch、vyaw。 control_config: [true, false, false, false, false, true]


  # Places limits on how large the acceleration term will be. Should match your robot's kinematics.


  # 限制加速度項(xiàng)的大小。應(yīng)與機(jī)器人的運(yùn)動(dòng)學(xué)相匹配。


      acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4]


  # Acceleration and deceleration limits are not always the same for robots.


  #機(jī)器人的加速和減速限制并不總是相同的。 deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5]


  # If your robot cannot instantaneously reach its acceleration limit, the permitted change can be controlled with these


  # gains


  #如果你的機(jī)器人不能立即達(dá)到其加速度極限，那么可以通過這些增益來控制允許的變化 acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9]


  # If your robot cannot instantaneously reach its deceleration limit, the permitted change can be controlled with these


  # gains


  # 如果你的機(jī)器人不能立即達(dá)到減速極限，那么允許的變化可以用這些增益來控制


      deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0]


  # [ADVANCED] The process noise covariance matrix can be difficult to tune, and can vary for each application, so it is


  # exposed as a configuration parameter. This matrix represents the noise we add to the total error after each


  # prediction step. The better the omnidirectional motion model matches your system, the smaller these values can be.


  # However, if users find that a given variable is slow to converge, one approach is to increase the


  # process_noise_covariance diagonal value for the variable in question, which will cause the filter's predicted error


  # to be larger, which will cause the filter to trust the incoming measurement more during correction. The values are


  # ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below if


  # unspecified.


  # [ADVANCED]過程噪聲協(xié)方差矩陣可能很難調(diào)整，并且可能因每個(gè)應(yīng)用而變化，因此它被公開為配置參數(shù)。


  # 這個(gè)矩陣表示我們在每個(gè)預(yù)測步驟之后添加到總誤差中的噪聲。全向運(yùn)動(dòng)模型與系統(tǒng)匹配得越好，這些值就越小。


  # 然而，如果用戶發(fā)現(xiàn)給定的變量收斂較慢，一種方法是增加該變量的process_noise_covariance對角值，這將導(dǎo)致濾波器的預(yù)測誤差更大，


  # 這將使得濾波器在校正期間更加信任傳入的測量。值的順序?yàn)閤、y、z、滾轉(zhuǎn)、俯仰、偏航、vx、vy、vz、vroll、vpitch、vyaw、ax、ay、az。


  # 如果未指定，則默認(rèn)為以下矩陣。


      process_noise_covariance: [0.05, 0.0,  0.0,  0.0,  0.0,  0.0,  0.0,   0.0,   0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
                    0.0,  0.05, 0.0,  0.0,  0.0,  0.0,  0.0,   0.0,   0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
                    0.0,  0.0,  0.06, 0.0,  0.0,  0.0,  0.0,   0.0,   0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
                    0.0,  0.0,  0.0,  0.03, 0.0,  0.0,  0.0,   0.0,   0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
                    0.0,  0.0,  0.0,  0.0,  0.03, 0.0,  0.0,   0.0,   0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
                    0.0,  0.0,  0.0,  0.0,  0.0,  0.06, 0.0,   0.0,   0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
                    0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.025, 0.0,   0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
                    0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,   0.025, 0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
                    0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,   0.0,   0.04, 0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
                    0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,   0.0,   0.0,  0.01, 0.0,  0.0,  0.0,  0.0,  0.0,
                    0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,   0.0,   0.0,  0.0,  0.01, 0.0,  0.0,  0.0,  0.0,
                    0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,   0.0,   0.0,  0.0,  0.0,  0.02, 0.0,  0.0,  0.0,
                    0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,   0.0,   0.0,  0.0,  0.0,  0.0,  0.01, 0.0,  0.0,
                    0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,   0.0,   0.0,  0.0,  0.0,  0.0,  0.0,  0.01, 0.0,
                    0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,   0.0,   0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.015]


  # [ADVANCED] This represents the initial value for the state estimate error covariance matrix. Setting a diagonal


  # value (variance) to a large value will result in rapid convergence for initial measurements of the variable in


  # question. Users should take care not to use large values for variables that will not be measured directly. The values


  # are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below


  #if unspecified.


  # [ADVANCED]這表示狀態(tài)估計(jì)誤差協(xié)方差矩陣的初始值。將對角線值（方差）設(shè)置為大值將導(dǎo)致所討論變量的初始測量的快速收斂。


  # 用戶應(yīng)注意不要對不會(huì)直接測量的變量使用大值。


  # 用戶應(yīng)注意不要對不會(huì)直接測量的變量使用大值。


  # 值的順序?yàn)閤、y、z、滾轉(zhuǎn)、俯仰、偏航、vx、vy、vz、vroll、vpitch、vyaw、ax、ay、az。


  # 如果未指定，則默認(rèn)為以下矩陣。


      initial_estimate_covariance: [1e-9, 0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,   0.0,   0.0,   0.0,  0.0,  0.0,
                     0.0,  1e-9, 0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,   0.0,   0.0,   0.0,  0.0,  0.0,
                     0.0,  0.0,  1e-9, 0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,   0.0,   0.0,   0.0,  0.0,  0.0,
                     0.0,  0.0,  0.0,  1e-9, 0.0,  0.0,  0.0,  0.0,  0.0,  0.0,   0.0,   0.0,   0.0,  0.0,  0.0,
                     0.0,  0.0,  0.0,  0.0,  1e-9, 0.0,  0.0,  0.0,  0.0,  0.0,   0.0,   0.0,   0.0,  0.0,  0.0,
                     0.0,  0.0,  0.0,  0.0,  0.0,  1e-9, 0.0,  0.0,  0.0,  0.0,   0.0,   0.0,   0.0,  0.0,  0.0,
                     0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  1e-9, 0.0,  0.0,  0.0,   0.0,   0.0,   0.0,  0.0,  0.0,
                     0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  1e-9, 0.0,  0.0,   0.0,   0.0,   0.0,  0.0,  0.0,
                     0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  1e-9, 0.0,   0.0,   0.0,   0.0,  0.0,  0.0,
                     0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  1e-9, 0.0,   0.0,   0.0,  0.0,  0.0,
                     0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,   1e-9, 0.0,   0.0,  0.0,  0.0,
                     0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,   0.0,   1e-9, 0.0,  0.0,  0.0,
                     0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,   0.0,   0.0,   1e-9, 0.0,  0.0,
                     0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,   0.0,   0.0,   0.0,  1e-9, 0.0,
                     0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,   0.0,   0.0,   0.0,  0.0,  1e-9]

可選項(xiàng)很多，但是需要配置的內(nèi)容其實(shí)圍繞在odom0和imu0上了，大家可以根據(jù)內(nèi)容做出一些修改。這里大家就會(huì)發(fā)現(xiàn)odom似乎有點(diǎn)奇怪，我們一般在base程序中會(huì)發(fā)布一個(gè)odom話題，此外還有一個(gè)odom坐標(biāo)系，那么和這里有什么關(guān)系呢？其實(shí)這里的意思是，大家可以不用在base中去tf廣播odom了，直接發(fā)布o(jì)dom的數(shù)據(jù)即可，那個(gè)數(shù)據(jù)會(huì)來到ekf中，有ekf來釋放一個(gè)新的融合的odom數(shù)據(jù)。

啟用robot-localization

這里需要寫一個(gè)launch文件了，其實(shí)比較簡單，本身咱們二進(jìn)制下載robot-localization之后就有一個(gè)ekf.launch.py了，如果沒有做過多更改就可以直接使用了。但是為了匹配修改后的ekf.yaml文件，一般還是會(huì)copy一份出來另外處理。

    from launch import LaunchDescription
    from ament_index_python.packages 
    import get_package_share_directory
    import launch_ros.actionsimport os
    import yaml
    from launch.substitutions import EnvironmentVariable
    import pathlib
    import launch.actions
    from launch.actions import DeclareLaunchArgument


    def generate_launch_description(): 
      return LaunchDescription([ launch_ros.actions.Node( package='robot_localization', 
      executable='ekf_node', name='ekf_filter_node', output='screen', parameters=
      [os.path.join(get_package_share_directory("robot_localization"), 'params', 'ekf.yaml')], ),])

簡要總結(jié)

robot-localization確實(shí)是一個(gè)比較好用的技術(shù)，可以比較好的去優(yōu)化機(jī)器人的定位，因?yàn)樗诤狭硕鄠€(gè)傳感器的輸入。對于使用者而言還是需要在ekf.yaml中去配置一下你希望它分別融合odom/imu的哪些數(shù)據(jù)，比如你的里程計(jì)很好，那么完全可以把位置信息都融合上等等。

當(dāng)然如果想要真正做到好的定位robot-localziation還是不夠的，因?yàn)楸举|(zhì)上還是依賴了傳感器，那么如何更好的去處理定位呢。是不是可以考慮加上視覺里程計(jì)/傳感器校準(zhǔn)等。甚至把ekf出來的odom數(shù)據(jù)給到類似cartographer算法增加激光雷達(dá)的一些處理都可以進(jìn)一步優(yōu)化定位。

總的來說robot-localization可以當(dāng)作最后的輸出，也可以當(dāng)作定位的中間一環(huán)，看我們怎么去使用了。

審核編輯：湯梓紅