主要总结下IMU怎么使用，陀螺仪、加速度计、磁力计的作用，四元数<-->欧拉角 如何转换，以及ROS中的消息格式。

本文所有内容均采用右手坐标系。

1. IMU（惯性测量单元），能够输出空间坐标系下的6个自由度，x,y,z 的加速度(Acceleration)，围绕单个轴进行旋转的姿态角(Attitude/Orientation),机器人中一般都为右手坐标系。

   

2. 在右手坐标系中，X正方向指向前方时 Y的正方向指向左边。而欧拉角可以使用右手定则判断旋转方向的正负：大拇指指向坐标轴正方向四指弯曲的方向为旋转的正方向。
3. 如何测量这6个自由度的值呢，就得在IMU中集成加速度计以及陀螺仪，甚至磁力计、压力计，前面两个是必须具备的，后面的这些传感器作为辅助来增加测量精度。

加速度计 (Acceleration)

1. 传感器测量线性加速度，(测量值) = (运动加速度) + (重力加速度)，单位为 m/s² 或 g，使用时通过积分可得到速度，再积分得到位移。
2. 当IMU的Z轴与地面垂直时，存在9.8 m/s²的重力，此时其他轴读数应为0，不过对于时刻在运动的物体来说不可能时刻都跟地面平行，会存在一定的倾斜角，所以在把加速度拿来计算用之前会根据当前的姿态角来去除重力分量。

陀螺仪 (Gyroscope)

1. 传感器测量角速度，通过积分可得到欧拉角，但陀螺仪有零偏（Bias），静态时输出不归零，不断积分会有累计误差，所以不要买便宜货！！一分钱一分货，越贵零偏越少，一般都会加个磁力计来避免yaw漂移。磁力计（Magnetometer）： 用于测量物体周围的磁场。

姿态角 (Attitude/Orientation)

特指描述物体在空间中朝向的三个欧拉角（Euler Angles），常用英文为 Roll, Pitch, Yaw。

对角速度进行时间积分也能算出姿态角，对于欧拉角可复习下旋转矩阵二维与三维旋转变换-CSDN博客。空间中的任意坐标都可以通过旋转平移来转换。

ROS消息格式

1. IMU 消息：sensor_msgs/msg/Imu

这是用于承载IMU原始数据或经过初步处理的数据的标准消息。其完整定义如下：

# 这是一个用于存放IMU（惯性测量单元）数据的消息
# 加速度单位应为 m/s^2，角速度单位应为 rad/s
std_msgs/Header header
geometry_msgs/Quaternion orientation
float64[9] orientation_covariance
geometry_msgs/Vector3 angular_velocity
float64[9] angular_velocity_covariance
geometry_msgs/Vector3 linear_acceleration
float64[9] linear_acceleration_covariance

各字段详细说明：

header:
  stamp:
    sec: 1714556789          # 【时间戳-秒】自1970年起的整数秒
    nanosec: 123456789       # 【时间戳-纳秒】
  frame_id: imu_link         # 【坐标系ID】需要有对应的坐标系变换或者urdf

# ============== 姿态（四元数） ==============
orientation:
  x: 0.0                     # 四元数x分量：绕X轴无旋转
  y: 0.0                     # 四元数y分量：绕Y轴无旋转
  z: 0.09983341664682815     # 四元数z分量：sin(0.2/2) = sin(0.1) ≈ 0.09983
  w: 0.9950041652780258      # 四元数w分量：cos(0.2/2) = cos(0.1) ≈ 0.9950
# ============== 姿态协方差矩阵 ==============
orientation_covariance:
- 0.001                      # X轴姿态方差 
- 0.0                        # X与Y的协方差 
- 0.0                        # X与Z的协方差
- 0.0                        # Y与X的协方差
- 0.001                      # Y轴姿态方差
- 0.0                        # Y与Z的协方差
- 0.0                        # Z与X的协方差
- 0.0                        # Z与Y的协方差
- 0.001                      # Z轴姿态方差 
# ============== 角速度 ==============
angular_velocity:
  x: 0.01                    # 绕X轴
  y: -0.02                   # 绕Y轴
  z: 0.5                     # 绕Z轴
