R OBOTIC M OBILITY G ROUP

1. Human-inspired Vehicle Navigation Through Fields of Safe Travel Karl Iagnemma (PI), Sterling Anderson, Steve Peters ROBOTIC MOBILITY GROUP

2. Outline MURI Research Program Context ARO MURI (Iagnemma) DARPA M3 Program High Speed Teleoperation Semi-Autonomous Control High Speed Autonomous Control Terrain Estimation ARO MURI (Frazzoli) ARO MURI (Tsiotras) Efficient Kinodynamic Planning High Speed Autonomous Control

3. Outline • Introduction to Semi-Autonomous Control • Need • Problem Statement • Constraint Planning Algorithm Development • Objectives • Constraint Planning • Constraint Enforcement • Experimental Implementation (DARPA M3 leverage) • Setup • Results • Conclusions Outline

4. Introduction Algorithm Development Experimental Implementation Conclusion • Passenger Vehicles (2008)1 • 37,261 deaths • 2.3 million injuries • $230 Billion economic cost • Manned Military Vehicles2 • 248 deaths caused by ground vehicle crashes during Operation Iraqi Freedom • Unmanned Military Vehicles3 • 6-20 hour mean time between failure • Industrial vehicles (forklifts, cranes, etc.)4 • 900 deaths and 94,000 injuries from forklift-related accidents in USA (1990) • Driver error the primary cause of accidents5 • Sole factor in 60% • Contributing factor in 95% Need Context The Need for Semi-Autonomous Planning and Control [1] National Highway Traffic Safety Administration, 2008 Traffic Safety Annual Assessment [2] Defense Manpower Data Center, Statistical Information Analysis Division, Military Casualty Summary: Global War on Terrorism, 2010 [3] J. Carlson and R. Murphy, “How UGVs physically fail in the field,” Robotics, IEEE Transactions on, vol. 21, 2005, pp. 423-437 [4] Industrial Forklift Truck Fatalities – A Summary, Report from Office of Data Analysis, OSHA, June 1990 [5] L. Evans, "The dominant role of driver behavior in traffic safety," American Journal of Public Health, v. 86, n. 6, pp. 784-786, 1996.

5. Introduction Algorithm Development Experimental Implementation Conclusion • Forklift operation (in USA)1 • One in six of all workplace fatalities in US are forklift related • Collisions with pedestrians • 100 deaths, >20,000 injuries • Other forklift-related accidents • 900 deaths, >94,000 injuries Need Context The Need for Semi-Autonomous Planning and Control [1] United States Department of Labor Occupational Safety & Health Administration, “Section 10 - X. Summary of the Final Economic Analysis, including the Regulatory Flexibility Analysis.” [2] Industrial Forklift Truck Fatalities – A Summary, Report from Office of Data Analysis, OSHA, June 1990

6. Introduction Algorithm Development Experimental Implementation Conclusion Need Context Problems With Manned Vehicle Control [1] Office of Electrical/Electronic and Mechanical Engineering Safety Standards, Directorate of Safety Standards Programs OSHA, 1990

7. Introduction Algorithm Development Experimental Implementation Conclusion Need Context Problems With Manned Vehicle Control [1] Office of Electrical/Electronic and Mechanical Engineering Safety Standards, Directorate of Safety Standards Programs OSHA, 1990

8. Introduction Algorithm Development Experimental Implementation Conclusion • Compelling reasons to keep humans “in the loop” • Superior judgment and reasoning capabilities1,2 • High automation costs • Significant socioeconomic pressures • Technical challenges • How to define safe operating region for systems? • Constraint definition in Cartesian space, state space, input space • How to safely share control? • Threat assessment to determine when, how much to intervene Need Context The Need for Semi-Autonomous Planning and Control [1] Fitts, P.M., et al. “Human engineering for an effective air navigation and traffic control system”. Washington, DC: National Research Council, 1951. [2] Sheridan, T. “Computer control and human alienation”. Technology Review, 1980, pp. 10,61-73.

