Prognostics-enhanced Receding Horizon Mission

3 downloads 0 Views 980KB Size Report
Note that s'' is not always on the corners of the grids while it can be ... Therefore, the vehicle moves from n1 to n2 at velocity v1 and from n2 to n3 ...... Application to Bearing Fault Detection, IEEE Transactions on Industrial Electronics, in print.
AIAA 2011-6294

AIAA Guidance, Navigation, and Control Conference 08 - 11 August 2011, Portland, Oregon

Prognostics-enhanced Receding Horizon Mission Planning for Field Unmanned Vehicles Bin Zhang 1, Liang Tang 2, Jonathan Decastro 3 Impact Technologies, LLC. 200 Canal View Blvd., Rochester, NY 14623 and

Downloaded by Bin Zhang on July 15, 2013 | http://arc.aiaa.org | DOI: 10.2514/6.2011-6294

Kai Goebel 4 NASA Ames Research Center, Moffett Field, CA. 94035-1000

This paper presents a preliminary study for using prognostic information to enhance the mission/path planning in a non-uniform environment. Prognostic information is introduced in order to ensure that the mission failure can be minimized even when a fault occurs. This will enhance the performance of autonomous vehicles that often work in harsh environments that cause aging, fatigue, and fracture. When a fault occurs, the proposed path planning scheme predicts the remaining useful life (RUL) of the vehicle. This RUL is then used as a constraint in path planning to minimize the life consumption with other factors such as minimization of energy and travel time. The proposed planning algorithm integrates the prognosis and path planning in a receding horizon planning framework. Like field D* searching algorithm, the map is described by grids while nodes are defined on corners of grids. The planning algorithm divides the map into three areas, implementation area, observation area, and unknown area. We assume that the autonomous vehicle is equipped with onboard sensors that are able to detect and determine the terrain in a certain range, which is observation area. The implementation area consists of the gird next to the current node. The area beyond observation area is the unknown (un-observed) area where the terrain is unknown to vehicle. At a node, the vehicle plans the path from the vehicles’ current location to the destination. Only the path planned in the implementation area is executed. This process is repeated until the destination is reached or it turns out that no route can lead to destination or the vehicle reaches its end of life. The simulation results demonstrate the effectiveness of the proposed approach.

I. Introduction

P

ATH/mission planning is a fundamental enabling technique for vehicle autonomy that has been widely used on vehicles from unmanned ground vehicles (UGVs), unmanned surface vessels (USVs), to unmanned air vehicles (UAVs), micro air vehicles (MAVs), unmanned underwater vehicles (UUVs), and others. It plays an important role in improving the vehicle availability/sustainability/survivability/safety, reducing operating cost, guaranteeing mission success, and enabling tasks in harsh and remote environments. The research in this area has drawn extensive interests from both military and civilian/commercial communities. In the past a few decades, many algorithms were developed and utilized on various vehicles. The initial efforts on this, like Dijkstra’s algorithm and A* algorithm [1], find the least-cost path from a given starting point to a destination point. They use a heuristic function combining distance and path cost associated with terrain to determine the order in which the search visits nodes. The heuristic function includes two parts: the part from starting node to the current node and the part of estimation from the current node to the destination. The advantage of these 1

200 Canal View Blvd, Rochester NY, 14623, [email protected]. 200 Canal View Blvd, Rochester NY, 14623, [email protected]. 3 200 Canal View Blvd, Rochester NY, 14623, [email protected]. 4 NASA Ames Research Center, Moffett Field, CA. 94035-1000, [email protected]. 1 American Institute of Aeronautics and Astronautics 2

Copyright © 2011 by the American Institute of Aeronautics and Astronautics, Inc. All rights reserved.

Downloaded by Bin Zhang on July 15, 2013 | http://arc.aiaa.org | DOI: 10.2514/6.2011-6294

methods is that it is complete and will always find a solution if one exists. There are many variants on this kind of search algorithms, such as lifelong Planning A* (LPA*) [2], Generalized Adaptive A* [3], Fringe Saving A* [4], Iterative-deepening A* [5], and others. Another branch of searching algorithms developed from A* is the so-called “dynamic A*”, or D* in short [6]. It is an incremental search algorithm, which makes assumption about the unknown part of the terrain and finds a least cost path from its current location to the destination under this assumption. When the vehicle observes new area as it follows the selected path, it adds the observed information to its map and re-plans a new path when this becomes necessary. This process is repeated until the vehicle reaches the goal. The advantage of D* over A* is that it re-plans faster than A* because incremental search algorithms speed up search by using experience with the previous problems and therefore more efficient than A*. D* and its variants, such as D*-lite and field D*, have been widely used for autonomous vehicles, including the Mars rovers Opportunity and Spirit [7]. Field D* is an interpolation based path planning and re-planning algorithm [9]. It enables autonomous vehicles to generate direct, low-cost path through a non-uniform environment consisting of grids. Although many algorithms on mission/path planning had been developed, all of these planning algorithms are working on a healthy vehicle. The cost functions for optimality are typically terrain (for non-uniform environments), travel time, energy consumption, or the combination of these factors. That is, there are no constraints on maneuverability, survivability, and availability of vehicles and mission failure caused by faults in the vehicles. In real world, however, fault may occur due to harsh environment, aggressive operation, and fatigue, among others. The components or subsystems may have fault even with careful operation and maintenance. When a fault occurs, the vehicle performance and maneuverability will be affected. Consequently, mission planning should consider the constraints induced to vehicle dynamics and the development and progression of the fault. Under such a situation, we may want to complete the critical tasks first and avoid some tasks with very low priorities. One of the goal is to make sure that, at the end of mission, the fault growth is smaller than a critical value that leads to the failure of vehicle. This requires different thoughts from planning with a healthy vehicle and introduction of failure prognosis algorithm in the planning. The prognostic algorithm will provide information of vehicle’s health state in the future and remaining useful life (RUL). With this new constraint on vehicle’s remaining useful life, the same task may take very different path from its healthy scenario. To our knowledge, there are no reported works on integrating fault diagnosis and identification (FDI) and failure prognosis (FP) in mission planning. For such a mission planning integrating prognosis, we need to have enabling techniques of FDI and FP available. There are many efforts on FDI and FP in the past decades [10-13]. In general, the approaches are divided into two main categories: model-based and data-driven. Some approaches that combines them also exist. In this paper, our main purpose is to introduce an approach to integration FP in mission planning rather than FP algorithm itself. Therefore, we will simplify the failure prognosis by assuming that fault progression model and system measurement indicting fault growth, as well as an extended Kalman filter (EKF) based prognosis algorithm, are available to provide an accurate estimate of fault dimension and remaining useful life in the future. With projected future health state and RUL from prognostic algorithm, we can use RUL as one of the constraints in path planning to minimize the life consumption. The proposed planning algorithm integrates the prognosis and path planning in a receding horizon planning framework. To achieve this goal, the map, described by evenly divided square grids, is divided into three areas: implementation area that contains grids next to the current node, observation area that consists of the grids that can be detected by vehicle onboard sensors, and unobserved area that is the area beyond observation area. At any node, the planning algorithm plans a path from its current node to the destination. However, only the path planned in the implementation area is executed. When the next node is reached, more map area will be explored and, with this updated map, path is re-planned to generate a new path for next movement. This process is repeated until the destination is reached or it turns out that no route can lead to destination or the vehicle reaches its end of life. The remaining part of the paper is organized as follows: Section II presents the overview of prognosis enhanced mission planning, which is followed by an introduction of receding-horizon planning (RHP) in Section III. The recap of prognostic algorithm is addressed in Section IV. The integration of prognosis with RHP is given in Section V. In Section VI, the cost function is discussed. A series of simulation results are presented in Section VII to demonstrate the proposed planning solution, which is followed by concluding remarking in Section VIII.

