Feedback Motion Plan Verification for Vehicles with Bounded Curvature Constraints

10/15/2019 ∙ by Giovanni Miraglia, et al. ∙ 0

The kinematic approximation of Dubin's Vehicle has been largely exploited in the formulation of various motion planning methods. In the majority of these methods, planning and control phases are decoupled, and the burden of rejecting disturbances is left to the controller. An alternative to this approach is the use of a feedback motion plan, where for each state there is a specific pre-computed action that will be executed. This planning approach provides the ability to verify all trajectories off-line. The verification can be performed using backward reachability, which provides the set of configurations from which a region is reachable. In this paper, we formulate a verification process that relies on the computation of the backward reachable set using geometric principles. In addition to the theoretical foundation of the method, we provide a numerical implementation of the method and we illustrate a practical example.

READ FULL TEXT VIEW PDF
POST COMMENT

Comments

There are no comments yet.

Authors

page 1

page 2

page 3

page 4

This week in AI

Get the week's most popular data science and artificial intelligence research sent straight to your inbox every Saturday.

I Introduction

Currently, we are witnessing rapid growth in the use of autonomy in both ground and aerial vehicles. Many high-level challenges relating to the two types of vehicles can be solved with similar solutions. For instance, one of the most crucial tasks, motion planning, can be simplified using the well-known kinematic approximation of Dubin’s Vehicle [5] for both ground and aerial vehicles operating at a constant altitude. Various motion planning methods relying on this approximation have been formulated [9, 4, 10, 17]. In the majority of these methods, planning and control phases are decoupled, and the burden of rejecting disturbances is left to the controller. However, unpredictable events may lead to excessive deviations that should not be managed by the controller alone. In these scenarios, an on-line replan is usually required to find a new appropriate path. In many instances this is perfectly acceptable; however, for applications such as those involving unmanned or autonomous air vehicle systems, planned paths are required to be pre-verified in order to assure compliance with regulations and safety constraints and thus cannot be changed on-line. One solution to this conflict is the use of a feedback motion plan [8, 12, 13, 7], where for each state there is a specific pre-computed action that will be executed. This planning approach provides the ability to reject disturbances, recover from deviations, and the possibility to verify all trajectories off-line.

A feedback motion plan can be continuous or discrete. While for continuous plans, it is possible to prove properties such as stability using classical theorems, for the discrete case the problem is much harder. For this reason, a common practice to perform verification of discrete plans is to use Monte Carlo simulations [11]

. However, the drawback is that this is a probabilistic approach in which a specific trajectory is tested with a certain probability. Instead in this paper, we formulate a verification method based on geometric principles that allow for an exhaustive analysis of the whole 3-D configuration space. This allows several advantages including verification and subsequent approval of a large number of paths simultaneously, the ability to compare different plans, and the possibility to optimize a plan over specific metrics.

The method that we developed to perform this analysis is based on the use of reachability[2, 3, 14, 15, 18], which is employed in the verification of hybrid and switched systems. There exist two types of reachability: forward [6] and backward [19, 1, 16]. Our method combines both backward and forward reachable sets that are obtained exploiting geometric properties. In particular, a time-independent backward propagation of 2-D sets is performed to study the backward reachability of a region of interest.

The paper illustrates the geometric principles underlying the proposed method along with practical implementation. In particular, Section II provides a formal definition of the problem. In Section III, we illustrate how to derive the Cellular Backward Reachability, which provides the set of configurations from which a specific border of a cell is reachable. Instead in Section IV, we illustrate how to compute the forward reachable set for trajectories starting from a border and ending in another border of the same cell. In Section V, we illustrate an iterative method that combines the two abovementioned types of set to perform a global reachability analysis. Finally, in the conclusions, we discuss future developments.

Ii Problem Formulation

Our objective is to study the backward reachability of a region of interest of the workspace for a discrete feedback plan having the following characteristics:

  • The feedback plan is formulated in the workspace and indicates the direction with which the vehicle must travel.

  • The workspace is decomposed in a grid having cell size smaller than the vehicle’s minimum turning radius .

  • Bang-bang control is used to correct the error between the vehicle’s heading and the commanded heading (which is constant in a cell). Therefore, the vehicle can either travel straight with speed or turn with its minimum turning radius ( is the angular speed). As shown in Fig. 0(a), the vehicle turns in the direction of the minimum angular distance.

