I Introduction
Success in autonomous offroad driving will be assured in part through the use of diverse static and realtime data sources in planning and control decisions. A vehicle traversing complex terrain over a long distance must define a route that balances efficiency with safety. Offroad driving requires including both global and local metrics in path planning. Algorithms such as A* and D* [1] for waypoint definition and sequencing can be combined with local path planning strategies [2] to guarantee obstacle avoidance given system motion constraints and terrain properties such as slope and surface composition.
Offroad navigation studies todate have primarily focused on avoiding obstacles and improving local driving paths. A* [3] and dynamic programming [4] support globallyoptimal planning over a discrete grid or predefined waypoint set. In Ref. [5], the surface slope is considered during A* search over a gridbased mobility map. Slope is used to assign feasible traversal velocity constraints that maintain acceptable risk of lossofcontrol (e.g., spinout) or rollover. In Ref. [6], local driving path optimization is studied, while Ref. [7] applies a Pythagorean Hodograph (PH) [8] cubic curve to provide a smooth path that avoids obstacles by generating a kinematic graph data structure. Vehicle models that consider continuouslyvariable (rough) terrain are introduced in Refs. [9], [10], [11], [12] and [13].
This paper proposes a datadriven approach to autonomous vehicle motion planning and control for offroad driving scenarios. The decisionmaking architecture consists of three layers: (i) Global Route Planning (GRP), (ii) Local Path Planning (LPP), and (iii) Feedback Control (FC). The GRP planning layer assigns optimal waypoints using dynamic programming (DP) [14, 15]. The GRP must rely on cloud and stored database information to define and sequence waypoints beyond onboard sensor range (lineofsight). In this paper, DP cost is defined based on realistic weather and geographic data provided by the National Center for Environmental Information in the National Oceanographic and Atmospheric Administration (NOAA) along with geographical information system (GIS) data. The LPP computes a continuoustime trajectory between optimal waypoints assigned by the GRP. The FC applies a nonlinear controller to asymptotically track desired vehicle trajectory. To our knowledge, this is the first publication in which NOAA weather and GIS data provide input into autonomous offroad driving decisionmaking.
Autonomous offroad driving has been proposed for multiple applications. The DARPA Grand Challenge series and PerceptOR program have led to numerous advances in perception and autonomous driving decisionmaking. For example, Ref. [16] proposes a threetier deliberative, perception, reaction architecture to navigate an offroad cluttered environment with limited GPS availability and changing lighting conditions. A review of navigation or perception systems relevant to agricultural applications is provided in Ref. [17]. Because agriculture equipment normally operates in open fields with minimal slope, precision driving and maneuvering tends to be more important than evaluating field traversability. Ref. [18] describes how LIDAR and stereo video data can be fused to support offroad vehicle navigation, providing critical realtime traversability information for the area within range of sensors. To augment LIDAR and vision with information on soil conditions, Ref. [19] proposes use of a thermal camera to provide realtime measurements of soil moisture content, which in turn can be used to assess local traversability. Our paper provides complementary work that incorporates GIS and cloudbased (NOAA) data sources to enable an offroad vehicle planner to build a traversable and efficient route through complex offroad terrain. Onboard sensors would then provide essential feedback to confirm the planned route is safe and update databaseindicated traversability conditions as needed.
A second contribution of this paper is a feedback linearization controller for trajectory tracking over nonlinear surfaces. Most literature on vehicle control and trajectory tracking usually assumes that the car moves on a flat surface [20, 21, 22, 23]. In Ref. [23], model predictive controller (MPC) is deployed for trajectory tracking and motion control on flat surfaces. Ref. [9] introduces a vehicle body frame for motion over a nonlinear surface and realizes velocity with respect to the local coordinate frame. Bases of the body and ground (world) coordinate system are related through Euler angles , , . A firstorder kinematic model for motion over a nonlinear surface is presented in Ref. [9].
In this paper, we extend the kinematic car model given in Refs. [24, 25] by including both position and velocity as control states. The car is modeled by two wheels connected by a rigid bar. Sideslip of the rear car wheel (axle) is presumed zero. Car tangent acceleration (drive torque) and steering rate control inputs are chosen such that the desired trajectory on a nonlinear motion surface is asymptotically tracked.
Ii Preliminaries
Three motion planning and control layers are integrated to enable offroad autonomous driving. The top layer is ”global route planning” (GRP) using dynamic programming (DP) as reviewed in Section IIA. The second layer, local path planning (LPP), assigning a continuoustime vehicle trajectory to waypoints sequenced by the GRP via path and speed planning computations. The paper defines a deterministic finite state machine (DFSM) [26] to specify desired speed along the desired path. Elements of a DFSM are defined in Section IIB. The paper presents a nonlinear feedback control approach for trajectory tracking over an arbitrary motion surface as the innerloop (third) layer. Motion of the car is expressed in coordinate frames defined in Section IIC using driving dynamics given in Section IID.
Iia Dynamic Programming
A dynamic programming (DP) problem [14] can be defined by the tuple
where is a set of discrete states with cardinality , and is the set of discrete actions with cardinality . Furthermore, is the cost function and is a deterministic transition function.
The Bellman Equation defines optimality with respect to action selected for each state :
(1) 
where is the utility or value function. Therefore,
(2) 
assigns the optimal action for each state . Case study simulations in this paper use a traditional value iteration algorithm [27] to solve the Bellman equation.
IiB Deterministic Finite State Machine
The behavior of a discrete system can be represented by a directed graph formulated as a finite state machine (FSM). In a FSM, nodes represent discrete states of the system and edges assign transitions between states. A FSM is deterministic if a unique input signal always returns the same result. A deterministic finite state machine (DFSM) [26] is mathematically defined by the tuple
where is a finite set of inputs, is the finite set of states, is the set of terminal states, defines transitions over the DFSM states, and is the initial state.
IiC Motion Kinematics
IiC1 Ground Coordinate System
Bases of a groundfixed or inertial coordinate system are denoted , , and , where and locally point East and North, respectively. Position of the vehicle can be expressed with respect to ground coordinate system by
(3) 
Because the ground coordinate system is stationary, , , and . Velocity and accelration are assigned by
(4) 
It is assumed that the vehicle (car) moves on a surface
(5) 
IiC2 Local Coordinate System
Bases of a local terrain coordinate system are denoted , , and , where is normal to surface . Therefore,
(6) 
Bases of the local coordinate system are related to the bases of the ground frame by
(7) 
where
IiC3 Body Coordinate System
The bases of the vehicle body coordinate system , denoted , , and , can be related to , and by
(10) 
where is the yaw or approximate heading angle. Car angular velocity can be expressed by
(11) 
where
(12) 
IiD Vehicle Dynamics
Fig. 1 shows a schematic of vehicle/car configuration in motion plane ; we assume is tangent to the terrain surface. The car is modeled by two wheels connected by a rigid bar with length . In the figure, and are the centers of the front and rear tires, respectively. Given steering angle and car speed , motion dynamics can be expressed by [24, 25]
(13) 
Motion Constraint: This work assumes side slip of the rear tire is zero. This assumption can be mathematically expressed by
(14) 
where
(15) 
is the rear tire relative velocity with respect to the local body frame. Therefore
(16) 
By taking the time derivative of Eq. (13), acceleration of the car is computed as
(17) 
where and are the car tangential acceleration and steering rate, respectively. and are determined by
(18) 
The magnitude of vehicle normal force,
(19) 
must be always positive, e.g. . This guarantees that the car never leaves the motion surface.
Iii Methodology
This section presents the proposed threelayer planning strategy comprised of Global Route Planning (GRP), Local path planning (LPP), and feedback control (FC). GRP applies DP to assign optimal driving waypoints given terrain navigability (traversability), weather conditions, and driving motion constraints. LPP is responsible for computing a trajectory between consecutive waypoints assigned by DP. Local obstacle information obtained during LPP is applied by FC to track the desired trajectory.
Iiia Global Route Planning
DP state set : A uniform grid is overlaid onto a local terrain of the United States. Grid nodes are defined by the set ; the node is considered as an obstacle if:

There exists water at node location ,

Node contains foliage (trees) or buildings, or

There is a considerable elevation difference (steep slope) at node location .
Let the set define obstacle index numbers. Then,
(20) 
defines the DP states. We assume that has cardinality , e.g. .
Transition from node to node is defined by a directed graph. Inneighbor nodes of node are defined by the set
(21) 
where is the cardinality of the set .
DP Actions: DP actions are defined by the set
(22) 
where actions through command the vehicle to drive to an adjacent node directly East, Northeast, North, Northwest, West, Southwest, South, and Southeast, respectively. Action is the ”Stay” command. As shown in Fig. 2, all directions defined by the set may not necessarily be reached from every node . Therefore, actions available at node are defined by .
Transition Function: Let and denote planar positions of nodes and ; land slope over the straight path connecting and is considered as the criterion for land navigability in this paper. We define and as upper bounds for land slope for dry and hazardous (wet) surface conditions, respectively, leading to the following constraints:

In a dry weather condition, can be reached from only when .

In a wet weather condition, can be reached from only when .
Suppose that is the expected outcome state when executing action in . Then, transition function is defined as follows:
(23) 
DP cost: The DP cost at node under DP action is defined by
(24) 
where is the average slope (elevation difference) along the path segment connecting and . Also, is the distance between nodes and . Note that scaling factors and are assigned by
(25) 
where
(26) 
IiiB Local Path Planning
The main responsibility of the local path planner (LPP) is to define a desired trajectory between consecutive waypoints assigned by the DPbased GRP. The LPP also might interact with the GRP to share information about dynamicallychanging obstacle and environment properties (in future work). LPP trajectory computation is discussed below.
IiiB1 Trajectory Planning
Suppose , , and are and components of three consecutive way points, where the path segments connecting these three waypoints are navigable, e.g. the path connecting these three waypoints are obstaclefree. Let
(27) 
then the path segments connecting , , and intersect if .
Remark: We define a nominal speed for traversal along the desired path. If , then should satisfy the following inequality:
(28) 
where is the maximum yaw rate.
Given and , one of the following two conditions holds:
IiiB2 Trajectory Planning State Machine (TPSM)
A trajectory planning state machine (TPSM), shown in Fig. 3, describes how the desired trajectory can be planned given vehicle (i) actual speed , (ii) nominal speed , (iii) turning radius , (iv) maximum yaw rate , and (v) consecutive path segment parameters and . TPSM inputs are defined by the set
TPSM states are defined by
(29) 
where atomic propositions , and are assigned as follows:
Note that is the initial TPSM state. TPSM terminal states are defined by the set
where and command the car to accelerate and decelerate, respectively, and commands the car to move at constant speed.
Transitions over TPSM states are shown by solid and dashed arrows. If (
) is satisfied, transition to the next state is shown by a solid arrow; otherwise, state transition is shown by a dashed vector.
IiiC Motion Control
Suppose
(30) 
defines the desired trajectory of the car over the surface . Let and components of the car acceleration be chosen as follows:
(31) 
The error signal is then updated by the following second order dynamics:
(32) 
The error dynamics is asymptotically stable and asymptotically converges to if and . Given and assigned by Eq. (31), is specified by
(33) 
By knowing , the car control inputs and are assigned by Eq. (18).
Iv Case Study Results
This section describes processing and infusion of map and weather data into our offroad multilayer planner. In Section IIIA, data training and global route planning using dynamic programming are described. A local path planning example is provided in Section IVB, and trajectory tracking results are presented in Section IVC.
Iva Global Route Planning
IvA1 Data Training
The elevation data used for generating the gridbased map is downloaded from the United States Geographic Survey (USGS) TNM download [28], Elevation Source Data (3DEP). The USGS elevation data is in ”.las” format and needed to be transformed into a grid map. An online tool is used to transform the data into ”.csv” format (see Ref. [29]). After data processing, we can obtain a map with raw elevation data for input to planning.
Two different locations are chosen for this study. One is a mountainous area near the Ochoco National Forest, Oregon. The center location coordinate is [30]. The second locale is in Indiana, near Lake Michigan, which is a relatively flat terrain area. The center location coordinate for the Indiana region is [31].
The surface maps and contour plots of both areas are shown in Fig. 5. The first Mountain area (Oregon Forest) has an average altitude of feet and is covered by trees. The left lower area of the map has higher elevation and is covered with fewer trees.
The second land area (Indiana) is flat with only several trees and roads as notable features. This area offers easier traversability in all weather conditions than the mountainous region.
The weather data are downloaded from a National Center for Environmental Information (NOAA) website. Thee database contains temperature, weather type, and wind speed information. The weather is almost the same across each regions being traversed but it varies over time. If the weather is severe, such as snowy and rainy, the weather is called harsh or wet. The constraint (threshold) on driving slope is decreased to under a wet weather condition. If the weather is dry, the slope constraint is set to .
By using the two typical land type elevation maps, global route planning simulation results are generated using DP. A grid map is obtained by spatial discretization of the study areas. A DP state represents a node in the grid map. As mentioned in Section IIIA, dicrete actions assign motion direction at a node . Given , the next waypoint given is considered unreachable if elevation change along the connecting path exceeds applicable upperbound limit or . GRP case study results for Oregon and Indiana Maps are shown in Fig. 6. Three different destinations are defined in different GRP executions given the same initial location for each. Optimal paths connecting initial and final locations are obtained under nominal and harsh weather conditions as shown by blue, red and green in Fig. 6.
IvA2 Results For Nominal Weather Condition
For nominal weather condition, we choose as the upperlimit (threshold) slope. Figs. 6 (b) and (e) show corresponding optimal paths. Blue, red and green paths are reachable in both figures. The heavilyforested Oregon area shown in Fig. 6 impacts traversals. Except for the blue path starting from the edge of the forest, initial traversals are flat in the remaining paths.
IvA3 Results For Harsh (Wet) Weather Conditions
For harsh or wet weather conditions, we choose as the terrain slope constraint. We consider the same start and target destinations to compute driving paths under harsh weather condition as in the previous cases. Figs. 6 (c) and (f) show optimal driving paths under harsh (wet) weather conditions. Note that in Fig. 6 (c), the blue path destination is unreachable because the endpoint region is not connected to the center (start state) region due to terrain slope constraints. The red and green paths are reachable but differ nontrivially compared to paths obtained for nominal weather conditions. As shown in Fig. 6 (f), GRP chooses a safer but longer path to avoid a low elevation region in the depicted bottom right region given bad (wet) weather. Path planning results under wet and nominal weather conditions are quantitatively compared in Table I.
Appropriate  Harsh  