II. Prognosis-enhanced Mission Planning

2 American Institute of Aeronautics and Astronautics

Downloaded by Bin Zhang on July 15, 2013 | http://arc.aiaa.org | DOI: 10.2514/6.2011-6294

Before we introduce the prognosis enhanced mission planning algorithm, let’s introduce several assumptions that will be used in the following description. Assumption 1: The vehicle is operated in a two-dimensional (2-D) area. The map is divided into square grids with unit length of edge. Assumption 2: For each grid, there is an associated terrain value indicting the difficulty to traverse this grid. The area consists of non-uniform grids. Assumption 3: The vehicle has onboard sensors that are able to detect the environment and there are some processing algorithms to calculate the terrain value in area of operation from these sensor measurements. That is, when the vehicle moves in the area, the terrain value in each grid in the sensing range of the vehicle will be attained. Assumption 4: The vehicle travels a straight line in each grid. Assumption 5: The vehicle in the consideration has a detected fault with known fault mode. That is, the fault has been detected and we do not need to develop the fault detection algorithm. This assumption is introduced so that we can focus on the main topic of prognosis integration in mission planning. All of these assumptions are well accepted and widely used in research community. Figure 1 illustrates the structure of prognostic enhanced mission planning. It follows a receding-horizon planning strategy. The terrain values from the processing of sensor data are input to Map block in Figure 1. The vehicle will then plan the path according to the map information in the area of sensing and prior knowledge in the unknown (unobserved) area. For each possible path, the loading profile can be obtained. With this information, an extended Kalman filter based prognostic algorithm is implemented based on a fault progression model to calculate the fault dimension at the end of the mission and remaining useful life. The results will be evaluated against the acceptable fault dimension or acceptable life consumption to check if the prognostic constraint is satisfied. The path costs on travel time and terrain value are also investigated to find the optimal path. These costs are illustrated by dashed boxes. If these constraints are satisfied on a path, this path will become a candidate path. This process is repeated until the optimal path at this location is found. If not all the available paths can satisfy the constraints, the path with the minimal cost will be selected. The selected path, which consists of a series of waypoints on grids, is sent to vehicle for implementation. After the vehicle moves to a new node and attains new map observations, the current health state is estimated and path is re-planned with the updated map information. This whole process is repeated until the robot reaches the destination or it finds that there is no path to reach the destination.

Figure 1 Prognostic Enhanced Mission Planning Different from traditional searching algorithm in which the cost function contains deterministic terms such as path distance, path terrain cost, the introduction of prognosis will bring uncertainties in the planning. The reason is that the prognosis can only provide an estimate of future fault dimension and vehicle remaining useful life, which are often described by probability density function (pdf). This requires considering the optimization in a stochastic framework. In addition, as mentioned in Assumption 4, the vehicle travels a straight line in each grid, like many other mission planning algorithms. To maintain path optimality, it often requires the size of grid to be small. These small grids require more computation resources. It also brings difficulties to prognosis if traditional searching algorithms are utilized. The reason is that with these algorithms, the planning is only carried out in one grid and the prediction horizon is small. However, prognostic algorithms usually require a reasonable large prediction horizon to achieve a meaningful prognosis on future health dimension and remaining useful life. If we consider the planning in the next grid like traditional approaches, the prognostic algorithm will not generate useful results to be considered 3 American Institute of Aeronautics and Astronautics

Downloaded by Bin Zhang on July 15, 2013 | http://arc.aiaa.org | DOI: 10.2514/6.2011-6294

