This paper presents an efficient optimization framework that solves trajectory optimization problems efficiently by decoupling state variables from timing variables, thereby decomposing a challenging nonlinear programming (NLP) problem into two easier subproblems. With timing fixed, the state variables can be optimized efficiently using convex optimization, and so the time variables can be optimized using a separate outer optimization. This is a bilevel optimization in which the outer objective function itself requires an optimization to compute. The challenge is that gradient optimization methods require the gradient of the objective function with respect to the time variables, which is not available. Whereas the finite difference method must solve many optimization problems to compute a gradient, this paper proposes a more efficient method: the dual solution (Lagrange multipliers) of the convex optimization problem is exploited to calculate the analytical gradient. Since the dual solution is a by-product of the convex optimization problem, the gradient can be obtained for free' with high accuracy. The framework is demonstrated on solving minimum-jerk trajectory optimization problems in safety corridors for unmanned aerial vehicles (UAVs). Experiments demonstrate that bilevel optimization improves performance over a standard NLP solver, and analytical gradients outperforms finite differences. With a 40 ,ms cutoff time, our approach achieves over 8 times better suboptimality than the current state-of-the-art.
Despite intensive research, it is still challenging to find an optimal time allocation scheme of a spline trajectory in real-time. One strategy is to generate a time allocation scheme with heuristics @cite_0 , @cite_19 and stick with the scheme during the optimization stage. For example, Gao @cite_0 use the heuristic of selecting velocity according to the distance to the nearest obstacle, closer the trajectory is to the obstacle, lower the velocity. Though the heuristic is reasonably chosen, the velocity of the trajectory after the optimization stage does not necessarily follow the heuristic. Heuristics, though being cheap to compute, are often not optimal and will sometimes lead to spikes in jerk or snap near connection points, see Fig. for an example.
@cite_0 @cite_19
2068190491 2028186501 2100512461 2335306738
We discuss the problem of minimum cost trajectory planning for robotic manipulators. It consists of linking two points in the operational space while minimizing a cost function, taking into account dynamic equations of motion as well as bounds on joint positions, velocities, jerks and torques. This generic optimal control problem is transformed, via a clamped cubic spline model of joint temporal evolutions, into a non-linear constrained optimization problem which is treated then by the Sequential Quadratic Programming (SQP) method. Applications involving grasping mobile object or obstacle avoidance are shown to illustrate the efficiency of the proposed planner. Abstract The shortest path problem is of great importance to the real world in such areas as transportation, network design, telecommunication, etc. The deterministic version of the problem is easily solved. However, in the real world, uncertainty is frequently encountered and must be dealt with. For instance, in a transportation network, the path that has the least travel time when no traffic is present might be prone to accidents and traffic jams, and thus it may lead to a drastic increase in travel time during rush hours. In situations with significant uncertainty, the deterministic approach can be far from sufficient. New and appropriate criteria and models to handle uncertainties along with efficient solution techniques are in need. In this paper, we take a minimax approach to measure the performance under uncertainties. Under our measure, one path is superior to all others only if its worst case performance is better than that of the others. The complexity of the defined models, exact and heuristic solution methods are explored and presented. Analysis is done to examine the performance of the heuristic method. The shortest path (SP) problem in a network with nonnegative arc lengths can be solved easily by Dijkstra's labeling algorithm in polynomial time. In the case of significant uncertainty of the arc lengths, a robustness approach is more appropriate. In this paper, we study the SP problem under arc length uncertainties. A scenario approach is adopted to characterize uncertainties. Two robustness criteria are specified: the absolute robust criterion and the robust deviation criterion. We show that under both criteria the robust SP problem is NP-complete even for the much more restricted layered networks of width 2, and with only 2 scenarios. A pseudo-polynomial algorithm is devised to solve the robust SP problem in general networks under bounded number of scenarios. Also presented is a more efficient algorithm for layered networks. However, in the case of unlimited number of scenarios, we show that the robust SP problem is strongly NP-hard. A simple heuristic for finding a good robust shortest path is provided, and the worst case performance is analyzed. This paper presents a new approach to trajectory optimization for autonomous fixed-wing aerial vehicles performing large-scale maneuvers. The main result is a planner which designs nearly minimum time planar trajectories to a goal, constrained by no-fly zones and the vehicle's maximum speed and turning rate. Mixed-Integer Linear Programming (MILP) is used for the optimization, and is well suited to trajectory optimization because it can incorporate logical constraints, such as no-fly zone avoidance, and continuous constraints, such as aircraft dynamics. MILP is applied over a receding planning horizon to reduce the computational effort of the planner and to incorporate feedback. In this approach, MILP is used to plan short trajectories that extend towards the goal, but do not necessarily reach it. The cost function accounts for decisions beyond the planning horizon by estimating the time to reach the goal from the plan's end point. This time is estimated by searching a graph representation of the environment. This approach is shown to avoid entrapment behind obstacles, to yield near-optimal performance when comparison with the minimum arrival time found using a fixed horizon controller is possible, and to work consistently on large trajectory optimization problems that are intractable for the fixed horizon controller. The problem of maneuvering a vehicle through a race course in minimum time requires computation of both longitudinal (brake and throttle) and lateral (steering wheel) control inputs. Unfortunately, solving the resulting nonlinear optimal control problem is typically computationally expensive and infeasible for real-time trajectory planning. This paper presents an iterative algorithm that divides the path generation task into two sequential subproblems that are significantly easier to solve. Given an initial path through the race track, the algorithm runs a forward-backward integration scheme to determine the minimum-time longitudinal speed profile, subject to tire friction constraints. With this fixed speed profile, the algorithm updates the vehicle's path by solving a convex optimization problem that minimizes the resulting path curvature while staying within track boundaries and obeying affine, time-varying vehicle dynamics constraints. This two-step process is repeated iteratively until the predicted lap time no longer improves. While providing no guarantees of convergence or a globally optimal solution, the approach performs very well when validated on the Thunderhill Raceway course in Willows, CA. The predicted lap time converges after four to five iterations, with each iteration over the full 4.5 km race course requiring only thirty seconds of computation time on a laptop computer. The resulting trajectory is experimentally driven at the race circuit with an autonomous Audi TTS test vehicle, and the resulting lap time and racing line is comparable to both a nonlinear gradient descent solution and a trajectory recorded from a professional racecar driver. The experimental results indicate that the proposed method is a viable option for online trajectory planning in the near future.