# 9.4为FishBot配置IMU惯性测量单元

大家好，我是小鱼，上节课通过配置两轮差速控制器我们已经成功的让fishbot在gazebo中动了起来，本节课我们通过给fishbot的URDF配置IMU传感器插件，让IMU模块工作起来。

## 1.惯性测量单元IMU介绍

惯性测量单元是测量物体三轴姿态角(或角速率)以及加速度的装置。一般的，一个IMU包含了三个单轴的加速度计和三个单轴的陀螺，加速度计检测物体在载体坐标系统独立三轴的加速度信号，而陀螺检测载体相对于导航坐标系的角速度信号，**测量物体在三维空间中的角速度和加速度**，并以此解算出**物体的姿态**。在导航中有着很重要的应用价值。

上面这段话是小鱼从百科中摘抄出来的，你需要知道的一个关键点是IMU可以测量以下三组数据：

- 三维角度
- 三维加速度
- 三维角速度

### 1.1 IMU长啥样？

便宜的长这样：

<img src="9.4%E4%B8%BAFishBot%E6%B7%BB%E5%8A%A0IMU%E4%BC%A0%E6%84%9F%E5%99%A8/imgs/image-20220409150448805.png" alt="image-20220409150448805" style="zoom:50%;" />



贵的长这样：

<img src="9.4%E4%B8%BAFishBot%E6%B7%BB%E5%8A%A0IMU%E4%BC%A0%E6%84%9F%E5%99%A8/imgs/image-20220409150658019.png" alt="image-20220409150658019" style="zoom:50%;" />

不要钱的长什么样？

![image-20220409150754602](9.4%E4%B8%BAFishBot%E6%B7%BB%E5%8A%A0IMU%E4%BC%A0%E6%84%9F%E5%99%A8/imgs/image-20220409150754602.png)

仿真的不要钱哈哈，接着我们来配置一下仿真的IMU。



## 2.Gazebo-IMU插件介绍

仿真的IMU也是对应一个后缀为`.so`的动态链接库，使用下面的指令可以查看所有的动态链接库：

```
ls /opt/ros/foxy/lib/libgazebo_ros*
```

```
/opt/ros/foxy/lib/libgazebo_ros2_control.so
/opt/ros/foxy/lib/libgazebo_ros_ackermann_drive.so
/opt/ros/foxy/lib/libgazebo_ros_bumper.so
/opt/ros/foxy/lib/libgazebo_ros_camera.so
/opt/ros/foxy/lib/libgazebo_ros_diff_drive.so
/opt/ros/foxy/lib/libgazebo_ros_elevator.so
/opt/ros/foxy/lib/libgazebo_ros_factory.so
/opt/ros/foxy/lib/libgazebo_ros_force.so
/opt/ros/foxy/lib/libgazebo_ros_force_system.so
/opt/ros/foxy/lib/libgazebo_ros_ft_sensor.so
/opt/ros/foxy/lib/libgazebo_ros_gps_sensor.so
/opt/ros/foxy/lib/libgazebo_ros_hand_of_god.so
/opt/ros/foxy/lib/libgazebo_ros_harness.so
/opt/ros/foxy/lib/libgazebo_ros_imu_sensor.so
/opt/ros/foxy/lib/libgazebo_ros_init.so
/opt/ros/foxy/lib/libgazebo_ros_joint_pose_trajectory.so
/opt/ros/foxy/lib/libgazebo_ros_joint_state_publisher.so
/opt/ros/foxy/lib/libgazebo_ros_node.so
/opt/ros/foxy/lib/libgazebo_ros_p3d.so
/opt/ros/foxy/lib/libgazebo_ros_planar_move.so
/opt/ros/foxy/lib/libgazebo_ros_projector.so
/opt/ros/foxy/lib/libgazebo_ros_properties.so
/opt/ros/foxy/lib/libgazebo_ros_ray_sensor.so
/opt/ros/foxy/lib/libgazebo_ros_state.so
/opt/ros/foxy/lib/libgazebo_ros_template.so
/opt/ros/foxy/lib/libgazebo_ros_tricycle_drive.so
/opt/ros/foxy/lib/libgazebo_ros_utils.so
/opt/ros/foxy/lib/libgazebo_ros_vacuum_gripper.so
/opt/ros/foxy/lib/libgazebo_ros_video.so
/opt/ros/foxy/lib/libgazebo_ros_wheel_slip.so
```