either in cost function or in constraints. For example, suppose the vehicle has a speed of 0.5 m/s and grid is square with unit edge length of 1 meter. If the robot travels diagonally, the travel time will be about 3 seconds. It is meaningless to predict the growth of fault in the next 3 seconds. This requires us to consider the planning beyond the grids next to the vehicle’s current location. We therefore introduce a receding horizon planning strategy. The basic idea is that we plan the path from the robot’s current location at time instant T1 to the destination at the projected future time instant T2. The prognosis is carried out on the whole time horizon [T1 T2] to calculate the prognosis costs on all feasible paths. From the planned path, we can also calculate costs on terrain, path length, and travel time. These three individual costs are weighted and summed to achieve an overall path cost. This overall path cost is used to find the optimal path. When this optimal path is found, the robot implements however only the path segment in the next grid (implementation area) but ignore the path segment beyond the next grid. When the robot reaches the next grid, this planning is carried out in the new horizon again with the updated map. To make such a system work, we need to define each sub-function in the framework, which include cost definition, failure prognosis, receding horizon planning, and optimal search. Like filed D* algorithm, the nodes on the map are defined on the corners of grids. The vehicle is allowed to traverse the grid in any heading direction. That is, the path can cross the grid on the edge. With the above description, the prognosis enhanced mission planning problem can be defined as follows: Given a vehicle with a detected fault that is growing with the execution of mission, suppose the vehicle is operated in a non-uniform region that consists of evenly divided square grids, suppose each grid is associated with non-uniform values on cost to terrain, travel time, and fault progression, and suppose the vehicle has onboard sensors that are able to detect the grid values around the vehicle, find the optimal path from a starting node to a destination node that has the minimum weighted sum of costs.

III. Receding-Horizon Planning Traditional search algorithms use discrete state transitions that artificially constrain a vehicle’s motion to a small set of possible headings (usually 8 discrete directions: E, NE, N, NW, W, SW, S, SE), as the dashed lines shown in Figure 2. This often results in unnatural, suboptimal paths with unnecessary turns and awkward directions. Although the severity of this problem can be reduced by post-processing the path to find a smoother alternative, it often does not work well and may increase the path cost in non-uniform environments as in our problem [9]. Therefore, it is desirable that the vehicle can select path with more flexibility in heading direction, such as the path having a heading direction as shown in green line. Field D* is able to achieve this by using linear interpolation strategy. In our problem, since prognosis is nonlinear and non-deterministic, it is difficult, if not impossible, to implement linear interpolation. Therefore, we will use a receding horizon planning in a larger area to increase the flexibility in heading direction.

Figure 2 Illustration of grids, node, and heading directions 4 American Institute of Aeronautics and Astronautics

Downloaded by Bin Zhang on July 15, 2013 | http://arc.aiaa.org | DOI: 10.2514/6.2011-6294

In this receding horizon planning, we take advantage of field D* searching algorithm’s definition of node on the corners of the grids as the red solid dots shown in Figure 2. In the previous efforts, the nodes are defined in the center of the grids. When the vehicle moves from one node to a neighboring node, the first half segment and the second half segment fall into different grids. These two grids may have different terrain causing different loads that may increase the computation. With definition of node on corner of grids, when the vehicle moves from one node to a neighboring node, the entire path falls into the same grid. This will not cause change of terrain and loading and make the computation easier compared to its previous counterparts. Oppose to receding horizon planning is fixed horizon planning. Fixed horizon planning leads to a sequence of waypoints [pi, pi+1, …, pi+N-1] and implement these waypoints. One limitation is that it cannot handle the unexpected that is not predicted by or included in the model. For path planning, new obstacles may be observed as the vehicle moves forward and the planned waypoints may have to be discarded and new waypoints needs to be re-planned. Another major limitation is that the planning algorithm typically does not perform optimization as time approaches the end of the horizon because there is little time to do any meaningful optimization. Receding horizon planning is introduced to address the limitations from fixed horizon planning. The optimization for a receding horizon planning is carried out iteratively in following steps. 1. At waypoint pi and for the current health state xi, we solve a fixed horizon optimization problem over [pi+1, pi+N]. The optimization takes into account all the current and future constraints. 2. With the optimization results, the vehicle implements only the first waypoint pi+1 generated by step 1. That is, the vehicle moves to waypoint pi+1. 3. Estimate the health state after the vehicle reaches the waypoint pi+1 with an measurement at waypoint pi+1. 4. Repeat the fixed horizon optimization at waypoint pi+1 over [pi+2 pi+N+1]. As mentioned early, the map in our problem is composed of evenly divided grids. The waypoints can be defined on grid corners or edges. The receding horizon is therefore in the sense of grid, as shown in Figure 3. In Figure 3 (a), the vehicle location is s and the optimal path is s-s’-goal. The implementation area is the green box, the observation area is the magenta box, and the area beyond that is unknown (unobserved) area. The path segment s-s’ crosses the implementation area at s’’. The vehicle will travel from s to s’’. Meanwhile, the path s-s’-goal allows us to estimate the fault dimension and RUL at the end of mission. Note that s’’ is not always on the corners of the grids while it can be on the edge of the grids. This will generate more flexible heading directions and reduce the awkward vehicle movements. 16

16

14

14

12

12

10

10 8

8

s'

6 4

4

s''

2 0

6

0

5

10

15

(a) Planning at step 1 from initial start (s)

20

0

s'

s s''

2

s

0

5

10

15

20

(b) Planning at step 2 from new location (s in this figure is the s’’ in left figure)

Figure 3 Illustration of receding-horizon planning When the vehicle reaches the location s’’, we update the fault dimension via an extended Kalman filter based estimation algorithm. This s’’ in Figure 3 (a) then becomes the current location of the vehicle and is denoted as s in Figure 3 (b). The optimization is carried out again with new observations. A new waypoint s’ is selected and the new path is s-s’-goal, which has a cross point with implantation area at s’’. Then the vehicle moves to s’’, as in Figure 3 (a). Again, the path s-s’-goal allows us to conduct prognosis and estimates the fault dimension and RUL at the end of the mission. This process is repeated until the goal is reached. Note that the magenta area in Figure 3 (b) is larger than the magenta area in Figure 3 (a), which indicates the increase of map exploration. Additionally, the 5 American Institute of Aeronautics and Astronautics