If the initial angle is equal to the commanded angle , then the vehicle will just travel straight. Instead, if , the vehicle will first turn and then travel straight once it reaches the commanded heading . Nevertheless, since the vehicle may not converge to the desired angle before leaving the cell. In this last case, the exit angle will be different from and the path consists of a single turn. In summary, three types of paths are possible:

  • straight line (S path);

  • single turn followed by a straight line (CS path);

  • single turn (C path);

where C stands for a circular portion, which can correspond to either a right or left turn. Therefore, the set of possible paths is: .

(a)
(b)
Fig. 1: Example of vehicle moving inside a single cell (a) and global feedback plan with region of interest (b).

Given a region of interest , (red cells in Fig. 0(b)) and a feedback plan (blue arrows in Fig. 0(b)) the Backward Reachable Set of is the set of all the configurations from which is reachable. In particular, a configuration is in the backward reachable set of , if starting from and following the feedback plan , there exists a time in which the vehicle’s configuration in the set (i.e. inside the cell occupied by ). What stated above can be summarized as follows:

(1)
such that

The region of interest can be represented with its outer borders as illustrated in Fig. 0(b). The generic outer border is , while the set of all the outer borders is . We can reformulate the backward reachable set by observing that is reachable from if the trajectory starting from intersects . Exploiting this observation, the backward reachable set becomes:

(2)

This second formulation allows us to encode and , which are respectively 2-D and 3-D subspaces, with the union of respectively 1-D sets (outer borders) and 2-D regions (coordinate and heading angles at the borders).

Iii Cellular Backward Reachability

In this section, we describe how to compute the backward reachable set of a border in a point inside a cell. We start by observing (Fig. 1(a)) that the commanded direction and the quantity delimit two regions: left turns (LT) and right turns (RT). The vehicle’s orientation can be either in one of the abovementioned regions or equal to . Consequentially, the control law can be formulated as follows:

(3)

In order to establish the direction of the turn, must be compared with and . In our formulation, we assume that angles less than and greater then are compared wrapping them in the interval , while angles outside this range are compared wrapping them in the interval .

(a)
(b)
Fig. 2: Regions of left and right turns (a). Maximum and minimum turns for trajectories starting from the considered point and ending in the top border’s edges (b).

We illustrate the formulation only for the case of top border backward reachability (Fig. 1(b)), because for other borders we can exploit the same principles.

The backward reachable set ( means top border) for a point is the set of angles from which the top border is reachable. Such a set is continuous and delimited by a minimum and a maximum provided by the two piece-wise functions and 111We omit the dependence from the cell size and the minimum turning radius .. Our objective in this section is to illustrate the geometric principles that are used to derive the functions and .

From Fig. 1(b) we can infer that minimum and maximum angles can be found by considering the top border’s edges. There are two circles having radius passing through the point and the edge . Each circle corresponds to either a left turn or a right turn and the centers of the two circles are obtained as follows:

(4)

In our formulation, we use the local reference frame illustrated in Fig. 2(a).

With the coordinates we can find the initial angles depicted in Fig. 1(b). In particular, for left turns we can find the direction from the center ( is the type of edge, which is either L or R) towards the considered point and then rotate it counterclockwise of 90 degrees:

(5)

For right turns, the rotation must be 90 degrees clockwise:

(6)

The set identified by and must contain only trajectories that are entirely contained in the cell. This requirement implies that in some location the minimum or maximum left/right turn is not given by a circle intersecting an edge. In particular, it might be necessary to find the circle tangential to one of the side borders or the top border. In Fig. 3, there are examples showing when it is necessary to use the circles tangential to the borders. In those examples, continuous blue lines are tangential turns, while dotted red lines are turns given by circles intersecting the edges.

(a)
(b) (c) (d) (e)
Fig. 3: Local reference frame (a). Right turn tangential to the left border (b), right turn tangential to the top border (c), left turn tangential to the right border (d), left turn tangential to the top border (e).

In order to establish if the trajectory exits from one of the side borders or a point internal to the top border we must test the exit angles. In particular, we have four cases:

  • if (Fig. 2(b)), find right turn tangential to left border;

  • if (Fig. 2(c)), find right turn tangential to top border;

  • if (Fig. 2(d)), find left turn tangential to right border;

  • if (Fig. 2(e)), find left turn tangential to top border;

