Development and Simulation on V-REP of an Algorithm for the BNT

Transcription

1 2014 IEEE International Conference on Development and Simulation on V-REP of an Algorithm for the BNT Rui Filipe Teixeira Alen Department of Electrical Engineering, School of Engineering, Polytechnic of Porto, Rua Dr. António Bernardino de Almeida, Porto, Portugal Manuel F. Silva INESC TEC INESC Technology and Science (formerly INESC Porto) and ISEP/IPP School of Engineering, Polytechnic of Porto, Rua Dr. António Bernardino de Almeida, Porto, Portugal Abstract Recently the league emerged in the world's largest robotics competition, intended for competitors wishing to compete in the field of mobile robotics for manipulation tasks in industrial environments. This competition consists of several tasks with one reflected in this work (Basic Navigation Test). This project involves the simulation in Virtual Robot Experimentation Platform (V-REP) of the behavior of a KUKA youbot. The goal is to verify that the robots can navigate in their environment, in a standalone mode, in a robust and secure way. To achieve the proposed objectives, it was necessary to create a program in Lua and test it in simulation. This involved the study of robot kinematics and mechanics, Simultaneous Localization And Mapping (SLAM) and perception from sensors. In this work is introduced an algorithm developed for a KUKA youbot platform to perform the SLAM while reaching for the goal position, which works according to the requirements of this competition BNT. This algorithm also minimizes the errors in the built map and in the path travelled by the robot. Keywords SLAM, youbot, RoCKIn, Robot, RoboCup. I. INTRODUCTION Each year several robotic competitions take place around the world, in order to foster robotics development and to attract young people to this scientific area. Recently, a new league emerged in the largest world robotics competition, called RoboCup@Work, intended to competitors who wish to compete in the field of mobile robotics in industrial environment. This competition is defined by multiple tasks, or tests, namely Basic Manipulation Test (BMT), Basic Transportation Test (BTT), Conveyor Belt Test (CBT), Precision Placement Test (PPT) and Basic Navigation Test (BNT), on which this paper will focus [1]. The main objective of this work is the development of an algorithm to allow the KUKA youbot robot (used by all teams participating in this competition) perform the BNT task and simulate its execution in the V-REP application [2]. This step consists on checking if the robot can navigate in its environment, in an autonomous mode, in a robust and safe way. To achieve the proposed objectives, it was created a program on the Lua [3] programming language, which was tested in the V-REP simulator. This involves the study of the mechanics and kinematics of the robot, its SLAM and perception from sensors. Bearing these ideas in mind, this paper is organized as follows. In Section II is presented the state of the art. The adopted system architecture is introduced in Section III, and Section IV describes the adopted solutions and their implementation. Next, Section V describes the tests performed and the results obtained. Finally, Section VI is devoted to present the main conclusions of this work and presents several ideas for possible future developments. II. STATE OF THE ART In this section, the scientific background of this work is addressed through four sections, namely: mobile robot competitions, mobile robots, simulation in robotics and planning and navigation. A. Mobile robot competitions There are several national and international robotic competitions, among which RoboCup [4]. This is an annual competition, whose aim is the study and development of artificial intelligence and robotics, providing challenges and problems where multiple technologies and methodologies can be combined to obtain the best results. RoboCup@Work is a new RoboCup league in the field of mobile robotics manipulation for industrial environments. Within this competition there are several tasks, performed in an arena (with one or more service areas), each one with a specific purpose for a given job [5]. The BNT serves to check if the robots can navigate correctly within the arena, in an oriented, autonomous and safe way. For its execution, is sent to the robot a line of text (string) specifying the location, orientation and pause duration of the robot on a goal [5]. B. Mobile robots In the past, heavy and expensive computers had to be connected by cable to control mobile robots. Today it is possible to build mobile robots with small dimensions, with numerous low cost actuators and sensors, and with embedded systems. There has been a huge increase of interest in mobile /14/$ IEEE 315

