A Feedback Motion Plan for Vehicles with Bounded Curvature Constraints

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

The use of a feedback motion plan instead of the decoupled scheme consisting of separate plan and control phases can facilitate the task of proving the properties of an autonomous system. The advantage of using a feedback motion plan is the possibility to validate the whole plan offline before its execution, which means that trajectories having different initial states can be tested simultaneously. In this paper, we formulate a feedback motion plan based on the extension of the wavefront expansion to the case of vehicles having bounded curvature. Additionally, the use of a transition function and a Gaussian filter limits undesired oscillations in the resultant trajectories. The method is suitable for both single goal missions and path following. The paper illustrates the algorithm for the generation of the plan and presents simulation data containing example trajectories and analysis of tuning parameters. Finally, future developments are discussed.



There are no comments yet.


page 2

page 4

page 5

page 6

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

Both industry and academia are working to define proper standards and certification strategies for autonomous transportation systems [7]. The common requirement in these strategies is the ability to prove the robustness and safety of the autonomous system in different scenarios. To satisfy this general requirement for the navigation task specifically, it is crucial to prove that the autonomous system can recover safely after the occurrence of unpredicted events. This task is not trivial, especially if planning and control phases are decoupled. In this case, if an excessive deviation occurs it may be necessary to perform an on-line replan. Although in many instances this is acceptable, for applications such as those involving unmanned or autonomous air vehicle systems, computed paths are required to be pre-approved in order to assure compliance with regulations and safety constraints. An alternative approach that allows for the simultaneous verification of a large number of paths [15], is the use of a feedback motion plan [12], where for each vehicle position and heading there is a specific pre-computed action that will be executed. This approach provides the ability to reject disturbances and recover from deviations thanks to the integration of planning and control.

In our method, we generate a feedback motion plan for vehicles with bounded curvatures moving in the 2-D space. The method extends the wavefront expansion algorithm [2] to the case of vehicles having minimum turning radius constraints. The use of a transition function and a Gaussian filter limits the number of oscillations in the resulting path. Finally, since the formulation is based on the wavefront expansion, it allows for the presence of obstacles having non-convex shape.

The effectiveness of the method is demonstrated via simulations based on a 2-D kinematic model.

The paper is organized as follows. Section II provides a review of relevant related work. Section III provides the theoretical formulation of the problem, while Section IV illustrates the vector field generation process. In Section V, simulations results are shown and discussed. Finally, we offer conclusions and discuss future work.

Ii Related Work

The feedback motion plan presented in this paper consists of the generation of a pre-calculated vector field. In a vector field, a vector is assigned to each point on a manifold [22] to control a robot indicating the direction of state evolution. Vector fields have been exploited in both classical path planning and path following problems. The strength of methods based on vector fields is that it is possible to integrate path planning and robot control in the same approach. Furthermore, vector field guidance can lead the robot in the right direction even in the presence of small localization errors [6]. A vector field can be generated employing the gradient of a potential function . Given a potential function, the robot is guided using Gradient Descent [2], where the robot takes a “downhill” path to reach the goal configuration. The main challenge is to guarantee the existence of a minimum only at the goal configuration. Many potential functions do not lead to complete path planners because of the existence of multiple local minima [2, 10, 12], which means that the robot can be “trapped” in wrong configurations. A solution to this problem is the use of harmonic functions [9]. However, in this case, often it is difficult to find the appropriate harmonic function.

In addition to the local minima problem, another problem that affects methods based on vector fields is the presence of oscillation in the resulting trajectory. This phenomenon is often referred to as chattering [2]. To alleviate this problem Ren et al. in [20] propose the use of the modified Newton’s method (MNM), which is a technique used in optimization theory. They show that the use of MNM reduces the presence of oscillations regardless of the type of potential function. Nevertheless, the study presented in [20] is restricted to the case of convex obstacles; therefore, it neglects the local minima problem that can arise with non-convex obstacles.

Vector fields have also been employed for the guidance of nonholonomic robots [19, 17, 3]. In [13] Lindemann and LaValle illustrate a method in which cell decomposition and local vector fields are employed. However in this case, instead of using the gradient of a potential function they directly build local vector fields and combine them smoothly using bump functions and generalized Voronoi diagram (GVD).

