An End-to-End UAV Simulation Platform for Visual SLAM and Navigation

Visual simultaneous localization and mapping (v-SLAM) and navigation of unmanned aerial vehicles (UAVs) are receiving increasing attention in both research and education. However, extensive physical testing can be expensive and time-consuming due to safety precautions, battery constraints, and the complexity of hardware setups. For the efficient development of navigation algorithms and autonomous systems, as well as for education purposes, the ROS-Gazebo-PX4 simulator was customized in-depth, integrated into our previous released research works, and provided as an end-to-end simulation (E2ES) solution for UAV, v-SLAM, and navigation applications. Unlike most other similar works, which can only stimulate certain parts of the navigation algorithms, the E2ES platform simulates all of the localization, mapping, and path-planning kits in one simulator. The navigation stack performs well in the E2ES test bench with the absolute pose errors of 0.3 m (translation) and 0.9 degree (rotation), respectively, for an 83 m length trajectory. Moreover, the E2ES provides an out-of-box, click-and-fly autonomy in UAV navigation. The project source code is opened for the benefit of the research community.

Path Planning Strategy for a Manipulator Based on a Heuristically Constructed Network

Collision-free path planning of manipulators is becoming indispensable for space exploration and on-orbit operation. Manipulators in these scenarios are restrained in terms of computing resources and storage, so the path planning method used in such tasks is usually limited in its operating time and the amount of data transmission. In this paper, a heuristically constructed network (HCN) construction strategy is proposed. The HCN construction contains three steps: determining the number of hub configurations and selecting and connecting hub configurations. Considering the connection time and connectivity of HCN, the number of hub configurations is determined first. The selection of hub configurations includes the division of work space and the optimization of the hub configurations. The work space can be divided by considering comprehensively the similarity among the various configurations within the same region, the dissimilarity among all regions, and the correlation among adjacent regions. The hub configurations can be selected by establishing and solving the optimization model. Finally, these hub configurations are connected to obtain the HCN. The simulation indicates that the path points number and the planning time is decreased by 45.5% and 48.4%, respectively, which verify the correctness and effectiveness of the proposed path planning strategy based on the HCN.

An Approach to Air-to-Surface Mission Planner on 3D Environments for an Unmanned Combat Aerial Vehicle

Recently, interest in mission autonomy related to Unmanned Combat Aerial Vehicles(UCAVs) for performing highly dangerous Air-to-Surface Missions(ASMs) has been increasing. Regarding autonomous mission planners, studies currently being conducted in this field have been mainly focused on creating a path from a macroscopic 2D environment to a dense target area or proposing a route for intercepting a target. For further improvement, this paper treats a mission planning algorithm on an ASM which can plan the path to the target dense area in consideration of threats spread in a 3D terrain environment while planning the shortest path to intercept multiple targets. To do so, ASMs are considered three sequential mission elements: ingress, intercept, and egress. The ingress and egress elements require a terrain flight path to penetrate deep into the enemy territory. Thus, the proposed terrain flight path planner generates a nap-of-the-earth path to avoid detection by enemy radar while avoiding enemy air defense threats. In the intercept element, the shortest intercept path planner based on the Dubins path concept combined with nonlinear programming is developed to minimize exposure time for survivability. Finally, the integrated ASM planner is applied to several mission scenarios and validated by simulations using a rotorcraft model.

Dynamic path planning over CG-Space of 10DOF Rover with static and randomly moving obstacles using RRT* rewiring

Dynamic path planning is a core research content for intelligent robots. This paper presents a CG-Space-based dynamic path planning and obstacle avoidance algorithm for 10 DOF wheeled mobile robot (Rover) traversing over 3D uneven terrains. CG-Space is the locus of the center of gravity location of Rover while moving on a 3D terrain. A CG-Space-based modified RRT* samples a random space tree structure. Dynamic rewiring this tree can handle the randomly moving obstacles and target in real time. Simulations demonstrate that the Rover can obtain the target location in 3D uneven dynamic environments with fixed and randomly moving obstacles.

An Integrated Remote Control-Based Human-Robot Interface for Education Application

