A method for optimizing spatiotemporal trajectory comprehensive performance of mobile manipulator based on improved NSGA-III

Abstract

The application belongs to the field of robot motion planning and trajectory optimization, and discloses a mobile manipulator space-time trajectory comprehensive performance optimization method based on improved NSGA-III. The method comprises the following steps: step 1, mobile manipulator initial path generation; step 2, trajectory space-time parameterization representation; step 3, multi-objective function establishment, adding specific task constraints, in order to generate smooth, safe and dynamic constraint trajectories for the mobile manipulator to complete specific tasks; step 4, improved NSGA-II algorithm is used to realize trajectory optimization. The method can realize efficient optimization by directly controlling the road point and time through space-time trajectory representation method, reduce the calculation cost in the optimization process, and overcome the difficulty in seeking the optimal solution caused by nonlinearity by using the intelligent optimization algorithm for the multiple targets of obstacle avoidance, path smoothness, dynamic constraint and time optimization of the high-degree-of-freedom mobile manipulator.

Description

Technical Field

[0001] This invention belongs to the field of robot motion planning and trajectory optimization, and particularly relates to a method for optimizing the spatiotemporal trajectory of a mobile robotic arm based on an improved NSGA-III. Background Technology

[0002] With the development of automation and intelligent manufacturing, efficient operation of mobile robotic arms in complex and ever-changing working environments has become a critical requirement. When performing tasks, these robotic arms not only face single objectives such as finding the shortest path and fastest arrival time, but also need to comprehensively consider multiple objectives such as obstacle avoidance, path smoothness, vibration reduction, and energy minimization to ensure overall operational optimization. However, traditional path planning and trajectory optimization methods often focus only on one or a few objectives, neglecting the balance and improvement of overall performance. Due to the numerous uncertainties and constraints in real-world environments, traditional optimization algorithms are prone to getting trapped in local optima, failing to solve the problem globally. Local optima can lead to low path efficiency, increased operating costs, and even, in some cases, failure to meet safety and operational requirements. Furthermore, because traditional trajectory representation methods create a high degree of coupling between time and space during path planning, spatiotemporal joint optimization is impossible, resulting in a trade-off between achieving both spatiotemporal optima and optimal optimization.

[0003] Currently, robot motion planning methods are broadly categorized into three types: search-based methods, sampling-based methods, and optimization-based methods. Search-based methods include Dijkstra's algorithm, A* algorithm, D Lite algorithm, and depth-first search (DFS). These algorithms can guarantee finding the shortest path and are adaptable to dynamic, complex, and non-convex environments. However, as dimensionality increases, the search space explodes, and the dimensionality of the joint configuration space grows exponentially with the number of robots. Sampling-based methods include RRT*, bidirectional RRT, RRT-Connect, and Dynamic-RRT. These algorithms can effectively generate trajectories in high-dimensional and complex environments, but they cannot guarantee the completeness and optimality of the solution within a finite time. Optimization-based methods include traditional numerical optimization methods (such as Chomp, Stomp, AGD, and Adam) and intelligent optimization methods (such as genetic algorithms, particle swarm optimization, and ant colony optimization). These methods can obtain feasible trajectories under high-dimensional dynamic constraints, but they often get stuck in local optima, especially in low-dimensional environments and with complex obstacle shapes, making it difficult to find the optimal solution. For multi-objective optimization, due to its high problem and computational complexity, existing optimization algorithms often solve problems efficiently but frequently get trapped in local minima. This means that superior performance in one class does not guarantee superior performance in other classes. Commonly used continuous-time trajectory representation methods include polynomial spline methods, B-spline methods, and Bézier curves. However, these methods have high coupling in the time domain optimization, and some require the use of control points and cannot be directly optimized using control path points. Summary of the Invention

[0004] The purpose of this invention is to provide a method for optimizing the spatiotemporal trajectory of a mobile robotic arm based on an improved NSGA-III, in order to solve the technical problem that the optimization process is prone to getting trapped in local minima due to the complexity and nonlinearity of multi-objective functions.

[0005] To address the aforementioned technical problems, the specific technical solution of this invention, a method for optimizing the spatiotemporal trajectory of a mobile robotic arm based on an improved NSGA-III, is as follows:

[0006] A method for optimizing the spatiotemporal trajectory of a mobile robotic arm based on an improved NSGA-III includes the following steps:

[0007] Step 1: Initial path generation for the mobile robotic arm: In a given obstacle environment, use the A* algorithm to search for a given obstacle avoidance path from the starting point to the ending point, which will serve as the initial trajectory for subsequent trajectory optimization. This trajectory will only generate multiple nodes and will contain spatial information but not temporal information.

[0008] Step 2: Spatial-Temporal Parametric Representation of Trajectory: The initial path points generated by the A* algorithm are simplified and classified according to their distance from obstacles and the number of the nearest obstacle. These simplified trajectory points are then represented using the MINCO parameter method.

[0009] Step 3: Establish a multi-objective function and add specific task constraints in order to generate a smooth, safe trajectory that satisfies the dynamic constraints for the mobile robotic arm to complete a specific task;

[0010] Step 4: Improve the NSGA-II algorithm to achieve trajectory optimization. Establish the objective function and use the improved NSGA-III algorithm for optimization to obtain the global optimal solution of the function. This yields the optimized trajectory, which is the trajectory with the best overall performance in terms of obstacle avoidance, dynamic constraints, smoothing, terminal constraints, and time.

[0011] Furthermore, the A* algorithm in step 1 is a heuristic search algorithm that is implemented by estimating a function. The robot needs to calculate the value of this function for each step it takes, and the node with the smallest function value is the next step for the robotic arm.

[0012] The general expression for the heuristic function to determine the desired location is as follows:

[0013] F(n)=g(n)+h(n) (1)

[0014] Where g(n) is the actual cost incurred by the robot from the initial node of the path to node b in the path planning; since the robotic arm knows the actual time spent and the actual distance traveled from the initial node to node n, the value of function g(n) is the actual value; h(n) is the estimated cost incurred from the current node n to the end point of the path planning; the function F(n) is the total cost in the entire path planning, and the estimated function g(n) is the distance between the end position of the robotic arm and the target node, i.e.

[0015]

[0016] Furthermore, in step 2, the number of trajectory segments is set to 5, and 4 points are selected from the trajectory excluding the starting point and the target point.

[0017] Choose generalized co-configuration As a planned space, q x ,q y These represent the positions of the robot's mobile chassis. These represent the angles of the six joints of the robotic arm;

[0018] The trajectory is divided into 5 segments, each represented as a polynomial of degree 2s-1. Therefore, the coefficients of the polynomial for each trajectory segment are represented as a 2s*9 matrix c. i Assume the duration of each trajectory segment is T. i Then the trajectory of this segment is represented as p. i (t), the entire trajectory can be represented by the following piecewise function:

[0019]