The circle tangential to a border can be easily obtained by imposing the intersection in the point and that its distance from the border is equal to . Imposing these two conditions, two circles are obtained; thus, we must make sure to pick the correct solution. For instance in the case of left turn tangential to right border we have:

(7)

The functions and are piece-wise and their conditions are obtained by comparing and with either or . In the following sections, we illustrate how to obtain the two functions making the distinction between the two types of commands: (forward command) and (backward command).

Iii-a Top Border Backward Reachable set: Forward Command

In the case of a forward command, the type of curve for minimum and maximum heading angle is determined by comparing with both the initial and exit angles. If and , then the minimum is given by a left turn (Figs. 3(b) and 3(a)) and it is equal to (if , is in the RT region, therefore a left turn cannot start with it). Instead, if and , then the minimum is given by a right turn (Fig. 3(c)) and it is equal to . If none of the abovementioned conditions is true, then the minimum will be given by either an S or a CS path. In this case the minimum is obtained using the geometric construction depicted in Figs. 3(e) and 3(d). The straight line (CS border) intersecting the right edge and having equation:

(8)

splits the cell in three regions. A point can be above the line (A region), on the line (L region), or below the line (B region):

  1. (A region)

  2. (L region)

  3. (B region)

For locations lying on the border, the minimum is and it is given by a straight line. Instead for positions that are in the regions above and below the CS border, the minimum is given by a left and a right CS path, respectively. The exit angle is equal to the commanded heading angle , while the minimum is computed using the circle that is tangential to the CS border and that intersects the point . If , the circle tangential to the CS border corresponds to a left turn. The center of the circle and the corresponding initial angle are computed as follows:

(9)

where and are the ones computed in Eq. 8. When , the circle tangential to the CS border corresponds to a right turn. In this case, the equations for the circle and the initial angle are the same as in Eq. 9 except for and the angle , which are computed as follows:

(10)
(a)
(b)
(c)
(d)
(e)
Fig. 4: Cases for minimum: left turn (a), limited left turn (b), right turn (c), LS path (d), RS path (e).

The formulation for is based on the same considerations used for the minimum. In this case, we must deal with the left side of the cell as illustrated in Fig. 5. If and , then the maximum is given by a right turn (Figs. 4(b) and 4(a)) and it is equal to (if , is in the LT region, therefore a right turn cannot start with it). When and , then the maximum is given by a left turn (Fig. 3(c)) and it is equal to . If none of the above conditions is true, then the maximum is given by either a S or a CS path. In this case we use the equations for the CS border used before with the following differences:

  • (the CS border must intersect the left edge );

  • a circle in region A corresponds to a right turn, while a circle in region B corresponds to a left turn as shown in Figs. 4(d) and 4(e).

(a)
(b)
(c)
(d)
(e)
Fig. 5: Cases for maximum: right turn (a), limited right turn (b), left turn (c), LS path (d), RS path (e).

In Fig. 6 there in an examples of cellular backward reachable set (for the top border) in the case of forward command.

Fig. 6: Example of backward reachable set for forward command.

Iii-B Top Border Backward Reachable set: Backward Command

In this section, we study the backward reachability for . The first difference with the previous case is that only C paths are admissible. To determine the direction of the minimum and maximum turns we must compare angles with .

(a)
(b)
Fig. 7: Angle regions for minimum (a) and maximum (b) in the case of backward command.

Referring to Fig. 7, we can see that for the definition of the minimum and the maximum we can identify three angle intervals. For the minimum, the first interval is and the minimum is given by a right turn with starting angle . Instead for the maximum, in the first interval we have and the maximum is .

In the second interval, the condition for the minimum is and the minimum is given by a left turn with starting angle (a right turn would intersects the right border rather than the top border). While for the maximum we have and the maximum is .

Finally, in the last region the condition for the minimum is and the minimum is (the angle cannot be smaller, otherwise the circle would intersect the right border); while the condition for the maximum is and the maximum is .

With backward commands the top border might not be reachable from some location. This happens when the minimum turning radius is close to the cell size. In particular, if the minimum is associated to a left turn and , then such a turn does not lead to the top border. In fact, any left turn with initial angle greater than exits the cell from the left border. For the maximum there is a similar problem. There is no solution if the maximum is given by a right turn with . The complete condition for the existence of a solution is the following:

