EXPERIMENTAL VALIDATION OF HIGH SPEED HAZARD AVOIDANCE CONTROL FOR UNMANNED GROUND VEHICLES. Matthew Spenko, Steven Dubowsky, and Karl Iagnemma

Transcription

1 EXPERIMENTAL VALIDATION OF HIGH SPEED HAZARD AVOIDANCE CONTROL FOR UNMANNED GROUND VEHICLES Matthew Spenko, Steven Dubowsky, an Karl Iagnemma Massachusetts Institute of Technology, Department of Mechanical Engineering Cambrige, MA, 39 Abstract: At high spees in natural terrain, unmanne groun vehicles can eperience unepecte situations that require rapi hazar avoiance maneuvers. In these scenarios there is often limite time to replan a path base on high-orer ynamic moels. This paper presents a novel metho for high spee navigation an hazar avoiance base on the two imensional trajectory space, a compact an computationally efficient moelbase representation of a robot s ynamic performance limits on natural terrain. This paper also presents a novel metho for trajectory replanning, base on a curvature matching technique. Eperimental results on a small high-spee UGV emonstrate the metho s effectiveness. Copyright 6 IFAC Keywors: Autonomous Mobile Robots, Obstacle Avoiance.. INTRODUCTION AND LITERATURE REVIEW There are many important military, isaster relief, an surveillance applications that require a unmanne groun vehicle (UGV) to move at high spees through uneven, natural terrain with various compositions an physical parameters. Often a UGV is irecte to follow a pre-planne path (or navigate through pre-efine waypoints) esignate by an offline mission-level planning algorithm. However, in natural terrain at high spees, it is likely that angerous an unepecte situations will occur. These may be the result of outate topographical ata, unientifie hazars ue to sensor limitations or errors, or unanticipate physical terrain conitions. In these situations a UGV must quickly eecute a maneuver that allows it to safely avoi the impening hazar. Despite increasing computing power, at high spees there is little time to perform navigation base on etaile ynamic vehicle an terrain moels. Hazar avoiance has been traitionally performe either by selecting from a set of preetermine paths (i.e. search techniques over small spaces), or by reactive (refleive) behaviors, which evoke a preetermine action in response to specific sensor signals. Many of these techniques have been esigne for use on flat or slightly rolling terrain, at spees that o not ecite vehicle ynamics. Here the problem of navigation an hazar avoiance on flat, rough, an uneven terrain at spees that ecite the vehicle s ynamics is aresse. Previous researchers have aresse this problem with a search-base technique to navigate a HMMWV-class vehicle at spees up to m/s while avoiing large hazars (Coombs, et al., ). The metho relies on a pre-compute atabase of approimately.5 7 clothoial paths. Since the vehicle is assume to travel on relatively flat terrain at fairly low spees, the moel use in the calculations oes not consier vehicle ynamics. An online algorithm eliminates caniate clothois that intersect with hazars or are not feasible given the initial steering conitions. From the remaining path, the algorithm chooses one that follows the most benign terrain. Several conteners of the 5 DARPA Gran Challenge utilize similar approaches which have proven to be successful for spees in ecess of 8 m/s. However, the techniques o not consier the important aspects of terrain roughness, inclination, an vehicle/terrain traction characteristics, all of which will become increasingly more important as autonomous vehicles move from traversing roas an relatively benign terrain to more angerous an etreme topography.