2 2014 IEEE International Conference on robots, not only as toys but also as an excellent tool for education in engineering. Currently mobile robots are used in several courses and post-graduate degrees in Computer Science, Computer Engineering, Information Technology, Cybernetics, Electrotechnical Engineering, Mechanical Engineering and Mechatronics [6]. The participation in the is the final goal of this work, and the organization of the event is very strict in relation to the type of platform to be used by the participants [5]. The choice of the robot for this participation falls on the KUKA youbot [7] platform (Fig. 1), currently recognized and accepted by the organization. The youbot is an omnidirectional mobile platform, with four Swedish wheels with 45º rollers. This means that the robot can move in any direction. Fig. 1. KUKA youbot with the arm/manipulator in home position [7]. C. Simulation in robotics Simulation is a method used to test ideas, theories and, in this case, software and behaviors of the robots. It is advisable, before starting a simulation, to decide what can be simplified and what has necessarily to be realistic. Currently, there are on the market, several open source simulators, among which can be mentioned Gazebo [8], Webots [9], USARSim [10]. V-REP (from Coppelia Robotics) [2] is a simulator with numerous functions, features or APIs. It is used for the development of algorithms, simulations, and in industrial robotics. It was chosen for this work since it currently has a free full license for education purposes, with all the characteristics of the commercial version, and also includes, on its library, the model of the KUKA youbot robot. Also, V-REP is a cross-platform with six programming approaches that are mutually compatible and that can even work hand-in-hand. Other simulation works can be found in the youbot GitHub repositories and contributions [11]. D. Navigation and planning The first thing to do, to perform the navigation in the best possible way, is the decomposition of obstacles in a discrete map. After this step algorithms can be used in order to avoid obstacles in navigation and determine the shortest path/route until the goal. There are three main strategies for decomposing the space or surrounding environment: map of routes, decomposition into cells and potential field. Regardless of the strategy used to obtain reliable results, the robot must not only create a map, but must do so while moving and exploring the surrounding space. This concept is known as SLAM. The implementation of SLAM is difficult due to the interaction between the robot position and its actions of mapping. If the position of the robot is inaccurate, the position of the obstacle on the map is correlated with this imprecision [12]. The approximate cell decomposition is one of the most popular techniques for planning trajectories in mobile robotics. This is in part due to the popularity of environmental representations, based on systems of grids (pixels). The decomposition into fixed-length cells is the most common technique and more currently used in mobile robotics for map representation and navigation planning. The cell size does not depend on the objects in the environment, and possibly, in narrow passages may lose information, due to the inaccuracy of the grid. In practical terms, this is rarely a problem, due to the size of cell used, which is usually very small. The greatest advantage of fixed size cellular decomposition is the low computational burden of navigation planning [12]. The algorithm of the transformation of distance, also known as wave front or methods of forest fires, is widely used in cell decomposition and planning the trajectory/exact path. It is based on a matrix that takes into account the proximity of obstacles and generates continuous interpolations of several directions in a gradual manner, at any point in the array. With this type of planning, the important is the ability of the robot to navigate from its current position (a free cell) to a possible adjacent free cell with the smallest distance value to goal [12]. III. SYSTEM ARCHITECTURE The model of the youbot robot used in the simulations is provided on V-REP, whit several scripts attached. To maintain the software reusable, the code of the robot was kept and, among it, were inserted the blocks: User interface, Kinematic, Perception and location, Planning and navigation and Graph. With the purpose of saving CPU processing time, the blocks of Perception and location, Planning and navigation and Graph, are only handled in case there is any goal or task. Fig. 2 describes this model, and the relationships between the blocks. Fig. 3 is a flowchart of the program implemented in the youbot script, depicting the necessary cycle conditions for the robot to navigate within the main objectives for the task. IV. IMPLEMENTATION In this section (divided into subsections corresponding to each block of Figure 2) a brief approach is presented on how the changes in the script for the youbot were made, for this to be capable of performing the BNT, without deleting the code in the script. A. User interface For the start of the BNT, the user must enter a formatted string or triplet, e.g. <(location, orientation, pause)>, by a command line on a user interface, as depicted in Fig. 4. The location corresponds to one of the specific areas of the arena, e.g. S1. Orientation could be N; S; E or W. The pause is a digit between 1 and 3 and indicates (in seconds) how much time should the robot stay in pause before starting the next task in the string. 316

3 2014 IEEE International Conference on In order to avoid errors, the algorithm that performs the reading of these characters, must convert the entire text to uppercase. The user can send more than one series of triplets. In this case, the information must be stored in a data matrix as depicted in Fig. 5. After a goal is reached, and the pause time ended, the line of the matrix corresponding to the actual goal must be deleted, so the robot can proceed to the next objective. Fig. 5. Matrix to store information of the strings sent to the robot. Fig. 2. Block diagram of the code blocks and their integration into the script of the youbot model. B. Kinematics This block is responsible for determining the speed of the robot in the inertial frame. It is important that the values obtained here, are as accurate as possible. The first step for a kinematic model of the robot is to express restrictions on the movement of individual wheels. Thus, the movement of these can be combined to calculate the movement of the robot as a whole. It is assumed that the plane of the wheel remains always upright, that there is always a single point of contact between the wheel and the ground plane, and that there are no external aids to this [12]. If it is considered the chassis of the robot as depicted in Fig. 6, the equations of velocity are determined according to (1). R is the radius of the wheel (0.05 m), and ω i is the angular velocity obtained from the encoders of the wheels [14]. ] [ ] [ ] Fig. 3. Flowchart of the youbot script in V-REP. Fig. 4. User interface created in V-REP for the string command. Fig. 6. Important dimensions of the youbot in the local and inertial frames. 317