Portable interfaced robot arms equipped with mobile user interactions are significantly being utilized in modern world. The application of teaching robotics is being used in challenging pandemic situation but it is still challenging due to mathematical formulation. This article utilizes the augmented reality (AR) concept for remote control-based human-robot interaction using the Bluetooth correspondence. The proposed framework incorporates different modules like a robot arm control, a regulator module and a distant portable smartphone application for envisioning the robot arm points for its real-time relevance. This novel approach fuses AR innovation into portable application which permit the continuous virtual coordination with actual physical platform. The simulation yields effective outcomes with 96.94% accuracy for testing stage while maintaining error and loss values of 0.194 and 0.183 respectively. The proposed interface gives consistent results for teaching application in real time changing environment by outperforming existing methods with an accuracy improvement of 13.4

Kimera: From SLAM to spatial perception with 3D dynamic scene graphs

Humans are able to form a complex mental model of the environment they move in. This mental model captures geometric and semantic aspects of the scene, describes the environment at multiple levels of abstractions (e.g., objects, rooms, buildings), includes static and dynamic entities and their relations (e.g., a person is in a room at a given time). In contrast, current robots’ internal representations still provide a partial and fragmented understanding of the environment, either in the form of a sparse or dense set of geometric primitives (e.g., points, lines, planes, and voxels), or as a collection of objects. This article attempts to reduce the gap between robot and human perception by introducing a novel representation, a 3D dynamic scene graph (DSG), that seamlessly captures metric and semantic aspects of a dynamic environment. A DSG is a layered graph where nodes represent spatial concepts at different levels of abstraction, and edges represent spatiotemporal relations among nodes. Our second contribution is Kimera, the first fully automatic method to build a DSG from visual–inertial data. Kimera includes accurate algorithms for visual–inertial simultaneous localization and mapping (SLAM), metric–semantic 3D reconstruction, object localization, human pose and shape estimation, and scene parsing. Our third contribution is a comprehensive evaluation of Kimera in real-life datasets and photo-realistic simulations, including a newly released dataset, uHumans2, which simulates a collection of crowded indoor and outdoor scenes. Our evaluation shows that Kimera achieves competitive performance in visual–inertial SLAM, estimates an accurate 3D metric–semantic mesh model in real-time, and builds a DSG of a complex indoor environment with tens of objects and humans in minutes. Our final contribution is to showcase how to use a DSG for real-time hierarchical semantic path-planning. The core modules in Kimera have been released open source.

LPWAN-Based Real-Time 2D SLAM and Object Localization for Teleoperation Robot Control

In this study, the teleoperation robot control on a mobile robot with 2D SLAM and object localization using LPWAN is proposed. The mobile robot is a technology gaining popularity due to flexibility and robustness in a variety of terrains. In search and rescue activities, the mobile robots can be used to perform some missions, assist and preserve human life. However, teleoperation control becomes a challenging problem for this implementation. The robust wireless communication not only allows the operator to stay away from dangerous area, but also increases the mobility of the mobile robot itself. Most of teleoperation mobile robots use Wi-Fi having high-bandwidth, yet short communication range. LoRa as LPWAN, on the other hand, has much longer range but low-bandwidth communication speed. Therefore, the combination of them complements each other’s weaknesses. The use of a two-LoRa configuration also enhances the teleoperation capabilities. All information from the mobile robot can be sent to the PC controller in relatively fast enough for real-time SLAM implementation. Furthermore, the mobile robot is also capable of real-time object detection, localization, and transmitting images. Another problem of LoRa communication is a timeout. We apply timeout recovery algorithms to handle this issue, resulting in more stable data. All data have been confirmed by real-time trials and the proposed method can approach the Wi-Fi performance with a low waiting time or delay.

Safe Reinforcement Learning With Stability Guarantee for Motion Planning of Autonomous Vehicles

Reinforcement learning with safety constraints is promising for autonomous vehicles, of which various failures may result in disastrous losses. In general, a safe policy is trained by constrained optimization algorithms, in which the average constraint return as a function of states and actions should be lower than a predefined bound. However, most existing safe learning-based algorithms capture states via multiple high-precision sensors, which complicates the hardware systems and is power-consuming. This article is focused on safe motion planning with the stability guarantee for autonomous vehicles with limited size and power. To this end, the risk-identification method and the Lyapunov function are integrated with the well-known soft actor-critic (SAC) algorithm. By borrowing the concept of Lyapunov functions in the control theory, the learned policy can theoretically guarantee that the state trajectory always stays in a safe area. A novel risk-sensitive learning-based algorithm with the stability guarantee is proposed to train policies for the motion planning of autonomous vehicles. The learned policy is implemented on a differential drive vehicle in a simulation environment. The experimental results show that the proposed algorithm achieves a higher success rate than the SAC.