The objective of the abovementioned guidance methods is to reach a single goal location; however, vector fields have also been employed in path following problems [6][14] . In [16], Nelson et al. formulate a vector field for the guidance of a Small Unmanned Aerial Vehicle (UAV). In this case, the vectors represent the desired course inputs that must be fed to the attitude control loop. More specifically, the authors define continuous vector fields that can be used to follow both straight-line paths and circular arcs or orbits. In [8] Jung et al. extend the path following for straight-line paths and orbits to account for both angle and time arrival.

In all the above-mentioned methods, single goal mission and path following are addressed singularly. Furthermore, many of the methods consider only convex obstacles for single missions or neglect the presence of obstacles for path following. Instead, the method presented in this paper is versatile, because it can be used for both single goal missions and path following. Finally, the proposed method is general because it does not require a specific control law.

Iii Problem Formulation

The feedback motion plan proposed in this paper is formulated for a specific class of constrained systems, which travel with a constant speed and have a limited turning rate . The constraints to the vehicle’s motion can be formulated in terms of minimum turning radius:


A vehicle with this type of motion is usually referred to as Dubin’s vehicle [4]. In this case, we can use the following kinematic model:


where are the vehicle’s coordinates in the workspace , which is a bounded subset of . In Eq. 2 the only control variable is the turning rate ; therefore, it is the model of a first-order underactuated system. Path planning for this type of vehicle is often referred to as the Steering Problem [11]. In our formulation, the vehicle’s configuration is , with , and the action space is . With the identification , the configuration space is . The set is the region occupied by obstacles, therefore the free work space is . The free configuration space is defined as . Without loss of generality, we assume that is defined accounting for the actual vehicle’s dimensions. In our discussion, the state space is , thus the state transition equation 111With a little abuse of notation we use to indicate both the state and the state variable that corresponds to the coordinate in the Euclidean space corresponds to Eq. 2. To account for minimum turning radius constraints, we define the buffer region as follows:


where the scalar defines the width of the buffer region. Finally, we define the region of safe start (SS) as . The regions in the configuration space (and thus in the states space ) are obtained by extending the corresponding regions of the workspace along the -axis.

Fig. 1: Example of workspace (a) and configuration space (b). Green indicates free region, yellow indicates buffer region and black indicates obstructed region.

Our objective is to find a feedback plan , which indicates the action that must be taken to reach the goal set for every state . In our formulation we consider two scenarios:

  1. single goal destination;

  2. predefined path.

In the first case, given the goal position in the workspace , the goal set is defined as follows:


Therefore, the goal set is a cylinder having radius . This definition of the goal set is due to the vehicle’s kinematics, which has a nonzero constant speed. In the second case, the goal set is defined as follows:


where is the maximum acceptable error for each state variable. Henceforth, we will rely on the following assumption.

Assumption 1

the goal region is entirely contained in the region of safe start , therefore and .

The feedback plan is based on a discrete 2-D vector field, which can be defined as follows:


Since the vehicle’s velocity vector has a constant magnitude, we can use a normalized vector field, in which the vector magnitudes are . In this case, can be characterized using a single grid-map, in which the vector orientations is stored. Therefore, Eq. 6 becomes:


We can now define the feedback plan as follows:


where is the state, is the orientation error and is the control law applied to correct the error. The formulation in Eq. 8 means that if the vehicle is inside the buffer region it must turn with its minimum turning radius, while outside the buffer region it can use any control law to correct the heading angle.

The heading error can be calculated as follows:


Finally, we assume that the work space is represented with a 2-D bitmap array as follows:


where a indicates a cell of , while a indicates a cell of . In our formulation, we will use the 2-neighborhood [1], often referred to as 8-point connectivity [2, 18], which means that in the wavefront expansion, in addition to horizontal and vertical directions, the expansion is performed also in the diagonal directions.

Remark 1

In our formulation we discretize the and axes of the -space, while the -axis is continuous. Therefore, the -space is decomposed in rectangular boxes , where and are the number of cells for respectively -axis and -axis, and res is the resolution of the grid-map used for the workspace .

