S M A R T B O T. Autonomous Parallel Parking Robot. David Johnson ( ) Tim Miller ( ) Huzefa Rangwala ( ) Under the guidance of

Transcription

1 S M A R T B O T Autonomous Parallel Parking Robot By David Johnson ( ) Tim Miller ( ) Huzefa Rangwala ( ) Under the guidance of Professor Stergios Roumeliotis (Csci 5551)

2 Parallel Parking Project Report Abstract Autonomous parallel parking is an area of robotics in which much research has been done. For this project we implemented a backwards parallel parking algorithm on a Pioneer I robot with a differential drive system. The algorithm used traces a trajectory which is generated by the dimensions of the parking spot. Environment sensing and error correction are dealt with in the project using a SICK laser sensor. Using this approach our robot was able to successfully parallel park itself in parking spots with size approaching the minimal possible, given the dimensions of the robot. 1. Introduction While fully automated navigation of vehicles remains out of our grasp, certain parts of driving are entirely possible to automate. A particular problem that merits solving is a universal algorithm to parallel parking. For years, parallel parking has plagued student drivers and frustrated motorists. While a self parking automated system might not become as ubiquitous as air bags have, it certainly would be an attractive feature on high end cars and massive SUVs that are difficult to see around. To begin this project, it must be understood that our robot turns not with a fixed axle, but instead with wheel differential. The speed difference between the robot s front two angular wheel speeds provides the only mechanism for turning. Our problem then is to develop an algorithm for parallel parking a tri-wheel robot using the front two wheel differentials as a basis for turning. Another aspect of this problem is the possibility of using external data to constantly update and correct for error incurred due to the environment and the robot s sensing and movement irregularities. The first task of the project is to detect a suitable parking slot using the Pioneer robot s

3 laser and sonar scanning capabilities. It is important to identify correctly whether the width and depth of the parking slot will allow sufficient space for the backward parking maneuver. Once the parking spot is detected, the robot will start moving towards the slot in its backward motion following a well defined trajectory between the starting and the final position, taking care not to bump into any of the walls or other objects in its path. The desired trajectory will be the optimal path with respect to the distance the robot travels in one continuous motion to the parking spot, given the dimensions of the parking spot. 2. Literary Review Current approaches to solve the parallel parking problem usually can be classified into the following: Planning an optimal trajectory between the start and the goal position, and subsequently following that definite path. Getting feedback from the environment to decide at each step the next small motion of the robot towards the goal without violating the environment space e.g. bumping into the objects in the environment. A combination of the above two techniques, using both the feedback from the environment and the planning of an optimal path. Generation of smooth trajectories for mobile robots has been studied extensively and some of the work in [Y1, Y2] proposes path planning based on spirals and complex curves. However the problem associated with strictly following a generated trajectory may not be ideal for all situations. Sometimes the reference path may differ from the feasible path due to the inaccuracies in modeling or dynamics of the problem. Also, generating such feasible reference paths turns out to be time consuming in the case of a nonholonomic system such as a vehicle or robot.

4 In the approach described in [I3], an iterative algorithm was used for the parallel parking maneuver. To control the steering angle and longitudinal motion of the vehicle a sinusoidal reference curve was proposed, along with a reactive strategy to prevent collisions of the vehicle. The algorithm divides the reference path into a series of sub-goals to be attained by the vehicle. This piecewise approach seems ideal for a vehicle with good reactive abilities. Most of the literature for autonomous parallel parking was based on a vehicle which allowed steering directly by rotating the axle of the front wheel and giving the same velocity to both the wheels. However, for a differential drive system, as in the case of the Pioneer robot system, velocities of the left and right wheel need to be set separately, so as to set the orientation of the robot in a particular direction. The material described in [G1, E1] addressed this issue of setting the wheel velocities to get a particular orientation of motion, angle. 3. Problem Formulation Our problem involves parallel parking a three wheeled robot. We will represent our robot at any given point in time as (x, y, ) where x and y are coordinates of the center of the robot s axle in our defined plane and is our robot's orientation to our x and y axis. We will define our point of origin (0, 0) as being the point where our initial laser sensors detect the front corner of the first obstacle, as seen in Figure 1. In regards to this Figure 1: Problem Layout frame, we have some destination position (fx, fy) that we wish the center of our axle to achieve after a motion.

