Fpga Based Multi-Robot Collision Avoidance System

This paper presents a Multi-robot collision avoidance system suitable for FPGA implementation. The proposed scheme attempts a novel method of Reciprocal Collision Avoidance(RCA) along with Accelerating Velocity Obstacles(AVO). Parallel architecture is used for developing the collision analysis of robots with different targets. Each of these robots moves and reaches its target, avoiding collisions on its path. Simulations for the digital design hardware have been carried out in Vivado software and the robotic movements have been demonstrated using V-rep software. The proposed scheme would result in reduced cost and power dissipation while deploying in FPGAs. The details of the collision detection algorithm and the working of different modules used in the scheme along with test results are presented in the paper.


Introduction
The field of autonomous Multi-robot systems is expanding every day with the increase in automation in industries. Obstacles always pose a great threat in Multi-robot systems. Consequently, the obstacle avoidance system is a major focus of such systems. In those systems, where there are no other moving objects other than the robot itself, static obstacle avoidance algorithms are used. Various algorithms for static obstacle avoidance have been found efficient in the field of robotics [1].Spatial planning is one such algorithm based on characterizing the position and orientation of each robot as a single point in the workspace. Other algorithms include time-varying artificial potential fields and rapidly exploring random trees which have gained popularity in static obstacle avoidance [2,3].
Multi-robot environments involve moving obstacles.Hence, dynamic obstacle avoidance algorithms are in need to have a collision free traversal. [4]. Omnidirectional vision sensor system with catadioptric panoramic cameras can be used for collision avoidance [5]. After applying some existing image processing techniques to the images obtained, the real image is restored and the environmental information around a robot is identified. However realtime application of computer vision requires high computational power and is expensive. A* algorithm can be used to plan the path for collision avoidance by using dynamic real-time monitoring and iterative move-evaluate-move cycles [6]. This can be used to avoid static as well as dynamic obstacles. Distributed Autonomous Robotic System (DARS) uses multilayered reinforcement learning methods. By dividing a learning curriculum into multiple layers, the number of expected situations can be reduced [7]. The aforementioned methods are computationally intensive, require extensive usage of memory, and may not be feasible for small scale systems.
The hardware implementation of Multi-robot collision avoidance systems has been done by various methods with the help of different sensors such as RFID [8], ZigBee communication, but these have practical difficulties such as Dead corners in sensing. Long distance transmission of data also poses a problem in hardware implementation which can be improved with repeaters, this would increase the cost of the system. Unlike a hardware system, a simulated system enables testing of the algorithm for a range of system parameters like the number of bots, robot constraints and nature of robot used.Simulation system based research on mid-air collision avoidance between pairwise noncooperative aircraft presents an extended 3D case method to the geometric approach of collision cone based algorithms which are generally used in planar mobile robots. The algorithm presents avoidance maneuvers both in horizontal and vertical planes and the effectiveness is checked using numerical simulations in proper conflict scenarios [9].
A very popular approach of collision avoidance is based on velocity and is known as the Velocity Obstacle (VO) approach. Moving robots are known as Velocity Obstacles. When acceleration constraints are included, it is called AVO [10]. When every robot takes the responsibility to avoid collisions, it is termed as RCA [11].In the aforementioned algorithm, the current position and velocities of each robot in the workspace are used to find the set of velocities that can avoid the obstacles by satisfying the constraints. The set of velocities that will result in a collision forms the collision cone [10,11,12].
The collision cone inequality can be solved using various approaches. One of these uses linear programming to solve all the inequalities with the acceleration constraints and obtain the velocity that falls out of the collision cone. The linear inequalities can be solved using recurrent  [13]. The main disadvantage of this approach is that it is expensive in terms of clock utilization and resources when executed in FPGA.
The need for FPGA arises from the fact that it is highly reconfigurable. This feature is particularly helpful in the robotic industry, as each robot would have different requirements, different tasks to perform, different sensor data to collect [14]. So finding processors for each robot's provision would be tedious, and also customized products would lead to an increase in overall system cost. As FPGA is flexible and scalable, it has the potential to revolutionize the already rapidly changing robotics industry [15].
One of the critical factors to note is that most of the microcontrollers currently used in robotics are sequential in nature, whereas parallel architecture can be designed and executed in FPGAs. With FPGA as the processing unit, the size, the cost of the robot, and execution time decrease. FPGAs allow development in gate-level architecture, which makes them highly reliable for speed and accuracy [16]. This gives them an added advantage over the existing microcontrollers. The use of FPGA reduces the power dissipation as only the shortest path is implemented. Parallelism can be achieved into the system by making multiple instantiations of the same elementary level modules and using them in parallel, ensuring fast computation with minimum hardware. When a network of a large number of robots is deployed, a significant reduction in power dissipation can be witnessed. [17] 1.

