A Successive Approximation-Based Approach for ... - Semantic Scholar
52nd IEEE Conference on Decision and Control December 10-13, 2013. Florence, Italy
A Successive Approximation-Based Approach for Optimal Kinodynamic Motion Planning with Nonlinear Differential Constraints Jung-Su Ha, Ju-Jang Lee and Han-Lim Choi Abstract— This paper presents an extension to RRT* [1], a sampling-based motion planning with asymptotic optimality guarantee, in order to incorporate nonlinear differential equations in motion dynamics. The main challenge due to nonlinear differential constraints is the computational complexity of solving a two-point boundary-value problem that arises in the tree expansion process to optimally connect two given states. This work adapts the successive approximation method that transforms a nonlinear optimal control problem into a sequence of linear-quadratic-like problems to solve these TPBVPs. The resulting algorithm, termed SA-RRT*, is demonstrated to create more realistic plans compared to existing kinodynamic extensions of RRT*, while preserving asymptotic optimality.
I. I NTRODUCTION Sampling-based algorithms [2]–[5] have been studied and implemented for robot motion planning, taking advantage of their compactness that allows for handling high dimensional confuguration space. The Rapidly-exploring Random Tree (RRT) [4] and its variants [6]–[8], which build a trajectory from initial state to goal state by sampling the state randomly and growing a tree toward that state, is one of the most popular sampling-based motion planners. The RRT framework is flexible and the algorithm exhibits good scalability so that a variant allows for planning with 1000+ dimensions [9]. In addition to the scalability, the RRT algorithm is probabilistic complete [1], which means that if a feasible trajectory exists, the probability that the algorithm fails to find said feasible trajectory decreases to zero as the number of iterations goes to infinity. The RRT methodology has also successfully been extended/applied to problems with differential constraints [10]–[13], which are called kinodynamic motion planning problems. However, the RRT framework has only focused on finding a feasible trajectory, without explicit consideration of the quality of trajectory. Recently, [1] has proposed a RRT* algorithm that incrementally finds the optimal trajectory, in the RRT framework. RRT* inherits all the nice properties of RRT such as compactness and probabilistic completeness; in addition, enables building the optimal trajectory with a reasonable amount of additional computation resources. The key differentiator in the RRT* algorithm is that instead of connecting new nodes to the nearest nodes in the tree, it finds an optimal connection from the tree to the new nodes, and J.-S. Ha is with the Division of Aerospace Engineering, KAIST, Daejeon, Korea [email protected] J.-J. Lee is with the Department of Electrical Engineering, KAIST, Daejeon, Korea; is Fellow, IEEE. [email protected] H.-L. Choi is with the Division of Aerospace Engineering & the Center of Field Robotics for Innovation, Exploration, and Defense, KAIST, Daejeon, Korea [email protected]
978-1-4673-5717-3/13/$31.00 ©2013 IEEE
vice versa. This new procedure is called ‘choosing parent’ step and ‘rewiring tree’ step. However, incorporating dynamic constraints into the RRT* for kinodynamic planning is not trivial compared to the kinodynamic extentions to RRT, as in the ‘choosing parent’ and ‘rewiring tree’ steps, two nodes need to be connected exactly and optimally. The algorithm needs a piecewise optimal trajectory between two nodes, to ensure the entire trajectory becomes optimal. The baseline RRT* does not explicitly consider differential constraints and used a straight line as the piecewise trajectory; several previous approaches have tried to address this difficulty: [14] solves the shortest path problem specifically for Dubin’s car model by connecting two states with six families of canonical paths, and [15] develops a shooting method to connect the two states to allow for handling more general forms of cost functions. Despite its generality, the shooting method does not guarantee an exact connection between the two states, and may lead to a suboptimal/infeasible trajectory. [16] has proposed to solve the resulting two-point boundary-value problem (TPBVP) associated with the optimal connection between the two nodes in the RRT* process, in particular when the differential constraints are linear. With the cost function representing trade-off between the maneuver time and the energy consumption, the problem can be reduced to a standard linear-quadratic regular problem for which a closed-loop solution is available. The authors of [16] have commented that their methodology, termed Kinodynamic-RRT*, can be applied to problems with nonlinear differential constraints by linearization of the constraints. But, the trajectory obtained with this linearization may not be realistic for actual systems, as will be shown in section V. In addition, the trajectory can be arbitrarily suboptimal due to the linearization error. This work also tries to directly solve the TPBVP associated for optimal connection of the two nodes; in this sense, the approach is similar to [16]. However, a much more general class of differential equations are addressed. A successive approximation approach (SAA) [17,18], which solves nonlinear differential equations by approximating the equation successively in the form of a linear system plus a nonlinear disturbance term, is adapted to solve the TPBVPs. The asymptotic optimality property of the SAA developed for a regulator problem with free final state is first extended to the case with fixed terminal state constraints, which arises in the RRT* process; then, the successive approximationbased TPBVP solver is embedded in the RRT* procedure. Numerical case studies demonstrate that the algorithm,
3623
termed ‘SA-RRT*’, provides natural and close-to optimal trajectories for the nonlinear systems. II. P ROBLEM F ORMULATION This paper deals with kinodynamic motion planning problem that involves system dynamics. The problem is defined over a state space χ ⊂
A Successive Approximation-Based Approach for Optimal Kinodynamic Motion Planning with Nonlinear Differential Constraints Jung-Su Ha, Ju-Jang Lee and Han-Lim Choi Abstract— This paper presents an extension to RRT* [1], a sampling-based motion planning with asymptotic optimality guarantee, in order to incorporate nonlinear differential equations in motion dynamics. The main challenge due to nonlinear differential constraints is the computational complexity of solving a two-point boundary-value problem that arises in the tree expansion process to optimally connect two given states. This work adapts the successive approximation method that transforms a nonlinear optimal control problem into a sequence of linear-quadratic-like problems to solve these TPBVPs. The resulting algorithm, termed SA-RRT*, is demonstrated to create more realistic plans compared to existing kinodynamic extensions of RRT*, while preserving asymptotic optimality.
I. I NTRODUCTION Sampling-based algorithms [2]–[5] have been studied and implemented for robot motion planning, taking advantage of their compactness that allows for handling high dimensional confuguration space. The Rapidly-exploring Random Tree (RRT) [4] and its variants [6]–[8], which build a trajectory from initial state to goal state by sampling the state randomly and growing a tree toward that state, is one of the most popular sampling-based motion planners. The RRT framework is flexible and the algorithm exhibits good scalability so that a variant allows for planning with 1000+ dimensions [9]. In addition to the scalability, the RRT algorithm is probabilistic complete [1], which means that if a feasible trajectory exists, the probability that the algorithm fails to find said feasible trajectory decreases to zero as the number of iterations goes to infinity. The RRT methodology has also successfully been extended/applied to problems with differential constraints [10]–[13], which are called kinodynamic motion planning problems. However, the RRT framework has only focused on finding a feasible trajectory, without explicit consideration of the quality of trajectory. Recently, [1] has proposed a RRT* algorithm that incrementally finds the optimal trajectory, in the RRT framework. RRT* inherits all the nice properties of RRT such as compactness and probabilistic completeness; in addition, enables building the optimal trajectory with a reasonable amount of additional computation resources. The key differentiator in the RRT* algorithm is that instead of connecting new nodes to the nearest nodes in the tree, it finds an optimal connection from the tree to the new nodes, and J.-S. Ha is with the Division of Aerospace Engineering, KAIST, Daejeon, Korea [email protected] J.-J. Lee is with the Department of Electrical Engineering, KAIST, Daejeon, Korea; is Fellow, IEEE. [email protected] H.-L. Choi is with the Division of Aerospace Engineering & the Center of Field Robotics for Innovation, Exploration, and Defense, KAIST, Daejeon, Korea [email protected]
978-1-4673-5717-3/13/$31.00 ©2013 IEEE
vice versa. This new procedure is called ‘choosing parent’ step and ‘rewiring tree’ step. However, incorporating dynamic constraints into the RRT* for kinodynamic planning is not trivial compared to the kinodynamic extentions to RRT, as in the ‘choosing parent’ and ‘rewiring tree’ steps, two nodes need to be connected exactly and optimally. The algorithm needs a piecewise optimal trajectory between two nodes, to ensure the entire trajectory becomes optimal. The baseline RRT* does not explicitly consider differential constraints and used a straight line as the piecewise trajectory; several previous approaches have tried to address this difficulty: [14] solves the shortest path problem specifically for Dubin’s car model by connecting two states with six families of canonical paths, and [15] develops a shooting method to connect the two states to allow for handling more general forms of cost functions. Despite its generality, the shooting method does not guarantee an exact connection between the two states, and may lead to a suboptimal/infeasible trajectory. [16] has proposed to solve the resulting two-point boundary-value problem (TPBVP) associated with the optimal connection between the two nodes in the RRT* process, in particular when the differential constraints are linear. With the cost function representing trade-off between the maneuver time and the energy consumption, the problem can be reduced to a standard linear-quadratic regular problem for which a closed-loop solution is available. The authors of [16] have commented that their methodology, termed Kinodynamic-RRT*, can be applied to problems with nonlinear differential constraints by linearization of the constraints. But, the trajectory obtained with this linearization may not be realistic for actual systems, as will be shown in section V. In addition, the trajectory can be arbitrarily suboptimal due to the linearization error. This work also tries to directly solve the TPBVP associated for optimal connection of the two nodes; in this sense, the approach is similar to [16]. However, a much more general class of differential equations are addressed. A successive approximation approach (SAA) [17,18], which solves nonlinear differential equations by approximating the equation successively in the form of a linear system plus a nonlinear disturbance term, is adapted to solve the TPBVPs. The asymptotic optimality property of the SAA developed for a regulator problem with free final state is first extended to the case with fixed terminal state constraints, which arises in the RRT* process; then, the successive approximationbased TPBVP solver is embedded in the RRT* procedure. Numerical case studies demonstrate that the algorithm,
3623
termed ‘SA-RRT*’, provides natural and close-to optimal trajectories for the nonlinear systems. II. P ROBLEM F ORMULATION This paper deals with kinodynamic motion planning problem that involves system dynamics. The problem is defined over a state space χ ⊂
