陀螺仪：表示方法g[x,y,z]，右手定则
绕X轴旋转 → 大拇指指向X+ → 四指方向 = 正旋转（从Y转向Z） 绕Y轴旋转 → 大拇指指向Y+ → 四指方向 = 正旋转（从Z转向X） 绕Z轴旋转 → 大拇指指向Z+ → 四指方向 = 正旋转（从X转向Y）
gyro[i] = [0, 0, -np.pi/2] # 弧度/秒，负值表示顺时针
| 轴 | 值 | 含义 |
|---|---|---|
gyro[0] = X轴 |
0 | 没有绕X轴旋转 → 没有横滚运动 |
gyro[1] = Y轴 |
0 | 没有绕Y轴旋转 → 没有俯仰运动 |
gyro[2] = Z轴 |
-np.pi/2 |
绕Z轴以-90°/秒的速度旋转 |
-np.pi/2≈ -1.57 rad/s ≈ -90°/s
方向判断：
Z轴向下看，负值 = 逆时针旋转
或者：正值为顺时针，负值为逆时针
加速度计：
如果你手上有一颗不确定型号或朝向的传感器，可以用以下方法标定方向：
静止（水平朝上）： 记录三轴值，记为
(0, 0, +1g)理想情况。绕X转90度（立起来）： 观察数值如何变化，确定Y和Z的正负。
绕Y转90度： 确定X和Z的正负。
自由落体（或抛起）： 三轴读数应均趋近于0（验证量程和极性）。
模拟数据：
import numpy as np
import matplotlib.pyplot as plt
from scipy import signal
import pandas as pd
# 设置中文显示
plt.rcParams['font.sans-serif'] = ['SimHei']
plt.rcParams['axes.unicode_minus'] = False
print("环境准备完成！")第二步：模拟真实IMU数据
场景设计：一个手持设备，经历5个阶段
-
0-5秒：静止，平放在桌面
-
5-10秒：匀速抬升，缓慢向上移动
-
10-15秒：向前加速，突然向前推
-
15-20秒：向左旋转，绕垂直轴转动
-
20-25秒：自由落体，松开掉落
==================== 数据模拟函数 ====================
def generate_imu_data(duration=25, fs=100):
"""
生成模拟的IMU数据
参数：
duration: 总时长(秒)
fs: 采样频率(Hz)
返回：
time: 时间戳
acc: 加速度计数据 [ax, ay, az] (包含重力)
gyro: 陀螺仪数据 [gx, gy, gz]
"""
t = np.linspace(0, duration, duration*fs)
dt = t[1] - t[0]# 初始化数据数组 n_samples = len(t) acc = np.zeros((n_samples, 3)) gyro = np.zeros((n_samples, 3)) # 重力常数 g = 9.81 # m/s² for i, time in enumerate(t): # ----- 加速度计模拟（包含重力）----- # 默认：平放桌面，重力在Z轴负方向 if time < 5: # 阶段1：静止 acc[i] = [0, 0, -g] gyro[i] = [0, 0, 0] elif time < 10: # 阶段2：匀速抬升 # Z轴向上0.5 m/s，但匀速所以加速度为0，仍然是重力 acc[i] = [0, 0, -g] # 匀速时加速度为0 gyro[i] = [0, 0, 0] elif time < 15: # 阶段3：向前加速 # X轴向前加速 3 m/s² acc[i] = [3, 0, -g] gyro[i] = [0, 0, 0] elif time < 20: # 阶段4：向左旋转 # 绕Z轴顺时针旋转 90度/秒 acc[i] = [0, 0, -g] # 仍然平放 gyro[i] = [0, 0, -np.pi/2] # 弧度/秒，负值表示顺时针 else: # 阶段5：自由落体 acc[i] = [0, 0, 0] # 失重！ gyro[i] = [0, 0, 0] # ---------- 添加噪声，使数据更真实 ---------- # 加速度计噪声：±0.1 m/s² acc_noise = np.random.normal(0, 0.1, acc.shape) # 陀螺仪噪声：±0.01 rad/s gyro_noise = np.random.normal(0, 0.01, gyro.shape) acc += acc_noise gyro += gyro_noise return t, acc, gyro==================== 生成数据 ====================
fs = 100 # 100Hz采样率
t, acc, gyro = generate_imu_data(duration=25, fs=fs)print(f"生成数据：{len(t)}个采样点")
print(f"加速度范围：{acc.min():.2f} ~ {acc.max():.2f} m/s²")
print(f"陀螺仪范围：{gyro.min():.2f} ~ {gyro.max():.2f} rad/s")保存原始数据
df = pd.DataFrame({
'time': t,
'acc_x': acc[:, 0], 'acc_y': acc[:, 1], 'acc_z': acc[:, 2],
'gyro_x': gyro[:, 0], 'gyro_y': gyro[:, 1], 'gyro_z': gyro[:, 2]
})
df.to_csv('simulated_imu_data.csv', index=False)
print("数据已保存到 simulated_imu_data.csv")
第三步：数据预处理（分离重力加速度和运动加速度）
# ==================== 1. 单位转换（这里已经在SI单位）====================
print("数据已在标准单位：加速度 m/s²，陀螺仪 rad/s")
# ==================== 2. 去除重力 ====================
def remove_gravity(acc):
"""
从加速度中分离运动加速度和重力
简化方法：低通滤波提取重力分量
"""
# 设计低通滤波器
fs = 100
cutoff = 0.5 # 截止频率0.5Hz，保留重力（直流分量）
b, a = signal.butter(4, cutoff/(fs/2), 'low')
# 对每个轴低通滤波，得到重力分量
gravity = np.zeros_like(acc)
for i in range(3):
gravity[:, i] = signal.filtfilt(b, a, acc[:, i])
# 运动加速度 = 总加速度 - 重力
motion_acc = acc - gravity
return motion_acc, gravity
# 执行去除重力
motion_acc, gravity = remove_gravity(acc)
print("\n=== 去除重力结果 ===")
print(f"Z轴重力分量均值：{gravity[:,2].mean():.2f} m/s²（应为-9.81）")
print(f"静止时段运动加速度均方根：{np.sqrt(np.mean(motion_acc[:500]**2, axis=0))}")
# ==================== 3. 陀螺仪去零偏 ====================
# 使用静止的前5秒数据计算零偏
gyro_bias = np.mean(gyro[:500], axis=0) # 5秒 * 100Hz = 500点
gyro_calibrated = gyro - gyro_bias
print(f"\n陀螺仪零偏：[{gyro_bias[0]:.4f}, {gyro_bias[1]:.4f}, {gyro_bias[2]:.4f}] rad/s")第四步：姿态解算（互补滤波）
# ==================== 互补滤波姿态估计 ====================
def complementary_filter(acc, gyro, dt, alpha=0.02):
"""
互补滤波：融合加速度计和陀螺仪
输入：
acc: 原始加速度数据（包含重力）
gyro: 校准后的陀螺仪数据
dt: 采样间隔
alpha: 滤波系数（越小越相信陀螺仪）
输出：
pitch, roll: 俯仰角和横滚角（度）
"""
n = len(acc)
pitch = np.zeros(n)
roll = np.zeros(n)
for i in range(1, n):
# 1. 从加速度计计算参考角度
# 注意：atan2(y, sqrt(x^2+z^2)) 等公式取决于坐标系定义
acc_pitch = np.arctan2(-acc[i, 0],
np.sqrt(acc[i, 1]**2 + acc[i, 2]**2))
acc_roll = np.arctan2(acc[i, 1], acc[i, 2])
# 转换为度
acc_pitch_deg = np.degrees(acc_pitch)
acc_roll_deg = np.degrees(acc_roll)
# 2. 陀螺仪积分预测
pitch[i] = pitch[i-1] + gyro[i, 1] * dt # 绕Y轴旋转
roll[i] = roll[i-1] + gyro[i, 0] * dt # 绕X轴旋转
# 3. 互补滤波融合
pitch[i] = (1 - alpha) * pitch[i] + alpha * acc_pitch_deg
roll[i] = (1 - alpha) * roll[i] + alpha * acc_roll_deg
return pitch, roll
# 计算姿态
dt = t[1] - t[0]
pitch, roll = complementary_filter(acc, gyro_calibrated, dt)
print("\n=== 姿态计算完成 ===")
print(f"俯仰角范围：{pitch.min():.1f}° ~ {pitch.max():.1f}°")
print(f"横滚角范围：{roll.min():.1f}° ~ {roll.max():.1f}°")第五步：运动状态分类
# ==================== 运动状态分类 ====================
def classify_motion(motion_acc, gyro, pitch, roll, threshold_acc=0.5, threshold_gyro=0.1):
"""
基于多传感器特征的运动状态分类
"""
n = len(motion_acc)
states = []
# 计算合加速度（去重力后）
acc_magnitude = np.sqrt(np.sum(motion_acc**2, axis=1))
# 计算合角速度
gyro_magnitude = np.sqrt(np.sum(gyro**2, axis=1))
# 计算姿态变化率
pitch_rate = np.gradient(pitch)
roll_rate = np.gradient(roll)
for i in range(n):
# 1. 自由落体检测
if np.abs(acc_magnitude[i]) < 2.0: # 加速度接近0
states.append('自由落体')
continue
# 2. 静止检测
if acc_magnitude[i] < threshold_acc and gyro_magnitude[i] < threshold_gyro:
states.append('静止')
continue
# 3. 旋转检测
if gyro_magnitude[i] > 1.0: # 大于1 rad/s ≈ 57°/s
states.append('快速旋转')
continue
elif gyro_magnitude[i] > threshold_gyro:
states.append('缓慢旋转')
continue
# 4. 线性运动检测
if acc_magnitude[i] > threshold_acc:
# 判断方向
if np.abs(motion_acc[i, 0]) > np.abs(motion_acc[i, 1]):
if motion_acc[i, 0] > 0:
states.append('向前加速')
else:
states.append('向后减速')
else:
if motion_acc[i, 1] > 0:
states.append('向左加速')
else:
states.append('向右减速')
else:
states.append('匀速运动')
return states, acc_magnitude, gyro_magnitude
# 执行分类
states, acc_mag, gyro_mag = classify_motion(motion_acc, gyro_calibrated, pitch, roll)
# 统计各状态时长
from collections import Counter
state_counts = Counter(states)
print("\n=== 运动状态统计 ===")
for state, count in state_counts.items():
duration = count / fs
print(f"{state}: {duration:.1f}秒 ({count}个采样点)")
# 添加状态到数据框
df_result = pd.DataFrame({
'time': t,
'acc_magnitude': acc_mag,
'gyro_magnitude': gyro_mag,
'pitch': pitch,
'roll': roll,
'state': states
})第六步：可视化分析
# ==================== 创建可视化 ====================
fig, axes = plt.subplots(4, 1, figsize=(15, 12))
fig.suptitle('IMU数据分析与运动状态识别', fontsize=16)
# 子图1：原始加速度
ax1 = axes[0]
ax1.plot(t, acc[:,0], label='X轴', alpha=0.7)
ax1.plot(t, acc[:,1], label='Y轴', alpha=0.7)
ax1.plot(t, acc[:,2], label='Z轴', alpha=0.7)
ax1.axhline(y=-9.81, color='gray', linestyle='--', alpha=0.5, label='重力基准')
ax1.set_ylabel('加速度 (m/s²)')
ax1.set_title('原始加速度数据（包含重力）')
ax1.legend()
ax1.grid(True, alpha=0.3)
# 子图2：运动加速度（去除重力后）
ax2 = axes[1]
ax2.plot(t, motion_acc[:,0], label='X轴', alpha=0.7)
ax2.plot(t, motion_acc[:,1], label='Y轴', alpha=0.7)
ax2.plot(t, motion_acc[:,2], label='Z轴', alpha=0.7)
ax2.axhline(y=0, color='gray', linestyle='--', alpha=0.5)
ax2.set_ylabel('加速度 (m/s²)')
ax2.set_title('运动加速度（已去除重力）')
ax2.legend()
ax2.grid(True, alpha=0.3)
# 子图3：姿态角
ax3 = axes[2]
ax3.plot(t, pitch, label='俯仰角', linewidth=2)
ax3.plot(t, roll, label='横滚角', linewidth=2)
ax3.set_ylabel('角度 (度)')
ax3.set_title('融合后的姿态角（互补滤波）')
ax3.legend()
ax3.grid(True, alpha=0.3)
# 子图4：运动状态（着色）
ax4 = axes[3]
# 创建状态映射
state_colors = {
'静止': 'gray',
'匀速运动': 'lightblue',
'向前加速': 'green',
'向后减速': 'red',
'缓慢旋转': 'orange',
'快速旋转': 'darkorange',
'自由落体': 'purple'
}
# 为不同状态着色
unique_states = list(set(states))
for state in unique_states:
mask = np.array([s == state for s in states])
ax4.fill_between(t[mask], 0, 1, alpha=0.5, label=state, color=state_colors.get(state))
ax4.set_ylim(0, 1)
ax4.set_ylabel('状态')
ax4.set_xlabel('时间 (秒)')
ax4.set_title('运动状态识别结果')
ax4.legend(loc='upper right', ncol=3)
plt.tight_layout()
plt.show()
# ==================== 额外可视化：运动特征 ====================
fig2, axes2 = plt.subplots(2, 2, figsize=(15, 8))
fig2.suptitle('运动特征分析', fontsize=16)
# 合加速度
axes2[0,0].plot(t, acc_mag, color='blue')
axes2[0,0].axhline(y=0.5, color='r', linestyle='--', label='加速阈值')
axes2[0,0].set_ylabel('加速度 (m/s²)')
axes2[0,0].set_title('合加速度（去重力）')
axes2[0,0].legend()
axes2[0,0].grid(True)
# 角速度
axes2[0,1].plot(t, gyro_mag, color='orange')
axes2[0,1].axhline(y=0.1, color='r', linestyle='--', label='旋转阈值')
axes2[0,1].set_ylabel('角速度 (rad/s)')
axes2[0,1].set_title('合角速度')
axes2[0,1].legend()
axes2[0,1].grid(True)
# 姿态角变化率
axes2[1,0].plot(t, np.gradient(pitch), label='俯仰角变化率')
axes2[1,0].plot(t, np.gradient(roll), label='横滚角变化率')
axes2[1,0].set_xlabel('时间 (秒)')
axes2[1,0].set_ylabel('变化率 (度/秒)')
axes2[1,0].set_title('姿态变化率')
axes2[1,0].legend()
axes2[1,0].grid(True)
# 状态分布饼图
state_durations = {state: count/fs for state, count in state_counts.items()}
axes2[1,1].pie(state_durations.values(), labels=state_durations.keys(), autopct='%1.1f%%')
axes2[1,1].set_title('运动状态分布')
plt.tight_layout()
plt.show()第七步：导出详细报告
# ==================== 生成分析报告 ====================
print("\n" + "="*60)
print(" IMU运动分析报告")
print("="*60)
print(f"\n【数据概览】")
print(f" 采样频率: {fs} Hz")
print(f" 总时长: {t[-1]:.1f} 秒")
print(f" 总采样点: {len(t)}")
print(f"\n【加速度统计】")
print(f" 原始X轴: {acc[:,0].mean():.2f} ± {acc[:,0].std():.2f} m/s²")
print(f" 原始Y轴: {acc[:,1].mean():.2f} ± {acc[:,1].std():.2f} m/s²")
print(f" 原始Z轴: {acc[:,2].mean():.2f} ± {acc[:,2].std():.2f} m/s²")
print(f" 运动加速度峰值: {acc_mag.max():.2f} m/s²")
print(f"\n【姿态分析】")
print(f" 俯仰角范围: [{pitch.min():.1f}°, {pitch.max():.1f}°]")
print(f" 横滚角范围: [{roll.min():.1f}°, {roll.max():.1f}°]")
print(f"\n【运动状态总结】")
for state, duration in sorted(state_durations.items(), key=lambda x: x[1], reverse=True):
percentage = duration / t[-1] * 100
print(f" {state}: {duration:.1f}秒 ({percentage:.1f}%)")
print(f"\n【关键事件检测】")
# 检测加速事件
acc_events = np.where(acc_mag > 1.0)[0]
if len(acc_events) > 0:
print(f" 检测到 {len(acc_events)} 次明显的加速事件")
# 检测旋转事件
rot_events = np.where(gyro_mag > 0.5)[0]
if len(rot_events) > 0:
print(f" 检测到 {len(rot_events)} 次明显的旋转事件")
# 保存完整分析结果
df_result.to_csv('imu_analysis_result.csv', index=False)
print(f"\n完整分析结果已保存到 imu_analysis_result.csv")
print("="*60)互补滤波的核心原理：
用陀螺仪的短时精度 + 加速度计的长时稳定 = 完美的角度跟踪
互补滤波 = 积分 + 参考 + 加权平均
原始数据 ↓ 加速度计 ──→ 加速度角度 ──┐ ↓ ↓ 包含重力 互补滤波融合 ──→ 稳定角度 ↓ ↑ 陀螺仪 ────→ 陀螺仪积分 ──┘ ↓ 角速度
角度 = 0.98 × (角度 + 陀螺仪×dt) + 0.02 × 加速度计角度 ↑ ↑ ↑ 相信过去+变化 短期精准 长期基准
俯仰角 = arctan( 对边 / 邻边 ) 对边 = X轴的重力分量 邻边 = YZ平面的重力分量合 = √(Y² + Z²)几何意义：
横滚是绕X轴旋转
Y轴和Z轴构成垂直平面
arctan2(Y, Z)直接给出这个平面的倾斜角
俯仰是绕Y轴旋转
X轴和Z轴构成垂直平面
arctan2(X, Z)直接给出这个平面的倾斜角
pitch[i] = pitch[i-1] + gyro[i, 1] * dt # 绕Y轴旋转 roll[i] = roll[i-1] + gyro[i, 0] * dt # 绕X轴旋转
比较：
陀螺仪算"变化"
角度_now = 角度_before + 角速度 × 时间
优点 ：绝对基准，不漂移
缺点：怕晃动，噪声大按照速度计的变化
参考角度 = atan2(加速度Y, 加速度Z)
优点：绝对基准，不漂移
缺点：怕晃动，噪声大
互补融合（核心公式）
最终角度 = 0.98 × 陀螺仪角度 + 0.02 × 加速度计角度 ↑ ↑ ↑ 输出结果 相信98%的短期 相信2%的长期 精准变化 稳定基准
打个比方：蒙眼走路
角色 相当于 特点 陀螺仪 蒙眼走路的内耳 每一步都准，但10分钟后不知道在哪 加速度计 偶尔睁眼看路标 路标永远对，但边走边看会晃 互补滤波 内耳+偶尔看路标 平时靠内耳，每50步看一眼路标校正
具体的步骤：
# 初始化
angle = 0 # 起始角度
# 每10ms执行一次
while True:
# 1. 陀螺仪预测（相信短期变化）
angle = angle + gyro * dt
# 2. 加速度计参考（相信长期基准）
acc_angle = atan2(acc_y, acc_z) * 180/pi
# 3. 互补融合（核心！）
angle = 0.98 * angle + 0.02 * acc_angle
# ↑陀螺仪主导 ↑一点点拉回真实值系数0.02的含义：每秒钟，向真实值拉近2%
小计：（核心都是在计算权值变化）
Madgwick滤波算法是一种基于传感器融合的姿态估计算法
卡尔曼滤波
补充知识：（验证重力）
假设重力 = 9.81 m/s²，设备倾斜30°：
X轴 = 9.81 × sin(30°) = 4.905 m/s²
Z轴 = 9.81 × cos(30°) = 8.496 m/s²
Y轴 = 0
验证：√(4.905² + 0² + 8.496²) = √(24.06 + 72.18) = √96.24 = 9.81 ✓
把手机平放桌上：
acc = [0.01, 0.02, 9.81] # 重力只在Z轴
把手机竖起来：
acc = [0.01, 9.81, 0.02] # 重力跑到Y轴
把手机斜45°放在支架上：
acc = [0.01, 6.94, 6.94] # 重力分给Y和Z
注意：重力总量永远是9.81，只是在不同轴上的分量不同！
因为传感器坐标系和世界坐标系不重合！
世界坐标系 ：Z↑天，Y↑北，X↑东 → 重力只在-Z轴
传感器坐标系 ：随设备转动 → 重力在三个轴上都有投影
去除重力 ≠ 把每个轴都减掉9.81！
去除重力 = 用姿态角把重力分量算出来，再从各轴减去
四种典型姿态的3轴重力分布：
| 设备姿态 | 重力在X轴 | 重力在Y轴 | 重力在Z轴 | 合加速度 |
|---|---|---|---|---|
| 平放桌面（屏幕向上） | 0 g | 0 g | +1 g | 1 g |
| 平放桌面（屏幕向下） | 0 g | 0 g | -1 g | 1 g |
| 竖立（像相框） | 0 g | +1 g | 0 g | 1 g |
| 侧立（像书立） | +1 g | 0 g | 0 g | 1 g |
| 45°倾斜 | 0.7 g | 0 g | 0.7 g | 1 g |
关键公式 ：
√(ax² + ay² + az²) = 1 g（静止时）
三轴加速度计主要记录的是物体在三个互相垂直的轴向（X轴、Y轴和Z轴）上的线性加速度。具体来说，它测量的是物体在三维空间中沿这三个方向的加速度变化，单位通常为重力加速度（g）或米每二次方秒（m/s²）。这种测量能力使其能够全面反映物体的运动状态
注意：
- 坐标系假设不明确
acc_pitch = np.arctan2(-acc[i, 0], ...)这个负号是硬编码的，假设了特定的坐标系。如果你的传感器安装方向不同，这个公式要调整！
- 没有处理万向锁
当俯仰角接近±90°时，横滚角计算会不稳定。
- 没有磁力计
只能算俯仰和横滚，算不了偏航角。