ECE532 DIGITAL SYSTEM DESIGN - University of Torontopc/courses/432/2014/projects/6... ·...
Transcript of ECE532 DIGITAL SYSTEM DESIGN - University of Torontopc/courses/432/2014/projects/6... ·...
UNIVERSITY OF TORONTO
ECE532 - DIGITAL SYSTEM DESIGN
ROBOTIC ARM CONTROLLED BY VIDEO INPUT
FINAL DESIGN REPORT
Izaz Ullah
Willian Rigon Silva
Yuka Kyushima Solano
April 9th, 2014
1
1
Table of Contents
1 Overview ..................................................................................................................................... 3
1.1 Background and Motivation ................................................................................................ 3
1.2 Goals .................................................................................................................................... 4
1.3 Design Changes ................................................................................................................... 6
1.3.1 Servo motor has serial/UART communication protocol ............................................. 6
1.3.2 MATLAB as interface between the FPGA and robotic arm ......................................... 6
1.3.3 No Depth Motion Recognition .................................................................................... 6
2 Outcome ...................................................................................................................................... 7
2.1 Results ................................................................................................................................. 7
2.2 Improvements Suggestion .................................................................................................. 9
3 Description of the blocks ........................................................................................................... 10
3.1 VmodCam IPcore ............................................................................................................... 10
3.2 HDMI_Out IPcore .............................................................................................................. 10
3.3 Color and Position Recognition Algorithm ........................................................................ 11
3.3.1 Software Implementation ......................................................................................... 11
3.3.2 Hardware Implementation ........................................................................................ 13
3.3.3 MATLAB Interface ..................................................................................................... 18
4 Description of Design Tree ........................................................................................................ 19
5 Project Schedule ........................................................................................................................ 20
6 References ................................................................................................................................. 22
2
2
Table of Figures
Figure 1 – Overall view of the design .................................................................................................. 4
Figure 2 - AX-12A robotic arm, FPGA (Atlys Spartan 6), and VmodCAM ............................................ 5
Figure 3 – First full system`s test ......................................................................................................... 8
Figure 4 - Ideal block diagram of the system – FSL not implemented in actual system ..................... 9
Figure 5 – Ideal design of the Color and Position Recognition state machine .................................. 12
Figure 7 - - State Machine for reading from the DDR memory ......................................................... 15
Figure 6 - State machine for reading from the FIFO and recognizing the pixel color ....................... 15
Figure 8 – Start of SEARCH_POSITIONS_PROC process .................................................................... 16
Figure 9 - End of the loop of SEARCH_POSITIONS_PROC process .................................................... 17
3
3
1 OVERVIEW
1.1 BACKGROUND AND MOTIVATION
Currently, robots are being used to execute a great deal of tasks. In the manufacture
industry, the tasks executed by robots are repetitive and pre-programmed, so the robot work
on their own. Their movements are calculated based on a mathematical model of the robot,
using Lagrangian Mechanics, and afterwards calculating the motion plan using Forward
and Inverse Kinematics. Moreover, to make sure that the joint angles will be precisely the
ones calculated by the Forward and Inverse Kinematics, it is also required a control
feedback loop. This approach requires a deep understanding and study of the task that the
robot shall execute in a specific environment.
On another hand, in the medical and defense industry robots are required to perform
precise and dangerous tasks – a surgery or a bomb disarming, for instance. In this life
treating tasks, a person usually controls the robot directly. Therefore, a high precision and
reliable human-machine interface is required; however, these interfaces are expensive and
this is our main project motivation.
The main goal of our project is to develop a cheap human-machine interface to
control a robotic arm using the human body movements. A FPGA – Digilent Atlys Spartan
6 [1] as the video input/processing and control signal generation, and a robotic arm,
composes the system (figure 2). The robotic arm used was an AX-12A - USB [2]. This type
of approach permits to extend the project to a more complex robot controlled by the same
interface.
The differences between this project and existing ones are the low cost to assembly
all the system and the lack of physical connection between the user and the robot (no wires
or sensors, just video input). Another advantage is the portability of this low cost
methodology to control any kind of robotic arm, or even expand the system to allow control
of an entire robot using all the body movements (or a not humanoid type of robot). The
main market of this application could be the medical area – actual surgery robots are
expensive and this type of system can assist in specific situations, without any physical
contact between the doctor and the machine – or in the defense area – used for bomb
disarming or remotely controlling a robot to access dangerous places.
4
4
After the technology is full developed and accessible to the public, hence, if it is
cheap, this human-machine interface can be applied to a large variety of applications.
1.2 GOALS
The goal of our project is to develop a human-machine interface, using video input,
to control a robotic arm. The main idea was to attach coloured points in the operator`s
hand/arm, a stereo camera [3] captures the image of the hand/arm, afterward an IPCore
inside the FPGA will recognize the colour and relative position of each coloured point and
convert this to a control signal to the robotic arm in real-time and with good precision.
The control chain works as follows, for the video input it was used a VmodCam
connected directly to the FPGA, a custom IPcore process the image and generates the
control signals for the robotic arm servo motors, that will change the robotic arm joint
angles (figure 1).
Figure 1 – Overall view of the design
In order to control the robotic arm`s rotation we thought to use a stereo (3D) camera
to compute also the motion of the operator`s hand/arm in depth (see next section). Since the
majority of servo motors uses PWM signal as control input, we originally tought in using
the FPGA`s internal clock to generate this PWM and put it in a GPIO I/O port (Pmod
conector). Alternatively, as a plan B, we thought to add an AVR ATMega microcontroller
5
5
as interface between the FPGA and the robotic arm via UART/serial communication;
however, this was not necessary and will be discussed in the next section.
The used IPcores were:
VmodCam IPcore (from previous group) – This IPcore manages the video input
from the stereo camera and stores in the DDR memory. It was necessary little
modifications to adapt to our project.
HDMI_Out IPcore (from previous group) – We used this IPcore mostly for
debbuging and evaluation that our Color and Position Recognition algorithm works.
The main purpose is to show on a HDMI monitor what the camera is capturing at
that moment.
Color and Position Recognition IPcore (designed by our group) – This custom
IPcore implements our state machine for recognizing and calculating the X,Y
position on the screen of each coloured point on the operator`s arm. These positions
are returned to the MicroBlaze processor, through a Faster Simplex Link (FSL), to
compute the control signals to the robotic arm.
See section 2.1 for block diagram and section 3 for detailed description of each IPcore.
Figure 2 - AX-12A robotic arm, FPGA (Atlys Spartan 6), and VmodCAM
6
6
1.3 DESIGN CHANGES
After the initial design, we had to make some changes in order to advance with the
project. The changes were:
1.3.1 SERVO MOTOR HAS SERIAL/UART COMMUNICATION PROTOCOL
When we began testing all communications between the hardware parts, we
discovered that the servo motors of the AX-12A robotic arm are a modern type that
does use a serial/UART communication protocol [4] instead of PWM signal as
control input. However, this had an advantage and a drawback at the same time. The
advantage was that we did not had to be concerned about the generation of the PWM
signal and its output in the FPGA board (or use the AVR ATMega microcontroller as
interface). The drawback is that the serial communication protocol of the servo
motors are complex and with few documentation, hence we spent a good time
learning how to properly send the correct commands.
1.3.2 MATLAB AS INTERFACE BETWEEN THE FPGA AND ROBOTIC ARM
With our color and position recognition algorithm fully operational on software,
we tried to do a full system`s test; however, we found difficulties communicating the
FPGA with the robotic arm through the Serial/UART communication (as stated in the
previous section). We evaluated that to make it properly work we had 2 options, first
one was to add more hardware to handle the USB/UART communication, or the
second one was to use the MATLAB software, running in a PC/MAC, as interface.
Since we had a deadline for the project, and with Prof. Paul Chow`s advice, we opted
to use the MATLAB as communication interface between the FPGA and the robotic
arm (see section 3.3.3 for detailed info.).
1.3.3 NO DEPTH MOTION RECOGNITION
After we did the first software test with our custom color and position
recognition algorithm, we evaluated the difficulty to implement the depth recognition
algorithm using input from the 2 cameras of the VmodCam board. Our conclusion
was that this algorithm will not be trivial and will have some intricacies. Therefore,
after asking advice to Prof. Paul Chow, we decided to not implement this algorithm
7
7
because the limited time, also because another group was developing this as a custom
IPCore. For future expansion of this project, the integration of the depth recognition
IPCore to our project could be made.
2 OUTCOME
2.1 RESULTS
We achieved our main goals that was to create a human-machine interface system
where you can control a robotic arm from video input (figure 3); however, our color and
position recognition algorithm only fully worked on software, hence the system`s
performance was not real-time. We created a custom IPcore that implements our Color and
Position Recognition algorithm as a state-machine (see section 3 for detailed info.), but our
IPcore was not working properly (table 1); the performance it is approximately real-time,
but only the color recognition is working properly (the position recognition its returning
random values for the X,Y positions). Our custom IPCore need improvements, due to the
short time we did not discovered how to solve the position recognition error in our custom
IPcore. Moreover, we did not had time to implement the Faster Simplex Link (FSL)
between our IPcore and the MicroBlaze processor (figure 4).
In summary, we designed the algorithm that does the color and position recognition,
it is tested in simulation, it is fully working on software, however it is partially working on
hardware (IPcore). In order to make our custom IPcore fully operational it is necessary
more testing and debugging to find what is wrong with the position recognition (as stated
before, the problem is that it is returning random values for the X,Y position of the
coloured point).
Table 1 - Project goals status
Goals Status
Video input IPcore Working
HDMI Out IPcore Working
FPGA – AX-12A communication Working (through MATLAB)
Depth Recognition using Stereo camera Changed
Color and Position Recognition custom IPcore Created (needs more testing)
8
8
The Color and Position Recognition algorithm works as follows:
First it recognizes the color of each point on the operator`s arm/hand (based
on configurable color threshold), and writes the image back in the DDR
memory identifying the points were the colours were been recognized by the
algorithm (we write back in the DDR just for debugging).
Secondly it computes the (X,Y) position of each coloured point on the
operator`s arm/hand.
Finally, will compute the distance between each coloured point and generates
a control signal to a specific joint in the robotic arm (based on a maximum
and minimum servo input mapping).
Figure 3 is showing the HDMI output of the Color and Position Recognition
algorithm; note the 2 black squares and the calculated distance (black line) from the center
of each square. This distance is translated as a control signal to the robotic arm claw (end-
effector)
Figure 3 – First full system`s test
9
9
Figure 4 - Ideal block diagram of the system – FSL not implemented in actual system
In conclusion, our custom IPcore recognizes the colour of each point and re-write
the image back into the DDR memory; afterward the HDMI_Out IPcore reads from that
address and shows in a HDMI monitor. Note that this operation is done only for debbuging,
for fast system response this function (re-write back in the DDR) and the HDMI_Out
IPcore could be excluded from the system.
2.2 IMPROVEMENTS SUGGESTION
The first improvement that should be made is discovering what is wrong with the
position recognition part in our custom IPcore; the second main improvement should be the
communication between the custom IPcore and the MicroBlaze processor through a Faster
Simplex Link (FSL). These two improvements shall enable the real-time control of the
robotic arm. After the real-time control is guaranteed, the next improvement could be the
precision optimization of the robotic arm movement based on the operator`s arm/hand
movement. This could be done by changing the function that relates the servo limits with
the computed distance of the coloured points; currently this curve has a proportional
10
10
relation, but for instance, could be changed to an exponential curve. However, have in mind
that the precision also depends on the actuator, in this case is the AX-12 servo motor.
Afterwards, when you have both real-time control and precision of the movement, the
next step is to integrate a depth recognition IPCore. Integrating it to the system will give 3D
control over the robotic arm; this human-machine control interface could be applied to any
other kind of system. In order to exclude the MATLAB interface from the system you need
a USB to TTL converter (for instance - CP2102 [5]).
The variety of applications of this human-machine interface is virtually infinite, and
this approach is relatively simple and cheap compared to existing ones.
3 DESCRIPTION OF THE BLOCKS
In this section are descibred details of each IPcore used in this project.
3.1 VMODCAM IPCORE
This IPcore, designed by our classmates, receives the streams from the VmodCam
board and stores it in the DDR memory. Since this is a stereo camera, the stream from one
camera are stored at the memory address 0xA0000000 and the other at 0xA0100000. The
image resolution is 640x480 and the pixel size is 2bytes (RGB565); therefore, the memory
size needed for each camera is 640x480x2 = 614400 bytes (0x96000).
3.2 HDMI_OUT IPCORE
The HDMI_Out IPcore was designed by Ana Klimovic, Bryce Long and Victor
Zhang, 2013 ECE532 students. This IPcore reads an image stored in the DDR and displays
it at a monitor via a HDMI cable. In the XPS we can define the resolution of the image
(640x480 or 1280x720) and the pixel size (2bytes or 4bytes). In the software application we
need to set 3 parameters via slave registers: the image width, the base address of the frame
in the DDR and the go signal. An example is showed below.
11
11
volatile int *hdmi_addr = (volatile int *) XPAR_HDMI_OUT_0_BASEADDR;
hdmi_addr[0] = 640; // image width in pixels
hdmi_addr[1] = 0xA0000000; // base address of the frame
hdmi_addr[2] = 1; // go
3.3 COLOR AND POSITION RECOGNITION ALGORITHM
We implemented our project in two different approaches. The first one it traverse the
whole image in software searching for the red and green colors. The second one is an
IPcore that reads the frames from the DDR in packets of 64 bytes and searches the red and
green colors in these pixels.
In figure 5 we show the original design for hardware implementation of our
algorithm as a state machine; this figure helps to understand our main idea, but it was not
implemented as it shows (see section 3.3.2).
3.3.1 SOFTWARE IMPLEMENTATION
In the software we use the display library, it has functions such as ‘gpDrawSquare’
and ‘gpFillLine’ that help us for debug. The main function of this library is ‘search’; it
traverses the whole image and for each read pixel from DDR converts it to RGB888 and
checks if it is red or green. At the end of the loop this function computes the red and green
object positions and the distance between them and returns this value.
To find the objects positions for each pixel red or green found we increment its
coordinates and keep track of the number of pixels red and green found. At the end of the
loop we divide the sum of coordinates of the red pixels by the number of red pixels and do
the same for the green pixels. After that, we have the center of mass of the red and green
pixels in the image.
Therefore in the software we only need to set the base address where is stored the
frames from the camera, set the size and base address where the new image will be stored,
set the parameters of HDMI_Out IPcore and call our search function inside of the infinite
while.
12
12
Figure 5 – Ideal design of the Color and Position Recognition state machine
13
13
//set the base address of frames from the CAMERA (CAM1 = 0xA0000000)
volatile unsigned short *ddr_addrCAM = (volatile unsigned short *) CAM1;
//set the size and base address of the image (X = 640, Y = 480, MEM = 0xA0600000)
gpImg *img = gpCreateImage(X, Y, MEM);
gpSetImage(img, 0x00, 0x00, 0x00);
//set paramenters of HDMI out
volatile int *hdmi_addr = (volatile int *) XPAR_HDMI_OUT_0_BASEADDR;
hdmi_addr[0] = img->xres; // stride length in pixels
hdmi_addr[1] = (int)img->imageData; // set frame base address
hdmi_addr[2] = 1; // go
//d: distance between red and green positions
int d;
// if DEBUG = 1 it will show squares around the red and green positions
// and a line between them. It will also paint as pure red all the pixels
// it considered red and paint as pure green all pixels it consired green
// if PRINT = 1 it will print the found position (x,y) and the distance
// values on the terminal. It need to be set 0 when communicate with the robot.
while(1){
d = search(img, ddr_addrCAM, 'r', 'g', DEBUG, PRINT);
}
3.3.2 HARDWARE IMPLEMENTATION
We created an IPcore that implements our Color and Position Recognition algorithm
- attached to an AXI4 bus which is burst capable for high-throughput memory mapped
interface. This custom IPcore reads an image from a base address in the DDR and compute
the red and green object positions.
14
14
In order to implement this functionality we had added two processes:
SEARCH_SM_RW_PROC (figure 6) and SEARCH_POSITIONS_PROC (figure 7). The
first one requests the function ‘read’ to the DDR in packets of 64 bytes and stores them in
the FIFO. The second one reads 4 bytes (2 pixels) each time from the FIFO and check if
they are red or green; in case they are, it sums their coordinates and keeps track of the
number of red or green pixels found. After finish traversing the whole image, it computes
the geometric center of the red and green objects and write them in the slave registers,
which can be accessed by the MicroBlaze processor. Then it reset all signal and restart the
loop.
Our custom IPcore has 8 slave registers.
slv_reg0 and slv_reg1 are for debug;
slv_reg2 saves the threshold (2 bytes for the red threshold, 2 bytes for the
green and 2 bytes for the blue);
slv_reg3 and slv_reg4 saves the base address of read and write;
slv_reg5 and slv_reg6 has the X,Y positions (4 bytes for the x and 4 bytes
for the y) of the red and green geometric center;
slv_reg7 save the ‘done’ value.
In the software implementation, we only need to tune the color threshold, the read
address and the write address - currently our custom IPcore does not write back to the
DDR, so we can leave it without the write base address.
volatile unsigned int *search_addr = (volatile unsigned int *) SEARCH_BASEADDR;
search_addr[2] = T_RED << 24 + T_GREEN << 16 + T_BLUE << 8; //color
threshold
search_addr[3] = CAM1; // read base address
search_addr[4] = MEM; // write base address
int x_red, y_red, x_green, y_green, done;
while(1){
x_red = search_addr[5] >> 16;
15
15
y_red = search_addr[5] & 0xFFFF;
x_green = search_addr[6] >> 16;
y_green = search_addr[6] & 0xFFFF;
done = search_addr[7];
printf("RED (%4d, %4d) GREEN (%4d, %4d) => done: %d\n\r", x_red,
y_red, x_green, y_green, done);
}
Figure 6 - - State Machine for
reading from the DDR memory
Figure 7 - State machine for reading from the
FIFO and recognizing the pixel color
16
16
Label of the signals:
f_full: FIFO is full;
cmdack: bus2ip_mst_cmdack -- Bus to Ip master command acknowledgment;
cmplt: bus2ip_mst_cmplt -- Bus to Ip master transfer complete;
f_rd_en: FIFO read enabled.
In figures 8 and 9 we can view a simulation of our custom IPcore in the software
ISE Project Navigator. This simulation tests the SEARCH_POSITIONS_PROC process.
Figure 8 – Start of SEARCH_POSITIONS_PROC process
17
17
Figure 9 - End of the loop of SEARCH_POSITIONS_PROC process
Figure 8 shows the start of the process. It stays on the search_idle state while
f_rd_en is zero. When this signal is set, the process changes to search_color state and starts
counting the red and green pixels. Figure 9 shows the end of loop of the process. When
end_loop signal is set, it changes to search_end_loop state, computes the x and y values for
the red and green objects, sets the search_done signal and goes to search_reset state, where
it resets all signals and restart the loop.
18
18
3.3.3 MATLAB INTERFACE
In this section, we will present some examples to operate the robot through
MATLAB, only for debugging/testing without the FPGA. In table 2, we can view a simple
example to turn LED ON in the servo motor 1. The packet initialize with a default value
0xFF 0xFF, and at the end it is calculated a Check-Sum for guaranteeing the command
reliability (detailed information is available in [4])
ID 0x01 is the identification of servo motor 1.
Instruction 0x03 means the write in the EEPROM of the servo.
Parameter 1 0x19 means to write in the LED parameter address.
Parameter 2 0x01 is to command ON (0x00 is OFF).
Table 2 – AX-12A Robotic Arm communication protocol example
For more examples of instructions refer to our project`s MATLAB files. There are 3
files which implements the Check-Sum function (buildPacket.m), tests the robotic arm
alone (Robot_move_TEST.m), and initialize the communication with the FPGA
(FPGA_to_Robot.m).
19
19
4 DESCRIPTION OF DESIGN TREE
Our design directory contains the following folders:
RoboticArm Project
This folder contains our two main XPS project - Search_Hardware and Search_Software - ,
a Documantation folder with the project documentation and a README with explanations
about how to use it.
The Search_Hardware and Search_Software folders contain the following files:
System.mhs: A higher level description of the hardware modules in the system
System.xmp: The system project file used by XPS
SDK: It contains an XML description of the hardware description that is exported
by the XPS and then used by SDK to build a board support package
Data: contains the user constraint file(.ucf)
HDL: Contains the upper level system file and wrapper for each of the peripherals
Implementation: contains the synthesis files, bit files and initialization files for the
BRAMS
Sysnthesis: Has the output from the XST
Pcores: user defined peripherals
Workspace: main SDK workspace with ready to use bitstream files
README: Top level description of the content of this directory
The Documentation folder contains the following files:
Robotic Arm Manual
VmodCam Reference Manual
VmodCam Schematic
Xilinx Device Driver Programmer Guide
20
20
AXI Slave and Master Burst Datasheets
Atlys Schematic
Contains a copy of this design report
5 PROJECT SCHEDULE
The original project schedule was the following:
Atlys PWM I/O interface to the servo motors – 1 week.
o Adjusting Atlys board to properly generate PWM signals and send them
to board outputs (Pmod conector probably).
o If Atlys board has some limitation using PWM, we will use an AVR
ATMega microcontroller to control the servo motors of the robotic arm.
Integrating IP (VmodCAM) – 1 week.
o Integrate the VmodCAM 3D camera in the Atlys FPGA board, for
image capture.
Colour recognition algorithm – 1 weeks.
o Improvement of our IP project for colour recognition, adjustment of
colors thresholds.
Position calculation algorithm – 2 weeks.
o Improvement of our IP project for position recognition.
o We will try a new approach to be able to calculate multiple points of the
same color.
System integration + optimization – 1 week.
o General system integration, testing, movement dynamics evaluation and
system`s time-response optimization.
As the project advanced, we had to face and solve several issues, related to the
robotic arm communication protocol, the Color and Position algorithm and its
implementation in hardware (custom IPCore). Therefore, we slightly changed our schedule
21
21
to accomplish the keys points first and fully implement the algorithm in software before
going to the VHDL implementation. The schedule was changed to:
AX-12A Robotic Arm Communication Protocol – 1 week.
o We focused all our efforts to learn, as soon as possible, the
communication protocol of the AX-12A servo motors.
o We used MATLAB to test the initial communication with the robotic
arm.
Color and Position Recognition Algorithm - Software – 1 week.
o Full Software implementation and testing of our color and position
recognition algorithm that will be implemented in our custom IPcore
afterwards.
Integrating VmodCam and HDMI_Out IPcores – 1 week.
o The integration of the VmodCam and HDMI_Out IPcores make it
possible to test and tune our Color and Position Recognition algorithm
working on software.
System integration + optimization – 1 week.
o We integrated our FPGA with the robotic arm, at this point we mapped
the robotic arm`s servo limits and related these limits as a proportional
gain from the operator`s hand movement.
Custom IPcore creation – implementation of our Color and Position
Recognition algorithm in VHDL – 2 weeks.
o We modeled our algorithm as a state machine, tested it with ISE Project
Navigator and implemented as a custom IPCore.
o We focused in making the IPcore work correctly before trying to
improve the FIFO/burst functions to read from the DDR, or creating the
FSL between our IPcore and the MicroBlaze processor.
Our group achieved all our proposed goals, but we run-out of time and could not
improve our custom IPcore to work in real-time. The changes in the schedule were
necessary in order to achieve a minimal functionality, and this also gave us the assurance
that our approach works.
22
22
6 REFERENCES
Hold Ctrl key on your PC/MAC keyboard (or Shift, depends on your reader) and click
in one of the following links to go to the related website
[1] Atlys Spartan 6 FPGA board reference manual
[2] AX-12A USB robotic arm
[3] VmodCam for Atlys FPGA Board
[4] AX-12A Communication Protocol
[5] CP2102 USB to TTL module converter