Path Planning for Automatic Guided Vehicles (AGVs) Fusing MH-RRT with Improved TEB

In this paper, an AGV path planning method fusing multiple heuristics rapidly exploring random tree (MH-RRT) with an improved two-step Timed Elastic Band (TEB) is proposed. The modified RRT integrating multiple heuristics can search a safer, optimal and faster converge global path within a short time, and the improved TEB can optimize both path smoothness and path length. The method is composed of a global path planning procedure and a local path planning procedure, and the Receding Horizon Planning (RHP) strategy is adopted to fuse these two modules. Firstly, the MH-RRT is utilized to generate a state tree structure as prior knowledge, as well as the global path. Then, a receding horizon window is established to select the local goal point. On this basis, an improved two-step TEB is designed to optimize the local path if the current global path is feasible. Various simulations both on static and dynamic environments are conducted to clarify the performance of the proposed MH-RRT and the improved two-step TEB. Furthermore, real applicative experiments verified the effectiveness of the proposed approach.

Path Planning for Autonomous Mobile Robots: A Review

Providing mobile robots with autonomous capabilities is advantageous. It allows one to dispense with the intervention of human operators, which may prove beneficial in economic and safety terms. Autonomy requires, in most cases, the use of path planners that enable the robot to deliberate about how to move from its location at one moment to another. Looking for the most appropriate path planning algorithm according to the requirements imposed by users can be challenging, given the overwhelming number of approaches that exist in the literature. Moreover, the past review works analyzed here cover only some of these approaches, missing important ones. For this reason, our paper aims to serve as a starting point for a clear and comprehensive overview of the research to date. It introduces a global classification of path planning algorithms, with a focus on those approaches used along with autonomous ground vehicles, but is also extendable to other robots moving on surfaces, such as autonomous boats. Moreover, the models used to represent the environment, together with the robot mobility and dynamics, are also addressed from the perspective of path planning. Each of the path planning categories presented in the classification is disclosed and analyzed, and a discussion about their applicability is added at the end.

A new gap-based obstacle avoidance approach: follow the obstacle circle method

One of the most challenging tasks for autonomous robots is avoiding unexpected obstacles during their path following operation. Follow the gap method (FGM) is one of the most popular obstacle avoidance algorithms that recursively guides the robot to the goal state by considering the angle to the goal point and the distance to the closest obstacles. It selects the largest gap around the robot, where the gap angle is calculated by the vector to the midpoint of the largest gap. In this paper, a novel obstacle avoidance procedure is developed and applied to a real fully autonomous wheelchair. This proposed algorithm improves the FGM’s travel safety and brings a new solution to the obstacle avoidance task. In the proposed algorithm, the largest gap is selected based on gap width. Moreover, the avoidance angle (similar to the gap center angle of FGM) is calculated considering the locus of the equidistant points from obstacles that create obstacle circles. Monte Carlo simulations are used to test the proposed algorithm, and according to the results, the new procedure guides the robot to safer trajectories compared with classical FGM. The real experimental test results are in parallel to the simulations and show the real-time performance of the proposed approach.

MP-RRT#: a Model Predictive Sampling-based Motion Planning Algorithm for Unmanned Aircraft Systems

This paper introduces a kinodynamic motion planning algorithm for Unmanned Aircraft Systems (UAS), called MP-RRT#. MP-RRT# joins the potentialities of RRT# with a strategy based on Model Predictive Control to efficiently solve motion planning problems under differential constraints. Similar to other RRT-based algorithms, MP-RRT# explores the map constructing an asymptotically optimal graph. In each iteration the graph is extended with a new vertex in the reference state of the UAS. Then, a forward simulation is performed using a Model Predictive Control strategy to evaluate the motion between two adjacent vertices, and a trajectory in the state space is computed. As a result, the MP-RRT# algorithm eventually generates a feasible trajectory for the UAS satisfying dynamic constraints. Simulation results obtained with a simulated drone controlled with the PX4 autopilot corroborate the validity of the MP-RRT# approach.

A Bidirectional Interpolation Method for Post-Processing in Sampling-Based Robot Path Planning