4 2014 IEEE International Conference on The kinematic model determined in (1) is not accurate and presents errors. After performing several tests in V-REP, the linearization of the transversal and longitudinal average velocities, depending on the speed variables, was determined. This is shown in the equations presented on this page footnote. To obtain the corrected longitudinal velocity, substitutions into (2), (3) and (4) are necessary, according to the values previously obtained. The corrected transversal velocity can be found by the replacements of (5), (6) and (7). The inclusion of a gyroscope helps to determine the angular velocity of the robot with greater accuracy. Comparing this velocity with the rotational speed obtained in (1), the gyroscope speed information is better, and for the rest of the simulation this method is used as default. To perform the conversion of the corrected transversal and longitudinal velocities to the inertial frame, it is necessary to determine the angle θ (Fig. 6), according to (8). The integration interval in the simulation is the predefined time step of 0.05 s. Equation (9) gives the model to convert the velocities [12]. It is not necessary to convert the rotational speed because this is the same in the inertial frame. ] [ ] [ ] [ [ C. Perception and location After obtaining the velocities of the robot in the inertial frame, it is necessary to determine its position on a theoretical frame, and determine also the obstacles. The robot position is obtained through integration of its velocities previously determined, according to (10). ] [ ] KUKA sells a youbot model with the Hokuyo URG-04LX- GU01 LASER rangefinder sensor [15] (Fig. 7). The model of this sensor was attached in the front of the simulated youbot, to perform the perception of the surrounding environment. Fig. 7. Hokuyo URG-04LX-GU01 LASER rangefinder sensor [15]. This sensor sends to the youbot script an array with the coordinates of the obstacles detected, in a range of 240º, in relation to the sensor reference. To convert these values to the robot reference (geometric center of the platform) a conversion is required according to (11) and (12). being SL n the distance of the laser to the obstacle along the x- axis, and CL n the distance along the y-axis. As (12) shows, it is necessary to add the distance of the center of the robot to the center of the LASER, dr L, to obtain the correct y-axis distance. After obtaining the position of the obstacles with the Hokuyo LASER sensor, this information is saved in a discrete matrix, where value 1 represents cells with obstacles and free cells are coded as value 0. The coordinates for the insertion of the data, index (i) on the matrix and distance values (n) can be obtained using (13) and (14). The number of columns and rows of the matrix (M) must be two times larger than the absolute map size prediction in cm, because the initial position of the robot is in the middle of the matrix. The map resolution is obtained by the cell size (c) also in cm. ( ) ( ) ( ) With the purpose of saving system processing resources in terms of cells of arrays/maps that have not suffered any type of modification, a reduced size of the matrix is determined, as shown in Fig. 8. For a safe navigation of the robot is added a safe margin in the map, based on a circle [16] around the obstacle, so that when calculating the navigation path it takes into consideration the distance r, the biggest radius of the robot, so it does not collide with obstacles. D. Planning and navigation For the robot to be able to navigate, it is necessary to establish a path (as short as possible) between its initial position and the goal position. Knowing the current position of the robot in the global reference, a new matrix is build, containing the distance of each cell to the destination, in order to bypass the obstacles with the respective margin of safety. This matrix is determined according to the Jarvis distance transform algorithm with a Euclidean scan window [17], as presented in Fig. 9. ( ). ( ) ( ) ( ) ( ) ( ) ( ) ( ). ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) 318 ( ) ( ) ( ).