Iv Vector Field Generation

Fig. 2: Map generation process (WE: wavefront expansion, VF: vector field).

The process followed to generate the vector field is illustrated in Fig. 2. The inputs are the bitmap and the ordered set of cells occupied by the goal region . For single goal mission the goal consists in one cell; while for path following, the set consists of all the adjacent cells traversed by the path.

In the first step, a “complete” map is generated by additing the buffer region and the goal region . Like in , and are encoded respectively with and ; instead, is indicated with the value , and with . The cells occupied by are computed by using the Brushfire Algorithm [2], in which a grid is used to approximate distances.

input : bitmap BM, goal set
output : complete map M
4 while  do
6       foreach  do
9             if  then
11             else if  then
13             end if
15       end
17 end while
18foreach  do
20 end
21foreach  do
23 end
return M
Algorithm 1 Complete Map Genereation

In Algorithm 1, is computed in the loop from line 4 to line 15 using the 8-point connectivity. The width of the buffer region is computed in Line 2, where res is the resolution of the map in terms of cell’s width. In Fig. 3, there is an example of computation of the buffer region. The remaining part of Algorithm 1 fills the cells occupied by and .

Fig. 3: Example of Buffer region.

In the second step the cost map CM is generated applying the wavefront expansion, as shown in Algorithm 2, whose differences with respect to Algorithm 1 is that the expansion starts from cost 2 and all the zero-valued cells are filled. Instead Algorithm 1 ends once the buffer region has been covered. It is important to notice that in the wavefront expansion performed in Algorithm 2 cells of the buffer region are considered obstacles.

input : map M
output : cost map CM
2 while   do
3       foreach  do
6             if  then
8             else if  then
10             end if
12       end
14 end while
return CM
Algorithm 2 8-point connectivity WE

The cost map CM is then used to generate , where in cells of the vector field is generated using gradient descent, while for cells occupied by and , the vectors point towards the closest point of the buffer region’s external border.

Fig. 4: Example of complete map (a) and “raw” vector field (b).

In the generated vector field there are inward flows in proximity of both the path that must be followed and the obstacles’ borders. Due to the minimum turning radius constraints, an inward flow leads to oscillations that cause a waste of time and energy. Therefore, it is necessary to mitigate this effect and reduce the presence of oscillations in the path. In our solution, a transition region is used in proximity of both the target path and the buffer regions’ borders. The definition of this region is based on the sliding mode resulting from Filippov’s convex method [5], where for a nonlinear system with discontinuous right-hand side


at switching boundary , we have:


In our vector field we thicken the boundary obtaining a region having a width proportional to the minimum turning radius. Referring to Eq. 12, in our case is either the vector field tangential to the path or the vector field at the buffer region’s outer border. We refer to these two regions with the term “edge”. Instead, is the original vector field in the considered location. We indicate the former with and the latter with , where is the position of the closest edge with respect to the considered position . In our definition, is a potential function of both the distance from the closest edge and the angle difference between the two vectors. The new vector field is obtained as follows:


The tuning parameters for the transition regions are: , , , where stands for path and for border. The value of is used to tune the “speed” of the rotation (i.e. the smaller the value, the sooner the flow becomes parallel to the edge). Instead regulates the width of the transition region.

To further reduce the presence of oscillations, in the final step of our method, we exploit a technique that is widely used in image processing. In particular, we use a Gaussian filter [21], which is appealing for our application because it removes high frequency components, therefore in our case it reduces abrupt changes in the flow’s direction. A 2-D Gaussian is defined as follows:


A Gaussian kernel is a discrete approximation (i.e. a matrix) of Eq. 19

. In our filter, we employ a kernel with a variable size, which depends upon the standard deviation



The final vector field is obtained by performing the convolution between each component of and the Gaussian kernel:

Fig. 5: Example of non-smoothed vector field (a) and smoothed vector field (b).

V Simulation Results

Fig. 6: Example of single goal mission (a) and corresponding (b) for (solid yellow line), (dotted purple line), (solid blue line).
Fig. 7: Example of path following mission (a) and corresponding distance from path (b) for (solid yellow line), (dotted purple line), (solid blue line).

