From 132f27a91adbbc2aa93a07cc5989c6b82018e5eb Mon Sep 17 00:00:00 2001 From: zouboan Date: Sun, 13 Feb 2022 13:46:40 +0800 Subject: [PATCH] lib_observer: add nolinear fluxlink observer --- include/dsp.h | 26 ++++- libs/libdsp/lib_observer.c | 195 ++++++++++++++++++++++++++++++++++++- 2 files changed, 217 insertions(+), 4 deletions(-) diff --git a/include/dsp.h b/include/dsp.h index 923dbceada..46102d0c82 100644 --- a/include/dsp.h +++ b/include/dsp.h @@ -293,12 +293,13 @@ struct motor_sobserver_div_f32_s }; /* Speed observer PLL method data */ -#if 0 + struct motor_sobserver_pll_f32_s { - /* TODO */ + float pll_phase; + float pll_kp; + float pll_ki; }; -#endif /* Motor Sliding Mode Observer private data */ @@ -320,6 +321,14 @@ struct motor_observer_smo_f32_s ab_frame_f32_t sign; /* Bang-bang controller sign */ }; +/* Motor Nonlinear FluxLink Observer private data */ + +struct motor_observer_nfo_f32_s +{ + float x1; + float x2; +}; + /* FOC initialize data */ struct foc_initdata_f32_s @@ -367,6 +376,7 @@ struct foc_data_f32_s struct motor_phy_params_f32_s { uint8_t p; /* Number of the motor pole pairs */ + float flux_link; /* Flux linkage */ float res; /* Phase-to-neutral resistance */ float ind; /* Average phase-to-neutral inductance */ float one_by_ind; /* Inverse phase-to-neutral inductance */ @@ -535,6 +545,16 @@ void motor_sobserver_div_init(FAR struct motor_sobserver_div_f32_s *so, void motor_sobserver_div(FAR struct motor_observer_f32_s *o, float angle, float dir); +void motor_observer_nfo_init(FAR struct motor_observer_nfo_f32_s *nfo); +void motor_observer_nfo(FAR struct motor_observer_f32_s *o, + FAR ab_frame_f32_t *i_ab, FAR ab_frame_f32_t *v_ab, + FAR struct motor_phy_params_f32_s *phy, float gain); + +void motor_sobserver_pll_init(FAR struct motor_sobserver_pll_f32_s *so, + float pll_kp, float pll_ki); +void motor_sobserver_pll(FAR struct motor_observer_f32_s *o, + float angle, float dir); + /* Motor openloop control */ void motor_openloop_init(FAR struct openloop_data_f32_s *op, float per); diff --git a/libs/libdsp/lib_observer.c b/libs/libdsp/lib_observer.c index 8e0546277b..dfc7e6e502 100644 --- a/libs/libdsp/lib_observer.c +++ b/libs/libdsp/lib_observer.c @@ -31,6 +31,15 @@ #define ANGLE_DIFF_THR (M_PI_F) +/* nan check for floats */ + +#define IS_NAN(x) ((x) != (x)) +#define NAN_ZERO(x) (x = IS_NAN(x) ? 0.0 : x) + +/* Squared */ + +#define SQ(x) ((x) * (x)) + /**************************************************************************** * Public Functions ****************************************************************************/ @@ -258,7 +267,7 @@ void motor_observer_smo(FAR struct motor_observer_f32_s *o, { filter = 0.99f; } - else if (filter <= 0.0f) + else if (filter < 0.005f) { filter = 0.005f; } @@ -495,6 +504,190 @@ void motor_sobserver_div(FAR struct motor_observer_f32_s *o, so->angle_prev = angle; } +/**************************************************************************** + * Name: motor_observer_nfo_init + * + * Description: + * Initialize motor nolinear fluxlink observer. + * + * Input Parameters: + * smo - pointer to the nolinear fluxlink observer private data + * kslide - SMO gain + * err_max - linear region upper limit + * + * Returned Value: + * None + * + ****************************************************************************/ + +void motor_observer_nfo_init(FAR struct motor_observer_nfo_f32_s *nfo) +{ + LIBDSP_DEBUGASSERT(smo != NULL); + + /* Reset structure */ + + memset(nfo, 0, sizeof(struct motor_observer_nfo_f32_s)); +} + +/**************************************************************************** + * Name: motor_observer_nfo + * + * Description: + * nolinear fluxlink observer. + * REFERENCE: http://cas.ensmp.fr/~praly/Telechargement/Journaux/ + * 2010-IEEE_TPEL-Lee-Hong-Nam-Ortega-Praly-Astolfi.pdf + * + * Input Parameters: + * o - (in/out) pointer to the common observer data + * i_ab - (in) inverter alpha-beta current + * v_ab - (in) inverter alpha-beta voltage + * phy - (in) pointer to the motor physical parameters + * gain - (in) dynamic observer gain + * + * Returned Value: + * None + * + ****************************************************************************/ + +void motor_observer_nfo(FAR struct motor_observer_f32_s *o, + FAR ab_frame_f32_t *i_ab, FAR ab_frame_f32_t *v_ab, + FAR struct motor_phy_params_f32_s *phy, float gain) +{ + FAR struct motor_observer_nfo_f32_s *nfo = + (FAR struct motor_observer_nfo_f32_s *)o->ao; + float angle; + float err; + float x1_dot; + float x2_dot; + + float l_ia = (3.0 / 2.0) * phy->ind * i_ab->a; + float l_ib = (3.0 / 2.0) * phy->ind * i_ab->b; + float r_ia = (3.0 / 2.0) * phy->res * i_ab->a; + float r_ib = (3.0 / 2.0) * phy->res * i_ab->b; + + err = SQ(phy->flux_link) - (SQ(nfo->x1 - l_ia) + SQ(nfo->x2 - l_ib)); + + /* Forcing this term to stay negative helps convergence according to + * http://cas.ensmp.fr/Publications/Publications/Papers/ + * ObserverPermanentMagnet.pdf and + * https://arxiv.org/pdf/1905.00833.pdf + */ + + if (err > 0.0) + { + err = 0.0; + } + + x1_dot = -r_ia + v_ab->a + gain * (nfo->x1 - l_ia) * err; + x2_dot = -r_ib + v_ab->b + gain * (nfo->x2 - l_ib) * err; + nfo->x1 += x1_dot * o->per; + nfo->x2 += x2_dot * o->per; + + NAN_ZERO(nfo->x1); + NAN_ZERO(nfo->x2); + + /* Prevent the magnitude from getting too low + * as that makes the angle very unstable. + */ + + if (vector2d_mag(nfo->x1, nfo->x2) < (phy->flux_link * 0.5)) + { + nfo->x1 *= 1.1; + nfo->x2 *= 1.1; + } + + angle = fast_atan2(nfo->x2 - l_ib, nfo->x1 - l_ia); + + /* Normalize angle to range <0, 2PI> */ + + angle_norm_2pi(&angle, 0.0f, 2.0f * M_PI_F); + + /* Store estimated angle in observer data */ + + o->angle = angle; +} + +/**************************************************************************** + * Name: motor_sobserver_pll_init + * + * Description: + * Initialize PLL speed observer + * + * Input Parameters: + * so - (in/out) pointer to the PLL speed observer data + * pll_kp - (in) pll proportional gain + * pll_ki - (in) pll integral gain + * + * Returned Value: + * None + * + ****************************************************************************/ + +void motor_sobserver_pll_init(FAR struct motor_sobserver_pll_f32_s *so, + float pll_kp, float pll_ki) +{ + LIBDSP_DEBUGASSERT(so != NULL); + LIBDSP_DEBUGASSERT(pll_kp > 0); + LIBDSP_DEBUGASSERT(pll_ki > 0.0f); + + /* Reset observer data */ + + memset(so, 0, sizeof(struct motor_sobserver_pll_f32_s)); + + /* Store kp for PLL observer */ + + so->pll_kp = pll_kp; + + /* Store ki for PLL observer speed */ + + so->pll_ki = pll_ki; +} + +/**************************************************************************** + * Name: motor_sobserver_pll + * + * Description: + * Estimate motor electrical speed based on motor electrical angle + * difference. + * + * Input Parameters: + * o - (in/out) pointer to the common observer data + * angle - (in) electrical angle normalized to <0.0, 2PI> + * dir - (in) electrical rotation direction. Valid values: + * DIR_CW (1.0f) or DIR_CCW(-1.0f) + * + ****************************************************************************/ + +void motor_sobserver_pll(FAR struct motor_observer_f32_s *o, + float angle, float dir) +{ + FAR struct motor_sobserver_pll_f32_s *so = + (FAR struct motor_sobserver_pll_f32_s *)o->so; + float delta_theta = 0.0; + + NAN_ZERO(so->pll_phase); + + /* Normalize angle to range <-PI, PI> */ + + angle_norm_2pi(&angle, -M_PI_F, -M_PI_F); + + delta_theta = angle - so->pll_phase; + + /* Normalize angle to range <-PI, PI> */ + + angle_norm_2pi(&delta_theta, -M_PI_F, -M_PI_F); + + NAN_ZERO(o->speed); + + so->pll_phase += (o->speed + so->pll_kp * delta_theta) * o->per; + + /* Normalize angle to range <-PI, PI> */ + + angle_norm_2pi(&so->pll_phase, -M_PI_F, -M_PI_F); + + o->speed += so->pll_ki * delta_theta * o->per; +} + /**************************************************************************** * Name: motor_observer_speed_get *