implementation areas, denoted by green boxes, in these two figures are also different, which indicates that the implementation area changes as the vehicle moves. It is worth noting that the vehicle velocity in our problem is not constant. Therefore, in the path planning, apart from path selection, we also need to determine the vehicle velocity. When a path is planned at the current location n1, the velocity in the planned path is assumed remaining the same, say v1, from the current location to the destination. When the vehicle moves to the next location n2 at the velocity v1, the optimization on the new horizon is carried out again. This will generate a new path with a new different velocity, say v2. With this velocity, the vehicle moves to a new location n3 at velocity v2. Therefore, the vehicle moves from n1 to n2 at velocity v1 and from n2 to n3 at velocity v2. This process is repeated and, at the end, the entire path from the starting point to the destination point will have different velocities at different path segments.

Downloaded by Bin Zhang on July 15, 2013 | http://arc.aiaa.org | DOI: 10.2514/6.2011-6294

IV. Recap of Prognostic Algorithm in Mission Planning The main contribution of this paper is to integrate the prognosis in the mission planning. This will introduce uncertainties in the planning. Our focus will be on how to utilize the estimation of future fault states and remaining useful life of the vehicle in planning rather than prognostic algorithms. Therefore, the prognostic algorithm can be any efficient prognostic algorithm. In this paper, we will use an extended Kalman filter to estimate the fault at any waypoints on the path and introduce a fault progression model, along with fault related measurements, to project the fault growth and estimate the remaining useful life. As mentioned in Assumption 5, we assume the fault has been detected and its fault mode has been identified. In addition, since an extended Kalman filter is utilized in prognosis, the initial fault state is given as a Gaussian distribution. The input to the prognostic algorithm is a measurement that is assumed to have a one-to-one mapping with fault dimension. Such a measurement is often a feature or condition indicator that is extracted from vehicle’s raw data, such as vibration, temperature, current, voltage, or others. In the simulation, since the measurement is not available, it is randomly taken from the estimated pdf of health state to simulate an online measurement. To implement the extended Kalman filter based state estimation and failure prognosis, a fault progression model is needed. For this purpose, a Paris’ law-based fault growth model is defined. The model has the format of:

f k +1 = f k + p1 ⋅ Lp3 ⋅ f kp2 + N

(1)

where fk is the measurement at time instant k, p1, p2 and p3 are model parameters, L is the load to the vehicle, and N is the noise subject to N(0, Nk). Note that we have assumed a one-to-one mapping between measurement f and fault dimension, this measurement can be treated as fault dimension. The L in the model can be determined by the path. It is assumed that the load profile of the vehicle is a function of vehicle velocity v and terrain te. The function has the format of:

L = f ( te , v )

(2)

With such a function of loading profile, the selection of paths containing different terrain values will have a direct influence on vehicle’s loading profile and, consequently, have a direct influence on fault growth. Then, with the fault progression model and measurement, an extended Kalman filter (EKF) based prognostic algorithm can be carried out to estimate the fault state and run progress against the projected loading profile to calculate fault dimension and RUL at the end of the mission when the destination is reached. Extended Kalman filter is the nonlinear Kalman filter, which linearizes the system about the current mean and covariance. For a system given by

= xk +1 f ( xk , uk ) + wk

(3)

= z k h ( xk ) + vk

where wk and vk are the process and observation noises that are subject to N(0, Qk) and N(0, Rk), respectively. The function f can be used to compute the predicted state from the previous estimation and h can be used to compute the predicted measurement. Since f and h cannot be applied to the covariance directly, Jacobian matrix needs to be computed with the currently predicted state as follows: State transition Jacobian:

Fk =

∂f |xˆ ,u ∂x k k k

6 American Institute of Aeronautics and Astronautics

(4)

Hk =

Observation Jacobian:

∂h |xˆ ∂x k k −1

(5)

Then, the predictions of state and covariance are given as: State:

xˆˆk +1|k = f ( xk |k , uk −1 )

= Pk +1|k Covariance:

(6)

Fk Pk |k FkT + Qk

(7)

When a new measurement zk becomes available, the prediction can be corrected and the updated state and covariance are given as: State:

xˆˆk +1|k +1 =xk +1|k + K k ( zk − h( xk +1|k ) )

(8)

( I − K k +1H k +1 ) Pk +1|k

(9)

Covariance:

Pk +1|k += 1

25

−1

(10)

70

Expectation low 95% bound up 95% bound critical value current time RUL pdf

60 20 50 Fault prediction

Downloaded by Bin Zhang on July 15, 2013 | http://arc.aiaa.org | DOI: 10.2514/6.2011-6294

T T Kalman gain: K k +1 Pk +1|k H k +1 ( H k +1Pk +1|k H k +1 + Rk +1 ) =

15

10

40

30

20 5 10

0

0

20

40

60

80

(a) Fault state estimation

100

120

0

0

10

20

30 Time

40

50

60

(b) Failure prognosis and RUL pdf Figure 4 Illustration of EKF

Figure 4 (a) shows an example of using EKF to estimate the current fault dimension from measurements in the whole process from start to destination. In this figure, the blue dashed line is the raw measurement from vehicle while the red dotted line is the result of EKF estimation. Figure 4 (b), on the other hand, shows the prognostic results at a waypoint. The figure shows the estimated progression of fault dimension described by expectation, lower and upper bound of 95% confidence interval of fault dimension pdf. From this figure, it is clear that the long-term prediction involved in prognosis cause the growth of uncertainties. In addition, different from fault state estimation, since the measurement in the long-term prediction is not available, there is no update or correction process. Therefore, the prognosis relies on the fault progression model. When a critical value of fault dimension is defined to indicate the failure of vehicle, the remaining useful life of the vehicle can be estimated as well.

V. Prognosis Enhanced Receding Horizon Planning As mentioned earlier, it is meaningless to do prognostics in one grid. Therefore, the prognostics are carried out on the entire path, including from current location to the destination (goal) location. Since the path is not yet known we project the path in different areas. To this end, we divide the map from current location to goal location into three areas: Implementation area, observation area, and unknown (unobserved) area, as shown in Figure 5 (a). The Implementation area only covers the grids next to the vehicle’s current location, see the green box in Figure 5 (a). The observation area is the area that onboard sensors can sense, see the magenta box in Figure 5 (a). Implementation area is a subset of observation area. In the observation area, the map information is known, and therefore, the planning in this area is reliable. The unknown area covers from edge of observation area to goal location. We regard the prognosis in observation area is short-term prognosis while that prognosis cover the whole path is long-term 7 American Institute of Aeronautics and Astronautics