This paper proposes a post-processing method called bidirectional interpolation method for sampling-based path planning algorithms, such as rapidly-exploring random tree (RRT). The proposed algorithm applies interpolation to the path generated by the sampling-based path planning algorithm. In this study, the proposed algorithm is applied to the path created by RRT-connect and six environmental maps were used for the verification. It was visually and quantitatively confirmed that, in all maps, not only path lengths but also the piecewise linear shape were decreased compared to the path generated by RRT-connect. To check the proposed algorithm's performance, visibility graph, RRT-connect algorithm, Triangular-RRT-connect algorithm and post triangular processing of midpoint interpolation (PTPMI) were compared in various environmental maps through simulation. Based on these experimental results, the proposed algorithm shows similar planning time but shorter path length than previous RRT-like algorithms as well as RRT-like algorithms with PTPMI having a similar number of samples.

Physical interaction as communication: Learning robot objectives online from human corrections

When a robot performs a task next to a human, physical interaction is inevitable: the human might push, pull, twist, or guide the robot. The state of the art treats these interactions as disturbances that the robot should reject or avoid. At best, these robots respond safely while the human interacts; but after the human lets go, these robots simply return to their original behavior. We recognize that physical human–robot interaction (pHRI) is often intentional: the human intervenes on purpose because the robot is not doing the task correctly. In this article, we argue that when pHRI is intentional it is also informative: the robot can leverage interactions to learn how it should complete the rest of its current task even after the person lets go. We formalize pHRI as a dynamical system, where the human has in mind an objective function they want the robot to optimize, but the robot does not get direct access to the parameters of this objective: they are internal to the human. Within our proposed framework human interactions become observations about the true objective. We introduce approximations to learn from and respond to pHRI in real-time. We recognize that not all human corrections are perfect: often users interact with the robot noisily, and so we improve the efficiency of robot learning from pHRI by reducing unintended learning. Finally, we conduct simulations and user studies on a robotic manipulator to compare our proposed approach with the state of the art. Our results indicate that learning from pHRI leads to better task performance and improved human satisfaction.

An Improved Rapidly-Exploring Random Trees Algorithm Combining Parent Point Priority Determination Strategy and Real-Time Optimization Strategy for Path Planning

In order to solve the problems of long path planning time and large number of redundant points in the rapidly-exploring random trees algorithm, this paper proposed an improved algorithm based on the parent point priority determination strategy and the real-time optimization strategy to optimize the rapidly-exploring random trees algorithm. First, in order to shorten the path-planning time, the parent point is determined before generating a new point, which eliminates the complicated process of traversing the random tree to search the parent point when generating a new point. Second, a real-time optimization strategy is combined, whose core idea is to compare the distance of a new point, its parent point, and two ancestor points to the target point when a new point is generated, choosing the new point that is helpful for the growth of the random tree to reduce the number of redundant points. Simulation results of 3-dimensional path planning showed that the success rate of the proposed algorithm, which combines the strategy of parent point priority determination and the strategy of real-time optimization, was close to 100%. Compared with the rapidly-exploring random trees algorithm, the number of points was reduced by more than 93.25%, the path planning time was reduced by more than 91.49%, and the path length was reduced by more than 7.88%. The IRB1410 manipulator was used to build a test platform in a laboratory environment. The path obtained by the proposed algorithm enables the manipulator to safely avoid obstacles to reach the target point. The conclusion can be made that the proposed strategy has a better performance on optimizing the success rate, the number of points, the planning time, and the path length.

Effective motion planning of manipulator based on SDPS-RRTConnect

In order to improve the speed of motion planning, this paper proposes an improved RRTConnect algorithm (SDPS-RRTConnect) based on sparse dead point saved strategy. Combining sparse expansion strategy and dead point saved strategy, the algorithm can reduce the number of collision detection, fast convergence, avoid falling into local minimum, and ensure the completeness of search space. The algorithm is simulated in different environments. The results show that in complex environments, the sparse dead point saved strategy plays a good effect. In simple environments, the greedy connection strategy works well. Compared with the standard RRT algorithm, the standard RRTConnect algorithm, and the SDPS-RRT algorithm, the SDPS-RRTConnect algorithm has the shortest planning time, and it is suitable for both simple and complex environments. The 500 experiments show that the algorithm has strong robustness. The actual robot experiments show that the path planned by SDPS-RRTConnect algorithm is effective.

Strategies for Deploying a Sensor Network to Explore Planetary Lava Tubes

