This paper focuses on cooperative speed planning for multiple connected and automated vehicles (CAVs) traversing along intersected fixed paths. Nominally, this task is formulated as an optimal control problem incorporating logical operators to represent collision-avoidance constraints. This formulation requires solving a mixed-integer nonlinear programming (MINLP) problem, while handling non-differentiable integer variables remains challenging for gradient-based solvers. Instead of solving the MINLP, we propose a cosine-based method, a novel geometric strategy for formulating collision-avoidance constraints between CAVs. Constructing such a geometric model introduces potential approximation errors, which are mitigated by fitted correction terms designed to compensate for geometric deviations and refine the distance calculation. We propose a simulation-based planner to provide the speed profile with the globally optimal passing order, serving as a warm start for the solver. A lightweight iterative optimization strategy is also adopted to enhance robustness. Additionally, we propose a fault-tolerant strategy to ensure both system safety and operational efficiency. Extensive simulation results verify the proposed method, and comparative experiments demonstrate its efficiency.

This study proposes a risk-tolerant dispatch approach for a fleet of autonomous mining trucks in an open-pit mine, leveraging early signs to reduce the impact of potential failures that may or may not occur later. Unlike traditional methods that ignore these early signs or wait until a failure has actually happened, our approach proactively plans for both possible outcomes without relying on probabilities. We propose a Y-shaped solution structure composed of a shared trunk that covers the period before it becomes clear if the failure will occur and two separate branches that address the final scenarios. We formulate the dispatch problem as a mixed-integer linear program and solve it via Gurobi. To facilitate the solution process with Gurobi, an evolutionary algorithm is adopted to explore the solution space for a good initial guess. A high-performance discrete-event simulator is embedded in the cost function evaluation module of the evolutionary algorithm for quickly selecting qualified solution candidates. By integrating both the failure and non-failure scenarios into one unified plan, we avoid extreme risk-taking or undue conservatism, ensuring stable operational performance. Simulations and field trials at a real open-pit mine confirm that this risk-tolerant approach effectively manages failure risks when early signs are available.

Ceiling painting is an essential yet labor-intensive task in the construction industry, making automation necessary to overcome issues such as inconsistent manual quality and associated health hazards. This paper addresses the multi-target coverage trajectory planning problem for ceiling painting robots operating in complex indoor environments, aiming to generate efficient, collision-free, and kinematically feasible trajectories that fully cover all designated peeling areas. The planning problem is first formulated as a mixed-integer optimal control model and then discretized into a mixed-integer nonlinear program. Conventional solvers struggle with prohibitive computational complexity due to integer variables and non-convex constraints arising from kinematic feasibility, collision avoidance, and coverage requirements. To overcome these computational challenges, we propose a two-stage planning structure. In the first stage, the planner constructs a coarse trajectory by integrating waypoint clustering, probabilistic roadmap-based collision-free path planning, and Traveling Salesman Problem optimization. The second stage addresses the non-convexity of the coverage constraints through an alternating optimization approach, iteratively fixing integer and continuous variables to achieve convexification and refine the trajectory. To further improve computational efficiency and scalability, the trajectory is segmented and each segment optimized independently. Simulation results demonstrate the effectiveness and reliability of the proposed planner, highlighting significant reductions in computational time compared with baseline methods.

Optimization-based methods are commonly applied in autonomous driving trajectory planners, which transform the continuous-time trajectory planning problem into a finite nonlinear program with constraints imposed at finite collocation points. However, potential violations between adjacent collocation points can occur. To address this issue thoroughly, we propose a safety-guaranteed collision-avoidance model to mitigate collision risks within optimization-based trajectory planners. This model introduces an “embodied footprint”, an enlarged representation of the vehicle’s nominal footprint. If the embodied footprints do not collide with obstacles at finite collocation points, then the ego vehicle’s nominal footprint is guaranteed to be collision-free at any of the infinite moments between adjacent collocation points. According to our theoretical analysis, we define the geometric size of an embodied footprint as a simple function of vehicle velocity and curvature. Particularly, we propose a trajectory optimizer with the embodied footprints that can theoretically set an appropriate number of collocation points prior to the optimization process. We conduct this research to enhance the foundation of optimization-based planners in robotics. Comparative simulations and field tests validate the completeness, solution speed, and solution quality of our proposal.

