An official website of the United States government
This paper presents an integrated motion planning system for autonomous vehicle (AV) parking in the presence of other moving vehicles. The proposed system includes 1) a hybrid environment predictor that predicts the motions of the surrounding vehicles and 2) a strategic motion planner that reacts to the predictions. The hybrid environment predictor performs short-term predictions via an extended Kalman filter and an adaptive observer. It also combines short-term predictions with a driver behavior cost-map to make long-term predictions. The strategic motion planner comprises 1) a model predictive control-based safety controller for trajectory tracking; 2) a search-based retreating planner for finding an evasion path in an emergency; 3) an optimization-based repairing planner for planning a new path when the original path is invalidated. Simulation validation demonstrates the effectiveness of the proposed method in terms of initial planning, motion prediction, safe tracking, retreating in an emergency, and trajectory repairing.
more »
« less
Efficient Robot Motion Planning via Sampling and Optimization
Robot motion planning is one of the important elements in robotics. In environments full of obstacles, it is always challenging to find a collision-free and dynamically feasible path between the robot's initial configuration and goal configuration. While many motion planning algorithms have been proposed in the past, each of them has its pros and cons. This work presents a benchmark which implements and compares existing planning algorithms on a variety of problems with extensive simulation. Based on that, we also propose a hybrid planning algorithm, RRT*-CFS, that combines the merits of sampling-based planning methods and optimization-based planning methods. The first layer, RRT*, quickly samples a semi-optimal path. The second layer, CFS, performs sequential convex optimization given the reference path from RRT*. The proposed RRT*-CFS has feasibility and convergence guarantees. Simulation results show that RRT*-CFS benefits from the hybrid structure and performs robustly in various scenarios including the narrow passage problems.
more »
« less
- Award ID(s):
- 1734109
- PAR ID:
- 10287139
- Date Published:
- Journal Name:
- 2021 American Control Conference (ACC)
- Page Range / eLocation ID:
- 4196 to 4202
- Format(s):
- Medium: X
- Sponsoring Org:
- National Science Foundation
More Like this
-
-
Rapidly-exploring Random Trees (RRT) and its variations have emerged as a robust and efficient tool for finding collision-free paths in robotic systems. However, adding dynamic constraints makes the motion planning problem significantly harder, as it requires solving two-value boundary problems (computationally expensive) or propagating random control inputs (uninformative). Alternatively, Iterative Discontinuity Bounded A* (iDb-A*), introduced in our previous study, combines search and optimization iteratively. The search step connects short trajectories (motion primitives) while allowing a bounded discontinuity between the motion primitives, which is later repaired in the trajectory optimization step.Building upon these foundations, in this paper, we present iDb-RRT, a sampling-based kinodynamic motion planning algorithm that combines motion primitives and trajectory optimization within the RRT framework. iDb-RRT is probabilistically complete and can be implemented in forward or bidirectional mode. We have tested our algorithm across a benchmark suite comprising 30 problems, spanning 8 different systems, and shown that iDb-RRT can find solutions up to 10x faster than previous methods, especially in complex scenarios that require long trajectories or involve navigating through narrow passages.more » « less
-
Safe path planning is critical for bipedal robots to operate in safety-critical environments. Common path planning algorithms, such as RRT or RRT*, typically use geometric or kinematic collision check algorithms to ensure collision-free paths toward the target position. However, such approaches may generate non-smooth paths that do not comply with the dynamics constraints of walking robots. It has been shown that the control barrier function (CBF) can be integrated with RRT/RRT* to synthesize dynamically feasible collision-free paths. Yet, existing work has been limited to simple circular or elliptical shape obstacles due to the challenging nature of constructing appropriate barrier functions to represent irregularly shaped obstacles. In this paper, we present a CBF-based RRT* algorithm for bipedal robots to generate a collision-free path through space with multiple polynomial-shaped obstacles. In particular, we used logistic regression to construct polynomial barrier functions from a grid map of the environment to represent irregularly shaped obstacles. Moreover, we developed a multi-step CBF steering controller to ensure the efficiency of free space exploration. The proposed approach was first validated in simulation for a differential drive model, and then experimentally evaluated with a 3D humanoid robot, Digit, in a lab setting with randomly placed obstacles.more » « less
-
This paper reports a novel result: with proper robot models based on geometric mechanics, one can formulate the kinodynamic motion planning problems for rigid body systems as exact polynomial optimization problems. Due to the nonlinear rigid body dynamics, the motion planning problem for rigid body systems is nonconvex. Existing global optimization-based methods do not parameterize 3D rigid body motion efficiently; thus, they do not scale well to long-horizon planning problems. We use Lie groups as the configuration space and apply the variational integrator to formulate the forced rigid body dynamics as quadratic polynomials. Then, we leverage Lasserre’s hierarchy of moment relaxation to obtain the globally optimal solution via semidefinite programming. By leveraging the sparsity of the motion planning problem, the proposed algorithm has linear complexity with respect to the planning horizon. This paper demonstrates that the proposed method can provide globally optimal solutions or certificates of infeasibility at the second-order relaxation for 3D drone landing using full dynamics and inverse kinematics for serial manipulators. Moreover, we extend the algorithms to multi-body systems via the constrained variational integrators. The testing cases on cart-pole and drone with cable-suspended load suggest that the proposed algorithms can provide rank-one optimal solutions or nontrivial initial guesses. Finally, we propose strategies to speed up the computation, including an alternative formulation using quaternion, which provides empirically tight relaxations for the drone landing problem at the first-order relaxation.more » « less
-
This paper focuses on the motion planning problem for the systems exhibiting both continuous and discrete behaviors, which we refer to as hybrid dynamical systems. First, the motion planning problem for hybrid systems is formulated using the hybrid equation framework, which is general to capture most hybrid systems. Second, a propagation algorithm template is proposed that describes a general framework to solve the motion planning problem for hybrid systems. Third, a rapidly-exploring random trees (RRT) implementation of the proposed algorithm template is designed to solve the motion planning problem for hybrid systems. At each iteration, the proposed algorithm, called HyRRT, randomly picks a state sample and extends the search tree by flow or jump, which is also chosen randomly when both regimes are possible. Through a definition of concatenation of functions defined on hybrid time domains, we show that HyRRT is probabilistically complete, namely, the probability of failing to find a motion plan approaches zero as the number of iterations of the algorithm increases. This property is guaranteed under mild conditions on the data defining the motion plan, which include a relaxation of the usual positive clearance assumption imposed in the literature of classical systems. The motion plan is computed through the solution of two optimization problems, one associated with the flow and the other with the jumps of the system. The proposed algorithm is applied to an actuated bouncing ball system and a walking robot system so as to highlight its generality and computational features.more » « less
- Free Publicly Accessible Full Text
-
Accepted Manuscript1.0
- Conference Paper:
- https://doi.org/10.23919/ACC50511.2021.9483146