5 3.1 Parking Spot Measurement There are three measurements we must obtain in order for the robot to properly parallel park. The margin is the distance in the y-direction from the center of the robot to the outermost point on the front car. The length of the spot is defined as the distance from the front of the back car to the back of the front car. Third, the depth of the spot must be measured. This distance is measured from the outermost point of the back car to the wall along the y-axis. There are at least two ways in which these measurements may be obtained. The first possibility is using a combination of sonar and laser scanners. This method takes advantage of the fact that the laser does not work for short distances. Starting at the back car, the robot will move forward taking measurements with both laser and sonar at 90 degrees to the right. This distance is at first too small for the laser, so the equipment will return the max value. Once the first value is received that is Figure 2: Gathering Data not the maximum, the robot will know that it is past the first vehicle. It can then use the previous point for which sonar data was obtained, to place the reference axes. Next, the robot will continue forward, using the laser scanner to record the depth of the spot, gamma. Once the laser starts returning incorrect values again, the robot will know it has reached the second car. At this point it will calculate the length of the spot by a simple subtraction of origin from the current position. The margin will then be calculated using the sonar. The second method of obtaining the three measurements uses the laser scanner

6 exclusively. In this method, the whole range of the laser scanner is utilized. Line-fitting algorithms are used to construct a line for the back of the front car. The length of this line can be used as gamma. The depth of the spot can be measured as the robot passes it by. Alpha can then be calculated as depth gamma. Finally, beta can be calculated using the Pythagorean Theorem, with the beta and gamma being the sides of the right triangle, and the measurement from the origin to the inner part of front car as the hypotenuse. Since our forward wheels are the ones that cause the rotation, it is best for it to pull past the parking spot create an approach with the third "swivel" wheel as in Figure 3. With the dimensions of our robot and of the prospective parking space, we can determine a desirable final resting point for the center of our robot. Our initial coordinate set is (, ) and our desired resting point for the middle of the axle can be described with values from our environment in Figure 2 and values from the specifications of the robot in Figure 3. Our final x (fx) coordinate will be calculated as + ( ). In this manner, the very center of our Figure 3: Robot Diagram robot will ideally rest in the very middle of the length of our parking space when we are done. Our final y (fy) coordinate will contain some error and so must be calculated as - + sqrt(( - )^2 + ^2) + where denotes some amount of error that will be calculated empirically. Using the Pythagorean Formula, we can figure out how far the back corner will swing out if it is rotated around the middle of the axle. Allowing for this much room will prevent our robot from touching the back wall of the parking space. With our initial and final positions determined, we can now examine some solutions to this problem

7 3.2 Trajectory Design Solution 1: Trajectory design Triangular function The trajectory needs to model an approximate cosine wave. We model our trajectory based on a triangular function described in [C1]. This can be defined as y = a * cos( w*x + ) + b where the constants a, b, w, and will vary depending upon the initial and final position of the robot. From the initial position x = 0 and y = 0 we get 0 = a * cos( ) + b Also from the final position : x = fx and y = fy we get fy = a * cos(w*fx + ) + b This results in a = b = fy/2 w = fx/ = Figure 4:Trajectories along the negative cosine curve. If we divide this curve into very small sections, we are basically breaking up the task into a series of subtasks. During a particular task the robot will be assigned some left wheel and right wheel velocities so that it traces the sub-path. We can get velocities of each section by using the orientation of robot, previous velocities and the path it needs to follow from [E1]. Then we can set the velocity of the left and right wheels for the interval that the section will last. Another approach would be to get the required orientation of robot at each point on the curve, which is the slope tangent drawn at points on the curve. This can easily be figured by calculating the derivative of the curve at each point, and hence getting the orientation (angles) at all these points as seen in figure 4. Once we have these points we can use the setheading(angle)