(11)

In Fig. 7(a) there is an example in which for every point there exists a solution, while Fig. 7(b) illustrates an example where the top border is not reachable from some locations.

(a)
(b)
Fig. 8: Examples of backward reachable set for backward command.

Iii-C Border to Border Backward Reachability

In our method, we propagate sets from one border to another, therefore we use the univariate functions and , where is the arrival border and is the starting border. For instance, and provide the top border backward reachable set in the right border ( is the coordinate at the border according to the local reference frame in Fig. 2(a)) and they are obtained from and imposing .

Iv Border to Border Forward Reachability

In the previous sections, the functions and provide the set of all the angles that a trajectory starting from a point of border B can have in order to reach border A. In this case, the arrival position and heading angle are not considered. Instead, in this section, we deal with the opposite problem, which consists in deriving the piece-wise functions and that provide minimum and maximum arrival angles in a point of border A for trajectories starting from border B. Therefore, this two functions define the forward reachable set of the border A at the border B.

The process is illustrated for one border because the functions for other borders can be obtained using the same geometric principles. In particular, we illustrate the cases in which the bottom border is the starting border and the top and right borders are the ending borders. The case of left border is dual to the case of the right border, therefore we do not provide a detailed explanation.

In the description, for the top border we distinguish between: (forward command) and (backward command), while for the right border we distinguish between (right command) and (left command).

Iv-a Bottom Border Forward Reachable Set at Top Border: Forward Command

We are considering the forward reachability for the bottom border; therefore, we must consider turns that start from the edges of the bottom border. The ending point is located in the top border. Like the case of backward reachability, Eq. 4 is used to find the circles intersecting the edges and the considered point . In this case, with the local reference frame in Fig. 2(a), is always equal to , while the edges of the bottom border are and . Also for the forward reachable set, only trajectories that are entirely contained in the cell must be considered. In particular, for trajectories starting from the bottom border and ending at the top border there are two cases in which the tangential circle must be computed: (Fig. 8(b)) and (Fig. 8(c)).

(a)
(b)
(c)
Fig. 9: Angles for forward reachability (a). Right turn tangential to left border (b). Left turn tangential to right border (c).

For forward commands, we must evaluate the exit angles and there are two possible scenarios: and

We start considering the first one, which is illustrated in Fig. 9(a). We can identify five angle intervals. For the case of commanded heading angle , both minimum and maximum are given by a right turn and they are respectively and . In fact, all the trajectories starting from the bottom border attempt to reach , but none of them is able to reach it (the minimum reachable angle is ).

In the second interval, the commanded heading angle is . For angles in this interval the maximum is still , instead the minimum is . Therefore, the maximum is given by an R path, while the minimum by an RS path.

In the interval given by the condition , neither left nor right turns are possible. In this case, minimum and maximum are exactly . Therefore, such a forward configuration is the end configuration for an S or a CS path. In Fig. 9(c), the angles and are obtained from the slopes of the straight lines passing through the considered point and respectively left and right edges of the bottom border. Since , there always exist an S path, a set of RS paths and a set of LS paths. The elements of these sets are obtained by varying the position in which the circle intersects the bottom border.

The fourth and fifth intervals are given by the condition and . These last two cases are dual to the first two cases. For both ranges, the minimum is given by a left turn and it is . Instead the maximum is different for the two cases. In the first one, it is and it is given by LS paths, while in the second case it is and it corresponds to a left turn.

When there are still five intervals as shown in Fig. 9(b). For the following four intervals:

the rules derived previously for the angles , , , and are still valid. Instead for , now we have the condition . In this case, we do not have a singleton anymore, instead we have a set having minimum given by a left turn and maximum given by CS paths.

(a)
(b)
(c)
Fig. 10: Angle intervals for: (a), (b). Example of forward reachable set at the top border consisting in a singleton (c).

Iv-B Bottom Border Forward Reachable Set at the Top Border: Backward Command