# ============== 角速度协方差矩阵 ==============
# 说明：数值越小表示角速度测量越精准
angular_velocity_covariance:
- 0.0001                     # X轴角速度方差 
- 0.0
- 0.0
- 0.0
- 0.0001                     # Y轴角速度方差
- 0.0
- 0.0
- 0.0
- 0.0001                     # Z轴角速度方差

# ============== 线性加速度 ==============
linear_acceleration:
  x: 0.05                  
  y: -0.03                 
  z: 9.81                    # ★Z轴为 9.81 m/s²，说明重力方向与Z轴完全对齐（水平放置）
# ============== 线性加速度协方差矩阵 ==============
linear_acceleration_covariance:
- 0.01                       # X轴加速度方差
- 0.0
- 0.0
- 0.0
- 0.01                       # Y轴加速度方差
- 0.0
- 0.0
- 0.0
- 0.01                       # Z轴加速度方差

关于协方差矩阵：所有协方差矩阵都是9元素的 float64 数组。如果某个测量值没有协方差信息，应将矩阵的所有元素设为 0。

---

2. 里程计消息：nav_msgs/msg/Odometry

此消息用于描述机器人在自由空间中的位置和速度的估计值。它通常融合了IMU、轮式编码器等多种传感器的数据。其完整定义如下：

# 消息中的pose（位姿）应在header.frame_id指定的坐标系中表示
# 消息中的twist（速度）应在child_frame_id指定的坐标系中表示
std_msgs/Header header
string child_frame_id
geometry_msgs/PoseWithCovariance pose
geometry_msgs/TwistWithCovariance twist

各字段详细说明：

header:
  stamp:
    sec: 1714556790
    nanosec: 987654321
  frame_id: odom             # ★【父坐标系】"odom" 是固定的世界坐标系原点（0,0,0）
child_frame_id: base_link    # ★【子坐标系】"base_link" 是机器人自身的中心点

# ============== 位姿（Pose）：机器人在哪里、朝哪看 ==============
pose:
  pose:
    position:
      x: 1.55                # 距离odom原点 X方向 1.55 米
      y: 0.22                # 距离odom原点 Y方向 0.22 米
      z: 0.0                 # 地面高度 0
    orientation:# 四元数
      x: 0.0                
      y: 0.0                
      z: 0.154              
      w: 0.988              
      # 过去所有帧的 (角速度 × dt) 累加得到的总和
  # ============== 位姿协方差 (6×6矩阵) ==============
  # 顺序固定为：[x, y, z, roll(横滚), pitch(俯仰), yaw(偏航)]
  covariance:
  - 0.01                     # 行0: X轴位置方差 
  - 0.0                      # 行0: X-Y协方差
  - 0.0                      # 行0: X-Z协方差
  - 0.0                      # 行0: X-roll协方差
  - 0.0                      # 行0: X-pitch协方差
  - 0.0                      # 行0: X-yaw协方差
  - 0.0                      # 行1: Y-X协方差
  - 0.01                     # 行1: Y轴位置方差 (标准差 0.1米)
  - 0.0                      # ...以下依此类推，只在对角线上有数值
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.01                     # 行2: Z轴位置方差 
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.001                    # 行3: roll(横滚)方差 
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.001                    # 行4: pitch(俯仰)方差
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.001                    # 行5: ★yaw(偏航)方差 
# ============== ★速度（Twist）：机器人移动多快  ==============
twist:
  twist:
    linear:
      x: 0.52                # 前进线速度 0.52 米/秒 
      y: 0.0                 # 无横向滑动 
      z: 0.0                 # 无垂直运动 
    angular:
      x: 0.0                 # 无翻滚角速度
      y: 0.0                 # 无俯仰角速度 
      z: 0.31                # 角速度：绕自身Z轴右转 0.31 rad/s (≈ 17.76°/s)
  # ============== 速度协方差 (6×6矩阵) ==============
  # 顺序同样是：[linear.x, linear.y, linear.z, angular.x, angular.y, angular.z]
  covariance:
  - 0.001                    # 线速度X方差 (标准差 0.0316 m/s)
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.001                    # 线速度Y方差
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.001                    # 线速度Z方差
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.001                    # 角速度X方差
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.001                    # 角速度Y方差
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.001                    # ★角速度Z方差 (标准差 0.0316 rad/s)