-
Notifications
You must be signed in to change notification settings - Fork 2
Expand file tree
/
Copy pathqpid.dox
More file actions
430 lines (430 loc) · 23.5 KB
/
qpid.dox
File metadata and controls
430 lines (430 loc) · 23.5 KB
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
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
/*! @page qpid_desc PID Controller
* @tableofcontents
* A Proportional Integral Derivative (PID) controller is an automatically
* optimized and accurate control system responsible for ensuring that a
* process remains as close to the desired value as possible regardless of
* various disruptions. The controller compares the measured process variable
* \f$ y(t)\f$ and a desired Setpoint \f$ r(t)\f$ (desired outcome). Based on
* that comparison, the controller calculates the Proportional, Integral and
* Derivative terms and adds them to obtain the output signal \f$ u(t) \f$ that
* attempts to minimize the error. This output signal is then used to command
* the Final Control Element (e.g an actuator) that is in charge of operating
* the process.
*
* Proportional is used to find out the error between the desired value and
* actual value and is responsible for the corrective response. Integral is
* applied to calculate all the past values of error and then integrate them to
* find out the Integral term. When error is expelled from the system this
* integral stops increasing. The derivative is used to predict the expected
* error values in the future based on the present values. Controlling effect can be
* increased if the system has a rapid rate of change, which is also based on
* Derivative. Combining all these three operations gives the name Proportional
* Integral Derivative (PID) Controller.
*
* qLibs provides the \ref qlibs::pidController implementation, which, apart from the simplified
* overview representation of the PID controller, it also addresses some practical
* issues and includes additional features as listed below:
*
* - Derivative filter
* - Anti-windup
* - Bumpless transfer
* - SetPoint weighting
* - Auto-tuning
* - Additive MRAC
*
* @section qpid_approach PID Controller approach
*
* The mathematical model and practical loop use a direct control action for all
* the PID terms, which means an increasing positive error results in an
* increasing positive control output correction.
*
* The overall control function implemented in \ref qlibs::pidMode::PID_AUTOMATIC mode is
* given by:
*
* <center>
* \f$ v(t)= \psi(t)r(t) + [ K_{c}e_{b}(t) + K_{i}\int [ e(t) + c(t-1) ]dt + K_{d}f_{d}(t) ] \f$
*
*
* \f$ u(t) = \text{Sat}[ v(t), u_{min}, u_{max} ]\f$
*
* </center>
* where \f$r(t)\f$ is the set-point, \f$y(t)\f$ the process output, \f$ e(t) \f$
* its the error \f$K_{c},K_{i},K_{d}\f$ the PID gains respectively and
* \f$u(t)\f$ the control action.
*
* \f$\psi(t)\f$ is the adaptive gain from the additive MRAC (later explained)
*
* As shown above, the derivative term \f$f_{d}(t)\f$ is the output of a
* low-pass filter that takes the raw derivative as input.
*
* <center> \f$ f_{d}(t) = \text{LPF}[ \frac{de_{c}(t)}{dt} ] = de_c(t) + e^{-dt/T_f} \left [ f_d(t-1) - de_c(t)\right ]\f$ </center>
*
* and \f$ c(t)\f$, the saturation feedback for the anti-windup, with \f$K_{w}\f$
* as the adjustment parameter.
*
* <center> \f$ c(t) = K_{w}[ u(t) - v(t) ] \f$ </center>
*
* <center>
* @htmlonly
* <!DOCTYPE html>
* <html>
* <head>
* <title>qpid</title>
* <meta charset="utf-8"/>
* </head>
* <body><div class="mxgraph" style="max-width:100%;border:1px solid transparent;" data-mxgraph="{"highlight":"#0000ff","nav":true,"zoom":0.85,"resize":true,"toolbar":"zoom layers tags lightbox","edit":"_blank","xml":"<mxfile host=\"app.diagrams.net\" modified=\"2022-10-31T00:47:47.906Z\" agent=\"5.0 (Windows NT 10.0; Win64; x64) AppleWebKit/537.36 (KHTML, like Gecko) Chrome/107.0.0.0 Safari/537.36\" etag=\"YkSCUyH4jOQ27EaRfF8V\" version=\"20.5.1\" type=\"google\"><diagram id=\"Y54Sw13SpVrxC9X4pJjL\" name=\"Página-1\">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</diagram></mxfile>"}"></div>
* <script type="text/javascript" src="https://viewer.diagrams.net/js/viewer-static.min.js"></script>
* </body>
* </html>
* @endhtmlonly
* <em>PID Implementation</em>
* </center>
*
* @section qpid_maninput Manual Mode
* When the operation mode is changed to \ref qlibs::pidMode::PID_MANUAL, the feedback is disconnected and
* the controller works in open loop being commanded by the manual input \f$ m(t) \f$.
* Since the controller is a dynamic system, it is necessary to make sure that
* the state of the system is correct when switching the controller between manual
* and automatic mode. When the system is in manual mode, the control algorithm
* produces a control signal that may be different from the manually generated
* control signal. It is necessary to make sure that the two outputs coincide at
* the time of switching. This is called bumpless transfer and it's given by
*
* <center> \f$ v(t) = \int [ K_{T}m(t) + c(t-1) ]dt\f$ </center>
*
* @section qpid_create Creating a PID Controller
*
* Before you start using a controller, it must be instantiated first
* via the \ref qlibs::pidController type and then configured using the \ref qlibs::pidController::setup() method,
* where you can define the PID gains and time step.
*
* Example: Instantiating an configuring a PID controller:
* @code{.c}
* pidController control;
* const real_t kc = 1.0f, ki = 0.1f, kd = 0.0f;
* const real_t dt = 0.01f; // time-step of 10mS
* bool ret;
*
* ret = control.setup( kc, ki, kd, dt );
* if ( !ret ) {
* // error, pid controller cant be configured
* }
* @endcode
*
*
* @section qfpid_usage Using the controller
*
* The following example illustrates how a PID controller can be used to regulate
* the speed of a DC motor. PID control operates on a separate task running
* periodically at a rate of 50mS. The speed measurement is read through
* an analog input and then scaled to the appropriate units (RPM). The \ref qlibs::pidController::control()
* function will be in charge of computing the control action and updating the internal
* states of the controller. Then, the control output gets scaled back in order to
* send the control command by using the PWM(Pulse Width Modulation) module.
*
* @subsection qpid_ex1 Example: Speed control using a PID controller:
* @code{.c}
* #include <iostream>
* #include "freertos/FreeRTOS.h"
* #include "freertos/task.h"
* #include "bsp.h"
* #include <qlibs.h>
*
* using namespace qlibs;
*
* const TickType_t dt = 50; //50mS time-step
* void xTaskPIDspeedControl( void *arg );
* real_t SetPoint = 300.0f; // desired motor speed 300rpm
*
* void xTaskPIDspeedControl( void *arg )
* {
* pidController *controller = static_cast<pidController *)( arg );
* real_t processMeasurement;
* real_t controlOutput;
* for ( ;; ) {
* processMeasurement = BSP_ScaletoSpeed( BSP_AnalogRead( BSP_AI_SPEED_CHANNEL ) );
* controlOutput = controller->control( SetPoint, processMeasurement );
* BSP_PWMSet( BSP_AO_SPEED_PWM_CHANNEL, BSP_ScaletoPWM( controlOutput ) );
* vTaskDelay( dt / portTICK_RATE_MS) ;
* }
* }
*
* int main( int argc, char *argv[] )
* {
* pidController speedControl;
* bool ret;
*
* BSP_SystemInit();
* ret = speedControl.setup( 1.0f, 0.1f, 0.0f, static_cast<real_t>( dt/1000.0f ) );
* if ( !ret ) {
* puts( "ERROR: Cant configure PID controller" );
* }
* speedControl.setSaturation( 0.0f, 100.0f );
* // Create the task that handles the speed control at the defined rate
* xTaskCreate( xTaskPIDspeedControl, "speedControl", 1024, &speedControl, configMAX_PRIORITIES - 1 , nullptr );
* vTaskStartScheduler();
* for( ;; );
* return EXIT_SUCCESS;
* }
* @endcode
*
*
* @section qpid_mrac Additive MRAC (Model Reference Adaptive Control)
*
* Model Reference Adaptive Control (MRAC) is a strategy where the controller
* of the closed-loop is adapted by an adjustment mechanism, which takes \f$ y_{m}(t) \f$
* from a reference model as input and tries to adjust the controller output \f$v(t)\f$.
* The adjustment mechanism used by \ref qpid is the enhanced MIT-rule approach,
* which adapts a feed-forward gain by the error between the system \f$ y(t) \f$
* and a reference model \f$ y_{m}(t) \f$, therefore is the so-called Gradient approach.
*
* <center>
* \f$ e_{m}(t) = y(t) - y_{m}(t)\f$
*
* \f$ \delta(t) = -\gamma \frac{ e_{m}(t) y_{m}(t) }{ \beta + y_{m}^{2}(t) } \f$
* </center>
*
* where \f$\gamma(t)\f$ is the adaptation gain and \f$\beta(t)\f$ is introduced
* to remove the problem of the possible division by zero if \f$y_{m}^{2}(t) \f$
* gets too small.
*
* The MRAC adaptation is then computed by integrating the \f$\delta(t)\f$ term
* as follows:
*
* <center> \f$ \psi(t) = \int [ \delta(t) + c(t) ] dt \f$ </center>
*
* This method can be used to adapt to slower changes but can become unstable for
* abrupt changes to the system. Therefore, this implementation uses a so-called
* modified MRAC (M-MRAC) where the adaptation is later added to a PID controller,
* being the MRAC the additive loop.
*
* Abrupt changes to the system are absorbed by the PID controller, while the
* change of the dynamics will still be handled by the MIT gain \f$\psi(t)\f$.
* This results in the equation of the control function presented at the
* beginning of this section.
*
* To use the additive MRAC, you should first instantiate a reference model and
* then, enable the MRAC by using the \ref qlibs::pidController::setModelReferenceControl()
* method. Here, you must provide the variable that stores the output of the reference model
* and the adaptation gain \f$\gamma(t)\f$
*
* @subsection qpid_ex2 Example: Speed control with PID controller with an additive MRAC:
* @code{.c}
* #include <stdio.h>
* #include <stdlib.h>
* #include "freertos/FreeRTOS.h"
* #include "freertos/task.h"
* #include "bsp.h"
* #include <qlibs.h>
*
* using namespace qlibs;
*
* #define REF_MODEL_ORDER ( 1 )
*
* const TickType_t dt = 50; //50mS time-step
* void xTaskPIDspeedControl( void *arg );
*
* void xTaskPIDspeedControl( void *arg )
* {
* picController *controller = static_cast<pidcontroller *>( arg );
* continuousTF<1> ctf= {
* { 0.0f, 1.0f },
* { 3.0f, 1.0f },
* };
* continuousSystem ref_model( ctf, 0.01f );
* real_t processMeasurement;
* real_t SetPoint = 300.0f; // desired motor speed 300rpm
* real_t refmodel_output = 0.0f;
* real_t controlOutput = 0.0f;
*
* controller->setModelReferenceControl( refmodel_output, 0.01f );
* for ( ;; ) {
* refmodel_output = ref_model->excite( controlOutput );
* processMeasurement = BSP_ScaletoSpeed ( BSP_AnalogRead( BSP_AI_SPEED_CHANNEL ) );
* controlOutput = controller->control( SetPoint, processMeasurement );
* BSP_PWMSet( BSP_AO_SPEED_PWM_CHANNEL, BSP_ScaletoPWM( controlOutput ) );
* vTaskDelay( dt / portTICK_RATE_MS) ;
* }
* }
*
* int main( int argc, char *argv[] )
* {
* picController speedControl;
* bool ret;
*
* BSP_SystemInit( );
* ret = speedControl.setup( 1.0f, 0.1f, 0.0f, static_cast<real_t>( dt/1000.0f ) );
* if ( !ret ) {
* puts( "ERROR: Cant configure PID controller" );
* }
* speedControl.setSaturation( 0.0f, 100.0f );
* // Create the task that handles the speed control at the defined rate
* xTaskCreate( xTaskPIDspeedControl, "speedControl", 1024, &speedControl, configMAX_PRIORITIES - 1 ,nullptr );
* vTaskStartScheduler();
* for( ;; );
* return EXIT_SUCCESS;
* }
* @endcode
*
* @section qpid_autotune Autotuning
*
* Autotuning eliminates much of the trial-and-error required by manual PID tuning,
* especially when loop dynamics are complex or the user lacks tuning experience.
* The procedure provides controller gains that are close to optimal, but a fine
* adjustment step may still be necessary to achieve the best performance.
*
* The Autotune feature is executed only for a limited time after activation;
* it is not a continuous background process. If the plant dynamics change
* significantly (e.g., due to load variation or environmental conditions),
* the autotuning sequence must be repeated to update the control gains.
*
* The underlying method is based on a Recursive Least Squares (RLS) identification
* of the process, followed by an analytical mapping to PID parameters. The RLS
* algorithm estimates a first-order process model, from which the controller
* computes suitable gains. The recursive update makes the method efficient for
* real-time embedded systems, as it avoids repeated excitation tests or large
* datasets.
*
* @note
* The estimation assumes a first-order stable process model. Therefore,
* this autotuning method is only reliable when applied to inherently
* stable processes. For unstable or integrating processes, the RLS
* identification may diverge and the computed PID gains may not be valid.
*
* The algorithm proceeds as follows:
*
* Compute the correction gain \f$ L(t) \f$
*
* \f$ L(t) = \frac{ P(t-1)\phi }{ \lambda + \phi^{T}P(t-1)\phi } \f$
*
* Perform correction
*
* \f$\theta(t) = \theta(t-1) + L[ y(t) - \phi^{T}\theta(t) ] \f$
*
* where,
*
* \f$ \theta(t) = \begin{bmatrix} \theta_{1}(t) & \theta_{2}(t) \end{bmatrix}^T \f$
* and \f$ \phi(t) = \begin{bmatrix} -y(t-1) & u(t-1) \end{bmatrix}^T \f$
*
* Update covariance
*
* \f$ P(t) = \lambda^{-1}[ I - L(t)+\phi^{T}]P(t-1) \f$
*
* Compute the system gain \f$ g(t) \f$
*
* \f$ g(t) = \frac{ (1 - \mu)\theta_{2}(t) }{ 1 + \theta_{1} } + \mu g(t-1) \f$
*
* Compute the system time-constant \f$ \tau(t) \f$
*
* \f$ \tau(t) = \frac{ ( \mu - 1 )dt }{ ln( |\theta_{1}| ) } + \mu \tau(t-1) \f$
*
* Compute the PID gains
*
* \f$ K_{c}(t) = \alpha \frac{ 1.35 }{ g(t) } \left ( \frac{ \tau(t) }{ T_d } + 0.185 \right ) \f$
*
* \f$ K_{i}(t) = K_{c}(t) \frac {\tau(t) + 0.611 T_d}{ 2.5 T_d( \tau(t) + 0.185 T_d)} \f$
*
* \f$ K_{d}(t) = \frac{ 0.37 K_{c}(t) T_d \tau(t) }{ \tau(t) + 0.185 T_d} \f$
*
*
* and the remaining parameters \f$\mu\f$, \f$\alpha\f$, \f$\lambda\f$ are internally
* computed for best performance.
*
* @subsection qpid_autotune_usage Autotuning Usage
*
* In order to use the autotuner, you must first instantiate the \ref qlibs::pidAutoTuning
* object and bind it to a previously configured PID controller by using the
* \ref qlibs::pidController::bindAutoTuning().
*
* After this, you can enable autotuning via \ref qlibs::pidController::enableAutoTuning()
* for a defined number of intervals. When the autotune ends, the resulting PID
* gains will be applied to the bounded controller.
*
* @subsection qpid_ex3 Example: Speed control with PID controller and autotuning:
* This example takes advantage of the FreeRTOS task notifications to enable the
* autotuning when a rising edge is detected on a certain digital input. Autotuning
* is enabled for 5 sec. Note that the setpoint is briefly modified during this
* process to stimulate the process. Upon completion, the setpoint is restored to
* its original value.
*
* @code{.c}
* #include <stdio.h>
* #include <stdlib.h>
* #include "freertos/FreeRTOS.h"
* #include "freertos/task.h"
* #include "bsp.h"
* #include <qlibs.h>
*
* using namespace qlibs;
*
* const TickType_t dt = 50; //50mS time-step
* void xTaskPIDspeedControl( void *arg );
* TaskHandle_t pid_task;
*
* void gpio_Int_Handler( void )
* {
* BaseType_t xHigherPriorityTaskWoken = pdFALSE;
*
* vTaskNotifyGiveFromISR( pid_task, &xHigherPriorityTaskWoken);
* portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
* HAL_GPIO_ClearStatusFlag();
* }
*
* void xTaskPIDspeedControl( void *arg )
* {
* picController *controller = static_cast<pidcontroller *>( arg );
* continuousTF<1> ctf= {
* { 0.0f, 1.0f },
* { 3.0f, 1.0f },
* };
* continuousSystem ref_model( ctf, 0.01f );
* real_t processMeasurement;
* real_t SetPoint = 300.0f; // desired motor speed 300rpm
* real_t tmpSetPoint = 320.0f;
* real_t *p_setpoint = &SetPoint;
* real_t refmodel_output = 0.0f;
* real_t controlOutput = 0.0f;
* uint32_t notification_autotune_enable;
*
* controller->setModelReferenceControl( refmodel_output, 0.01f );
* for ( ;; ) {
* notification_autotune_enable = ulTaskNotifyTake( pdTRUE, 0 ); // dont wait
* if ( notification_autotune_enable ) {
* p_setpoint = &tmpSetPoint;
* controller->enableAutoTuning( 100 ); //enable for 5 seconds ( 100*50mS )
* }
* if ( controller->isAutoTuningComplete() ) {
* p_setpoint = &SetPoint;
* controller->enableAutoTuning( 0 ); //disable
* }
* processMeasurement = BSP_ScaletoSpeed ( BSP_AnalogRead( BSP_AI_SPEED_CHANNEL ) );
* controlOutput = controller->control( *p_setpoint, processMeasurement );
* BSP_PWMSet( BSP_AO_SPEED_PWM_CHANNEL, BSP_ScaletoPWM( controlOutput ) );
* vTaskDelay( dt / portTICK_RATE_MS) ;
* }
* }
*
* int main( int argc, char *argv[] )
* {
* pidController speedControl;
* pidAutoTuning at;
* bool ret;
*
* BSP_SystemInit( );
* HAL_GPIO_Enable( GPIO12, GPIO_INPUT );
* HAL_GPIO_SetInterruptMode( GPIO12, RISING_EDGE );
* HAL_GPIO_EnableInterrupts( );
* ret = speedControl.setup( 1.0f, 0.1f, 0.0f, static_cast<real_t>( dt/1000.0 ) );
* if ( !ret ) {
* puts( "ERROR: Cant configure PID controller" );
* }
* speedControl.setSaturation( 0.0f, 100.0f );
* speedControl.bindAutoTuning( at );
* // Create the task that handles the speed control at the defined rate
* xTaskCreate( xTaskPIDspeedControl, "speedControl", 1024, &speedControl, configMAX_PRIORITIES - 1 , &pid_task );
* vTaskStartScheduler();
* for( ;; );
* return EXIT_SUCCESS;
* }
* @endcode
*/