Here we consider the case . The first substantial difference between the two cases is that while for forward commands the reachable set evaluated in one point is a continuous set delimited by a minimum and a maximum, for backward commands there are scenarios in which the reachable set splits in two regions. The other difference is that rather than comparing the exit angles with the commanded heading angle , we compare the starting angles with the angle . Furthermore, S and CS paths cannot exist.

To derive the functions in this case, we make the following distinction: and . We start considering the first one, which is depicted in Fig. 11, where we can identify five regions.

Fig. 11: Angle regions for .

In the first region, where , is the minimum and is the maximum.

In the second interval, the condition is . In this case, the maximum is still , while the minimum is computed using the geometrical construction depicted in Fig. 11(a). In particular, we have to find the circle having the following properties:

  1. it intersects the top border in the considered point ;

  2. it is tangential to the straight-line having slope ;

  3. it intersects the bottom border in any point.

The minimum corresponding to this circle in our local reference frame is computed as follows:

(12)

The cases and are dual to the previous two. More specifically, in the first one the minimum is , while the maximum is . Instead, for in order to compute the maximum we must use the circle that intersects the top border in and that is tangential to the straight line with slope as shown in Fig. 11(b). In this case, using the local reference frame the formula for the maximum is the following:

(13)
(a)
(b)
(c)
Fig. 12: Example of minimum (a) and maximum (b) limited by . Example of forwad set consisting of two continuous regions (c).

The last case that we must consider is . In this scenario, the forward reachable set for the considered point is an empty set because neither left nor right turns starting from the bottom border can reach the top border in the considered point.

Although we mentioned that for backward commands the forward reachable set can consist of two disjoint sets, we can notice that for the case of we always have a continuous set.

Fig. 13: Angle regions for .

When we can identify again five regions as shown in Fig. 13. For the cases , , , the forward reachable set is a continuous set delimited by the same minimum and maximum computed for their homonyms in the case of .

For , in general we have two disjoint sets, which are computed using the geometric construction depicted in Fig. 11(c). The first set is delimited by maximum and minimum given by left turns, while in the second set minimum and maximum are given by right turns. In the first case, the maximum is , while the minimum is given by a left turn having circle tangential to the straight line having slope . The derivation of the second set is dual. More specifically, in this case the minimum is , while the maximum is obtained with a right turn whose circle is tangential to the straight line having slope .

Iv-C Bottom Border Forward Reachable Set at the Right Border: Right Command

In this section, we consider trajectories that start from the bottom border and end in the right border. We start with the case of right commands, where the boundary of the reachable set is obtained comparing with the exit angles. Using Eq. 4 we can find the circles intersecting the edges and and the considered point .

Also in this case, we must assure that trajectories do not exit from any border before reaching . In particular, in two cases we must recompute the turns:

  • if in a left turn starting from the initial angle is , the circle tangential to the bottom border and intersecting must be computed;

  • left turns starting from the right extremity are always tangential to the right border as shown in Fig. 13(b), therefore for every point .

(a)
(b)
Fig. 14: Example of left turn tangential to the bottom border (Fig. 13(a)) and the right border (Fig. 13(b)).

When deriving the angle limits, we must distinguish between the two cases: and .

(a)
(b)
Fig. 15: Angle intervals for: (a) and (b).

The case is illustrated in Fig. 14(a), where we can identify five intervals for the commanded heading angle. In the first interval the condition is . In this case, the minimum is , while for the maximum we must compare with . If , then the minimum is , otherwise it is computed using the circle tangential to the straight line having slope as illustrated in Fig. 15(a).

(a)
(b)
Fig. 16: Example of maximum limited by (a). Example of forward reachable set at the right border consisting in a singleton (b).

In this case the maximum is computed as follows:

(14)

The second interval is the one in which . In this case, minimum and maximum are respectively and .

In the third interval the commanded heading angle is , therefore the maximum is still , while the minimum is and it is given by RS paths (and an S path if the straight line having slope intersects the bottom border).

In the fourth interval, the forward reachable set in the considered point is a singleton because the condition does not allow the existence of right and left turns that can reach the right border. Instead, as shown in Fig. 15(b), the considered point is reached by an S path and CS paths with final heading equal to .

The last case is , where the minimum is , while the maximum is equal to and is given by S and CS paths.

When minimum and maximum for the angles , , , and depicted in Fig. 14(b), are the ones computed for the homonym angles in the case . For the fourth interval, since , the minimum is and the maximum is .

