AE4610
  • Welcome
  • Experiments
  • Lab 0: MATLAB & SIMULINK
  • Lab 1: Rotary Servo Base
    • A. Integration (Week 1)
    • B. Modeling (Week 1)
    • C. Control Design (Week 2)
    • D. Controller Implementation & Evaluation (Week 3)
  • Lab 2: 3 DOF Gyroscope
    • A & B. Modeling and Control Design (Week 1)
    • C. Controller Implementation (Week 2)
  • Lab 3: Rotary Flexible Link
    • A. System Identification (Week 1)
    • B & C. Control Design and Controller Implementation (Week 2)
  • Lab 4: Rotary Inverted Pendulum
    • A & B. Modeling and Balance Control
    • C. Swing-up Control (Demo)
  • Final Project
  • Archive
    • Old Lab 1 Model Validation
    • Problem Set
    • DC Motor
    • Lab 1: Rotary Servo Base (Older Version)
      • A. Integration (Week 1)
      • B. Modelling (Week 1)
      • C. Position Control (Week 2)
    • Quadcopter
    • Gyroscope
    • Inverted Pendulum
    • 3DOF Helicopter
    • Torsional Pendulum
    • Aero LQR
      • A. System Identification
      • A. System Identification (Week 1) last check
      • B. Control Design & Implementation (Week 2)
    • Rotary Gyro
      • Rotary Gyro
    • 2 DOF AERO
      • A. System Identification
      • B. Control Design
      • C. Controller Implementation
    • Copy of Lab 4: Rotary Inverted Pendulum
      • Week 1
      • Week 2
Powered by GitBook
On this page
  • Objective
  • Equipment
  • Week 1
  • A. Modeling
  • Rotary Inverted Pendulum Model
  • Model Convention
  • Nonlinear Equations of Motion
  • Linearization
  • Linearization of Inverted Pendulum Equations
  • Analysis: Modeling
  • B. Balance Control
  • Specification
  • Stability
  • Controllability
  • Companion Matrix
  • Pole Placement Theory
  • Desired Poles
  • Simulation Model with Feedback
  • B.1 Experiment: Designing the Balance Control
  • B.2 Experiment: Simulating the Balance Control
  • Results for Report
  • A) Modeling
  • B.1) Balance Control Design
  • B.2) Balance Control Simulation
  • Questions for Report
  • B.1) Balance Control Design
  • Appendix
  • Table A Main Rotary Servo Base Unit Specifications
  • Reference

Was this helpful?

Export as PDF
  1. Archive
  2. Copy of Lab 4: Rotary Inverted Pendulum

Week 1

Objective

The objectives of this laboratory experiment are as follows:

  1. Obtain the linear state-space representation of the rotary pendulum plant.

  2. Design a state-feedback control system that balances the pendulum in its upright position using Pole Placement.

  3. Simulate the closed-loop system to ensure the given specifications are met.

Equipment

Week 1

A. Modeling

This experiment involves modeling of the rotary inverted pendulum.

Rotary Inverted Pendulum Model

Table 1 Rotary Pendulum components (Figure 1)

ID#
Component

1

Rotary Servo

2

Thumbscrews

3

Rotary Arm

4

Shaft Housing

5

Shaft

6

Pendulum T-Fitting

7

Pendulum Link

8

Pendulum Encoder Connector

9

Pendulum Encoder

Table 2 Main Parameters associated with the Rotary Pendulum module

Symbol
Description
Value
Unit

Mass of pendulum

0.127

Total length of pendulum

0.337

Distance from pivot to center of mass

0.156

Pendulum moment of intertia about center of mass

0.0012

Pendulum viscous damping coeffi­cient as seen at the pivot axis

0.0024

Mass of rotary arm with two thumb screws

0.257

Rotary arm length from pivot to tip

0.216

Rotary arm length from pivot to center of mass

0.0619

Rotary arm moment of inertia about its center of mass

9.98 x 10^-4

Rotary arm viscous damping coeffi­cient as seen at the pivot axis

0.0024

Rotary arm moment of inertia about pivot

