电量计 -- 阻抗追踪的嵌入式实现：定点运算与牛顿迭代

电量计
,

阻抗追踪的嵌入式实现：定点运算与牛顿迭代

上一篇介绍了阻抗追踪的算法原理：二阶RC等效电路模型、动态R_LF更新、温度补偿机制。Python版本使用 numpy、scipy.fsolve 轻松实现了完整算法。但要将其部署到没有浮点运算单元（FPU）的电量计MCU上，需要解决三个核心工程问题：

  1. 浮点→定点：所有运算必须用整数完成，保持足够精度
  2. 数学函数替代：exp()、sqrt() 需要用泰勒级数和牛顿迭代实现
  3. 溢出防护：32位整数的乘法链随时可能溢出

本文详细介绍这些工程实现，代码基于STM32平台的实际产品。

定点数运算体系

缩放策略

核心思想：将所有浮点数统一乘以1000，用整数存储和运算。

物理量 Python类型 MCU类型 单位/缩放
电阻 R_LF float (Ω) int32_t mΩ (×1000)
电压 V float (V) uint32_t mV (×1000)
电流 I float (A) int32_t mA (×1000)
电容 C float (F) int32_t 原值×1000
gamma系数 float (0~1) int32_t 原值×1000
温度系数 β float (~-0.08) int32_t 原值×1000
exp()结果 float int32_t 原值×1000

运算宏定义：

1
2
3
#define IT_SCALE_1000           1000
#define IT_MUL_SCALE(a, b, s)   (((int32_t)(a) * (int32_t)(b)) / (s))
#define IT_DIV_SCALE(a, b, s)   (((int32_t)(a) * (s)) / (int32_t)(b))

缩放乘法链的精度问题

计算温度补偿后的电阻时：

$$R_k = R_{LF} \times \gamma_{Rk} \times e^{\beta_{Rk} \times \Delta T}$$

三个操作数都是×1000缩放，直接相乘结果是×10^9，需要除以10^6回到×1000。但如果 gamma 和 exp_factor 都小于1000（即实际值<1），中间结果可能在除法后变为0。

解决方案：额外乘以10，保留一位小数精度：

1
2
3
4
// r_k结果保持×10×1000缩放，使用时除以10
r_k[0] = ((uint32_t)handle->r_lf * gamma_rk[0] * exp_factor_r1) * 10 / (1000 * 1000);
r_k[1] = ((uint32_t)handle->r_lf * gamma_rk[1] * exp_factor_r2) * 10 / (1000 * 1000);
*r_s = ((uint32_t)handle->r_lf * (*gamma_rs) * exp_factor_rs) * 10 / (1000 * 1000);

int32_t vs uint32_t

温度系数 $\beta_{Rk}$ 为负数（电阻随温度升高而降低），温度差也可能为负，所以 exp_arg 必须使用有符号类型：

1
2
int32_t exp_arg_r1 = rbk[0] * temp_diff;  // 可能为负
uint32_t exp_factor_r1 = (uint32_t)it_exp_approx(exp_arg_r1);  // 结果恒正

指数函数的泰勒级数实现

MCU没有 math.h 的 exp() 函数（或者有但依赖软浮点，极慢）。我们用5阶泰勒展开来逼近：

$$e^x \approx 1 + x + \frac{x^2}{2!} + \frac{x^3}{3!} + \frac{x^4}{4!} + \frac{x^5}{5!}$$

定点实现的关键在于：输入 x_scaled 是实际值×1000，输出也是×1000。每一项的迭代计算中，除以 n * 1000 来维持缩放不变。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
static int32_t it_exp_approx(int32_t x_scaled)
{
    // 范围限制：避免溢出和无意义计算
    if (x_scaled > 2000) return 10000;      // exp(2) ≈ 7.39, cap at 10
    if (x_scaled < -50000) return 1;        // exp(-50) ≈ 0
    if (x_scaled < -10000) return 10;       // exp(-10) ≈ 0.00005

    int64_t x = x_scaled;  // 用int64防止中间结果溢出
    int64_t result = 1000; // 第零项: 1.0 × 1000

    int64_t term = x;      // 第一项: x (已是×1000)
    result += term;

    term = (term * x) / (2 * 1000);  // 第二项: x²/2!
    result += term;

    term = (term * x) / (3 * 1000);  // 第三项: x³/3!
    result += term;

    term = (term * x) / (4 * 1000);  // 第四项: x⁴/4!
    result += term;

    term = (term * x) / (5 * 1000);  // 第五项: x⁵/5!
    result += term;

    // 输出钳位
    if (result < 100) result = 100;      // 最小0.1
    if (result > 10000) result = 10000;  // 最大10.0

    return (int32_t)result;
}