Contributions of this paper
This work primarily focuses on multiple robots reaching their respective targets by avoiding collisions with each other. The proposed scheme has utilized the V-rep platform for the simulation which allows the design of customized bots with proper handling of kinematics/inverse kinematics. RCA with AVO is used in the present scheme which primarily works with velocity as a parameter for avoiding obstacles. This technique makes the system more robust to the dynamic changes in the environment besides consuming lesser power, faster and less computationally intensive in contrast to the already existing methods.
The novelty of this paper lies in the execution of the velocity selection strategy, communication between the software, and limited-time path planning.
• A hybrid of to goal and maximum velocity strategy is proposed in velocity selection to reduce the traversal time. • Handshake signals have been interspersed with the communication modules to counter the mismatch in the rate of data transfer between the two software • The path planner deployed is made of primitive logic structures thus making optimal use of resources.
Two major platforms are analyzed for the implementation of the scheme -namely ModelSim and Xilinx Vivado. Vivado is an integrated design environment (IDE) with system-to-IC level tools built on a shared, scalable data model and a common debug environment. The software also contains an elaborate IP catalog that can be used for multiple protocol designs and allows direct synthesis onto an FPGA system. Hence this software was chosen over ModelSim. The present work has utilized the un-paralleled runtime and memory capabilities of the software to perform computations at timings closest to runtime.

. Collision cone
This paper focuses on the novel implementation of RCA with AVO. This implementation is cost-effective and has lower power dissipation. In this algorithm, the current position and velocities of each robot in the workspace are used along with constraints of the robot to find the set of velocities that can avoid the obstacles by satisfying the constraints. The set of velocities other than those used to avoid collision are known as the Velocity Obstacle. This set of velocities forms the collision cone. Figure.1 shows a collision cone (C1/2) where the collision between a robot at (x1,y1) and a dynamic obstacle at (x2,y2) is predicted. Any velocity outside this cone is the velocity which the robot can take to avoid obstacles successfully. In these equations, robots other than one which is controlled are considered as passive. All the velocities satisfying this equation are the avoidance velocities. The collision cone forms the basis for collision avoidance in this paper [18].

Update module
The update module predicts the positions and velocities of the robots at the next instant. Throughout this paper, the robots have uniform acceleration in the direction of motion. Thus, the equations of kinematics are applicable and are used to predict the state of the robot in the next instant. v n+1 = v n + a n t r n+1 = r n + v n t + 1 2 a n t 2 where, This module was implemented at the gate level, with the same multipliers and adders used repeatedly over different clock cycles. Figure.2 shows the architecture of the update module. Each horizontal level in the diagram represents a clock cycle. The clock frequency is set according to the constraints of gate propagation delays. The major inputs to the modules are the current position, velocity, acceleration, and sampling time and the module outputs the predicted position and velocity of the robot for the next instant.