prognosis, as shown in Figure 5 (b). Note that, when the goal location becomes covered by observation, the longterm prognosis reduces to short-term prognosis.

Downloaded by Bin Zhang on July 15, 2013 | http://arc.aiaa.org | DOI: 10.2514/6.2011-6294

(a)

Illustration of areas in map

(b) Illustration of areas and prognosis horizons

Figure 5 Areas in Map and short-/long-term prognosis In Figure 5 (a), the current location is marked by s. The implementation area is given by green box and the selected path crosses this box at s’’. It can be seen from this figure that s’’ is located on the edge of a grid that leads to a flexible heading direction. This enables this algorithm to generate a smooth path like field D* but without interpolation that is not suitable for prognosis. The s-s’’ is the implementation path segment. The observation area is given by magenta box and the selected path crosses the observation area at s’. The s-s’ is the short-term prognosis horizon. Similar to field D*, the cost on s-s’ is denoted as c(s,s’). For area beyond s’, the map information is unknown, we have to calculate the cost on s’-goal part, denoted as g(s’), based on some assumptions. The total path cost, denoted as c(s,s’) + g(s’), from all available s’ in the observation area, denoted as obsv(s), will be used to select the optimal path. That is, we optimize:

min

s '∈obsv ( s )

( c( s, s ') + g ( s ') )

(11)

At current location s, when an optimal path, s-s’’-s’-goal, is selected, according to receding horizon theory, the path segment s’’-s’-goal will not be implemented. Only segment s-s’’ is implemented. When the robot reaches s’’, the current location becomes s’’ and more map information becomes available (the magenta box will move forward as shown in Figure 3). Then, the optimization is repeated to find an updated optimal path. Note that, since s’ is in the observation area which may not be the neighboring node of s, s’’ could be either on a node or on the edge of two neighboring nodes. This, similar with field D*, will reduce the unnecessary turns of vehicle. However, different from field D* that is working on neighboring nodes using interpolation, our planning is working in a bigger region (observation area). That is, our planning algorithm relies on s’ to determine s’’. In traditional path planning algorithms, c(s,s’) and g(s’) only carry the deterministic information like terrain value, distance, and travel time. The prognostic results, however, are given with uncertainties and non-deterministic for both fault dimension and RUL estimates. They are represented in a form of probability density function. To include these values in a cost function with a deterministic format, the expected value of pfd, fault value at risk (FVaR) with a given risk [13], or other values from pdf are taken and used as the cost. In this paper, the fault value at risk of 95% (FVaR95%) of fault dimension pdf is used as prognostic value in the cost function. FVaR has been adapted from the field of actuarial science and has been provided as a means for online evaluation of the severity of the fault condition. Let p(x) be a state pdf estimate generated online via a prognostic algorithm, considering all measurements available until current time instant t. Let x be the state coordinate associated to the fault dimension under analysis. The FVaR associated to the fault dimension x at current time instant t is computed by generate an estimate of the predicted state pdf p’ at time tfuture, with t ≤ tfuture. Then, FVaR is computed at a degree of confidence 0 ≤ α ≤ 1.

VI. Cost Function The cost function is a weighted sum of individual cost factor. When the path is planned, the cost function often contains more than one factor. Some factors are terrain value and path distance. When the vehicle travels at different velocities, travel time could be used to replace path distance. The terrain value in the cost function makes the vehicle 8 American Institute of Aeronautics and Astronautics

select the easiest path, which will reduce the wear of components and lower the risk of mission. The travel time in the cost function tends to find the quickest way to accomplish the mission. The two above-mentioned factors only consider the situation of a healthy vehicle. To enable prognosis enhanced planning when a fault occurs, it is necessary to include the cost on either fault dimension or consumption of remaining useful life. Therefore, the factor about fault dimension that is retrieved from fault state pdf will be included in the cost function. The cost function is the weighted sum of three factors and is defined as:

min = J wTime ( to ( s, s ') + tu ( s ') ) + wTerrain ( d o ( s, s ') + d u ( s ') ) + wPrognosis ( po ( s, s ') + pu ( s ') )

(12)

s'

where to(s,s’), do(s,s’), and po(s,s’) are travel time, terrain value, and FVaR95% on the path segment from ss’, respectively, while tu(s’), du(s’), and pu(s’) are those corresponding values on path segment from s’goal, respectively; wTime, wTerrain, and wPrognosis are weighting factors on each cost factor and wTime+wTerrain+wPrognosis=1. terrain cost 0.9 0.8 0.7

Downloaded by Bin Zhang on July 15, 2013 | http://arc.aiaa.org | DOI: 10.2514/6.2011-6294

travel time 1 0.5 0 prognosis 1 0.5 0 combined cost 0.8 0.6 0.4

5

10

15

20

25

30

35

40

45

Figure 6 Costs of each factor and the weighted sum of overall cost Figure 6 shows the costs at a location (x-axis is the index of node in the observation area). In this figure, the first subfigure shows the terrain cost on all nodes in the observation area. The second subfigure show the travel-time cost. In this scenario, we consider three different velocities and, therefore, there are three colors indicating three different velocities. The third one is the cost on prognosis. That is, we take the path follow each node in the observation area and use different velocity to accomplish the mission. The values show the cost on fault dimension at the end of mission. To make the weighted sum implementable, all these three individual costs are normalized to a value between 0 and 1. The last subfigure shows the weighed sum of the above-mentioned three costs. Again, the weighed-sum cost has three values for each node indicating three different velocities. The minimum cost associated with each individual factor can appears at different locations. The minimum value of weighted sum may lead to a path has a balanced minimum cost of path. By using different weighting factors in the cost function, the vehicle may choose different paths.