9. Introduction Algorithm Development Experimental Implementation Conclusion Need Context Traditional Approaches • Traditional vehicle navigation approaches are path-based • Plan path around hazards • Cost-based • Track path with low-level controller • Reasonable for autonomous control • Path-based approaches are not well suited for semi-autonomous operation • Restricting motion to specific paths can result in overly restrictive intervention • Humans do not follow paths • Operate within “field of safe travel” • Paths within field can have equal “goodness” f(s1)=k1 f(s2)=k2 f(s3)=k3 [1] Leonard et al., 2008 Besselmann and Morari, 2008 Keviczky, Falcone, et al., 2007 Borrelli, Falcone, et al., 2005 [2] Keifer et al., 2005 Brunson et al., 2002 Engelman et al., 2006

10. Introduction Algorithm Development Experimental Implementation Conclusion Need Context Traditional Approaches [1] J. J. Gibson and L. E. Crooks, “A Theoretical Field-Analysis of Automobile-Driving,” The American Journal of Psychology, vol. 51, no. 3, pp. 453-471, Jul. 1938.

11. Introduction Algorithm Development Experimental Implementation Conclusion • Objective Guaranteed safety in high-speed vehicle operation in challenging environments through semi-autonomous planning and control • Key Insight • Human operators tend to navigate within fields of safe travel • Paths within field can have equal “goodness” • Approach Manage driver/machine interaction through the planning and selective application of constraints • Define constraint set over some horizon • Identify field(s) of safe travel • Characterize goodness of constraint set • Selectively enforce constraints based on threat • Allow driver freedom to choose specific path • Requirements • Constraint planner • Threat assessor • Semi-autonomous intervention mechanism Objectives Constraint Planning Constraint Enforcement Constraint-Based Semi-Autonomous Control

12. Introduction Algorithm Development Experimental Implementation Conclusion • Constraint planning objective: identify, characterize, and select a single “best” constrained region • Spatial constraints (from environment) • Velocity constraints (due to speed limits, power limitations, etc.) • Stability constraints (from vehicle dynamics and environmental interaction) • Input constraints (from actuator limits) • Challenges • Planning in “constraint space” requires a fundamentally new set of tools • Path-centric metrics such as length, curvature, etc. ill-defined in the context of a constraint space • Candidate constraint sets may contain an infinite set of feasible paths • Characterizing a given constraint space may require a bulk evaluation of these paths Objectives Constraint Planning Constraint Enforcement Constraint Planning

13. Introduction Algorithm Development Experimental Implementation Conclusion • Method • Plan a feasible path through environment • Expand path to encompass the homotopic class to which it belongs • HomotopicClass: Set of paths that can be continuously deformed into one another • Advantages • Computationally feasible • Constraints admit at least one feasible path • Problems • “Best” homotopy class may not contain optimal path • Implications for both semi-autonomous and autonomous control • Homotopic class goodness difficult to assess • Relationship between class properties and vehicle dynamics difficult to define • Robustness/feasibility of homotopic class difficult to quantify Objectives Constraint Planning Constraint Enforcement Naïve Approach: Path-Based Constraint Planning

14. Introduction Algorithm Development Experimental Implementation Conclusion • Challenge • Efficient identification and characterization of available homotopic classes • Approach • Partition environment into cells • Constrained Delaunay Triangulation • Identify canonical path linking contiguous cells • Define homotopic class constraints from sequences of contiguous cells • Characterize “goodness” of each cell • “Length” and “width” from canonical path • Dynamic reachability across adjacent edges Objectives Constraint Planning Constraint Enforcement Constraint Planning Based on Homotopic Classes Obstacles Generalized Voronoi Diagram Delaunay Triangulation Canonical path Chosen Corridor

15. Introduction Algorithm Development Experimental Implementation Conclusion Objectives Constraint Planning Constraint Enforcement Triangulation-Based Homotopy Characterization – Coarse Method • Coarse method for constraint set characterization • Key idea: Maximum speed and input freedom within a given corridor related to: • Corridor width • Required heading changes ( path curvature) • Constraint set goodness also driven by approximate “length” of corridor • Result: Dijkstra on cost function: L4 L0 ϕ3 L3 L1 w1 w3 L2 ϕ2 ϕ1 w2