Collision detection module
This module evaluates the inequality in the equation below and predicts whether there will be a collision or not. If the velocity falls outside the collision cone, the inequality holds true and there will be no collision at the next instant. If a collision is detected, the velocity is modified and the inequality is rechecked with the updated value of velocity.
Where r is the relative position, R is the sum of the radii of both the robots and V is the relative velocity of the robot 1 with respect to robot 2 respectively. Similar to the update module, the collision detection module is also implemented at the gate level with recursive usage of multipliers and adders. The inputs to this module are the velocity and position of two different robots in cartesian form. The collision detection module implemented has two 32 bit multipliers, four 32 bit subtractors, one 64 bit subtractor, three 32 bit adders, and one 64 bit comparator. This architecture is shown diagrammatically in Figure.

Methodology
To achieve the given objective we propose a three-module scheme.
• Vivado Modules -consisting of Verilog modules implementing the necessary computations and algorithms for collision avoidance • VREP Modules -consisting of modules required to create robots, control the kinematics of the bot and configuring the initial and simulation setup • Communication Modules-consisting of both Vivado and Vrep modules along with text files to facilitate the communication between the above-mentioned modules.  Figure 4: Three module scheme.Verilog modules implementing the necessary computations and algorithms for collision avoidance,Vrep modules controls the robot kinematics and configures the environment and Communication modules to aid communication between Vrep and Vivado modules.

Velocity Selector Module
Once a collision is detected by any of the robots, it has to take steps to avoid the collision. The velocity selector module changes the current velocity either by changing the magnitude and keeping phase constant or by changing both magnitude and phase. This module needs to access the coordinates in polar form, hence rectangular to polar conversion and vice-e-versa are implemented using CORDIC IP [19]. Coordinate rotational digital computer (CORDIC) algorithm was initially developed to solve non-linear trigonometric equations, then developed to solve other non-linear equations such as square root and hyperbolic equations. The CORDIC CORE of the CORDIC IP implements a various range of equations based on the set configuration. Xilinx's CORDIC v6.0 IP is being used in this paper [20]. The CORDIC architecture is shown in Figure.5.   Figure 6: Velocity selector flowchart. Takes the current velocity in rectangular coordinates as the input, uses CORDIC IP for rectangular to polar and polar to rectangular conversions. Follows hybrid to goal and maximum velocity strategy. Gives the modified velocity as the output. Figure.6 shows the flowchart of the velocity selector module. Once the necessary conversions have been made, the module has two steps of action. First, the magnitude of the velocity is decreased by a predetermined step size (to be designed based on the robot's speed and torque constraints). The new velocity is passed through the update and collision detection modules to recheck for collisions. If there is no collision, the velocity is fed to the robots. If a collision is detected, the velocity is changed again. However, just decreasing the magnitude of velocity may eventually stop the robot and also increase the traversal time of the robot. This paper uses a novel method which is a hybrid of the to goal and velocity selection strategies. This method involves modifying the phase of the robot once the velocity reduces below a given threshold. Once the phase is modified, the magnitude of velocity is set to the maximum possible velocity. Hence the bots try to reach the target in the least possible time. The CORDIC IP then converts the polar values to rectangular values and the output of the module is the modified velocity of the bot.

Path planner and target checker
The velocity that has been changed by the velocity selector need not be in the direction of the bot's target destination. In order to make the bot reach its target destination, a path planning algorithm is being implemented.
The path planning algorithm is implemented as a part of the integrator module. For each iteration when no collisions are detected for a robot, a count variable is incremented. When the count goes above eight, the robot is given velocity in the direction towards its target from its current position. The velocity vector is calculated by assigning the rectangular components, values proportional to the difference in the cartesian coordinates of the robot's current position, and its target coordinates. Since such an approach will yield very small velocities when the robot is close to the target, the proportionality constant is scaled up. The count is reset whenever a collision is detected with another robot to allow velocity selector to take in-charge and assign a velocity that does not result in a collision with the other robots. The surface of the traversal of the robots is assumed to be smooth and accelerations are held approximately constant throughout a unidirectional motion. The accelerations (ax and ay) are set such that the net acceleration, a is in the direction of current velocity and stays the same till the velocity is changed.
While the path planner makes sure the robot is moving in a direction towards the target, the target checker ensures the robot stops once it has reached. The target checker module of each robot takes as inputs the current coordinates of the robot and compares it with its target coordinates to decide if it has reached the target. Keeping in mind the robot's dimensions with an 11cm*19cm chassis, an error of 2cm is permitted currently and can be changed if high precision is required according to the application. Each time the positions and velocity feedback from the robot reaches the Verilog script, the target checker operates. Once the target checker module establishes that the robot has reached its target, it sets both the robot velocities (Vx and Vy) and accelerations (Ax and Ay) to zero to bring it to a stop immediately. The integrator module corresponding to the robot is disabled as it has completed its journey. The update modules of all the robots and integration modules of other robots continue to operate until all the robots have reached their targets after which simulation is stopped.