为什么每一项除以 n * 1000？

设 term_prev 表示第 n-1 项（已×1000），则第 n 项为：

$$term_n = \frac{term_{n-1} \times x}{n} = \frac{(term_{prev} \times 1000) \times (x_{actual} \times 1000)}{n}$$

为保持结果仍为×1000缩放：term = (term * x) / (n * 1000)

为什么用 int64_t？

当 x_scaled = -3000（即 x = -3）时，第二项 term * x = (-3000) * (-3000) = 9,000,000，仍在int32范围内。但后续项的乘法可能超过 $2^{31}$，使用int64彻底消除风险。

精度分析：

  • 在 x ∈ [-3, 2] 范围内（对应温度补偿的典型输入），5阶泰勒展开误差 < 1%
  • 对于 x < -10 的强衰减场景，直接返回近似零值，不影响物理结果

牛顿迭代求平方根

解二次方程需要计算判别式的平方根。MCU没有 sqrt()，用牛顿迭代法（巴比伦法）实现：

$$x_{n+1} = \frac{x_n + \frac{a}{x_n}}{2}$$

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
static uint32_t it_sqrt_newton(uint32_t x) {
    if (x == 0) return 0;
    if (x == 1) return 1;

    uint32_t guess = x / 2000;  // 初始猜测

    for (int i = 0; i < 10; i++) {
        if (guess == 0) break;
        uint32_t new_guess = (guess + x / guess) / 2;
        if (abs((int32_t)new_guess - (int32_t)guess) < 5) break;  // 收敛
        guess = new_guess;
    }

    return guess;
}

初始猜测值为什么是 x/2000？

判别式 discriminant = B² - 4AC，其中B和C都是×1000缩放，所以判别式量级是×10^6。其平方根量级是×1000。初始猜测 x/2000 大约是正确结果的一半，牛顿法从这里出发只需3-4次迭代就能收敛。

收敛速度： 牛顿法求平方根具有二次收敛性——每次迭代有效精度翻倍。10次迭代可以覆盖32位整数的全部精度范围。实际测试中通常3-5次即满足 |new - old| < 5 的收敛条件。

二次方程求解

这是整个移植中最关键的部分。Python用 scipy.fsolve 一行代码搞定的事情，MCU需要手动构建和求解二次方程。

构建标准形式

从原始非线性方程整理为 $A \times R_{LF}^2 + B \times R_{LF} + C = 0$：

1
2
3
4
5
6
7
8
9
// A = gamma_rs × I × exp(Rbs × ΔT)
// 三个×1000的数相乘，除以10^6保持×1000
int32_t A = ((int32_t)gamma_rs * (int32_t)abs_current * (int32_t)exp_rbs) / 1000000;

// B = -(OCV - V_term - Σ(V_k + I×Δt/C_k))
int32_t B = -((int32_t)ocv - (int32_t)v_term - (int32_t)vk_sum);

// C = -Δt × Σ(V_k / (gamma_Rk × C_k × exp(Rbk×ΔT)))
int32_t C = -(int32_t)term3;

求根

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
int32_t discriminant = B * B - 4 * A * C;

if (discriminant >= 0 && A != 0) {
    uint32_t sqrt_disc = it_sqrt_newton((uint32_t)discriminant);

    // 关键：分子×1000防止整除截断
    int32_t sol1 = ((-B + (int32_t)sqrt_disc) * 1000) / (2 * A);
    int32_t sol2 = ((-B - (int32_t)sqrt_disc) * 1000) / (2 * A);

    // 选择物理合理解
    if (sol1 > IT_MIN_R_LF && sol1 < IT_MAX_R_LF) {
        new_r_lf = (uint32_t)sol1;
    } else if (sol2 > IT_MIN_R_LF && sol2 < IT_MAX_R_LF) {
        new_r_lf = (uint32_t)sol2;
    } else {
        // 回退：使用DoD查表值
        uint8_t dod_index = handle->dod / 10;
        new_r_lf = R_LF_Table[dod_index];
    }
}