2 Researchers have evelope a fuzzy logic-base algorithm for reactive outoor hazar avoiance (Daily, et al., 988; Olin, et al., 99). The approach arbitrates between hazar avoiance an goal seeking an allows for UGV navigation at spees up to m/s. Another successful reactive behavior-base technique was evelope where the behaviors are caniate steering angles, an an arbitrator chooses a steering angle base on hazar an goal locations (Kelly an Stentz, 998). Other work in the area has focuse on problems arising from partially known an ynamic environments (Laugier, et al., 998) or sensing issues in outoor terrains (Langer, et al., 994). Although these techniques have been successful at low to moerate spees, they o not eplicitly consier vehicle ynamics an changing terrain characteristics. In this paper, a hazar avoiance metho that consiers vehicle ynamics, terrain parameters, an hazar properties is eperimentally valiate. It is computationally efficient enough for high-spee applications. The work has similarities to the ynamic winow metho for low-spee collision avoiance in structure environments (Fo, et al., 997). The technique escribe here incorporates features that are critical to UGV navigation, such as vehicle/terrain interaction, the presence of hazars, an terrain roughness an unevenness. The algorithm relies on the trajectory space, a compact framework for analyzing a UGV s ynamic performance on uneven, natural terrain (Spenko, et al., 4). In aition, an algorithm is presente here for trajectory replanning after a hazar avoiance maneuver has been enacte. Here, the effectiveness of the propose hazar avoiance an replanning algorithms is emonstrate through eperimental results of a UGV moving at high spees over flat an slope terrain. It is shown that the algorithms operate favorably in harsh, real worl conitions.. PROBLEM STATEMENT AND ASSUMPTIONS Here the problem of high spee UGV hazar avoiance in natural terrain is aresse. High spee is loosely efine as spees that inuce vehicle ynamic effects incluing wheel slip, sieslip, roll-over, an ballistic motion. Hazars are efine as iscrete objects or terrain features that significantly impee or halt UGV motion, such as trees, boulers, or patches of very soft soil. Hazars are assume to be etecte from on-boar range sensors. Hazar etection an sensing are not a focus of this work. The UGV is assume to be following a pre-planne path erive from coarse map ata. The goal of the algorithm is to rapily plan maneuvers that permit the UGV to avoi unepecte hazars while consiering vehicle ynamics, steering ynamics, vehicle/terrain interaction, an vehicle performance limits. After the hazar avoiance maneuver is complete, the algorithm must efficiently resume the nominal path, again consiering the above factors. It is assume that coarse estimates of the tire/groun traction coefficient an groun roughness are known or can be etermine online. Techniques for measuring or estimating the above parameters are available (Dugeon an Gopalakrishnan, 996; Iagnemma an Dubowsky, 4). It is also assume that vehicle inertial an kinematic properties are known with reasonable uncertainty. The vehicle is assume to be equippe with a range sensor for measuring terrain elevation an locating hazars up to 3 vehicle lengths ahea, an inertial navigation sensor that can measure the vehicle s roll, pitch, yaw, an a global positioning system for measuring vehicle position. 3. TRAJECTORY SPACE DESCRIPTION The hazar avoiance algorithm is base on the trajectory space, a two-imensional space of a vehicle s instantaneous path curvature, κ, an longituinal velocity, v. Fig. is an illustration of the trajectory space with icons epicting a vehicle s actions corresponing to various points in the space. / v Low Velocity High Velocity Fig. : Representation of vehicle action as escribe by its location in the trajectory space. The trajectory space is a convenient space for navigation for two reasons. First, constraints can be impose on the space to yiel a compact representation of a vehicle s critical performance limits over uneven terrain. These constraints inclue ynamic roll over, sie slip, steering mechanism limits, over/unersteer, an acceleration, braking, an steering rate limits. Secon, the trajectory space maps easily to the UGV actuation space (generally consisting of the throttle/brake an steering angle). This section provies a very brief summary of the application of the trajectory space to UGV navigation. A more complete escription, incluing the effects of rough terrain on the analysis can be foun elsewhere (Spenko 5). 3.. The Dynamic Trajectory Space, A The ynamic trajectory space consists of curvature an velocity pairs (v, κ) that o not cause ecessive sie slip or rollover an are attainable consiering vehicle steering effects (i.e. over/unersteer). As an eample, rollover constraints can be calculate using the low-orer moel shown in Fig.. A terrain patch is escribe by its average roll (φ), pitch (ψ), roughness (ϖ), an traction coefficient (µ).

