FMT开源飞控开发(八)：电源管理与电池SOC估算

电池状态是飞控安全运行的关键指标。FMT通过ADC采样获取电池电压和电流，使用OCV-SOC查找表和递推最小二乘法（RLS）估算电池剩余电量（SOC），并通过多级阈值触发低电压保护。本文从硬件采样到算法实现，完整讲解电源管理系统。

1. 系统架构

┌──────────────┐ ADC采样 ┌──────────────┐ uMCN发布 ┌──────────────┐ │ 电池 │──────────────►│ PMU模块 │──────────────►│ 其他模块 │ │ (LiPo) │ 电压/电流 │ (power_manage)│ bat_status │ (FMS/GCS) │ └──────────────┘ └──────────────┘ └─────────────┘ │ │ mlog记录 ▼ ┌──────────────┐ │ 日志系统 │ └──────────────┘

2. 硬件接口

2.1 ADC采样

电池电压和电流通过ADC通道采集：

// power_manage.cstaticrt_device_tadc_dev;fmt_err_tpmu_init(void){adc_dev=rt_device_find("adc0");if(adc_dev!=NULL){rt_device_open(adc_dev,RT_DEVICE_FLAG_RDONLY);returnFMT_EOK;}returnFMT_ERROR;}

2.2 参数配置

// power_manage.cstaticparam_t__param_list[]={PARAM_FLOAT(BAT_V_DIV,1.0,false),// 电压分压比PARAM_FLOAT(BAT_A_PER_V,1.0,false),// 电流灵敏度 (A/V)PARAM_UINT8(BAT_N_CELLS,0,false),// 电池串数PARAM_FLOAT(CAPACITY_OF_BAT,3500.0,false),// 电池容量 (mAh)PARAM_FLOAT(VOLTAGE_EMPTY,3.0,false),// 放空电压 (V/cell)PARAM_FLOAT(VOLTAGE_CHARGED,4.2,false),// 充满电压 (V/cell)PARAM_FLOAT(R_INTERNAL_CONST,-0.005,false),// 内阻估计常数PARAM_FLOAT(R_INTERNAL,0.020,false),// 初始内阻 (Ω)PARAM_FLOAT(LOW_THR,3.5,false),// 低电压阈值 (V/cell)PARAM_FLOAT(CRIT_THR,3.4,false),// 严重低电压阈值PARAM_FLOAT(EMERGEN_THR,3.3,false),// 紧急低电压阈值};PARAM_GROUP_DEFINE(POWER,__param_list);

关键参数：

• BAT_V_DIV：电压分压比，将电池高压衰减到ADC量程内
• BAT_A_PER_V：电流传感器灵敏度，将电压转换为电流
• BAT_N_CELLS：电池串数，用于计算单体电压

3. 数据采集

3.1 电压电流读取

// power_manage.cfmt_err_tpmu_poll_battery_status(void){structbattery_statusbat_status;uint32_tvalue;// 读取电压ADCif(rt_device_read(adc_dev,BAT1_V_CHANNEL,&value,sizeof(value))==sizeof(value)){bat_status.battery_voltage=value*PARAM_GET_FLOAT(POWER,BAT_V_DIV);// mVbattery_update_voltage(&battery,bat_status.battery_voltage/1000.0f);}// 读取电流ADCif(rt_device_read(adc_dev,BAT1_I_CHANNEL,&value,sizeof(value))==sizeof(value)){bat_status.battery_current=value*PARAM_GET_FLOAT(POWER,BAT_A_PER_V);// mAbattery_update_current(&battery,bat_status.battery_current/1000.0f);}// 更新SOCbattery_update_state_of_charge(&battery,timestamps);// 发布到uMCNmcn_publish(MCN_HUB(bat_status),&bat_status);// 记录到mlogmlog_push_msg((uint8_t*)&bat_status,BATTERY_ID,sizeof(bat_status));returnFMT_EOK;}

3.2 数据滤波

电压和电流使用一阶低通滤波（Alpha Filter）：

// battery.c#defineAlpha_filter(alpha,new,old)((alpha)*(new)+(1.0f-(alpha))*(old))voidbattery_update_voltage(Battery*battery,floatvoltage_v){if(battery->voltage_v==0.0f){battery->voltage_v=voltage_v;// 首次赋值}else{battery->voltage_v=Alpha_filter(0.01f,voltage_v,battery->voltage_v);// 低通滤波}}voidbattery_update_current(Battery*battery,floatcurrent_a){if(battery->current_a==0.0f){battery->current_a=current_a;}else{battery->current_a=Alpha_filter(0.1f,current_a,battery->current_a);}}

电压滤波系数0.01（更平滑），电流滤波系数0.1（响应更快）。

4. SOC估算算法

4.1 OCV-SOC查找表

锂聚合物电池的开路电压（OCV）与SOC有确定的对应关系：

// battery.cconstOCV_SOC_Map ocv_soc_table[]={{4.200,100.0},// 满电{4.150,95.0},{4.100,90.0},{4.050,85.0},{4.000,80.0},{3.950,70.0},// 进入平坦区{3.900,60.0},{3.850,50.0},// 中间点{3.800,40.0},{3.750,30.0},// 离开平坦区{3.700,20.0},{3.650,15.0},{3.600,10.0},{3.500,5.0},{3.000,0.0}// 放空};

4.2 线性插值

