|
1 | | -__all__ = ['sisotool'] |
| 1 | +__all__ = ['sisotool', 'rootlocus_pid_designer'] |
2 | 2 |
|
3 | 3 | from control.exception import ControlMIMONotImplemented |
4 | 4 | from .freqplot import bode_plot |
5 | 5 | from .timeresp import step_response |
6 | 6 | from .lti import issiso, isdtime |
7 | | -from .xferfcn import TransferFunction |
| 7 | +from .xferfcn import tf |
| 8 | +from .statesp import ss |
8 | 9 | from .bdalg import append, connect |
| 10 | +from .iosys import tf2io, ss2io, summing_junction, interconnect |
| 11 | +from control.statesp import _convert_to_statespace, StateSpace |
| 12 | +from control.lti import common_timebase, isctime |
9 | 13 | import matplotlib |
10 | 14 | import matplotlib.pyplot as plt |
11 | 15 | import warnings |
@@ -176,3 +180,156 @@ def _SisotoolUpdate(sys, fig, K, bode_plot_params, tvect=None): |
176 | 180 | fig.subplots_adjust(top=0.9,wspace = 0.3,hspace=0.35) |
177 | 181 | fig.canvas.draw() |
178 | 182 |
|
| 183 | +# contributed by Sawyer Fuller, minster@uw.edu 2021.11.02, based on |
| 184 | +# an implementation in Matlab by Martin Berg. |
| 185 | +def rootlocus_pid_designer(plant, gain='P', sign=+1, input_signal='r', |
| 186 | + Kp0=0, Ki0=0, Kd0=0, tau=0.01, |
| 187 | + C_ff=0, derivative_in_feedback_path=False, |
| 188 | + plot=True): |
| 189 | + """Manual PID controller design based on root locus using Sisotool |
| 190 | +
|
| 191 | + Uses `Sisotool` to investigate the effect of adding or subtracting an |
| 192 | + amount `deltaK` to the proportional, integral, or derivative (PID) gains of |
| 193 | + a controller. One of the PID gains, `Kp`, `Ki`, or `Kd`, respectively, can |
| 194 | + be modified at a time. `Sisotool` plots the step response, frequency |
| 195 | + response, and root locus. |
| 196 | +
|
| 197 | + When first run, `deltaK` is set to 0; click on a branch of the root locus |
| 198 | + plot to try a different value. Each click updates plots and prints |
| 199 | + the corresponding `deltaK`. To tune all three PID gains, repeatedly call |
| 200 | + `rootlocus_pid_designer`, and select a different `gain` each time (`'P'`, |
| 201 | + `'I'`, or `'D'`). Make sure to add the resulting `deltaK` to your chosen |
| 202 | + initial gain on the next iteration. |
| 203 | +
|
| 204 | + Example: to examine the effect of varying `Kp` starting from an intial |
| 205 | + value of 10, use the arguments `gain='P', Kp0=10`. Suppose a `deltaK` |
| 206 | + value of 5 gives satisfactory performance. Then on the next iteration, |
| 207 | + to tune the derivative gain, use the arguments `gain='D', Kp0=15`. |
| 208 | +
|
| 209 | + By default, all three PID terms are in the forward path C_f in the diagram |
| 210 | + shown below, that is, |
| 211 | +
|
| 212 | + C_f = Kp + Ki/s + Kd*s/(tau*s + 1). |
| 213 | +
|
| 214 | + If `plant` is a discrete-time system, then the proportional, integral, and |
| 215 | + derivative terms are given instead by Kp, Ki*dt/2*(z+1)/(z-1), and |
| 216 | + Kd/dt*(z-1)/z, respectively. |
| 217 | +
|
| 218 | + ------> C_ff ------ d |
| 219 | + | | | |
| 220 | + r | e V V u y |
| 221 | + ------->O---> C_f --->O--->O---> plant ---> |
| 222 | + ^- ^- | |
| 223 | + | | | |
| 224 | + | ----- C_b <-------| |
| 225 | + --------------------------------- |
| 226 | +
|
| 227 | + It is also possible to move the derivative term into the feedback path |
| 228 | + `C_b` using `derivative_in_feedback_path=True`. This may be desired to |
| 229 | + avoid that the plant is subject to an impulse function when the reference |
| 230 | + `r` is a step input. `C_b` is otherwise set to zero. |
| 231 | +
|
| 232 | + If `plant` is a 2-input system, the disturbance `d` is fed directly into |
| 233 | + its second input rather than being added to `u`. |
| 234 | +
|
| 235 | + Remark: It may be helpful to zoom in using the magnifying glass on the |
| 236 | + plot. Just ake sure to deactivate magnification mode when you are done by |
| 237 | + clicking the magnifying glass. Otherwise you will not be able to be able to choose |
| 238 | + a gain on the root locus plot. |
| 239 | +
|
| 240 | + Parameters |
| 241 | + ---------- |
| 242 | + plant : :class:`LTI` (:class:`TransferFunction` or :class:`StateSpace` system) |
| 243 | + The dynamical system to be controlled |
| 244 | + gain : string (optional) |
| 245 | + Which gain to vary by `deltaK`. Must be one of `'P'`, `'I'`, or `'D'` |
| 246 | + (proportional, integral, or derative) |
| 247 | + sign : int (optional) |
| 248 | + The sign of deltaK gain perturbation |
| 249 | + input : string (optional) |
| 250 | + The input used for the step response; must be `'r'` (reference) or |
| 251 | + `'d'` (disturbance) (see figure above) |
| 252 | + Kp0, Ki0, Kd0 : float (optional) |
| 253 | + Initial values for proportional, integral, and derivative gains, |
| 254 | + respectively |
| 255 | + tau : float (optional) |
| 256 | + The time constant associated with the pole in the continuous-time |
| 257 | + derivative term. This is required to make the derivative transfer |
| 258 | + function proper. |
| 259 | + C_ff : float or :class:`LTI` system (optional) |
| 260 | + Feedforward controller. If :class:`LTI`, must have timebase that is |
| 261 | + compatible with plant. |
| 262 | + derivative_in_feedback_path : bool (optional) |
| 263 | + Whether to place the derivative term in feedback transfer function |
| 264 | + `C_b` instead of the forward transfer function `C_f`. |
| 265 | + plot : bool (optional) |
| 266 | + Whether to create Sisotool interactive plot. |
| 267 | +
|
| 268 | + Returns |
| 269 | + ---------- |
| 270 | + closedloop : class:`StateSpace` system |
| 271 | + The closed-loop system using initial gains. |
| 272 | + """ |
| 273 | + |
| 274 | + plant = _convert_to_statespace(plant) |
| 275 | + if plant.ninputs == 1: |
| 276 | + plant = ss2io(plant, inputs='u', outputs='y') |
| 277 | + elif plant.ninputs == 2: |
| 278 | + plant = ss2io(plant, inputs=['u', 'd'], outputs='y') |
| 279 | + else: |
| 280 | + raise ValueError("plant must have one or two inputs") |
| 281 | + C_ff = ss2io(_convert_to_statespace(C_ff), inputs='r', outputs='uff') |
| 282 | + dt = common_timebase(plant, C_ff) |
| 283 | + |
| 284 | + # create systems used for interconnections |
| 285 | + e_summer = summing_junction(['r', '-y'], 'e') |
| 286 | + if plant.ninputs == 2: |
| 287 | + u_summer = summing_junction(['ufb', 'uff'], 'u') |
| 288 | + else: |
| 289 | + u_summer = summing_junction(['ufb', 'uff', 'd'], 'u') |
| 290 | + |
| 291 | + if isctime(plant): |
| 292 | + prop = tf(1, 1) |
| 293 | + integ = tf(1, [1, 0]) |
| 294 | + deriv = tf([1, 0], [tau, 1]) |
| 295 | + else: # discrete-time |
| 296 | + prop = tf(1, 1, dt) |
| 297 | + integ = tf([dt/2, dt/2], [1, -1], dt) |
| 298 | + deriv = tf([1, -1], [dt, 0], dt) |
| 299 | + |
| 300 | + # add signal names by turning into iosystems |
| 301 | + prop = tf2io(prop, inputs='e', outputs='prop_e') |
| 302 | + integ = tf2io(integ, inputs='e', outputs='int_e') |
| 303 | + if derivative_in_feedback_path: |
| 304 | + deriv = tf2io(-deriv, inputs='y', outputs='deriv') |
| 305 | + else: |
| 306 | + deriv = tf2io(deriv, inputs='e', outputs='deriv') |
| 307 | + |
| 308 | + # create gain blocks |
| 309 | + Kpgain = tf2io(tf(Kp0, 1), inputs='prop_e', outputs='ufb') |
| 310 | + Kigain = tf2io(tf(Ki0, 1), inputs='int_e', outputs='ufb') |
| 311 | + Kdgain = tf2io(tf(Kd0, 1), inputs='deriv', outputs='ufb') |
| 312 | + |
| 313 | + # for the gain that is varied, replace gain block with a special block |
| 314 | + # that has an 'input' and an 'output' that creates loop transfer function |
| 315 | + if gain in ('P', 'p'): |
| 316 | + Kpgain = ss2io(ss([],[],[],[[0, 1], [-sign, Kp0]]), |
| 317 | + inputs=['input', 'prop_e'], outputs=['output', 'ufb']) |
| 318 | + elif gain in ('I', 'i'): |
| 319 | + Kigain = ss2io(ss([],[],[],[[0, 1], [-sign, Ki0]]), |
| 320 | + inputs=['input', 'int_e'], outputs=['output', 'ufb']) |
| 321 | + elif gain in ('D', 'd'): |
| 322 | + Kdgain = ss2io(ss([],[],[],[[0, 1], [-sign, Kd0]]), |
| 323 | + inputs=['input', 'deriv'], outputs=['output', 'ufb']) |
| 324 | + else: |
| 325 | + raise ValueError(gain + ' gain not recognized.') |
| 326 | + |
| 327 | + # the second input and output are used by sisotool to plot step response |
| 328 | + loop = interconnect((plant, Kpgain, Kigain, Kdgain, prop, integ, deriv, |
| 329 | + C_ff, e_summer, u_summer), |
| 330 | + inplist=['input', input_signal], |
| 331 | + outlist=['output', 'y']) |
| 332 | + if plot: |
| 333 | + sisotool(loop, kvect=(0.,)) |
| 334 | + cl = loop[1, 1] # closed loop transfer function with initial gains |
| 335 | + return StateSpace(cl.A, cl.B, cl.C, cl.D, cl.dt) |
0 commit comments