- Email: [email protected]
RAPID PROTOTYPING OF MOBILE ROBOTS EXTENDING LEGO MINDSTORMS PLATFORM
RAPID PROTOTYPING OF MOBILE ROBOTS EXTENDING LEGO MINDSTORMS PLATFORM Jos´ e Gon¸ calves, Jos´ e Lima, Paulo Costa
Polytechnic Institute of Bragan¸ca, Portugal {goncalves, jllima}@ipb.pt Faculty of Engineering of the University of Porto, Portugal [email protected]
Abstract: Some areas of knowledge require the application of new learning methods in order to motivate and involve the students in the learning process. This paper describes the rapid prototyping of mobile robots with the flexible, modular and educational LEGOT M Mindstorms platform, supporting activities on some mobile robotics topics like control, positioning estimation and reconfiguration. The diversity of involved areas (mechanics, electronics, control,...) illustrates quite well the inter-disciplinarity of robotics. Those who are taking their first steps in robotics, specially in mobile robotics, need tools that allow them to learn some c 2006 IFAC introductory concepts. Copyright Keywords: Control education, Prototyping, Robotics
1. INTRODUCTION In this paper the rapid prototyping and control architecture of a three wheel omnidirectional and a differential robots is described. Both were based on a LEGOT M Mindstorms platform with the purpose of studying positioning estimation and trajectory control. The presented robots use the LEGOT M DC motors with encoders, allowing position estimation based on the odometry calculation. The robotics control problems have inevitable hard real time requisites, so it is necessary to pay special attention when executing mathematical operations, they must be optimized in order to fulfill real time requisites. The paper is organized in the following sequence: Initially, Section 2 describes why use LEGOT M Mindstorms in robot prototyping. Section 3 describes the Hardware development of the two prototypes. Section 4 describes the Kinematics of the developed robots. Section 5 describes the Control
Software Architecture. Finally, Section 6 rounds up the paper with conclusions.
2. WHY USE LEGOT M MINDSTORMS Statistical studies related to scholar success present some preoccupying indicators in the learning of some areas of knowledge, namely physics and mathematics. This statement deserves a deeply reflection about the current learning methods and the way to motivate and involve the students in the learning process. A current trend in learning programs, even at high-degree studies and research activities, is applying the concepts of ’learning by doing’ and ’learning by enjoying’. In this context, the LEGOT M Mindstorms platform appears as a simple, flexible, attractive and educational way to achieve the referred challenge in several domains of knowledge. The LEGOT M Modularity makes
the rapid prototyping of different robot configurations easier (Reshko et al., 2000). This easiness presents itself as an extra motivation for the persons who are taking their first steps in the world of mobile robotics. The LEGOT M parts allow connectivity, suppressing the need of using glue or screws. Some difficulty arises while building a three wheel omnidirectional robot, due to the hardness of making connectivity of LEGOT M parts with angles different from multiples of 90o . It’s also an ecological tool because although it’s not easy to recycle plastic, the LEGOT M parts can be reused. It’s a modular tool, it is possible for the same part to be a different thing depending on the application that we want, motivating those who are using it and it is presented at a relatively low cost. In short, LEGOT M Mindstorms platform is a good educational tool to study some mobile robotics subjects.
3. HARDWARE DEVELOPMENT Attaching encoders to the wheels is a very important element, allowing the odometry calculation which has the goal of estimating the robot absolute positioning. The motors used in the construction of the prototypes are 9 Volt DC motors with internal gearbox, provided with the LEGOT M Mindstorms platform. The sensors used to measure the wheel velocity are encoders, being described in the next subsection. The first developed prototype, a differential robot, is composed by two wheels and a light sensor. Its movement is controlled by varying the velocity of each wheel independently. This is presented in Figure 1.
Fig. 2. Developed omnidirectional prototype 3.1 Encoder Schematic description The encoders, that allow the odometry update, are based in the schematic presented in Figure 3. They were developed specifically for this application because there is no alternative provided with LEGOT M Mindstorms platform Robotics Invention System 2.0. There are commercial sensors available to do this task (Educational, 2005).
Fig. 3. Encoder circuit schematic When the infrared signal is interrupted, the photo-transistor becomes unpolarized,the current in the resistance Rt is suppressed, consequently the output equals the polarization voltage (E) (Pallas-Areny and Webster, 1991). The output signal in the Photo-transistor collector, presented in Figure 4, is converted in a square wave using a Schmitt trigger.
Fig. 1. Developed differential prototype The second developed prototype is a three wheel omnidirectional robot, which is presented in Figure 2. In order to assure the omnidirectionality characteristic it is necessary that the wheels have negligible friction in the motor shaft direction. This is achieved with a special set of wheels for omnidirectional robots.
Fig. 4. Signal in the collector of the phototransistor An acetate leaf, which interrupts the infrared signal, is coupled to the motor shaft, as shown in figure 5. Each transition corresponds to an angular displacement.
4. KINEMATICS OF THE DEVELOPED PROTOTYPES 4.1 Differential robot model The differential robot is probably the most used mobile robot (presented in Figure 6). The differential robot is composed by two wheels, with their shaft passing by the same axle. Their movement is controlled by varying each wheel velocity independently. Y Trajectory
Fig. 5. Bottom view of the developed differential robot
V2 V
The transitions are converted in linear velocity (D/T ), where D is traveled distance and T the sampling time. The velocity of each wheel can be found multiplying the transitions (C) by (K), using a first order approximation (assuming that the velocity is constant during a sampling time). This operation is represented by equation (1).
D = K.C T
(1)
K converts transitions in linear velocity, following several steps. Initially, transitions are converted in linear displacement, 18 transitions corresponding to a displacement of P i*Wheel diameter. Then dividing the displacement by the sampling time, the linear velocity is obtained as shown in equation (2).
K=
π.diam (CV.T )
(2)
• CV -number of transitions by wheel turn • diam-wheel diameter
3.1.1. Schmitt trigger use motivation It is very common in mobile robotics to compare an analog signal with a threshold voltage, in order to decide if the analog signal is bigger or smaller than a threshold voltage. When the signal is near to the threshold voltage the signal fluctuations, due to noise signal, may turn the system unstable. To prevent this situation it can be used a Schmitt trigger (Mancini, August 2002).
C (x,y)
V1
X
Fig. 6. Differential robot The mechanical structure of the differential robot, presented in Figure 6, does not allow movements along the axle that cross wheel shafts (Dudek and Jenkin, 2000). Considering that there is no lateral slip (wheel velocity in the ground contact point perpendicular to the axle along the wheel shaft), the state vector given by equation (3) can be obtained. X(t)T = x(t) y(t) θ(t) v(t) w(t)
(3)
In equation (3), x(t), y(t) and θ(t) represent the point C in the plane and w(t) represents the angular velocity. Another possibility to represent the chosen state variables is using the next equation: X(t)T = x(t) y(t) θ(t) V1 (t) V2 (t)
(4)
In this case V1 (t) and V2 (t) are the velocities measured in the contact point between the wheel and the ground. There are two alternative representations, being possible to change between them using equation (5) and equation (6). In equation (6), b represents the distance between the ground contact points. v(t) =
V1 (t) + V2 (t) 2
(5)
w(t) =
V1 (t) − V2 (t) b
(6)
Considering the non slip situation the differential robot kinematics is described by the following equation: x˙ v(t)cos(θ(t)) y˙ = v(t)sin(θ(t)) w(t) θ˙
(7)
This equation allows, using equations (5) and (6), to write the linear x, ˙ y˙ and the angular θ˙ velocities using each wheel measured velocity (Choset et al., 2005). 4.2 Omnidirectional robot model The developed omnidirectional robot has 3 special wheels, that allow the robot to move in every direction. The movement of these robots does not have the restraints of the differential robots, presenting the disadvantage of a more complex control. The omnidirectional robots appeared to avoid the limitations of the commonly used differential robots, allowing movements in every direction (Huang et al., 2004). It is possible to conclude from the geometry of a three wheel omnidirectional robot, presented in Figure 7, that the velocities Vx , Vy and w vary with the linear velocities V1 , V2 and V3 , as shown in equation (8) (Kalm´ ar-Nagy et al., 2002).
the odometry calculation. The equation requires the evaluation of several sin() and cos(). It is important to optimize it, making the controller less heavier computationally, in order to fulfill real time requisites. This optimization can be achieved by using basic trigonometric equivalences, converting several sin() and cos() in just one sin() and one cos(). For the calculation of V2 , it is computationally more efficient to use equation (10) instead of equation (9). π π − θ)x˙ − cos( − θ)y˙ + Lθ˙ (9) 3 3 π π ˙ v2 = −(sin( )cos(θ) − cos( )sin(θ))x− 3 3 (10) π π (cos( )cos(θ) − sin( )sin(θ))y˙ + Lθ˙ 3 3 v2 = −sin(
The same is valid for the calculation of V3 . It is more efficient to use equation (12) instead of equation (11). π π + θ)x˙ − cos( + θ)y˙ + Lθ˙ 3 3 π π v3 = (sin( )cos(θ) + cos( )sin(θ))x− ˙ 3 3 π π (cos( )cos(θ) − sin( )sin(θ))y˙ + Lθ˙ 3 3
v3 = −sin(
(11)
(12)
5. JAVA CONTROL SOFTWARE V1
”It is amazing that they managed to squeeze a multithreading environment into what is effectively 14 kilobytes of memory. And that still leaves you with 16 kilobytes for your programs.”
Vy
V2
- Simon Ritter, Java Technology Evangelist, Sun Microsystems
Vx
L
5.1 Why Java
Y
V3 X
Fig. 7. Geometry of a three wheel omnidirectional robot −sin(θ) cos(θ) L V1 π π Vx V2 = −sin( − θ) cos( − θ) L Vy 3 3 π π V3 w −sin( + θ) cos( + θ) L 3 3 (8)
Equation (8) is used to develop the controller and also to estimate the robot position, executing
The chosen Programming Language to the Control Software was JAVA, because the Lejos API implementation is much faster than the alternative Not Quite C (NQC). It was also an extra motivation to use Java to make Control Software, using threads to implement the timers. The Control program consists in two non synchronized threads (Thread Main and Thread Tmot). The periodical events are generated placing the threads to sleep during the time left to the next event. After this time the threads wake up, make all the calculations/operations and go back to sleep.
5.2 Thread Main The main thread objective is the controller calculation. It has a sampling time associated, chosen so that the controller can be updated as fast as possible. The choice was 200 ms for the omnidirectional robot, because after several tests it was verified that the processor took between 140 and 160 ms to make the controller calculation. The 200 ms choice, was made to create a safety gap, allowing more operations without having to worry with their calculation time. The differential robot needed only 100 ms to calculate the controller.
teta=teta+V_teta*Sample_time; The code used to update the encoders transitions is: if(Sensor.S1.readBooleanValue() != Odo1) { Odo1=!Odo1; if(SpeedMotor1>0) OdoCount1++; if(SpeedMotor1<0) OdoCount1--; }
5.3 Differential robot controller
5.4 Omnidirectional robot controller
The goal of the differential robot controller is to follow a black line, while the robot position is estimated based on odometry. Initially, the control algorithm comprises the configuration and activation of the light sensor and the encoders. Such configuration, for the light sensor connected to the port 2 and the encoders connected to port 1 and 3 is obtained by using the following Java code:
The controller goal is the robot displacement to a target position with controlled velocity. Initially, a position vector, pointing to the target position is calculated. Then the position vector is normalized converting it into a velocity vector, finally equation (8) is used to calculate the velocity of each wheel in order to accomplish the final objective. Each sampling time, the position change, involves the position vector, the velocity vector and the speed reference of each motor to change.
\\light sensor configuration Sensor.S2.setTypeAndMode (3, 0x00); Sensor.S2.activate(); \\Port 1 encoder configuration OdoCount1=0; Sensor.S1.setTypeAndMode (0, 0x20); Sensor.S1.activate(); Odo1=Sensor.S1.readBooleanValue(); After the sensors configuration, the algorithm loops an infinite while cycle. The first action done in each cycle is to read the value of the infrared sensor, giving information to decide the next move. The robot position can also be estimated based on odometry calculation, using the java code shown below: Odo1=FloydMotors.OdoCount1; Odo3=FloydMotors.OdoCount3; do1=Odo1-lastOdo1; do3=Odo3-lastOdo3; lastOdo1=Odo1; lastOdo3=Odo3; v1=K*do1; v2=K*do3; cos_teta=Math.cos(teta); sin_teta=Math.sin(teta); V_y = (v1+v2)*sin_teta/2; V_teta = (v1-v2)/L; V_x = (v1+v2)*cos_teta/2; //Considering a First-Order Approximation x=x+V_x*Sample_time; y=y+V_y*Sample_time;
Some limitations were found, mainly for the omnidirectional robot prototype such as input/output number of ports, the encoders absolute error, the lag time and the impossibility of real time monitoring due to all the processing being made inside the LEGOT M Brick. With the objective of improving the controller robustness it was developed another prototype with a mechanical structure made of alumina and using AVR microcontrollers and a personal computer, its system block diagram is presented in figure 8. The controller is calculated in the PC using a Delbaud 19200 PWM PC
RS232
uCuC
H bridge
Direction
Encoder
DC Motor
Fig. 8. System block diagram of the developed prototype made with alumina structure phi application, which is communicating with a micro-controller programed in C, using the RS232 standard. By this way the lag time was reduced to less than 50 ms, the hardware was improved and it was made possible the real time monitoring of the controller performance (Gon¸calves et al., 2005).
5.5 Thread Tmot Thread Tmot objective is the encoder transition update and the motors velocity control. The received transitions maximum frequency establishes the maximum sampling time and the necessary time to make all the calculations the minimum one. The Thread Tmot calculations lasts nearly 15 ms, being the minimum sample time. The chosen sample time was 20 ms, because it allows all the calculations without losing transitions. The minimum time between transitions is nearly 40 ms, as shown in Figure 4, in which the wheel is spinning to it´s maximum speed. The motor controller present, as limitation, the difficulty in their velocity control, because the lejos API Native function Motor.A.setPower(x) presents a small speed variation for different input parameters, as shown in Figure 9. Due to this fact, it was decided to use the Floyd Steinberg Algorithm (Floyd-Steinberg, 2005), to make the Pulse Width Modulation, being a recursive algorithm commonly used in image processing. Using this approach, it was succeeded to improve the linearity of the input parameters vs motor velocity, as shown in figures 9 and 10.
Fig. 9. Native Lejos Function
Fig. 10. Floyd Steinberg Algorithm
6. CONCLUSIONS The robot positioning estimation, based on the odometry calculation, was one of the main objectives of this work, since it is a commonly used method when there is the need for a robot to
become temporarily autonomous. The robot positioning estimation is crucial to the success of the robot mission. The odometry calculation should not be the only method to estimate the robot position because the errors are cumulative, becoming bigger and bigger. Despite the hardware limitations, mainly in robustness and input/output number of ports, it is very motivating to make robots with the LEGOT M Mindstorms platform, existing several mobile robotics concepts that can be studied using this technology. Summarizing, LEGOT M Mindstorms platform is an excellent educational tool for the study of some principles of mobile robotics. It is a reusable tool, the same part can become different things depending on the application, it is very motivating to make constructions, programming and is not very expensive. Being powerful enough for senior students and easy enough for children.
REFERENCES Choset, Howie, Kevin M. Lynch, Seth Hutchinson, George Kantor, Wolfram Burgard, Lydia E. Kavraki and Sebastian Thrun (2005). Principles of Robot Motion : Theory, Algorithms, and Implementations. MIT press. Dudek, Gregory and Michael Jenkin (2000). Computational Principles of Mobile Robotics. Cambridge University Press. Educational, Moore (2005). Lego rotation sensor. Floyd-Steinberg (2005). Floyd-steinberg motor control. http://home.earthlink.net/˜ mrob/pub/lego/fsmotor.html. Gon¸calves, Jos´e, Paulo Costa and Paulo Moreira (2005). Controlo e estima¸c˜ao do posicionamento absoluto de um robot omnidireccional de trˆes rodas. Revista rob´ otica. Huang, L., Y. S. Lim, David Lee and Christopher E. L. Teoh (2004). Design and analysis of a four-wheel omnidirectional mobile robot. 2nd International Conference of Autonomous Robots and Agents. Kalm´ar-Nagy, Tam´as, Raffaello D’Andrea and Pritam Ganguly (2002). Near-optimal dynamic trajectory generation and control of an omnidirectional vehicle. Sibley School of Mechanical and Aerospace Engineering Cornell University Ithaca, NY 14853, USA. Mancini, Ron (August 2002). Op amps for everyone. Texas instruments. Pallas-Areny, Ramon and John G. Webster (1991). Sensors and Signal Conditioning. John Wiley and Sons Inc. Reshko, Grigory B., Matthew T. Mason and Ill R. Nourbakhsh (2000). Rapid prototyping of small robots. Carnegie Mellon University.
Polytechnic Institute of Bragan¸ca, Portugal {goncalves, jllima}@ipb.pt Faculty of Engineering of the University of Porto, Portugal [email protected]
Abstract: Some areas of knowledge require the application of new learning methods in order to motivate and involve the students in the learning process. This paper describes the rapid prototyping of mobile robots with the flexible, modular and educational LEGOT M Mindstorms platform, supporting activities on some mobile robotics topics like control, positioning estimation and reconfiguration. The diversity of involved areas (mechanics, electronics, control,...) illustrates quite well the inter-disciplinarity of robotics. Those who are taking their first steps in robotics, specially in mobile robotics, need tools that allow them to learn some c 2006 IFAC introductory concepts. Copyright Keywords: Control education, Prototyping, Robotics
1. INTRODUCTION In this paper the rapid prototyping and control architecture of a three wheel omnidirectional and a differential robots is described. Both were based on a LEGOT M Mindstorms platform with the purpose of studying positioning estimation and trajectory control. The presented robots use the LEGOT M DC motors with encoders, allowing position estimation based on the odometry calculation. The robotics control problems have inevitable hard real time requisites, so it is necessary to pay special attention when executing mathematical operations, they must be optimized in order to fulfill real time requisites. The paper is organized in the following sequence: Initially, Section 2 describes why use LEGOT M Mindstorms in robot prototyping. Section 3 describes the Hardware development of the two prototypes. Section 4 describes the Kinematics of the developed robots. Section 5 describes the Control
Software Architecture. Finally, Section 6 rounds up the paper with conclusions.
2. WHY USE LEGOT M MINDSTORMS Statistical studies related to scholar success present some preoccupying indicators in the learning of some areas of knowledge, namely physics and mathematics. This statement deserves a deeply reflection about the current learning methods and the way to motivate and involve the students in the learning process. A current trend in learning programs, even at high-degree studies and research activities, is applying the concepts of ’learning by doing’ and ’learning by enjoying’. In this context, the LEGOT M Mindstorms platform appears as a simple, flexible, attractive and educational way to achieve the referred challenge in several domains of knowledge. The LEGOT M Modularity makes
the rapid prototyping of different robot configurations easier (Reshko et al., 2000). This easiness presents itself as an extra motivation for the persons who are taking their first steps in the world of mobile robotics. The LEGOT M parts allow connectivity, suppressing the need of using glue or screws. Some difficulty arises while building a three wheel omnidirectional robot, due to the hardness of making connectivity of LEGOT M parts with angles different from multiples of 90o . It’s also an ecological tool because although it’s not easy to recycle plastic, the LEGOT M parts can be reused. It’s a modular tool, it is possible for the same part to be a different thing depending on the application that we want, motivating those who are using it and it is presented at a relatively low cost. In short, LEGOT M Mindstorms platform is a good educational tool to study some mobile robotics subjects.
3. HARDWARE DEVELOPMENT Attaching encoders to the wheels is a very important element, allowing the odometry calculation which has the goal of estimating the robot absolute positioning. The motors used in the construction of the prototypes are 9 Volt DC motors with internal gearbox, provided with the LEGOT M Mindstorms platform. The sensors used to measure the wheel velocity are encoders, being described in the next subsection. The first developed prototype, a differential robot, is composed by two wheels and a light sensor. Its movement is controlled by varying the velocity of each wheel independently. This is presented in Figure 1.
Fig. 2. Developed omnidirectional prototype 3.1 Encoder Schematic description The encoders, that allow the odometry update, are based in the schematic presented in Figure 3. They were developed specifically for this application because there is no alternative provided with LEGOT M Mindstorms platform Robotics Invention System 2.0. There are commercial sensors available to do this task (Educational, 2005).
Fig. 3. Encoder circuit schematic When the infrared signal is interrupted, the photo-transistor becomes unpolarized,the current in the resistance Rt is suppressed, consequently the output equals the polarization voltage (E) (Pallas-Areny and Webster, 1991). The output signal in the Photo-transistor collector, presented in Figure 4, is converted in a square wave using a Schmitt trigger.
Fig. 1. Developed differential prototype The second developed prototype is a three wheel omnidirectional robot, which is presented in Figure 2. In order to assure the omnidirectionality characteristic it is necessary that the wheels have negligible friction in the motor shaft direction. This is achieved with a special set of wheels for omnidirectional robots.
Fig. 4. Signal in the collector of the phototransistor An acetate leaf, which interrupts the infrared signal, is coupled to the motor shaft, as shown in figure 5. Each transition corresponds to an angular displacement.
4. KINEMATICS OF THE DEVELOPED PROTOTYPES 4.1 Differential robot model The differential robot is probably the most used mobile robot (presented in Figure 6). The differential robot is composed by two wheels, with their shaft passing by the same axle. Their movement is controlled by varying each wheel velocity independently. Y Trajectory
Fig. 5. Bottom view of the developed differential robot
V2 V
The transitions are converted in linear velocity (D/T ), where D is traveled distance and T the sampling time. The velocity of each wheel can be found multiplying the transitions (C) by (K), using a first order approximation (assuming that the velocity is constant during a sampling time). This operation is represented by equation (1).
D = K.C T
(1)
K converts transitions in linear velocity, following several steps. Initially, transitions are converted in linear displacement, 18 transitions corresponding to a displacement of P i*Wheel diameter. Then dividing the displacement by the sampling time, the linear velocity is obtained as shown in equation (2).
K=
π.diam (CV.T )
(2)
• CV -number of transitions by wheel turn • diam-wheel diameter
3.1.1. Schmitt trigger use motivation It is very common in mobile robotics to compare an analog signal with a threshold voltage, in order to decide if the analog signal is bigger or smaller than a threshold voltage. When the signal is near to the threshold voltage the signal fluctuations, due to noise signal, may turn the system unstable. To prevent this situation it can be used a Schmitt trigger (Mancini, August 2002).
C (x,y)
V1
X
Fig. 6. Differential robot The mechanical structure of the differential robot, presented in Figure 6, does not allow movements along the axle that cross wheel shafts (Dudek and Jenkin, 2000). Considering that there is no lateral slip (wheel velocity in the ground contact point perpendicular to the axle along the wheel shaft), the state vector given by equation (3) can be obtained. X(t)T = x(t) y(t) θ(t) v(t) w(t)
(3)
In equation (3), x(t), y(t) and θ(t) represent the point C in the plane and w(t) represents the angular velocity. Another possibility to represent the chosen state variables is using the next equation: X(t)T = x(t) y(t) θ(t) V1 (t) V2 (t)
(4)
In this case V1 (t) and V2 (t) are the velocities measured in the contact point between the wheel and the ground. There are two alternative representations, being possible to change between them using equation (5) and equation (6). In equation (6), b represents the distance between the ground contact points. v(t) =
V1 (t) + V2 (t) 2
(5)
w(t) =
V1 (t) − V2 (t) b
(6)
Considering the non slip situation the differential robot kinematics is described by the following equation: x˙ v(t)cos(θ(t)) y˙ = v(t)sin(θ(t)) w(t) θ˙
(7)
This equation allows, using equations (5) and (6), to write the linear x, ˙ y˙ and the angular θ˙ velocities using each wheel measured velocity (Choset et al., 2005). 4.2 Omnidirectional robot model The developed omnidirectional robot has 3 special wheels, that allow the robot to move in every direction. The movement of these robots does not have the restraints of the differential robots, presenting the disadvantage of a more complex control. The omnidirectional robots appeared to avoid the limitations of the commonly used differential robots, allowing movements in every direction (Huang et al., 2004). It is possible to conclude from the geometry of a three wheel omnidirectional robot, presented in Figure 7, that the velocities Vx , Vy and w vary with the linear velocities V1 , V2 and V3 , as shown in equation (8) (Kalm´ ar-Nagy et al., 2002).
the odometry calculation. The equation requires the evaluation of several sin() and cos(). It is important to optimize it, making the controller less heavier computationally, in order to fulfill real time requisites. This optimization can be achieved by using basic trigonometric equivalences, converting several sin() and cos() in just one sin() and one cos(). For the calculation of V2 , it is computationally more efficient to use equation (10) instead of equation (9). π π − θ)x˙ − cos( − θ)y˙ + Lθ˙ (9) 3 3 π π ˙ v2 = −(sin( )cos(θ) − cos( )sin(θ))x− 3 3 (10) π π (cos( )cos(θ) − sin( )sin(θ))y˙ + Lθ˙ 3 3 v2 = −sin(
The same is valid for the calculation of V3 . It is more efficient to use equation (12) instead of equation (11). π π + θ)x˙ − cos( + θ)y˙ + Lθ˙ 3 3 π π v3 = (sin( )cos(θ) + cos( )sin(θ))x− ˙ 3 3 π π (cos( )cos(θ) − sin( )sin(θ))y˙ + Lθ˙ 3 3
v3 = −sin(
(11)
(12)
5. JAVA CONTROL SOFTWARE V1
”It is amazing that they managed to squeeze a multithreading environment into what is effectively 14 kilobytes of memory. And that still leaves you with 16 kilobytes for your programs.”
Vy
V2
- Simon Ritter, Java Technology Evangelist, Sun Microsystems
Vx
L
5.1 Why Java
Y
V3 X
Fig. 7. Geometry of a three wheel omnidirectional robot −sin(θ) cos(θ) L V1 π π Vx V2 = −sin( − θ) cos( − θ) L Vy 3 3 π π V3 w −sin( + θ) cos( + θ) L 3 3 (8)
Equation (8) is used to develop the controller and also to estimate the robot position, executing
The chosen Programming Language to the Control Software was JAVA, because the Lejos API implementation is much faster than the alternative Not Quite C (NQC). It was also an extra motivation to use Java to make Control Software, using threads to implement the timers. The Control program consists in two non synchronized threads (Thread Main and Thread Tmot). The periodical events are generated placing the threads to sleep during the time left to the next event. After this time the threads wake up, make all the calculations/operations and go back to sleep.
5.2 Thread Main The main thread objective is the controller calculation. It has a sampling time associated, chosen so that the controller can be updated as fast as possible. The choice was 200 ms for the omnidirectional robot, because after several tests it was verified that the processor took between 140 and 160 ms to make the controller calculation. The 200 ms choice, was made to create a safety gap, allowing more operations without having to worry with their calculation time. The differential robot needed only 100 ms to calculate the controller.
teta=teta+V_teta*Sample_time; The code used to update the encoders transitions is: if(Sensor.S1.readBooleanValue() != Odo1) { Odo1=!Odo1; if(SpeedMotor1>0) OdoCount1++; if(SpeedMotor1<0) OdoCount1--; }
5.3 Differential robot controller
5.4 Omnidirectional robot controller
The goal of the differential robot controller is to follow a black line, while the robot position is estimated based on odometry. Initially, the control algorithm comprises the configuration and activation of the light sensor and the encoders. Such configuration, for the light sensor connected to the port 2 and the encoders connected to port 1 and 3 is obtained by using the following Java code:
The controller goal is the robot displacement to a target position with controlled velocity. Initially, a position vector, pointing to the target position is calculated. Then the position vector is normalized converting it into a velocity vector, finally equation (8) is used to calculate the velocity of each wheel in order to accomplish the final objective. Each sampling time, the position change, involves the position vector, the velocity vector and the speed reference of each motor to change.
\\light sensor configuration Sensor.S2.setTypeAndMode (3, 0x00); Sensor.S2.activate(); \\Port 1 encoder configuration OdoCount1=0; Sensor.S1.setTypeAndMode (0, 0x20); Sensor.S1.activate(); Odo1=Sensor.S1.readBooleanValue(); After the sensors configuration, the algorithm loops an infinite while cycle. The first action done in each cycle is to read the value of the infrared sensor, giving information to decide the next move. The robot position can also be estimated based on odometry calculation, using the java code shown below: Odo1=FloydMotors.OdoCount1; Odo3=FloydMotors.OdoCount3; do1=Odo1-lastOdo1; do3=Odo3-lastOdo3; lastOdo1=Odo1; lastOdo3=Odo3; v1=K*do1; v2=K*do3; cos_teta=Math.cos(teta); sin_teta=Math.sin(teta); V_y = (v1+v2)*sin_teta/2; V_teta = (v1-v2)/L; V_x = (v1+v2)*cos_teta/2; //Considering a First-Order Approximation x=x+V_x*Sample_time; y=y+V_y*Sample_time;
Some limitations were found, mainly for the omnidirectional robot prototype such as input/output number of ports, the encoders absolute error, the lag time and the impossibility of real time monitoring due to all the processing being made inside the LEGOT M Brick. With the objective of improving the controller robustness it was developed another prototype with a mechanical structure made of alumina and using AVR microcontrollers and a personal computer, its system block diagram is presented in figure 8. The controller is calculated in the PC using a Delbaud 19200 PWM PC
RS232
uCuC
H bridge
Direction
Encoder
DC Motor
Fig. 8. System block diagram of the developed prototype made with alumina structure phi application, which is communicating with a micro-controller programed in C, using the RS232 standard. By this way the lag time was reduced to less than 50 ms, the hardware was improved and it was made possible the real time monitoring of the controller performance (Gon¸calves et al., 2005).
5.5 Thread Tmot Thread Tmot objective is the encoder transition update and the motors velocity control. The received transitions maximum frequency establishes the maximum sampling time and the necessary time to make all the calculations the minimum one. The Thread Tmot calculations lasts nearly 15 ms, being the minimum sample time. The chosen sample time was 20 ms, because it allows all the calculations without losing transitions. The minimum time between transitions is nearly 40 ms, as shown in Figure 4, in which the wheel is spinning to it´s maximum speed. The motor controller present, as limitation, the difficulty in their velocity control, because the lejos API Native function Motor.A.setPower(x) presents a small speed variation for different input parameters, as shown in Figure 9. Due to this fact, it was decided to use the Floyd Steinberg Algorithm (Floyd-Steinberg, 2005), to make the Pulse Width Modulation, being a recursive algorithm commonly used in image processing. Using this approach, it was succeeded to improve the linearity of the input parameters vs motor velocity, as shown in figures 9 and 10.
Fig. 9. Native Lejos Function
Fig. 10. Floyd Steinberg Algorithm
6. CONCLUSIONS The robot positioning estimation, based on the odometry calculation, was one of the main objectives of this work, since it is a commonly used method when there is the need for a robot to
become temporarily autonomous. The robot positioning estimation is crucial to the success of the robot mission. The odometry calculation should not be the only method to estimate the robot position because the errors are cumulative, becoming bigger and bigger. Despite the hardware limitations, mainly in robustness and input/output number of ports, it is very motivating to make robots with the LEGOT M Mindstorms platform, existing several mobile robotics concepts that can be studied using this technology. Summarizing, LEGOT M Mindstorms platform is an excellent educational tool for the study of some principles of mobile robotics. It is a reusable tool, the same part can become different things depending on the application, it is very motivating to make constructions, programming and is not very expensive. Being powerful enough for senior students and easy enough for children.
REFERENCES Choset, Howie, Kevin M. Lynch, Seth Hutchinson, George Kantor, Wolfram Burgard, Lydia E. Kavraki and Sebastian Thrun (2005). Principles of Robot Motion : Theory, Algorithms, and Implementations. MIT press. Dudek, Gregory and Michael Jenkin (2000). Computational Principles of Mobile Robotics. Cambridge University Press. Educational, Moore (2005). Lego rotation sensor. Floyd-Steinberg (2005). Floyd-steinberg motor control. http://home.earthlink.net/˜ mrob/pub/lego/fsmotor.html. Gon¸calves, Jos´e, Paulo Costa and Paulo Moreira (2005). Controlo e estima¸c˜ao do posicionamento absoluto de um robot omnidireccional de trˆes rodas. Revista rob´ otica. Huang, L., Y. S. Lim, David Lee and Christopher E. L. Teoh (2004). Design and analysis of a four-wheel omnidirectional mobile robot. 2nd International Conference of Autonomous Robots and Agents. Kalm´ar-Nagy, Tam´as, Raffaello D’Andrea and Pritam Ganguly (2002). Near-optimal dynamic trajectory generation and control of an omnidirectional vehicle. Sibley School of Mechanical and Aerospace Engineering Cornell University Ithaca, NY 14853, USA. Mancini, Ron (August 2002). Op amps for everyone. Texas instruments. Pallas-Areny, Ramon and John G. Webster (1991). Sensors and Signal Conditioning. John Wiley and Sons Inc. Reshko, Grigory B., Matthew T. Mason and Ill R. Nourbakhsh (2000). Rapid prototyping of small robots. Carnegie Mellon University.