VII. Simulations A. Simulation Maps The map is composed of evenly divided square grids, which have unit edge length. The terrain values are given in five discrete values [0.1, 0.35, 0.55, 0.75, 1] in which 0.1 is the easiest terrain to traverse and 1 is the obstacle grid that cannot be traversed. When a map is built, the terrain value is randomly generated for each grid. The higher the terrain, the higher the terrain factor cost in cost function. The vehicle is assumed to have a sensing range of 4 grids. That is, on any direction, the vehicle can detect the terrain in 4 grids. When the vehicle moves, new grids will be explored and the map information of those grids moving into the observation area becomes available. For the unknown area, the terrain value can be the operator’s initial guess on the whole region. This will results in a fixed estimation of unknown area. Another solution is to use the average of the grids in the vehicles’ observation area to estimate the unknown area. The only difference between these two solutions is that the second 9 American Institute of Aeronautics and Astronautics

one has an updated estimation of unknown area when new grids are explored. In this paper, the first solution is taken to estimate the unknown area. In the following simulations, we use a map that has 20 grids on horizontal axis and 16 grids on vertical axis. The start location is [2 2] and the goal is [18, 15]. B. Vehicle Velocity and Loading It is assumed that the vehicle travels at different velocities. For simplicity, three different velocities at [0.2 0.25 0.4] are used. To calculate the loading profile of vehicle on the path from the current location to the destination, a function of terrain cost and velocity is assumed. The loading profile will be used in prognosis to estimate the fault dimension and RUL at the end of the mission. The loading is based on the following assumption: The more difficult the terrain, the higher the load; the higher the velocity, the higher the load. The load function is given as follows and illustrated in Figure 10: (14)

100 80

vehicle load

Downloaded by Bin Zhang on July 15, 2013 | http://arc.aiaa.org | DOI: 10.2514/6.2011-6294

L = f (te , v ) = 50 × ( v × te 0.2 + te × v 0.5 )

60 40 20 0 1 1 0.8

0.5

0.6 0.4 0

speed

0.2 0

terrain value

Figure 7 Illustration of loading calculation from terrain and velocity load 40 30 20 10 0

0

10

20

30

40

50

60

70

80

50

60

70

80

speed 0.5 0.4 0.3 0.2 0.1

0

10

20

30

40

Figure 8 Load profile and speed for the optimal path from start to goal When the path is planned from the current location s to the destination node, the vehicle is assumed will have a constant velocity, i.e., the vehicle will take only one velocity. As we mentioned earlier, the receding-horizon planning is carried out in the sense of grid. When the vehicle reaches the neighboring node or edge of the neighboring grid, a new horizon is taken and the optimization is solved again. At this new location, the vehicle may select a new velocity for a new path. Note that both velocity and path could be different from those selected in the 10 American Institute of Aeronautics and Astronautics

Downloaded by Bin Zhang on July 15, 2013 | http://arc.aiaa.org | DOI: 10.2514/6.2011-6294

previous location. Therefore, on the entire path from the starting node to the destination node, the vehicle will have different velocities. Then, for a selected path from the vehicle’s current location to destination, no matter in the observation area or in the unknown area (based on the assumed terrain value), the terrain value of path and the velocity is known. By using the loading function (14) given above, the load on the entire path can be determined for prognosis. At the end of the mission, the vehicle load and speed can be recorded for further analysis, as shown in Figure 11. From this figure, it is clear that the entire process from starting node to destination node has non-uniform load and velocity.

Figure 9 Loading profile for two different paths at three different velocities As an example, Figure 12 shows the loading profile for the situation from two different path defined by s’ in the observation area. The loading at Point 1 and Point 2 has three profiles, respectively, indicating three different velocities. With a selected path at a given velocity, the loading profile and travel time (the duration that a load applied to the vehicle) can be determined as shown in Figure 12. Note that with a higher speed, for the same path, each segment has a higher loading while each loading lasts a shorter time. That is, the vehicle travels faster with a shorter-duration and heavier load. On the other hand, at a lower speed, the vehicle travels slower with a longerduration and lighter load. Each load and path will have different travel times, terrain values, and prognostic results. The weighted sum of these will determine the cost for optimal path-planning, as discussed earlier. C. Fault progression As mentioned earlier, the fault progression follows the model (1). In the simulations, model parameter p2 and p3 can be adjusted to simulate different fault progress to investigate their influence of path planning. Change of p2 has a direct influence on current fault dimension to future fault progression. On the other hand, change of p3 will show how loading will influence the fault progression and consequently the path planning. In real applications, to make the model catch the nonlinearity in fault progression, it is necessary to introduce a model parameter online adaptation algorithm as in [11]. The initial fault dimension is set as an estimated pdf subject to Gaussian distribution N(0, 0.01). To predict the fault progression via EKF, measurements from system is needed. To this end, a value from the state pdf in interval defined on [µ-2σ, µ +2σ] is randomly taken to simulate the real-time measurements, where µ and σ are the expectation and standard deviation of the state pdf, respectively. With this simulated measurement, the extended Kalman filter can be used for state estimation and failure prognosis. Figure 13 shows a case study of failure prognosis. The three different colors indicate three different velocities. Each color set contains the prognosis result for paths defined by all nodes in the observation area. In this figure, only the mean value of failure prognosis is shown.

11 American Institute of Aeronautics and Astronautics

Prognosis Curves 3000

2500

2000

1500

1000

500

0

0

50

100

150

D. Simulation Scenarios In this section, a series of simulation results are presented to demonstrate the efficiency of the proposed scheme. In these simulations, the parameters are given as P=[p1, p2, p3] = [5e-5, 0.5, 3] unless otherwise defined. With this parameter set, the fault growth model is:

f k +1 = f k + 5e − 5 ⋅ L3 ⋅ f k0.5 + N

(17)

