[feat]:完成部分IMU

docs/chapt9/9.4为FishBot添加IMU传感器.md

@@ -4,17 +4,195 @@
 
 ## 1.惯性测量单元IMU介绍
 
-惯性测量单元是测量物体三轴姿态角(或角速率)以及加速度的装置。一般的，一个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
+```
+
 
 
 ```