为什么分子要×1000？

-B 和 sqrt_disc 都是×1000缩放，2*A 也是×1000。如果直接除，×1000/×1000 = ×1，结果变成实际欧姆值，对于毫欧级的电阻会被整除为0。乘以1000后得到毫欧单位的结果。

回退策略

方程可能无解（discriminant < 0）或两个解都不在物理合理范围内（1~500mΩ）。此时使用R_LF查表的默认值作为回退，避免算法发散。

数据稳定化：滑动窗口均值

二次方程求解的结果受测量噪声影响较大，单次计算可能出现跳变。采用4样本滑动窗口平均来平滑R_LF：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
static uint32_t it_calculate_sliding_window_average(IT_HANDLE_T *handle, uint32_t new_r_lf)
{
    // 范围限制
    if (new_r_lf < IT_MIN_R_LF) new_r_lf = IT_MIN_R_LF;
    if (new_r_lf > IT_MAX_R_LF) new_r_lf = IT_MAX_R_LF;

    // 异常跳变限制：新值不超过当前均值的±50%
    if (handle->buffer_count > 0) {
        uint32_t current_avg = sum / handle->buffer_count;
        if (new_r_lf > current_avg * 3 / 2)
            new_r_lf = current_avg * 3 / 2;
        else if (new_r_lf < current_avg / 2)
            new_r_lf = current_avg / 2;
    }

    // 环形缓冲区写入
    handle->r_lf_buffer[handle->buffer_index] = new_r_lf;
    handle->buffer_index = (handle->buffer_index + 1) % 4;
    if (handle->buffer_count < 4) handle->buffer_count++;

    // 计算均值
    uint32_t sum = 0;
    for (int i = 0; i < handle->buffer_count; i++)
        sum += handle->r_lf_buffer[i];
    return sum / handle->buffer_count;
}

为什么不用卡尔曼滤波？R_LF变化缓慢（分钟级），简单的滑动窗口均值足够抑制测量噪声，且只需16字节（4个uint32_t），代码量极小。

查表与插值的整数实现

DoD→表索引映射

所有参数表以DoD 10%为步长，共11个点。DoD范围0-100映射为索引0-10：

1
2
3
4
5
6
7
8
uint32_t index = dod / 10;       // 主索引
uint32_t remainder = dod % 10;   // 插值权重 (0-9)
uint8_t id1 = (uint8_t)index;
uint8_t id2 = (id1 + 1 > 10) ? 10 : (id1 + 1);

// 线性插值
gamma_rk[i] = gamma_Rk_Table[id1][i] +
    (int32_t)(remainder * (int32_t)(gamma_Rk_Table[id2][i] - gamma_Rk_Table[id1][i])) / 10;

注意 remainder * diff 可能为负（表值非单调），必须强转 int32_t 后再除以10。

OCV 65点查表

SOC-OCV表使用65个点（步长约1.56%），以0.01%精度索引：

1
2
3
4
5
6
7
8
9
10
uint32_t soc_scaled = soc * 100;  // SOC 0-100 → 0-10000

for (int i = 0; i < 64; i++) {
    if (soc_scaled >= soc_points[i] && soc_scaled <= soc_points[i+1]) {
        uint32_t weight_numerator = soc_scaled - soc_points[i];
        int32_t ocv_diff = (int32_t)ocv_values[i+1] - (int32_t)ocv_values[i];
        *ocv = ocv_values[i] + (weight_numerator * ocv_diff) / soc_diff;
        break;
    }
}

软件架构

数据结构