A growing interest in autonomous driving (AD) and intelligent vehicles (IVs) is fueled by their promise for enhanced safety, efficiency, and economic benefits. While previous surveys have captured progress in this field, a comprehensive and forward-looking summary is needed. Our work fills this gap through three distinct articles. The first part, a “survey of surveys” (SoS), outlines the history, surveys, ethics, and future directions of AD and IV technologies. The second part, “Milestones in AD and IVs Part I: Control, Computing System Design, Communication, high-definition map (HD map), Testing, and Human Behaviors” delves into the development of control, computing system, communication, HD map, testing, and human behaviors in IVs. This part, the third part, reviews perception and planning in the context of IVs. Aiming to provide a comprehensive overview of the latest advancements in AD and IVs, this work caters to both newcomers and seasoned researchers. By integrating the SoS and Part I, we offer unique insights and strive to serve as a bridge between past achievements and future possibilities in this dynamic field.

Effective road maintenance can not only achieve a balance between limited resources and long-term high-efficiency performance of road but also reduce the loss of life and property caused by road damage to vehicles and pedestrians. Due to the lack of a multidimensional dynamic monitoring system and enough extremely special data, the existing road maintenance system cannot accurately assess the road surface condition and provide timely early warning of sudden road damage. In this article, the M-RM system is proposed, that is, a metaverse-enabled road maintenance system based on cyber–physical–social systems (CPSSs), which fully utilizes the social and artificial system information of CPSS, as well as the simulation, monitoring, diagnosis and prediction functions of road systems in the virtual world of the metaverse. Then, in the road damage detection of system model in the virtual world, for the virtual data of the core assets of the metaverse, we propose an adaptive and information-preserving data augmentation (AIDA) algorithm-based nonclassical receptive field suppression and enhancement, an algorithm developed from human visual cognition. This algorithm enables the generation of a large amount of scarce fidelity data and avoids the introduced noise from impairing the performance of nonaugmented data. Finally, a crack detection algorithm named pay attention twice (PAT) is proposed, which uses the generated virtual data for training, and achieves secondary attention to high-frequency targets by fusing frequency-division convolution and mixed-domain attention mechanism. The detection performance of small targets in uncertain environments is enhanced. The metaverse system built in the current research can not only be used for road maintenance but also empower the traffic metaverse by using the traffic flow prediction module embedded in the algorithm. Experimental results demonstrate that the proposed algorithm can be applied to the road damage detection task under different noise and weather conditions, and the performance outweighs other state-of-the-art algorithms.

Replacing traffic signals with roadside vehicle-to-infrastructure systems in the era of connected and autonomous vehicles (CAVs) is promising. Managing CAVs in a signal-free intersection, known as autonomous intersection management (AIM), controls the driving behavior of each intersection-traverse CAV to maximize the throughput. Although AIM improves the gross throughput, the fairness of each individual vehicle in its right of way is not seriously considered. This study sets up an AIM system in the cyber–physical–social space to trade traverse priorities quantitatively and fairly. To that end, one needs an AIM method that is optimal and stable, otherwise no convincing trades of traverse priorities could be made. This study proposes a near-optimal lane-free AIM method based on numerical optimal control, wherein log-exp functions are deployed to convexify nondifferentiable collision-avoidance constraints. Besides that, a parameterized social force model (SFM) is proposed to provide a tunable initial guess for numerical optimal control. By tuning the urgency weights in SFM, one may get cooperative trajectories in different homotopy classes, which are further utilized to decide the amount of virtual currency to reward those CAVs who tend to share their traverse priorities. The overall method improves the traverse throughput with individual fairness respected. In experiencing this system, passengers learn how to behave with politeness when they drive manually. Experiments show the efficiency and robustness of the AIM method and also show the efficacy of the overall priority-sharing system.

This paper is focused on the cooperative trajectory planning problem for multiple car-like robots in a cluttered and unstructured environment narrowed by static obstacles. The concerned multi-vehicle trajectory planning (MVTP) problem is challenging because i) the scenario is nonconvex and tiny; ii) the vehicle kinematics is nonconvex; and iii) a feasible homotopy class is unavailable a priori. We propose a two-stage MVTP method: Stage 1 identifies a feasible homotopy class, and Stage 2 quickly finds a local optimum based on the identified homotopy class. Numerical optimal control, adaptive scaling, grouping, and trust region construction strategies are integrated into the proposed planner. Our planner is extensively compared in 100 benchmark cases with the state-of-the-art MVTP methods such as incremental sequential convex programming, numerical optimal control, conflict-based search, priority-based trajectory optimizer, and optimal reciprocal collision avoidance. The simulation results demonstrate our method's superiority in runtime and optimality. Experiments with three car-like robots demonstrate the efficiency of our proposed planner. Source codes are in https://github.com/libai1943/MVTP_benchmark.

