Combining Homotopy Methods and Numerical Optimal Control to Solve Motion Planning Problems

This paper presents a systematic approach for computing local solutions to motion planning problems in non-convex environments using numerical optimal control techniques. It extends the range of use of state-of-the-art numerical optimal control tools to problem classes where these tools have previously not been applicable. Today these problems are typically solved using motion planners based on randomized or graph search. The general principle is to define a homotopy that perturbs, or preferably relaxes, the original problem to an easily solved problem. By combining a Sequential Quadratic Programming (SQP) method with a homotopy approach that gradually transforms the problem from a relaxed one to the original one, practically relevant locally optimal solutions to the motion planning problem can be computed. The approach is demonstrated in motion planning problems in challenging 2D and 3D environments, where the presented method significantly outperforms a state-of-the-art open-source optimizing sampled-based planner commonly used as benchmark

Dynamically Stable 3D Quadrupedal Walking with Multi-Domain Hybrid System Models and Virtual Constraint Controllers

Hybrid systems theory has become a powerful approach for designing feedback controllers that achieve dynamically stable bipedal locomotion, both formally and in practice. This paper presents an analytical framework 1) to address multi-domain hybrid models of quadruped robots with high degrees of freedom, and 2) to systematically design nonlinear controllers that asymptotically stabilize periodic orbits of these sophisticated models. A family of parameterized virtual constraint controllers is proposed for continuous-time domains of quadruped locomotion to regulate holonomic and nonholonomic outputs. The properties of the Poincare return map for the full-order and closed-loop hybrid system are studied to investigate the asymptotic stabilization problem of dynamic gaits. An iterative optimization algorithm involving linear and bilinear matrix inequalities is then employed to choose stabilizing virtual constraint parameters. The paper numerically evaluates the analytical results on a simulation model of an advanced 3D quadruped robot, called GR Vision 60, with 36 state variables and 12 control inputs. An optimal amble gait of the robot is designed utilizing the FROST toolkit. The power of the analytical framework is finally illustrated through designing a set of stabilizing virtual constraint controllers with 180 controller parameters.Comment: American Control Conference 201

Optimal path planning for nonholonomic robotics systems via parametric optimisation

Abstract. Motivated by the path planning problem for robotic systems this paper considers nonholonomic path planning on the Euclidean group of motions SE(n) which describes a rigid bodies path in n-dimensional Euclidean space. The problem is formulated as a constrained optimal kinematic control problem where the cost function to be minimised is a quadratic function of translational and angular velocity inputs. An application of the Maximum Principle of optimal control leads to a set of Hamiltonian vector field that define the necessary conditions for optimality and consequently the optimal velocity history of the trajectory. It is illustrated that the systems are always integrable when n = 2 and in some cases when n = 3. However, if they are not integrable in the most general form of the cost function they can be rendered integrable by considering special cases. This implies that it is possible to reduce the kinematic system to a class of curves defined analytically. If the optimal motions can be expressed analytically in closed form then the path planning problem is reduced to one of parameter optimisation where the parameters are optimised to match prescribed boundary conditions.This reduction procedure is illustrated for a simple wheeled robot with a sliding constraint and a conventional slender underwater vehicle whose velocity in the lateral directions are constrained due to viscous damping

Feedback Synthesis for Controllable Underactuated Systems using Sequential Second Order Actions

This paper derives nonlinear feedback control synthesis for general control affine systems using second-order actions---the needle variations of optimal control---as the basis for choosing each control response to the current state. A second result of the paper is that the method provably exploits the nonlinear controllability of a system by virtue of an explicit dependence of the second-order needle variation on the Lie bracket between vector fields. As a result, each control decision necessarily decreases the objective when the system is nonlinearly controllable using first-order Lie brackets. Simulation results using a differential drive cart, an underactuated kinematic vehicle in three dimensions, and an underactuated dynamic model of an underwater vehicle demonstrate that the method finds control solutions when the first-order analysis is singular. Moreover, the simulated examples demonstrate superior convergence when compared to synthesis based on first-order needle variations. Lastly, the underactuated dynamic underwater vehicle model demonstrates the convergence even in the presence of a velocity field.Comment: 9 page

A path planning and path-following control framework for a general 2-trailer with a car-like tractor

Maneuvering a general 2-trailer with a car-like tractor in backward motion is a task that requires significant skill to master and is unarguably one of the most complicated tasks a truck driver has to perform. This paper presents a path planning and path-following control solution that can be used to automatically plan and execute difficult parking and obstacle avoidance maneuvers by combining backward and forward motion. A lattice-based path planning framework is developed in order to generate kinematically feasible and collision-free paths and a path-following controller is designed to stabilize the lateral and angular path-following error states during path execution. To estimate the vehicle state needed for control, a nonlinear observer is developed which only utilizes information from sensors that are mounted on the car-like tractor, making the system independent of additional trailer sensors. The proposed path planning and path-following control framework is implemented on a full-scale test vehicle and results from simulations and real-world experiments are presented.Comment: Preprin

A Global Steering Method for Nonholonomic Systems

In this paper, we present an iterative steering algorithm for nonholonomic systems (also called driftless control-affine systems) and we prove its global convergence under the sole assumption that the Lie Algebraic Rank Condition (LARC) holds true everywhere. That algorithm is an extension of the one introduced in [21] for regular systems. The first novelty here consists in the explicit algebraic construction, starting from the original control system, of a lifted control system which is regular. The second contribution of the paper is an exact motion planning method for nilpotent systems, which makes use of sinusoidal control laws and which is a generalization of the algorithm described in [29] for chained-form systems