Exact Wavefront Propagation for Globally Optimal One-to-All Path Planning on 2D Cartesian Grids

This paper introduces an efficient $\mathcal{O}(n)$ compute and memory complexity algorithm for globally optimal path planning on 2D Cartesian grids. Unlike existing marching methods that rely on approximate discretized solutions to the Eikonal equation, our approach achieves exact wavefront propagation by pivoting the analytic distance function based on visibility. The algorithm leverages a dynamic-programming subroutine to efficiently evaluate visibility queries. Through benchmarking against state-of-the-art any-angle path planners, we demonstrate that our method outperforms existing approaches in both speed and accuracy, particularly in cluttered environments. Notably, our method inherently provides globally optimal paths to all grid points, eliminating the need for additional gradient descent steps per path query. The same capability extends to multiple starting positions. We also provide a greedy version of our algorithm as well as open-source C++ implementation of our solver.

An Efficient Solution to the 2D Visibility Problem in Cartesian Grid Maps and its Application in Heuristic Path Planning

This paper introduces a novel, lightweight method to solve the visibility problem for 2D grids. The proposed method evaluates the existence of lines-of-sight from a source point to all other grid cells in a single pass with no preprocessing and independently of the number and shape of obstacles. It has a compute and memory complexity of $\mathcal{O}(n)$, where $n = n_{x}\times{} n_{y}$ is the size of the grid, and requires at most ten arithmetic operations per grid cell. In the proposed approach, we use a linear first-order hyperbolic partial differential equation to transport the visibility quantity in all directions. In order to accomplish that, we use an entropy-satisfying upwind scheme that converges to the true visibility polygon as the step size goes to zero. This dynamic-programming approach allows the evaluation of visibility for an entire grid orders of magnitude faster than typical ray-casting algorithms. We provide a practical application of our proposed algorithm by posing the visibility quantity as a heuristic and implementing a deterministic, local-minima-free path planner, setting apart the proposed planner from traditional methods. Lastly, we provide necessary algorithms and an open-source implementation of the proposed methods.

ASAP-MPC: An Asynchronous Update Scheme for Online Motion Planning with Nonlinear Model Predictive Control

This paper presents a Nonlinear Model Predictive Control (NMPC) scheme targeted at motion planning for mechatronic motion systems, such as drones and mobile platforms. NMPC-based motion planning typically requires low computation times to be able to provide control inputs at the required rate for system stability, disturbance rejection, and overall performance. Although there exist various ways in literature to reduce the solution times in NMPC, such times may not be low enough to allow real-time implementations. This paper presents ASAP-MPC, an approach to handle varying, sometimes restrictively large, solution times with an asynchronous update scheme, always allowing for full convergence and real-time execution. The NMPC algorithm is combined with a linear state feedback controller tracking the optimised trajectories for improved robustness against possible disturbances and plant-model mismatch. ASAP-MPC seamlessly merges trajectories, resulting from subsequent NMPC solutions, providing a smooth and continuous overall trajectory for the motion system. This frameworks applicability to embedded applications is shown on two different experiment setups where a state-of-the-art method fails: a quadcopter flying through a cluttered environment in hardware-in-the-loop simulation and a scale model truck-trailer manoeuvring in a structured lab environment.

FATROP : A Fast Constrained Optimal Control Problem Solver for Robot Trajectory Optimization and Control

Trajectory optimization is a powerful tool for robot motion planning and control. State-of-the-art general-purpose nonlinear programming solvers are versatile, handle constraints in an effective way and provide a high numerical robustness, but they are slow because they do not fully exploit the optimal control problem structure at hand. Existing structure-exploiting solvers are fast but they often lack techniques to deal with nonlinearity or rely on penalty methods to enforce (equality or inequality) path constraints. This works presents FATROP: a trajectory optimization solver that is fast and benefits from the salient features of general-purpose nonlinear optimization solvers. The speed-up is mainly achieved through the use of a specialized linear solver, based on a Riccati recursion that is generalized to also support stagewise equality constraints. To demonstrate the algorithm's potential, it is benchmarked on a set of robot problems that are challenging from a numerical perspective, including problems with a minimum-time objective and no-collision constraints. The solver is shown to solve problems for trajectory generation of a quadrotor, a robot manipulator and a truck-trailer problem in a few tens of milliseconds. The algorithm's C++-code implementation accompanies this work as open source software, released under the GNU Lesser General Public License (LGPL). This software framework may encourage and enable the robotics community to use trajectory optimization in more challenging applications.

