Stochastic Trajectory Optimization Research Articles

Stochastic trajectory optimization for 6-DOF spacecraft autonomous rendezvous and docking with nonlinear chance constraints

This paper addresses the problem of stochastic trajectory planning for 6-DOF spacecraft close proximity in the presence of external disturbances and initial state uncertainties. Nonlinear chance constraints on collision avoidance, docking corridor, sensor field-of-view, and control magnitude are considered in the corresponding optimal control problem. Nominal trajectory along with affine feedback controller that restricts the dispersion caused by disturbances are optimized simultaneously. The nonlinear stochastic dynamics are transformed into equality constraints on the first two statistical moments of the random states by linearization and discretization, and further, the non-convex covariance update equation is relaxed to a semidefinite cone constraint. The nonlinear path chance constraints are first approximated by affine chance formations, and then are convexified by introducing auxiliary variables instead of direct linearization of the covariance matrix. Combined with the tail bound of sub-exponential random variables, the constraints on control input are replaced with conservative deterministic inequalities. The sub-optimal solution to the original problem is obtained by iteratively solving a series of semidefinite cone programs. The effectiveness of the proposed framework is verified using numerical simulations.

An online collision-free trajectory generation algorithm for human–robot collaboration

The premise of human–robot collaboration is that robots have adaptive trajectory planning strategies in hybrid work cell. The aim of this paper is to propose a new online collision avoidance trajectory planning algorithm for moderate dynamic environments to insure human safety when sharing collaborative tasks. The algorithm contains two parts: trajectory generation and local optimization. Firstly, based on empirical Dirichlet Process Gaussian Mixture Model (DPGMM) distribution learning, a neural network trajectory planner called Collaborative Waypoint Planning network (CWP-net) is proposed to generate all key waypoints required for dynamic obstacle avoidance in joint space according to environmental inputs. These points are used to generate quintic spline smooth motion trajectories with velocity and acceleration constraints. Secondly, we present an improved Stochastic Trajectory Optimization for Motion Planning (STOMP) algorithm which locally optimizes the generated trajectories of CWP-net by constraining the trajectory optimization range and direction through the DPGMM model. Simulations and real experiments from an industrial use case of human–robot collaboration in the field of aircraft assembly testing show that the proposed algorithm can smoothly adjust the nominal path online and effectively avoid collisions during the collaboration. • An online trajectory planning algorithm for dynamic obstacles avoidance is proposed. • A neural network CWP-net with multiple collision-free waypoints output is designed. • An improved STOMP algorithm is proposed for local trajectory optimization. • Continuous smooth trajectories are generated and satisfy joint constraints. • This work builds a bridge between advanced offline planners and online planning tasks.

Flexible Final-Time Stochastic Differential Dynamic Programming for Autonomous Vehicle Trajectory Optimization

In this paper, the problem of autonomous vehicle trajectory optimization with flexible final time is concerned under the consideration of stochastic disturbances. Stochastic differential dynamic programming (SDDP) has been widely applied to address this type of problem due to its fast convergence and capability to handle model errors. Typically, traditional SDDP is designed with a deterministic final time which is mainly assigned based on expert experiences. However, this may limit the implementation of this approach. To deal with this issue, we define the final time as a new optimal variable and propose an enhanced version of SDDP, named flexible final time SDDP (FFT-SDDP). The stochastic dynamic system is then characterized as a deterministic one with random state perturbances. An unscented transform approach is utilised to cope with the perturbed expected values. To verify the effectiveness of the proposed approach, a three-dimensional missile trajectory optimization problem is tested as an example. The simulation results show that the proposed method is able to address the stochastic trajectory optimization problem and can provide stronger robustness compared to other algorithms.

Mixtures of Gaussian Processes for Robot Motion Planning Using Stochastic Trajectory Optimization

