Advanced Control for Robotics

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

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 pole attached to it. The cart can only move in one axis and the pole can freely rotate around a pivot point. The objective of this problem is 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 pole. 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 the variables of the system of the system (states), multiply by some gains and adds all the response together. This process will try to drive 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 the gains. 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 Inverted Pendulum. 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 pole based on the Quanser experiment <name> 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 Panel

To better analyze this system, follow these steps:

1. Run the VI by pressing CTRL-R and verify that the cart moves to desired position.
2. On the tab control, select the “Controller design”. In there you can see open loop poles of the system, the desired closed loop poles 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 to achieve the expected result.
3. Select the Controller tab and try to change the setpoint. The cart should move to expected position. Change to other positions and analyze the chart.
4. Go to the block diagram using CTRL-E and you should see the code in Figure 2. You can divide it into two parts:
   1. The control design process: This is responsible to create the gains used to control the plant. You can see this in Figure 4.
   2. The implementation of the control algorithm: Using the Control and Simulation Loop, the plant and controller are implemented using the function in the Simulation palette.

Figure 4 - Block diagram of inverted pendulum

5. The code outside the Control and Simulation loop is used to design the gain used in the controller. Due to dataflow, this code is executed only one time and the result matrices are using during the loop execution.
6. The process to design a controller is:
   1. Create a linear model of the inverted pendulum and generate the state-space model of the system. This model will be used to analyze and design the controller. This operation is provided by Construct Space-State Model. The function Poles.vi shows 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 Pendulum

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

Figure 6. Feedback Control Code

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, these systems are called stochastic systems. Unfortunately, if you try to apply any classic control techniques to stochastic systems that have high content of noise, the controller won’t be able to track any signal since it thinks that the actual response is fast and it will try 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 estimate the states based on the noise measurements of the system. Using the Kalman filter, the states of the system are estimated with more 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 will focus on the Extended Kalman Filter (EKF), which is designed to estimate the states of variables that are nonlinear.

Using the Extended Kalman Filter

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

Figure 7. Kalman Filter Example

This problem is defined as a car that moves according to some trajectory and it has to track the position of a specific landmark while the system has a high amount of noise. For this problem, we defined the reference of the Cartesian system with the Y-axis point 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 the distance from the tip and its angle. The picture shows how the coordinate system is defined.

Figure 8. Car Frame of Reference

Based 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 and its response is measured with sensors. Noise is introduced in the measurement system and its behavior. The input and output signals of the stochastic system (system with noise) is apllied to the Extended Kalman Filter, which will try to estimate the states of the 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 the trajectory obtained by the landmark in reference to the inertial reference in the car and without noise; in red is the measurement of distance and angle of the landmark with present of noise and in green is the estimated values from the Extended Kalman Filter. As you can see, in the first instance, the trajectory is further away from the true trajectory. As time goes by, the EKF updates its internal 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 initial states. Find the “Use Initial Measurement” under “Initial States” and change it to true and press the “Restart” button. You will notice that 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 position related to the car, as it moves.

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 to simulate this system. The trajectory to the system is a sinewave and the system is simulated with the Continuous Nonlinear Noisy Plant.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. In LabVIEW Control and Simulation, the model is defined by VI with specific connector pane and call by the functions by a static VI reference <add link>. 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

Figure 12. 

8.     The model is defined as

Figure 13. Model Definition

For advance users, the LabVIEW user interface allow you to evaluate the covariance matrix as it changes over time.

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 Logic Toolkit 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 the control 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.vi

In this example, the control problem is defined as how to guide a car parking it backwards to a garage. For this example, do the following 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 the garage. 

Image 14. Fuzzy Logic Example

 

2.    Go to the block diagram and analize how the system works. Notice that you have two Fuzzy Controllers for the forward and backward movement. 

Image 15. Fuzzy Logic Example Block Diagram

3.    Go to tools>>Control and Simulation and select Fuzzy Logic Designer. This will show the design tool where rules and sets are defined.

 

Image 16. Fuzzy Logic Designer

Conclusion

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

 

 

 

Reader Comments | Submit a comment »

You would need the following LabVIEW Control Design and Simulation Module LabVIEW PID & Fuzzy Logic Toolkit
- Andy Chang, National Instruments. andy.chang@ni.com - Apr 26, 2010

Add-on toolkit Needed
The simulation vi said it needed an add- on toolkit that was not installed...what toolkits are needed?
- Joe Varnell, Lockheed Martin MFC-D. joe.varnell@lmco.com - Feb 10, 2010