8 function for the Pioneer robot to trace the curve. Solution 2: Generating a piecewise solution to the problem. In another approach, we can make the movement piecewise. For this second solution, there are three basic periods of movement, as seen in figure 5. First, the robot makes a rotation, then it slides into the stall, and finally it rotates the opposite direction as before, back into the correct orientation. The slide into stall will be motion along a straight line directed towards some point in the parking slot. The orientations before and after the stall will be the same and the distance that the robot should slide for depends on what the Figure 5: Piecewise Solution environment space is, so that robot can freely rotate about the center of the line joining the two wheels, without bumping into the curb or adjoining car. A two piece solution where the angular velocity of one wheel is a time dependent cosine function and switches to the other wheel at half of the total estimated time. 3.3 Error correction using laser sensing and line fitting. In a real world situation, it is possible that a given vehicle will not be perfectly parallel to the road at the beginning of the parking procedure. For our robot, we desired some form of correction for this possible problem. This can be achieved by using the laser sensor, along with the assumption that the wall opposite where the robot will park is a straight line and is in the proper direction.

9 The first step is to check the laser readings for all angles greater than 90 (to the left of the robot. In practice, we used a smaller subset of these, because the values closest to 90 usually hit the wall to the front of the robot, and would thus corrupt the line-fitting. Next, these values need to be converted to Cartesian coordinates (they arrive in polar coordinates). The Cartesian coordinates are passed into a line-fitting algorithm, which returns the parameters a and b for the line y = a + b * x. Referring to figure 6, the angle between the robot and the wall can be determined by the slope b of the line that tracks the left wall. The inverse tangent of b gives the angle of the wall with respect to the robot s perception of horizontal. By subtracting from 90, it is possible to obtain the angle between the wall and the robots perception of vertical. During the measurement states, we want this angle to be zero, so we can use the setdeltaheading function to correct this error. We were able to implement Figure 6: Orientation with back wall guidance this functionality on the simulator. However, we had a little trouble integrating this behavior with the other behaviors on the real robot, so in the interest of producing a working demonstration, we left it off. It is possible to do a similar correction during the backing in phase. First, one can obtain the angle of the left wall exactly as above. This effectively gives the true angle of orientation of the robot. This can be compared to the robots internal idea of its current orientation to check for error. In this way, we can use the current angle as computed by the line-fitting method instead of the current orientation that the robots internal state has. We did not conduct extensive tests in order to check whether one measure is any more accurate than the other. It is probably likely

10 that the laser is more accurate. However, even if it has more average error, since scans are taken so often, as time goes on errors have a chance to balance out. In the case of the robot, its position errors are generally caused by things like sticky floors or imperfect wheels, and the errors will probably not be balanced. Figure 7: State Transition Diagram

11 4. Implementation This section traces through the steps that the robot takes for successfully parking itself. This is explained with the help of the State Transition Diagram shown in Figure 7 shown below: When the robot starts up it is programmed to be in the INITIAL State where it waits for the laser to be powered on, since the laser is an essential component and we do not want to start our robot unless it has its sensing capabilities to use. Once the laser powers up, the robot will be in state MARGIN MEASURE State where it uses the laser to calculate the distance m from the first box, see figure 8. Since the laser readings are accurate within a certain percentage, we compute the average of these readings taken in state measure to get the final calculated m. The ratio of the current value with this average value is checked in this state, and as soon as the ratio rises above a certain threshold, the robot has moved to position 2 in the figure 8, i.e at start of a potential parking spot. The robot s origin is reset to this point to compute the length of spot efficiently. When the robot moves into the DEPTH MEASURE, it does a similar computation as it was in the MARGIN MEASURE state to get the depth of the box. Here also, the ratio of the current reading of the laser to the average of all previous readings is checked to exit this state. This will happen when the robot will move to end of the spot indicated by position 3 in the diagram. Since the SICK laser does not give very good readings at exactly 90 degrees, a cone of readings from 85 degrees to 90 degrees needs to be taken for the measurement of depths and state change conditions. Since the average of the shortest distance given by the laser to the wall is used, depth calculation is not such a big problem. The problem occurs with the fact that the robot moves into the next state not at the ideal position 3 but at position 3 as

12 indicated in Figure 8. Hence the robot moves into an error correcting state called BLIND FORWARD where it just moves in the same orientation for 1 second. This is an attempt to move the robot from position 3 to 3. Once it is done with this state, it calculates the length of the spot and determines if the spot length and depth are sufficient for it to park assuming the error conditions described earlier. The origin will be set to this new position and then the robot will move to the FINAL State. Figure 8: Environment Map In the FINAL state the robot will basically trace the trajectory defined above. It gets the current position of the robot along the x direction, and computes the y-coordinate and the orientation the robot should be in, given as the tangent of the line at that position on the curve. Using the setheading() function the robot moves in that particular direction. Once the robot has traversed the trajectory, it goes to the END state and stops. Also sometimes the robot will park itself inaccurately i.e not go into the parking spot fully or park too close to one of the boxes. The SLIDING State is an iterative state which uses the sliding maneuver technique, where the curve will be iteratively followed forwards and

13 backwards within the parking spot. Thus the robot will parks itself parallel and right in the center of the spot. 5. Results & Conclusions The parallel parking project has been successful to the extent that the Pioneer I robot is capable of detecting a sufficient parking spot for itself, generating an optimal trajectory that it needs to follow, and tracing the trajectory to fit itself into the detected spot. One of the aims of the project was to design the robot so that it would park itself in tight places. Results performed on the robot with its specification - length 0.500m (with the laptop jutting out at the back), a turning radius of 0.15 m and a width of 0.4m show that the robot would park itself in a minimum parking spot of 0.8m. However, running the test several times for the same parking spot length, it was noticed that the robot would crash into the other car or back wall 3 out of 10 times, giving us a probability of 0.7. This was due to the errors induced due to incorrect laser readings, robots imperfect odometry, the robot s motion and the environment. Steps such as using the back wall as a reference were taken to provide a feedback to the robot about its actual position and various solutions were discussed to handle the induced error. As an additional safety constraint, the minimum parking spot was set to 0.825m, so that the robot would park itself without damage. Thus our robot claims to successfully park itself in a spot having minimum length of 0.825, using the most optimal trajectory with respect to the distance it travels and the changes in orientations (minimized). 6. Future Work To complete the project, and to extend it, we will need to accomplish the following tasks: Use the laser to plot a map of the world and get accurate position of the robot to correct errors due to its odometry. See figures 9 and 10 for plots of readings of the back wall. The information displayed there can be analyzed by the robot to achieve desired error

14 correction. Extend the scope of the project to use the camera to detect certain images on the parking slot and then process these images to check if the robot was parking in its own reserved place. For example a template matching algorithm can be used for this purpose. Also the robot should be able to detect any obstacles in its path using its sensing capabilities and take the necessary avoiding action. Figure 9: GNU plot of back wall. Figure 10: GNU plot of back wall.

15 Reference [C1] [I1] [I2] [I3] Chunyu Zhu. GPS based automated parking, Thesis-University of Minnesota, Twin Cities (2000). Igor E. Paromtchik, Christian Laugier. Autonomous Parallel Parking of a Nonholonomic Vehhicle, Proc. of the IEEE Intelligent Vehicle Symp., pp (1996) Igor E. Paromtchik, Christian Laugier. Automatic Parallel Parking and Returning to Traffic Maneuvers, Miscellaneous, Ch. licra98 inria - ucis (1998) Igor E. Paromtchik, Christian Laugier. Motion Generation and Control for Parking an Autonomous Vehicle, Proc. of the IEEE Robotics and Automation., pp (1996) [G1] Gregory Dudek, Michael Jenkin. Computational Principles of Mobile Robotics (2000) [E1] [S1] [L1] [A1] [Y1] [Y2] Evangelos Papadopoulos, John Poulakakis. Trajectory Planning and Control for Mobile Manipulator Systems, Proc. of the IEEE Control and Automation (2000). S.Sastry, R. Murray Walsh. Stabilization pf Trajectory for Systems with Nonholonomic Constraints, IEEE Trans. On Automatic Control(1994) Lyon. Parallel parking with curvature and Nonholonomic Constraint, Proc. of the IEEE Intelligent Vehicles (1992). Ackermann, Guldner, Sienel. Linear and nonlinear controller design for robust automatic steering, IEEE transactions on Control Systems Technology (1995). Y. Kanayama and N. Miyake. Trajectory Generation fro Mobile Robots, Proc. of the Intl. J. of Robotics Research., pp (1986). Y. Kanayama and B. I Hartman. Smooth Local Path Planning for Autonomous Vehicles, Proc. of the IEEE Int. Conf. Robotics and Automation, pp (1989).