0.0020

Pendulum encoder resolution

4096

Model Convention

Nonlinear Equations of Motion

Instead of using classical (Newtonian) mechanics, the Lagrange method is used to find the equations of motion of the system. This systematic method is often used for more complicated systems such as robot manipulators with multiple joints.

Specifically, the equations that describe the motions of the rotary arm and the pendulum with respect to the servo motor voltage, i.e., the dynamics, will be obtained using the Euler-Lagrange equation:

\frac{d}{dt}\frac{\partial L}{\partial \dot{q_i}} - \frac{\partial L}{\partial q_i} = Q_i \qquad \qquad \tag{1}

q(t) =\begin{bmatrix} \theta(t) & \alpha(t) \end{bmatrix}^T \tag{2}

\dot{q}(t)=\begin{bmatrix} \displaystyle \frac{d\theta(t)}{dt} & \displaystyle\frac{d\alpha(t)}{dt} \end{bmatrix} ^T\tag{3}

With the generalized coordinates defined, the Euler-Lagrange equations for the rotary pendulum system are\frac{d}{dt} \frac{\partial L}{\partial \dot{\theta}} - \frac{\partial L}{\partial \theta} = Q_1 \tag{4a}

\frac{d}{dt}\frac{\partial L}{\partial \dot{\alpha}} - \frac{\partial L}{\partial \alpha} = Q_2 \tag{4b}

The Lagrangian of the system is described byL = T - V \tag{5}

and acting on the pendulum isQ_2 = -B_\rm{p}\dot{\alpha} \tag{6b}

Once the kinetic and potential energy are obtained and the Lagrangian is found, then the task is to compute various derivatives to get the EOMs. After going through this process, the nonlinear equations of motion for the Rotary Pendulum are:

​\displaystyle\left (m_pL_r^2 + \frac{1}{4}m_pL_p^2 - \frac{1}{4}m_pL_p^2 \cos^2(\alpha)+J_r \right )\ddot{\theta} - \left (\frac{1}{2}m_pL_pL_r\cos(\alpha) \right)\ddot{\alpha} \\+ \displaystyle\left(\frac{1}{2}m_pL_p^2\sin(\alpha)\cos(\alpha)\right)\dot{\theta}\dot{\alpha} + \left( \frac{1}{2}m_pL_pL_r\sin(\alpha)\right)\dot{\alpha}^2 = \tau - B_r\dot{\theta} \tag{7}​

\displaystyle{\left(-\frac{1}{2}m_pL_pL_r\cos(\alpha) \right)\ddot{\theta} + \left(J_p + \frac{1}{4}m_pL_p^2 \right) \ddot{\alpha} - \left(\frac{1}{4} m_pL_p^2\cos(\alpha)\sin(\alpha)\right)\dot{\theta}^2 \\-\frac{1}{2} m_pL_pg\sin(\alpha) = -B_p\dot{\alpha}} \tag{8}

\tau = \displaystyle\frac{\eta_gK_g\eta_mk_t(V_m - K_gk_m\dot{\theta})}{R_m} \tag{9}

Linearization

Linearization of a nonlinear function about a selected point is obtained by retaining upto first order term in the Taylor Series expansion of the function about the selected point. For example, linearization of a two variable nonlinear function f(z) where

about the point

can be written as

​

Linearization of Inverted Pendulum Equations

The nonlinear equations of the inverted pendulum system obtained as Equations (7) and (8) are linearized about the equilibrium point with the pendulum in the upright position, i.e.,

Linearization of Equation (7) about the above equilibrium state gives

Likewise, linearization of Equation (8) about the equilibrium point with the pendulum in the upright position gives

Equation (10) and (11) can be arranged in the matrix form as

Equation (12) can be rewritten in state space form as

Analysis: Modeling

  1. Download the above zip file and extract it.

  2. Note down your state-space matrices for your report. Note: You may want to cross-check the state-space matrix with TAs before proceeding to balance control.

  3. Find the open-loop poles of the system. Hint: Use eig(A).

B. Balance Control

Specification

Stability

