mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
battery: add internal resistance estimation
This commit is contained in:
chfriedrich98
Silvan Fuhrer
parent
71029689e7
commit
f65653a391
@ -25,7 +25,6 @@
|
||||
param set-default BAT1_CAPACITY 4000
|
||||
param set-default BAT1_N_CELLS 6
|
||||
param set-default BAT1_V_EMPTY 3.3
|
||||
param set-default BAT1_V_LOAD_DROP 0.5
|
||||
param set-default BAT_AVRG_CURRENT 13
|
||||
|
||||
# Square quadrotor X PX4 numbering
|
||||
|
||||
@ -76,3 +76,11 @@ float32 design_capacity # The design capacity of the battery
|
||||
uint16 average_time_to_full # The predicted remaining time until the battery reaches full charge, in minutes
|
||||
uint16 over_discharge_count # Number of battery overdischarge
|
||||
float32 nominal_voltage # Nominal voltage of the battery pack
|
||||
|
||||
float32 internal_resistance_estimate # [Ohm] Internal resistance per cell estimate
|
||||
float32 ocv_estimate # [V] Open circuit voltage estimate
|
||||
float32 ocv_estimate_filtered # [V] Filtered open circuit voltage estimate
|
||||
float32 volt_based_soc_estimate # [0, 1] Normalized volt based state of charge estimate
|
||||
float32 voltage_prediction # [V] Predicted voltage
|
||||
float32 prediction_error # [V] Prediction error
|
||||
float32 estimation_covariance_norm # Norm of the covariance matrix
|
||||
|
||||
@ -46,6 +46,7 @@
|
||||
#include <px4_platform_common/defines.h>
|
||||
|
||||
using namespace time_literals;
|
||||
using namespace matrix;
|
||||
|
||||
Battery::Battery(int index, ModuleParams *parent, const int sample_interval_us, const uint8_t source) :
|
||||
ModuleParams(parent),
|
||||
@ -53,10 +54,9 @@ Battery::Battery(int index, ModuleParams *parent, const int sample_interval_us,
|
||||
_source(source)
|
||||
{
|
||||
const float expected_filter_dt = static_cast<float>(sample_interval_us) / 1_s;
|
||||
_voltage_filter_v.setParameters(expected_filter_dt, 1.f);
|
||||
_current_filter_a.setParameters(expected_filter_dt, .5f);
|
||||
_current_average_filter_a.setParameters(expected_filter_dt, 50.f);
|
||||
_throttle_filter.setParameters(expected_filter_dt, 1.f);
|
||||
_ocv_filter_v.setParameters(expected_filter_dt, 1.f);
|
||||
_cell_voltage_filter_v.setParameters(expected_filter_dt, 1.f);
|
||||
|
||||
if (index > 9 || index < 1) {
|
||||
PX4_ERR("Battery index must be between 1 and 9 (inclusive). Received %d. Defaulting to 1.", index);
|
||||
@ -81,9 +81,6 @@ Battery::Battery(int index, ModuleParams *parent, const int sample_interval_us,
|
||||
snprintf(param_name, sizeof(param_name), "BAT%d_CAPACITY", _index);
|
||||
_param_handles.capacity = param_find(param_name);
|
||||
|
||||
snprintf(param_name, sizeof(param_name), "BAT%d_V_LOAD_DROP", _index);
|
||||
_param_handles.v_load_drop = param_find(param_name);
|
||||
|
||||
snprintf(param_name, sizeof(param_name), "BAT%d_R_INTERNAL", _index);
|
||||
_param_handles.r_internal = param_find(param_name);
|
||||
|
||||
@ -97,29 +94,36 @@ Battery::Battery(int index, ModuleParams *parent, const int sample_interval_us,
|
||||
_param_handles.bat_avrg_current = param_find("BAT_AVRG_CURRENT");
|
||||
|
||||
updateParams();
|
||||
|
||||
// Internal resistance estimation initializations
|
||||
_RLS_est(0) = OCV_DEFAULT * _params.n_cells;
|
||||
_RLS_est(1) = R_DEFAULT * _params.n_cells;
|
||||
_estimation_covariance(0, 0) = OCV_COVARIANCE * _params.n_cells;
|
||||
_estimation_covariance(0, 1) = 0.f;
|
||||
_estimation_covariance(1, 0) = 0.f;
|
||||
_estimation_covariance(1, 1) = R_COVARIANCE * _params.n_cells;
|
||||
_estimation_covariance_norm = sqrtf(powf(_estimation_covariance(0, 0), 2.f) + 2.f * powf(_estimation_covariance(1, 0),
|
||||
2.f) + powf(_estimation_covariance(1, 1), 2.f));
|
||||
}
|
||||
|
||||
void Battery::updateVoltage(const float voltage_v)
|
||||
{
|
||||
_voltage_v = voltage_v;
|
||||
_voltage_filter_v.update(voltage_v);
|
||||
}
|
||||
|
||||
void Battery::updateCurrent(const float current_a)
|
||||
{
|
||||
_current_a = current_a;
|
||||
_current_filter_a.update(current_a);
|
||||
}
|
||||
|
||||
void Battery::updateBatteryStatus(const hrt_abstime ×tamp)
|
||||
{
|
||||
if (!_battery_initialized) {
|
||||
_voltage_filter_v.reset(_voltage_v);
|
||||
_current_filter_a.reset(_current_a);
|
||||
resetInternalResistanceEstimation(_voltage_v, _current_a);
|
||||
}
|
||||
|
||||
// Require minimum voltage otherwise override connected status
|
||||
if (_voltage_filter_v.getState() < LITHIUM_BATTERY_RECOGNITION_VOLTAGE) {
|
||||
if (_voltage_v < LITHIUM_BATTERY_RECOGNITION_VOLTAGE) {
|
||||
_connected = false;
|
||||
}
|
||||
|
||||
@ -132,7 +136,7 @@ void Battery::updateBatteryStatus(const hrt_abstime ×tamp)
|
||||
|
||||
sumDischarged(timestamp, _current_a);
|
||||
_state_of_charge_volt_based =
|
||||
calculateStateOfChargeVoltageBased(_voltage_filter_v.getState(), _current_filter_a.getState());
|
||||
calculateStateOfChargeVoltageBased(_voltage_v, _current_a);
|
||||
|
||||
if (!_external_state_of_charge) {
|
||||
estimateStateOfCharge();
|
||||
@ -149,9 +153,7 @@ battery_status_s Battery::getBatteryStatus()
|
||||
{
|
||||
battery_status_s battery_status{};
|
||||
battery_status.voltage_v = _voltage_v;
|
||||
battery_status.voltage_filtered_v = _voltage_filter_v.getState();
|
||||
battery_status.current_a = _current_a;
|
||||
battery_status.current_filtered_a = _current_filter_a.getState();
|
||||
battery_status.current_average_a = _current_average_filter_a.getState();
|
||||
battery_status.discharged_mah = _discharged_mah;
|
||||
battery_status.remaining = _state_of_charge;
|
||||
@ -167,6 +169,14 @@ battery_status_s Battery::getBatteryStatus()
|
||||
battery_status.warning = _warning;
|
||||
battery_status.timestamp = hrt_absolute_time();
|
||||
battery_status.faults = determineFaults();
|
||||
battery_status.internal_resistance_estimate = _internal_resistance_estimate;
|
||||
battery_status.ocv_estimate = _voltage_v + _internal_resistance_estimate * _params.n_cells * _current_a;
|
||||
battery_status.ocv_estimate_filtered = _ocv_filter_v.getState();
|
||||
battery_status.volt_based_soc_estimate = math::interpolate(_ocv_filter_v.getState() / _params.n_cells,
|
||||
_params.v_empty, _params.v_charged, 0.f, 1.f);
|
||||
battery_status.voltage_prediction = _voltage_prediction;
|
||||
battery_status.prediction_error = _prediction_error;
|
||||
battery_status.estimation_covariance_norm = _estimation_covariance_norm;
|
||||
return battery_status;
|
||||
}
|
||||
|
||||
@ -213,27 +223,69 @@ float Battery::calculateStateOfChargeVoltageBased(const float voltage_v, const f
|
||||
// remaining battery capacity based on voltage
|
||||
float cell_voltage = voltage_v / _params.n_cells;
|
||||
|
||||
// correct battery voltage locally for load drop to avoid estimation fluctuations
|
||||
if (_params.r_internal >= 0.f && current_a > FLT_EPSILON) {
|
||||
cell_voltage += _params.r_internal * current_a;
|
||||
// correct battery voltage locally for load drop according to internal resistance and current
|
||||
if (current_a > FLT_EPSILON) {
|
||||
updateInternalResistanceEstimation(voltage_v, current_a);
|
||||
|
||||
} else {
|
||||
vehicle_thrust_setpoint_s vehicle_thrust_setpoint{};
|
||||
_vehicle_thrust_setpoint_0_sub.copy(&vehicle_thrust_setpoint);
|
||||
const matrix::Vector3f thrust_setpoint = matrix::Vector3f(vehicle_thrust_setpoint.xyz);
|
||||
const float throttle = thrust_setpoint.length();
|
||||
if (_params.r_internal >= 0.f) { // Use user specified internal resistance value
|
||||
cell_voltage += _params.r_internal * current_a;
|
||||
|
||||
_throttle_filter.update(throttle);
|
||||
|
||||
if (!_battery_initialized) {
|
||||
_throttle_filter.reset(throttle);
|
||||
} else { // Use estimated internal resistance value
|
||||
cell_voltage += _internal_resistance_estimate * current_a;
|
||||
}
|
||||
|
||||
// assume linear relation between throttle and voltage drop
|
||||
cell_voltage += throttle * _params.v_load_drop;
|
||||
}
|
||||
|
||||
return math::interpolate(cell_voltage, _params.v_empty, _params.v_charged, 0.f, 1.f);
|
||||
_cell_voltage_filter_v.update(cell_voltage);
|
||||
return math::interpolate(_cell_voltage_filter_v.getState(), _params.v_empty, _params.v_charged, 0.f, 1.f);
|
||||
}
|
||||
|
||||
void Battery::updateInternalResistanceEstimation(const float voltage_v, const float current_a)
|
||||
{
|
||||
Vector2f x{1, -current_a};
|
||||
_voltage_prediction = (x.transpose() * _RLS_est)(0, 0);
|
||||
_prediction_error = voltage_v - _voltage_prediction;
|
||||
const Vector2f gamma = _estimation_covariance * x / (LAMBDA + (x.transpose() * _estimation_covariance * x)(0, 0));
|
||||
const Vector2f RSL_est_temp = _RLS_est + gamma * _prediction_error;
|
||||
const Matrix2f estimation_covariance_temp = (_estimation_covariance
|
||||
- Matrix<float, 2, 1>(gamma) * (x.transpose() * _estimation_covariance)) / LAMBDA;
|
||||
const float estimation_covariance_temp_norm =
|
||||
sqrtf(powf(estimation_covariance_temp(0, 0), 2.f)
|
||||
+ 2.f * powf(estimation_covariance_temp(1, 0), 2.f)
|
||||
+ powf(estimation_covariance_temp(1, 1), 2.f));
|
||||
|
||||
if (estimation_covariance_temp_norm < _estimation_covariance_norm) { // Only update if estimation improves
|
||||
_RLS_est = RSL_est_temp;
|
||||
_estimation_covariance = estimation_covariance_temp;
|
||||
_estimation_covariance_norm = estimation_covariance_temp_norm;
|
||||
_internal_resistance_estimate =
|
||||
math::max(_RLS_est(1) / _params.n_cells, 0.f); // Only use positive values
|
||||
|
||||
} else { // Update OCV estimate with IR estimate
|
||||
_RLS_est(0) = voltage_v + _RLS_est(1) * current_a;
|
||||
}
|
||||
|
||||
_ocv_filter_v.update(voltage_v + _internal_resistance_estimate * _params.n_cells * current_a);
|
||||
}
|
||||
|
||||
void Battery::resetInternalResistanceEstimation(const float voltage_v, const float current_a)
|
||||
{
|
||||
_RLS_est(0) = voltage_v;
|
||||
_RLS_est(1) = R_DEFAULT * _params.n_cells;
|
||||
_estimation_covariance.setZero();
|
||||
_estimation_covariance(0, 0) = OCV_COVARIANCE * _params.n_cells;
|
||||
_estimation_covariance(1, 1) = R_COVARIANCE * _params.n_cells;
|
||||
_estimation_covariance_norm = sqrtf(powf(_estimation_covariance(0, 0), 2.f) + 2.f * powf(_estimation_covariance(1, 0),
|
||||
2.f) + powf(_estimation_covariance(1, 1), 2.f));
|
||||
_internal_resistance_estimate = R_DEFAULT;
|
||||
_ocv_filter_v.reset(voltage_v + _internal_resistance_estimate * _params.n_cells * current_a);
|
||||
|
||||
if (_params.r_internal >= 0.f) { // Use user specified internal resistance value
|
||||
_cell_voltage_filter_v.reset(voltage_v / _params.n_cells + _params.r_internal * current_a);
|
||||
|
||||
} else { // Use estimated internal resistance value
|
||||
_cell_voltage_filter_v.reset(voltage_v / _params.n_cells + _internal_resistance_estimate * current_a);
|
||||
}
|
||||
}
|
||||
|
||||
void Battery::estimateStateOfCharge()
|
||||
@ -354,7 +406,6 @@ void Battery::updateParams()
|
||||
param_get(_param_handles.v_charged, &_params.v_charged);
|
||||
param_get(_param_handles.n_cells, &_params.n_cells);
|
||||
param_get(_param_handles.capacity, &_params.capacity);
|
||||
param_get(_param_handles.v_load_drop, &_params.v_load_drop);
|
||||
param_get(_param_handles.r_internal, &_params.r_internal);
|
||||
param_get(_param_handles.source, &_params.source);
|
||||
param_get(_param_handles.low_thr, &_params.low_thr);
|
||||
|
||||
@ -58,7 +58,6 @@
|
||||
#include <uORB/topics/battery_status.h>
|
||||
#include <uORB/topics/flight_phase_estimation.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/vehicle_thrust_setpoint.h>
|
||||
|
||||
/**
|
||||
* BatteryBase is a base class for any type of battery.
|
||||
@ -118,7 +117,6 @@ protected:
|
||||
param_t v_charged;
|
||||
param_t n_cells;
|
||||
param_t capacity;
|
||||
param_t v_load_drop;
|
||||
param_t r_internal;
|
||||
param_t low_thr;
|
||||
param_t crit_thr;
|
||||
@ -132,7 +130,6 @@ protected:
|
||||
float v_charged;
|
||||
int32_t n_cells;
|
||||
float capacity;
|
||||
float v_load_drop;
|
||||
float r_internal;
|
||||
float low_thr;
|
||||
float crit_thr;
|
||||
@ -155,7 +152,6 @@ private:
|
||||
void computeScale();
|
||||
float computeRemainingTime(float current_a);
|
||||
|
||||
uORB::Subscription _vehicle_thrust_setpoint_0_sub{ORB_ID(vehicle_thrust_setpoint)};
|
||||
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
|
||||
uORB::SubscriptionData<flight_phase_estimation_s> _flight_phase_estimation_sub{ORB_ID(flight_phase_estimation)};
|
||||
uORB::PublicationMulti<battery_status_s> _battery_status_pub{ORB_ID(battery_status)};
|
||||
@ -167,12 +163,11 @@ private:
|
||||
uint8_t _priority{0};
|
||||
bool _battery_initialized{false};
|
||||
float _voltage_v{0.f};
|
||||
AlphaFilter<float> _voltage_filter_v;
|
||||
AlphaFilter<float> _ocv_filter_v;
|
||||
AlphaFilter<float> _cell_voltage_filter_v;
|
||||
float _current_a{-1};
|
||||
AlphaFilter<float> _current_filter_a;
|
||||
AlphaFilter<float>
|
||||
_current_average_filter_a; ///< averaging filter for current. For FW, it is the current in level flight.
|
||||
AlphaFilter<float> _throttle_filter;
|
||||
float _discharged_mah{0.f};
|
||||
float _discharged_mah_loop{0.f};
|
||||
float _state_of_charge_volt_based{-1.f}; // [0,1]
|
||||
@ -183,4 +178,19 @@ private:
|
||||
bool _armed{false};
|
||||
bool _vehicle_status_is_fw{false};
|
||||
hrt_abstime _last_unconnected_timestamp{0};
|
||||
|
||||
// Internal Resistance estimation
|
||||
void updateInternalResistanceEstimation(const float voltage_v, const float current_a);
|
||||
void resetInternalResistanceEstimation(const float voltage_v, const float current_a);
|
||||
matrix::Vector2f _RLS_est; // [Open circuit voltage estimate [V], Total internal resistance estimate [Ohm]]^T
|
||||
matrix::Matrix2f _estimation_covariance;
|
||||
float _estimation_covariance_norm{0.f};
|
||||
float _internal_resistance_estimate{0.005f}; // [Ohm] Per cell estimate of the internal resistance
|
||||
float _voltage_prediction{0.f}; // [V] Predicted voltage of the estimator
|
||||
float _prediction_error{0.f}; // [V] Error between the predicted and measured voltage
|
||||
static constexpr float LAMBDA = 0.95f; // [0, 1] Forgetting factor (Tuning parameter for the RLS algorithm)
|
||||
static constexpr float R_DEFAULT = 0.005f; // [Ohm] Initial per cell estimate of the internal resistance
|
||||
static constexpr float OCV_DEFAULT = 4.2f; // [V] Initial per cell estimate of the open circuit voltage
|
||||
static constexpr float R_COVARIANCE = 0.1f; // Initial per cell covariance of the internal resistance
|
||||
static constexpr float OCV_COVARIANCE = 1.5f; // Initial per cell covariance of the open circuit voltage
|
||||
};
|
||||
|
||||
@ -39,33 +39,11 @@ parameters:
|
||||
instance_start: 1
|
||||
default: [4.05, 4.05, 4.05]
|
||||
|
||||
BAT${i}_V_LOAD_DROP:
|
||||
description:
|
||||
short: Voltage drop per cell on full throttle
|
||||
long: |
|
||||
This implicitly defines the internal resistance
|
||||
to maximum current ratio for the battery and assumes linearity.
|
||||
A good value to use is the difference between the
|
||||
5C and 20-25C load. Not used if BAT${i}_R_INTERNAL is
|
||||
set.
|
||||
|
||||
type: float
|
||||
unit: V
|
||||
min: 0.07
|
||||
max: 0.5
|
||||
decimal: 2
|
||||
increment: 0.01
|
||||
reboot_required: true
|
||||
num_instances: *max_num_config_instances
|
||||
instance_start: 1
|
||||
default: [0.1, 0.1, 0.1]
|
||||
|
||||
BAT${i}_R_INTERNAL:
|
||||
description:
|
||||
short: Explicitly defines the per cell internal resistance for battery ${i}
|
||||
long: |
|
||||
If non-negative, then this will be used in place of
|
||||
BAT${i}_V_LOAD_DROP for all calculations.
|
||||
If non-negative, then this will be used instead of the online estimated internal resistance.
|
||||
|
||||
type: float
|
||||
unit: Ohm
|
||||
@ -76,7 +54,7 @@ parameters:
|
||||
reboot_required: true
|
||||
num_instances: *max_num_config_instances
|
||||
instance_start: 1
|
||||
default: [0.005, 0.005, 0.005]
|
||||
default: [-1.0, -1.0, -1.0]
|
||||
|
||||
BAT${i}_N_CELLS:
|
||||
description:
|
||||
|
||||
@ -618,6 +618,7 @@ SquareMatrix <Type, M> choleskyInv(const SquareMatrix<Type, M> &A)
|
||||
return L_inv.T() * L_inv;
|
||||
}
|
||||
|
||||
using Matrix2f = SquareMatrix<float, 2>;
|
||||
using Matrix3f = SquareMatrix<float, 3>;
|
||||
using Matrix3d = SquareMatrix<double, 3>;
|
||||
|
||||
|
||||
@ -73,7 +73,6 @@ class Ekf final : public EstimatorInterface
|
||||
public:
|
||||
typedef matrix::Vector<float, State::size> VectorState;
|
||||
typedef matrix::SquareMatrix<float, State::size> SquareMatrixState;
|
||||
typedef matrix::SquareMatrix<float, 2> Matrix2f;
|
||||
|
||||
Ekf()
|
||||
{
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user