1/7 www.ni.com

1. 2. 3. 4.

Advanced Control for RoboticsPublish Date: Apr 24, 2012

Overview

This article describes how to use the LabVIEW Control Design and Simulation (LV CD&SIM) and PID and Fuzzy Logic Toolkit (LVPID) to analyze and implement controller for your robotic application. Those products are part of the LabVIEW Robotics and itcomplements and enhance the user experience when developing robotics application such as position and velocity control ofmotors in manipulators, path following and trajectory control, force feedback control, and several others. This document willdescribe three common tasks used in robotics and by using existent examples in those products, we will highlight how some of itsfeatures and capabilities. This document is not intended to describe each problem in details, but only pose the control problem andsolve using the tools available. For further more information on each problem, consult the references in the end of each portion.

Table of Contents

State Feedback Control of linear inverted pendulumLocalization with Extended Kalman FilterParking a Car with Fuzzy LogicConclusion

1. State Feedback Control of linear inverted pendulum

Introduction

The inverted pendulum is one of the most known plants used in advanced control. The problem consists of a cart with a poleattached to it. The cart can only move in one axis and the pole can freely rotate around a pivot point. The objective of this problemis to develop a controller that moves the cart and always keep the cart to a specific position.

Figure 1 - Inverted Pendulum problem Analyzing the system you can identify the following variables:X Absolute position of the cart (m)Q Angle of the pole related to the center line (rad)F Force applied to the cart (N)M Mass of cart (kg) If you consider the cart without frictions and linearize its nonlinear equations around the Q=0, you can write in state-space format,defined as:

If you substitute the variables for a real plant, you should have the following equation:

Where the states of the [x(t)] are the position (x), velocity (dx) of the cart and the angle (theta) and angular velocity (dtheta) of the

2/7 www.ni.com

1. 2.

3.

4. 1.

2.

Where the states of the [x(t)] are the position (x), velocity (dx) of the cart and the angle (theta) and angular velocity (dtheta) of thepole. The input of the system [u(t)] is force applied to the cart and the output of the system [y(t)] is the position (x) and the angle(theta).One of the techniques used to control solve the control problem is called state-feedback control. It consists of measuring all thevariables of the system of the system (states), multiply by some gains and adds all the response together. This process will try todrive all the states to zero, which in our case, is the pole angle.The challenge is how to design all the gain that will drive all the states to zero. There are several techniques used to design thegains. One of those is called Pole Placement, where the we use the Lyapunov equation to evaluate the gains.Using the linear pendulum

Lets use the LabVIEW Control Design and Simulation Module shipping example to demonstrate the operation of InvertedPendulum. To open the example, go to Find Example in Help and navigate to: Robotics>> Control and Simulation and select

Linear Inverted Pendulum Simulation.vi

Figure 2. NI Example finder This will open the example on your screen as show in the figure 2. In the Front Panel, you can see 3-D image of the cart and polebased on the Quanser experiment using the 3D Picture control with VRML import and in the graphs, the cart position (X),angle from center (Theta) and desired cart position (SP X).

Figure 3 - Inverted Pendulum Front PanelTo better analyze this system, follow these steps:

Run the VI by pressing CTRL-R and verify that the cart moves to desired position.On the tab control, select the Controller design. In there you can see open loop poles of the system, the desired closed looppoles and the resulting operation from the poleplace design. The poles indicates how fast the system responds to disturbance,which negative values indicate faster responses. If the desired poles and the calculated poles are further away from each other,this means that the algorithm cannot calculate the gains for this specific position and you have to change the poles location toachieve the expected result.Select the Controller tab and try to change the setpoint. The cart should move to expected position. Change to other positionsand analyze the chart.Go to the block diagram using CTRL-E and you should see the code in Figure 2. You can divide it into two parts:

The control design process: This is responsible to create the gains used to control the plant. You can see this in Figure 4.

The implementation of the control algorithm: Using the Control and Simulation Loop, the plant and controller are

3/7 www.ni.com

4.

2.

1.

2. 1.

1.

2.

3.

4. 5.

The implementation of the control algorithm: Using the Control and Simulation Loop, the plant and controller areimplemented using the function in the Simulation palette.

Figure 4 - Block diagram of inverted pendulumThe code outside the Control and Simulation loop is used to design the gain used in the controller. Due to dataflow, this code isexecuted only one time and the result matrices are using during the loop execution.The process to design a controller is:

Create a linear model of the inverted pendulum and generate the state-space model of the system. This model will be usedto analyze and design the controller. This operation is provided by . The function Poles.viConstruct Space-State Modelshows the poles location of the open loop system and the function CD Pole Placement.vi will calculate the control gain.

Figure 5 - Control Design of Inverted PendulumThe gain is then transferred inside the control and simulation loop where the controller is implemented. The input of thecontroller is the X, Theta and the SP X and the output is the control effort applied to the system.Since we do not measure the velocities of X and Theta, the derivative block is used to estimate the system velocities,composing the controller.Since this controller is not designed to drive the states to zero, since we need to drive the cart to a specific position, we justneed to remove the setpoint, which was accomplied by Junction block.To smooth the set point change, a transfer function with time constant 0.2 s (1/5). In the end, the final controller uses the function from State-Feedback Control to implement the control effort that will beapplied to the plant.

