Agile Reactive Navigation for A Non-Holonomic Mobile Robot Using A Pixel Processor Array

SCAMP-5d (see Fig. 2) is the latest version of the programmable, general-purpose, power-efficient and high-performance pixel processor vision chip family developed by the University of Manchester Carey2013 . The PPA of the SCAMP-5 vision chip integrates a massively parallel Single Instruction Multiple Data (SIMD) processor array into the image sensor pixels. It is the combination of the image sensor’s pixels and processor granting it the ability of both a traditional visual sensor and a programmable processor. The vision sensor of the SCAMP-5 system consists of a grid of 65,536 (256 $\times$ 256) processing elements (PE). Each PE in the image array consists of 7 analogue registers, 13 digital registers, and 1 digital ‘FLAG’ register, which enables this vision system to store 7 grey-value images, 13 binary images and to support conditional execution among frames. Moreover, arithmetic operations, logic operations and neighbourhood operations on the image plane are available.

Unlike the conventional image sensor which only contains the light sensitive pixel circuits, SCAMP-5 features the integration of image sensors and processors on a single silicon die. By doing so, the image processing happens directly on the image plane. Thus, the image processing results rather than the image itself can be transferred to other controllers (see Fig. 3). For most tasks, the data/calculations resulting from this image processing are far smaller than the raw image data, which greatly reduces the transfer time between SCAMP-5 and the controller compared to a standard camera. The vision chip controller is based on the ARM-Cortex-M, it supports programming and general-purpose computation tasks. The image processing algorithms can be developed using a standard C++ compiler. on the vision chip controller. After compiling the program, the image processing kernels for acceleration on the processor array are sent to each PE to perform processing tasks. These PEs are able to perform basic computation tasks in parallel, hence enabling fast-speed on-sensor image processing. Finally, the image processing results are sent back to the chip controller and then transferred to next-level hardware, such as the single board computer through GPIO or USB interface. Moreover, the SCAMP-5 system enables real-time image processing on the focal plane while consuming minimal power (between 0.1W and 2W depending on the algorithm), which is suitable for embedded systems in the field of robotics Bose2017 ; Greatwood2017 ; martel2016real ; Chen2017 ; Chen2020icra .

This subsection describes the SCAMP-5 algorithm used to detect the pre-designed patterns. Patterns are a common means to give instructions to human drivers or even to autonomous cars such as in line following. The challenges include tolerance to noise, clutter and fast enough processing. The patterns we use are primarily aimed at demonstrating agile visuo-control behaviours.

For the gate’s task, each pattern contains four black disks surrounded by two black concentric squares. The disk coordinates in the image plane are utilised to adjust the rover’s position and orientation, guiding it to go through multiple gates at high speed. Fig. 4 and Fig. 5 show the image processing procedure conducted on SCAMP-5. High-frame-rate real-time image processing is typically challenging to conduct with a standard camera and computer setup due to the limited rate of image capture and delay in data transfer that such devices have. In this work the target extraction algorithm is designed to exploit SCAMP-5’s fast image flooding ability to minimise computation time per image and improve the image processing robustness. Iterative flooding operations are used to extract centres of four disks out of the cluttered background Greatwood2017 . This method fills regions with value ‘1’s except these areas closed by value ‘0’s. Hence those areas that are out of the closed regions would be filled with ‘0’s and with this method, the background of the pattern is easily eliminated within several iterations. As shown in Fig. 4, after the first flooding iteration, the background is eliminated. Therefore, the time cost for this kind of operation is much less than that of the traditional camera, because this flooding operation is carried out in all PEs in parallel and asynchronously rather than pixel by pixel for traditional image processing.

The extraction of four points in the pattern relies on the presence of two black concentric boundaries. However, issues, such as target occlusion, bright light reflections or simply the target being distant from the camera, may break or merge together those two boundaries, causing failure of the dot extraction process. For example, in Fig. 5, the pattern in the source image is partly outside the view field and the boundaries broken on the right hand side for demonstration. In this case, the direct flooding method is not useful since four disks are not enclosed by ‘0’s. To improve the robustness of the pattern extraction, the prior knowledge of where the dots were located in the previous frame is used whenever the two black boundaries are not present. This is illustrated in Fig. 5, when there is no object remaining after two flooding and inversion operations. Firstly, the inversion of the current binary image is performed. Then, a point from the last detected disk centre is loaded into the current frame. Since the frame rate of the SCAMP-5 is set to 200 fps or more, the shift between two consecutive frames is small, and thus, we assume that the loaded point falls into the current corresponding disk. Then, flooding is conducted outwards from this point using the current inverted image as a mask. If the point from which flooding was performed existed within the disk, the resulting image contains only that disk which can then be easily extracted. After getting rid of the first extracted disk from the inverted image, the location of the remainder of the disks can be obtained using the loading point and flooding method iteratively. The detailed algorithm description can be seen in Algorithm 1.

