Orientation and Position

The final module in the image processing chain is the orientation module. During this stage the orientation of the robots is calculated, and the position of the robots, and the ball, are passed to the communication module for transmission.

The position of a robot can be determined using the centre of gravity of the team patch. This is because the team patch is designed to be at the centre of the robot. However, this only provides the coordinates of the robots within the frame of the camera. In order to control the robots, it is necessary to first convert the frame coordinates to real-world coordinates. To

88 achieve this, the camera would need to localise the playing field within its field of vision, as well as correcting for all lens distortions. Since the camera doesn’t move after it has been placed above the field, a procedure would only need to operate once in order to calculate the required conversion factors. Due to the complexity of such an algorithm, and because it only needs to operate once, it is more efficient to perform this calibration in software, at start-up, rather than on the FPGA itself. After calculating the values, the coordinate and orientation conversions can be performed on either the FPGA or the computer. However, since this calibration would initially be conducted in software, it can be considered outside of the scope for this thesis. Therefore, this section will only concentrate on the calculation of the

coordinates, and orientations, with respect to the camera frame. Algorithm Development

The orientation can be calculated from the angle between the centres of the two orientation patches as shown in Figure 83 (a). An offset of 45° is added to find the orientation of the robot relative to the front face, as demonstrated by Figure 83 (b).

Figure 83 - (a) orientation baseline, (b) orientation offset

In software, the angle can be calculated as

arctan

,

180 /

45

degrees

Angle

x

x

y

y

u

S

An efficient way of calculating angles in hardware is to implement a CORDIC function [64]. CORDIC, short for ‘coordinate rotation digital computer’ is an efficient algorithm for calculating trigonometric functions. The CORDIC function can be applied using only addition, subtraction, bit shifting and table lookup, making it a relatively simple and resource efficient process. The function works by applying a series of rotations to a vector in an attempt to reduce the angle to 0. If any of the rotations causes the vector to become negative, then this rotation is not applied. After a series of K rotations, an approximation of the angle for the original vector is the sum of the angles of the individual rotations. The key to an efficient implementation is to arrange all multiplications to be a power of 2 (a bit shift). This can be achieved using angles tan-1₂-k_{. The accuracy of the approximation can be increased by increasing the number of}

rotations.

A challenge with CORDIC, in hardware, is how to efficiently represent the angle. When digital variables go below 0, integer overflow or ‘wraparound’ occurs. This requires special handling in

89 order to compensate for the wraparound for values between 0-360°. Bailey [12] outlines a solution, “With a binary system, the use of degrees (or even radians) is inconvenient. An alternative is to normalise the angle so that a complete cycle goes from zero to one, which maximises the hue resolution for a given number of bits and avoids the need to manage wraparound of values outside of the range of 0-360°.

Hardware Realisation

The processing stage consists of the CORDIC algorithm. In order to implement the algorithm a pipeline series of addition, subtraction and multiplexors are used, as shown in Figure 84.

Figure 84 - Block diagram for CORDIC function

Each level in Figure 84 represents an individual rotation. This would require K levels for K iterations. Two signals (x and y) are used to control the rotation of the vector, while a third (z) is used to accumulate the sum of the rotated vectors. A standard CORDIC algorithm would use logic to check the rotation vector and use this to accumulate the approximation. In order to take advantage of pipelining, the vector approximation will always perform either an addition or subtraction operation, depending if the approximation exceeds its target value. This derives the same result as the standard approach, but is more optimised for a hardware

implementation.

The angles are represented using 11-bit values, this allows for 0.17° of accuracy for each bit. Using 7 rotations, the angle can be approximated to within 1° of accuracy, which is enough for this case study. A closer approximation can be achieved by increasing the number of rotations, however doing so adds extra latency and resources.

Each of the rotation levels can in theory be performed as a cascade, allowing a result to be determined within a single clock cycle. This assumes the propagation delay for the calculations in series is smaller than one clock cycle. However, when operating at higher clock speeds, the total propagation delay is longer than a clock cycle. This can be overcome by performing only one rotation per clock cycle and using pipelining to maintain the throughput. Therefore, new

90 angles can be calculated every clock cycle, with a 7 clock cycle latency needed for the result to propagate through the pipeline.

To allow multiple operations to be calculated sequentially, a 4th signal ‘T’ will be used to track valid data within the pipeline, as shown in Figure 85. When an operation is started, a 1 is injected at ‘T0’ which propagates at the same rate as the processing. On the output, the ‘TN’

signal indicates the presence of completed calculations.

Figure 85 - Block diagram for orientation processing algorithm

By utilising a continuous algorithm, the module can be simplified to a pass-through design, with a latency of 7 clock cycles. Figure 86 shows the black box diagram for the module.

Figure 86 - Black box diagram for the orientation and position module

Testing

When testing the results, a problem was encountered when comparing the processed angles from the FPGA with the angles calculated using Matlab. Matlab calculates the angles in degrees, whereas the CORDIC function in the FPGA calculates a binary representation of the angle. To correct this, the FPGA angles are transferred to a computer first, and then converted to degrees by equation (36), for comparison with the ‘gold standard’.

91 360 2048 degrees x Angle u (36)

Where x represents the binary angle.