State Feedback
System Modeling and Control · Chapter 15
Stabilization of an Inverted Pendulum
\[\theta (s)=-\frac{\kappa m\ell }{s^{2}-\alpha ^{2}}U(s).\] where \[\alpha ^{2}\triangleq \frac{mg\ell (M+m)}{Mm\ell ^{2}+J(M+m)}, \kappa \triangleq \frac{1}{Mm\ell ^{2}+J(M+m)}.\]
With \(x_{1}=\theta ,\) \(x_{2}=\dot{\theta}\): \[\frac{d}{dt}\left[ \begin{array}{c} x_{1} \\ x_{2} \end{array} \right] =\left[ \begin{array}{cc} 0 & 1 \\ \alpha ^{2} & 0 \end{array} \right] \left[ \begin{array}{c} x_{1} \\ x_{2} \end{array} \right] +\left[ \begin{array}{c} \text{}0 \\ -\kappa m\ell \end{array} \right] u.\]
\[\frac{d}{dt}\left[ \begin{array}{c} x_{1} \\ x_{2} \end{array} \right] =\underset{A}{\underbrace{\left[ \begin{array}{cc} 0 & 1 \\ \alpha ^{2} & 0 \end{array} \right] }}\left[ \begin{array}{c} x_{1} \\ x_{2} \end{array} \right] +\underset{b}{\underbrace{\left[ \begin{array}{c} 0 \\ -\kappa m\ell \end{array} \right] }}u.\]
Objective: Keep \(\theta =0\) and \(\dot{\theta}=0\) using the input \(u.\)
Try a state feedback controller given by \[u=-\underset{k}{\underbrace{\left[ \begin{array}{cc} k_{1} & k_{2} \end{array} \right] }}\left[ \begin{array}{c} \theta (t) \\ \dot{\theta}(t) \end{array} \right] =-\left[ \begin{array}{cc} k_{1} & k_{2} \end{array} \right] \left[ \begin{array}{c} x_{1} \\ x_{2} \end{array} \right] .\]
\[\begin{aligned} \frac{d}{dt}\left[ \begin{array}{c} x_{1} \\ x_{2} \end{array} \right] &=\left[ \begin{array}{cc} 0 & 1 \\ \alpha ^{2} & 0 \end{array} \right] \left[ \begin{array}{c} x_{1} \\ x_{2} \end{array} \right] -\left[ \begin{array}{c} 0 \\ -\kappa m\ell \end{array} \right] \left[ \begin{array}{cc} k_{1} & k_{2} \end{array} \right] \left[ \begin{array}{c} x_{1} \\ x_{2} \end{array} \right] \\ &=\underset{A-bk}{\underbrace{\left( \left[ \begin{array}{cc} 0 & 1 \\ \alpha ^{2} & 0 \end{array} \right] -\left[ \begin{array}{cc} 0 & 0 \\ -\kappa m\ell k_{1} & -\kappa m\ell k_{2} \end{array} \right] \right) }}\left[ \begin{array}{c} x_{1} \\ x_{2} \end{array} \right] \\ &=\left[ \begin{array}{cc} 0 & 1 \\ \alpha ^{2}+\kappa m\ell k_{1} & \kappa m\ell k_{2} \end{array} \right] \left[ \begin{array}{c} x_{1} \\ x_{2} \end{array} \right] . \end{aligned}\]
\(\qquad \qquad \qquad \mathbf{\Longrightarrow }\) \[\left[ \begin{array}{c} x_{1}(t) \\ x_{2}(t) \end{array} \right] =e^{(A-bk)t}\left[ \begin{array}{c} x_{1}(0) \\ x_{2}(0) \end{array} \right] .\]
For any initial conditions we want \[\left[ \begin{array}{c} x_{1}(t) \\ x_{2}(t) \end{array} \right] =e^{(A-bk)t}\left[ \begin{array}{c} x_{1}(0) \\ x_{2}(0) \end{array} \right] \rightarrow \left[ \begin{array}{c} 0 \\ 0 \end{array} \right] .\] Equivalently, we require \[e^{(A-bk)t}\rightarrow \left[ \begin{array}{cc} 0 & 0 \\ 0 & 0 \end{array} \right] .\] Must choose \(k\) to make this happen!
Motor Parameters
\(K_{T}=K_{b}\) the torque constant (back-emf constant).
\(R\) the armature resistance of the DC motor.
\(n_{2}/n_{1}\) is the gear ratio.
Etc.
Motor Variables
\(V(t)\) is the voltage applied to the DC motor of the cart’s powered wheel.
\(x\) is the cart position along the track.
\(v=dx/dt\) is the cart’s velocity.
\(F_{d}(t)=Mg\sin (\phi )\) is the force (disturbance) on the cart due to gravity.
Transfer Function Model
\[a_{0}\triangleq \frac{K_{b}K_{T}\left( \dfrac{n_{2}}{n_{1}}\dfrac{1}{r_{m}} \right) ^{2}}{RJ/r_{m}^{2}+RM},b_{0}\triangleq \frac{K_{T}\dfrac{ n_{2}}{n_{1}}\dfrac{1}{r_{m}}}{RJ/r_{m}^{2}+RM},K_{D}\triangleq \frac{R}{K_{T}\dfrac{n_{2}}{n_{1}}\dfrac{1}{r_{m}}},\text{}b_{0}K_{D}= \dfrac{1}{J/r_{m}^{2}+M}\]
\(K_{D}F_{d}=K_{D}Mg\sin (\phi )\) is an input voltage.
It produces the same effect on the cart’s position \(x(t)\) and speed \(v(t)\) as \(F_{d}.\)
\(b_{0}K_{D}F_{d}=\dfrac{Mg\sin (\phi )}{J/r_{m}^{2}+M}\) is the acceleration of the cart due to gravity.
Statespace Model \[\begin{aligned} \frac{dx}{dt} &=v \\ \frac{dv}{dt} &=-a_{0}v+b_{0}V(t)-\underset{d}{\underbrace{b_{0}K_{D}F_{d}}} . \end{aligned}\] where \[d\triangleq b_{0}K_{D}F_{d}=\frac{Mg\sin (\phi )}{J/r_{m}^{2}+M}.\]
With \(u\triangleq V(t)\) and \(F_{d}=0\) this becomes \[\begin{aligned} \frac{dx}{dt} &=v \\ \frac{dv}{dt} &=-a_{0}v+b_{0}u. \end{aligned}\]
Objective: Have the cart follow (track) a specified trajectory \((x_{ref}(t),v_{ref}(t))\).
Trajectory Specifications: \[\begin{array}{lcl} v_{ref}(0)\;=0 & & \dot{v}_{ref}(0)\;=0 \\ v_{ref}(t_{1})=v_{\max } & & \dot{v}_{ref}(t_{1})=0 \\ v_{ref}(t)\;\,=v_{\max } & & t_{1}\leq t\leq t_{2} \\ v_{ref}(t_{2})=v_{\max } & & \dot{v}_{ref}(t_{2})=0 \\ v_{ref}(t_{3})=0 & & \dot{v}_{ref}(t_{3})=0. \end{array}\] and \[\begin{aligned} v_{ref}(t) &=v_{ref}(t_{3}-t)\text{for}t_{2}\leq t\leq t_{3} \\ \int_{0}^{t_{3}}v_{ref}(\tau )d\tau &=x_{f}. \end{aligned}\]
Try \[\begin{aligned} v_{ref}(t) &=c_{1}t^{2}+c_{2}t^{3}\text{for}0\leq t\leq t_{1} \\ v_{ref}(0) &=0 \\ \dot{v}_{ref}(0) &=0. \end{aligned}\] At \(t_{1}\) \[\begin{aligned} v_{ref}(t_{1}) &=c_{1}t_{1}^{2}+c_{2}t_{1}^{3}=v_{\max } \\ \dot{v}_{ref}(t_{1}) &=2c_{1}t_{1}+3c_{2}t_{1}^{2}=0\text{} \end{aligned}\] or \[\left[ \begin{array}{cc} t_{1}^{2} & t_{1}^{3} \\ 2t_{1} & 3t_{1}^{2} \end{array} \right] \left[ \begin{array}{c} c_{1} \\ c_{2} \end{array} \right] =\left[ \begin{array}{c} v_{\max } \\ 0 \end{array} \right] .\]
From previous slide: \[\left[ \begin{array}{cc} t_{1}^{2} & t_{1}^{3} \\ 2t_{1} & 3t_{1}^{2} \end{array} \right] \left[ \begin{array}{c} c_{1} \\ c_{2} \end{array} \right] =\left[ \begin{array}{c} v_{\max } \\ 0 \end{array} \right] .\] \(\Longrightarrow\) \[\left[ \begin{array}{c} c_{1} \\ c_{2} \end{array} \right] =\frac{1}{t_{1}^{4}}\left[ \begin{array}{rr} 3t_{1}^{2} & -t_{1}^{3} \\ -2t_{1} & t_{1}^{2} \end{array} \right] \left[ \begin{array}{c} v_{\max } \\ 0 \end{array} \right] =\left[ \begin{array}{c} +3v_{\max }/t_{1}^{2} \\ -2v_{\max }/t_{1}^{3} \end{array} \right] .\] \(v_{ref}(t):\) \[v_{ref}(t)=\left\{ \begin{array}{lcl} c_{1}t^{2}+c_{2}t^{3}\text{,} & & 0\leq t\leq t_{1} \\ v_{\text{max}}, & & t_{1}\leq t\leq t_{2} \\ c_{1}(t_{3}-t)^{2}+c_{2}(t_{3}-t)^{3}, & & t_{2}\leq t\leq t_{3} \end{array} \right.\]
At time \(t_{1}\): \[x_{ref}(t_{1})=\int_{0}^{t_{1}}v_{ref}(\tau )d\tau =c_{1}\frac{t_{1}^{3}}{3} +c_{2}\frac{t_{1}^{4}}{4}=\frac{3v_{\max }}{t_{1}^{2}}\frac{t_{1}^{3}}{3}- \frac{2v_{\max }}{t_{1}^{3}}\frac{t_{1}^{4}}{4}=\frac{v_{\max }t_{1}}{2}.\] Total distance traveled: \[x_{f}=\int_{0}^{t_{3}}v_{ref}(\tau )d\tau =2x_{ref}(t_{1})+v_{\max }(t_{2}-t_{1})=2\frac{v_{\max }t_{1}}{2}+v_{\max }(t_{2}-t_{1})=v_{\max }t_{2}.\]
\(\mathbf{x}_{\mathbf{ref}}\mathbf{(t):}\) \[x_{ref}(t)=\left\{ \begin{array}{lcl} c_{1}t^{3}/3+c_{2}t^{4}/4, & & 0\leq t\leq t_{1} \\ v_{\text{max}}t_{1}/2+v_{\text{max}}(t-t_{1}), & & t_{1}\leq t\leq t_{2} \\ v_{\text{max}}t_{2}-c_{1}(t_{3}-t)^{3}/3-c_{2}(t_{3}-t)^{4}/4, & & t_{2}\leq t\leq t_{3}. \end{array} \right.\]
\[\alpha _{ref}(t)=\left\{ \begin{array}{lcl} 2c_{1}t+3c_{2}t^{2}, & & 0\leq t\leq t_{1} \\ 0, & & t_{1}\leq t\leq t_{2} \\ -2c_{1}(t_{3}-t)-3c_{2}(t_{3}-t)^{2}, & & t_{2}\leq t\leq t_{3}. \end{array} \right.\]
Control Problem: With \[\frac{d\epsilon }{dt}=A\epsilon +bw\] find \(k\) such that \[w=-\left[ \begin{array}{cc} k_{1} & k_{2} \end{array} \right] \left[ \begin{array}{c} \epsilon _{1} \\ \epsilon _{2} \end{array} \right]\] results in \[\frac{d\epsilon }{dt}=(A-bk)\epsilon\] being stable, i.e., \[\epsilon (t)=e^{(A-bk)t}\left[ \begin{array}{c} \epsilon _{1}(0) \\ \epsilon _{2}(0) \end{array} \right] \rightarrow \left[ \begin{array}{c} 0 \\ 0 \end{array} \right] .\]
With \(\epsilon \triangleq x_{d}-x\) and \(w\triangleq u_{d}-u\) \[\frac{d}{dt}\epsilon =A\epsilon +bw.\]
Choose \(w=-k\epsilon\) to obtain \[\begin{aligned} \frac{d}{dt}\epsilon &=(A-bk)\epsilon \\ \Longrightarrow \left[ \begin{array}{c} \epsilon (t) \\ \end{array} \right] &=\left[ \begin{array}{ccc} & & \\ & e^{(A-bk)t} & \\ & & \end{array} \right] \left[ \begin{array}{c} \epsilon (0) \\ \end{array} \right] . \end{aligned}\]
Need to find \(k\) such that \[e^{(A-bk)t}\rightarrow 0_{n\times n}.\]
Recall that \[e^{(A-bk)t}\rightarrow 0_{n\times n}\]
if and only if the roots of \[\det (sI-(A-bk))=0\] are in the open left half-plane.
Equivalent Formulation With \[r(t)\triangleq u_{d}(t)+kx_{d}(t)\] the block diagram
is equivalent to the block diagram on the previous slide.
Magnetic Levitation
Linear statespace model \[\frac{d}{dt}\left[ \begin{array}{c} i-i_{eq} \\ x-x_{eq} \\ v-v_{eq} \end{array} \right] =\underset{A}{\underbrace{\left[ \begin{array}{ccc} -\dfrac{R}{L_{0}} & 0 & \dfrac{rL_{1}}{L_{0}}\dfrac{i_{eq}}{x_{eq}^{2}} \\ 0 & 0 & 1 \\ -\dfrac{2g}{i_{eq}} & \dfrac{2g}{x_{eq}} & 0 \end{array} \right] }}\left[ \begin{array}{c} i-i_{eq} \\ x-x_{eq} \\ v-v_{eq} \end{array} \right] +\underset{b}{\underbrace{\left[ \begin{array}{c} \dfrac{1}{L_{0}} \\ 0 \\ 0 \end{array} \right] }}(u-u_{eq})\]
Set \[\left[ \begin{array}{c} z_{1} \\ z_{2} \\ z_{3} \end{array} \right] \triangleq \left[ \begin{array}{c} i-i_{eq} \\ x-x_{eq} \\ v-v_{eq} \end{array} \right] ,w=u-u_{eq}\text{and}A=\left[ \begin{array}{ccc} a_{11} & 0 & a_{13} \\ 0 & 0 & 1 \\ a_{31} & a_{32} & 0 \end{array} \right] ,b=\left[ \begin{array}{c} b_{1} \\ 0 \\ 0 \end{array} \right]\]
Magnetic Levitation (continued)
\[\frac{d}{dt}\left[ \begin{array}{c} z_{1} \\ z_{2} \\ z_{3} \end{array} \right] =\left[ \begin{array}{ccc} a_{11} & 0 & a_{13} \\ 0 & 0 & 1 \\ a_{31} & a_{32} & 0 \end{array} \right] \left[ \begin{array}{c} z_{1} \\ z_{2} \\ z_{3} \end{array} \right] +\left[ \begin{array}{c} b_{1} \\ 0 \\ 0 \end{array} \right] w\]
With \[w=-\left[ \begin{array}{ccc} k_{1} & k_{2} & k_{3} \end{array} \right] \left[ \begin{array}{c} z_{1} \\ z_{2} \\ z_{3} \end{array} \right]\] the closed-loop system is \[\frac{dz}{dt}=(A-bk)z\Longrightarrow z(t)=e^{(A-bk)t}z(0).\]
Magnetic Levitation (continued)
\[A-bk=\left[ \begin{array}{ccc} a_{11} & 0 & a_{13} \\ 0 & 0 & 1 \\ a_{31} & a_{32} & 0 \end{array} \right] -\left[ \begin{array}{c} b_{1} \\ 0 \\ 0 \end{array} \right] \left[ \begin{array}{ccc} k_{1} & k_{2} & k_{3} \end{array} \right] =\left[ \begin{array}{ccc} a_{11}-b_{1}k_{1} & -b_{1}k_{2} & a_{13}-b_{1}k_{3} \\ 0 & 0 & 1 \\ a_{31} & a_{32} & 0 \end{array} \right]\] \[sI-(A-bk)=\left[ \begin{array}{ccc} s-(a_{11}-b_{1}k_{1}) & b_{1}k_{2} & -(a_{13}-b_{1}k_{3}) \\ 0 & s & -1 \\ -a_{31} & -a_{32} & s \end{array} \right]\] Find \(k\) so that \[z(t)=\left[ \begin{array}{c} z_{1}(t) \\ z_{2}(t) \\ z_{3}(t) \end{array} \right] \triangleq \left[ \begin{array}{c} i(t)-i_{eq} \\ x(t)-x_{eq} \\ v(t)-v_{eq} \end{array} \right] \rightarrow \left[ \begin{array}{c} 0 \\ 0 \\ 0 \end{array} \right]\]
\[\begin{aligned} \epsilon _{a}(\infty )=\lim_{s\rightarrow 0}sE(s) &=\lim_{s\rightarrow 0}s\left( sI-(A_{a}-b_{a}k_{a})\right) ^{-1}\epsilon _{a}(0)+\left( sI- (A_{a}-b_{a}k_{a})\right) ^{-1}p_{a}d \\ &=-\left( A_{a}-b_{a}k_{a}\right) ^{-1}p_{a}d. \end{aligned}\]
It is shown below that \[\epsilon _{1}(\infty )=x_{ref}(\infty )-x(\infty )=0,\epsilon _{2}(\infty )=v_{ref}(\infty )-v(\infty )=0,\text{\ and}\epsilon _{0}(\infty )=\dfrac{d}{b_{0}k_{0}}.\]
\[\begin{aligned} E_{1}(s) &=\frac{-b_{0}k_{0}\epsilon _{0}(0)+\left( s^{2}+(b_{0}k_{2}+a_{0})s\right) \epsilon _{1}(0)+s\epsilon _{2}(0)+d}{ s^{3}+(b_{0}k_{2}+a_{0})s^{2}+b_{0}k_{1}s+b_{0}k_{0}} \\ &=\frac{A_{1}}{s+r_{1}}+\frac{B_{1}}{s+r_{2}}+\frac{C_{1}}{s+r_{3}} \\ \Longrightarrow x_{ref}(t)-x(t) &=\epsilon _{1}(t)=A_{1}e^{-r_{1}t}+B_{1}e^{-r_{2}t}+C_{1}e^{-r_{3}t}\rightarrow 0. \end{aligned}\]
\[\begin{aligned} E_{2}(s) &=\frac{-b_{0}k_{0}s\epsilon _{0}(0)-(b_{0}k_{1}s+b_{0}k_{0})\epsilon _{1}(0)+s^{2}\epsilon _{2}(0)+sd}{ s^{3}+(b_{0}k_{2}+a_{0})s^{2}+b_{0}k_{1}s+b_{0}k_{0}} \\ &=\frac{A_{2}}{s+r_{1}}+\frac{B_{2}}{s+r_{2}}+\frac{C_{2}}{s+r_{3}} \\ \Longrightarrow v_{ref}(t)-v(t) &=\epsilon _{2}(t)=A_{2}e^{-r_{1}t}+B_{2}e^{-r_{2}t}+C_{2}e^{-r_{3}t}\rightarrow 0. \\ && \end{aligned}\]
\[\begin{aligned} \lim_{s\rightarrow 0}sE_{0}(s) &=\lim_{s\rightarrow 0}s\frac{ (s^{2}+(b_{0}k_{2}+a_{0})s+b_{0}k_{1})\epsilon _{0}(0)+(s+b_{0}k_{2}+a_{0})\epsilon _{1}(0)+\epsilon _{2}(0)+ \mathbf{(d/s)} }{s^{3}+(b_{0}k_{2}+a_{0})s^{2}+b_{0}k_{1}s+b_{0}k_{0}} \\ &=\frac{d}{b_{0}k_{0}}. \end{aligned}\]
\[V(t)=u(t)=u_{ref}(t)+\left( k_{0}\int_{0}^{t}\epsilon _{1}(\tau )d\tau +k_{1}\epsilon _{1}(t)+k_{2}\epsilon _{2}(t)\right)\]
Statespace Model and Equilibrium Points
\[\dot z=Az+bu, \qquad z=\left[ \begin{array}{c} x \\ v \\ \theta \\ \omega \end{array} \right].\]
\[A=\left[ \begin{array}{cccc} 0 & 1 & 0 & 0 \\ 0 & 0 & -\dfrac{gm^{2}\ell ^{2}}{Mm\ell ^{2}+J(M+m)} & 0 \\ 0 & 0 & 0 & 1 \\ 0 & 0 & \dfrac{mg\ell (M+m)}{Mm\ell ^{2}+J(M+m)} & 0 \end{array} \right], \qquad b=\left[ \begin{array}{c} 0 \\ \dfrac{J+m\ell ^{2}}{Mm\ell ^{2}+J(M+m)} \\ 0 \\ -\dfrac{m\ell }{Mm\ell ^{2}+J(M+m)} \end{array} \right].\]
\[z_{eq}=\left[ \begin{array}{c} x_{0} \\ 0 \\ 0 \\ 0 \end{array} \right], \qquad u_{eq}=0, \qquad (A,b)\text{ is controllable}.\]
We can find \(k\in \mathbb{R} ^{1\times 4}\) so \(u=u_{eq}-k(z-z_{eq})=-k(z-z_{eq})\) gives
\(\dfrac{d}{dt}(z-z_{eq})=(A-bk)(z-z_{eq})\) with \(A-bk\) stable so \(z(t)\rightarrow z_{eq}.\)
Choose \(u=-kz\) to place the closed-loop poles.
Equivalent block diagram
Transfer function representation
Add integrator to track step inputs
If \(k_{0}\) can be chosen so that the closed-loop system is stable then \(x(t)\rightarrow x_{ref}.\) \[\begin{aligned} \frac{dz}{dt} &=Az+bu \\ u &=-kz-k_{0}z_{0} \\ y &=cz=x \\ \frac{dz_{0}}{dt} &=x_{ref}-x \end{aligned}\] or \[\begin{aligned} \frac{d}{dt}\underset{\in \mathbb{R} ^{5}}{\underbrace{\left[ \begin{array}{c} z \\ z_{0} \end{array} \right] }} &=\underset{A_{a}\in \mathbb{R} ^{5\times 5}}{\underbrace{\left[ \begin{array}{cc} A & 0_{4\times 1} \\ -c & 0 \end{array} \right] }}\underset{\in \mathbb{R} ^{5}}{\underbrace{\left[ \begin{array}{c} z \\ z_{0} \end{array} \right] }}+\underset{b_{a}\in \mathbb{R} ^{5}}{\underbrace{\left[ \begin{array}{c} b \\ 0 \end{array} \right] }}u+\underset{p\in \mathbb{R} ^{5}}{\underbrace{\left[ \begin{array}{c} 0_{4\times 1} \\ 1 \end{array} \right] }}x_{ref} \\ u &=-\underset{\in \mathbb{R} ^{1\times 5}}{\underbrace{\left[ \begin{array}{cc} k & k_{0} \end{array} \right] }}\left[ \begin{array}{c} z \\ z_{0} \end{array} \right] . \end{aligned}\]
What about the value of \(z_{0_eq}\mathbf{?}\)
In the figure \[u=-\left[ \begin{array}{cc} k & k_{0} \end{array} \right] \left[ \begin{array}{c} z \\ z_{0} \end{array} \right]\] not \[u=-\left[ \begin{array}{cc} k & k_{0} \end{array} \right] \left( \left[ \begin{array}{c} z \\ z_{0} \end{array} \right] -\left[ \begin{array}{c} z_{eq} \\ z_{0_eq} \end{array} \right] \right) .\] These are the same if \[\begin{aligned} \left[ \begin{array}{cc} k & k_{0} \end{array} \right] \left[ \begin{array}{c} z_{eq} \\ z_{0_eq} \end{array} \right] &=\underset{k_{a}}{\underbrace{\left[ \begin{array}{ccccc} k_{1} & k_{2} & k_{3} & k_{4} & k_{0} \end{array} \right] }}\left[ \begin{array}{c} x_{ref} \\ 0 \\ 0 \\ 0 \\ z_{0_eq} \end{array} \right] =0 \\ \text{or}z_{0_eq} &=-(k_{1}/k_{0})x_{ref}. \end{aligned}\]
The value of \(z_{0_eq}\mathbf{.}\)
That is, the feedback \(u=-k_{a}z_{a}\) applied to \[\frac{dz_{a}}{dt}=A_{a}z_{a}+b_{a}u+p_{a}x_{ref}\]
results in \[z_{a}(t)\rightarrow z_{a_eq}=\left[ \begin{array}{c} x_{ref} \\ 0 \\ 0 \\ 0 \\ -(k_{1}/k_{0})x_{ref} \end{array} \right] \text{and so}k_{a}z_{a_eq}=0.\]
