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

Commitee7c35d

Browse files
committed
Add smart selection of gains in root locus plot. Calculation of breakpoints of real axis.
1 parent5ab74cf commitee7c35d

File tree

1 file changed

+156
-44
lines changed

1 file changed

+156
-44
lines changed

‎control/rlocus.py

Lines changed: 156 additions & 44 deletions
Original file line numberDiff line numberDiff line change
@@ -43,43 +43,52 @@
4343
# RMM, 2 April 2011: modified to work with new LTI structure (see ChangeLog)
4444
# * Not tested: should still work on signal.ltisys objects
4545
#
46+
# GDM, 16 February 2017: add smart selection of gains based on axis.
47+
# * Add gains up to a tolerance is achieved
48+
# * Change some variables and functions names ir order to improve "code style"
49+
#
50+
#
4651
# $Id$
4752

4853
# Packages used by this module
54+
fromfunctoolsimportpartial
55+
4956
importnumpyasnp
57+
importpylab# plotting routines
58+
importscipy.signal# signal processing toolbox
5059
fromscipyimportarray,poly1d,row_stack,zeros_like,real,imag
51-
importscipy.signal# signal processing toolbox
52-
importpylab# plotting routines
53-
from .xferfcnimport_convertToTransferFunction
60+
61+
from .importxferfcn
5462
from .exceptionimportControlMIMONotImplemented
55-
fromfunctoolsimportpartial
5663

5764
__all__= ['root_locus','rlocus']
5865

66+
5967
# Main function: compute a root locus diagram
60-
defroot_locus(sys,kvect=None,xlim=None,ylim=None,plotstr='-',Plot=True,
61-
PrintGain=True):
68+
defroot_locus(dinsys,kvect=None,xlim=None,ylim=None,plotstr='-',plot=True,
69+
print_gain=True):
6270
"""Root locus plot
6371
6472
Calculate the root locus by finding the roots of 1+k*TF(s)
6573
where TF is self.num(s)/self.den(s) and each k is an element
66-
ofkvect.
74+
ofgvect.
6775
6876
Parameters
6977
----------
70-
sys : LTI object
78+
dinsys : LTI object
7179
Linear input/output systems (SISO only, for now)
7280
kvect : list or ndarray, optional
7381
List of gains to use in computing diagram
7482
xlim : tuple or list, optional
7583
control of x-axis range, normally with tuple (see matplotlib.axes)
7684
ylim : tuple or list, optional
7785
control of y-axis range
78-
Plot : boolean, optional (default = True)
86+
plot : boolean, optional (default = True)
7987
If True, plot magnitude and phase
80-
PrintGain: boolean (default = True)
88+
print_gain: boolean (default = True)
8189
If True, report mouse clicks when close to the root-locus branches,
8290
calculate gain, damping and print
91+
plotstr: string that declare of the rlocus (see matplotlib)
8392
8493
Returns
8594
-------
@@ -90,21 +99,74 @@ def root_locus(sys, kvect=None, xlim=None, ylim=None, plotstr='-', Plot=True,
9099
"""
91100

92101
# Convert numerator and denominator to polynomials if they aren't
93-
(nump,denp)=_systopoly1d(sys)
102+
(nump,denp)=_systopoly1d(dinsys)
94103

95104
ifkvectisNone:
96-
kvect=_default_gains(sys)
105+
gvect=_default_gains(nump,denp)
106+
else:
107+
gvect=np.asarray(kvect)
97108