5 2014 IEEE International Conference on With this transformation, the matrix is reduced to a map as shown in Fig. 10, where each position has the respective distance from its cell to the goal cell. This can be graphically displayed, on a color scale, where the cells are darker as the distance to the goal is greater. In addition, the barriers or the safety area, are presented in black. ] [ ] Fig. 11. Diagram of decision to navigation. Fig. 8. Example of a reduced map with the creation of the security circle. Fig. 9. Scan window used by the Jarvi s distance transform algorithm [17]. Fig. 10. Navigation map represented in color scale. The rotational, longitudinal and transversal velocities are determined by the smaller angular difference between the current position of the robot and the desired orientation. This angle θ is shown in Fig. 11. Here CW corresponds to Clockwise and CCW to Counter Clockwise rotation, respectively. Figure 12 shows the implementation of (15), which is responsible for determining the velocities of the robot according to θ. Fig. 12. Speed curves of the robot according to CW or CCW rotation needed. This expression computes the derivative of the desirable velocity ( ) by the previous velocity sample ( ) by a factor 1.4. According to tests performed, the curves obtained from these equations have a slope similar to the acceleration of the youbot in V-REP, as can be seen in Fig. 13. ] [ [ ( ) ] [ ( ) ] [ ( ) ] ] [ ] The speed change imposed by the algorithm mentioned above is abrupt. To smooth this impulse was used a smoothing function, according to (16). Fig. 13. Example of smoothing the change of speed imposed on the youbot. 319

6 2014 IEEE International Conference on E. Graph Although not critical to the simulation, graphics are an advantage for the user to view the map of obstacles, security margins, and location of the goal position(s). This code is only executed if a goal exists, as depicted in Fig. 3; otherwise there will be an excessive consumption of CPU resources. V. PERFORMED TESTS AND RESULTS OBTAINED Several tests were made in V-REP, mainly regarding the kinematics, location and mapping. For the realization of the experimental tests, was created the scenario presented in Fig. 14, with walls surrounding the useful space, leaving only one opening for the robot to access the arena. study that the determination of the robot accurate position is central to the achievement of a correct mapping and, therefore, for the planning of navigation. Although the construction of the obstacles map has fulfilled the desired requirements, and the robot has achieved the expected objectives, it was found during the experiments performed, that the fixed positions on the map are changing according to the errors of the odometry and errors inherent to the reading of the Hokuyo LASER rangefinder sensor. In the future this situation can be corrected using probabilistic (instead of deterministic) maps. It was also observed that navigation in multiples of 45º is restrictive and requires a greater demand on the processor to perform the computations necessary. This can be improved by applying the Exact Euclidean Distance Transform algorithm [17]. VII. ACKNOWLEDGEMENTS This work is funded (or part-funded) by the ERDF- European Regional Development Fund through the COMPETE Programme (operational programme for competitiveness) and by National Funds through the FCT - Fundação para a Ciência e a Tecnologia (Portuguese Foundation for Science and Technology) within project FCOMP FEDER Fig. 14. Scenario mounted in V-REP. With the implementation of the algorithm for the velocities error correction, it was possible to obtain an improvement around 95%, in relation to the case where the speeds were determined only by kinematics, as shown in Fig. 15, by comparing the curves of with. Fig. 15. Longitudinal velocity comparision. VI. CONCLUSIONS This paper presented an algorithm for the implementation of the RoboCup@Work BNT competition, which allows a KUKA youbot platform to perform this task while minimizing the trajectory errors and performing the SLAM while reaching the goal position. Concerning the calculation of the platform kinematics, although not being accurate, it was found in V-REP that with the correction algorithm described in this paper, there can be a better approximation to reality. It was observed during this REFERENCES [1] RoboCup@Work, available: [2] V-REP, available: [3] Lua, available: [4] RoboCup, available: [5] RoboCup@Work Rule ook, Version 201.1; June 0, 201, available on: -0.pd, [6] R. D. eer, H. J. Chiel, and R. F. Drushel, Using autonomous robotics to teach science and engineering, Communications o the ACM, vol. 42, no. 6, pp , June [7] KUKA, available: [8] Gazebo, available: [9] Webots, available: [10] USARsim, available: [11] GitHub, available: [12] Siegwart, Roland; R. Nourbakhsh, Illah, Introduction to Autonomous Mobile Robots, IS N X. [13] KUKA you ot User Manual, Version 1.02, Locomotec, available: [14] Ioan Doroftei, Victor Grosu and Veaceslav Spinu (2007). Omnidirectional Mobile Robot - Design and Implementation, Bioinspiration and Robotics Walking and Climbing Robots, Maki K. Habib (Ed.), ISBN: , InTech, pp [15] Hokuyo, available: [16] Gustavson, Ste an, An e icient circle drawing algorithm, available: [17] Juan Carlos Elizondo-Leal, Ezra Federico Parra-Gonz lez and Jos Gabriel Ram rez-torres. The Exact Euclidean Distance Transform: A New Algorithm for Universal Path Planning. Int J Adv Robot Syst, 2013, 10:266. doi: /