2. 力矩数据采集与预处理：传感器标定、数据滤波、异常值剔除、时间戳对齐

好，咱们进入第二章。上一章聊了力矩感知到底能帮导航做什么，这一章就得落地了——数据怎么来，来了怎么洗干净。

说实话，我见过太多团队在算法上花了大把时间，结果发现数据本身就有问题。传感器没标定、噪声没滤掉、时间戳对不上……这些坑，我几乎都踩过。所以这一章，咱们把数据采集和预处理这四件事一次讲透：传感器标定、数据滤波、异常值剔除、时间戳对齐。

核心观点：预处理做得好，后面调参省一半时间。数据质量决定了算法性能的上限。

2.1 传感器标定——把原始值翻译成物理量

力矩传感器输出的原始值，通常是电压或数字码值。你得把它映射到真实的牛顿·米（N·m）。这个映射过程，就是标定。

我个人习惯用最小二乘法做线性拟合。操作很简单：挂已知重量的砝码，记录传感器读数，然后拟合一条直线。

# Python 示例：线性标定拟合
import numpy as np

# 已知力矩值 (N·m) 与传感器原始读数
torque_true = np.array([0.0, 1.0, 2.0, 3.0, 4.0, 5.0])
raw_values = np.array([512, 614, 716, 818, 920, 1022])

# 最小二乘拟合：torque = slope * raw + bias
A = np.vstack([raw_values, np.ones_like(raw_values)]).T
slope, bias = np.linalg.lstsq(A, torque_true, rcond=None)[0]

print(f"标定结果：斜率 = {slope:.6f}, 截距 = {bias:.6f}")
# 输出：斜率 ≈ 0.0098, 截距 ≈ -5.02

标定完一定要做回检。我曾经遇到过一台机器人，标定完第二天数据就飘了——后来发现是温度变化导致的。所以，我建议标定后至少做三组不同温度下的验证。

小技巧：如果传感器有非线性特性（比如某些应变片），可以用多项式拟合代替线性拟合。但大多数场景下，线性模型已经够用。

2.2 数据滤波——把噪声压下去

原始力矩信号里，噪声主要来自电机振动、齿轮啮合、结构共振。你想想看，这些噪声频率通常不低，而我们要的力矩变化往往是比较慢的（比如人推机器人的力）。所以，低通滤波是首选。

2.2.1 低通滤波（一阶 RC 数字实现）

最简单的低通滤波，就是一阶 RC 的数字版本。公式就一行：

y[k] = alpha * x[k] + (1 - alpha) * y[k-1]

其中 alpha = T / (T + RC)，T 是采样周期，RC 是时间常数。alpha 越小，滤波越平滑，但延迟也越大。

我在项目中一般把截止频率设在 5~10 Hz。为什么？因为人的操作频率通常不超过 3 Hz，而电机振动一般在 20 Hz 以上。留点余量，刚好能滤掉高频噪声，又不损失有用信号。

注意：低通滤波会引入相位延迟。如果你需要实时控制，延迟太大会导致系统不稳定。这时候可以考虑零相位滤波（离线）或卡尔曼滤波（在线）。

2.2.2 卡尔曼滤波——更聪明的选择

卡尔曼滤波本质上是一个预测-校正的过程。它不光滤波，还能估计状态。对于力矩信号，我通常把状态设为力矩值和它的变化率。

下面是一个简单的一维卡尔曼滤波实现：

class KalmanFilter1D:
    def __init__(self, process_noise=1e-3, measurement_noise=1e-1):
        self.x = 0.0      # 状态估计
        self.P = 1.0      # 估计误差协方差
        self.Q = process_noise    # 过程噪声
        self.R = measurement_noise # 测量噪声

    def update(self, measurement):
        # 预测
        self.P += self.Q
        # 更新
        K = self.P / (self.P + self.R)  # 卡尔曼增益
        self.x += K * (measurement - self.x)
        self.P = (1 - K) * self.P
        return self.x

卡尔曼滤波的好处是：你可以根据信号特性调整 Q 和 R。Q 大表示相信测量更多，R 大表示相信模型更多。我一般先跑一段数据，看残差分布，再调这两个参数。

滤波方法	优点	缺点	适用场景
一阶低通	计算量小，实现简单	有相位延迟	离线分析、非实时系统
卡尔曼滤波	自适应，可估计状态	需要调参，计算稍大	实时控制、高动态场景
中值滤波	抗脉冲噪声好	平滑效果一般	异常值剔除后的补充

2.3 异常值剔除——把脏数据扔掉

滤波能压噪声，但扛不住突发的大毛刺。比如线缆被勾了一下、传感器瞬间掉电——这些异常值如果不处理，会直接把你的控制输出打飞。

我常用的方法是3σ 准则 + 滑动窗口。具体来说：

维护一个长度为 N 的滑动窗口（我一般取 50~100 个点）
计算窗口内数据的均值 μ 和标准差 σ
如果当前数据点与 μ 的差超过 3σ，就判定为异常值
用窗口均值或前一有效值替换

def outlier_removal(data, window_size=50, threshold=3.0):
    filtered = np.copy(data)
    for i in range(window_size, len(data)):
        window = data[i - window_size:i]
        mean = np.mean(window)
        std = np.std(window)
        if abs(data[i] - mean) > threshold * std:
            filtered[i] = mean  # 用均值替换
    return filtered

避坑指南：我曾经在调试时发现异常值总是成片出现，后来发现是通信丢包导致的。所以，异常值剔除之前，先检查一下数据链路是否稳定。硬件问题别指望软件兜底。

2.4 时间戳对齐——让数据说同一种语言

力矩数据、里程计数据、激光雷达数据……它们来自不同的传感器，采样频率不同，触发时间也不同。如果不做时间戳对齐，你拿到的就是一本「错页的书」。

对齐的核心思路是：选一个主时钟，其他传感器数据向它插值。

我一般用线性插值，因为计算快，而且力矩信号在短时间内的变化近似线性。具体做法：

选定一个参考时间轴（比如控制循环的时间戳）
对每个传感器，找到参考时间点前后两个最近的数据点
按时间比例做线性插值

def align_timestamps(ref_timestamps, sensor_timestamps, sensor_values):
    aligned = np.interp(ref_timestamps, sensor_timestamps, sensor_values)
    return aligned

这里有个细节：如果传感器时间戳本身就有抖动（比如 ROS 的 time stamp 有时会漂），我建议先对时间戳做一次平滑，再插值。否则插出来的数据也会带抖动。

重要：时间戳对齐的精度直接影响后续的力矩-位姿关联分析。如果对齐误差超过 10 ms，在高速运动场景下，力矩和位置的对应关系就会错位。我一般要求对齐误差控制在 1 ms 以内。

2.5 本章知识体系总览

下面这张图把整个预处理流程串起来了。你可以把它当作一个检查清单：

这四步做完，你的力矩数据才算「能用」。别嫌麻烦——我见过太多人跳过标定直接滤波，结果调了三天参数，最后发现是传感器装歪了。嗯，数据预处理这件事，慢就是快。

📚 课程目录

2. 力矩数据采集与预处理：传感器标定、数据滤波、异常值剔除、时间戳对齐

2.1 传感器标定——把原始值翻译成物理量

2.2 数据滤波——把噪声压下去

2.2.1 低通滤波（一阶 RC 数字实现）

2.2.2 卡尔曼滤波——更聪明的选择

2.3 异常值剔除——把脏数据扔掉

2.4 时间戳对齐——让数据说同一种语言

2.5 本章知识体系总览