The stability of a system can be determined from its poles ([2]):

  • Stable systems have poles only in the left-hand plane.

  • Unstable systems have at least one pole in the righthand plane and/or poles of multiplicity greater than 1 on the imaginary axis.

  • Marginally stable systems have one pole on the imaginary axis and the other poles in the left-hand plane.

Controllability

Rank Test The system is controllable if the rank of its controllability matrix

T = [B \ AB \ A^2B \ ... \ A^{n-1}B] \tag{16} equals the number of states in the system, \mathrm{rank}(T) = n \tag{17}

Companion Matrix

If (A,B) are controllable and B is n×1, then A is similar to a companion matrix ([1]). Let the characteristic equation of A be s^n + a_ns^{n-1}+\dots +a_1 \tag{18} Then the companion matrices of A and B are \tilde{A} = \begin{bmatrix} 0 & 1 & \dots & 0& 0 \\ 0 & 0 & \dots & 0& 0 \\ \vdots & \vdots & \ddots & \vdots& \vdots \\ 0 & 0 & \dots & 0& 1 \\ -a_1 & -a_2 & \dots & -a_{n-1}& -a_n \end{bmatrix} \tag{19}

and\tilde{B} = \begin{bmatrix} 0\\\vdots\\ \\1 \end{bmatrix} \tag{20}

Pole Placement Theory

>> K = acker(A,B,DP);

Desired Poles

Simulation Model with Feedback

B.1 Experiment: Designing the Balance Control

  1. In the same rotpen_week1_student.mlx live script, go to the Balance Control section. Enter the chosen zeta and omega_n values.

  2. Find gain K using a predefined Compensator Design MATLAB command K = acker(A,B,DP), which is based on pole-placement design. Note: DP is a row vector of the desired poles found in Step 3.

For sanity check, if you use damping ratio of 0.7 and natural frequency of 4 rad/s, you should get around K = [-12 63 -5.5 7].

B.2 Experiment: Simulating the Balance Control

  1. Run rotpen_week1_student.mlx live script. Ensure the gain K you found is loaded in the workspace (type K matrix in the command window).

  2. Open and run the s_rotpen_bal.mdl for 10 seconds. The responses in the scopes shown in Figure 6 were generated using an arbitrary feedback control gain. Note: When the simulation stops, the last 10 seconds of data is automatically saved in the MATLAB workspace to the variables data_theta, data_alpha, and data_Vm.

  3. Save the data corresponding to the simulated response of the rotary arm, pendulum, and motor input voltage obtained using your obtained gain K. Note: The time is stored in the data_theta(:,1) vector, the desired and measured rotary arm angles are saved in the data_theta(:,2) and data_theta(:,3) arrays. Similarly, the pendulum angle is stored in the data_alpha(:,2) vector, and the control input is in the data_Vm(:,2) structure.

Results for Report

A) Modeling

  1. Open-loop poles of the system.

B.1) Balance Control Design

B.2) Balance Control Simulation

Questions for Report

B.1) Balance Control Design

Appendix

Table A Main Rotary Servo Base Unit Specifications

Motor nominal input voltage

6.0 V

Motor armature resistance

2.6 Ω ± 12%

Motor armature inductance

0.18 mH

Motor current-torque constant

7.68 × 10−3 N-m/A ± 12%

Motor back-emf constant

7.68 × 10−3 V/(rad/s) ± 12%

High-gear total gear ratio

70

Motor efficiency

0.69 ± 5%

Gearbox efficiency

0.90 ± 10%

rotor Rotor moment of inertia

3.90 × 10−7 kg-m2 ± 10%

High-gear equivalent moment of inertia without external load

1.823 × 10−3 kg-m2

High-gear Equivalent viscous damping coefficient

0.015 N-m/(rad/s)

Mass of bar load

0.038 kg

Length of bar load

0.1525 m

Mass of disc load

0.04 kg

Radius of disc load

0.05 m

Maximum load mass

5 kg

Maximum input voltage frequency

50 Hz

Maximum input current

1 A

Maximum motor speed

628.3 rad/s

Reference