The initial fault dimension is set as an estimated pdf subject to Gaussian distribution N(0, 0.01). The white noise N is given as a noise having zero mean and covariance of 0.01. The starting location is given by [3, 2] and the destination location is given by [18, 15]. The shortest straight-line distance is 19.85. Scenario 1: Time optimal; [wTime, wTerrain, wPrognosis] = [1 0 0] In this scenario, only the travel time in the cost function is in the consideration. The weighting factors are wTime; wTerrain; wPrognosis]=[1; 0; 0]. Obviously, the vehicle will select the shortest path and the highest speed with this setting as shown in Figure 14 (a). From this figure, we can see that the path is almost a straight line that avoids all the obstacles. The length of the entire path is 19.91 and is very close to the straight-line distance from the starting location to the destination location. Note that the path is not a perfect straight line. The reason is that the map information is not known at the beginning and is explored in real time. Figure 14 (b) shows the fault growth. At the end of mission, the fault state is subject to N(267.6, 0.14) . Figure 14 (c) shows the loading profile along the path while Figure 14 (d) indicates that the vehicle travels at the highest velocity along the entire path to get optimality on travel time. Measurement and Fault growth 300

250

Fault measurement

Downloaded by Bin Zhang on July 15, 2013 | http://arc.aiaa.org | DOI: 10.2514/6.2011-6294

Figure 10 Prognosis on different paths evaluated at three different velocities

200

150

100

50

0

0

20

40

60

80

time

(a) Planned path on the map

(b) Measurement and fault growth

12 American Institute of Aeronautics and Astronautics

100

Velocity

Load 45

0.5 0.45

40 0.4 35

velocity

load

0.35 30

0.3 0.25

25 0.2 20 0.15 15

0

10

20

30 time

40

50

0.1

60

0

10

(c) Loading profile on the path

20

30 time

40

50

60

(d) Travel velocity on the path

Scenario 2: Terrain optimal; [wTime, wTerrain, wPrognosis] = [0 1 0] In this scenario, only the terrain value is considered in the cost function. The weighting factors are [wTime; wTerrain; wPrognosis]=[0; 1; 0]. Obviously, the vehicle should select the easiest path with this setting. The velocity does not have any influence on planning. The result is shown in Figure 15 (a). The path has a distance of 21.7. Clearly, this new path is longer than the previous one in Scenario 1. Note that when a path follows the common edge of two grids, the terrain value is selected as the smaller terrain value. As shown in Figure 15 (b), the fault dimension is subject to N(56.4, 0.116) at the end of the mission. Comparing the loading profile in Figure 14 (c) and Figure 15 (c), it is clear to see that the load in this new scenario is much lower than the previous scenario. Measurement and Fault growth 60

Fault measurement

50

40

30

20

10

0

0

(a) Planned path on the map

20

40

60 time

80

100

120

(b) Measurement and fault growth

Load

Velocity

22 0.28 20 0.26 18

0.24 0.22

velocity

16

load

Downloaded by Bin Zhang on July 15, 2013 | http://arc.aiaa.org | DOI: 10.2514/6.2011-6294

Figure 11 Simulation results of Scenario 1

14

0.2 0.18 0.16

12

0.14 10 0.12 8

0

20

40

60 time

80

(c) Loading profile on the path

100

120

0.1

0

20

40

60 time

80

100

(d) Travel velocity on the path

Figure 12 Simulation results of Scenario 2 13 American Institute of Aeronautics and Astronautics

120

Figure 15 (d) shows the vehicle velocity along the entire path. Since velocity does not have influence in this scenario, it is chose as 0.2 because this is the first value assigned to the vehicle. Scenario 3: Prognosis optimal; [wTime, wTerrain, wPrognosis] = [0 0 1] In this scenario, only prognosis on fault growth is considered in the cost function. That is, the path is selected so that the fault growth is the minimum. The weighting factors are [wTime; wTerrain; wPrognosis]=[0; 0; 1]. The selected path is shown in Figure 16 (a). The entire path has a distance of 22.34. At the end of mission, the fault dimension is subject to N(18.75, 0.105) as shown in Figure 16 (b). It is clear that this path lead to the minimum fault growth comparing to the previous two scenarios. By investigating load profiles shown in Figure 16 (c), we can see that the loading profile in this scenario is even lower. Figure 16 (d) indicates that the vehicle select the lowest velocity on the entire path. Measurement and Fault growth 18 16

Fault measurement

10 8 6 4 2 0

0

(a) Planned path on the map

20

60 time

40

80

100

120

(b) Measurement and fault growth

Load

Velocity

22 0.28 20 0.26 18

0.24 0.22

velocity

16

load

Downloaded by Bin Zhang on July 15, 2013 | http://arc.aiaa.org | DOI: 10.2514/6.2011-6294

14 12

14

0.2 0.18 0.16

12

0.14 10 0.12 8

0

20

40

60 time

80

(c) Loading profile on the path

100

120

0.1

0

20

40

60 time

80

100

120

(d) Travel velocity on the path

Figure 13 Simulation results of Scenario 3

Scenario 4: Weighted sum optimal; [wTime, wTerrain, wPrognosis] = [0.2 0.3 0.5] In this scenario, we consider all of the three factors with different weighting factors in the cost function. The weighting factors are [wTime; wTerrain; wPrognosis]=[0.2; 0.3; 0.5]. With this setting, the path planned is shown in Figure 17 (a). This path has a distance of 22.344. At the end of the mission, the fault dimension is subject to N(36.1, 0.106) as in Figure 17 (b). This indicates that the fault grows to a value larger than that in Scenario 3 (prognosis optimal) but smaller than those in Scenarios 1 (time optimal) and 2 (terrain optimal). Clearly, this scenario is a balance of the previous three scenarios according to the new set of weighting factors. Figure 17 (c) shows the loading profile while Figure 17 (d) the vehicle velocity on the path. Different from previous scenarios in which the vehicle take identical velocity from the starting location to the destination. In this scenario, the vehicle travels at different velocity on different waypoints. 14 American Institute of Aeronautics and Astronautics

Measurement and Fault growth 35

30

Fault measurement

25

20

15

10

5