Figure 6. Feedback Control Code2. Localization with Extended Kalman Filter

Introduction

When analyzing robot system, noise can be present inside the system or in its measurements. In the control domain, thesesystems are called stochastic systems. Unfortunately, if you try to apply any classic control techniques to stochastic systems thathave high content of noise, the controller wont be able to track any signal since it thinks that the actual response is fast and it willtry to overcorrect the system.One technique used to avoid this issue was developed by Rudolph E. Kalman in 1960 where he proposes a method to estimatethe states based on the noise measurements of the system. Using the Kalman filter, the states of the system are estimated withmore precision and allow the controller to act as expected.There are several types of Kalman Filters in the literature that can be used depending of the application. For this document, we willfocus on the Extended Kalman Filter (EKF), which is designed to estimate the states of variables that are nonlinear.

4/7 www.ni.com

Using the Extended Kalman Filter

To illustrate the use of the Extended Kalman Filter, open the example in Robotics>>Control and Simulation>>Localization withExtended Kalman Filter. You should see the Figure 7

Figure 7. Kalman Filter ExampleThis problem is defined as a car that moves according to some trajectory and it has to track the position of a specific landmarkwhile the system has a high amount of noise. For this problem, we defined the reference of the Cartesian system with the Y-axispoint outward from the tip of the center of the car and the X-axis is tangent to right to this point. The landmark is tracked by thedistance from the tip and its angle. The picture shows how the coordinate system is defined.

Figure 8. Car Frame of ReferenceBased on the description, do the following steps:1. Select the Configuration tab and analyze its control diagram in the front panel. An arbitrary signal is applied to the system andits response is measured with sensors. Noise is introduced in the measurement system and its behavior. The input and outputsignals of the stochastic system (system with noise) is apllied to the Extended Kalman Filter, which will try to estimate the states ofthe system.2. Run the VI using CTRL-R and verify the Trajectories chart. You will see three signals been traced: in white you can see thetrajectory obtained by the landmark in reference to the inertial reference in the car and without noise; in red is the measurement ofdistance and angle of the landmark with present of noise and in green is the estimated values from the Extended Kalman Filter. Asyou can see, in the first instance, the trajectory is further away from the true trajectory. As time goes by, the EKF updates itsinternal states and after a while, he is able to track the expected trajectory, even with a large content of noise.3. To avoid this large difference, instead of arbitrarily define a state, you can define the first noise measurement as the initialstates. Find the Use Initial Measurement under Initial States and change it to true and press the Restart button. You will noticethat the next time the simulation repeats, the trajectories will be the same.4. Select the Tracking tab and verify how the landmark changes over time. The localization is based on its X and Y positionrelated to the car, as it moves.

5/7 www.ni.com

Figure 9. 5. Stop the VI with button Stop and go to the block diagram with CTRL-E. We are using the Control and Simulation Loop tosimulate this system. The trajectory to the system is a sinewave and the system is simulated with the Continuous Nonlinear NoisyPlant.vi. The Extended Kalman Filter is used to estimate the states and LabVIEW user interface is used to show this relationship.

Figure 10. Kalman Filter Example Code

6. Scrolling to the left, you will see model definition of the system based on the nonlinear model and the noise characteristics. InLabVIEW Control and Simulation, the model is defined by VI with specific connector pane and call by the functions by a static VIreference . This works as a call back function to the example and this will compute its states based on the model.

Figure 11.7. To access this VI, double-click on the Localization Autonomous Robot.vi. This should load the VI shown in figure 12

6/7 www.ni.com

Figure 12. 8. The model is defined as

Figure 13. Model DefinitionFor advance users, the LabVIEW user interface allow you to evaluate the covariance matrix as it changes over time.3. Parking a Car with Fuzzy Logic

Introduction

Another technique used to control system in robotics is Fuzzy Logic. This toolkit is part of the LabVIEW PID and Fuzzy LogicToolkit and this allow you to define and control a system based on rules and variables loosely undefined.The interesting aspect of this technique is that you do not need to define an accurate model of the system and you can define thecontrol law purely based on users experience and heuristics. To demonstrate its use, let open the example:C:\Program Files\National Instruments\LabVIEW 2009\examples\control\fuzzy\Car Parking\FuzzyEx Car Backward Parking.viIn this example, the control problem is defined as how to guide a car parking it backwards to a garage. For this example, do thefollowing steps:1. Run the example and try to move the green cursor in any position in the graph, and the car should change its initial position.After that, switch the Operation Mode to Park with Fuzzy control. This should move car back and forth until it parks into thegarage.

Image 14. Fuzzy Logic Example

7/7 www.ni.com

Image 14. Fuzzy Logic Example 2. Go to the block diagram and analyze how the system works. Notice that you have two Fuzzy Controllers for the forward andbackward movement.

Image 15. Fuzzy Logic Example Block Diagram3. Go to tools>>Control and Simulation and select Fuzzy Logic Designer. This will show the design tool where rules and sets aredefined.

Image 16. Fuzzy Logic Designer4. Conclusion

This article demonstrated how you can fully use the tools for Control and Simulation to solve more advanced control problems forautonomous robots and mechatronics systems. Although its use require some specific knowledge, LabVIEW graphical interfaceallow you to easily understand how it is used and learn. Also, by using deferent models of computation, you can easily create anddescribe the overall system, from design to implementation.