Recently discovered pits on the surface of the Moon and Mars are theorized to be remnants of lava tubes, and their interior may be in pristine condition. Current landers and rovers are unable to access these areas of high interest. However, multiple small, low-cost robots that can utilize unconventional mobility through ballistic hopping can work as a team to explore these environments. In this work, we propose strategies for exploring these newly discovered Lunar and Martian pits with the help of a mother-daughter architecture for exploration. In this architecture, a highly capable rover or lander would tactically deploy several spherical robots (SphereX) that would hop into the rugged pit environments without risking the rover or lander. The SphereX robots would operate autonomously and perform science tasks, such as getting inside the pit entrance, obtaining high-resolution images, and generating 3D maps of the environment. The SphereX robot utilizes the rover or lander’s resources, including the power to recharge and a long-distance communication link to Earth. Multiple SphereX robots would be placed along the theorized caves/lava tube to maintain a direct line-of-sight connection link from the rover/lander to the team of robots inside. This direct line-of-sight connection link can be used for multi-hop communication and wireless power transfer to sustain the exploration mission for longer durations and even lay a foundation for future high-risk missions.

Optimal Trajectory Generation for Intelligent Vehicles in Complex Traffic Based on Iteration Convex Optimization

Intelligent vehicles face considerable challenges in the complex traffic environment since they need to deal with various constraints and elements. This dissertation puts forward a novel trajectory planning framework for intelligent vehicles to generate safe and optimal driving trajectories. First, we design a spatiotemporal occupancy framework to deal with all kinds of elements in the complex driving environment based on the Frenét frame. This framework unifies various constraints on the road in the three-dimensional spatiotemporal representation and clearly describes the collision-free configuration space. Then we use the convex approximation method to construct a time-varying convex feasible region based on the above accurate temporal and spatial description. We formulate the trajectory planning problem as a standard quadratic programming formulation with collision-free and dynamics constraints. Finally, we apply the iterative convex optimization algorithm to solve the quadratic programming problem in the time-varying convex feasible region. Moreover, we design several typical experimental scenarios and have verified that the proposed method has good effectiveness and real-time.

A Method of Enhancing Rapidly-Exploring Random Tree Robot Path Planning Using Midpoint Interpolation

It is difficult to guarantee optimality using the sampling-based rapidly-exploring random tree (RRT) method. To solve the problem, this paper proposes the post triangular processing of the midpoint interpolation method to minimize the planning time and shorten the path length of the sampling-based algorithm. The proposed method makes a path that is closer to the optimal path and somewhat solves the sharp path problem through the interpolation process. Experiments were conducted to verify the performance of the proposed method. Applying the method proposed in this paper to the RRT algorithm increases the efficiency of optimization by minimizing the planning time.

Creating Better Collision-Free Trajectory for Robot Motion Planning by Linearly Constrained Quadratic Programming

Many algorithms in probabilistic sampling-based motion planning have been proposed to create a path for a robot in an environment with obstacles. Due to the randomness of sampling, they can efficiently compute the collision-free paths made of segments lying in the configuration space with probabilistic completeness. However, this property also makes the trajectories have some unnecessary redundant or jerky motions, which need to be optimized. For most robotics applications, the trajectories should be short, smooth and keep away from obstacles. This paper proposes a new trajectory optimization technique which transforms a polygon collision-free path into a smooth path, and can deal with trajectories which contain various task constraints. The technique removes redundant motions by quadratic programming in the parameter space of trajectory, and converts collision avoidance conditions to linear constraints to ensure absolute safety of trajectories. Furthermore, the technique uses a projection operator to realize the optimization of trajectories which are subject to some hard kinematic constraints, like keeping a glass of water upright or coordinating operation with dual robots. The experimental results proved the feasibility and effectiveness of the proposed method, when it is compared with other trajectory optimization methods.

Optimal Motion Planning in GPS-Denied Environments Using Nonlinear Model Predictive Horizon