In this section, we illustrate simulation results relative to two scenarios. In the first one, the vehicle must reach a single goal location; instead, in the second one it must follow a predefined path. These results were obtained simulating the kinematic model of a vehicle moving in a cluttered environment with obstacles having irregular shapes. The vehicle was assumed to travel with a linear speed and a minimum turning radius .

In the examples illustrated in Fig. 6 and Fig. 7, the feedback motion plan was based on a discrete vector field having a resolution of . In both examples, we compare the resulting trajectories for different values of used in the Gaussian filter. In Fig. 5(a), there are paths obtained with three different values of , while Fig. 5(b) shows the vehicle’s orientation for the three cases. As expected, the increment of the parameter leads to smoother transitions in the vehicle’s heading angle. In fact, with higher we obtain a smoother vector field.

For the case of path following, Fig. 6(a) illustrates the resulting paths, while the chart in Fig. 6(b) shows the distance of the vehicle from the predefined path that must be followed. We can notice that with greater values of there are higher deviations from the predefined path. This is also expected because the Gaussian filter removes high-frequency components limiting changes in the vector field. Therefore, sharp corners in the path are smoothed.

Fig. 8: Heading angle in single goal mission (a) and distance error in a path following mission (b) for (solid green line), (dotted red line), (solid purple line).

While in Fig. 6 and Fig. 7 we compared the cases of different values of , in Fig. 8 we compare the cases obtained with different resolutions of the grid-map. In particular, in this case, the resolutions of , , and were tested for both the single goal mission and the path following mission illustrated in Fig. 5(a) and Fig. 6(a). In this case, in order to have the same level of ”smoothness”, we had to use a value of sigma proportional to the grid resolution. Therefore, since for we used , for the cases of and , we used and , respectively . As expected, with the resolution of we obtain the least smooth path in the single goal mission and the highest average error in the path following. However, it is interesting to notice that while the average error is higher with the resolution, the spikes obtained at the corners are the same as those obtained with and .

The examples discussed above illustrate the effectiveness of the method. Nevertheless, they also indicate that further research needs to be conducted on this method to define the criteria for optimal tuning of the parameters. In particular, we have seen that the same parameter needs to be tuned differently based on the type of mission. In fact, in the first case, a higher is desirable to limit chattering. On the other hand, for a path following mission, a lower is preferable for more precise tracking of the predefined path. Finally, the tuning of the grid-map resolution versus the quality of the solution is another relationship that must be investigated. In fact, it is always desirable to use the minimum number of cells in order to reduce the computational time. Nevertheless, the number of cells must be high enough to have an acceptable solution.

Vi Conclusion

In this paper, we formulated a feedback motion plan based on the extension of the wavefront expansion to the case of bounded curvature constraints. The approach can be used for both single goal missions and path following missions in the presence of obstacles having arbitrary shape.