Noise in an image is caused by various factors, such as overexposure, cluttered environment and quantisation. In this scenario, the light spots caused by overexposure or reflection are the main sources of noise. To eliminate the noise, an image filtering method based on the neighbour communication is conducted upon SCAMP-5. Since each processing element can communicate with its four neighbours (north, south, west, and east), a given pixel can access its neighbours’ data directly. Four new pictures are obtained after moving the source image into four directions with a pixel distance $P\textsubscript{step}$. Then using ‘AND’ operator to add these four images together on the image plane directly. In Fig. 6, noisy area whose radius is smaller than shrink radius will be eliminated. The pseudo codes are given in Algorithm 2.

We now discuss the way of getting each dot’s centroid in the image plane. First an inbuilt function $scan\underline{~{}}event$ of the SCAMP-5 is used to find the location of a white pixel in the image. This white pixel must be located within one of the disks we wish to extract. We then perform a flooding operation originating from this extracted point, using current image as a mask, resulting in an image consisting of the entire disk the point was within, as shown in the second image of Fig. 7. The bounding box of all white pixels in the image (which in this case are those of the disk) is then extracted using another built-in SCAMP-5 function $scan\underline{~{}}boundingbox$. After that, the disk is removed from the original binary image using a NOT operation. With this method, one white disk can be extracted. This process is similar to that used in Fig. 5 where squares are not intact. The $scan\underline{~{}}boundingbox$ function outputs the centre position of the white region. After performing this type of operation for four iterations, all centre positions of the disks can be extracted separately.

The image processing time of all algorithms mentioned above is recorded by the SCAMP-5 application that receives the image processing information from the SCAMP-5 vision system through a USB cable. In this scenario, the frame rate is set to 200 fps given the illumination conditions in the indoor arena. It is noteworthy that the proposed image processing algorithm can be easily performed at more than 2000 fps with enough light illumination.

The SCAMP-5 extracts the four disks located within any visible targets which are then utilised to steer the mobile robot through the gate with the observed target. Controls are generated by comparing the four disks extracted from a pattern in view against four reference disk for a pattern at a known distance and angle. However, when carrying out the disk extraction, the order of these four dots is unknown because the relative position between the camera and the gate can be random. Hence, a method is developed to make these eight points correspond to each other before making a comparison.

As shown in Fig. 10, the centre of these four dots $P\textsubscript{cc}$ can be obtained by $P\textsubscript{cc}=\sum P\textsubscript{ci}/4$. By comparing the difference between $P\textsubscript{cc}$ and $P\textsubscript{ci}$, these four dots can be allocated into the corresponding quadrant. The current picture is divided into four quadrants each of which will contain a specific disk which can then be compared to the corresponding disk in a reference pattern.

Where $Sign$ represents the sign symbol of the difference between $P\textsubscript{ccy}$ and $P\textsubscript{ciy}$ along $y$ axis.

This subsection compares the difference in both position and shape between the reference rectangle and the currently processed quadrangle to generate instructions for the mobile robot to run through gates vertically. The reference image is defined with an image that was pre-set by putting the ground vehicle in front of the gate at a distance which is the minimum distance that is in sight of the sensor. The reference rectangle is compared to the currently processed quadrangle; from this comparison, controls are generated for the RC rover which will bring the observed rectangle closer to the reference, guiding it towards the gate. As the pose of the rover gets closer to that at which the reference image was taken, the similarity between the quadrangles correspondingly increases. This control guides the rover through the gate without collision enabling the rover to see the next gate.

During reactive navigation, the distance $D\textsubscript{y}$ between $P\textsubscript{cc}$ and $P\textsubscript{rc}$, the deformation $\delta y$ along y axis (see Fig. 10) act as the input for the PID control.

where $D\textsubscript{y}$ is utilised to adjust the angle of the front wheels towards the target. $\delta y$ describes the deformation of the pattern along y axis caused by the relative position between the pattern and the rover. In this scenario, $\delta y$ is the difference between top side centre and bottom side centre in the quadrangle.

where $\delta y$ is used as a compensation to slightly change the wheel angle and attempt to take the rover straight through the gate rather than going through at an angle. The adopted control method is as follows:

Where, $i\in\{1,2\}$, $K_{P}$, $K_{I}$ and $K_{D}$ are coefficients adjusted experimentally in this paper using Ziegler-Nichols ziegler1942optimum . $e(k)_{i}$ is the error between $D\textsubscript{y}$, $\delta y$ and 0. As we can see from the equation, the visual information is slightly processed before generating control instructions for the mobile robot. The aim of this PID control is to minimise both $D\textsubscript{y}$ and $\delta y$ to 0. During this process, the ground vehicle is moving towards the gate while adjusting its pose to enter the gate head on.