[0020] Where β(t) = [1, t, ..., t 2s-1 The expression obtained by further abstraction is as follows:

[0021] For the abstract representation of the entire trajectory, c is determined by the intermediate path point q and the duration T. According to the definition of the MINCO trajectory, it is found that the coefficient matrix c can be optimized by optimizing the path points and time, thereby optimizing the entire trajectory.

[0022] Furthermore, if step 2 is to obtain the configuration information of a path point in the trajectory, it is calculated using the following formula.

[0023] get:

[0024]

[0025] Where, q i,j T represents the path point information at time j of the i-th segment. i,j Let represent the duration at time j of the i-th segment. MINCO spatiotemporal trajectory representation is used to achieve joint spatiotemporal optimization.

[0026] Furthermore, step 3 formulates the objective function of the trajectory generation problem as follows:

[0027]

[0028] Among them, J s,i J represents the smoothing cost of the i-th trajectory segment, obtained by integrating the s-th derivative of the i-th trajectory segment with respect to q. T,i J represents the time cost of the i-th segment of the trajectory. This term ensures that the execution time of the trajectory is minimized. p,i This represents the additional penalty term for the i-th trajectory segment. This term is customized according to the task and takes into account obstacle avoidance, dynamic constraints, and end-effector pose maintenance for a specific trajectory. M i This indicates that the i-th segment has been divided into M segments. i This is a small segment, a fine-grained trajectory discretization operation, I Σ (·) represents sparse and dense sampling strategies, ω T ωd The time and the weight of the additional penalty term are represented by d and D. The specific meanings of d and D depend on the specific additional penalty term. The MINCO trajectory optimization solution is obtained by solving equation (6).

[0029] Furthermore, the improved NSGA-III algorithm in step 4 uses adaptive crossover and mutation probabilities based on Gaussian distribution to improve the efficiency of the algorithm in searching for solutions and simulating population evolution. The adaptive crossover probability p c and adaptive mutation probability p m as follows:

[0030]

[0031] in, These represent the minimum and maximum crossover probabilities. and These represent the minimum and maximum probabilities of mutation, gen represents the current population size, and generation represents the total number of individuals in the population.

[0032] The improved NSGA-III algorithm performs random crossover at the crossover points. In addition to random crossover, the positions of gene segments in individuals are exchanged during the crossover process, and the communication mechanism between individuals is enhanced. In each generation, the algorithm retains a portion of excellent individuals, which are still used in the next generation to ensure that excellent genetic traits are preserved. In this way, the objective function can escape local minima and obtain the global optimum.

[0033] This invention presents a method for optimizing the spatiotemporal trajectory of a mobile robotic arm based on an improved NSGA-III algorithm. This method aims to achieve joint optimization of multiple objectives, including obstacle avoidance, smoothness, dynamic safety constraints, task constraints, and time optimization. It employs MINCO trajectory representation to reduce the complexity of the trajectory optimization problem and effectively escapes local minima through the improved NSGA-III algorithm, significantly improving the overall operational efficiency of the mobile robot. This project utilizes the Minimum Control Quantity Spatiotemporal Trajectory Representation (MINCO) method and the improved Non-Dominated Sorting Genetic Algorithm III (NSGA-III) trajectory optimization method. It efficiently generates the initial trajectory using A*, providing a better starting point for subsequent optimization processes. Based on this, a multi-objective function model incorporating obstacle avoidance, smoothness, dynamic safety constraints, task constraints, and time is constructed and combined with intelligent optimization methods to further optimize the trajectory planning effect. This planning method uses the novel trajectory parameterization method MINCO, describing the motion path of a 9-DOF mobile robotic arm with waypoints and time as parameters. It precisely controls the position and attitude of the robotic arm at different time points, thereby better adapting to complex task requirements. To overcome the problem of traditional optimization algorithms easily getting trapped in local optima, NSGA-III was improved, enhancing its exploration capabilities and enabling it to find the global optimum more effectively in the search space, thus improving the success rate of solving multi-objective optimization problems for mobile robotic arms. Addressing the slow performance optimization speed for multiple metrics under redundant degrees of freedom, MINCO trajectory representation was adopted, using linear complexity to perform gradient iteration for multi-objective optimization, resulting in rapid optimization and a comprehensive performance improvement for the mobile robotic arm.

[0034] In summary, the method of this invention addresses multiple objectives of high-degree-of-freedom mobile robotic arms, such as obstacle avoidance, path smoothness, dynamic constraints, and time optimization. By employing a spatiotemporal trajectory representation method, it can reduce computational costs during the optimization process through direct control of waypoints and time, achieving efficient optimization. Furthermore, it utilizes an intelligent optimization algorithm to overcome the difficulty in finding the optimal solution caused by nonlinearity. Attached Figure Description

[0035] Figure 1a A schematic diagram of kinematic modeling for a mobile robotic arm;

[0036] Figure 1b A schematic diagram of kinematic modeling for a mobile robotic arm;

[0037] Figure 2 A flowchart for improving the NSGA-III algorithm;

[0038] Figure 3 A schematic diagram of the framework for optimizing the spatiotemporal trajectory of a mobile robotic arm;

[0039] Figure 4a This is a schematic diagram of a mobile robotic arm in operation.

[0040] Figure 4b This is a schematic diagram of the working trajectory of a mobile robotic arm. Detailed Implementation

[0041] To better understand the purpose, structure, and function of this invention, the following description, in conjunction with the accompanying drawings, provides a more detailed explanation of a spatiotemporal trajectory comprehensive performance optimization method for a mobile robotic arm based on an improved NSGA-III.

[0042] This invention presents a spatiotemporal trajectory optimization method for a mobile robotic arm based on an improved NSGA-III. Its greatest feature and innovation lies in its ability to simultaneously optimize time while ensuring obstacle avoidance, smoothness, and dynamic safety constraints, thus solving the problem of easily getting trapped in local minima during optimization for complex and nonlinear multi-objective functions. The design process of this planning method mainly includes the following steps:

[0043] Step 1: Initial Path Generation for the Mobile Robotic Arm. In a given obstacle environment, the A* algorithm is used to search for a given obstacle avoidance path from the starting point to the ending point, which serves as the initial trajectory for subsequent trajectory optimization. This trajectory generates only multiple nodes and includes spatial information but not temporal information.

[0044] The A* algorithm is a heuristic search algorithm that works by estimating a function. For each step the robot takes, it calculates the value of this function, and the node with the smallest function value is the position the robot arm needs to reach next. The general expression for the heuristic function is as follows:

[0045] F(n)=g(n)+h(n) (1)

[0046] Where g(n) is the actual cost incurred by the robot from the initial node of the path to node b in the path planning; since the robotic arm knows the actual time spent and the actual distance traveled from the initial node to node n, the value of function g(n) is the actual value; h(n) is the estimated cost incurred from the current node n to the end point of the path planning; and the function F(n) is the total cost in the entire path planning. The estimated function g(n) is the distance between the end position of the robotic arm and the target node, i.e.

[0047]

[0048] Step 2: Spatiotemporal parameterization of the trajectory. The initial path points generated by the A* algorithm are simplified and classified according to their distance from obstacles and the number of the nearest obstacle. Since this invention sets the number of trajectory segments to 5, 4 points are selected from the trajectory excluding the starting point and the target point. These simplified trajectory points are represented using the MINCO parameter method.

[0049] Choose generalized co-configuration As planned space. Among them q x ,q y These represent the positions of the robot's mobile chassis. These represent the angles of the six joints of the robotic arm.

[0050] The trajectory is divided into 5 segments, each represented as a polynomial of degree 2s-1. Therefore, the coefficients of the polynomial for each trajectory segment are represented as a 2s*9 matrix c. i Assume the duration of each trajectory segment is T. i Then the trajectory of this segment is represented as p. i The entire trajectory (t) can be represented by the following piecewise function:

[0051]

[0052] Where β(t) = [1, t, ..., t 2s-1 The expression obtained by further abstraction is as follows:

[0053] For an abstract representation of the entire trajectory, c is determined by the intermediate path point q and the duration T. According to the definition of the MINCO trajectory, it can be found that by optimizing the path points and time to optimize the coefficient matrix c, the entire trajectory can be optimized. Compared with Bézier curves, this trajectory representation method can intuitively optimize continuous variables such as path points and time.

[0054] To obtain the configuration information of a path point in the trajectory, it can be easily calculated using the following formula:

[0055]

[0056] Where, q i,j T represents the path point information at time j of the i-th segment. i,j Let represent the duration at time j of the i-th segment. Using MINCO spatiotemporal trajectory representation, joint spatiotemporal optimization can be achieved, and this trajectory representation method can maintain local smoothness of the trajectory through linear complexity operations.

[0057] Step 3: Establishing a multi-objective function and adding specific task constraints. To generate smooth, safe trajectories that satisfy dynamic constraints for the mobile robotic arm to complete specific tasks, we formulate the objective function for the trajectory generation problem as follows:

[0058]

[0059] J T,i =ω T T i(8)

[0060]

[0061] Among them, J s,i Let J represent the smoothing cost of the i-th trajectory segment, obtained by integrating the s-th derivative of the i-th trajectory segment with respect to q. T,i J represents the time cost of the i-th segment of the trajectory, ensuring that the execution time of the trajectory is minimized. p,i This represents an additional penalty term for the i-th trajectory segment. This term can be customized according to the task. In this invention, this term considers obstacle avoidance, dynamic constraints, and maintaining a specific trajectory for the end-effector pose. M i This indicates that the i-th segment has been divided into M segments. i This small segment is a fine-grained trajectory discretization operation, which is beneficial for more precise iteration of trajectory cost, so as to avoid skipping the global minimum. Σ (·) indicates sparse and dense sampling strategies. ω T ω d The time and the weights of the additional penalty terms are represented by d and D, respectively. The specific meanings of d and D depend on the specific additional penalty terms. The MINCO trajectory optimization solution can be obtained by solving equation (6).

[0062] Step 4: Improve the NSGA-II algorithm to achieve trajectory optimization. After establishing the objective function described above, use the improved NSGA-III algorithm for optimization to obtain the global optimal solution of the function. This yields the optimized trajectory, which is the trajectory with the best overall performance in terms of obstacle avoidance, dynamic constraints, smoothing, terminal constraints, and time.

[0063] An improved NSGA-III algorithm is proposed, employing adaptive crossover and mutation probabilities based on a Gaussian distribution to enhance its efficiency in both solution searching and simulating population evolution. The adaptive crossover probability p... c and adaptive mutation probability p m as follows:

[0064]

[0065] in, These represent the minimum and maximum crossover probabilities. and These represent the minimum and maximum probabilities of mutation. `gen` represents the current population size, and `generation` represents the total number of individuals in the population.

[0066] The improved NSGA-III algorithm uses random crossover at the crossover points. In addition to random crossover, gene segments are swapped within individuals during the crossover process. This method helps preserve certain individual traits while introducing new genetic variations. Furthermore, it enhances communication mechanisms between individuals. In each generation, the algorithm retains a subset of superior individuals, which are reused in the next generation to ensure the preservation of excellent genetic traits. In this way, the objective function escapes local minima and arrives at the global optimum.

[0067] Example:

[0068] refer to Figure 1a , Figure 1b First, the mobile robotic arm is modeled to facilitate the establishment of the objective function. The mobile robotic arm is approximated as a set of colliding spheres. l p l,m Let represent the center position of the m-th sphere in link l, where l ∈ {0, 1, 2, 3, 4, 5, 6}. When l is 0, it refers to the chassis of the moving robotic arm. These spheres are in the world coordinate system. The center of the movement can be obtained through the forward kinematics of the mobile manipulator:

[0069]

[0070] Where m∈{1,…,m l}, m l This indicates the number of spheres covered by link l. This represents the distance from link l to the world coordinate system. Homogeneous transformation. Using r l This represents the radius of the sphere covering the connecting rod.

[0071] Then, regarding the additional penalty items mentioned above, this patent mainly divides them into three parts based on the actual problem: obstacle avoidance and self-obstacle avoidance, dynamic feasibility constraints, and task constraints that minimize the deviation between the end position and the target position.

[0072] Obstacle avoidance and collision avoidance are included as additional penalty terms. To ensure no collisions occur between obstacles and the robot, we constructed an ESDG graph and defined the ESDG value of the colliding sphere as D. ESDF (·)

[0073] To avoid colliding with obstacles, D is needed. ESDF (·) is greater than the radius r of each sphere. l :

[0074]

[0075] Where, l∈{0,1,2,3,4,5,6}, m∈{1,…,m} l}

[0076] To avoid self-collisions, the following inequality constraint is defined:

[0077]

[0078] Where, l′∈{0,…,l-1}, l∈{0,1,2,3,4,5,6}, i∈{1,…,m} l′},j∈{1,…,m l}