The effectiveness of the method was demonstrated using simulated examples. The obtained results encourage us to focus our future work on the derivation of optimal tuning parameters accounting for both vehicle kinematics and environmental constraints. For instance, in the examples provided in this paper, we could see that the tuning of in the Gaussian filter must be different for the case of single goal mission and path following.


  • [1] J. Barraquand, B. Langlois, and J. Latombe (1992) Numerical potential field techniques for robot path planning. IEEE transactions on systems, man, and cybernetics 22 (2), pp. 224–241. External Links: ISSN 0018-9472 Cited by: §III.
  • [2] H. Choset, K. M. Lynch, S. Hutchinson, G. Kantor, W. Burgard, L. E. Kavraki, and S. Thrun (2005) Principles of robot motion: theory, algorithms, and implementations. 2005. MITPress, Boston, pp. 77–105. Cited by: §I, §II, §II, §III, §IV.
  • [3] D. C. Conner, A. A. Rizzi, and H. Choset (2003) Composition of local potential functions for global robot control and navigation. In Intelligent Robots and Systems, 2003.(IROS 2003). Proceedings. 2003 IEEE/RSJ International Conference on, Vol. 4, pp. 3546–3551. External Links: ISBN 0780378601 Cited by: §II.
  • [4] 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: §III.
  • [5] A. F. Filippov (2013) Differential equations with discontinuous righthand sides: control systems. Vol. 18, Springer Science & Business Media. External Links: ISBN 9401577935 Cited by: §IV.
  • [6] V. M. Gonçalves, 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: ISSN 1552-3098 Cited by: §II, §II.
  • [7] L. R. Hook, M. Clark, D. Sizoo, M. A. Skoog, and J. Brady (2016) Certification strategies using run-time safety assurance for part 23 autopilot systems. In Aerospace Conference, 2016 IEEE, pp. 1–10. External Links: ISBN 1467376760 Cited by: §I.
  • [8] W. Jung, S. Lim, D. Lee, and H. Bang (2016) Unmanned aircraft vector field path following with arrival angle control. Journal of Intelligent & Robotic Systems 84 (1-4), pp. 311–325. External Links: ISSN 0921-0296 Cited by: §II.
  • [9] J. Kim and P. K. Khosla (1992) Real-time obstacle avoidance using harmonic potential functions. IEEE Transactions on Robotics and Automation 8 (3), pp. 338–349. External Links: ISSN 1042-296X Cited by: §II.
  • [10] Y. Koren and J. Borenstein (1991) Potential field methods and their inherent limitations for mobile robot navigation. In Robotics and Automation, 1991. Proceedings., 1991 IEEE International Conference on, pp. 1398–1404. External Links: ISBN 081862163X Cited by: §II.
  • [11] S. M. LaValle and J. J. Kuffner Jr (2001) Randomized kinodynamic planning. The international journal of robotics research 20 (5), pp. 378–400. External Links: ISSN 0278-3649 Cited by: §III.
  • [12] S. M. LaValle (2006) Planning algorithms. Cambridge university press. External Links: ISBN 1139455176 Cited by: §I, §II.
  • [13] S. R. Lindemann and S. M. LaValle (2005) Smoothly blending vector fields for global robot navigation. In Decision and Control, 2005 and 2005 European Control Conference. CDC-ECC’05. 44th IEEE Conference on, pp. 3553–3559. External Links: ISBN 0780395670 Cited by: §II.
  • [14] G. Miraglia and L. Hook (2017) Dynamic geo-fence assurance and recovery for nonholonomic autonomous aerial vehicles. In 2017 IEEE/AIAA 36th Digital Avionics Systems Conference (DASC), pp. 1–7. External Links: Document, ISBN VO - Cited by: §II.
  • [15] G. Miraglia and I. Loyd Hook (2019-10) Feedback motion plan verification 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.
  • [16] D. R. Nelson, D. B. Barber, T. W. McLain, and R. W. Beard (2007) Vector field path following for miniature air vehicles. IEEE Transactions on Robotics 23 (3), pp. 519–529. External Links: ISSN 1552-3098 Cited by: §II.
  • [17] K. Pathak and S. K. Agrawal (2005) An integrated path-planning and control approach for nonholonomic unicycles using switched local potentials. IEEE Transactions on Robotics 21 (6), pp. 1201–1208. External Links: Document, ISSN 15523098 Cited by: §II.
  • [18] C. Petres, Y. Pailhas, P. Patron, Y. Petillot, J. Evans, and D. Lane (2007) Path planning for autonomous underwater vehicles. IEEE Transactions on Robotics 23 (2), pp. 331–341. External Links: ISSN 1552-3098 Cited by: §III.
  • [19] J. Ren, K. A. McIsaac, and R. V. Patel (2008) Modified Newton’s method applied to potential field-based navigation for nonholonomic robots in dynamic environments. Robotica 26 (1), pp. 117–127. External Links: Document, ISSN 0263-5747 Cited by: §II.
  • [20] J. Ren, K. A. McIsaac, and R. V. Patel (2006) Modified Newton’s method applied to potential field-based navigation for mobile robots. IEEE Transactions on Robotics 22 (2), pp. 384–391. External Links: ISSN 1552-3098 Cited by: §II.
  • [21] R. Szeliski (2010) Computer vision: algorithms and applications. Springer Science & Business Media. External Links: ISBN 1848829353 Cited by: §IV.
  • [22] B. Triggs (1993) Motion planning for nonholonomic vehicles: An introduction. Cited by: §II.