Navigating robotic systems autonomously through unknown, dynamic and GPS-denied environments is a challenging task. One requirement of this is a path planner which provides safe trajectories in real-world conditions such as nonlinear vehicle dynamics, real-time computation requirements, complex 3D environments, and moving obstacles. This paper presents a methodological motion planning approach which integrates a novel local path planning approach with a graph-based planner to enable an autonomous vehicle (here a drone) to navigate through GPS-denied subterranean environments. The local path planning approach is based on a recently proposed method by the authors called Nonlinear Model Predictive Horizon (NMPH). The NMPH formulation employs a copy of the plant dynamics model (here a nonlinear system model of the drone) plus a feedback linearization control law to generate feasible, optimal, smooth and collision-free paths while respecting the dynamics of the vehicle, supporting dynamic obstacles and operating in real time. This design is augmented with computationally efficient algorithms for global path planning and dynamic obstacle mapping and avoidance. The overall design is tested in several simulations and a preliminary real flight test in unexplored GPS-denied environments to demonstrate its capabilities and evaluate its performance.

The State of Lifelong Learning in Service Robots:

Service robots are appearing more and more in our daily life. The development of service robots combines multiple fields of research, from object perception to object manipulation. The state-of-the-art continues to improve to make a proper coupling between object perception and manipulation. This coupling is necessary for service robots not only to perform various tasks in a reasonable amount of time but also to continually adapt to new environments and safely interact with non-expert human users. Nowadays, robots are able to recognize various objects, and quickly plan a collision-free trajectory to grasp a target object in predefined settings. Besides, in most of the cases, there is a reliance on large amounts of training data. Therefore, the knowledge of such robots is fixed after the training phase, and any changes in the environment require complicated, time-consuming, and expensive robot re-programming by human experts. Therefore, these approaches are still too rigid for real-life applications in unstructured environments, where a significant portion of the environment is unknown and cannot be directly sensed or controlled. In such environments, no matter how extensive the training data used for batch learning, a robot will always face new objects. Therefore, apart from batch learning, the robot should be able to continually learn about new object categories and grasp affordances from very few training examples on-site. Moreover, apart from robot self-learning, non-expert users could interactively guide the process of experience acquisition by teaching new concepts, or by correcting insufficient or erroneous concepts. In this way, the robot will constantly learn how to help humans in everyday tasks by gaining more and more experiences without the need for re-programming. In this paper, we review a set of previously published works and discuss advances in service robots from object perception to complex object manipulation and shed light on the current challenges and bottlenecks.

A new method on motion planning for mobile robots using jump point search and Bezier curves

Innovative applications in rapidly evolving domains such as robotic navigation and autonomous (driverless) vehicles rely on motion planning systems that meet the shortest path and obstacle avoidance requirements. This article proposes a novel path planning algorithm based on jump point search and Bezier curves. The proposed algorithm consists of two main steps. In the front end, the improved heuristic function based on distance and direction is used to reduce the cost, and the redundant turning points are trimmed. In the back end, a novel trajectory generation method based on Bezier curves and a straight line is proposed. Our experimental results indicate that the proposed algorithm provides a complete motion planning solution from the front end to the back end, which can realize an optimal trajectory from the initial point to the target point used for robot navigation.

Trajectory Planner CDT-RRT* for Car-Like Mobile Robots toward Narrow and Cluttered Environments

In order to achieve the safe and efficient navigation of mobile robots, it is essential to consider both the environmental geometry and kinodynamic constraints of robots. We propose a trajectory planner for car-like robots on the basis of the Dual-Tree RRT (DT-RRT). DT-RRT utilizes two tree structures in order to generate fast-growing trajectories under the kinodynamic constraints of robots. A local trajectory generator has been newly designed for car-like robots. The proposed scheme of searching a parent node enables the efficient generation of safe trajectories in cluttered environments. The presented simulation results clearly show the usefulness and the advantage of the proposed trajectory planner in various environments.

Bounded Cost Path Planning for Underwater Vehicles Assisted by a Time-Invariant Partitioned Flow Field Model

A bounded cost path planning method is developed for underwater vehicles assisted by a data-driven flow modeling method. The modeled flow field is partitioned as a set of cells of piece-wise constant flow speed. A flow partition algorithm and a parameter estimation algorithm are proposed to learn the flow field structure and parameters with justified convergence. A bounded cost path planning algorithm is developed taking advantage of the partitioned flow model. An extended potential search method is proposed to determine the sequence of partitions that the optimal path crosses. The optimal path within each partition is then determined by solving a constrained optimization problem. Theoretical justification is provided for the proposed extended potential search method generating the optimal solution. The path planned has the highest probability to satisfy the bounded cost constraint. The performance of the algorithms is demonstrated with experimental and simulation results, which show that the proposed method is more computationally efficient than some of the existing methods.