Iv-D Bottom Border Forward Reachable Set at the Right Border: Left Command

We now consider the case . In this scenario, we must compare the starting angles with making the distinction between the two cases: and . In the first case, the forward reachable set is either a continuous set or an empty set.

Fig. 17: Angle regions for .

In order to compute minimum and maximum we identify four intervals as shown in Fig. 17. In the first one, since , is the minimum and is the maximum.

When , is still the maximum, while the minimum is limited by . In particular, we use the geometric construction shown in Fig. 17(a) and the minimum is computed as follows:

(15)
(a)
(b)
(c)
Fig. 18: Example of minimum (a) and maximum (b) limited by . Example of forward reachable set consisting in two disjoint sets (c).

For there is no solution because there not exits a C path that can reach the considered point.

The last case is , for which is the minimum, while the maximum is computed using the circle tangential to the straight line having slope as shown in Fig. 17(b)222 for left commands cannot be greater than .. In this case the maximum is computed with Eq. 14.

Fig. 19: Angle regions for .

When minimum and maximum for the cases , and shown in Fig. 19 are computed as described above for the case .

For , the forward reachable set in can be either a continuous set or consist of two disjoint sets. In particular, there is always the set with maximum and minimum computed with Eq. 15. The condition for the existence of the second set is , with computed as follows:

(16)

In Fig. 19(a) there is an example depicting the scenario described above, in which for the point there are two disjoint sets and the second one is a set of right turns that start with initial angle .

(a)
(b)
Fig. 20: Forward reachable set in a single point for (a). Forward reachable set for the whole right border, with focus on the region of right turns starting with (b).

In Fig. 19(b) , there is an example of 2-D forward reachable set at the right border, where the horizontal axis is the position, while the vertical axis is the orientation . The upper region of the forward reachable set is given by left turns. Instead in the lower region, the portion on left of the first vertical dashed line corresponds to the case of of Fig. 19, while the region between the two lines corresponds to of Fig. 19. For this portion of the lower region, the figure illustrates also the trajectories for the extreme points.

Also in the case we can have two disjoint sets as illustrated in Fig. 17(c). In this case, in the first set, the maximum is , while the minimum is computed with Eq. 15. In the second set, the minimum is , while the maximum is computed with Eq. 14.

V Global Backward Reachability

In the previous sections, we first illustrated how to compute the backward reachable set of a border in a generic point. Then we mentioned that fixing one of the coordinates, we obtain the function specific for points lying on a border (different from ). Subsequently, we illustrated how to compute the forward reachable set of a border in points lying on a border with . In this section, we illustrate how to combine the two types of sets to compute the global backward reachable set of a region of interest .

The strength of this method is that rather than performing a backpropagation of the region in the 3-D space, we expand the borders. The advantages are twofold: reduced computational complexity and reduced spatial complexity. The computational complexity is reduced because rather than discretizing the whole 3-D space and evolving the trajectories, we evolve the boundaries of the cells. The memory usage is also reduced because the reachability of a region is encoded with a set of 2-D maps rather than a 3-D map.

V-a Backward Border Mapping

The pivotal mechanism for the computation of the global backward reachable set is the propagation of a region using the intersection of the regions delimited by and .

(a)
(b)
Fig. 21: Example of workspace with region of interst (red cells) (a). 3-D view of backward expansion in one direction.
(a)
(b)
Fig. 22: Intersection between region of interest backward reachable set (blue area) and forward reachable set (red curves) (a). Set is a subset of the cellular backward reachable set (delimited by green curves) (b).

To illustrate this process we consider the example shown in Fig. 21, where for the sake of clarity, we illustrate the backward expansion only for one side and only in one direction, therefore the figures shown in the following explanation show “incomplete” sets. Referring to Fig. 20(a), we consider the backward expansion of the bottom border of the cell (5,4)333In the notation (,), is the row and is the column. downwards across the lower three cells neglecting the expansion through the side borders.

We start with the set which is in one of the borders of the region of interest, therefore because any configuration is considered part of the backward reachable set. In order to propagate the set downwards to the bottom border of cell (4,4), we must use and , where and means that trajectories start from the bottom border and end in the top border. These two functions provide the limits of the set , which contains all the initial configurations of the bottom border of the cell (4,4) from which the top border of the same cell is reachable.

