Document Sample

Optimization-Based Reﬁnement Stage Planner for an Autonomous Vehicle in Dynamic Environments Martin O. Larsson, Lund University Faculty Mentor: Richard M. Murray September 21, 2006 Abstract In November 2007 Caltech intends to participate in the DARPA Urban Chal- lenge, a competition where autonomous vehicles are to complete a race in an urban environment. A crucial component of an autonomous system is the path planner, which generates feasible trajectories for the vehicle to follow. The purpose of this project is to draw upon Team Caltechs earlier experience of autonomous driving and investigate the possibility of reusing and adapting existing technology to an urban setting. The main conclusion is that the approach should not be ruled out, although a number of issues need to be addressed. The deterministic nature of the algorithm makes the strong dependence on prediction of other vehicles’ motion potentially problematic. A separate module would likely be necessary to encode logics, such as traﬃc laws, in terms of costs and constraints in the optimization problem. If the approach is used, the implementation should not build on the existing code; the lack of documentation and the fact that the code was optimized for the 2005 Grand Challenge makes it diﬃcult to extend in a reliable way. 1 Introduction In 2005, Caltech participated in the DARPA Grand Challenge (DGC) with the objective to conceive, develop and build an autonomous ground vehicle capable of traversing a 175 mile desert race course in 10 hours. In May 2006 DARPA announced a new competition, the Urban Challenge, due to take place in November 2007. This time the participating vehicles must be able not only to traverse a race course, but also avoid moving obstacles, obey traﬃc laws and employ defensive driving. This SURF project is intended as part of an eﬀort to investigate technologies that may prove useful in extending the functionality of Alice, Team Caltech’s entry in the 2005 DGC. The goal is to enable Alice to function in dynamic environments, where moving obstacles have to be avoided and traﬃc laws obeyed. A crucial part of the system is Alice’s ability to choose an optimal path from point A to point B. The existing path 1 planner assumes static surroundings, which is clearly not a valid assumption in an urban environment. The planner therefore has to be modiﬁed to account for moving obstacle avoidance. Figure 1: In a dynamic environment the path planner must take the moving obstacles into account when planning a trajectory This problem – trajectory planning in dynamic environments – is not a new one and although not entirely solved, a number of diﬀerent approaches have been proposed. For instance, Fiorini and Schiller [1] compute a preliminary trajectory by searching a tree where the nodes correspond to spatial locations and the edges represent avoidance maneuvers. This trajectory is then reﬁned so as to fulﬁll additional constraints, such as actuator constraints. In Hsu et al. [2] the nodes are chosen in a randomized fashion according to the so called Probabilistic Roadmap Method (PRM), which is useful to keep computational time reasonably low when the vehicle/robot has many degrees of freedom. Alice’s current trajectory planning module uses another technique, namely an optimization- based method in a receding horizon framework. This means that the problem is posed as an optimization problem, where the computed trajectory is chosen so as to minimize a cost function (typically traversal time) subject to speed limits and the vehicle’s dynam- ical constraints. Receding horizon means that the planner only produces trajectories from the vehicle’s present position up to a horizon some distance ahead (see Figure 2). The approach relies on earlier work by Milam [3], where receding horizon control in air vehicle maneuvering is implemented on the Caltech Ducted Fan testbed. 2 Objectives and Approach The goal of this project is to investigate whether the optimization-based, receding horizon approach is suitable for the new situations Alice will encounter. The general approach 2 ﬁeld. This is inadequate for our purpose, we want to be able to plan through both Since the DGC race is over 100 miles long, it is both edances, where one could potentially travel, computationally prohibitive and unnecessary to plan the ve- d speed. Another well-researched approach to hicle’s trajectory all the way to the ﬁnish line at all times. A em is a graph search. This is a scheme where good compromise is to run the planner in a receding horizon esented as a graph with nodes corresponding framework. A receding horizon scenario is one where plans ns, and the edges representing to the cost are computed not to the ﬁnal goal, but to a point a set horizon m one location to another. A well-known (spatial or temporal) ahead on some rough estimate of the path type is A*. It can plan an optimal path towards the goal (Figure 3). mapped environment, but this information is nown a priori [1]. D* is an extension of Trackline path amically and efﬁciently modify the plan if Vehicle Obstacle Target point n obstacle is encountered [2]. Unfortunately, raph is searched, D* and A* are insufﬁcient uld include no dynamics and would operate Final plan Sensor range A car-like robot includes non-holonomic h would render unfollowable many spatial Fig. 3. An illustration of the receding horizon framework. A very inaccurate that employ A* to search an extended graph (but2: Receding horizon control (picture taken afrom [4]) Figure easily computable) path is used as the global plan. Here this is spline e trajectories exist ( [3], [4]). Unfortunately, based on the trackline. The solver that produces a higher-quality plan performs its computation to reach a point on the lower quality path, some distance oo slow or simplify the problem too much, ahead. In this ﬁgure, that distance (the horizon) is set to the range of the acles. sensors of the vehicle. was to extend the current software with moving obstacle avoidance capability. In the old efforts compute the optimal spatial path, and planner, obstacle avoidance is accounted for through the speed limit constraint in the e optimal velocity proﬁle that is feasible for a separation of spatial A. Vehicle the e that path [5], [6]. This optimization problem. Sincemodelenvironment was assumed to be static, the speed limit depended of space only. A straightforward way, at least in principle, of incorporating nning, however, is a potential source on non- As mentioned earlier, the planner is designed to generate moving the fastest he shortest path is not necessarilyobstacle is to add time dependency to the speed limit constraint. All this is laid trajectories that are dynamically feasible in respect to some model of the vehicle. The model chosen solving the problem at hand is thus out in more detail in Appendix A. The approach for here is the kinematic done to ﬁnd ways to e issues, work has been to implement this seemingly simple extension to the existing software. bicycle model (Figure 4):. ally feasible trajectories around obstacles in database of precom- f those algorithms use a A key issue is the interfaces with other relevant modules on Alice, including designing ˙ N = v cos θ a computations [7]. ves to speed up the online suitable representation of the moving obstacles (in collaboration with other SURF ˙ E = v sin θ the drawback of not necessarily producing (1) ˙ v = toadiﬀerent parts of the functionality were d making it difﬁcult toprojects). Also, test scenarios corresponding v specify constraints. ˙ θ = L tan φ created with the from the discrete nature of the precomputed objective of testing them in simulations and in the ﬁeld. The perfor- where to an E are the Northing and about 5 plans per mance objective was set N and update frequency of Easting coordinates of second, which was mization method to produce the plans would the middle of the rear axle, v is the scalar speed of the center the rate during the 2005 Grand Challenge. ems. An optimization problem could be set of the rear axle, θ is the yaw of the vehicle, a is the scalar ptimal plans that also It has not been a part of this project to consider howisto generate a suitable seed, or avoid obstacles and longitudinal acceleration of the vehicle, L the wheelbase initial guess, for of the vehicle, This is the the words ”Reﬁnement easibility constraints. An optimization-based the optimizer. and φ is whysteering angle. This model isStage” are present temporal planning derived ormed the spatial andin the title of this report.by assuming that the two bicycle wheels do not slip on sed its spatial plans off D* was attempted the ground, which implies that each wheel is always tangent Figure 3 illustrates the structure of the dynamic planner module and how it relates to the modules with which it communicates. The red box and its subcomponents are described in a more detailed fashion in Appendix B. 2.1 The Dynamic Map and Map Accessor The dynamic map (CDynMap) was implemented from scratch1 and contains a vectorized representation of the present state and geometry, i.e. position, velocity, orientation and side lengths (all moving obstacles are assumed to be rectangular). This is the same representation used by the moving obstacle detection and tracking module. The old software uses a grid-based representation of the environment, and to be able to reuse as much as possible of the existing code, CDynMap was equipped with functions converting from the vectorized to a grid-based representation. The result is essentially a binary space-time map, where a zero indicates the presence of a moving obstacle. 1 Part of this work was done by David Bolin 3 Terrain sensors Moving obstacle Dynamic Fusion detection and Mapper tracking Dynamic Planner Module (CDynPlannerModule) Dynamic Map Static Map (CDynMap) (CMap) RDDF corridor Dynamic planner (CDynPlanner) Vehicle state Trajectory Follower Figure 3: The dynamic planner structure and the system framework. C++ class names are given in parentheses. Moving obstacle detection and tracking as well as the dynamic map were not present in the old architecture. 4 In the planner code (MapAccess.h/.cc) the map accessor was extended with calls to the dynamic map and the subsequent fusion of the static and dynamic maps, simply by element-wise multiplication. This happens before the interpolation and smoothing needed to make the speed map diﬀerentiable as required by the optimizer (cf. Kogan [4]), implying that nothing of that had to be re-implemented for the dynamic parts of the map. 2.2 The Constraint Function Evaluating the new constraint function involved calling the new map accessor, which required implementing a function computing the time coordinate at each collocation point. The computation was carried out by subsequent integration of s dτ t(s) = t(0) + Sf (1) 0 v(τ ), where t(0) is the time at the start of the trajectory (”now”), Sf is the trajectory length and v(τ ) is the speed along the trajectory as a function of arc length. This eventually amounts to evaluating the integral 1 ds , 0 v0 + (v1 − v0 )s which was done by integrating the ﬁfth order Taylor expansion of the integrand around s = 0.5 (cf. Kogan [4]). The computation of the new Jacobian of the constraint function was not implemented due to the complexity of the previous code and the lack of documentation (see Appendix A for details), but this doesn’t seem to have had any signiﬁcant impact on the result. The numerical solver continually monitors the provided Jacobians and outputs warning messages if they don’t seem reasonable. No such messages appeared. 2.3 Corridor and Target Point The old planner uses a so-called RDDF (Route Deﬁnition Data File) to specify the maximum speed and the corridor in which the vehicle must remain. This representation is not suitable for urban environments, but to be able to test the extended optimizing algorithm without implementing a lot of peripheral functionality and integrate it with the planner code, the RDDF format was reused. The trajectory target point is chosen along the trackline path, as illustrated in Figure 2. 5 2.4 Computing a Seed As mentioned above, the problem of ﬁnding a suitable seed for the optimizer was not a part of this project. However, an initial guess still has to be provided to the solver. Two qualitatively diﬀerent approaches were tested. The ﬁrst was simply to use a smooth curve from the current position to a position suﬃciently far ahead along the RDDF. The curve was generated using the existing class CPath (see the Alice documentation). The second approach was to seed the subsequent planning cycle with the previously computed trajectory extended with the CPath curve. See Figure 4 for a schematic illustration of the two approaches. previous trajectory CPath:seedFromLocation CPath:seedFromLocation Figure 4: Two qualitatively diﬀerent ways of generating an initial guess for the numerical optimizer 3 Results Testing was mostly performed in simulations, where simple scenarios were executed and the behavior observed visually. The performance in terms of computation time was logged during each run. See appendix C for a description of the system setup. The diﬀerent scenarios are presented in Table 1, and in Figure 5 subsequent screenshots from scenario 3 are shown. The dynamic planner was tested in the ﬁeld on Alice on one occasion, although with- out any moving obstacles. The planner worked as expected, but since no moving ob- stacles were present the initial objective of testing the scenarios in the ﬁeld was not fulﬁlled. In the presence of moving obstacles, the update frequency of the planner was normally about 0.3 – 0.5 seconds per plan, i.e. 2 – 3 Hz. This does not quite fulﬁll the objective of 0.2 seconds per plan, but is still acceptable at moderate speeds. Furthermore, these numbers come from the lab computers, whereas testing revealed that the planner gen- 6 Figure 5: Screenshot from simulation of scenario 3. The current implementation of the graphical user interface causes the other car to leave a trace behind it. The diagrams on the right illustrate the intended speed proﬁles along the respective trajectories Scenario Tested functionality Result 1. Drive across an open ﬁeld Ability to navigate an empty The planner avoids the obsta- (zone) with no static obstacles parking lot (or other open cle. Sometimes indecisive be- and another car driving across area) with another moving ve- havior; cannot decide whether our path hicle to pass in front of or behind the car. 2. Drive across an open ﬁeld Ability to navigate an empty In some cases the planner (zone) with no static obsta- parking lot (or other open avoids both obstacles. How- cles and two other cars cross- area) with two other moving ever, it relatively often fails to ing our path vehicles. converge. 3. Drive down a lane behind a Ability to pass a slow-moving The planner performs the slow-moving car with the cor- vehicle passing maneuver, sometimes ridor wide enough to allow for passing on the left side, some- a passing maneuver. times on the right. 4. Drive down a lane behind Ability to follow a lead vehicle The planner is unable to slow another car down and follow the other car. It either fails to converge, or tries to go outside the RDDF. Table 1: List of scenarios with description, purpose in terms of which functionality is tested, and outcome from simulations 7 erally ran faster on Alice’s computers. Also, parts of the algorithm could potentially be optimized, in particular functions associated with the dynamic map. Considerable variations in update frequency were observed between diﬀerent runs. Fig- ure 6 shows the time for each planning cycle in two diﬀerent runs of scenario 3. In these diagrams the computation times span from 0.2 seconds up to more than 1 second per plan. 29/-1:-.-,:;1"<1:,((22=2=>1!<13./62= 18.,09,-,+9:0";09+''11<1<=0!;02-.51< # # " " ! ! !" !" ! "! #! $! %! &! ! "! #! $! %! &! ()*+,-.-/)01-/*213)412.(51+6.00/071(8(62 '()*+,-,.(/0,.)102(301-'40*5-//./60'7'51 !'% # !'# " ! ! ! "! #! $! %! &! ! "! #! $! %! &! -/*21+241*.+1.((2::1?,*@ ,.)10*130)-*0-''1990>+)? #!! #!! "!! "!! ! ! ! "! #! $! %! &! ! "! #! $! %! &! Figure 6: Logged performance from two diﬀerent runs of scenario 3. The time per map access is fairly constant once the moving obstacle appears, but the full computation time varies considerably. In these examples, the planner almost never failed to converge. 4 Conclusions and Further Work Most of the time, the planner was able to solve the basic task of computing a trajectory while taking into account the motion of the other vehicles. However, there are some situations where it fails completely or exhibits a behavior which by a human would immediately be taken as unsafe or unpredictable. In particular in scenario 1 the planner tends to switch rapidly between several radically diﬀerent trajectories, not ”making up its mind” until the last minute. Sometimes the planner fails to converge altogether. By using a better seed and correcting the gradient of the constraint function the risk of convergence failure or too long computation time can probably be reduced. The large variations in computation time between diﬀerent planning cycles are trouble- some. It is not obvious what causes this, and it adds a very undesirable unpredictability element to the algorithm. Perfect knowledge about the future state of moving obstacles is not a realistic assump- tion. For the algorithm to be useful in practice, the uncertainty in predicted motion of other vehicles must somehow be accounted for. 8 When deciding on whether or not to use an optimization-based approach in the Urban Challenge, there some issues that should be considered: • Logics, such as traﬃc laws, may be diﬃcult to encode. This could for instance be solved by having a component upstream of the dynamic planner formulating the solver constraints at each planning cycle. • Provides that the situation can be encoded properly, the optimization approach is a very general way of planning a trajectory. The same framework can thus be used in parking lots and empty roads alike. However, many times the solution space is highly constrained ((for instance, the width of a lane is on the order of twice the cars width), and the approach may be unnecessarily complicated. It all comes down to how fast you can make the planner run. • Although the numerical optimization approach as such should not be ruled out, the implementation should not build on the existing planning software due to the lack of documentation and to the fact that the code was optimized for the 2005 Grand Challenge. 5 Acknowledgments I would like to thank the SURF 2006 Urban Challenge team, in particular David Bolin, with whom I worked on the dynamic map, David Knowles, who also has been working on path planning, and Laura Lindzey, whose knowledge about the existing system has been absolutely indispensable. I also want to thank professor Anders Rantzer at the Depart- ment of Automatic Control at Lund University for putting me in touch with Caltech, and of course professor Richard Murray for making my SURF possible at all. References [1] Paul Fiorini, Zvi Shiller. Time Optimal Trajectory Planning in Dynamic Environ- ments, Proceedings of the 1999 IEEE International Conference on Robotics and Automation, pp. 1553-1558, 1996 [2] David Hsu et al. Randomized Kinodynamic Motion Planning with Moving Obstacles, International Journal of Robotics Research. Vol. 21, no. 3, pp. 233-255, March 2002 [3] Mark B. Milam. Real-Time Optimal Trajectory Generation for Constrained Dynam- ical Systems, PhD thesis, California Institute of Technology, 2003 [4] Dmitry L. Kogan. Realtime Path Planning through Optimization Methods, Califor- nia Institute of Technology, 2005 9 [5] Philip E. Gill et al. User’s Guide for SNOPT 5.3: A Fortran Package for Large- Scale Nonlinear Programming, February 1999 Appendices A Mathematical Details To ﬁnd the optimal trajectory, an optimization problem on the following general form is constantly solved in real-time: min J(x) s.t. L ≤ g(x) ≤ U x Here, x ∈ Rn is a vector representing the trajectory and J : Rn −→ R is a cost function. L, U ∈ Rm are lower and upper bounds on the constraint function g : Rn −→ Rm , where m is the number of constraints. The constraints are necessary to ensure that the computed trajectory is dynamically feasible and obeys the speed limit. As described below, they are also used for avoidance. Constraints are put on speed (v < vlimit ), acceleration, steering angle, steering rate and lateral acceleration. See Kogan [4] for details. The trajectory is fully determined by the yaw θ(s) and speed proﬁle v(s) along the trajectory (s is the normalized arc length), together with the trajectory length Sf . All user variables (Northing and Easting coordinates, velocity, acceleration, yaw, etc.) can be expressed in terms of these so-called ﬂat variables (see Kogan [4]). The user variables are necessary because it is for them the constraints are deﬁned. Also, the Northing and Easting coordinates are needed to compute the speed limit, since it depends on the position. Now, the yaw and speed proﬁles belong to inﬁnite dimensional spaces, which therefore have to be approximated by a ﬁnite dimensional space Rn due to the ﬁnite nature of the solver. This is done by sampling these two functions at equally spaced collocation points along the trajectory. Hence the solver representation x of the trajectory is: 1 2 x = ( (θ(s))s∈Scoll (v(s))s∈Scoll Sf ), Scoll = {0, , , ..., 1}, N −1 N −2 where N is the number of collocation points. The dimensionality n of the optimization problem is thus n = 2N + 1. The constraints are only guaranteed to be satisﬁed at the collocation points. It is therefore assumed that the collocation spacing is tight enough for the constraints to hold also in between the collocation points. Obstacle avoidance enters the problem through the speed limit constraint. In the old planner the speed limit depended on the spatial location only, since the environment was 10 assumed to be static. To account for moving obstacles the speed limit function vlimit (x, y) is replaced by vlimit (x, y, t), where t is the time when Alice would be at the location (x, y), given the candidate solution currently under consideration by the solver. An important observation is that the time coordinate t(s) along the trajectory can be expressed in terms of the ﬂat variables v(s) and Sf as in Equation (1) in Section 2.2. Thus, no new solver variables have to be introduced, implying that the introduction of a time dependent speed limit does not increase the dimensionality of the optimization problem. Since the numerical solver uses the Jacobian of the constraint function, this Jacobian is implemented in the planner. The fact that new speed limit vlimit (x, y, t) now also depends on t implies that a correction is required for the Jacobian to be accurate. The division into a static and dynamic speed map, vlimit (x, y, t) = vstatic (x, y)vdynamic (x, y, t), makes it possible to simplify the implementation. The partial derivatives with respect to space (∂vlimit /∂x and ∂vlimit /∂y) are computed numerically as in the old algorithm after the static and dynamic parts of the map have been multiplied together. As for the time derivative, we have ∂vlimit ∂vdynamic = vstatic . ∂t ∂t Computing ∂vdynamic /∂t involves smoothing and numerical diﬀerentiation in one dimen- sion only. However, the solver needs the Jacobian of the constraint function g with respect to the solver variables x. This requires more complicated modiﬁcations to the existing code, and was not implemented due to the complexity and lack of documenta- tion. ˙ The cost function J is a linear combination of traversal time T , steering eﬀort Eφ = φ 2 (φ is the steering angle) and acceleration eﬀort Ea = a 2 . The steering and acceleration terms are included to achieve a smoother driving behavior. Once the optimization problem has been posed, a numerical solver is used to perform the actual computations. The software used by the old planner, and also by the extended planner, is SNOPT (cf. Kogan [4] and [5]), which is a Fortran-based commercial package based on Mark Milams NTG software [3]. B Module Description Below is a slightly more detailed description of the dynamic planner module and its subcomponents (see Figure 3) than was given in Section 2. The (dynamic) planner module can be thought of as a container for the components where the planner algorithm and its support functions actually lives. It is responsible for 11 all interfacing with the outside world (obstacle tracking, fusion mapper, state estimation) through Skynet, the network communication software used on Alice. It continually initializes and runs the planner algorithm and sends the results to the trajectory following module. Two main changes were made to the old code; the calls to the planner were extended with a parameter indicating the current time, and a loop listening to Skynet messages on updates to the dynamic map was implemented. The static map contains a grid-base representation of the static parts of the environ- ment. It is constantly updated as new sensor readings of the environment are received. The sensor data is processed and transformed into the appropriate form by the (dy- namic) fusion mapper. The dynamic map contains a vectorized representation of the moving obstacles and is kept up to date by the moving obstacle detection and tracking module. The (dynamic) planner is where the planning algorithm is implemented. Apart from the optimizer itself, the 2005 planner contains a seed generator (the so-called reactive planner) that computes an initial guess for the optimizer. As mentioned in the Objectives section, modifying this component has not been part of this project. The planner also contains the optimizer (reﬁnement stage planner) where the cost and constraint functions are deﬁned, as well as other support functions required by the numerical solver. Figure 3 does not show the interface with the supervisory controller, SuperCon, which in the old architecture was responsible for system diagnostics and contingency manage- ment. The interface with SuperCon has not been part of this project, since the future functionality and structure of this component was far from being ﬁnalized in the summer 2006. C Simulation Setup Figure 7 shows the system setup for the simulation runs. The spoofCom component was implemented in order to emulate the moving obstacle detection and tracking module (cf. Figure 3). It continually sends Skynet messages about moving obstacles so that the dynamic map can be properly updated. The planner and fusion mapper shared an AMD64 computer, while the remaining modules were distributed over 2-3 Pentium 4 computers. Each test scenario has a corresponding RDDF specifying the trackline and a scenario ﬁle specifying the moving obstacles. The obstacles are always moving linearly with constant speed, and their trajectories are assumed to be known exactly to the planner. 12 vehicle state RDDF actuator trajectory commands Static speed map Dynamic trajFollower simulator (from ﬁle) plannerModule spoofCom (spoofed moving obstacle map) Dynamic map deltas GUI fusionMapper = component have been written or modiﬁed as a part of this project Figure 7: The information ﬂow between modules during simulations. 13

DOCUMENT INFO

Shared By:

Categories:

Tags:
Optimization Based Reﬁnement Stage Planner for an Autonomous Vehicle, system realization, Frequency Domain, design process, intermediate variables, Junior Research, Postdoctoral Scholar, Technical Staff, Graduate Student, performance variables

Stats:

views: | 6 |

posted: | 2/27/2009 |

language: | English |

pages: | 13 |

OTHER DOCS BY Kittibitti

How are you planning on using Docstoc?
BUSINESS
PERSONAL

By registering with docstoc.com you agree to our
privacy policy and
terms of service, and to receive content and offer notifications.

Docstoc is the premier online destination to start and grow small businesses. It hosts the best quality and widest selection of professional documents (over 20 million) and resources including expert videos, articles and productivity tools to make every small business better.

Search or Browse for any specific document or resource you need for your business. Or explore our curated resources for Starting a Business, Growing a Business or for Professional Development.

Feel free to Contact Us with any questions you might have.