Robot motion planning methods based on trajectory optimization can efficiently generate feasible and optimal trajectories by minimizing a suitable cost function, even in high-dimensional spaces. However, the main drawback of these methods lies in their proneness to infeasible local minima, especially in complex environments. To mitigate this issue, we propose a novel motion planning method that represents trajectories as samples from a mixture of continuous-time Gaussian processes (MGP) and employs stochastic optimization in order to update the MGP parameters in a cost-minimizing manner. The contributions of the proposed trajectory optimization method arise from the introduced mixture representation and stochastic gradient estimation, dominantly enabling better exploration of the trajectory space and including nondifferentiable optimizing costs. We evaluated the proposed method in multiple simulation benchmarks featuring 7 degree-of-freedom (DOF) robot arms and a 10DOF mobile manipulator. We also conducted a real-world experiment with a 14DOF dual-arm robot. The experimental results demonstrated that the proposed method achieves higher success rate than several state-of-the-art methods, while the advantages stemming from MGPs and stochastic optimization, like trajectory smoothness, support of nondifferentiable cost functions, multiple trajectory solutions, and the ability to tackle high-dimensional planning problems, are inherently kept.

Biomechanical Trajectory Optimization of Human Sit-to-Stand Motion With Stochastic Motion Planning Framework

Trajectory optimization has been an important approach in biomechanics for the analysis and prediction of the limb movement. Such approaches have paved the way for the motion planning of biped and quadruped robots as well. Most of these methods are deterministic, utilizing first-order iterative gradient-based algorithms incorporating the constrained differentiable objective functions. However, the limitation of prevailing methods concerning differentiability hinders the implementation of non-differentiable objective functions such as metabolic energy expenditure (MEE) function, which is highly relevant for physiological systems and can even be implemented across the muscular space. This paper consolidates the implementation of the prevalent direct collocation-based optimal control method with the stochastic trajectory optimization method based on Policy Improvement with Path Integral (PI2) for comprehending the human sit-to-stand (STS) motion. PI2 method, which utilizes reinforcement learning of Dynamic Movement Primitive (DMP) to learn a goal-based trajectory is implemented and validated by comparing with the experimental result in joint-space and muscle-space.

Convex Approach to Covariance Control with Application to Stochastic Low-Thrust Trajectory Optimization

This paper outlines a novel approach to the design of optimal space trajectories under significant uncertainty. Finite-horizon covariance control, i.e., the steering of a system from an initial probability distribution to a desired one at a prescribed time, is employed to plan an optimal nominal path along with a robust feedback controller that compensates for exogenous in-flight disturbances. A mindful convexification strategy is devised to recast the nonlinear covariance control problem as a deterministic convex optimization problem. The convexification is based on a convenient change of variables that allows for relaxing the covariance matrix discrete-time propagation into a set of semidefinite cone constraints. While featuring a larger feasible space, the relaxed problem shares the same optimal solution as the original one. The lossless property of the relaxation is analytically proven by leveraging optimal control theory and discussed on the basis of numerical experiments. Monte Carlo campaigns are carried out to validate the in-flight performance of the attained control policies.

Stochastic Trajectory Optimization of a Swarm of Spacecraft Exploring the Rings of Saturn

This article presents outcomes of a stochastic optimization technique for trajectories of spacecraft exploring the rings of Saturn. Despite data obtained by Cassini and Voyager on their missions to Saturn, many questions regarding the formation and dynamics of Saturn's rings and planetary rings in general remain unanswered. Sending traditional large spacecraft prove to be problematic as collisions and the probability of mission failure increase dramatically in the dynamically dense region of space within planetary rings. Because of this, a swarm of small unmanned probes with decentralized communication structures and research instruments distributed amongst the swarm can provide a possible solution. In this paper, the practicality of such a swarm mission is investigated through a stochastic model of the mission trajectory dynamics with the goal of minimizing fuel consumption across the entire swarm while avoiding collisions between swarm agents and dynamic rigid body ring particles. Known probability distributions of ring particle dynamical variables (velocity, rotation rate, size) are used to define random variable parameters in a system of differential equations defined by artificial potential fields describing the swarm dynamics. Using a Monte Carlo method, the stochastic optimization problem is solved yielding probabilities of various possible swarm trajectories with the lowest fuel consumption and probability of mission completion without collisions. These optimal parameters can then be onboarded to a swarm of spacecraft allowing them to explore the rings of Saturn following optimal trajectories.

Improving Local Trajectory Optimization by Enhanced Initialization and Global Guidance