Collision detection
The collision detection module constraint explained in the theory section is valid only when there is non zero relative velocity and for a non zero angle between relative velocity and relative displacement between the two bots. It has to be modified as follows to suit the application of the system presented in this paper. When the relative velocity and relative displacement are in the same direction, relative displacement can be directly compared with the R as shown in the below equation.
If the relative velocity is zero, then the entire collision detection module is bypassed.

Kinematics of the robot
The kinematic module implemented in the robotic simulation platform V-Rep comprises the computation of the motor speed of the left and right wheels of the robot and setting it to the dynamic joints actuating the wheels in simulation. Since the robot design used is a differential drive, there are certain non-holonomic constraints on establishing its position. For example, the robot cannot move laterally sideways along its axle. A simple approach to achieve any direction of the target velocity vector is to first turn the robot in the direction required and then move straight according to the magnitude of the target velocity vector. Since the orientation of the robot at any given instant need not align with the global axes, the relative rotation required is calculated before rotating. The orientation of the robot at any instant is calculated by setting it to align with the global axes initially before the motion starts and updating it after each rotation. Angle of target velocity vector is found by But since the solutions of tan −1 are in the range(-90 • , 90 • ) only, while the angle of target velocity vector can be in the range (-180 • , 180 • ), based on the signs of VX and VY , theta is modified to increase range and achieve rotation to any angle Then, required relative angle of rotation is where, φ is the current orientation of the robot in global axes. Based on whether ψ is negative or positive, the robot turns right or left respectively. Once the relative rotation is computed, the robot has to be rotated accordingly. The rotation is achieved by stopping the wheel in the direction of rotation and actuating the other wheel to move along a circular path with the stopped wheel as the center and the axle length as the radius like in Figure.7 [21].

Communication
Communication is an essential module between any two entities. In this work, efficient communication between the two software is a requirement for the smooth functioning of the implementation. The following methods have been used for communication purposes.
This work employs text files to pass data between the two software. Six text files (two for each bot) have been created and stored in the appropriate folders. For every bot, the first text file carries information from the V-rep software indicating the cartesian velocities and positions of the bot at any given instance. The second text file contains the computed values of velocities written by the computational software after performing the necessary calculations and checking the conditions. The first step of the simulation is to write the initial velocities and positions of the bots to the first text files. This operation is performed simultaneously for all the bots from the simulation software. Vivado software then reads these files simultaneously. Once the values are read and stored, the computations take place, and based on whether a collision is detected or not the magnitude of velocity is decreased or remains stable.
The These two software, however, are not synchronized. The rate at which Vivado writes to the text file is different from the rate at which V-rep reads the same and vice-versa. Hence, a problem of asynchronization between the two software occurs. The reading of text file by Vivado must take place only when the velocities and positions are updated by the bots simulated in V-rep. Similarly, the bots must read the velocities from the text file and update them to the simulation only when all the computation in Vivado has been completed and the revised velocities have been updated. To overcome these problems handshake signals have been used.
Handshaking is defined as "The establishment of synchronization between sending and receiving equipment by means of exchange of specific character configurations." Handshaking is used as one of the major methods for the establishment of a synchronized communication between two channels. It allows the receiver to know when the input is available and allows the sender to know when the outputs have been received successfully by the receiver. The sender waits until the handshake signal is set before sending the next set of information to the receiver. In case of an error, the handshake signal isn't set so the sender either waits for some time or resends the data.  This paper uses characters as handshake signals between the two software. As seen in Figure. The Figure.10a and Figure.10b represent the reverse handshake phenomenon between the computation and simulation software. Once the computational software i.e. Vivado receives the initial velocities and positions of the bot, it performs the analysis of whether there will be a collision or not and what to do in each case. Finally, it writes the updated velocities in the file along with the character 'w' in the first line, indicating that the new velocities have been updated. V-rep then reads the new velocities from the file, updating the character to 'r' indicating to the computational software that the data has been communicated successfully.