// battery.cfloatestimate_soc_from_ocv(Battery*battery){floatvoltage=battery->cell_voltage;// 边界检查if(voltage>=ocv_soc_table[0].ocv)return100.0;if(voltage<=ocv_soc_table[SOC_TABLE_SIZE-1].ocv)return0.0;// 查找区间并线性插值for(inti=0;i<SOC_TABLE_SIZE-1;i++){if(voltage<=ocv_soc_table[i].ocv&&voltage>ocv_soc_table[i+1].ocv){floatv1=ocv_soc_table[i].ocv;floatv2=ocv_soc_table[i+1].ocv;floatsoc1=ocv_soc_table[i].soc;floatsoc2=ocv_soc_table[i+1].soc;returnsoc1+(soc2-soc1)*(voltage-v1)/(v2-v1);}}return0.0;}

4.3 单体电压计算

// battery.cvoidbattery_update_state_of_charge(Battery*battery,uint32_ttimestamp){// 计算单体电压if(battery->params.n_cells>0){battery->cell_voltage_origin=battery->voltage_v/battery->params.n_cells;// 补偿内阻压降battery->cell_voltage=battery->cell_voltage_origin+battery->internal_resistance*battery->current_a;}// OCV法估算SOCbattery->state_of_charge_volt_based=estimate_soc_from_ocv(battery);// 电流积分法估算SOC（库仑计数）floatdt=(timestamp-battery->last_timestamp)/1000.0f;// 秒floatconsumed_mah=battery->current_a*dt*1000.0f/3600.0f;battery->discharged_mah+=consumed_mah;if(battery->params.capacity>0){battery->state_of_charge_current_based=100.0f-(battery->discharged_mah/battery->params.capacity*100.0f);}// 融合两种估算方法battery->state_of_charge=battery->state_of_charge_volt_based;battery->last_timestamp=timestamp;}

4.4 递推最小二乘法（RLS）内阻估算

FMT使用RLS算法在线估算电池内阻：

// battery.cstaticfloat_RLS_est[2]={0.0f,0.0f};// [OCV, 内阻×串数]staticfloat_estimation_covariance[2][2];voidbattery_update_state_of_charge_RLS(Battery*battery){floatvoltage=battery->voltage_v;floatcurrent=battery->current_a;// RLS更新// 模型：V_measured = OCV - I * R_internal// 观测向量：h = [1, -I]// 参数向量：theta = [OCV, R_internal * n_cells]floath[2]={1.0f,-current};floaty=voltage;// 计算增益floatPh[2];Ph[0]=_estimation_covariance[0][0]*h[0]+_estimation_covariance[0][1]*h[1];Ph[1]=_estimation_covariance[1][0]*h[0]+_estimation_covariance[1][1]*h[1];floatdenom=1.0f+h[0]*Ph[0]+h[1]*Ph[1];floatK[2]={Ph[0]/denom,Ph[1]/denom};// 更新参数floatinnovation=y-(h[0]*_RLS_est[0]+h[1]*_RLS_est[1]);_RLS_est[0]+=K[0]*innovation;_RLS_est[1]+=K[1]*innovation;// 更新协方差矩阵// ... (标准RLS更新公式)// 提取内阻if(battery->params.n_cells>0){battery->internal_resistance=fabsf(_RLS_est[1])/battery->params.n_cells;}}

5. 低电压保护

5.1 多级警告

// battery.h#defineBATTERY_WARNING_NONE0#defineBATTERY_WARNING_LOW_VOLTAGE1// 低电压#defineBATTERY_WARNING_CRITICAL_VOLTAGE2// 严重低电压#defineBATTERY_WARNING_EMERGENCY_VOLTAGE3// 紧急

5.2 阈值判断

// battery.cvoidbattery_check_warning(Battery*battery){floatcell_v=battery->cell_voltage;if(cell_v<battery->params.emergen_thr){battery->warning=BATTERY_WARNING_EMERGENCY;}elseif(cell_v<battery->params.crit_thr){battery->warning=BATTERY_WARNING_CRITICAL;}elseif(cell_v<battery->params.low_thr){battery->warning=BATTERY_WARNING_LOW;}else{battery->warning=BATTERY_WARNING_NONE;}}

5.3 响应动作

6. mlog记录

电池状态通过mlog记录，用于事后分析：

// power_manage.cstaticmlog_elem_tBattery_Elems[]={MLOG_ELEMENT(timestamp,MLOG_UINT32),MLOG_ELEMENT(voltage,MLOG_FLOAT),MLOG_ELEMENT(current,MLOG_FLOAT),MLOG_ELEMENT(remaining,MLOG_FLOAT),// SOC (%)MLOG_ELEMENT(remaining_c,MLOG_FLOAT),// 电流积分法SOCMLOG_ELEMENT(remaining_v,MLOG_FLOAT),// OCV法SOCMLOG_ELEMENT(SOH,MLOG_FLOAT),// 健康度MLOG_ELEMENT(connected,MLOG_UINT8),MLOG_ELEMENT(init_SOC,MLOG_FLOAT),MLOG_ELEMENT(resistance,MLOG_FLOAT),// 内阻MLOG_ELEMENT(cvoltage,MLOG_FLOAT),// 单体电压MLOG_ELEMENT(cvoltage_orin,MLOG_FLOAT),// 原始单体电压};MLOG_BUS_DEFINE(BATTERY,Battery_Elems);

7. 电池状态结构

// power_manager.hstructbattery_status{uint32_ttimestamp;floatbattery_voltage;// 总电压 (mV)floatbattery_current;// 总电流 (mA)floatbattery_remaining;// SOC (%)floatremaining_c;// 电流积分法SOCfloatremaining_v;// OCV法SOCfloatstate_of_health;// 健康度 (%)uint8_tbattery_init;// 是否初始化uint8_tconnected;// 是否连接floatinit_SOC;// 初始SOCfloatinternal_resistance;// 内阻 (Ω)floatcell_voltage;// 单体电压 (V)floatcell_voltage_origin;// 原始单体电压（未补偿内阻）};

总结

FMT电源管理的核心设计要点：