IMU对应的消息类型为`sensor_msgs/msg/Imu`

```
ros2 interface show sensor_msgs/msg/Imu
```

```
# This is a message to hold data from an IMU (Inertial Measurement Unit)
#
# Accelerations should be in m/s^2 (not in g's), and rotational velocity should be in rad/sec
#
# If the covariance of the measurement is known, it should be filled in (if all you know is the
# variance of each measurement, e.g. from the datasheet, just put those along the diagonal)
# A covariance matrix of all zeros will be interpreted as "covariance unknown", and to use the
# data a covariance will have to be assumed or gotten from some other source
#
# If you have no estimate for one of the data elements (e.g. your IMU doesn't produce an
# orientation estimate), please set element 0 of the associated covariance matrix to -1
# If you are interpreting this message, please check for a value of -1 in the first element of each
# covariance matrix, and disregard the associated estimate.

std_msgs/Header header

geometry_msgs/Quaternion orientation
float64[9] orientation_covariance # Row major about x, y, z axes

geometry_msgs/Vector3 angular_velocity
float64[9] angular_velocity_covariance # Row major about x, y, z axes

geometry_msgs/Vector3 linear_acceleration
float64[9] linear_acceleration_covariance # Row major x, y z
```

可以看到除了每个数据对应的三个协方差之外，每一个还都对应一个`3*3`的协方差矩阵。

## 3.给FIshbot配置IMU传感器

有了上节课的经验，我们可以很轻松的添加IMU传感器，但是还有一个需要注意的地方，为了更真实的模拟IMU传感器，我们需要给我们的仿真IMU传感器加点料。

加什么？加点高斯噪声，高斯噪声包含

```
    <gazebo reference="imu_link">
      <sensor name="imu_sensor" type="imu">
      <plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
          <ros>
            <namespace>/</namespace>
            <remapping>~/out:=imu</remapping>
          </ros>
          <initial_orientation_as_reference>false</initial_orientation_as_reference>
        </plugin>
        <always_on>true</always_on>
        <update_rate>100</update_rate>
        <visualize>true</visualize>
        <imu>
          <angular_velocity>
            <x>
              <noise type="gaussian">
                <mean>0.0</mean>
                <stddev>2e-4</stddev>
                <bias_mean>0.0000075</bias_mean>
                <bias_stddev>0.0000008</bias_stddev>
              </noise>
            </x>
            <y>
              <noise type="gaussian">
                <mean>0.0</mean>
                <stddev>2e-4</stddev>
                <bias_mean>0.0000075</bias_mean>
                <bias_stddev>0.0000008</bias_stddev>
              </noise>
            </y>
            <z>
              <noise type="gaussian">
                <mean>0.0</mean>
                <stddev>2e-4</stddev>
                <bias_mean>0.0000075</bias_mean>
                <bias_stddev>0.0000008</bias_stddev>
              </noise>
            </z>
          </angular_velocity>
          <linear_acceleration>
            <x>
              <noise type="gaussian">
                <mean>0.0</mean>
                <stddev>1.7e-2</stddev>
                <bias_mean>0.1</bias_mean>
                <bias_stddev>0.001</bias_stddev>
              </noise>
            </x>
            <y>
              <noise type="gaussian">
                <mean>0.0</mean>
                <stddev>1.7e-2</stddev>
                <bias_mean>0.1</bias_mean>
                <bias_stddev>0.001</bias_stddev>
              </noise>
            </y>
            <z>
              <noise type="gaussian">
                <mean>0.0</mean>
                <stddev>1.7e-2</stddev>
                <bias_mean>0.1</bias_mean>
                <bias_stddev>0.001</bias_stddev>
              </noise>
            </z>
          </linear_acceleration>
        </imu>
      </sensor>
    </gazebo>
```



## 4.编译测试

```
http://gazebosim.org/tutorials?tut=sensor_noise&cat=sensors
```



```
ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args --remap cmd_vel:=/fishbot/cmd_vel
```

















































--------------

技术交流&&问题求助：

- **微信公众号及交流群：鱼香ROS**
- **小鱼微信：AiIotRobot**
- **QQ交流群：139707339**

- 版权保护：已加入“维权骑士”（rightknights.com）的版权保护计划为FishBot添加IMU传感器