Emil Dumitrescu, Sanda Paturca, Mariana Ilas, Constantin Ilas

OPTIMAL PATH COMPUTATION FOR MOBILE ROBOTS USING MATLAB EMIL DUMITRESCU*, SANDA PATURCA*, MARIANA ILAS**, CONSTANTIN ILAS* Politehnica University of Bucharest Splaiul Independentei 313 *Department of Electrical Engineering **Department of Electronics Engineering e-mail: emdumi@yahoo.com, sanda.paturca@upb.ro, constantin.ilas@upb.ro
Abstract: In this article we describe a mobile robot control method based on the optimal path computation for a known environment. The movement control is implemented on the robot, while the algorithm for determining the optimal path is implemented in Matlab. The robot interrogates the PC for getting the coordinates of points on the reference trajectory. The communication between the robot and the PC is via Bluetooth connection and is controlled by the robot. The method for optimal path computation is based on creating a visibility graph and applying Dijkstra's algorithm. Keywords: robot remote control, optimal path, visibility graph. 1 INTRODUCTION The problem of finding the optimal paths in environments cluttered with obstacles is extremely important for autonomous mobile robots. A common particular situation is when the environment in which the robot operates is known and supplied to the path planner [3] - [7]. This is the case in either time invariant environments or environments that partially change in time and the robot is able to detect the changes. Examples include home cleaning robots and security robots, which move within a distinct perimeter with a known map. For such applications the map is either supplied to the robot or is determined by the robot. In this paper we consider that the map is known and we focus on determining the optimal navigation path between two given points within an environment that is populated with arbitrarily polygonal obstacles, randomly placed. Of course, by iteratively computing the solution of this problem for different points we can determine the optimal path for a complex movement that has to include a pre-defined set of intermediate points. The algorithm that we developed is based on the visibility graphs. The visibility graph is an important combinatorial structure in computational geometry and it is used in applications such as computing shortest paths [8]. The algorithm is implemented in Matlab, running on a PC and it is based on a known map of the environment. When the robot needs to move between two points it submits the points coordinates to the Matlab program on the PC and receives the optimal trajectory. In this way the robot can be regarded as an autonomous robot that uses the PC as a computation accelerator. The communication between the robot and the PC is implemented using Bluetooth. By using this type of communication we can develop very complex motion planning algorithms running on the PC while the robot computer controls the robot actions, including its movement. The robot maintains full control on when and where it moves to, while the PC is used as a computation device. 2 OPTIMAL PATH COMPUTATION As mentioned, the algorithm for computing the optimal path between a start and a destination point is based on finding the visibility graph for a set of polygonal obstacles existing between those points. Once the graph is determined, we apply the Dijkstra's algorithm to compute the shortest path. The visibility graph represents all the possible connections between the vertices (polygons corners) that do not intersect any polygon edge. In other words, the visibility graph represents the entire set of possible trajectories for the robot. An example of an arbitrarily environment and a pair of robot start and destination points is illustrated in figure 1.

Fig. 1 Example of a given environment

Robotica & Management, 14-2 / 2009

Optimal Path Computation for Mobile Robots Using MATLAB eliminated. This is illustrated in figure 2, where there is no path from B to A (or from A to B).