98109
# Compute out the loci
99-
mymat=_RLFindRoots(sys,kvect)
100-
mymat=_RLSortRoots(sys,mymat)
110+
mymat=_find_roots(dinsys,gvect)
111+
mymat=_sort_roots(mymat)
112+
113+
# set smoothing tolerance
114+
smtolx=0.01* (np.max(np.max(np.real(mymat)))-np.min(np.min(np.real(mymat))))
115+
smtoly=0.01* (np.max(np.max(np.imag(mymat)))-np.min(np.min(np.imag(mymat))))
116+
smtol=np.max(smtolx,smtoly)
117+
118+
if~(xlimisNone):
119+
xmin=np.min(np.min(np.real(mymat)))
120+
xmax=np.max(np.max(np.real(mymat)))
121+
deltax= (xmax-xmin)*0.02
122+
xlim= [xmin-deltax,xmax+deltax]
123+
124+
if~(ylimisNone):
125+
ymin=np.min(np.min(np.imag(mymat)))
126+
ymax=np.max(np.max(np.imag(mymat)))
127+
deltay= (ymax-ymin)*0.02
128+
ylim= [ymin-deltay,ymax+deltay]
129+
130+
done=False
131+
ngain=gvect.size
132+
133+
while~done& (ngain<2000)& (kvectisNone):
134+
done=True
135+
dp=np.abs(np.diff(mymat,axis=0))
136+
dp=np.max(dp,axis=1)
137+
idx=np.where(dp>smtol)
138+
139+
foriiinnp.arange(0,idx[0].size):
140+
i1=idx[0][ii]
141+
g1=gvect[i1]
142+
p1=mymat[i1]
143+
144+
i2=idx[0][ii]+1
145+
g2=gvect[i2]
146+
p2=mymat[i2]
147+
# isolate poles in p1, p2
148+
ifnp.max(np.abs(p2-p1))>smtol:
149+
newg=np.linspace(g1,g2,5)
150+
newmymat=_find_roots(dinsys,newg)
151+
gvect=np.insert(gvect,i1+1,newg[1:4])
152+
mymat=np.insert(mymat,i1+1,newmymat[1:4],axis=0)
153+
mymat=_sort_roots(mymat)
154+
done=False# need to process new gains
155+
ngain=gvect.size
156+
ifkvectisNone:
157+
newg=np.linspace(gvect[-1],gvect[-1]*200,5)
158+
newmymat=_find_roots(dinsys,newg)
159+
gvect=np.append(gvect,newg[1:5])
160+
mymat=np.concatenate((mymat,newmymat[1:5]),axis=0)
161+
mymat=_sort_roots(mymat)
162+
kvect=gvect
101163

102164
# Create the plot
103-
if(Plot):
165+
ifplot:
104166
f=pylab.figure()
105-
ifPrintGain:
167+
ifprint_gain:
106168
f.canvas.mpl_connect(
107-
'button_release_event',partial(_RLFeedbackClicks,sys=sys))
169+
'button_release_event',partial(_feedback_clicks,sys=dinsys))
108170
ax=pylab.axes()
109171