Energy-Efficient Control of Cable Robots Exploiting Natural Dynamics and Task Knowledge

This paper focusses on the energy-efficient control of a cable-driven robot for tasks that only require precise positioning at few points in their motion, and where that accuracy can be obtained through contacts. This includes the majority of pick-and-place operations. Knowledge about the task is directly taken into account when specifying the control execution. The natural dynamics of the system can be exploited when there is a tolerance on the position of the trajectory. Brakes are actively used to replace standstill torques, and as passive actuation. This is executed with a hybrid discrete-continuous controller. A discrete controller is used to specify and coordinate between subtasks, and based on the requirements of these specific subtasks, specific, robust, continuous controllers are constructed. This approach allows for less stiff and thus saver, and cheaper hardware to be used. For a planar pick-and-place operation, it was found that this results in energy savings of more than 30%. However, when the payload moves with the natural dynamics, there is less control of the followed trajectory and its timing compared to a traditional trajectory-based execution. Also, the presented approach implies a fundamentally different way to specify and execute tasks.

IMPACT: A Toolchain for Nonlinear Model Predictive Control Specification, Prototyping, and Deployment

We present IMPACT, a flexible toolchain for nonlinear model predictive control (NMPC) specification with automatic code generation capabilities. The toolchain reduces the engineering complexity of NMPC implementations by providing the user with an easy-to-use application programming interface, and with the flexibility of using multiple state-of-the-art tools and numerical optimization solvers for rapid prototyping of NMPC solutions. IMPACT is written in Python, users can call it from Python and MATLAB, and the generated NMPC solvers can be directly executed from C, Python, MATLAB and Simulink. An application example is presented involving problem specification and deployment on embedded hardware using Simulink, showing the effectiveness and applicability of IMPACT for NMPC-based solutions.

MPC-based Motion Planning for Autonomous Truck-Trailer Maneuvering

Time-optimal motion planning of autonomous vehicles in complex environments is a highly researched topic. This paper describes a novel approach to optimize and execute locally feasible trajectories for the maneuvering of a truck-trailer Autonomous Mobile Robot (AMR), by dividing the environment in a sequence or route of freely accessible overlapping corridors. Multi-stage optimal control generates local trajectories through advancing subsets of this route. To cope with the advancing subsets and changing environments, the optimal control problem is solved online with a receding horizon in a Model Predictive Control (MPC) fashion with an improved update strategy. This strategy seamlessly integrates the computationally expensive MPC updates with a low-cost feedback controller for trajectory tracking, for disturbance rejection, and for stabilization of the unstable kinematics of the reversing truck-trailer AMR. This methodology is implemented in a flexible software framework for an effortless transition from offline simulations to deployment of experiments. An experimental setup showcasing the truck-trailer AMR performing two reverse parking maneuvers validates the presented method.

An optimal open-loop strategy for handling a flexible beam with a robot manipulator

Fast and safe manipulation of flexible objects with a robot manipulator necessitates measures to cope with vibrations. Existing approaches either increase the task execution time or require complex models and/or additional instrumentation to measure vibrations. This paper develops a model-based method that overcomes these limitations. It relies on a simple pendulum-like model for modeling the beam, open-loop optimal control for suppressing vibrations, and does not require any exteroceptive sensors. We experimentally show that the proposed method drastically reduces residual vibrations -- at least 90% -- and outperforms the commonly used input shaping (IS) for the same execution time. Besides, our method can also execute the task faster than IS with a minor reduction in vibration suppression performance. The proposed method facilitates the development of new solutions to a wide range of tasks that involve dynamic manipulation of flexible objects.