Prior efforts to create real-time control software for the PUMA robots
have been limited to DOS or using third-party software, such as Control
Shell, in a non-real-time environment. These solutions, while viable,
have been constrained and sometimes unmanageable. This project is an
effort to show the feasibility of creating real-time control systems
using Mathworks' Simulink, a widely available dynamic system simulator,
a non-commercial version of QNX, one of the leading real-time operating
systems in both academics and industry.|
This project will demonstrate the flexibility of using Simulink, along with Mathworks Real-Time Workshop, to create models of control systems that can be easily created and debugged. Additionally, the target system can interface with the PUMA using either a Trident Robotics or Servo-To-Go card.
The benchmark for success of this project will be to create a model in Simulink, generate code, and compile cleanly on a QNX target machine. The model will specify a displacement for all six joints of the PUMA robot and command the robot to follow a smooth trajectory. This will be done for systems employing both a Servo-To-Go and Trident Robotics interface board.
In order to use Matlab and Simulink to create models which can be used
to generate code for a QNX target system, we need to install additional
files to the development system, which is assumed to be Windows-based. We need to add several things to this
system to make it generate compilable code for our target system. The table below has some compressed tar files which
are specific to the version of Matlab you will be using. The files include the Simulink libraries, target makefiles
and other configuration files. All of these files should be installed at the top level of Matlab or MATLAB_ROOT.
If you are using the version for Matlab6, you will need to add MATLAB_ROOT/toolbox/pumastg to the search path. This can be accomplished using Matlab’s addpath command or from Path under the File menu. If the Simulink Library Browser is now opened, we should be able to see PUMA w/STG Board Library.
Once these modifications have been made, we can now build a model.
Simulink provides a number of generic building blocks that can be used
to build very simple models. Let’s start with something simple by
connecting a Sine Wave to a Scope. The Sine Wave block can be found in
the Sources Library and the Scope can be found in the Sinks Library.
The model should look like the model to the right.|
After running the model and double-clicking on the scope, you should see the plot to the right.
If we eventually want to be able to dump the output to a file, we can add an output block to our model as seen in the figure at left.
Once we have built a model, we need to make a few more modifications
before we can generate code. From the Tools menu, select Real-Time
Workshop->Options. If you hit the Browse... button, the
following window will appear:|
To generate code, make sure to select the QNX Neutrino Real-Time Target and click OK. Go to the Solver tab, make sure to select Fixed-Step under Solver Options and make sure that the Fixed Step Size is the same as the smallest task time of any of the blocks in your model. Now, go back to the Real-Time Workshop tab and click on the Build button. The code will be written to a directory with the name model_name_qnx_rtw. Copy the entire directory to your QNX target. Note that if your development system is a Windows environment, you may need to use a utility such as dos2unix to strip your files of extra carriage returns.
Provided that everything is in its proper place, we should be able to
invoke the make command. Note that the automatically generated makefile
file is named model_name.mk. The compiled binary will be
placed in parent
directory and is invoked as make model_name.mk. The flag -tf T will run
the model to
final time T, or infinity if T is inf. If
the model runs properly, any
output from output blocks as well as the time steps will be placed in a
Matlab file called model_name.mat.|
With the blocks shown from the library, it should be very easy to create a model which has some degree of control over the PUMA robot. The model below will smoothly move the end manipulator to a specified position.
In this model, the three blocks on the left hand side are run before anything else. Board Initialization and Power On would generally be required of any model using a board interface. For this specific model, the Zero Encoders block is also included because a calibration block has yet to be written. The model gets the desired end position, which is a vector of radians, from the Constant block. This is fed into the Trajectory Generation block, which generates a third-order polynomial trajectory and outputs a point from that curve at every time step. Since the Trajectory Generation block and PD Controller run at different frequencies, a Transition block is needed. Transition blocks are needed everywhere to connect two blocks with different sample times. The PD Controller sends the torques to a Float Write block, which sends them to the PUMA controller via the interface board, and to a Log File block, which writes the output to a user-specified text file.
The PD Controller also gets input from a Float Read block, which reads the encoder positions of the PUMA. Finally, the current position of the PUMA and the current trajectory position are sent to another Log File block, which can then be used for analysis.
Note that the model has several different colors for different blocks and connecting lines. These different colors denote different sample times. In this model, the colors correspond to these times:
This model was built, the resulting code and other files transferred to a QNX system, and then cleanly compiled. The QNX system was a 433MHz PentiumIII processor running QNX6.3NC. This system also had a Servo-To-Go rev. XX board which was connected to a Unimate II controller which was directly connected to a PUMA 560.|
The resulting executable assumes the PUMA is in the up position. This corresponds to a triple of [0 pi/2 -pi/2] for the joint angles for the first three joints. Obviously, having to set the robot to a set position before starting is a shortcoming, but the addition of a few key components, described later in the Future Work section, would easily overcome this requirement.
With the arm in the up position, the position vector input to the model was [1.75 -pi/2 pi/2] and the model ran for ten seconds. The first joint was set to 1.75 to ensure that joint 1 was moving just slightly beyond the pi/2 position. In addition, the average velocity for the Trajectory Generation block was set to 0.25 radians/sec. Values for the PD Controller were set according to the standard for a PUMA560 arm with a 1kHz controller frequency. The following graphs are the data from the Log blocks of the previous model.
Both the desired and actual joint positions for the first three joints are shown in the graph to the left. The generated trajectory is shown in blue while the actual positions are shown in green. The controller follows the generated trajectory so well that it is barely possible to notice the two colors. The below graph shows a close-up of the graph for joint 1 as the robot nears its final position. One can see that the difference between desired and actual is less than 0.01 radians.
Note first that the joints all reach their final positions at the same time. Also, they all finish at 7 seconds, which is is expected from:
time_final = max_angle / avg_speed = 1.75 rads / 0.25 rads/sec = 7 secs
The controller continues to run until the default time of 10 seconds is reached. A simple modification to quit when reaching the desired position is discussed in the Future Work section.
Note that although the torque profiles are noisy the positions were clearly not affected.
I have developed and demonstrated that it is possible and easy to use Mathworks Simulink to create model based control software for robotic systems. Additionaly, these models can be used with Mathworks Real-Time Workshop to produce code the, with some slight modifications, can be compiled and run on a real-time platform. In this case, the chosed platform was QNX although this could easily be extended to anything from RTLinux or VxWorks.|
Using the developed models, I was able to create a position controller for a PUMA560 robot and ran the resulting executable on a QNX platform. From the shown graphs it was demonstrated that robot performed as expected.