Movatterモバイル変換

[0]ホーム
URL:

Skip to content

Navigation Menu

Sign in
Appearance settings

Search code, repositories, users, issues, pull requests...

Provide feedback

We read every piece of feedback, and take your input very seriously.

Saved searches

Use saved searches to filter your results more quickly

 Sign up
Appearance settings

Commitc3488cd

Browse files
authored
Merge pull request#827 from murrayrm/gainsched-23Dec2022
Add gain scheduling to create_statefbk_iosystem()
2 parents508dc7f +51f3b6b commitc3488cd

File tree

7 files changed

+592
-75
lines changed

7 files changed

+592
-75
lines changed

‎control/iosys.py

Lines changed: 18 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -536,6 +536,10 @@ def linearize(self, x0, u0, t=0, params=None, eps=1e-6,
536536
# functions.
537537
#
538538

539+
# If x0 and u0 are specified as lists, concatenate the elements
540+
x0=_concatenate_list_elements(x0,'x0')
541+
u0=_concatenate_list_elements(u0,'u0')
542+
539543
# Figure out dimensions if they were not specified.
540544
nstates=_find_size(self.nstates,x0)
541545
ninputs=_find_size(self.ninputs,u0)
@@ -1762,14 +1766,7 @@ def input_output_response(
17621766
ninputs=U.shape[0]
17631767

17641768
# If we were passed a list of initial states, concatenate them
1765-
ifisinstance(X0, (tuple,list)):
1766-
X0_list= []
1767-
fori,x0inenumerate(X0):
1768-
x0=np.array(x0).reshape(-1)# convert everyting to 1D array
1769-
X0_list+=x0.tolist()# add elements to initial state
1770-
1771-
# Save the newly created input vector
1772-
X0=np.array(X0_list)
1769+
X0=_concatenate_list_elements(X0,'X0')
17731770

17741771
# If the initial state is too short, make it longer (NB: sys.nstates
17751772
# could be None if nstates comes from size of initial condition)
@@ -2393,6 +2390,19 @@ def ss(*args, **kwargs):
23932390
returnsys
23942391

23952392

2393+
# Utility function to allow lists states, inputs
2394+
def_concatenate_list_elements(X,name='X'):
2395+
# If we were passed a list, concatenate the elements together
2396+
ifisinstance(X, (tuple,list)):
2397+
X_list= []
2398+
fori,xinenumerate(X):
2399+
x=np.array(x).reshape(-1)# convert everyting to 1D array
2400+
X_list+=x.tolist()# add elements to initial state
2401+
returnnp.array(X_list)
2402+
2403+
# Otherwise, do nothing
2404+
returnX
2405+
23962406
defrss(states=1,outputs=1,inputs=1,strictly_proper=False,**kwargs):
23972407
"""Create a stable random state space object.
23982408

‎control/statefbk.py

Lines changed: 178 additions & 53 deletions
Original file line numberDiff line numberDiff line change
@@ -41,6 +41,7 @@
4141

4242
# External packages and modules
4343
importnumpyasnp
44+
importscipyassp
4445

4546
from .importstatesp
4647
from .mateqnimportcare,dare,_check_shape
@@ -71,7 +72,7 @@ def sb03md(n, C, A, U, dico, job='X',fact='N',trana='N',ldwork=None):
7172
sb03od=None
7273

7374

74-
__all__= ['ctrb','obsv','gram','place','place_varga','lqr',
75+
__all__= ['ctrb','obsv','gram','place','place_varga','lqr',
7576
'dlqr','acker','create_statefbk_iosystem']
7677

7778

@@ -596,8 +597,10 @@ def dlqr(*args, **kwargs):
596597

597598
# Function to create an I/O sytems representing a state feedback controller
598599
defcreate_statefbk_iosystem(
599-
sys,K,integral_action=None,xd_labels='xd[{i}]',ud_labels='ud[{i}]',
600-
estimator=None,type='linear'):
600+
sys,gain,integral_action=None,estimator=None,type=None,
601+
xd_labels='xd[{i}]',ud_labels='ud[{i}]',gainsched_indices=None,
602+
gainsched_method='linear',name=None,inputs=None,outputs=None,
603+
states=None):
601604
"""Create an I/O system using a (full) state feedback controller
602605
603606
This function creates an input/output system that implements a
@@ -613,31 +616,47 @@ def create_statefbk_iosystem(
613616
feedback gain (eg, from LQR). The function returns the controller
614617
``ctrl`` and the closed loop systems ``clsys``, both as I/O systems.
615618
619+
A gain scheduled controller can also be created, by passing a list of
620+
gains and a corresponding list of values of a set of scheduling
621+
variables. In this case, the controller has the form
622+
623+
u = ud - K_p(mu) (x - xd) - K_i(mu) integral(C x - C x_d)
624+
625+
where mu represents the scheduling variable.
626+
616627
Parameters
617628
----------
618629
sys : InputOutputSystem
619630
The I/O system that represents the process dynamics. If no estimator
620631
is given, the output of this system should represent the full state.
621632
622-
K : ndarray
623-
The state feedback gain. This matrix defines the gains to be
624-
applied to the system. If ``integral_action`` is None, then the
625-
dimensions of this array should be (sys.ninputs, sys.nstates). If
626-
`integral action` is set to a matrix or a function, then additional
627-
columns represent the gains of the integral states of the
628-
controller.
633+
gain : ndarray or tuple
634+
If a array is give, it represents the state feedback gain (K).
635+
This matrix defines the gains to be applied to the system. If
636+
``integral_action`` is None, then the dimensions of this array
637+
should be (sys.ninputs, sys.nstates). If `integral action` is
638+
set to a matrix or a function, then additional columns
639+
represent the gains of the integral states of the controller.
640+
641+
If a tuple is given, then it specifies a gain schedule. The tuple
642+
should be of the form ``(gains, points)`` where gains is a list of
643+
gains :math:`K_j` and points is a list of values :math:`\\mu_j` at
644+
which the gains are computed. The `gainsched_indices` parameter
645+
should be used to specify the scheduling variables.
629646
630647
xd_labels, ud_labels : str or list of str, optional
631648
Set the name of the signals to use for the desired state and inputs.
632649
If a single string is specified, it should be a format string using
633-
the variable ``i`` as an index. Otherwise, a list of strings matching
634-
the size of xd and ud, respectively, should be used. Default is
635-
``'xd[{i}]'`` for xd_labels and ``'xd[{i}]'`` for ud_labels.
650+
the variable ``i`` as an index. Otherwise, a list of strings
651+
matching the size of xd and ud, respectively, should be used.
652+
Default is ``'xd[{i}]'`` for xd_labels and ``'ud[{i}]'`` for
653+
ud_labels. These settings can also be overriden using the `inputs`
654+
keyword.
636655
637656
integral_action : None, ndarray, or func, optional
638657
If this keyword is specified, the controller can include integral
639-
action in addition to state feedback. If ``integral_action`` isan
640-
ndarray, it will be multiplied by the current and desired state to
658+
action in addition to state feedback. If ``integral_action`` isa
659+
matrix, it will be multiplied by the current and desired state to
641660
generate the error for the internal integrator states of the control
642661
law. If ``integral_action`` is a function ``h``, that function will
643662
be called with the signature h(t, x, u, params) to obtain the
@@ -646,30 +665,63 @@ def create_statefbk_iosystem(
646665
``K`` matrix.
647666
648667
estimator : InputOutputSystem, optional
649-
If an estimator is provided,using the states of the estimator as
668+
If an estimator is provided,use the states of the estimator as
650669
the system inputs for the controller.
651670
652-
type : 'nonlinear' or 'linear', optional
653-
Set the type of controller to create. The default is a linear
654-
controller implementing the LQR regulator. If the type is 'nonlinear',
655-
a :class:NonlinearIOSystem is created instead, with the gain ``K`` as
656-
a parameter (allowing modifications of the gain at runtime).
671+
gainsched_indices : list of int or str, optional
672+
If a gain scheduled controller is specified, specify the indices of
673+
the controller input to use for scheduling the gain. The input to
674+
the controller is the desired state xd, the desired input ud, and
675+
the system state x (or state estimate xhat, if an estimator is
676+
given). The indices can either be specified as integer offsets into
677+
the input vector or as strings matching the signal names of the
678+
input vector. The default is to use the desire state xd.
679+
680+
gainsched_method : str, optional
681+
The method to use for gain scheduling. Possible values are 'linear'
682+
(default), 'nearest', and 'cubic'. More information is available in
683+
:func:`scipy.interpolate.griddata`. For points outside of the convex
684+
hull of the scheduling points, the gain at the nearest point is
685+
used.
686+
687+
type : 'linear' or 'nonlinear', optional
688+
Set the type of controller to create. The default for a linear gain
689+
is a linear controller implementing the LQR regulator. If the type
690+
is 'nonlinear', a :class:NonlinearIOSystem is created instead, with
691+
the gain ``K`` as a parameter (allowing modifications of the gain at
692+
runtime). If the gain parameter is a tuple, then a nonlinear,
693+
gain-scheduled controller is created.
657694
658695
Returns
659696
-------
660697
ctrl : InputOutputSystem
661698
Input/output system representing the controller. This system takes
662-
as inputs the desired state xd, the desired input ud, and the system
663-
state x. It outputs the controller action u according to the
664-
formula u = ud - K(x - xd). If the keyword `integral_action` is
665-
specified, then an additional set of integrators is included in the
666-
control system (with the gain matrix K having the integral gains
667-
appended after the state gains).
699+
as inputs the desired state ``xd``, the desired input ``ud``, and
700+
either the system state ``x`` or the estimated state ``xhat``. It
701+
outputs the controller action u according to the formula :math:`u =
702+
u_d - K(x - x_d)`. If the keyword ``integral_action`` is specified,
703+
then an additional set of integrators is included in the control
704+
system (with the gain matrix ``K`` having the integral gains
705+
appended after the state gains). If a gain scheduled controller is
706+
specified, the gain (proportional and integral) are evaluated using
707+
the scheduling variables specified by ``gainsched_indices``.
668708
669709
clsys : InputOutputSystem
670710
Input/output system representing the closed loop system. This
671-
systems takes as inputs the desired trajectory (xd, ud) and outputs
672-
the system state x and the applied input u (vertically stacked).
711+
systems takes as inputs the desired trajectory ``(xd, ud)`` and
712+
outputs the system state ``x`` and the applied input ``u``
713+
(vertically stacked).
714+
715+
Other Parameters
716+
----------------
717+
inputs, outputs : str, or list of str, optional
718+
List of strings that name the individual signals of the transformed
719+
system. If not given, the inputs and outputs are the same as the
720+
original system.
721+
722+
name : string, optional
723+
System name. If unspecified, a generic name <sys[id]> is generated
724+
with a unique integer id.
673725
674726
"""
675727
# Make sure that we were passed an I/O system as an input
@@ -705,53 +757,127 @@ def create_statefbk_iosystem(
705757
C=np.zeros((0,sys.nstates))
706758

707759
# Check to make sure that state feedback has the right shape
708-
ifnotisinstance(K,np.ndarray)or \
709-
K.shape!= (sys.ninputs,estimator.noutputs+nintegrators):
760+
ifisinstance(gain,np.ndarray):
761+
K=gain
762+
ifK.shape!= (sys.ninputs,estimator.noutputs+nintegrators):
763+
raiseControlArgument(
764+
f'Control gain must be an array of size{sys.ninputs}'
765+
f'x{sys.nstates}'+
766+
(f'+{nintegrators}'ifnintegrators>0else''))
767+
gainsched=False
768+
769+
elifisinstance(gain,tuple):
770+
# Check for gain scheduled controller
771+
iflen(gain)!=2:
772+
raiseControlArgument("gain must be a 2-tuple for gain scheduling")
773+
gains,points=gain[0:2]
774+
775+
# Stack gains and points if past as a list
776+
gains=np.stack(gains)
777+
points=np.stack(points)
778+
gainsched=True
779+
780+
else:
781+
raiseControlArgument("gain must be an array or a tuple")
782+
783+
# Decide on the type of system to create
784+
ifgainschedandtype=='linear':
710785
raiseControlArgument(
711-
f'Control gain must be an array of size{sys.ninputs}'
712-
f'x{sys.nstates}'+
713-
(f'+{nintegrators}'ifnintegrators>0else''))
786+
"type 'linear' not allowed for gain scheduled controller")
787+
eliftypeisNone:
788+
type='nonlinear'ifgainschedelse'linear'
789+
eliftypenotin {'linear','nonlinear'}:
790+
raiseControlArgument(f"unknown type '{type}'")
714791

715792
# Figure out the labels to use
716793
ifisinstance(xd_labels,str):
717-
#Gnerate the list of labels using the argument as a format string
794+
#Generate the list of labels using the argument as a format string
718795
xd_labels= [xd_labels.format(i=i)foriinrange(sys.nstates)]
719796

720797
ifisinstance(ud_labels,str):
721-
#Gnerate the list of labels using the argument as a format string
798+
#Generate the list of labels using the argument as a format string
722799
ud_labels= [ud_labels.format(i=i)foriinrange(sys.ninputs)]
723800

801+
# Create the signal and system names
802+
ifinputsisNone:
803+
inputs=xd_labels+ud_labels+estimator.output_labels
804+
ifoutputsisNone:
805+
outputs=list(sys.input_index.keys())
806+
ifstatesisNone:
807+
states=nintegrators
808+
809+
# Process gainscheduling variables, if present
810+
ifgainsched:
811+
# Create a copy of the scheduling variable indices (default = xd)
812+
gainsched_indices=range(sys.nstates)ifgainsched_indicesisNone \
813+
elselist(gainsched_indices)
814+
815+
# Make sure the scheduling variable indices are the right length
816+
iflen(gainsched_indices)!=points.shape[1]:
817+
raiseControlArgument(
818+
"length of gainsched_indices must match dimension of"
819+
" scheduling variables")
820+
821+
# Process scheduling variables
822+
fori,idxinenumerate(gainsched_indices):
823+
ifisinstance(idx,str):
824+
gainsched_indices[i]=inputs.index(gainsched_indices[i])
825+
826+
# Create interpolating function
827+
ifgainsched_method=='nearest':
828+
_interp=sp.interpolate.NearestNDInterpolator(points,gains)
829+
def_nearest(mu):
830+
raiseSystemError(f"could not find nearest gain at mu ={mu}")
831+
elifgainsched_method=='linear':
832+
_interp=sp.interpolate.LinearNDInterpolator(points,gains)
833+
_nearest=sp.interpolate.NearestNDInterpolator(points,gains)
834+
elifgainsched_method=='cubic':
835+
_interp=sp.interpolate.CloughTocher2DInterpolator(points,gains)
836+
_nearest=sp.interpolate.NearestNDInterpolator(points,gains)
837+
else:
838+
raiseControlArgument(
839+
f"unknown gain scheduling method '{gainsched_method}'")
840+
841+
def_compute_gain(mu):
842+
K=_interp(mu)
843+
ifnp.isnan(K).any():
844+
K=_nearest(mu)
845+
returnK
846+
724847
# Define the controller system
725848
iftype=='nonlinear':
726849
# Create an I/O system for the state feedback gains
727-
def_control_update(t,x,inputs,params):
850+
def_control_update(t,states,inputs,params):
728851
# Split input into desired state, nominal input, and current state
729852
xd_vec=inputs[0:sys.nstates]
730853
x_vec=inputs[-estimator.nstates:]
731854

732855
# Compute the integral error in the xy coordinates
733-
returnC @x_vec-C @xd_vec
856+
returnC @(x_vec-xd_vec)
734857

735-
def_control_output(t,e,z,params):
736-
K=params.get('K')
858+
def_control_output(t,states,inputs,params):
859+
ifgainsched:
860+
mu=inputs[gainsched_indices]
861+
K_=_compute_gain(mu)
862+
else:
863+
K_=params.get('K')
737864

738865
# Split input into desired state, nominal input, and current state
739-
xd_vec=z[0:sys.nstates]
740-
ud_vec=z[sys.nstates:sys.nstates+sys.ninputs]
741-
x_vec=z[-sys.nstates:]
866+
xd_vec=inputs[0:sys.nstates]
867+
ud_vec=inputs[sys.nstates:sys.nstates+sys.ninputs]
868+
x_vec=inputs[-sys.nstates:]
742869

743870
# Compute the control law
744-
u=ud_vec-K[:,0:sys.nstates] @ (x_vec-xd_vec)
871+
u=ud_vec-K_[:,0:sys.nstates] @ (x_vec-xd_vec)
745872
ifnintegrators>0:
746-
u-=K[:,sys.nstates:] @e
873+
u-=K_[:,sys.nstates:] @states
747874

748875
returnu
749876

877+
params= {}ifgainschedelse {'K':K}
750878
ctrl=NonlinearIOSystem(
751-
_control_update,_control_output,name='control',
752-
inputs=xd_labels+ud_labels+estimator.output_labels,
753-
outputs=list(sys.input_index.keys()),params={'K':K},
754-
states=nintegrators)
879+
_control_update,_control_output,name=name,inputs=inputs,
880+
outputs=outputs,states=states,params=params)
755881

756882
eliftype=='linear'ortypeisNone:
757883
# Create the matrices implementing the controller
@@ -768,9 +894,8 @@ def _control_output(t, e, z, params):
768894
])
769895

770896
ctrl=ss(
771-
A_lqr,B_lqr,C_lqr,D_lqr,dt=sys.dt,name='control',
772-
inputs=xd_labels+ud_labels+estimator.output_labels,
773-
outputs=list(sys.input_index.keys()),states=nintegrators)
897+
A_lqr,B_lqr,C_lqr,D_lqr,dt=sys.dt,name=name,
898+
inputs=inputs,outputs=outputs,states=states)
774899

775900
else:
776901
raiseControlArgument(f"unknown type '{type}'")
@@ -779,7 +904,7 @@ def _control_output(t, e, z, params):
779904
closed=interconnect(
780905
[sys,ctrl]ifestimator==syselse [sys,ctrl,estimator],
781906
name=sys.name+"_"+ctrl.name,
782-
inplist=xd_labels+ud_labels,inputs=xd_labels+ud_labels,
907+
inplist=inputs[:-sys.nstates],inputs=inputs[:-sys.nstates],
783908
outlist=sys.output_labels+sys.input_labels,
784909
outputs=sys.output_labels+sys.input_labels
785910
)

0 commit comments

Comments
 (0)
[8]ページ先頭
©2009-2025 Movatter.jp