Integration of modules
As shown in the block diagram in Figure.4, the Integrator module integrates various other modules required for computing the collision detection algorithm for the bot under consideration. Each of the modules discussed above is designed with input rdy signal as an input and output rdy signal as an output. These signals are used in coordinating the modules one after another. Input rdy signals are held high until the complete module's execution is completed, while output rdy signals are pulses that mark the end of the module's execution.
As the objective is to prevent any collision in a three bot system, each bot's integrator module is equipped with two collision detection modules. (Say bot 1 is under consideration, two collision detection modules are needed to prevent 1-2 and 1-3 collisions.) Velocity has to be changed even if one of the collision detection modules among CD1 and CD2, detects a collision. On detecting a rising edge in the input rdy of the Integrator, CD1 in rdy and CD2 in rdy signals are being set to logic 1 hence activating the CD1 and CD2 modules. After the execution of the CD1 and CD2 modules, CD1 out rdy and CD2 out rdy pulses are being generated by the respective CD modules. Integrator on detecting the output rdy pulses from CD modules deactivates the CD modules by writing logic 0 to CD1 in rdy and CD2 in rdy signals.
The output of the CD modules are stored in the coll detect1 and coll detect2 variables. When both coll detect1 and coll detect2 are logic 0 (implies no collision), the write test module is being activated by holding write check signal high. After the current velocities are written into the file by the write test module, IG output rdy pulse is being generated to mark the end of execution of the integrator module.
If either coll detect1 or coll detect2 is high (implies there is a collision with at least one bot), the velocity selector module(VS) is being activated by holding VS in rdy signal high. The current velocity is being passed on to the VS module. After the execution of the module, VS generates VS out rdy pulse conveying the end of its execution. Once the Velocity selection is completed, the current chosen speed has to be updated to check if it would result in a collision in the next instant.
Therefore, on detection of VS out rdy pulse, VS in rdy is held low, deactivating the VS module and UM in rdy is held high, activating the update module (UM). After the velocities are being updated, UM out rdy pulse is being generated in the update module. On detection of the UM out rdy pulse, CD modules are being activated again. This process continues until a condition of no collision with all the bots is being reached. The top module is the main module that controls each bot's activity and is the topmost in the hierarchy. The block diagram of the top module is given in Figure. 4. To read the data written by the VREP modules, the read test module is being employed. Current velocities and current coordinates are read from the text files in cartesian form and loaded on to the variables for each of the bot. On completion of the read module, input read pulse is being generated.
In the top module, on detection of the input read pulse, Update modules of 1,2 and 3 are activated with the respective input signals(UM in rdy1,UM in rdy2 and UM in rdy3). To reduce the number of update modules and hence to reduce the hardware required for synthesis, the first set of Update modules are being implemented in the top rather than in the integrator. Update modules completion is marked with logic 1 in UM out rdy1, UM out rdy2 and UM out rdy3. Once all the update modules are completed, UM all done signal is being set.
On the rising edge of UM all done, all the integrator blocks IG in rdy1, IG in rdy2 and IG in rdy3 are set. The number of integrator blocks equals the number of robots in the environment. Once the integrator block is completed, it generates IG out rdy pulses. On detecting the IG out rdy pulse, the respective integrator is deactivated by writing 0 to its IG in rdy.
Target checker is being used to check if the bot has reached its target destination, On reaching the target, top activates the write test module of the respective bot to write zero velocity into it.  Figure 12: Top source code hierarchy