16. Introduction Algorithm Development Experimental Implementation Conclusion Objectives Constraint Planning Constraint Enforcement Coarse Constraint Characterization in Semi-Autonomous Application

17. Introduction Algorithm Development Experimental Implementation Conclusion • Fine method for constraint set characterization • Key idea: Employ reachability analysis for homotopic class characterization • Reachability analysis yields metrics on available inputs • Two candidate metrics: • “Restrictiveness” of input constraints at time t0 • Restrictiveness of input constraints “in aggregate” through homotopy • At time t0, ΔSteer1 (t0) >ΔSteer2 (t0) • Homotopy1 contains paths that require less aggressive steering commands • Homotopy2in aggregate provides greater control freedom Objectives Constraint Planning Constraint Enforcement Triangulation-Based Homotopy Characterization – Fine Method ΔSteer1 (t0) ΔSteer2 (t0)

18. Introduction Algorithm Development Experimental Implementation Conclusion • Transition feasibility • Any path existing within a given homotopy described by a sequence of triangular cells must traverse from one unconstrained edge to the other without colliding with the constrained edge • Any homotopic path that successfully reaches the goal from the vehicle’s current position must be (forward) reachable from the vehicle, and (backward) reachable from the goal • Characterizing the “restrictiveness” of a given homotopy may be possible via an aggregate assessment of admissible transitions between triangles Objectives Constraint Planning Constraint Enforcement Constraint Characterization: Transition Feasibility & Reachable Sets Goal ψ ψmax 180 ψmin 90 x3 a b

19. Introduction Algorithm Development Experimental Implementation Conclusion Objectives Constraint Planning Constraint Enforcement Constraint Characterization: Transition Feasibility & Reachable Sets • Reachable sets a function of triangle dimensions and adjacency relations  Efficient set propagation  Close approximation of state and control “restrictiveness” of a given homotopic class • Homotopy “restrictiveness” metric may be used in constraint planning objective Goal ψ ψmax 180 ψmin 90 x3 a b

20. Introduction Algorithm Development Experimental Implementation Conclusion Objectives Constraint Planning Constraint Enforcement Control Input Constraints and Threat Assessment • Identify constrained input range from homotopic class constraints • Identify current optimal control input from with constrained MPC • Associated with optimal trajectory • “Threat level” computed from optimal trajectory • Describes severity of best-case trajectory in given homotopy • Various potential metrics computed over optimal trajectory • Maximum vehicle roll angle, RMS of tire forces, etc.

21. Introduction Algorithm Development Experimental Implementation Conclusion • Wheel resistance a function of predicted threat F • Torque constraints on operator tighten as threat (i.e. need for intervention) increases Objectives Constraint Planning Constraint Enforcement Semi-Autonomous Constraint Enforcement ks Restoring Torque (T) Steering Position Optimal Input Physical Limits

22. Introduction Algorithm Development Experimental Implementation Conclusion • Wheel resistance enforces steering constraints required to remain within safe homotopy • Input constraints (Δmin and Δmax) on operator tighten to ensure vehicle remains within safe set • Overlapping region allows driver to bias toward desired homotopy Objectives Constraint Planning Constraint Enforcement Semi-Autonomous Constraint Enforcement Restoring Torque (T) Steering Position ΔSteer1 (t0) ΔSteer2 (t0) Obstacle Avoidance Road Edge Lane Edge Stability Limit Physical Limits

23. Introduction Algorithm Development Experimental Implementation Conclusion • DARPA M3 Program (DSO, PM: Gill Pratt) • Focus: High speed vehicle teleoperation • Collaboration with Quantum Signal, LLC • 9+ acre property in Saline, MI • Various terrain types • Off road: scrub grass, tall grass, meadow, some culverts • Semi-prepared roads: smooth gravel, rough gravel • Prepared roads • Varying slopes • None, moderate (2-10 deg), severe (10-45 deg) • Varying roughness • Obstacles - trees, mud, sloped terrain, barrels, structures, etc Setup Results DARPA M3 Program Collaboration

