# 使用EKF融合里程计和IMU

在机器人导航过程中，实时的位置计算是非常重要的，一般我们可以通过轮子的编码器根据机器人的运动学模型性计算出里程计信息，但在一些特殊场景下，比如机器人轮子在原地打滑，此时机器人并没有动但此时对轮子进行积分计算的里程计显示机器人在不断的移动，此时IMU可以检测到机器人并未移动（没有任何方向上的加速度）。初次之外还有另外一种场景，机器人在原地画月饼，结果被人给绑架了（抬起来），移动一段距离，此时轮子没有转，里程计数据表示机器人没有动，此时IMU可以检测出机器人其实动了。

所以如果能有一种方法将IMU和里程计数据进行融合，之后给出一个更加准确的里程计信息，ros2里个功能包实现了这一功能。

这个功能报就是`robot localization`功能包`ekf_node`，该功能包使用扩展卡尔曼滤波器来融合里程信息并发布 `odom => base_link` 变换。

我们使用fishbot开源库进行测试学习：https://github.com/fishros/fishbot

## 1.安装
首先，安装 `robot_localization` 包，二进制安装
```
sudo apt install ros-$ROS_DISTRO-robot-localization
```

源码安装：
```
git clone https://github.com/cra-ros-pkg/robot_localization -b humble-devel
```


## 2.配置

### 2.1 ekf参数配置
在`src/fishbot_navigation2/config`下创建`ekf.yaml`

输入下面内容
```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 theinputs. 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

# 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

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

# Whether to broadcast the transformation over the /tf topic. Defaultsto true if unspecified.
        publish_tf: true

# 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_link  # Defaults to "base_link" ifunspecified
        world_frame: odom           # Defaults to the value ofodom_frame if unspecified

        odom0: /odom
        odom0_config: [true,  true,  true,
                       false, false, false,
                       false, false, false,
                       false, false, true,
                       false, false, false]

        imu0: /imu
        imu0_config: [false, false, false,
                      true,  true,  true,
                      false, false, false,
                      false, false, false,
                      false, false, false]
```

关于这些参数的含义可以参考：http://docs.ros.org/en/melodic/api/robot_localization/html/state_estimation_nodes.html#parameters

### 2.2 配置launch文件
新建`src/fishbot_navigation2/launch/odom_ekf.launch.py`文件，输入以下内容
```python
import os
import launch
import launch_ros


def generate_launch_description():
    package_name = 'fishbot_navigation2'

    ld =  launch.LaunchDescription()
    pkg_share = launch_ros.substitutions.FindPackageShare(package=package_name).find(package_name) 

    robot_localization_node = launch_ros.actions.Node(
        package='robot_localization',
        executable='ekf_node',
        name='ekf_filter_node',
        output='screen',
        parameters=[os.path.join(pkg_share, 'config/ekf.yaml'), {'use_sim_time': launch.substitutions.LaunchConfiguration('use_sim_time')}]
    )

    
    ld.add_action(launch.actions.DeclareLaunchArgument(name='use_sim_time', default_value='True',
                                                description='Flag to enable use_sim_time'))
    ld.add_action(robot_localization_node)                                           
    return ld
```

### 2.3 CMakeLists.txt安装
```cmake
install(
  DIRECTORY launch  config param maps
  DESTINATION share/${PROJECT_NAME}
)
```

## 3.编译测试
### 3.1 编译
```
colcon build --packages-select fishbot_navigation2
```

### 3.2 运行仿真
```
source install/setup.bash
ros2 launch fishbot_description gazebo.launch.py
```

![image-20220910221937530](/home/fishros/.config/Typora/typora-user-images/image-20220910221937530.png)

### 3.3 运行EKF
```
source install/setup.bash
ros2 launch fishbot_navigation2 odom_ekf.launch.p
```
![image-20220910221522793](/home/fishros/.config/Typora/typora-user-images/image-20220910221522793.png)

### 3.4查看融合后的数据

融合后的数据通过话题/odometry/filtered发布
```
ros2 topic echo /odometry/filtered
```

![image-20220910222043764](/home/fishros/.config/Typora/typora-user-images/image-20220910222043764.png)

## 4.总结

小鱼在仿真环境上测试了下，仿真中采用的里程计数据并不是轮子积分获取，而是通过gazebo直接获取的，所以当轮子打滑时并不会对里程计进行积分，所以测不出打滑时的效果，等fishbot完工后，在fishbot上做一下测试，看看效果如何。