[1] Bruce Francis. Ece1619 linear systems, 2001. [2] Norman S. Nise. Control Systems Engineering. John Wiley & Sons, Inc., 2008.

PreviousCopy of Lab 4: Rotary Inverted PendulumNextWeek 2

Last updated 8 months ago

Was this helpful?

​

​

​

​

​

​

​

​

​

​

​

​

​

​

The rotary inverted pendulum model is shown in Figure 2. The rotary arm pivot is attached to the Rotary Servo system and is actuated. The arm has a length of L_\rm{r}, a moment of inertia of J_\rm{r}, and its angle, θ\bm\thetaθ, increases positively when it rotates counterclockwise (CCW). The servo (and thus the arm) should turn in the CCW direction when the control voltage is positive, i.e. V_\rm{m} > 0.

The pendulum link is connected to the end of the rotary arm. It has a total length of LpL_pLp​ and it center of mass is {L_\rm{p}}/{2} . The moment of inertia about its center of mass is J_\rm{p}. The inverted pendulum angle, α\bm\alphaα, is zero when it is perfectly upright in the vertical position and increases positively when rotated CCW.

The variables qi\bm q_iqi​ are called generalized coordinates. For this system let

where, as shown in Figure 2, θ(t)\theta(t)θ(t) is the rotary arm angle and α(t)\alpha(t)α(t) is the inverted pendulum angle. The corresponding velocities are

where TTT is the total kinetic energy of the system and VVV is the total potential energy of the system. Thus the Lagrangian is the difference between a system’s kinetic and potential energies.

The generalized forces QiQ_iQi​ are used to describe the nonconservative forces (e.g. friction) applied to a system with respect to the generalized coordinates. In this case, the generalized force acting on the rotary arm isQ_1 = \tau - B_\rm{r}\dot{\theta} \tag{6a}

​See in the Appendix for a description of the corresponding Rotary Servo parameters (e.g., such as the back-emf constant, k_\rm{m}). Our control variable is the input servo motor voltage, V_\rm{m}. Opposing the applied torque is the viscous friction torque, or viscous damping, corresponding to the term B_\rm{r}. Since the pendulum is not actuated, the only force acting on the link is the damping. The viscous damping coefficient of the pendulum is denoted by B_\rm{p}.

The torque applied at the base of the rotary arm (i.e. at the load gear) is generated by the servo motor as described by the equation 7. Refer to in the Appendix for the Rotary Servo parameters.

z=[z1z2]Tz = \begin {bmatrix} z_1 & z_2\end {bmatrix}^Tz=[z1​​z2​​]T
z0=[ab]Tz_0 = \begin {bmatrix} a & b\end {bmatrix}^Tz0​=[a​b​]T
flin(z)=f(z0)+∂f(z)∂z1∣z=z0(z1−a)+∂f(z)∂z2∣z=z0(z2−b)f_{lin}(z) = f(z_0) + \frac{\partial f(z)}{\partial z_1}\Bigr|_{z = z_0} (z_1 - a)+\frac{\partial f(z)}{\partial z_2}\Bigr|_{z=z_0}(z_2 - b)flin​(z)=f(z0​)+∂z1​∂f(z)​​z=z0​​(z1​−a)+∂z2​∂f(z)​​z=z0​​(z2​−b)

θo=0\theta_o = 0θo​=0, αo=0\alpha_o = 0αo​=0, θ˙o=0\dot{\theta}_o = 0θ˙o​=0, α˙o=0\dot{\alpha}_o = 0α˙o​=0, θ¨o=0\ddot{\theta}_o = 0θ¨o​=0, α¨o=0\ddot{\alpha}_o = 0α¨o​=0

(Jr+mpLr2)θ¨−12mpLpLrα¨=kVm−bθ˙−Brθ˙(10)(J_r + m_pL_r^2)\ddot{\theta} - \frac{1}{2}m_pL_pL_r\ddot{\alpha} = kV_m-b\dot{\theta} - B_r\dot{\theta} \quad\quad (10)(Jr​+mp​Lr2​)θ¨−21​mp​Lp​Lr​α¨=kVm​−bθ˙−Br​θ˙(10)