0

0

(a) Planned path on the map

20

40

60 time

80

100

120

(b) Measurement and fault growth Velocity

Load 22

0.26 18

0.24 0.22

velocity

16

load

Downloaded by Bin Zhang on July 15, 2013 | http://arc.aiaa.org | DOI: 10.2514/6.2011-6294

0.28 20

14

0.2 0.18 0.16

12

0.14 10 0.12 8

0

20

40

60 time

80

100

120

0.1

0

(c) Loading profile on the path

20

40

60 time

80

100

120

(d) Travel velocity on the path

Figure 14 Simulation results of Scenario 4 The performance of the above presented four scenarios is summarized in Table 1. From this table, it is clear that Scenarios 3 has the minimum fault growth. The planned path is the longest one and the most conservative one. This is also the case of maximizing the mission success rate. Scenario 1 has the maximum fault growth but the path is the shortest one. This path is the most aggressive one and corresponding to the decision to accomplish the mission in the minimum time duration at all cost. Scenario 4 provides a solution to balance the weights on each cost to fulfill the task. Table 1 Performance comparison

Weighting factor Path distance Travel steps Fault at EOM

Scenario 1 Time Optimal wTime=1 wTerrain=0 wPrognosis=0 19.91 53 N(267.6, 0.14)

Scenario 2 Terrain optimal wTime=0 wTerrain=1 wPrognosis=0 21.71 110 N(56.4, 0.116)

Scenario 3 Scenario 4 Prognosis optimal weighted optimal wTime=0 wTime=0.2 wTerrain=0 wTerrain=0.3 wPrognosis=1 wPrognosis=0.5 22.344 21.74 111 101 N(18.75, 0.105) N(36.1, 0.106)

VIII. Conclusion In this paper, a prognosis enhanced mission planning algorithm is developed and discussed. Different from traditional planning algorithms on healthy vehicles, this paper considers the planning problem on a vehicle under faulty condition with failing components. Based on the estimated fault dimension or remaining useful life from prognostic algorithm, the planning algorithm take this information in the cost function, along with other costs 15 American Institute of Aeronautics and Astronautics

including travel time and terrain value, to find the optimal path so that mission can be accomplished or the percentage of mission accomplishment can be maximized. A receding horizon planning scheme is used so that current fault state and the future fault state at the end of mission can be estimated and evaluated at each waypoint. To this end, an extended Kalman filter based fault estimation and failure prognosis algorithm is introduced in a nonuniform environment. The map information is assumed unknown to vehicle but becomes available in an observation as the vehicle moves in the map. The map, consisted of evenly divided square grids with unit length with nodes being defined on the corners of grids, is divided into implementation area, observation area, and unobserved area to support receding horizon planning. That is, after each planning, only the path segment in the implementation area is executed. This process is repeated until the destination is reached. To demonstrate the efficiency of the proposed prognosis enhanced mission planning, a series of simulations is conducted and the simulation results are presented and compared.

References

Downloaded by Bin Zhang on July 15, 2013 | http://arc.aiaa.org | DOI: 10.2514/6.2011-6294

1

Hart, P. E.; Nilsson, N. J.; Raphael, B. (1968). "A Formal Basis for the Heuristic Determination of Minimum Cost Paths". IEEE Transactions on Systems Science and Cybernetics SSC4 4 (2):100-107 2 S. Koenig, M. Likhachev and D. Furcy. Lifelong Planning A*. Artificial Intelligence Journal, 155, (1-2), 93-146, 2004. 3 X. Sun, S. Koenig and W. Yeoh. Generalized Adaptive A*. In Proceedings of the International Joint Conference on Autonomous Agents and Multiagent Systems (AAMAS), 469-476, 2008. 4 X. Sun and S. Koenig. The Fringe-Saving A* Search Algorithm - A Feasibility Study. In Proceedings of the International Joint Conference on Artificial Intelligence (IJCAI), 2391-2397, 2007. 5 R.E. Korf, “Depth-first iterative-deepening: An optimal admissible tree search", Art. Intell., vol. 27, no 1, pp. 97-109, 1985. 6 Stentz, Anthony (1994), "Optimal and Efficient Path Planning for Partially-Known Environments", Proceedings of the International Conference on Robotics and Automation: 3310–3317 7 S. Singh, R. Simmons, T. Smith, A. Stentz, V. Verma, A. Yahja and K. Schwehr. "Recent Progress in Local and Global Traversability for Planetary Rovers," In Proceedings of the IEEE International Conference on Robotics and Automation, April 2000 8 D. Ferguson and A. Stentz. Field D*: An Interpolation-Based Path Planner and Replanner. Proceedings of the International Symposium on Robotics Research, 2005. 9 D. Ferguson and A. Stentz. “The Field D* Algorithm for Improved Path Planning and Replanning in Uniform and NonUniform Cost Environments”, CMU-RI-TR-05-19, June, 2005 10 C. Chen, B. Zhang, G. Vachtsevanos, M. Orchard, Machine Condition Prediction based on Adaptive Neuro-Fuzzy and High-Order Particle Filtering, IEEE Transactions on Industrial Electronics, in print. 11 B. Zhang, C. Sconyers, C. Byington, R. Patrick, M. Orchard, G. Vachtsevanos, A Probabilistic Fault Detection Approach: Application to Bearing Fault Detection, IEEE Transactions on Industrial Electronics, in print. 12 M. Abbas, A. Ferri, M. Orchard, G. Vachtsevanos, “An Intelligent Diagnostic/Prognostic Framework for Automotive Electrical Systems,” 2007 IEEE Intelligent Vehicles Symposium, June 2007, pp 352-357. 13 Orchard, M., Kacprzynski, G., Goebel, K., Saha, B., and Vachtsevanos, G., “Advances in Uncertainty Representation and Management for Particle Filtering Applied to Prognostics,” 2008 International Conference on Prognostics and Health Management PHM, Denver, CO, USA, October 9 – 12, 2008

16 American Institute of Aeronautics and Astronautics