110172
# plot open loop poles
@@ -121,49 +183,98 @@ def root_locus(sys, kvect=None, xlim=None, ylim=None, plotstr='-', Plot=True,
121183
ax.plot(real(col),imag(col),plotstr)
122184

123185
# Set up plot axes and labels
124-
ifxlim:
125-
ax.set_xlim(xlim)
126-
ifylim:
127-
ax.set_ylim(ylim)
186+
ax.set_xlim(xlim)
187+
ax.set_ylim(ylim)
128188
ax.set_xlabel('Real')
129189
ax.set_ylabel('Imaginary')
130190

131191
returnmymat,kvect
132192

133-
def_default_gains(sys):
134-
# TODO: update with a smart calculation of the gains using sys poles/zeros
135-
returnnp.logspace(-3,3)
193+
194+
def_default_gains(num,den):
195+
"""Insert gains up to a tolerance is achieved. This tolerance is a function of figure axis """
196+
nas=den.order-num.order# number of asymptotes
197+
maxk=0
198+
olpol=den.roots
199+
olzer=num.roots
200+
ifnas>0:
201+
cas= (sum(den.roots)-sum(num.roots))/nas
202+
angles= (2*np.arange(1,nas+1)-1)*np.pi/nas
203+
# print("rlocus: there are %d asymptotes centered at %f\n", nas, cas)
204+
else:
205+
cas= []
206+
angles= []
207+
maxk=100*den(1)/num(1)
208+
k_break,real_ax_pts=_break_points(num,den)
209+
ifnas==0:
210+
maxk=np.max([1,2*maxk])# get at least some root locus
211+
else:
212+
# get distance from breakpoints, poles, and zeros to center of asymptotes
213+
dmax=3*np.max(np.abs(np.concatenate((np.concatenate((olzer,olpol),axis=0),
214+
real_ax_pts),axis=0)-cas))
215+
ifdmax==0:
216+
dmax=1
217+
# get gain for dmax along each asymptote, adjust maxk if necessary
218+
svals=cas+dmax*np.exp(angles*1j)
219+
kvals=-den(svals)/num(svals)
220+
221+
ifk_break.size>0:
222+
maxk=np.max(np.max(k_break),maxk)
223+
maxk=np.max([maxk,np.max(np.real(kvals))])
224+
mink=0
225+
ngain=30
226+
gvec=np.linspace(mink,maxk,ngain)
227+
gvec=np.concatenate((gvec,k_break),axis=0)
228+
gvec.sort()
229+
returngvec
230+
231+
232+
def_break_points(num,den):
233+
"""Extract the break points over real axis and the gains that give these location"""
234+
# type: (np.poly1d, np.poly1d) -> (np.array, np.array)
235+
dnum=num.deriv(m=1)
236+
dden=den.deriv(m=1)
237+
brkp=np.poly1d(np.convolve(den,dnum)-np.convolve(num,dden))
238+
real_ax_pts=brkp.r
239+
real_ax_pts=real_ax_pts[np.imag(real_ax_pts)==0]
240+
real_ax_pts=real_ax_pts[num(real_ax_pts)!=0]# avoid dividing by zero
241+
k_break=-den(real_ax_pts)/num(real_ax_pts)
242+
idx=k_break>=0
243+
k_break=k_break[idx]
244+
real_ax_pts=real_ax_pts[idx]
245+
returnk_break,real_ax_pts
246+
136247

137248
# Utility function to extract numerator and denominator polynomials
138249
def_systopoly1d(sys):
139250
"""Extract numerator and denominator polynomails for a system"""
140251
# Allow inputs from the signal processing toolbox
141-
if(isinstance(sys,scipy.signal.lti)):
252+
ifisinstance(sys,scipy.signal.lti):
142253
nump=sys.num
143254
denp=sys.den
144255

145256
else:
146257
# Convert to a transfer function, if needed
147-
sys=_convertToTransferFunction(sys)
258+
sys=xferfcn.convertToTransferFunction(sys)
148259

149260
# Make sure we have a SISO system
150-
if(sys.inputs>1orsys.outputs>1):
261+
ifsys.inputs>1.orsys.outputs>1.:
151262
raiseControlMIMONotImplemented()
152263

153264
# Start by extracting the numerator and denominator from system object
154265
nump=sys.num[0][0]
155266
denp=sys.den[0][0]
156267

157268
# Check to see if num, den are already polynomials; otherwise convert
158-
if(notisinstance(nump,poly1d)):
269+
ifnotisinstance(nump,poly1d):
159270
nump=poly1d(nump)
160-
if(notisinstance(denp,poly1d)):
271+
ifnotisinstance(denp,poly1d):
161272
denp=poly1d(denp)
162273

163-
return(nump,denp)
274+
returnnump,denp
164275

165276

166-
def_RLFindRoots(sys,kvect):
277+
def_find_roots(sys,kvect):
167278
"""Find the roots for the root locus."""
168279

169280
# Convert numerator and denominator to polynomials if they aren't
@@ -179,36 +290,37 @@ def _RLFindRoots(sys, kvect):
179290
returnmymat
180291

181292

182-
def_RLSortRoots(sys,mymat):
183-
"""Sort the roots from sys._RLFindRoots, so that the root
293+
def_sort_roots(mymat):
294+
"""Sort the roots from sys._sort_roots, so that the root
184295
locus doesn't show weird pseudo-branches as roots jump from
185296
one branch to another."""
186297

187-
sorted=zeros_like(mymat)
298+
sorted_roots=zeros_like(mymat)
188299
forn,rowinenumerate(mymat):
189300
ifn==0:
190-
sorted[n, :]=row
301+
sorted_roots[n, :]=row
191302
else:
192303
# sort the current row by finding the element with the
193304
# smallest absolute distance to each root in the
194305
# previous row
195306
available=list(range(len(prevrow)))
196307
foreleminrow:
197-
evect=elem-prevrow[available]
308+
evect=elem-prevrow[available]
198309
ind1=abs(evect).argmin()
199310
ind=available.pop(ind1)
200-
sorted[n,ind]=elem
201-
prevrow=sorted[n, :]
202-
returnsorted
311+
sorted_roots[n,ind]=elem
312+
prevrow=sorted_roots[n, :]
313+
returnsorted_roots
203314

204315

205-
def_RLFeedbackClicks(event,sys):
316+
def_feedback_clicks(event,sys):
206317
"""Print root-locus gain feedback for clicks on the root-locus plot
207318
"""
208319
s=complex(event.xdata,event.ydata)
209-
K=-1./sys.horner(s)
210-
ifabs(K.real)>1e-8andabs(K.imag/K.real)<0.04:
320+
k=-1./sys.horner(s)
321+
ifabs(k.real)>1e-8andabs(k.imag/k.real)<0.04:
211322
print("Clicked at %10.4g%+10.4gj gain %10.4g damp %10.4g"%
212-
(s.real,s.imag,K.real,-1*s.real/abs(s)))
323+
(s.real,s.imag,k.real,-1*s.real/abs(s)))
324+
213325

214-
rlocus=root_locus
326+
rlocus=root_locus

0 commit comments

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