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

Commitd700ad7

Browse files
authored
Merge pull request#1071 from murrayrm/create_statefbk-29Nov2024
add reference gain design pattern to create_statefbk_iosystem
2 parentsda55654 +bb52b70 commitd700ad7

File tree

5 files changed

+249
-92
lines changed

5 files changed

+249
-92
lines changed

‎control/statefbk.py‎

Lines changed: 122 additions & 47 deletions
Original file line numberDiff line numberDiff line change
@@ -39,24 +39,25 @@
3939
#
4040
# $Id$
4141

42-
# External packages and modules
42+
importwarnings
43+
4344
importnumpyasnp
4445
importscipyassp
45-
importwarnings
4646

4747
from .importstatesp
48-
from .mateqnimportcare,dare,_check_shape
49-
from .statespimportStateSpace,_ssmatrix,_convert_to_statespace,ss
48+
from .configimport_process_legacy_keyword
49+
from .exceptionimportControlArgument,ControlDimension, \
50+
ControlNotImplemented,ControlSlycot
51+
from .iosysimport_process_indices,_process_labels,isctime,isdtime
5052
from .ltiimportLTI
51-
from .iosysimportisdtime,isctime,_process_indices,_process_labels
53+
from .mateqnimport_check_shape,care,dare
5254
from .nlsysimportNonlinearIOSystem,interconnect
53-
from .exceptionimportControlSlycot,ControlArgument,ControlDimension, \
54-
ControlNotImplemented
55-
from .configimport_process_legacy_keyword
55+
from .statespimportStateSpace,_convert_to_statespace,_ssmatrix,ss
5656

5757
# Make sure we have access to the right slycot routines
5858
try:
5959
fromslycotimportsb03md57
60+
6061
# wrap without the deprecation warning
6162
defsb03md(n,C,A,U,dico,job='X',fact='N',trana='N',ldwork=None):
6263
ret=sb03md57(A,U,C,dico,job,fact,trana,ldwork)
@@ -581,8 +582,9 @@ def dlqr(*args, **kwargs):
581582