The next step is the propagation of the set towards the bottom border of cell (3,4). The set delimited by and contains all the starting configurations from which the top border of cell (3,4) is reachable. However, among those configurations, we are only interested in those for which trajectories end in . Therefore, consists of all the configurations whose integration leads to the intersection between the forward reachable set delimited by and and the set (Fig. 21(a)). As illustrated in Fig. 21(b), is a subset of the cellular backward reachable set. In this example, the last step is the propagation of to the bottom of cell (2,4), obtaining . Fig. 20(b) shows the complete expansion from to .

V-B Iterative Border Expansion

The propagation of the borders must be computed in every direction. This leads to the presence of many disjoint sets for each border, which we must keep track of during the expansion. As an alternative to the propagation of single borders through the whole map, we can use Algorithm 1.

input : , grid-map GM
output : Backward Reachable Set BRS
1 while at least one configuration is added to BRS do
2       foreach border i do
3             foreach neighbor j do
4                   if  and GM margins  then
5                         propagate sets of i to j
6                  
7             end
8            
9       end
10      
11 end while
return BRS
Algorithm 1 Iterative Border Expansion

In this case, for each border, there is a bitmap that indicates which configurations are part of the backward reachable set of the region of interest. In the first iteration, only borders of the region of interest have nonempty sets. For each iteration, the set of a border is propagated to the neighboring borders as shown in Fig. 22(b). The algorithm iterates until no new configuration is marked as part of the backward reachable set and it returns a map for each border of the grid-map.

The 3-D reachability map can be computed in real-time by checking if the trajectory starting from the considered configuration (internal to the cell) ends in a cell of the border map marked as part of the backward reachable set. An example of complete backward reachable set is shown in Fig. 22(a).

(a)
(b)
Fig. 23: Global backward reachable set (a). Six neighboring borders(b).

The space complexity of the algorithm can be evaluated by assuming that the workspace is discretized with a square grid-map having cells per side. Neglecting the borders at the margins of the grid-map, the number of tables necessary to characterize the whole workspace is at most . Assuming that angle and position on the border are discretized using cells, the number of bits necessary to represent the backward reachability is . Instead, the discretization of the whole 3-D global map requires bits. For instance with and , the number of required bits in our method is , while for a 3-D discretization it is .

Vi Conclusions

In this paper, we presented a verification method that is based on the computation of the backward reachable set of a region of interest. The method is formulated specifically for vehicles having a minimum turning radius that is greater than the resolution of the discrete feedback motion plan that they must follow.

The paper illustrates the geometric principles used for the computation of the reachable sets and practical implementation of the method based on an iterative expansion of the borders. Future work will focus on the extension of the method to the case of bounded angular acceleration and the development of a more efficient numerical implementation.