3 z y mg y mg z A y z mg z mv +mg Fig. : Low-orer rollover moel For a UGV with a wheelbase greater than its track with, rollover most commonly occurs when the moment about either of the points A in Fig. is equal to zero. Thus the constraint function for the rollover space can be efine as: ma,min g z ± hg κ rollover = () hv where is one half the ale length, h is the center of mass height, an v is the UGV s longituinal velocity. The two solutions correspon to upslope/ownslope travel. Constraints for sie slip an steering effects can be erive from similarly low-orer moels. Details are presente in Spenko, 5. Fig. 3 illustrates the effect of terrain inclination on the ynamic trajectory space rollover limits. This eample correspons to a HMMVW-class vehicle traversing a sie slope of 3 eg with the fall line perpenicular to the vehicle s heaing. As epecte the vehicle can safely eecute ownhill turns (negative curvature) with greater velocity than it can eecute uphill turns ue to the interacting effects of gravity an centripetal acceleration. h A Path Curvature (/m) Trajectory Space for a HMMWV on Flat Groun Dynamic Trajectory Space Reachable Trajectory Space Fig. 4: Reachable trajectory space 3.3. The Hazar Trajectory Space, H The hazar trajectory space consists of curvatures an velocities that, if maintaine from the current UGV position, woul lea to intersection with hazars (see Fig. 5). There is no limit to the number of hazars involve. Here a point vehicle representation is employe. min obstacle ma obstacle Hazar Trajectory Space Steering Limits Roll Limits Flat Terrain Roll Limits Slope Terrain Fig. 5: Illustration of hazar trajectory space. 4. HIGH SPEED HAZARD AVOIDANCE During high-spee navigation, emergency situations are likely to occur that require a UGV to rapily perform a hazar avoiance maneuver. The two funamental issues are ) hazar representation, an ) hazar avoiance maneuver selection. These are iscusse below Fig. 3: Dynamic trajectory space limits for varying terrain roll angles (UGV wheelbase =.5 m). 3.. The Reachable Trajectory Space, B The reachable trajectory space consists of velocity an curvature pairs that can be transitione to in a finite time t. It is a function of the current UGV curvature an velocity as well as actuator, acceleration, braking, an steering characteristics. Fig. 4 shows a sample reachable trajectory space overlai on the ynamic trajectory space for a HMMWV size vehicle with a current location in the trajectory space of ( v =., κ =.). Steering rate limits are here fie such that κ& ma =. 5. Details of computation of B are given in Spenko, Hazar Representation Here a scenario similar to that illustrate in Fig. 6 is assume. A UGV attempts to follow a pre-planne nominal trajectory given by a high-level path planner, τ nominal ( v( ), κ ( ) ), where esignates the UGV position in space. If any of the hazars etecte by an onboar range sensor poses a threat, the UGV enacts an emergency hazar avoiance maneuver. The sensor scan is ivie into n iscrete vehicle-size patches an an ATS corresponing to each patch is compute. The size an number of these patches, sensor accuracy, an throughput are important issues, but are beyon the scope of this paper.

4 Sensor Scan Hazar nominal Fig. 6: ATSs efine with hazar present. Let N i enote the ATS for a patch that τ nominal intersects. Let N traj be efine as the intersection of all N i, i.e. Ntraj N... N, where m is the m number of patches that τ nominal intersects. A maneuver is enacte when a hazar lies on the vehicle s current esire path or when a part of τ will violate a constraint on nominal N (i.e. a UGV traj is commane to follow a ynamically inamissible trajectory for a given terrain). 4.. Hazar Avoiance Maneuver Selection To etermine which maneuver to enact, let the total amissible trajectory space be efine as the intersection of all ATSs in the sensor scan minus the hazar space, H: N total ( N N ) H... n () Let τ escribe the UGV velocity an curvature at the current position. The goal of hazar avoiance is to fin τ ( ) τ ( ) Ntotal where τ represents the hazar avoiance maneuver. The maneuver thus transitions the vehicle from a location that violates an ATS constraint to one that oes not violate a constraint on any patch in the surrouning terrain. There are numerous techniques for fining a τ that results in a goo maneuver. The following metho was aopte for its simplicity. First, the trajectory space is iscretize into i closely space gri points. τ is chosen as the location in the trajectory space that minimizes the istance,, from the current location in the trajectory space, τ = ( v,κ ), to a caniate point: = κ ma K κ min ( κ κ ) + ( v ) K i v i vma (3) where K an K are static non-negative gain factors. These factors affect the relative weighting of changes in velocity an curvature. The minimum istance over N total can be foun using a variety of search techniques. The resulting τ represents a ynamically amissible curvature an velocity pair that avois hazars in the current sensor scan. A low-level control algorithm is then employe to comman the UGV along the new trajectory. 5. PATH RESUMPTION After a hazar avoiance maneuver is eecute, the UGV must plan a kinematically an ynamically feasible path to return to the pre-planne nominal path. Assuming constant velocity, v, the state of a front-steere rear-rive wheele vehicle can be escribe by the following couple nonlinear equations. κ () s = u() s θ () s = v κ () s L () s = v cosθ () s s y() s = v sinθ () s L L s s (4) where s represents the vehicle istance along a path, u ( s) is the steering input, θ ( s) is the vehicle heaing angle, an ( s), y( s) is the vehicle position in space. Consier the situation illustrate by the plot shown in Fig. 7. Here the soli line represents a pre-planne nominal maneuver s curvature in path coorinates. A hazar avoiance maneuver is eecute at a, an the maneuver ens at b. The curvature of the nominal esire path, hazar avoiance maneuver, an path resumption maneuver are efine as κ () s, κ ( s), an κ 3( s) respectively. The goal of the path replanning problem is to fin κ 3() s in a computationally efficient manner such that: ( ( c), θ ( c), ( c), y( c) ) ( κ ( ), θ ( ), ( ), y( )) 3 κ = (5) Hazar Avoiance Maneuver ( ) ma Path Resumption Maneuver ( ) s a s b s c s Nominal Desire Curvature ( ) Fig. 7: Curvature iagram where c is the esire meeting point of the replanning maneuver an the nominal trajectory, an is the terminal point of the replanning maneuver. A computationally efficient replanning metho terme the curvature matching metho is presente here. An outline of the metho is presente below:. Make an initial choice of the meeting point on the nominal trajectory. Here c is initially chosen such that ( c b) = ( b a) an is initially chosen to be the smallest value such that it is possible to transition from κ ( b) to κ 3( ) without violating & κ & κ ma κ s such that:. Fin ( ) 3 c () s s = κ () s s + κ 3() s a a b This ensures that ( ) ( ) b κ s (6) θ c = θ 3. The curvature, κ 3, must also stay within the bounaries of the total amissible trajectory space. Details of this computation are given in (Spenko, 5). 3. Calculate 3 ( ) an ( ) 4. If 3 ( ) an ( ) y 3 using (4). y 3 are within the acceptable threshol the algorithm ens. If not, c an are ajuste as: s