582583
# Function to create an I/O sytems representing a state feedback controller
583584
defcreate_statefbk_iosystem(
584-
sys,gain,integral_action=None,estimator=None,controller_type=None,
585-
xd_labels=None,ud_labels=None,gainsched_indices=None,
585+
sys,gain,feedfwd_gain=None,integral_action=None,estimator=None,
586+
controller_type=None,xd_labels=None,ud_labels=None,ref_labels=None,
587+
feedfwd_pattern='trajgen',gainsched_indices=None,
586588
gainsched_method='linear',control_indices=None,state_indices=None,
587589
name=None,inputs=None,outputs=None,states=None,**kwargs):
588590
r"""Create an I/O system using a (full) state feedback controller.
@@ -592,7 +594,7 @@ def create_statefbk_iosystem(
592594
593595
.. math:: u = u_d - K_p (x - x_d) - K_i \int(C x - C x_d)
594596
595-
It can be called in the form::
597+
by calling
596598
597599
ctrl, clsys = ct.create_statefbk_iosystem(sys, K)
598600
@@ -608,6 +610,18 @@ def create_statefbk_iosystem(
608610
609611
where :math:`\mu` represents the scheduling variable.
610612
613+
Alternatively, a controller of the form
614+
615+
.. math:: u = k_f r - K_p x - K_i \int(C x - r)
616+
617+
can be created by calling
618+
619+
ctrl, clsys = ct.create_statefbk_iosystem(
620+
sys, K, kf, feedfwd_pattern='refgain')
621+
622+
In either form, an estimator can also be used to compute the estimated
623+
state from the input and output measurements.
624+
611625
Parameters
612626
----------
613627
sys : NonlinearIOSystem
@@ -631,14 +645,15 @@ def create_statefbk_iosystem(
631645
If an I/O system is given, the error e = x - xd is passed to the
632646
system and the output is used as the feedback compensation term.
633647
634-
xd_labels, ud_labels : str or list of str, optional
635-
Set the name of the signals to use for the desired state and
636-
inputs. If a single string is specified, it should be a format
637-
string using the variable `i` as an index. Otherwise, a list of
638-
strings matching the size of `x_d` and `u_d`, respectively, should
639-
be used. Default is "xd[{i}]" for xd_labels and "ud[{i}]" for
640-
ud_labels. These settings can also be overridden using the
641-
`inputs` keyword.
648+
feedfwd_gain : array_like, optional
649+
Specify the feedforward gain, `k_f`. Used only for the reference
650+
gain design pattern. If not given and if `sys` is a `StateSpace`
651+
(linear) system, will be computed as -1/(C (A-BK)^{-1}) B.
652+
653+
feedfwd_pattern : str, optional
654+
If set to 'refgain', the reference gain design pattern is used to
655+
create the controller instead of the trajectory generation
656+
('trajgen') pattern.
642657
643658
integral_action : ndarray, optional
644659
If this keyword is specified, the controller can include integral
@@ -680,17 +695,19 @@ def create_statefbk_iosystem(
680695
Returns
681696
-------
682697
ctrl : NonlinearIOSystem
683-
Input/output system representing the controller. This system
684-
takes as inputs the desired state `x_d`, the desired input
685-
`u_d`, and either the system state `x` or the estimated state
686-
`xhat`. It outputs the controller action `u` according to the
687-
formula `u = u_d - K(x - x_d)`. If the keyword
688-
`integral_action` is specified, then an additional set of
689-
integrators is included in the control system (with the gain
690-
matrix `K` having the integral gains appended after the state
691-
gains). If a gain scheduled controller is specified, the gain
692-
(proportional and integral) are evaluated using the scheduling
693-
variables specified by `gainsched_indices`.
698+
Input/output system representing the controller. For the 'trajgen'
699+
design pattern (default), this system takes as inputs the desired
700+
state `x_d`, the desired input `u_d`, and either the system state
701+
`x` or the estimated state `xhat`. It outputs the controller
702+
action `u` according to the formula `u = u_d - K(x - x_d)`. For
703+
the 'refgain' design pattern, the system takes as inputs the
704+
reference input `r` and the system or estimated state. If the
705+
keyword `integral_action` is specified, then an additional set of
706+
integrators is included in the control system (with the gain matrix
707+
`K` having the integral gains appended after the state gains). If
708+
a gain scheduled controller is specified, the gain (proportional
709+
and integral) are evaluated using the scheduling variables
710+
specified by `gainsched_indices`.
694711
695712
clsys : NonlinearIOSystem
696713
Input/output system representing the closed loop system. This
@@ -716,6 +733,15 @@ def create_statefbk_iosystem(
716733
specified as either integer offsets or as estimator/system output
717734
signal names. If not specified, defaults to the system states.
718735
736+
xd_labels, ud_labels, ref_labels : str or list of str, optional
737+
Set the name of the signals to use for the desired state and inputs
738+
or the reference inputs (for the 'refgain' design pattern). If a
739+
single string is specified, it should be a format string using the
740+
variable `i` as an index. Otherwise, a list of strings matching
741+
the size of `x_d` and `u_d`, respectively, should be used. Default
742+
is "xd[{i}]" for xd_labels and "ud[{i}]" for ud_labels. These
743+
settings can also be overridden using the `inputs` keyword.
744+
719745
inputs, outputs, states : str, or list of str, optional
720746
List of strings that name the individual signals of the transformed
721747
system. If not given, the inputs, outputs, and states are the same
@@ -753,6 +779,11 @@ def create_statefbk_iosystem(
753779
ifkwargs:
754780
raiseTypeError("unrecognized keywords: ",str(kwargs))
755781

782+
# Check for consistency of positional parameters
783+
iffeedfwd_gainisnotNoneandfeedfwd_pattern!='refgain':
784+
raiseControlArgument(
785+
"feedfwd_gain specified but feedfwd_pattern != 'refgain'")
786+
756787
# Figure out what inputs to the system to use
757788
control_indices=_process_indices(
758789
control_indices,'control',sys.input_labels,sys.ninputs)
@@ -812,14 +843,18 @@ def create_statefbk_iosystem(
812843
# Check for gain scheduled controller
813844
iflen(gain)!=2:
814845
raiseControlArgument("gain must be a 2-tuple for gain scheduling")
846+
eliffeedfwd_pattern!='trajgen':
847+
raiseNotImplementedError(
848+
"Gain scheduling is not implemented for pattern "
849+
f"'{feedfwd_pattern}'")
815850
gains,points=gain[0:2]
816851

817852
# Stack gains and points if past as a list
818853
gains=np.stack(gains)
819854
points=np.stack(points)
820855
gainsched=True
821856

822-
elifisinstance(gain,NonlinearIOSystem):
857+
elifisinstance(gain,NonlinearIOSystem)andfeedfwd_pattern!='refgain':
823858
ifcontroller_typenotin ['iosystem',None]:
824859
raiseControlArgument(
825860
f"incompatible controller type '{controller_type}'")
@@ -841,20 +876,29 @@ def create_statefbk_iosystem(
841876
raiseControlArgument(f"unknown controller_type '{controller_type}'")
842877

843878
# Figure out the labels to use
844-
xd_labels=_process_labels(
845-
xd_labels,'xd', ['xd[{i}]'.format(i=i)foriinrange(sys_nstates)])
846-
ud_labels=_process_labels(
847-
ud_labels,'ud', ['ud[{i}]'.format(i=i)foriinrange(sys_ninputs)])
848-
849-
# Create the signal and system names
850-
ifinputsisNone:
851-
inputs=xd_labels+ud_labels+estimator.output_labels
879+
iffeedfwd_pattern=='trajgen':
880+
xd_labels=_process_labels(xd_labels,'xd', [
881+
'xd[{i}]'.format(i=i)foriinrange(sys_nstates)])
882+
ud_labels=_process_labels(ud_labels,'ud', [
883+
'ud[{i}]'.format(i=i)foriinrange(sys_ninputs)])
884+
885+
# Create the signal and system names
886+
ifinputsisNone:
887+
inputs=xd_labels+ud_labels+estimator.output_labels
888+
eliffeedfwd_pattern=='refgain':
889+
ref_labels=_process_labels(ref_labels,'r', [
890+
f'r[{i}]'foriinrange(sys_ninputs)])
891+
ifinputsisNone:
892+
inputs=ref_labels+estimator.output_labels
893+
else:
894+
raiseNotImplementedError(f"unknown pattern '{feedfwd_pattern}'")
895+
852896
ifoutputsisNone:
853897
outputs= [sys.input_labels[i]foriincontrol_indices]
854898
ifstatesisNone:
855899
states=nintegrators
856900

857-
# Processgainscheduling variables, if present
901+
# Processgain scheduling variables, if present
858902
ifgainsched:
859903
# Create a copy of the scheduling variable indices (default = xd)
860904
gainsched_indices=_process_indices(
@@ -897,7 +941,7 @@ def _compute_gain(mu):
897941
returnK
898942

899943
# Define the controller system
900-
ifcontroller_type=='nonlinear':
944+
ifcontroller_type=='nonlinear'andfeedfwd_pattern=='trajgen':
901945
# Create an I/O system for the state feedback gains
902946
def_control_update(t,states,inputs,params):
903947
# Split input into desired state, nominal input, and current state
@@ -931,7 +975,7 @@ def _control_output(t, states, inputs, params):
931975
_control_update,_control_output,name=name,inputs=inputs,
932976
outputs=outputs,states=states,params=params)
933977

934-
elifcontroller_type=='iosystem':
978+
elifcontroller_type=='iosystem'andfeedfwd_pattern=='trajgen':
935979
# Use the passed system to compute feedback compensation
936980
def_control_update(t,states,inputs,params):
937981
# Split input into desired state, nominal input, and current state
@@ -955,7 +999,7 @@ def _control_output(t, states, inputs, params):
955999
_control_update,_control_output,name=name,inputs=inputs,
9561000
outputs=outputs,states=fbkctrl.state_labels,dt=fbkctrl.dt)
9571001

958-
elifcontroller_type=='linear'orcontroller_typeisNone:
1002+
elifcontroller_typein'linear'andfeedfwd_pattern=='trajgen':
9591003
# Create the matrices implementing the controller
9601004
ifisctime(sys):
9611005
# Continuous time: integrator
@@ -973,6 +1017,37 @@ def _control_output(t, states, inputs, params):
9731017
A_lqr,B_lqr,C_lqr,D_lqr,dt=sys.dt,name=name,
9741018
inputs=inputs,outputs=outputs,states=states)
9751019

1020+
eliffeedfwd_pattern=='refgain':
1021+
ifcontroller_typenotin ['linear','iosystem']:
1022+
raiseControlArgument(
1023+
"refgain design pattern only supports linear controllers")
1024+
1025+
iffeedfwd_gainisNone:
1026+
raiseControlArgument(
1027+
"'feedfwd_gain' required for reference gain pattern")
1028+
1029+
# Check to make sure the reference gain is valid
1030+
Kf=np.atleast_2d(feedfwd_gain)
1031+
ifKf.ndim!=2orKf.shape[0]!=sys.ninputsor \
1032+
Kf.shape[1]!=sys.ninputs:
1033+
raiseControlArgument("feedfwd_gain is not the right shape")
1034+
1035+
# Create the matrices implementing the controller
1036+
# [r, x]->[u]: u = k_f r - K_p x - K_i \int(C x - r)
1037+
ifisctime(sys):
1038+
# Continuous time: integrator
1039+
A_lqr=np.zeros((C.shape[0],C.shape[0]))
1040+
else:
1041+
# Discrete time: summer
1042+
A_lqr=np.eye(C.shape[0])
1043+
B_lqr=np.hstack([-np.eye(C.shape[0],sys_ninputs),C])
1044+
C_lqr=-K[:,sys_nstates:]# integral gain (opt)
1045+
D_lqr=np.hstack([Kf,-K])
1046+
1047+
ctrl=ss(
1048+
A_lqr,B_lqr,C_lqr,D_lqr,dt=sys.dt,name=name,
1049+
inputs=inputs,outputs=outputs,states=states)
1050+
9761051
else:
9771052
raiseControlArgument(f"unknown controller_type '{controller_type}'")
9781053

@@ -1020,7 +1095,7 @@ def ctrb(A, B, t=None):
10201095
bmat=_ssmatrix(B)
10211096
n=np.shape(amat)[0]
10221097
m=np.shape(bmat)[1]
1023-
1098+
10241099
iftisNoneort>n:
10251100
t=n
10261101

@@ -1042,7 +1117,7 @@ def obsv(A, C, t=None):
10421117
Dynamics and output matrix of the system
10431118
t : None or integer
10441119
maximum time horizon of the controllability matrix, max = A.shape[0]
1045-
1120+
10461121
Returns
10471122
-------
10481123
O : 2D array (or matrix)
@@ -1062,14 +1137,14 @@ def obsv(A, C, t=None):
10621137
cmat=_ssmatrix(C)
10631138
n=np.shape(amat)[0]
10641139
p=np.shape(cmat)[0]
1065-
1140+
10661141
iftisNoneort>n:
10671142
t=n
10681143

10691144
# Construct the observability matrix
10701145
obsv=np.zeros((t*p,n))
10711146
obsv[:p, :]=cmat
1072-
1147+
10731148
forkinrange(1,t):
10741149
obsv[k*p:(k+1)*p, :]=np.dot(obsv[(k-1)*p:k*p, :],amat)
10751150

‎control/statesp.py‎

Lines changed: 16 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -252,6 +252,12 @@ def __init__(self, *args, **kwargs):
252252
D=np.zeros((C.shape[0],B.shape[1]))
253253
D=_ssmatrix(D)
254254

255+
# If only direct term is present, adjust sizes of C and D if needed
256+
ifD.size>0andB.size==0:
257+
B=np.zeros((0,D.shape[1]))
258+
ifD.size>0andC.size==0:
259+
C=np.zeros((D.shape[0],0))
260+
255261
# Matrices defining the linear system
256262
self.A=A
257263
self.B=B
@@ -268,7 +274,7 @@ def __init__(self, *args, **kwargs):
268274

269275
# Process iosys keywords
270276
defaults=args[0]iflen(args)==1else \
271-
{'inputs':D.shape[1],'outputs':D.shape[0],
277+
{'inputs':B.shape[1],'outputs':C.shape[0],
272278
'states':A.shape[0]}
273279
name,inputs,outputs,states,dt=_process_iosys_keywords(
274280
kwargs,defaults,static=(A.size==0))
@@ -295,16 +301,15 @@ def __init__(self, *args, **kwargs):
295301
# Check to make sure everything is consistent
296302
#
297303
# Check that the matrix sizes are consistent
298-
ifA.shape[0]!=A.shape[1]orself.nstates!=A.shape[0]:
299-
raiseValueError("A must be square.")
300-
ifself.nstates!=B.shape[0]:
301-
raiseValueError("A and B must have the same number of rows.")
302-
ifself.nstates!=C.shape[1]:
303-
raiseValueError("A and C must have the same number of columns.")
304-
ifself.ninputs!=B.shape[1]orself.ninputs!=D.shape[1]:
305-
raiseValueError("B and D must have the same number of columns.")
306-
ifself.noutputs!=C.shape[0]orself.noutputs!=D.shape[0]:
307-
raiseValueError("C and D must have the same number of rows.")
304+
def_check_shape(matrix,expected,name):
305+
ifmatrix.shape!=expected:
306+
raiseValueError(
307+
f"{name} is the wrong shape; "
308+
f"expected{expected} instead of{matrix.shape}")
309+
_check_shape(A, (self.nstates,self.nstates),"A")
310+
_check_shape(B, (self.nstates,self.ninputs),"B")
311+
_check_shape(C, (self.noutputs,self.nstates),"C")
312+
_check_shape(D, (self.noutputs,self.ninputs),"D")
308313

309314
#
310315
# Final processing

0 commit comments

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