State  Path  
Oregon  Red  
Green  
Blue  
Indiana  Red  
Green  
Blue 
IvB Local Path Planning
Given three consecutive desired waypoints meters (), m, and , . The desired path therefore consists of two straight path segments connected by a circulararc turn (see Fig. 7). Note that the radius of the circular path is in our case study. Selecting as the upperbound for yaw rate, atomic proposition is satisfied if . Because the desired speed is constant, the desired trajectory is given by
(34) 
where the tangent vector
(35) 
is as follows:
(36) 
Note that is the desired path arc length, where ().
IvC Trajectory Tracking
By applying the proposed feedback controller design from Section IIIC, the desired LPP vehicle trajectory assigned by Eqs. (34) and (35) can be asymptotically tracked. The FC is assigned controller gains and for this example. Fig. 8 shows the and components of vehicle desired and actual positions. Error , the deviation between actual and desired position, is shown versus time in Fig. 9. Notice that error never exceeds . This deviation error occurs due to (i) surface nonlinearities and (ii) sudden acceleration changes along the desired path. While acceleration along the linear segments of the path is zero, acceleration rapidly changes when the car enters or exits a circular arc (turning) path.
V Conclusion
This paper presents a novel datadriven approach for offroad motion planning and control. A dynamic programming module defines optimal waypoints using available GIS terrain and recent weather data. A path planning layer assigns a feasible desired trajectory connecting planned waypoints. A feedback linearization controller successfully tracks the desired trajectory over a nonlinear surface. In future work, we will relax assumptions related to sideslip and incorporate more sophisticated models of terrain interactions to improve decisions across all three decision layers.
Vi Acknowledgement
This work was supported in part under Office of Naval Research grant N000141410596.
References
 [1] F. Duchoň, A. Babinec, M. Kajan, P. Beňo, M. Florek, T. Fico, and L. Jurišica, “Path planning with modified a star algorithm for a mobile robot,” Procedia Engineering, vol. 96, pp. 59–69, 2014.
 [2] J.C. Latombe, Robot motion planning. Springer Science & Business Media, 2012, vol. 124.
 [3] D. Gaw and A. Meystel, “Minimumtime navigation of an unmanned mobile robot in a 21/2d world with obstacles,” in Robotics and Automation. Proceedings. 1986 IEEE International Conference on, vol. 3. IEEE, 1986, pp. 1670–1677.
 [4] X. Zhao, W. Zhang, Y. Feng, and Y. Yang, “Optimizing gear shifting strategy for offroad vehicle with dynamic programming,” Mathematical Problems in Engineering, vol. 2014, 2014.
 [5] S. Karumanchi, T. Allen, T. Bailey, and S. Scheding, “Nonparametric learning to aid path planning over slopes,” The International Journal of Robotics Research, vol. 29, no. 8, pp. 997–1018, 2010.
 [6] K. Chu, M. Lee, and M. Sunwoo, “Local path planning for offroad autonomous driving with avoidance of static obstacles,” IEEE Transactions on Intelligent Transportation Systems, vol. 13, no. 4, pp. 1599–1616, 2012.
 [7] K. Chu, J. Kim, K. Jo, and M. Sunwoo, “Realtime path planning of autonomous vehicles for unstructured road navigation,” International Journal of Automotive Technology, vol. 16, no. 4, pp. 653–668, 2015.
 [8] R. T. Farouki and T. Sakkalis, “Pythagorean hodographs,” IBM Journal of Research and Development, vol. 34, no. 5, pp. 736–752, 1990.
 [9] T. M. Howard and A. Kelly, “Optimal rough terrain trajectory generation for wheeled mobile robots,” The International Journal of Robotics Research, vol. 26, no. 2, pp. 141–166, 2007.
 [10] Z. Shiller and J. Chen, “Optimal motion planning of autonomous vehicles in three dimensional terrains,” in Robotics and Automation, 1990. Proceedings., 1990 IEEE International Conference on. IEEE, 1990, pp. 198–203.
 [11] Z. Shiller and Y.R. Gwo, “Dynamic motion planning of autonomous vehicles,” IEEE Transactions on Robotics and Automation, vol. 7, no. 2, pp. 241–249, 1991.
 [12] F. B. Amar, P. Bidaud, and F. B. Ouezdou, “On modeling and motion planning of planetary vehicles,” in Intelligent Robots and Systems’ 93, IROS’93. Proceedings of the 1993 IEEE/RSJ International Conference on, vol. 2. IEEE, 1993, pp. 1381–1386.
 [13] D. Bonnafous, S. Lacroix, and T. Siméon, “Motion generation for a rover on rough terrains,” in Intelligent Robots and Systems, 2001. Proceedings. 2001 IEEE/RSJ International Conference on, vol. 2. IEEE, 2001, pp. 784–789.
 [14] D. P. Bertsekas, D. P. Bertsekas, D. P. Bertsekas, and D. P. Bertsekas, Dynamic programming and optimal control. Athena scientific Belmont, MA, 1995, vol. 1, no. 2.

