第 14.8.1.2 節
輪式里程計與IMU融合
0瀏覽次數0訪問次數--跳出率--平均停留
輪式里程計可以通過監測輪子轉動來估計機器人的位置和姿態變化，但容易受到滑動和不均勻地面等特殊情況的影響。IMU可以測量加速度和角速度，提供高頻率的運動信息，但容易受到積分漂移和噪聲的影響。將它們的數據融合可以彌補彼此的不足，提供更可靠和準確的定位結果，充分利用各自的優勢，減少侷限性。在ROS2中，提供了robot_localization功能包，可以方便的幫助開發人員實現二者的融合。
1.robot_localization簡介
robot_localization是ROS2中的一個功能包，用於實現多傳感器數據融合的機器人定位和導航。它提供了一個解決方案，可以將來自不同傳感器（如輪式里程計、IMU、GPS、激光雷達等）的數據進行融合，以獲得更準確和魯棒的定位結果。在該功能包中，融合算法已經通過節點封裝完畢，調用者只需在調用節點時注入參數即可。
請調用如下指令安裝robot_localization：
sudo apt install ros-<distro>-robot-localization
請將<distro>替換為正在使用的ROS2發行版代號。
2.robot_localization節點說明
在robot_localization功能包中提供了ekf_node、ukf_node和navsat_transform_node多個節點來實現機器人的狀態估計和定位。不同節點介紹如下：
ekf_node是基於增強卡爾曼濾波器（EKF）的節點，用於融合多個傳感器數據來進行狀態估計。它接收來自里程計、IMU、激光雷達等傳感器的數據，並將它們融合在一起以提供更準確的機器人狀態估計。EKF節點使用機器人的運動模型和傳感器測量模型來估計機器人的位置、姿態和速度等狀態變量。ukf_node是基於無跡卡爾曼濾波器（UKF）的節點，用於實現機器人的狀態估計和本地化。UKF節點採用無跡變換（Unscented Transform）來處理非線性系統，通過估計機器人在空間中的位置和姿態，提供更準確的狀態估計。與EKF相比，UKF節點可以更好地處理非線性系統的估計問題。navsat_transform_node用於將全球定位系統（GNSS）導航衛星數據與慣性測量單元（IMU）數據進行融合，以提供精確的位置估計。該節點校正GNSS信號的誤差，並將其轉化為帶有高精度的位置信息。這有助於改善在室內或具有GNSS信號不穩定性的環境中的機器人定位精度。
這些節點是robot_localization功能包中的核心組件，每個節點都可以融合任意數量的傳感器（慣性測量單元（IMU），里程計，室內定位系統，全球定位系統接收器等），它們支持 nav_msgs/msg/Odometry（位置、方向和線性、角速度）、geometry_msgs/msg/PoseWithCovarianceStamped（位置和方向）、geometry_msgs/msg/TwistWithCovarianceStamped（線性和角速度）和sensor_msgs/msg/Imu（方向、角速度和線性加速度）消息，以跟蹤機器人的15維（x、y、z、滾動、俯仰、偏航、x˙、y˙、z˙、滾動˙、俯仰˙、偏航˙、x¨、y¨、z¨）狀態。
基於這些測量數據，狀態估計節點將過濾後的位置、方向和線性、角速度（nav_msgs/Odometry）發佈到 /odometry/filtered 話題上，並在啟用時將過濾後的加速度發佈到 /accel/filtered 話題上。
此外，它們可以（默認啟用）將相應的變換作為 tf2 變換進行發佈，無論是 odom → base_link 變換還是 map → odom 變換。
3.robot_localization使用示例
假設要融合輪式里程計與IMU，那麼請先保證機器人底盤可以發佈里程計消息以及IMU消息。在MyCar導航機器人中，Stm32底盤是滿足條件的，但是Arduino底盤不包含IMU傳感器，需要自行安裝。融合時，可以使用ekf_node或ukf_node節點。本案例中使用的是stm32 4w底盤結合ekf_node實現。其大致步驟如下：
- 編寫luanch文件；
- 編寫配置文件；
- 編譯；
- 執行並查看結果。
由於robot_localization對融合實現已經做了很好的封裝，所以整個實現流程是比較簡單的。
（1）編寫launch文件
在ros2_stm32_bridge功能包的launch目錄下新建文件driver_ekf.launch.py，並輸入以下內容：
from launch import LaunchDescription
from ament_index_python.packages import get_package_share_directory
import launch_ros.actions
import os
def generate_launch_description():
MYCAR_MODEL = os.environ['MYCAR_MODEL']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("ros2_stm32_bridge"), 'params', 'ekf.yaml')],
),
launch_ros.actions.Node(
package='tf2_ros',
executable='static_transform_publisher',
arguments=['--frame-id', 'base_footprint', '--child-frame-id', 'imu_link', '--x', '-0.15']
),
launch_ros.actions.Node(
package="ros2_stm32_bridge",
executable="base_controller",
parameters=[
os.path.join(get_package_share_directory("ros2_stm32_bridge"), "params", MYCAR_MODEL + "_ekf.yaml"),],
)
])
該launch文件中，啟動了機器人底盤驅動，啟動了robot_localization包中ekf_node節點，並且還發布了base_footprint與imu_link的座標變換。
（2）編寫配置文件
- 在ros2_stm32_bridge功能包的params目錄下新建文件stm32_4w_ekf.yaml，並輸入以下內容：
/mini_driver:
# /**: ros__parameters: base_frame: base_footprint
baud_rate: 115200 control_rate: 10 encoder_resolution: 44.0 kd: 130 ki: 0 kp: 100 maximum_encoding: 100.0 model_param_acw: 0.45 model_param_cw: 0.45 odom_frame: odom
port_name: /dev/mycar
publish_tf: false qos_overrides:
/parameter_events:
publisher: depth: 1000 durability: volatile
history: keep_last
reliability: reliable
/tf:
publisher: depth: 100 durability: volatile
history: keep_last
reliability: reliable
reduction_ratio: 90.0 use_sim_time: false wheel_diameter: 0.080
在該文件中，一個重要操作是將publish_tf參數設置為false，設置為false後，底盤驅動不再發布base_footprint與odom的座標變換，而是由ekf_node取而代之。
- 在ros2_stm32_bridge功能包的params目錄下新建文件ekf.yaml，並輸入以下內容：
### ekf config file ###
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.
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.
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.
two_d_mode: false
# 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.
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.
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.
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.
debug: false
# Defaults to "robot_localization_debug.txt" if unspecified. Please specify the full path.
debug_out_file: /path/to/debug/file.txt
# Whether we'll allow old measurements to cause a re-publication of the updated state
permit_corrected_publication: false
# Whether to publish the acceleration state. Defaults to false if unspecified.
publish_acceleration: false
# Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified.
publish_tf: true
# 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:
# 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.
# 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.
# 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.
map_frame: map # Defaults to "map" if unspecified
odom_frame: odom # Defaults to "odom" if unspecified
base_link_frame: base_footprint # Defaults to "base_link" if unspecified
world_frame: odom # Defaults to the value of odom_frame if unspecified
# 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.
odom0: odom
# 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.
odom0_config: [false, false, false,
false, false, false,
true, false, false,
false, false, false,
false, false, false]
# 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.
odom0_queue_size: 2
# [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.
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.
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.
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<double>::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.
odom0_pose_rejection_threshold: 5.0
odom0_twist_rejection_threshold: 1.0
imu0: imu
imu0_config: [false, false, false,
false, false, true,
false, false, false,
false, false, true,
false, false, false]
imu0_nodelay: false
imu0_differential: false
imu0_relative: true
imu0_queue_size: 5
imu0_pose_rejection_threshold: 0.8 # Note the difference in parameter names
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.
imu0_remove_gravitational_acceleration: true
# [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.
use_control: true
# Whether the input (assumed to be cmd_vel) is a geometry_msgs/Twist or geometry_msgs/TwistStamped message. Defaults to
# false.
stamped_control: false
# The last issued control command will be used in prediction for this period. Defaults to 0.2.
control_timeout: 0.2
# Which velocities are being controlled. Order is 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.
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.
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
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
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.
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.
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]
在該文件中，關鍵配置如下：
#......
odom0_config: [false, false, false,
false, false, false,
true, false, false,
false, false, false,
false, false, false]
#......
imu0_config: [false, false, false,
false, false, true,
false, false, false,
false, false, true,
false, false, false]
上述配置主要是融合了里程計消息中的x方向速度與imu消息中的yaw角度生成新的里程計數據。
（3）編譯
終端中進入當前工作空間，編譯功能包：
colcon build --packages-select ros2_stm32_bridge
（4）執行
終端中進入當前工作空間，執行launch文件：
. install/setup.bash
ros2 launch ros2_stm32_bridge driver_ekf.launch.py
再啟動rviz2以及鍵盤控制節點，在rviz2中將參考座標系設置為odom，添加tf插件，再添加兩個odometry插件並將話題分別設置為odom和odometry/filtered。使用鍵盤控制機器人前進，兩個里程計插件顯示的數據和機器人的實際運行基本相符，前進一段時間後，手動搬動機器人讓其產生一定角度的偏航，該動作也能在融合後的里程計以及座標變換中準確顯示，而輪式里程計由於電機並未旋轉而沒有改變（如下圖所示）。