24. Introduction Algorithm Development Experimental Implementation Conclusion • Hardware (Kawasaki Mule) • Sensing: omnidirectional video head, NavCom 2050 GPS, Velodyne LIDAR • Onboard Linux PC and processor and control unit run sensing, constraint planning, and controller code • NLOS teleoperation via forward-looking imagery with significant comms latency Setup Results UGV Platform

25. Introduction Algorithm Development Experimental Implementation Conclusion • Teleoperation in a field of obstacles • Off-road (scrub grass, tall grass, meadow) • Gravel roads (smooth & rough) • Paved roads & parking lot • Varying slopes and roughness • Obstacles field • Shrubs & plastic barrels Setup Results Test Setup goal start

26. Introduction Algorithm Development Experimental Implementation Conclusion Constraint planner designs constraints to avoid sensed obstacles & vehicle instability Semi-autonomous controller ensures vehicle trajectory satisfies constraints Setup Results Experimental Results Position constraints designed to avoid hazards Optimal maneuver identified to characterize scenario threat

27. Introduction Algorithm Development Experimental Implementation Conclusion Setup Results Experimental Results

28. Introduction Algorithm Development Experimental Implementation Conclusion Conclusions

29. Introduction Algorithm Development Experimental Implementation Conclusion • Algorithmic Advances • Homotopy-based constraint planning • Model-based threat assessment • Constraint-based semi-autonomy • Experimental Achievements • Seamless integration of operator commands with controller corrections • 100% safe obstacle avoidance and stability maintenance in the presence of: • Operator error • Loss of communications • Loss of operator attention • High communication latency • Plans going forward • Bring new constraint planning tools to bear • Explore alternative force-feedback-based constraint enforcement mechanisms Conclusions

30. Introduction Algorithm Development Experimental Implementation Conclusion • Conference Papers—Submitted, published, and in preparation • Anderson, S.J, Peters, S.C., Overholt, J., Iagnemma, K.D., “Semi-Autonomous Stability Control and Hazard Avoidance for Manned and Unmanned Ground Vehicles", Proc. 27th Army Science Conference, 2010. • Peters, S., Frazzoli, E., and Iagnemma, K., “Differential Flatness of a Vehicle with Tire Force Control,” Proceedings of the IEEE International Conference on Robots and Systems, September 2011, pp. 298-304 • Journal Papers—Submitted, published, and in preparation • Anderson, S.J, Peters, S.C., Pilutti, T.P., Iagnemma, K.D., “An Optimal-Control-Based Framework for Trajectory Planning, Threat Assessment, and Semi-Autonomous Control of Passenger Vehicles in Hazard Avoidance Scenarios,” Intl Journal of Vehicle Autonomous Systems, Vol. 8, Nos. 2/3/4, pp.190-216. • Arndt, D., Bobrow, J., Peters, S., Iagnemma, K., and Dubowsky, S., “Two-Wheel Self-Balancing of a Four-Wheeled Vehicle,” IEEE Control Systems Magazine, Vol. 31, No. 2, pp. 29-37, April 2011 • Peters, S., Frazzoli, E., and Iagnemma, K., “Differential Flatness of a Front-Steered Vehicle with Tire Force Control,” Automatica, in preparation • Peters, S., Frazzoli, E., and Iagnemma, K., “Yaw stability analysis for vehicle control with front, rear, and all-wheel-drive configurations,” Vehicle System Dynamics, in preparation • Peters, S., and Iagnemma, K., “Optimal avoidance maneuvers for a point mass with acceleration circle constraint,” Automatica, in preparation • PhD Theses—Completed and in preparation • Peters, S., “Steve’s Award-Winning PhD Thesis,” MIT, expected completion 2011 • Anderson, S., “Sterling’s Award-Winning PhD Thesis,” MIT, expected completion 2012 Conclusions

31. Flatness-based Control of High Speed Vehicles