5 c i+ i c ( lon ) i+ = c k = k i ( e ) e lat (7) where k c an k are ajustable gains an e lon an e lat are the longituinal an lateral error respectively an are efine by: ( () c 3( )) cosθ() c + ( y() c y3( )) sinθ( c) ( ( ) ( c) ) sinθ () c + ( y () c y ( )) cosθ ( c) elon = elat = 3 3 (8) Although algorithm convergence cannot be formally guarantee, etensive analysis has shown that the metho performs well in practice (Spenko, 5). 6. EXPERIMENTAL RESULTS A select number of the etensive eperimental trials of the hazar avoiance maneuver conucte on the Autonomous Rough Terrain Eperimental System (ARTEmiS) shown in Fig. 8 are escribe here. ARTEmiS measures.88l.6w.38h m an has.5 m iameter pneumatic tires. It is equippe with a.5 Hp Zenoah GD7 gasoline engine, Crossbow AHRS-4 inertial navigation system, Novatel DGPS capable of. meter resolution, Futaba S55 servos for steering, brakes, an throttle, an a PIII 7 MHz PC4 computer. Instea of forwar-looking range sensors, using knowlege of ARTEmiS position, hazar locations are reveale once they are within the range of a virtual sensor. The sensor range varie among eperiments from m to m ( to 35 times the vehicle wheelbase). As epecte, given similar eperimental conitions a reuce sensor range results in hazar avoiance maneuvers that are usually more severe an performe at lower-spees. Fig. 8: ARTEmiS eperimental UGV 6.. Eperimental Valiation of Trajectory Space Constraints The accuracy of the trajectory space roll over constraints was stuie eperimentally on flat terrain at spees up to 8 m/s (see Fig. 9). The vehicle was commane on a esire path consisting of a straight line followe by a clothoi segment. Roll over was efine as occurring when a y gh, where a y is the lateral acceleration of the vehicle, g is gravity, h is the height of the vehicle center of mass an is one-half the ale with. This simple metric is commonly use for rollover stuies in the passenger vehicle inustry. Due to the high traction coefficient (µ.3), roll over occurre before ecessive sie slip. The eperimental ata matches the preicte ynamic limit well. The most prevalent source of error is the calculation of the path curvature, which can be highly sensitive to the GPS an INS position estimates Theoretical Moel Eperimental Data Fig. 9: Eperimentally valiate trajectory space constraints for flat groun 6.. High Spee Multiple Hazar Avoiance Fig. shows three snapshot subplots of an eperiment for high spee avoiance of two hazars performe on a grass fiel at 6 m/s. The nominal esire path was a straight path m long. The first hazar was etecte at = 6.4 m, shown in the top subplot of Fig.. At this point hazar avoiance an path resumption maneuvers were eecute. ARTEmiS followe the moifie path until a secon hazar was etecte at = 34. m, shown in the mile subplot of Fig.. A secon maneuver was eecute an ARTEmiS successfully complete the path, as shown in the lower section of Fig.. Y (m) First Hazar Hazars Desire Path 5 Actual Path Detecte Secon Hazar 5 Detecte Path Complete X (m) Fig. : Eperimental results of hazar avoiance maneuvers eecute for multiple hazars Fig. shows the trajectory spaces at the two instants that the hazars were etecte. The asterisks epict ARTEmiS location in the trajectory space. For the first hazar, ARTEmiS moifie its trajectory from τ = ( 6.,.) to τ = ( 6.,.3). For the secon hazar ARTEmiS moifie its trajectory by maintaining its current location insie the trajectory space until beyon the hazar..4.. First Hazar Velocity (m).4.. Secon Hazar Velocity (m) Fig. : Trajectory spaces at time of hazar etection

