4. 加速度计校准（六面法）：原理、步骤与实现

加速度计这东西，看着简单，用起来坑不少。你拿一个MPU6050或者ADXL345直接读数，静止时三个轴加起来不一定是1g，而且每个轴单独看，零位也不在0。这就是典型的零偏和标度因数误差。

六面法，说白了就是利用重力这个已知的1g激励，把加速度计每个轴的误差系数给揪出来。我最早在无人机飞控项目里用这个方法，当时IMU装歪了，悬停时老往一边飘，校准完立马稳了。

4.1 六面法的核心原理

理想情况下，加速度计三轴输出满足：

A_x = S_x · (a_x + B_x)
A_y = S_y · (a_y + B_y)
A_z = S_z · (a_z + B_z)

其中：

• A 是实际读数（原始ADC值或电压）
• a 是真实加速度（单位g）
• S 是标度因数（scale factor）
• B 是零偏（bias）

六面法就是让加速度计分别处于6个标准姿态，每个姿态下重力只在一个轴上产生±1g，另外两轴为0。这样就能解出6个未知数（3个零偏 + 3个标度因数）。

4.2 数据采集步骤

采集数据时，有几点我吃过亏，必须说清楚：

1. 找一块平整的台面，最好用水平仪确认。我曾在木桌上校，结果桌子本身不平，校准完反而更差。
2. 传感器要固定牢，不能晃动。用双面胶或者螺丝固定到一个小铝块上，手拿着不行。
3. 每个姿态静止采样，至少采集100个样本取平均。我习惯采200个，去掉头尾各20个，取中间160个平均。
4. 6个姿态顺序：X轴向上、X轴向下、Y轴向上、Y轴向下、Z轴向上、Z轴向下。

姿态	理想 a_x (g)	理想 a_y (g)	理想 a_z (g)
X向上	+1	0	0
X向下	-1	0	0
Y向上	0	+1	0
Y向下	0	-1	0
Z向上	0	0	+1
Z向下	0	0	-1

4.3 参数求解

以X轴为例，两个姿态的方程：

姿态1（X向上）：A_x1 = S_x · (1 + B_x)
姿态2（X向下）：A_x2 = S_x · (-1 + B_x)

解这个二元一次方程组：

S_x = (A_x1 - A_x2) / 2
B_x = (A_x1 + A_x2) / (A_x1 - A_x2)

Y轴和Z轴同理。注意，这里B_x的单位是g，如果你要转成m/s²，乘以9.8就行。

4.4 Matlab实现

% 六面法加速度计校准 - Matlab实现
% 输入：6个姿态的原始读数，每行一个姿态
% 顺序：X+ X- Y+ Y- Z+ Z-

raw_data = [
    16384, 256,  512;   % X向上
    -16384, 240,  508;   % X向下
    256,   16384, 520;   % Y向上
    240,   -16384, 510;  % Y向下
    256,   240,   16384; % Z向上
    260,   244,   -16384 % Z向下
];

% 提取各轴数据
A_x = [raw_data(1,1); raw_data(2,1)];
A_y = [raw_data(3,2); raw_data(4,2)];
A_z = [raw_data(5,3); raw_data(6,3)];

% 计算标度因数和零偏
S_x = (A_x(1) - A_x(2)) / 2;
B_x = (A_x(1) + A_x(2)) / (A_x(1) - A_x(2));

S_y = (A_y(1) - A_y(2)) / 2;
B_y = (A_y(1) + A_y(2)) / (A_y(1) - A_y(2));

S_z = (A_z(1) - A_z(2)) / 2;
B_z = (A_z(1) + A_z(2)) / (A_z(1) - A_z(2));

fprintf('标度因数: Sx=%.4f, Sy=%.4f, Sz=%.4f\n', S_x, S_y, S_z);
fprintf('零偏(g):  Bx=%.4f, By=%.4f, Bz=%.4f\n', B_x, B_y, B_z);

% 校准函数
calibrated = @(raw, S, B) (raw ./ S) - B;

4.5 Python实现

import numpy as np

def six_side_calibration(raw_data):
    """
    六面法加速度计校准
    raw_data: 6x3 numpy数组，顺序为X+ X- Y+ Y- Z+ Z-
    """
    # 提取各轴数据
    A_x = np.array([raw_data[0, 0], raw_data[1, 0]])
    A_y = np.array([raw_data[2, 1], raw_data[3, 1]])
    A_z = np.array([raw_data[4, 2], raw_data[5, 2]])
    
    # 计算参数
    S_x = (A_x[0] - A_x[1]) / 2.0
    B_x = (A_x[0] + A_x[1]) / (A_x[0] - A_x[1])
    
    S_y = (A_y[0] - A_y[1]) / 2.0
    B_y = (A_y[0] + A_y[1]) / (A_y[0] - A_y[1])
    
    S_z = (A_z[0] - A_z[1]) / 2.0
    B_z = (A_z[0] + A_z[1]) / (A_z[0] - A_z[1])
    
    return {
        'scale': np.array([S_x, S_y, S_z]),
        'bias_g': np.array([B_x, B_y, B_z])
    }

def apply_calibration(raw, scale, bias_g):
    """应用校准参数"""
    return (raw / scale) - bias_g

# 示例数据
raw_data = np.array([
    [16384, 256,  512],   # X向上
    [-16384, 240,  508],  # X向下
    [256,   16384, 520],  # Y向上
    [240,   -16384, 510], # Y向下
    [256,   240,   16384],# Z向上
    [260,   244,   -16384] # Z向下
])

params = six_side_calibration(raw_data)
print(f"标度因数: {params['scale']}")
print(f"零偏(g):  {params['bias_g']}")

# 验证：校准后静止时模长应为1g
test_raw = np.array([16384, 256, 512])
calibrated = apply_calibration(test_raw, params['scale'], params['bias_g'])
norm = np.linalg.norm(calibrated)
print(f"校准后模长: {norm:.4f}g")

4.6 六面法流程总览

下面这张图把整个流程串起来了，从硬件准备到参数验证，每一步都标清楚了。

4.7 验证与常见问题

校准完别急着用，先验证。把传感器随便放个姿态，算一下三轴模长：

norm = sqrt(ax² + ay² + az²)

理想情况下应该是1g。我一般接受±2%的误差，也就是0.98g到1.02g。如果超出这个范围，检查：

• 数据采集时传感器是否动了？ 我遇到过采集到一半手抖了一下，数据全废。
• 台面是否水平？ 用水平仪确认，或者换个地方试试。
• 传感器量程设置是否正确？ 比如你设了±2g，但代码里按±4g算，标度因数直接差一倍。

嗯，六面法就这些。代码可以直接拿去用，但记得根据你的传感器量程调整一下原始数据的数值范围。如果你用的是16位ADC、±2g量程，那1g对应的原始值大概是16384，跟我示例里的一样。