While constructed the visibility graph we took into account the fact that the robot cannot move on a path consisting of segments that go exactly from one corner to the other, because the robot needs a space for changing its direction. This space depends on the robot size and if not respected it will lead to the robot colliding with the obstacles. To cope with this problem our algorithm generates a contour around each polygon, at a safe distance (around 2/3 of the robot's length). This contour is processed as an extended polygon, which is used while constructing the visibility graph. These extended polygons are also useful for checking that the robot can pass between two obstacles. Thus, if two extended boundaries intersect, it means that there is not enough space for the robot to pass on that area and the algorithm will discard that segment when computing the visibility graph. To generate the extended contour around the obstacle the algorithm uses the coordinates of the mass center of the polygon. This is computed using the following formulas:
Cx = 1 [(xi + xi+1 )(xi yi+1 xi+1 yi )] 6 A i =0

n 1

Fig. 2 The visibility graph (2.1) The graph is constructed using the distances between vertices. The graph nodes represent the corners while the segments represent the paths between them. Using the generated graph, we apply the Dijkstra's algorithm that solves the shortest path problem. The algorithm is performed in MATLAB environment. MATLAB is a powerful software development tool and can dramatically reduce the programming workload during the period of algorithm development.

1 Cy = [( yi + yi+1 )(xi yi+1 xi+1 yi )] 6 A i =0

n 1

where A is the area of the polygon:

A= 1 (xi yi xi +1 yi ) 2 i =0
n 1

(2.2)

After generating the contours, we created the visibility graph, represented by all the possible paths between vertices. The free-space graph is constructed with line segments connecting the vertices of the expanded polygons, excluding the segments that intersect a polygon contour. To determine whether a segment intersects a polygon we use the following relation:
1 x1 y1 1 x1 y1 1 x2 y2 1 x3 y3 1 1 1 x2 y2 1 x3 y3 1 x4 0 y4 1 x4 0 y4

x3 * x1 y3 y1 1 1 x4 * x2 y4 y2

&&

(2.3)

where (x1, y1 ) , (x2 , y2 ) represent the coordinates of the segment limits while the (x3 , y3 ) , (x4 , y4 ) are the coordinates of a polygon edge limits. To reduce the total number of paths the algorithm discharges the paths that are redundant. For example, in the case of the vertices that can be directly connected to the destination point all connections to other vertices are

Fig. 3 The shortest path solution The final solution of the shortest path problem is presented in figure 3. Our algorithm returns a list of

Robotica & Management, 14-2 / 2009

Emil Dumitrescu, Sanda Paturca, Mariana Ilas, Constantin Ilas vertices representing the shortest path from target to source (indicated by numbers 1-5 in figure 3). These vertices represent the reference trajectory for the mobile robot. 3 EXPERIMENTAL TESTS We implemented and tested the algorithm described in section 1 using a Lego Mindstorms NXT robotic kit. The robot we constructed has a differential drive, with two parallel drive wheels, powered separately, and a caster. It also has an ultrasonic (US) sensor to detect obstacles. This is used to determine if the robot is approaching the obstacle area. The picture of the robot is shown in figure 4. Each motor that drives one of the wheels has incorporated an optical encoder which we used for tracking the robot position.

Fig. 4 The NXT robot used for experimental tests Using Matlab for implementing the algorithm creates two possible test scenarios with an NXT robot. One is to use Matlab for controlling the robot movement, through Direct Commands that are sent to the robot. The other is to use Matlab for computing the optimal path, and then send the coordinates of this path to the robot that will control the movement. The first is simpler to implement, but has some drawbacks, such as the impossibility of monitoring the robot sensors in real time, due to the latency of the Bluetooth connection. Therefore we implemented the second scenario. In this scenario the robot is the master and uses the PC as a computational accelerator. The embedded program that runs on the robot controls the movement as well as the communication with the PC. Thus, complex algorithms and computationally intensive applications are executed on the PC, while the real time control and data acquisition process is executed on the robot. This kind of interaction allows each device to perform the task it is best at, achieving a more efficient system. The embedded program, running on the robot, is written in NXC (Not eXactly C), a programming language similar to C, developed for the NXT robots. The functionality of the implemented tests is as follows:

Once approaching the obstacle area the robot sends to PC its location and the desired destination. The algorithm, implemented in MATLAB computes the optimal path between the two points (based on the obstacle map that is available to the algorithm) and sends back to the robot the coordinates of the vertices on the optimal path. These coordinates are sent to the NXT robot through a Bluetooth connection [13] and represent the reference trajectory for the robot. Using the encoders of the servo motors, we generated a tracking system to measure both traveled distances and angles. We used the relative positioning, which doesn't require the robot to know the environment map. It deduces its current position from its previous position. This is achieved through the use of encoders that precisely monitor the turns of the motors. There are two types of motion the robot needs to perform in order to reach the destination point: moving forward and pivoting (changing direction). For moving the robot a specified distance, we need to convert the distance into degrees depending on the wheel circumference. To make the robot to pivot, the program calculates the number of degrees to spin the motors, using the distance between the two powered wheels. For pivoting the motors spin in opposite directions. In the experimental tests we performed, the NXC program records the degrees of each motor rotation, by reading the data from encoders. These values are sent back to the PC. The Matlab program converts the degrees into distances and then it generates the actual trajectory performed by the robot. The actual and the reference trajectories are presented in figure 5.

Fig. 5 The optimal path movement: reference and actual trajectories It can be noticed that the actual and the reference trajectories are not identical. This is caused by the encoder position error that even small, it accumulates along the covered distance. To overcome this, in longer

Robotica & Management, 12-1 / 2009

Optimal Path Computation for Mobile Robots Using MATLAB [2] Bailey, I. Mobile robot localisation and maping in extensive outdoor environments, PhD thesis, ACFR, University of Sydney, Australia, 2002 [3] Alexopoulos, C. Griffin, P.M., Path planning for a mobile robot, Systems, Man and Cybernetics, IEEE Transactions, Vol. 22, p.318-322, 1992 [4] Janet, J.A. Luo, R.C. Kay, M.G., The essential visibility graph: an approach to global motionplanning for autonomous mobile robots, IEEE Proceedings, Robotics and Automation, Vol. 2, p.1958-1963, 1995 [5] R.A. Brooks, "Solving the Find-Path Problem by a Good Representation of Free Space", IEEE Trans. on Systems, Man and Cybernetics, SMC-13 No.3, pp.190197, March 1983. [6] Durrant-Whyte, H., and Csorba, M., A solution to the simultaneous localisation and map building problem. IEEE Trans. on Robotics and Automation 17, Nr.3 June, p.229-241, 2001. [7] D.T. Kuan, J.C. Zamiska and R.A. Brooks, "Natural Decomposition of Free Space for Path Planning", Proceedings of IEEE International Conference on Robotics and Automation, p.168-173, 1985. [8] Ghosh S.K., Mount D.M., An Output-Sensitive Algorithm for Computing Visibility Graphs, SIAM Journal on Computing, Volume 20, Issue 5, p. 888-910, 1991 [9] LEGO Mindstorms NXT Bluetooth Developer Kit, http://mindstorms.lego.com, LEGO Group, 2006 [10] LEGO Mindstorms NXT Direct commands, http://mindstorms.lego.com, LEGO Group, 2006 [11] HANSEN J., Not eXactly C (NXC) Programmer's Guide, 2007 [12] Leonard, J. J., Durrant-Whyte, H. F. Simultaneous map building and localization for an autonomous mobile robot, IROS-91, Japan, p.1442-1447, 1991 [13] Paturca, S., Ilas, M., Greu, A., Ilas, C., Bluetooth bi-directional communication between an NXT robot and a PC, Analele Universitatii Maritime Constanta, Vol.12, 2009

trajectories is important to introduce a periodic correction. This can be achieved if at some corners the robot re-computes the trajectory to the next point, using its actual position.

Fig. 6 The optimal path movement: variation in time of the robot distance from starting point For the movement on the trajectory represented in figure 5, the variation in time of the robot distance from the start point is represented in figure 6. It can be seen that this distance increases linearly, with a constant speed for each segment in the optimal path. Whenever the robot changes the direction to a new segment (corresponding to points 1-5 in figure 5), this distance remains practically constant for the time needed to perform that direction change. 4 CONCLUSIONS In this article we described a control method of a mobile robot based on the optimal path computation in a known environment. The algorithm for determining the optimal path is based on the rather complex Dijkstras algorithm and is implemented in Matlab, but the movement control is performed by the robot, which determines that it approaches the obstacle area, interrogates the PC for getting the coordinates of points on the reference trajectory. The communication between the robot and the PC is via Bluetooth connection and is controlled by the robot. This results in an efficient solution for robots moving within a space that includes an area with obstacles that are previously known. Also, if this area changes in time, the robot control can be expanded to determine the new obstacles (by shape and position) and thus the map used in the optimal path computation algorithm can be updated. 5 REFERENCES [1] Sahraei A., Manzuri M.T., Razvan M.R., Tajfard M. and Khoshbakht S., Real-Time Trajectory Generation for Mobile Robots, Springer Berlin / Heidelberg, 2007

Robotica & Management, 14-2 / 2009