6 6.3. Hazar Avoiance on Rough Terrain Eperiments on rough terrain were performe at Minute Man National Historic Park (see Fig. ). The terrain consiste of a bumpy, uncut grass fiel with features on the orer of one-half the wheel raius. The nominal esire path is a m long straight path. The hazar consists of a cluster of tall brushes, an small trees. Terrain roughness influences UGV mobility by inucing variation in the wheel normal forces. This can be represente as variation in the location of trajectory space constraints (Spenko, 5). Finish (Hien) eperimentally teste for over 8 hours at spees ranging from 4-9 m/s. It has been valiate on flat an rough terrain with slopes up to 8 egrees. 7. CONCLUSIONS This paper has presente eperimental valiation of a novel hazar avoiance algorithm for high-spee UGVs in natural terrain. A brief escription of the trajectory space was presente, which escribes UGV performance limits on uneven terrain. This information was use as the basis of a hazar avoiance algorithm. A computationally efficient path resumption algorithm was also presente. Eperimental results emonstrate the effectiveness of the algorithms. Nominal Path Hazar ARTEmiS ACKNOWLEDGEMENTS This research was supporte by the U.S. Army Tankautomotive an Armaments Comman an the Defense Avance Research Projects Agency. The authors woul like to thank Shingo Shimoa, Guillaume Morel, an Dariusz Gola for their assistance with the evelopment of ARTEmiS. Fig. : Rough terrain eperimental setup. Fig. 3 shows three snapshot subplots of the eperiment performe at a spee of 7. m/s. ARTEmiS etecte the first hazar at =.4 m. This is shown in the top subplot of Fig. 3. At this point hazar avoiance an path resumption maneuvers were eecute, as shown in the mile subplot of Fig. 3. The lower section of Fig. 3 shows the complete path. Y (m) Hazar Detecte Here Start Desire Path Actual Path Hazar Path Re planne Path Complete X (m) Fig. 3: Rough terrain eperimental results. This eperiment emonstrates that the propose hazar avoiance algorithm can be applie to UGVs operating at high spees on rough terrain. These conitions are epecte to be similar to actual operating conitions for many practical applications. The above results are representative of a boy of eperimental stuies that have been performe to analyze the performance of the propose algorithm. Generally, it has been observe that the algorithm quickly an effectively generates ynamically safe emergency hazar avoiance maneuvers for UGVs traveling a high spees. The algorithm has been REFERENCES Coombs, D., et al. (). Driving autonomously offroa up to 35 km/h. Proc. of the IEEE Intelligent Vehicle Symposium, Daily, M. et al. (988). Autonomous cross-country navigation with the ALV. Proceeings of the IEEE ICRA,, Dugeon, J. an R. Gopalakrishnan (996) Fractalbase moeling of 3D terrain surfaces. Proc. of the IEEE Conf. on Bringing Together Eucation, Science, an Technology, Fo, D., W. Burgar, an S. Thrun, (997) The ynamic winow approach to collision avoiance. IEEE Robotics an Automation, 3, Iagnemma, K., an S. Dubowsky (4) Mobile Robots in Rough Terrain, STAR Series on Avance Robotics, Springer. Kelly, A., an A. Stentz. (998) Rough terrain autonomous mobility part : a theoretical analysis of requirements. Autonomous. Robots Langer, D., J.K. Rosenblatt, an M. Hebert. (994) A behavior-base system for off-roa navigation. IEEE Robotics an Automation,.6, Laugier, C. et al. (998) Sensor-base control architecture for a car-like vehicle. Proc. of IEEE ICRA 6-. Olin, K. an D. Tseng, (99) Autonomous crosscountry navigation: an integrate perception an planning system. IEEE Epert, 6.4, 6-3. Spenko, M., K. Iagnemma, an S. Dubowsky, (4) High spee hazar avoiance for mobile robots in rough terrain. Proc. of SPIE Unmanne Groun Vehicle Technology VI, 54 Spenko, M. (5) Hazar Avoiance for High-Spee Rough-Terrain Unmanne Groun Vehicles Ph.D. Thesis. Massachusetts Institute of Technology.