Road shoulders and slopes often appear in unstructured environments. They make 2.5D vehicle trajectory planning commonly seen in our daily life, which lies on a 2D manifold embedded in a 3D space. The height difference of these terrains brings spatially dependent constraints on vehicle maneuvers, such as the limit on vehicle steering for vehicle tire protection when a vehicle approaches a road shoulder edge. These constraints have an “if-else” structure since they are activated only when the vehicle passes through the local area with a height difference, making the restriction on variables coupled with the judgment of variables. This makes the application of state-of-art optimization-based planners challenging. To solve this problem, we devise an approximation formulation for these constraints in the trajectory planning optimization problem, whose solution depends on a proper initial guess for the optimizer. We propose a two-stage trajectory planning framework, where the first stage improves the hybrid A* algorithm by adding spatially dependent constraints into node expansion to provide the initial guess. Then, the optimization problem with the formulated spatially dependent constraints is solved for further trajectory smoothness and quality. Finally, the simulation results validate the fast and high-quality planning performance of our proposed framework.

This paper is focused on planning fast, accurate, and optimal trajectories for autonomous parking. Nominally, this task should be described as an optimal control problem (OCP), wherein the collision-avoidance constraints guarantee travel safety and the kinematic constraints guarantee tracking accuracy. The dimension of the nominal OCP is high because it requires the vehicle to avoid collision with each obstacle at every moment throughout the entire parking process. With a coarse trajectory guiding a homotopic route, the intractably scaled collision-avoidance constraints are replaced by within-corridor constraints, whose scale is small and independent from the environment complexity. Constructing such a corridor sacrifices partial free spaces, which may cause loss of optimality or even feasibility. To address this issue, our proposed method reconstructs the corridor in an iterative framework, where a lightweight OCP with only box constraints is quickly solved in each iteration. The proposed planner, together with several prevalent optimization-based planners are tested under 115 simulation cases w.r.t. the success rate and computational time. Real-world indoor experiments are conducted as well.

Curvy roads are a particular type of urban road scenario, wherein the curvature of the road centerline changes drastically. This paper is focused on the trajectory planning task for autonomous driving on a curvy road. The prevalent on-road trajectory planners in the Frenet frame cannot impose accurate restrictions on the trajectory curvature, thus easily making the resultant trajectories beyond the ego vehicle’s kinematic capability. Regarding planning in the Cartesian frame, selection-based methods suffer from the curse of dimensionality. By contrast, optimization-based methods in the Cartesian frame are more flexible to find optima in the continuous solution space, but the new challenges are how to tackle the intractable collision-avoidance constraints and nonconvex kinematic constraints. An iterative computation framework is proposed to accumulatively handle the complex constraints. Concretely, an intermediate problem is solved in each iteration, which contains linear and tractably scaled collision-avoidance constraints and softened kinematic constraints. Compared with the existing optimization-based planners, our proposal is less sensitive to the initial guess especially when it is not kinematically feasible. The efficiency of the proposed planner is validated by both simulations and real-world experiments. Source codes of this work are available athttps://github.com/libai1943/CartesianPlanner.

Motion planning in unstructured and low-speed environments is a fundamental and difficult task for all mobile robotics. If we view motion planning as an optimization problem, the non-convex collision avoidance constraints and the nonlinear vehicle dynamic constraints make motion planning challenging and time-consuming. In this paper, we propose a Successive Linearization in Feasible Set (SLiFS) algorithm to address these two difficulties. SLiFS consists of two steps. The first step is to iteratively construct convex feasible sets around the current trajectory to approximate non-convex collision avoidance constraints. The second step is to successively linearize the nonlinear dynamic constraints along the current trajectory and further penalize them into the objective function to avoid infeasible linearized constraints, so that the current trajectory can be reshaped within the obtained convex feasible sets by iteratively solving the linearized optimization problem. The main innovation of SLiFS algorithm is that we consider the$L_{1}$norm type penalty function in the second step. We find that the sparsity of the$L_{1}$norm might help to satisfy the robotic dynamic constraints by numerical experiments. Numerical testing results show that our proposed SLiFS algorithm has a high success rate to find feasible trajectories and costs much less time than the classical interior-point method.