Trajectory optimization is a promising method for planning trajectories of robotic manipulators. With the increasing success of collaborative robots in dynamic environments, the demand for online planning methods grows and offers new opportunities as well as challenges for trajectory optimization. Special requirements in terms of real-time capabilities are one of the greatest difficulties. Optimizing a short planning horizon instead of an entire trajectory is one approach to reduce computation time, which nonetheless separates the optimality of local and global solutions. This contribution introduces, on the one hand, Extended Initialization as a new approach that reduces the risk of local minima and aims at improving the quality of the global trajectory. On the other hand, the particularly critical cases in which local solutions lead to standstills are mitigated by globally guiding local solutions. The evaluation performs four experiments with comparisons to Stochastic Trajectory Optimization for Motion Planning (STOMP) or Probabilistic Roadmap Method (PRM*) and demonstrates the effectiveness of both approaches.

Cartesian Constrained Stochastic Trajectory Optimization for Motion Planning

This paper presents novel extensions of the Stochastic Optimization Motion Planning (STOMP), which considers cartesian path constraints. It potentially has high usage in many autonomous applications with robotic arms, where preservation or minimization of tool-point rotation is required. The original STOMP algorithm is unable to use the cartesian path constraints in a trajectory generation because it works only in robot joint space. Therefore, the designed solution, described in this paper, extends the most important parts of the algorithm to take into account cartesian constraints. The new sampling noise generator generates trajectory samples in cartesian space, while the new cost function evaluates them and minimizes traversed distance and rotation change of the tool-point in the resulting trajectory. These improvements are verified with simple experiments and the solution is compared with the original STOMP. Results of the experiments show that the implementation satisfies the cartesian constraints requirements.

Optimizing Mobility of Robotic Arms in Collision-free Motion Planning

A major task in motion planning is to find paths that have a high ability to react to external influences while ensuring a collision-free operation at any time. This flexibility is even more important in human-robot collaboration since unforeseen events can occur anytime. Such ability can be described as mobility, which is composed of two characteristics. First, the ability to manipulate, and second, the distance to joint limits. This mobility needs to be optimized while generating collision-free motions so that there is always the flexibility of the robot to evade dynamic obstacles in the future execution of generated paths. For this purpose, we present a Rapidly-exploring Random Tree (RRT), which applies additional costs and sampling methods to increase mobility. Additionally, we present two methods for the optimization of a generated path. Our first approach utilizes the built-in capabilities of the RRT*. The second method optimize the path with the stochastic trajectory optimization for motion planning (STOMP) approach with Gaussian Mixture Models. Moreover, we evaluate the algorithms in complex simulation and real environments and demonstrate an enhancement of mobility.

Identification of the human postural control system through stochastic trajectory optimization

BackgroundSystem identification can be used to obtain a model of the human postural control system from experimental data in which subjects are mechanically perturbed while standing. However, unstable controllers were sometimes found, which obviously do not explain human balance and cannot be applied in control of humanoid robots. Eigenvalue constraints can be used to avoid unstable controllers. However, this method is hard to apply to highly nonlinear systems and large identification datasets. New methodTo address these issues, we perform the system identification with a stochastic system model where process noise is modeled. The parameter identification is performed by simultaneous trajectory optimizations on multiple episodes that have different instances of the process noise. ResultsThe stochastic and deterministic identification methods were tested on three types of controllers, including both linear and nonlinear controller architectures. Stochastic identification tracked the experimental data nearly as well as the deterministic identification, while avoiding the unstable controllers that were found with a deterministic system model. Comparison with existing methodComparing to eigenvalue constraints, stochastic identification has wider application potentials. Since linearization is not needed in the stochastic identification, it is applicable to highly nonlinear systems, and it can be applied on large data-sets. ConclusionsStochastic identification can be used to avoid unstable controllers in human postural control identification.

Robust Trajectory Design for Rendezvous and Proximity Operations with Uncertainties

A robust trajectory optimization approach for rendezvous and proximity operations in perturbed elliptical orbits with uncertainties is proposed. For robust trajectory design, a new discrete-time linear dynamics model describing the relative equations of motion for powered flight in perturbed elliptical orbits is developed. The linear dynamics model is used to formulate a stochastic trajectory optimization problem that takes into account navigation sensor errors, maneuver execution errors, and trajectory and control dispersion. The objective of the stochastic optimization problem is to minimize the sum of the expected maneuver and the maneuver dispersion subject to a constraint on the final state dispersion. A genetic algorithm is used to solve the stochastic trajectory optimization problem, and a series of examples demonstrate that the stochastic maneuver solution can be significantly better than the solution corresponding deterministic trajectory optimization problem. Nonlinear Monte Carlo simulations are used to validate the results.

