针对你这个用小腿传感器数据计算步长的项目，我来拆解一下关键步骤和具体的Python实现思路：

一、核心思路梳理 #

步长计算的核心是先识别足跟触地（HS）事件，再通过传感器数据计算两次HS之间的小腿摆动角度，最后结合身高和传感器高度用摆模型推导步长。

二、步骤1：识别足跟触地（HS）事件 #

你提到用陀螺仪数据识别HS，这是很合理的——足跟着地瞬间，小腿的角速度会出现一个显著的峰值（通常是绕X轴的角速度，因为足跟着地时小腿突然停止摆动，角速度由正转负或出现尖峰）。

实现要点： #

• 数据预处理：先对陀螺仪数据做低通滤波，去除高频噪声，推荐用Butterworth低通滤波（截止频率选5-10Hz即可）。
• 峰值检测：用scipy.signal.find_peaks函数检测角速度的峰值，需要设置合适的阈值（比如基于数据的标准差）和最小峰距（因为步行频率大概1-2Hz，100Hz采样率下最小峰距设为50-100个点比较合适）。

代码片段： #

import numpy as np
from scipy.signal import butter, filtfilt, find_peaks

# 低通滤波函数
def butter_lowpass_filter(data, cutoff=5, fs=100, order=2):
    nyq = 0.5 * fs
    normal_cutoff = cutoff / nyq
    b, a = butter(order, normal_cutoff, btype='low', analog=False)
    filtered_data = filtfilt(b, a, data)
    return filtered_data

# 读取传感器数据（假设txt列顺序：timestamp, acc_x, acc_y, acc_z, gyro_x, gyro_y, gyro_z, qw, qx, qy, qz）
data = np.loadtxt('walking_data.txt', delimiter=',')
gyro_x = data[:, 4]  # 取绕X轴的角速度数据

# 滤波
filtered_gyro_x = butter_lowpass_filter(gyro_x)

# 检测HS事件（假设峰值为负向尖峰，所以取绝对值）
peaks, _ = find_peaks(-filtered_gyro_x, height=2, distance=50)  # height是阈值，distance是最小峰距
hs_timestamps = data[peaks, 0]  # 获取HS对应的时间戳

三、步骤2：计算两次HS之间的小腿摆动角度 #

因为传感器在小腿，我们可以用四元数计算角度变化（比积分陀螺仪更准确，避免漂移问题）。四元数可以直接转成小腿相对于初始姿态的欧拉角，两次HS之间的角度差就是摆动角度θ。

实现要点： #

• 四元数转欧拉角（比如绕Y轴的角度，对应小腿前后摆动的角度），注意坐标系的定义（通常传感器的Y轴是小腿的纵向）。
• 提取两次HS时刻对应的四元数，计算角度差。

代码片段： #

def quaternion_to_euler(qw, qx, qy, qz):
    # 计算绕Y轴的角度（单位：弧度）
    yaw = np.arctan2(2*(qw*qz + qx*qy), 1 - 2*(qy**2 + qz**2))
    # 这里根据你的传感器坐标系调整，可能需要用俯仰角（pitch），具体看传感器安装方向
    pitch = np.arcsin(2*(qw*qy - qz*qx))
    roll = np.arctan2(2*(qw*qx + qy*qz), 1 - 2*(qx**2 + qy**2))
    return roll, pitch, yaw

# 提取所有HS时刻的四元数
hs_quats = data[peaks, 7:11]  # qw, qx, qy, qz

# 计算相邻HS之间的角度变化
angle_changes = []
for i in range(1, len(hs_quats)):
    qw_prev, qx_prev, qy_prev, qz_prev = hs_quats[i-1]
    qw_curr, qx_curr, qy_curr, qz_curr = hs_quats[i]
    # 计算两次姿态的角度差（这里用俯仰角pitch为例）
    _, pitch_prev, _ = quaternion_to_euler(qw_prev, qx_prev, qy_prev, qz_prev)
    _, pitch_curr, _ = quaternion_to_euler(qw_curr, qx_curr, qy_curr, qz_curr)
    angle_diff = np.abs(pitch_curr - pitch_prev)
    angle_changes.append(angle_diff)

四、步骤3：用摆模型计算步长 #

已知受试者身高H（单位：米），传感器放置高度h（地面到传感器的垂直距离，单位：米），可以用复摆模型计算步长：

步长 L = 2 * sqrt(H² - h²) * sin(θ/2)
其中θ是两次HS之间的小腿摆动角度（单位：弧度）

代码片段： #

subject_height = 1.75  # 示例：1.75米
sensor_height = 0.4    # 示例：传感器距地面0.4米

# 计算每一步的步长
step_lengths = []
for theta in angle_changes:
    L = 2 * np.sqrt(subject_height**2 - sensor_height**2) * np.sin(theta / 2)
    step_lengths.append(L)

# 输出结果
print("计算得到的步长列表（米）：", step_lengths)
print("平均步长：", np.mean(step_lengths))

五、优化建议 #

• 坐标系校准：实验前一定要校准传感器的初始姿态，确保欧拉角的计算符合实际摆动方向。
• 噪声处理：如果陀螺仪噪声大，可以增加滤波的阶数或者调整截止频率，也可以结合加速度计数据辅助验证HS事件（比如HS时加速度计会有冲击峰值）。
• 模型修正：复摆模型是近似，实际步长可能受步态影响，你可以用实际测量的步长（比如用步道或运动捕捉系统）来修正模型参数。

内容的提问来源于stack exchange，提问作者Ajit Wadalkar