Testing and results
Three levels of testing were performed and results were obtained. Level 1 testing contains every module testing parameters and results. Level 2 testing gives input via textfiles, hardcoding velocities and positions. The outputs are observed on the waveforms. The final stage of testing contains outputs obtained from running both the simulation and computational software simultaneously. Figure.13 shows the velocity selector operation where phase is held constant and magnitude is changed. Figure 13: Wave output of velocity selector with constant phase and reduced magnitude x real , y real are the input rectangular coordinates from the integrator. On receiving an active signal, the conversion process starts. The rectangular coordinates are converted to polar and stored in the variables mag and phase. From Figure.13, it is clear that the magnitude is changed while keeping phase constant(mag1 is reduced from mag and phase1 is held same as phase). x rot and y rot are the final rectangular coordinates having mag1 and phase1 as the magnitude and phase. The end of the velocity selector module is marked with the vs done pulse.  Figure 14: Wave output of velocity selector with constant magnitude and reduced phase Figure.14 shows the velocity selector operation where magnitude is held constant and phase is changed. This is done when the reduced magnitude is lesser than the threshold.(mag1 is held constant and phase1 is changed)

Integration module
The following figures show the testing of the integrator module that involves generating signals to activate and deactivate different modules computing for each robot.  Figure.15 shows a complete execution of the integrator module. This can be inferred from the input rdy signal going 0. At the rising edge of the input rdy, CD1 in rdy and CD2 in rdy goes high. After the execution of the collision detection module is complete, CD1 out rdy and CD2 out rdy pulses are being generated. Figure.16 shows the zoomed in version of the first collision detection output.
As coll detect1 gives a high output, Velocity selector is being activated by making VS in rdy high. From Figure.15, at the end of Velocity selector operation, VS out rdy pulses are being generated and Velocity selector module is being deactivated by holding VS in rdy to 0. The  16 VS out rdy pulse also triggers the Update module by making UM in rdy high. The end of the update module is marked by UM out rdy pulse, which activates the collision detection modules by making CD1 in rdy and CD2 in rdy high. At the end of CD modules with CD out rdy pulses are being generated. Figure.17 shows the zoomed in version of the second collision detection output. As both coll detect1 and coll detect2 are 0, output in rdy pulse is being generated, triggering the write modules to write the updated velocity in to the bots.       shows UM all done pulse triggers the integrator modules, by holding IG in rdy1, IG in rdy2 and IG rdy 3 pulse high. The completion of the integrator module is marked with the IG out rdy pulses, which the deactivates the corresponding integrator module by writing 0 to the IG in rdy signal.

Overall computational testing
In this Level, we combine all the individual modules and run a overall module testing to check the integration between each different modules. Table 1 displays the input parameters for  the 3 Robots, and Table 2   For the above mentioned test case, we expect the Robots to encounter collision, and the Overall Module behaves accordingly and detects collision and also takes steps to avoid it and reach their respective targets. Table 3 and Table 4 displays the values for the next test case. Here, in this case the robots does not encounter any collision and they reach their respective targets in their initial course without any deviation.   4.3. Simulation testing CASE 1 : When Robot1 and Robot2 are on the way to collision, the robot is expected to slow down. This can be seen in the values of vx and vy read from Vivado and printed in console log of VREP shown in Figure.

Conclusion
The paper has implemented a novel method of Reciprocal Collision Avoidance(RCA) along with Acceleration Velocity Obstacles(AVO); this implementation has been effective in reducing cost and power dissipation while deploying in FPGAs. This implementation would be a significant contribution to the field of low-cost robotic systems.
In this paper two software namely Vivado and V-REP have been used for computation and simulation respectively. The communication between the two software takes place using text files. Synchronization has been achieved using handshake signals. The surface of the traversal of the robots is assumed to be smooth and accelerations are held approximately constant throughout a unidirectional motion. Once the simulation begins, the positions and velocities of the bots are written to the respective text files which are then read by the simulation software and given to