where, from Equation(9) the servo motor torque coefficient kkk is

k=ηgKgηmktRmk = \frac{\eta_gK_g\eta_mk_t}{R_m}k=Rm​ηg​Kg​ηm​kt​​

and the back emf coefficient bbb is

b=ηgKg2ηmktkmRmb = \frac{\eta_gK_g^2\eta_mk_tk_m}{R_m}b=Rm​ηg​Kg2​ηm​kt​km​​
−12mpLpLrθ¨+(Jp+14mpLp2)α¨−12mpLpgα=−Bα˙(11)-\frac{1}{2}m_pL_pL_r\ddot{\theta} + (J_p + \frac{1}{4}m_pL_p^2)\ddot{\alpha} - \frac{1}{2}m_pL_pg\alpha= -B\dot{\alpha} \quad \quad (11)−21​mp​Lp​Lr​θ¨+(Jp​+41​mp​Lp2​)α¨−21​mp​Lp​gα=−Bα˙(11)

where ggg is the acceleration due to gravity. Note that the negative sign of the gravity torque (α\alphaα term) in Equation 11 indicates negative stiffness with the pendulum in the vertically upright position.

[M][θ¨α¨]+[D][θ˙α˙]+[K][θα]=[k0]Vm(12)[M]\begin{bmatrix}\ddot{\theta} \\ \ddot{\alpha} \end{bmatrix} +[D] \begin{bmatrix}\dot{\theta} \\ \dot{\alpha} \end{bmatrix} + [K] \begin{bmatrix}\theta \\ \alpha \end{bmatrix} = \begin{bmatrix}k \\ 0 \end{bmatrix}V_m \quad \quad (12)[M][θ¨α¨​]+[D][θ˙α˙​]+[K][θα​]=[k0​]Vm​(12)

Where mass matrix MMM, damping matrix DDD, and the stiffness matrix KKK become

[M]=[(Jr+mpLr2)−12mpLpLr−12mpLpLr(Jp+14mpLp2)][M] = \begin{bmatrix} (J_r + m_pL_r^2) & -\frac{1}{2}m_pL_pL_r \\ -\frac{1}{2}m_pL_pL_r & (J_p + \frac{1}{4}m_pL_p^2) \end{bmatrix}[M]=[(Jr​+mp​Lr2​)−21​mp​Lp​Lr​​−21​mp​Lp​Lr​(Jp​+41​mp​Lp2​)​]
[D]=[b+Br00Bp][D] = \begin{bmatrix} b+B_r & 0 \\ 0 & B_p \end{bmatrix}[D]=[b+Br​0​0Bp​​]
[K]=[000−12mpLpg][K] = \begin{bmatrix} 0 & 0 \\ 0 & -\frac{1}{2}m_pL_pg \end{bmatrix}[K]=[00​0−21​mp​Lp​g​]

Defining the state vector xxx, output vector yyy and the control input uuu as

x=[θαθ˙α˙]T,y=[θα]T,u=Vmx = \begin{bmatrix} \theta & \alpha & \dot{\theta} & \dot{\alpha}\end{bmatrix}^T , \quad y = \begin{bmatrix} \theta & \alpha \end{bmatrix}^T, \quad u = V_mx=[θ​α​θ˙​α˙​]T,y=[θ​α​]T,u=Vm​
xË™=Ax+Bu(13)\dot{x} = Ax + Bu \quad \quad (13)xË™=Ax+Bu(13)
y=Cx+Du(14)y = Cx + Du \quad \quad (14)y=Cx+Du(14)

where the system state matrix AAA, control matrix BBB, output matrix CCCand the feedthrough of input to the output matrix DDD become