[15]
M. ZolfpourArokhlo, A. Selamat, S. Z. M. Hashim, and H. Afkhami, “Modeling of route planning system based on q valuebased dynamic programming with multiagent reinforcement learning algorithms,”
Engineering Applications of Artificial Intelligence
, vol. 29, pp. 163–177, 2014.  [16] A. Kelly, A. Stentz, O. Amidi, M. Bode, D. Bradley, A. DiazCalderon, M. Happold, H. Herman, R. Mandelbaum, T. Pilarski et al., “Toward reliable off road autonomous vehicles operating in challenging environments,” The International Journal of Robotics Research, vol. 25, no. 56, pp. 449–483, 2006.
 [17] H. Mousazadeh, “A technical review on navigation systems of agricultural autonomous offroad vehicles,” Journal of Terramechanics, vol. 50, no. 3, pp. 211–232, 2013.
 [18] G. Reina, A. Milella, and R. Worst, “Lidar and stereo combination for traversability assessment of offroad robotic vehicles,” Robotica, vol. 34, no. 12, pp. 2823–2841, 2016.
 [19] R. González, A. López, and K. Iagnemma, “Thermal vision, moisture content, and vegetation in the context of offroad mobile robots,” Journal of Terramechanics, vol. 70, pp. 35–48, 2017.
 [20] J. hwan Jeon, R. V. Cowlagi, S. C. Peters, S. Karaman, E. Frazzoli, P. Tsiotras, and K. Iagnemma, “Optimal motion planning with the halfcar dynamical model for autonomous highspeed driving,” in American Control Conference (ACC), 2013. IEEE, 2013, pp. 188–193.
 [21] J. Kong, M. Pfeiffer, G. Schildbach, and F. Borrelli, “Kinematic and dynamic vehicle models for autonomous driving control design,” in Intelligent Vehicles Symposium (IV), 2015 IEEE. IEEE, 2015, pp. 1094–1099.
 [22] J. Ryu and J. C. Gerdes, “Integrating inertial sensors with global positioning system (gps) for vehicle dynamics control,” TRANSACTIONSAMERICAN SOCIETY OF MECHANICAL ENGINEERS JOURNAL OF DYNAMIC SYSTEMS MEASUREMENT AND CONTROL, vol. 126, no. 2, pp. 243–254, 2004.
 [23] M. Brown, J. Funke, S. Erlien, and J. C. Gerdes, “Safe driving envelopes for path tracking in autonomous vehicles,” Control Engineering Practice, vol. 61, pp. 307–316, 2017.
 [24] B. Paden, M. Čáp, S. Z. Yong, D. Yershov, and E. Frazzoli, “A survey of motion planning and control techniques for selfdriving urban vehicles,” IEEE Transactions on Intelligent Vehicles, vol. 1, no. 1, pp. 33–55, 2016.
 [25] H. G. Kwatny and G. Blankenship, Nonlinear Control and Analytical Mechanics: a computational approach. Springer Science & Business Media, 2000.
 [26] J. E. Hopcroft, R. Motwani, and J. D. Ullman, “Automata theory, languages, and computation,” International Edition, vol. 24, 2006.
 [27] M. L. Puterman, Markov decision processes: discrete stochastic dynamic programming. John Wiley & Sons, 2014.
 [28] U. G. Survey. (2017) USGS TNM Download (V1.0). [Online]. Available: https://viewer.nationalmap.gov/basic
 [29] P. Donato. (2017) USGS Lidar Data tool. [Online]. Available: https://bitbucket.org/umich_a2sys/usgs_lidar
 [30] U. G. Survey. (2017) USGS Lidar Point Cloud (LPC) OR_OLCOchoco_2011_000034 20140918 LAS. [Online]. Available: https://www.sciencebase.gov/catalog/item/58275640e4b01fad86fccc0d
 [31] ——. (2017) USGS Lidar Point Cloud (LPC) IN_StatewideFultonCo_2011_000049 20140906 LAS. [Online]. Available: https://www.sciencebase.gov/catalog/item/5827579de4b01fad86fceef5
Comments
There are no comments yet.