Autonomous intersection management (AIM) refers to planning the trajectories for multiple connected and automated vehicles (CAVs) when they traverse an unsignalized intersection cooperatively. As an extension of the conventional AIM, lane-free AIM allows the CAVs to adjust their velocities and paths flexibly within the intersection. Nominally, one needs to formulate a centralized optimal control problem (OCP) to describe the concerned lane-free AIM scheme, but solving such an intractably scaled problem is challenging. This work proposes a batch-processing framework, which divides the traffic flow into batches. The cooperative trajectories within one batch are planned by numerically solving a small-scale OCP; all the batches are managed via a reservation-based method following the first-come-first-serve policy. The proposed batch-processing framework aims to run as fast as a reservation-based method at the macro level while taking care of the cooperative driving quality at the micro level. The proposed method is validated via simulation and preliminary experiments.

A solution for a tractor-trailer vehicle's generic maneuver planning task can be introduced by an optimal control problem (OCP). However, the curse of dimensionality is excited along with the OCP solution due to the collision-avoidance constraints in a large scale. The collision-avoidance conditions are weakened by simply constructing a corridor along a homotopically guiding route such that the vehicle's maneuvers are safely separated from obstacles. This approach is motivated by the safe flight corridor (SFC) applied for path planning of unmanned aerial vehicle (UAV). But SFC cannot be applied directly to the ground vehicle cases because a tractor-trailer vehicle cannot be modeled as a mass point in a narrow environment. An extension of the SFC is proposed, which requires different bodies of a multi-body vehicle to stay in different safe travel corridors. In this way, a reduced-scale OCP is formulated, and the problem scale becomes irrelevant to the environmental complexity. Simulation results illustrate that near-optimal maneuvers can be derived within less CPU time.

Trajectory planning for a tractor-trailer vehicle is challenging because the vehicle kinematics consists of underactuated and nonholonomic constraints that are highly coupled. Prevalent sampling-based or search-based planners suitable for rigid-body vehicles are not capable of handling the tractor-trailer vehicle cases. This work aims to deal with generic n-trailer cases in the tiny environments. To this end, an optimal control problem is formulated, which is beneficial in being accurate, straightforward, and unified. An adaptively homotopic warm-starting approach is proposed to facilitate the numerical solution process of the formulated optimal control problem. Compared with the existing sequential warm starting strategies, our proposal can adaptively define the subproblems with the purpose of making the gaps between adjacent subproblems “pleasant” for the solver. Unification and efficiency of the proposed adaptively homotopic warm-starting approach have been investigated in several extremely tiny scenarios. Our planner finds solutions that other existing planners cannot. Online planning opportunities are briefly discussed as well.

In this paper, we propose a cooperative motion planning method for a group of connected and automated vehicles (CAVs) crossing a lane-free intersection without using explicit traffic signaling. This multi-vehicle motion planning task is formulated as a centralized optimal control problem. However, the solution to this optimal control problem is numerically intractable due to the high dimensionality of the collision-avoidance constraints and the nonlinearity of the vehicle kinematics. A two-stage strategy is proposed for generating online solutions: at Stage 1, the CAVs are requested to reach a standard formation before entering the intersection; at Stage 2, the vehicles cross the intersection. As the motion planning sub-problem at Stage 2 begins with a standard configuration, the optimal solution to this standard sub-problem can be computed offline in advance and applied online directly. On the other hand, the formation reconfiguration sub-problem at Stage 1 is easy to solve online. Through dividing the entire dynamic process into two periods, the difficulties in the original optimal control problem are significantly reduced so that the real-time performance is achieved.

This paper focuses on the scheme of cooperative lane change motion planning of multiple connected and automated vehicles, so as to minimize the time for lane change while penalizing large steering angles subject to hard collision avoidance constraints. Nominally this scheme should be formulated in a centralized way with the constraints of all the vehicles considered simultaneously. In order to facilitate the numerical solving process of this centralized optimization problem, we propose a stepwise computation framework. Starting with a sub-problem with all of the collision avoidance constraints removed, a sequence of sub-problems are defined by adding back the removed collision avoidance constraints gradually until the original problem takes shape in the end. The optimum of one sub-problem is always used as the initial guess when solving the next sub-problem. This iterative process continues until the optimum of the original problem is obtained. In this way, the difficulties in the original centralized problem are divided into multiple parts, and every progress made to address the partial difficulties is “solidified” by the initial guess.