A=[O2×2I2×2−[M]−1[K]−[M]−1[D]]A = \begin{bmatrix} O_{2\times2} & I_{2\times2} \\ -[M]^{-1}[K] & -[M]^{-1}[D] \end{bmatrix} A=[O2×2​−[M]−1[K]​I2×2​−[M]−1[D]​]
B=[O2×1M−1[k0]]B = \begin{bmatrix} O_{2\times1} \\ M^{-1} \begin{bmatrix} k \\0 \end{bmatrix} \end{bmatrix}B=​O2×1​M−1[k0​]​​
C=[10000100]C = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \end{bmatrix}C=[10​01​00​00​]
D=[00]D = \begin{bmatrix} 0 \\ 0 \end{bmatrix}D=[00​]

In the equations above, note that O2×2O_{2\times2}O2×2​ is a 2×22 \times 22×2 matrix of zeros, O2×1O_{2\times1}O2×1​ is a 2×12 \times 12×1 matrix of zeros, and I2×2I_{2 \times 2} I2×2​ is a 2×22\times22×2 identity matrix. Using the parameters of the system listed in and the appendix , the linear model matricesAAA and BBB can be computed.

Open rotpen_week1_student.mlx live script and run the Modelling section. It will automatically load the parameters required for the state-space representation, and subsequently generate the A, B, C and D matrices required for the upcoming analysis. Please refer to and for additional information regarding the parameters. Note: The representative C and D matrices have already been included. The actuator dynamics have been added to convert your state-space matrices to be in terms of voltage. Recall that the input of the state-space model is the torque acting at the servo load gear (i.e. the pivot of the pendulum). However, we control the servo input voltage instead of control torque directly. The script uses the voltage torque relationship given in Equation 9 to transform torque to voltage.

The control design and time-response requirements are: Specification 1: Damping ratio: 0.6 < ζ\zetaζ < 0.8 Specification 2: Natural frequency: 3.5 rad/s < \omega_\rm{n} < 4.5 rad/s Specification 3: Maximum pendulum angle deflection: ∣α∣|\alpha|∣α∣ < 15 deg. Specification 4: Maximum control effort / voltage: |V_\rm{m}| < 10 V. The necessary closed-loop poles are found from specifications 1 and 2. The pendulum deflection and control effort requirements (i.e. specifications 3 and 4) are to be satisfied when the rotary arm is tracking a ±20\pm 20±20 degree angle square wave.

The poles are the roots of the system’s characteristic equation. From the state-space, the characteristic equation of the system can be found using \mathrm{det}(sI - A) = 0 \tag{15} where det()\mathrm{det}()det()is the determinant function, sss is the Laplace operator, and III the identity matrix. These are the eigenvalues of the state-space matrix AAA.

If the control input uuu of a system can take each state variable, xix_ixi​ where i=1...ni = 1 ... ni=1...n, from an initial state to a final state in finite time then the system is controllable, otherwise it is uncontrollable ([2]).

Define W = T\tilde{T}^{-1} \tag{21} where TTT is the controllability matrix defined in Equation 16 and\tilde{T} = \begin{bmatrix}\tilde{B} & \tilde{A}\tilde{B} & \cdots & \tilde{A}^{n-1}\tilde{B}\end{bmatrix} \tag{22} Then W^{-1}AW = \tilde{A} \tag{23} and W^{-1}B = \tilde{B} \tag{24}

If (A,B)(A,B)(A,B) are controllable, then pole placement can be used to design the controller. Given the control law u=−Kxu = -Kxu=−Kx, the state-space model of Equation (13) becomes \dot{x} = Ax + B(-Kx) \\=(A - BK)x \tag{25}

We can generalize the procedure to design a gain KKK for a controllable (A,B)(A,B)(A,B) system as follows:

Step 1 Find the companion matrices A~\tilde{A}A~ and B~\tilde{B}B~. Compute W=TT~−1W = T\tilde{T}^{-1}W=TT~−1. Step 2 Compute K~\tilde{K}K~ to assign the poles of A~−B~K~\tilde{A} - \tilde{B}\tilde{K}A~−B~K~ to the desired locations.