[0079] Dynamic feasibility constraints are added as an additional penalty. These constraints can be divided into feasibility constraints for the moving chassis and feasibility constraints for the robotic arm joints. For the moving chassis, the angular velocities and accelerations of the left and right wheels are limited as follows:

[0080]

[0081] Where, ω l(r) α l(r) These represent the angular velocity and acceleration of the left and right wheels of the moving chassis, respectively. The following constraints are applied to the angle, velocity, and acceleration of each joint of the robotic arm:

[0082] g θ (q l ) max =q l -q l,max (17)

[0083] g θ (q l ) min =q l,min -q l (18)

[0084]

[0085] Where, q l q represents the angle of link k. l,max and q l,min These are the maximum and minimum allowable angles of the connecting rod l, ω l,max and α l,max It is the maximum allowable angular velocity and angular acceleration of the connecting rod l.

[0086] The task constraint of minimizing the deviation between the endpoint position and the target position is used as the last additional penalty term. The objective function formula is as follows:

[0087]

[0088] in, It is the position of the end effector. It is the end position of the end effector, d p These are the allowable deviation values ​​for the first two.

[0089] Figure 2 The optimization flowchart of the improved NSGA-III is recorded and presented. As can be seen from the flowchart, this invention focuses more on improving the intersection method, which can increase the exploratory nature and thus escape local minima. A flowchart of a spatiotemporal trajectory optimization method for a mobile robotic arm based on the improved NSGA-III is shown below. Figure 3 As shown, it mainly includes an A* initial trajectory generation module, an initial trajectory simplification module, a spatiotemporal trajectory MINCO representation trajectory module, a multi-objective function construction module, and an improved NSGA-III optimization module. The robot trajectory planning approach is relatively clear, straightforward, and easy to implement. Figure 4a , Figure 4b This is a schematic diagram of the working scenario and trajectory of a mobile robotic arm.