Stochastic Spacecraft Trajectory Optimization With the Consideration of Chance Constraints

This brief investigates a computational framework based on optimal control for addressing the problem of stochastic trajectory optimization with the consideration of chance constraints. This design employs a discretization technique to parameterize uncertain variables and create the trajectory ensemble. Subsequently, the resulting discretized version of the problem is solved by applying standard optimal control solvers. In order to provide reliable gradient information to the optimization algorithm, a smooth and differentiable chance-constraint approximation method is proposed to replace the original probability constraints. The established methodology is implemented to explore the optimal trajectories for a spacecraft entry flight planning scenario with noise-perturbed dynamics and probabilistic constraints. Simulation results and comparative studies demonstrate that the present chance-constraint handling strategy can outperform other existing approaches analyzed in this brief, and this computational framework can produce reliable and less conservative solutions for the chance-constrained stochastic spacecraft trajectory planning problem.

Stochastic Trajectory Optimization for Autonomous Soaring of UAV

Thermal soaring gliders exploit naturally occurring rising air (called thermals) as sources of energy. These sources generally appear in uncertain and random way as their position and strength are hardly predictable. Several atmospheric factors affect the behavior of thermals (season, time of the day and nature of vegetation on the ground, etc). However, a statistical model of thermals to be encountered can be build. Soaring in a optimal manner while taking advantage of thermals represents thus a stochastic optimization problem that can be solved and implemented in an autonomous glider. The present paper details how to find an optimal trajectory for going through given checkpoints, with the objective of doing as much rounds as possible within a predetermined time. The optimal control of the glider is determined by determining i) whether the glider should climb a thermal with a specific strength when it encounters one, or to cruise, ii) the optimal speed to fly during the cruising phases. The problem is solved using dynamic programming and the proposed optimal path planning strategy is validated by simulation.

Coverage Path Planning with Real-time Replanning and Surface Reconstruction for Inspection of Three-dimensional Underwater Structures using Autonomous Underwater Vehicles

We present a novel method for planning coverage paths for inspecting complex structures on the ocean floor using an autonomous underwater vehicle AUV. Our method initially uses a 2.5-dimensional 2.5D prior bathymetric map to plan a nominal coverage path that allows the AUV to pass its sensors over all points on the target area. The nominal path uses a standard mowing-the-lawn pattern in effectively planar regions, while in regions with substantial 3D relief it follows horizontal contours of the terrain at a given offset distance. We then go beyond previous approaches in the literature by considering the vehicle's state uncertainty rather than relying on the unrealistic assumption of an idealized path execution. Toward that end, we present a replanning algorithm based on a stochastic trajectory optimization that reshapes the nominal path to cope with the actual target structure perceived in situ. The replanning algorithm runs onboard the AUV in real time during the inspection mission, adapting the path according to the measurements provided by the vehicle's range-sensing sonars. Furthermore, we propose a pipeline of state-of-the-art surface reconstruction techniques we apply to the data acquired by the AUV to obtain 3D models of the inspected structures that show the benefits of our planning method for 3D mapping. We demonstrate the efficacy of our method in experiments at sea using the GIRONA 500 AUV, where we cover part of a breakwater structure in a harbor and an underwater boulder rising from 40i¾?m up to 27i¾?m depth.

ITOMP: Incremental Trajectory Optimization for Real-Time Replanning in Dynamic Environments

We present a novel optimization-based algorithm for motion planning in dynamic environments. Our approach uses a stochastic trajectory optimization framework to avoid collisions and satisfy smoothness and dynamics constraints. Our algorithm does not require a priori knowledge about global motion or trajectories of dynamic obstacles. Rather, we compute a conservative local bound on the position or trajectory of each obstacle over a short time and use the bound to compute a collision-free trajectory for the robot in an incremental manner. Moreover, we interleave planning and execution of the robot in an adaptive manner to balance between the planning horizon and responsiveness to obstacle. We highlight the performance of our planner in a simulated dynamic environment with the 7-DOF PR2 robot arm and dynamic obstacles.