This work formulates the multi-vehicle lane change motion planning task as a centralized optimal control problem, which is beneficial in being generic and complete. However, a direct solution to this optimal control problem is numerically intractable due to the dimensionality of the collision-avoidance constraints and nonlinearity of the vehicle kinematics. A progressively constrained dynamic optimization (PCDO) method is proposed to facilitate the numerical solving process of this complicated problem. PCDO guarantees to efficiently obtain an optimum to the original optimal control problem via solving a sequence of simplified problems which gradually judge and reserve only the active collision-avoidance constraints. A first-regularization-then-action strategy, together with the look-up table technique, is developed for online solutions. At the regularization stage, the vehicles form a standard formation by linear acceleration/deceleration only. At the action stage, the vehicles execute lane change motions computed offline and recorded in the look-up table. This makes online motion planning feasible because 1) the computational complexity at the regularization stage scales linearly rather than exponentially with the vehicle number; and 2) online computation at the action stage is fully avoided through data extraction from the look-up table.

Autonomous parking has been a widely developed branch of intelligent transportation systems. In autonomous parking, maneuver planning is a crucial procedure that determines how intelligent the entire parking system is. This paper concerns planning time-optimal parallel parking maneuvers in a straightforward, accurate, and purely objective way. A unified dynamic optimization framework is established, which includes the vehicle kinematics, physical restrictions, collision-avoidance constraints, and an optimization objective. Interior-point method (IPM)-based simultaneous dynamic optimization methodology is adopted to solve the formulated dynamic optimization problem numerically. Given that near-feasible solutions have been widely acknowledged to ease optimizing nonlinear programs (NLPs), a critical region-based initialization strategy is proposed to facilitate the offline NLP-solving process, a lookup table-based strategy is proposed to guarantee the on-site planning performance, and a receding-horizon optimization framework is proposed for online maneuver planning. A series of parallel parking cases is tested, and simulation results demonstrate that our proposal is efficient even when the slot length is merely 10.19% larger than the car length. As a unified maneuver planner, our adopted IPM-based simultaneous dynamic optimization method can deal with any user-specified demand provided that it can be explicitly described.

Trajectory planning is a critical aspect of autonomous tractor-trailer vehicle design. Trajectory planning algorithms usually compute paths first, trajectories are obtained thereafter. This multi-step feature makes those planners inefficacious to handle time-dependent constraints. In this study, we consider the original trajectory planning mission directly, which is described as an optimal control problem containing the kinematics, mechanical/physical constraints, environmental requirements as well as an optimization criterion. In this formulation, only the fundamental driving principles with no special issues (e.g., backing-up maneuver and jackknife) are considered. For example, the prevailing “small-angle assumption” is not utilized to prevent jackknifing. Instead, we only require that different parts of a tractor-trailer vehicle should not collide, since the emergence of jackknife does not physically violate the kinematics. An interior-point method based simultaneous approach is adopted to solve the formulated optimal control problem. Simulation results verify our proposal is capable of handling scenarios with various user-specified requirements.

This paper proposes a motion planner for autonomous parking. Compared to the prevailing and emerging studies that handle specific or regular parking scenarios only, our method describes various kinds of parking cases in a unified way regardless they are regular parking scenarios (e.g., parallel, perpendicular or echelon parking cases) or not. First, we formulate a time-optimal dynamic optimization problem with vehicle kinematics, collision-avoidance conditions and mechanical constraints strictly described. Thereafter, an interior-point simultaneous approach is introduced to solve that formulated dynamic optimization problem. Simulation results validate that our proposed motion planning method can tackle general parking scenarios. The tested parking scenarios in this paper can be regarded as benchmark cases to evaluate the efficiency of methods that may emerge in the future. Our established dynamic optimization problem is an open and unified framework, where other complicated user-specific constraints/optimization criteria can be handled without additional difficulty, provided that they are expressed through inequalities/polynomial explicitly. This proposed motion planner may be suitable for the next-generation intelligent parking-garage system.

Submarine search-evasion path planning aims to acquire an evading route for a submarine so as to avoid the detection of hostile anti-submarine searchers such as helicopters, aircraft and surface ships. In this paper, we propose a numerical optimization model of search-evasion path planning for invading submarines. We use the Artificial Bee Colony (ABC) algorithm, which has been confirmed to be competitive compared to many other nature-inspired algorithms, to solve this numerical optimization problem. In this work, several search-evasion cases in the two-dimensional plane have been carefully studied, in which the anti-submarine vehicles are equipped with sensors with circular footprints that allow them to detect invading submarines within certain radii. An invading submarine is assumed to be able to acquire the real-time locations of all the anti-submarine searchers in the combat field. Our simulation results show the efficacy of our proposed dynamic route optimization model for the submarine search-evasion path planning mission.