[0090] It is understood that the present invention has been described through some embodiments, and those skilled in the art will recognize that various changes or equivalent substitutions can be made to these features and embodiments without departing from the spirit and scope of the invention. Furthermore, under the teachings of the present invention, these features and embodiments can be modified to adapt to specific situations and materials without departing from the spirit and scope of the invention. Therefore, the present invention is not limited to the specific embodiments disclosed herein, and all embodiments falling within the scope of the claims of this application are within the protection scope of the present invention.

Claims

1. A method for optimizing the spatiotemporal trajectory of a mobile robotic arm based on an improved NSGA-III, characterized in that, Includes the following steps: Step 1: Initial path generation for the mobile robotic arm: In a given obstacle environment, use the A* algorithm to search for a given obstacle avoidance path from the starting point to the ending point, which will serve as the initial trajectory for subsequent trajectory optimization. This trajectory will only generate multiple nodes and will contain spatial information but not temporal information. Step 2: Spatial-Temporal Parametric Representation of Trajectory: The initial trajectory points generated by the A* algorithm are simplified and classified according to their distance from obstacles and the number of the nearest obstacle. The simplified trajectory points are then represented using the MINCO parameter method. Set the number of trajectory segments to 5, and filter out 4 points from the trajectory excluding the starting point and the target point; Choose generalized co-configuration As a planned space, q x ,q y These represent the positions of the mobile chassis of the robotic arm. These represent the angles of the six joints of the robotic arm; Each trajectory segment is represented as a polynomial of degree 2s⁻¹, therefore the coefficients of the polynomial for each trajectory are represented as a 2s*9 matrix c. i Assume the duration of each trajectory segment is T. i Then the trajectory segment is represented as p. i (t), the entire trajectory can be represented by the following piecewise function: Where β(t) = [1, t, ..., t 2s-1 The expression obtained by further abstraction is as follows: For an abstract representation of the entire trajectory, c is determined by the intermediate path point q and the duration T; Step 3: Establishing a multi-objective function; The multi-objective function for the trajectory generation problem is formulated as follows: J T,i =ω T T i (8) Among them, J s,i J represents the smoothing cost of the i-th trajectory segment, obtained by integrating the s-th derivative of the i-th trajectory segment with respect to q. T,i J represents the time cost of the i-th segment of the trajectory. p,i M represents the additional penalty term for the i-th trajectory segment, which considers obstacle avoidance, dynamic constraints, and end-effector pose maintenance for a specific trajectory. i This indicates that the i-th trajectory is divided into M... i A short segment, I Σ (·) represents sparse and dense sampling strategies, ω T ω d The time and the weight of the additional penalty term are represented by d and D. The specific meanings of d and D are related to the additional penalty term. The MINCO trajectory optimization solution is obtained by solving equation (6). Step 4: Improve the NSGA-III algorithm to achieve trajectory optimization; Improved adaptive crossover probability p of NSGA-III algorithm c and adaptive mutation probability p m as follows: in, These represent the minimum and maximum crossover probabilities. and The minimum and maximum values ​​of the mutation probability are represented by gen, where gen represents the current population size, and generation represents the total number of individuals in the population. The improved NSGA-III algorithm performs random crossover at the crossover points. In addition to random crossover, the positions of gene segments in individuals are exchanged during the crossover process, and the communication mechanism between individuals is enhanced. In each generation, the algorithm retains a portion of the best individuals, which are still used in the next generation to ensure that excellent genetic traits are preserved. In this way, the objective function can escape local minima and obtain the global optimum.

2. The method for optimizing the spatiotemporal trajectory of a mobile robotic arm based on the improved NSGA-III according to claim 1, characterized in that, The A* algorithm in step 1 is a heuristic search algorithm that works by estimating a function. The value of this function needs to be calculated for each step the robotic arm takes, and the node with the smallest function value is the position the robotic arm needs to reach next. The expression for the heuristic function is as follows: F(n) = g(n) + h(n) (1) where g(n) is the actual cost incurred by the robot from the initial node of the path to node b in the path planning; since the robotic arm knows the actual time spent and the actual distance traveled from the initial node to node n, the value of function g(n) is the actual value; h(n) is the estimated cost incurred from the current node n to the end point of the path planning; function F(n) is the total cost in the entire path planning, and the estimated function g(n) is the distance between the end position of the robotic arm and the target node, i.e.

Images