Implementation of PSO Algorithm in Robot(Lejos Nxt)

Many research in robotics area to find path using different bio-inspired algorithms are conducted recently. Despite many researches are limited to the simulation result. This project tries to verify the robustness of Particle Swarm Optimization (PSO) algorithm in the real-world implementation. PSO optimizes a problem by having a population of candidate solutions, and moving these particles around in the search-space according to simple mathematical formula over the particle's position and velocity. In this project we are implementing PSO algorithm with some modification in robotics. We conducted many experiments with different number of robots and different number of destination too. The result verifies that PSO is technically sound for real-world problems of path finding even in the presence of obstacles.

Implementation Details

In this implementation one Robot is considered as combination of one PC process plus one NXT brick. The Robots are communicating each other through a LAN connection connecting these computers. Internal communication, that is communication between NXT brick and PC process , is carried through blue-tooth. Robots are getting their current position and position of food from a different process, here we called this process as 'server', which calculates the positions of each color in the arena using a camera. A schematic diagram is shown below.

In NXT brick we used two motors and two sensors. Motors are used for rotating the tires. The sensors used are one Ultrasonic sensor, for getting the distance of an obstacle, and one compass sensor, for getting current alignment of the robot.

An outline of the program implementation is given below. Here we used three threads in PC program and two threads in NXT program. One PC program can send data into one NXT and it can multi-cast data into other processes (may be into processes in different systems connected via LAN). PSO algorithm is used to minimize the value of f(x,y) given in Table 1. Here we taken the values for the weights used in PSO as w = 0.73 and c₁ = c₂ = 2 . Value for r₁ and r₂ are taken as random values .

f(x, y)	√ [ (x – x1)2 + (y – y1)2 ]	where (x1 , y1) is the position of food and (x , y) is the position of particle.
Angle, θ	tan-1 [ ( newY – y) / (newX – x) ]	where (x , y) is the current position and (newX, newY) is the next position calculate using PSO algorithm.
Equations used in PSO algorithm	V[] = v[] * w + c1 * r1 * (Pbest [] - X[]) + c2 * r2 * (Gbest [] - X[]) newX[] = X[]+V[]

Table 1: Table of equations

In PC program we used 3 threads, one for connecting NXT , one for sending data to other PC processes and other one for getting particles coordinate details from the server program using camera. In NXT program we used 2 threads, one for connecting PC and other one for controlling the motors. We are doing all calculation parts in PC program and finding out what should be the angle of robot for next position and sending this angle and a flag ,indicating different conditions, to the NXT. The value of angle varies between -179 to 180 and is calculated using the equation given in Table 1.

Algorithm used in PC

1. Connect to the python server.
2. Connect to the appropriate NXT.
3. Initialize the position of the particle according to the values from the camera, and initial velocities to zero.
   1. Initialize the position of food, (x₁ , y₁).
   2. Calculate the value of f(x,y) and make this value as initial P_best and G_best
4. Make a multi-cast connection for sending P_best values to other robots.
5. Start three threads.
   1. Thread 1
      1. Receive the values of P_best from other robots and compare those values with the current G_best and update value of G_best .
      2. Goto Step 5.1.1
   2. Thread 2
      1. Receive the particles position from camera (discard some number of packets and then accept next valid packet).
      2. If the position of food is changed. Initialize the values as in Step 3 and goto step 5.2.1.
      3. Calculate what should be the next position of the robot using PSO algorithm explained above.
      4. Calculate the angle to the next point and flag value.
      5. Trigger thread 3 for sending data to the NXT (Set flagNXTSend to 1).
      6. Update current position of the particle (Robot) and P_best values.
      7. Set P_best as the current G_best .
      8. Send P_best value to other robots through multi-cast connection.
      9. Goto Step 5.2.1.
   3. Thread 3
      1. If flagNXTSend equal to zero sleep some time and then goto step 5.3.1.
      2. Calculate send_value.
         1. If angle is less than zero , send_value = flag * -1000 + angle
         2. If angle is greater than or equal to zero then
            send_value = flag * 1000 + angle.
      3. Send send_value to the NXT.
      4. Set flagNXTSend to zero and goto step 5.3.1.

Algorithm used in NXT brick

1. Connect with appropriate PC.
2. Start two threads.
   1. Thread 1
      1. Receive send_value from PC process.
      2. Calculate value of flag and angle.
         1. If send_value is negative
            1. send_value = send_value * -1;
            2. flag = send_value / 1000;
            3. angle = send_value % 1000; (mode operation)
            4. angle = -1 * angle;
            5. goto step 2.1.3
         2. flag = send_value / 1000;
         3. angle = send_value % 1000; (mode operation )
      3. goto step 2.1.1
   2. Thread 2
      1. If value of flag is equal to 10.
         1. Rotate the robot indicating reached food, goto step 2.2.1.
      2. Take the current orientation of the robot using compass sensor.
      3. Calculate the angle of rotation such that the orientation of robot should be in the angle received from the PC through thread 1.
      4. Rotate the robot by a value calculated in Step 2.2.3
      5. Check for the obstacle, that is whether the value from Ultra sonic sensor is less than 25. If so continue to step 2.2.5.1 else to step 2.2.6
         1. Move Backward until the distance to the obstacle become greater than 30.
         2. Rotate the Robot until the distance to the obstacle become greater than 35.
         3. Rotate the Robot through an angle of 30. Assign a value of 3 to the flag.
         4. Steer the Robot around the Obstacle, and Check is there any further obstacle in front. If so goto 2.2.4.1.
         5. Repeat step 2.2.4.4 while flag value is 3.
      6. Move forward, goto step 2.2.5.