1
2
3
4
5
6
7
8
9
10
11
12
13
14
typedef struct {
    uint32_t q_max;          // 电池容量 (mAh)
    uint32_t dod;            // 放电深度 (0-100%)
    uint32_t r_lf;           // 低频阻抗 (mΩ)
    int32_t t_ref;           // 参考温度 (0.1°C)
    uint32_t c_ref;          // 参考电容 (μF)
    int32_t v_k_curr[2];     // RC电压当前值 (mV)
    int32_t v_k_prev[2];     // RC电压前值 (mV)
    uint8_t initialized;     // 初始化标志
    uint8_t update_counter;  // 更新计数
    uint32_t r_lf_buffer[4]; // 滑动窗口
    uint8_t buffer_count;    // 窗口有效数
    uint8_t buffer_index;    // 窗口索引
} IT_HANDLE_T;

API设计

1
2
3
4
IT_ERROR_T it_init(IT_HANDLE_T *handle, IT_CONFIG_T *cfg);
IT_ERROR_T it_update(IT_HANDLE_T *handle, IT_PARAM_T *param, IT_RESULT_T *result);
IT_ERROR_T it_get_soc(IT_HANDLE_T *handle, uint32_t *soc);
IT_ERROR_T it_lookup_ocv(uint32_t soc, uint32_t *ocv);

it_update 是核心函数，每秒调用一次，输入当前V/I/T，输出SOC和阻抗信息。

资源占用

资源 占用
代码 (Flash) ~3.5KB
查表数据 (Flash) ~200B
运行时内存 (RAM) ~50B (IT_HANDLE_T)
计算时间 <1ms per update
CPU占用率 <0.1% (1s周期)

Python vs MCU 对比

维度 Python MCU C
算术类型 float64 (15位精度) int32_t (×1000, 3-4位)
exp() np.exp (IEEE 754) 5阶Taylor级数
sqrt() np.sqrt 牛顿迭代 (10次)
方程求解 scipy.fsolve (通用迭代) 二次方程公式解
内存 动态分配 (KB级) 静态50B
计算性能 ~ms (解释执行) <1ms (O(1) 编译执行)
代码体量 ~200行 ~800行 (含查表)
回退策略 无 (依赖fsolve收敛) 查表回退 + 跳变限制

工程经验总结

1. “扩大再除”技巧

整数除法的精度损失是最常见的bug来源。原则是：先乘后除，将被除数扩大到足够大再做除法。

1
2
3
4
5
// 错误：分子分母同量级，结果可能为0
int32_t result = (-B + sqrt_disc) / (2 * A);

// 正确：分子×1000扩大后除，得到毫欧单位结果
int32_t result = ((-B + sqrt_disc) * 1000) / (2 * A);

2. int/uint混用的隐蔽bug

温度差、温度系数、电容电压都可能为负。如果误用uint32_t，负数会变成极大的正数：

1
2
3
4
5
// 危险：temp_diff可能为负，uint32_t会回绕
uint32_t temp_diff = (temp - handle->t_ref) / 10;  // BUG!

// 正确
int32_t temp_diff = (temp - handle->t_ref) / 10;

3. 除零保护

查表插值、RC参数计算中多处可能出现0值除数：

1
2
3
if (c_k[i] > 0 && gamma_rk[i] > 0 && exp_rbk[i] > 0) {
    // 安全计算
}

4. 调试手段

条件编译的调试输出是嵌入式开发的标配：

1
2
3
4
5
6
7
#define IT_DEBUG 1

#ifdef IT_DEBUG
dbg_print("A=%d, B=%d, C=%d\r\n", A, B, C);
dbg_print("discriminant=%d, sqrt_disc=%d\r\n", discriminant, sqrt_disc);
dbg_print("R_LF SOLVE: sol1=%d, sol2=%d\r\n", sol1, sol2);
#endif

通过打印MCU中间变量，可以逐步对照Python仿真结果来定位精度偏差。

示例：大电流充放电端电压V_term跳变导致R跳变
通过打印中间变量定位原因：电池内阻是电池固有状态，放电电流的大小不应该影响真实电池内阻。但是调整电流时，端电压V_term会有大幅度跳变，会影响内阻计算，导致计算出的内阻跳变。属于算法在应用上的缺陷。

方案：平滑V_term，使用avg cellmv均值平滑电压参与阻抗方程计算。

image-20250804164236977