References

  • [1] D. Adzkiya, B. De Schutter, and A. Abate (2014) Backward Reachability of Autonomous Max-Plus-Linear Systems. IFAC Proceedings Volumes 47 (2), pp. 117–122. External Links: Document, ISSN 1474-6670 Cited by: §I.
  • [2] D. Althoff, M. Althoff, and S. Scherer (2015) Online safety verification of trajectories for unmanned flight with offline computed robust invariant sets. In 2015 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 3470–3477. External Links: Document, ISBN VO - Cited by: §I.
  • [3] E. Asarin, T. Dang, G. Frehse, A. Girard, C. L. Guernic, and O. Maler (2006) Recent progress in continuous and hybrid reachability analysis. In 2006 IEEE Conference on Computer Aided Control System Design, 2006 IEEE International Conference on Control Applications, 2006 IEEE International Symposium on Intelligent Control, pp. 1582–1587. External Links: Document, ISBN 2165-3011 VO - Cited by: §I.
  • [4] H. Chitsaz and S. M. LaValle (2007) Time-optimal paths for a Dubins airplane. In 2007 46th IEEE Conference on Decision and Control, pp. 2379–2384. External Links: Document, ISBN 978-1-4244-1497-0 Cited by: §I.
  • [5] L. E. Dubins (1957) On curves of minimal length with a constraint on average curvature, and with prescribed initial and terminal positions and tangents. American Journal of mathematics 79 (3), pp. 497–516. External Links: ISSN 0002-9327 Cited by: §I.
  • [6] A. El-Guindy, D. Han, and M. Althoff (2017) Estimating the region of attraction via forward reachable sets. In 2017 American Control Conference (ACC), pp. 1263–1270. External Links: Document, ISBN VO - Cited by: §I.
  • [7] V. M. Goncalves, L. C. A. Pimenta, C. A. Maia, B. C. O. Dutra, and G. A. S. Pereira (2010) Vector Fields for Robot Navigation Along Time-Varying Curves in n-Dimensions. IEEE Transactions on Robotics 26 (4), pp. 647–659. External Links: Document, ISSN 1552-3098 VO - 26 Cited by: §I.
  • [8] P. Konkimalla and S. M. LaValle (1999) Efficient computation of optimal navigation functions for nonholonomic planning. In Robot Motion and Control, 1999. RoMoCo’99. Proceedings of the First Workshop on, pp. 187–192. External Links: ISBN 0780356551 Cited by: §I.
  • [9] Y. Lin and S. Saripalli (2014) Path planning using 3D dubins curve for unmanned aerial vehicles. In Unmanned Aircraft Systems (ICUAS), 2014 International Conference on, pp. 296–304. External Links: ISBN 1479923761 Cited by: §I.
  • [10] D. G. Macharet, A. A. Neto, V. F. da Camara Neto, and M. F. M. Campos (2011) Nonholonomic path planning optimization for dubins’ vehicles. In Robotics and Automation (ICRA), 2011 IEEE International Conference on, pp. 4208–4213. External Links: ISBN 1612843859 Cited by: §I.
  • [11] K. Margellos and J. Lygeros (2013) Toward 4-D Trajectory Management in Air Traffic Control: A Study Based on Monte Carlo Simulation and Reachability Analysis. IEEE Transactions on Control Systems Technology 21 (5), pp. 1820–1833. External Links: Document, ISSN VO - 21 Cited by: §I.
  • [12] G. Miraglia and L. Hook (2017) Dynamic geo-fence assurance and recovery for nonholonomic autonomous aerial vehicles. In AIAA/IEEE Digital Avionics Systems Conference - Proceedings, Vol. 2017-Septe. External Links: Document, ISBN 9781538603659, ISSN 21557209 Cited by: §I.
  • [13] G. Miraglia and L. Hook (2019-10) A feedback motion plan for vehicles with bounded curvature constraints. In 2019 IEEE 10th Annual Ubiquitous Computing, Electronics & Mobile Communication Conference (UEMCON) (IEEE UEMCON 2019) (In press), Columbia University,New York, USA. Cited by: §I.
  • [14] I. M. Mitchell (2007) Comparing forward and backward reachability as tools for safety analysis. In International Workshop on Hybrid Systems: Computation and Control, pp. 428–443. Cited by: §I.
  • [15] J. Nilsson, J. Fredriksson, and A. C. E. Ödblom (2014) Verification of Collision Avoidance Systems using Reachability Analysis. IFAC Proceedings Volumes 47 (3), pp. 10676–10681. External Links: Document, ISSN 1474-6670 Cited by: §I.
  • [16] C. Pek and M. Althoff (2018) Efficient Computation of Invariably Safe States for Motion Planning of Self-Driving Vehicles. In 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 3523–3530. External Links: Document, ISBN VO - Cited by: §I.
  • [17] M. Shanmugavel, A. Tsourdos, B. White, and R. Żbikowski (2010) Co-operative path planning of multiple UAVs using Dubins paths with clothoid arcs. Control Engineering Practice 18 (9), pp. 1084–1092. External Links: ISSN 0967-0661 Cited by: §I.
  • [18] M. Shubert, M. Oishi, M. Baldwin, and R. S. Erwin (2018) Under-Approximating Reach-Avoid Sets for Space Vehicle Maneuvering in the Presence of Debris. IFAC-PapersOnLine 51 (12), pp. 142–147. External Links: Document, ISSN 2405-8963 Cited by: §I.
  • [19] B. Xue, Z. She, and A. Easwaran (2016) Under-Approximating Backward Reachable Sets by Polytopes BT - Computer Aided Verification. S. Chaudhuri and A. Farzan (Eds.), Cham, pp. 457–476. External Links: ISBN 978-3-319-41528-4 Cited by: §I.