\tilde{A} - \tilde{B}\tilde{K}= \begin{bmatrix} 0 & 1 & \dots & 0& 0 \\ 0 & 0 & \dots & 0& 0 \\ \vdots & \vdots & \ddots & \vdots& \vdots \\ 0 & 0 & \dots & 0& 1 \\ -a_1-k_1 & -a_2-k_2 & \dots & -a_{n-1}-k_{n-1}& -a_n -k_n \end{bmatrix} \tag{26} Step 3 Find K=K~W−1K = \tilde{K}W^{-1}K=K~W−1 to get the feedback gain for the original system (A,B)(A,B)(A,B). Remark-1: It is important to do the K~→K\tilde{K} \rightarrow KK~→K conversion. Remember that (A,B)(A,B)(A,B) represents the actual system while the companion matrices A~\tilde{A}A~ and B~\tilde{B}B~ do not.

Remark-2: The entire control design procedure using the pole placement method can be simply done in MATLAB using the function called 'place' or 'acker'. For a selected desired set of closed loop poles DP, the full state feedback gain matrix KKK is obtained from

The rotary inverted pendulum system has four poles. As depicted in Figure 3, poles p1p_1p1​ and p2p_2p2​ are the complex conjugate dominant poles and are chosen to satisfy the natural frequency, ωn\omega_nωn​, and damping ratio, ζ\zetaζ, as given in . Let the conjugate poles bep_1 = -\sigma + j\omega_d \tag{27a} and p_2 = -\sigma - j\omega_d \tag{27b} where​ σ=ζωn\sigma = \zeta\omega_nσ=ζωn​ and ωd=ωn1−ζ2\omega_d = \omega_n \sqrt{1-\zeta^2}ωd​=ωn​1−ζ2​ is the damped natural frequency. The remaining closed-loop poles, p3p_3p3​ and p4p_4p4​, are placed along the real-axis to the left of the dominant poles, as shown in Figure 3.

The feedback control loop that balances the rotary pendulum is illustrated in Figure 4. The reference state is defined as x_d = [\theta_d \ 0\ 0\ 0]^ \intercal \tag{28}where θd\theta_dθd​ is the desired rotary arm angle. The controller is u = K(x_d - x) \tag{29}Note that if xd=0x_d = 0xd​=0 then u=−Kxu = -Kxu=−Kx, which is the control used in the pole-placement algorithm.

Select ζ\zetaζ and ωn\omega_nωn​ such that they satisfy the given design specifications.

Determine the locations of the two dominant poles p1p_1p1​ and p2p_2p2​ based on the specifications and enter their values in the MATLAB live script. Ensure that the other poles are placed at p3 = -30 and p4 = -40. Hint: Use Equation 27.

The s_rotpen_bal SIMULINK diagram shown in Figure 5 is used to simulate the closed-loop response of the Rotary Pendulum using the state-feedback control described in with the control gain K found above. The Signal Generator block generates a 0.1 Hz square wave (with an amplitude of 1). The Amplitude (deg) gain block is used to change the desired rotary arm position. The state-feedback gain K is set in the Control Gain gain block and is read from the MATLAB workspace. The SIMULINK State-Space block reads the A, B, C, and D state-space matrices that are loaded in the MATLAB workspace. The Find State X block contains high-pass filters to find the velocity of the rotary arm and pendulum.

Measure the pendulum deflection and voltage used. Are the given satisfied?

The linear state-space representation of the rotary inverted pendulum system, i.e., AAA, BBB, CCC and DDD matrices (numerical values).

Chosen ζ\zetaζ and ωn\omega_nωn​ based on design specifications.

Corresponding locations of the two dominant poles p1p_1p1​ and p2p_2p2​.

Gain vector KKK.

Plots of the commanded position of the rotary arm (θc\theta_cθc​), simulated responses of the rotary arm (θ\thetaθ), pendulum (α\alphaα), and motor input voltage (VmV_mVm​) obtained using your obtained gain K.

Are satisfied? Justify using the measured maximum pendulum deflection and motor input voltage values.

Based on your open-loop poles found in , is the system stable, marginally stable, or unstable? Did you expect the stability of the inverted pendulum to be as what was determined?

Determine the controllability matrix TTT of the system. Is the inverted pendulum system controllable? Hint: Use Equation 17.

Using the open-loop poles, find the characteristic equation of AAA. Hint: The roots of the characteristic equation are the open-­loop poles.

Instead of using det(sI−A)=0\mathrm{det}(sI - A) = 0 det(sI−A)=0, characteristic polynomials can also be found using MATLAB function .

Find the corresponding companion matrices A~\tilde{A}A~ and B~\tilde{B}B~ . Hint: For A~\tilde{A}A~ , use the characteristic equation of A found in and Equation 19. For B~\tilde{B}B~, use Equation 20.

Determine the controllability matrix T~\tilde{T}T~ of the companion system.

Determine the transformation matrix WWW.

Check if A~=W−1AW\tilde{A} = W^{-1}AWA~=W−1AW and B~=W−1B\tilde{B} = W^{-1}BB~=W−1B with the obtained matrices.

Using the locations of the two dominant poles, p1p_1p1​ and p2p_2p2​, based on the (), and the other poles at p3=−30p_3 = -30p3​=−30 and p4=−40p_4 = -40p4​=−40, determine the desired closed-loop characteristic equation. Hint: The roots of the closed-loop characteristic equation are the closed-­loop poles.

When applying the control u=−K~xu = -\tilde{K}xu=−K~x to the companion form, it changes (A~,B~)(\tilde{A}, \tilde{B})(A~,B~) to (A~−B~K~,B~)(\tilde{A}-\tilde{B}\tilde{K}, \tilde{B})(A~−B~K~,B~). Find the gain K~\tilde{K}K~ that assigns the poles to their new desired location. Hint: Use Equation 26 and find the corresponding characteristic equation. Compare this equation with the desired closed-loop characteristic equation found in to determine the gain vector K~\tilde{K}K~.

Once you have found K~\tilde{K}K~, find KKK using .

Compare the gain vector KKKcalculated using Pole Placement Theory () with the gain vector KKK obtained using MATLAB ().

kg\mathrm{kg}kg
m\rm{m}m
m\rm{m}m
kgâ‹…m2\rm{kg \cdot m^2}kgâ‹…m2
Nâ‹…mâ‹…s/rad\rm{N\cdot m\cdot s/rad}Nâ‹…mâ‹…s/rad
kg\rm{kg}kg
m\rm{m}m
m\rm{m}m
kgâ‹…m2\rm{kg \cdot m^2}kgâ‹…m2
Nâ‹…mâ‹…s/rad\rm{N\cdot m\cdot s/rad}Nâ‹…mâ‹…s/rad
kgâ‹…m2\rm{kg\cdot m^2}kgâ‹…m2
counts/rev\rm{counts/rev}counts/rev
VnomV_{\rm{nom}}Vnom​
Table A
Table A
Table 2
Table A
Balance Control
specifications
Design Specifications 3 and 4
Result A.2
Table 2
Table A
Specification
poly
Question B.3
specifications
Result B.1
Question B.5
Step 3 in Pole Placement Theory
Question B.1.10
Result B.1.3
m_\rm{p}
L_\rm{p}
l_\rm{p}
J_\rm{p,cm}
B_\rm{p}
m_\rm{arm}
L_\rm{r}
l_\rm{arm}
J_\rm{arm,cm}
B_\rm{r}
J_\rm{arm}
K_\rm{enc}
R_\rm{m}
L_\rm{m}
k_\rm{t}
k_\rm{m}
K_\rm{g}
\eta_\rm{m}
\eta_\rm{g}
J_\rm{m}
J_\rm{eq}
B_\rm{eq}
m_\rm{b}
L_\rm{b}
m_\rm{d}
r_\rm{d}
m_\rm{max}
f_\rm{max}
l_\rm{max}
\omega_\rm{max}
19KB
Rotary Inverted Pendulum Week 1.zip
archive
Figure 1 Rotary servo base with Rotary Pendulum module
Figure 2 Rotary inverted pendulum conventions
Figure 3 Desired closed-loop pole locations
Figure 4 State-feedback control loop
Figure 5: s_rotpen_bal SIMULINK diagram used to simulate the state-feedback control
Figure 6: Balance control simulation