Limited angle six-degree-of-freedom mechanical arm low-energy-consumption path planning method based on improved RRT* algorithm

Abstract

The invention provides a limited angle six-degree-of-freedom mechanical arm low-energy-consumption path planning method based on an improved RRT * algorithm, and the method is based on the improvement of the RRT * algorithm, and comprises the following steps: S1, completing the setting of working space parameters, the initialization of a data structure and the construction of a 3D graphic environment; s2, random sampling points are generated through a target point sampling strategy, and joint angle constraint check is carried out; s3, expanding new nodes in the random tree, and adding the new nodes into the random tree after obstacle detection; s4, calculating the energy consumption of a new node by using an LQR controller, calculating the Euclidean distance through a formula, and obtaining a heuristic value through a heuristic function; s5, storing the new node and the heuristic value thereof into a priority queue realized by a minimum heap; s6, selecting a node with the minimum heuristic value from the priority queue, and optimizing a path, including updating a father node and performing reconnection operation; s7, judging whether to stop or not according to the distance from the new node to the end point and the maximum number of iterations; and S8, when a termination condition is satisfied, backtracking to generate a complete path.

Description

Technical Field

[0001] The present invention relates to the technical field of robot path planning, and in particular to a low-energy consumption path planning method for a limited-angle six-degree-of-freedom robot arm based on an improved RRT* algorithm. Background Art

[0002] Traditional robot path planning is of great significance in the fields of industrial automation, service robots, etc. Traditional path planning algorithms have problems such as poor path quality, low computational efficiency, and high energy consumption when facing complex environments and high-degree-of-freedom robot arms. As an asymptotically optimal sampling algorithm, the RRT* algorithm can effectively plan paths in complex environments, but it still has some limitations in robot path planning.

[0003] The original RRT* algorithm has poor path planning quality and is difficult to obtain the optimal or near-optimal path, causing the robot's motion path to be circuitous and lengthy, affecting work efficiency; the computational efficiency is low. Faced with the huge path planning calculation amount of a high-degree-of-freedom robot in a complex environment, its search strategy is inefficient and needs to traverse a large number of unnecessary nodes, resulting in a long calculation time and being unable to meet real-time requirements; the energy consumption is large and the energy consumption of the robot's motion is not fully considered. The planned path may cause the robot to consume too much energy, increase operating costs and reduce endurance; it is not flexible enough in dealing with the robot's joint angle restrictions. The original RRT* algorithm is prone to generate nodes that exceed the joint angle restrictions during sampling and node expansion, resulting in the inability to execute the planned path, affecting feasibility and reliability; at the same time, the original RRT* algorithm does not optimize energy consumption enough, does not take it as a key optimization target or the optimization method is poor, making it difficult to meet the actual needs of energy saving for the robot. Summary of the invention

[0004] The purpose of the present invention is to solve the above-mentioned problems, and a low-energy consumption path planning method for a constrained angle six-degree-of-freedom robotic arm based on an improved RRT* algorithm is proposed. On the basis of the RRT* algorithm, an LQR controller is added, and constrained angle constraints, target point sampling, optimized energy consumption and priority queues are introduced. It can significantly reduce the energy consumption of the path, improve the operating efficiency of the robotic arm, speed up the path planning, reduce the search time of the algorithm, and improve the efficiency and reliability of path planning.

[0005] In order to achieve the above purpose, the following technical solutions are adopted: A low-energy consumption path planning method for a limited-angle six-degree-of-freedom manipulator based on an improved RRT* algorithm. The path planning of the improved RRT* algorithm includes the following steps: S1: Initialize the workspace and parameters; define the boundary of the robot workspace and the robot DH parameters and determine the starting point S and the end point G, set the number of sampling points N, the step length s and the angle limit J of the robot joint as basic parameters, use the maximum number of iterations M as the termination condition, initialize the data structure, and use the Matplotlib library to create a 3D graphics environment; The range of the joint angle limit J is: joint 1 [-π, π], joint 2 [- , ], joint three [-π, π], joint four [- , ], joint five [-π,π], joint six [- , ]; The data structure includes a random tree T, a priority queue H and an LQR controller; The robot arm DH parameters include the joint angle θ, joint offset d, connecting rod length a and connecting rod torsion angle α of each connecting rod; S2: Generate random sampling points Pr; introduce target point sampling, generate sampling points Pr, perform constraint checks on sampling points Pr and obtain the spatial coordinates of sampling points Pr; The constraint check includes joint angle limit constraints; S3: Path planning and node expansion; Find the node Pn closest to the sampling point Pr in the random tree T, expand from the nearest point Pn along the direction of the sampling point Pr and generate a new node Pnew. After the new node Pnew is generated, detect obstacles between the new node Pnew and the nearest point Pn. If there are obstacles, abandon Pnew and regenerate a random sampling point Pr. If not, add Pnew to the random tree T to obtain the current state x of the new node Pnew, and then dynamically update the image display. S4: LQR controller optimizes energy consumption; LQR controller is used to calculate the energy consumption e of the new node Pnew and directly calculate the Euclidean distance d between the new node Pnew and the end point G according to the formula, and the heuristic value h is calculated by the heuristic function according to the energy consumption e and the Euclidean distance d; The heuristic function is a function that maps the relevant information of the current node into a numerical value, which is used to balance the path length and energy consumption, wherein the weight calculation of the energy consumption e and the Euclidean distance d adopts a dynamic normalization method; S5: Priority queue management node; add the new node Pnew added to the random tree T and its heuristic value h to the priority queue H; S6: Path optimization; extract the new node Pnew with the smallest heuristic value from the priority queue H, set a range with the new node Pnew as the center, traverse other nodes within the range and calculate the new path cost new_cost from the new node Pnew to these nearby nodes. If the new path cost is less than the path cost of the original node, update the parent node of these nodes to the new node Pnew, otherwise continue traversing, and then reconnect the affected nodes P around. The reconnection operation is to check whether the distance from the affected point to the starting point S can be reduced through the new node, and update the distance information from the relevant node to the starting point; S7: Determine the termination condition; if the termination condition is met, stop the algorithm; if not, execute steps S2 to S7 in a loop; The termination condition includes the distance from the new node Pnew to the end point G and the maximum number of iterations M; S8: Generate a path; when the termination condition is met, trace back from the end point G to the starting point S according to the parent-child relationship of the node P in the random tree T to generate a complete path.

[0006] Preferably, in step S1, initialization of the LQR controller includes initializing parameters, calculating a P matrix, and calculating a gain matrix K, wherein the parameters include a system state matrix A, a control matrix B, a state cost matrix Q, a control cost matrix R, and a time step dt.

[0007] Preferably, based on the initialization parameters, the scipy.linalg.solve_continuous_are function is used to solve the continuous-time algebraic Riccati equation to obtain the P matrix, and then the P matrix and the control matrix B are used to calculate the LQR gain matrix K, and the calculation formula is:

[0008] Where K is the gain matrix K, R−1 is the inverse matrix of the control cost matrix R, BT is the transposed matrix of the control matrix B, and P is the P matrix.

[0009] Preferably, in step S2, the target point sampling includes setting a fixed probability p, the probability p is set to 0.1, and when generating the sampling point Pr, first generating a random number r in the interval [0,1]. If r≤p, the end point G is directly sampled as the target point. If r>p, the sampling point Pr is randomly generated in the state space of the robot arm.

[0010] Preferably, in step S2, by comparing the joint angle corresponding to the sampling point Pr with the preset joint angle limit J, it is determined whether the range of the joint angle limit J is met, and if it exceeds the range interval, the sampling point Pr is regenerated.

[0011] Preferably, in step S3, the 3D image displays the drawn content including: obstacles, starting points, target points, extended paths and robotic arms, and the drawn content is updated in each iteration.

[0012] Preferably, in step S4, the LQR controller calculates the control input u using the gain matrix K calculated by initialization in step S1 and the current state x of the new node Pnew obtained in step S3. The calculation formula of the control input u is:

[0013] Among them, u is the control input, K is the gain matrix K, and x is the current state of the new node Pnew; The LQR controller calculates the energy loss e according to the obtained control input u and the time step dt initialized in step S1. The calculation formula of the energy loss e is:

[0014] Where e is the energy consumption, dt is the time step, u1 and u2 represent the control inputs of the starting state x1 and the target state x2, respectively, that is, the process of transferring from the current state to the next state; The Euclidean distance d is calculated based on the coordinates of the new node Pnew and the coordinates of the end point G. The calculation formula of the Euclidean distance d is:

[0015] Among them, d is the Euclidean distance, x Pnew ,y Pnew 、z Pnew are the spatial coordinates of the new node Pnew, x G ,y G and z G are the spatial coordinates of the end point G respectively.

[0016] Preferably, the LQR controller calculates the weight coefficient by a normalization method, and uses minimum and maximum value normalization to normalize the distance and energy consumption to between 0 and 1. The calculation formula of the normalization method is:

[0017] Among them, d normalized and e normalized are the normalized Euclidean distance and energy consumption, min_d and max_d are the minimum and maximum distances observed so far, min_e and max_e are the minimum and maximum energy consumption observed so far, and the role of 1e - 6 is to prevent the denominator from being 0 and unable to be calculated in the actual program; Then, according to the normalized d normalized and enormalized Dynamically calculate weights. The weight calculation formula is:

[0018] Among them, d_weight is the distance weight, e_weight is the energy weight, d normalized and e normalized They are the normalized Euclidean distance and energy consumption respectively. The role of 1e - 6 is to prevent the denominator from being 0 and unable to be calculated in the actual program. The LQR controller calculates the heuristic value h through the heuristic function according to the calculated energy consumption e and the Euclidean distance d. The final heuristic value h is the weighted sum of the distance and the energy consumption. The formula in the heuristic function is:

[0019] Among them, d_weight and e_weight are weight coefficients, h is the heuristic value, d is the Euclidean distance, and e is the energy consumption.

[0020] Preferably, in step S5, the priority queue H is implemented using a minimum heap data structure and is sorted from small to large according to the heuristic value h, to ensure that the node extracted from the queue each time is the node with the smallest heuristic value h.

[0021] Preferably, in step S6, for the traversed nodes P, the path cost to reach them through the new node Pnew is recalculated and compared with the original path cost. If the new path cost is lower, the distance information from the relevant nodes to the starting point is updated, and the parent nodes of these nodes are updated to Pnew.

[0022] Compared with the prior art, the present invention has the following beneficial effects: The RRT* algorithm is combined with the LQR controller, and the global planning ability of the RRT* algorithm and the local optimal control ability of the LQR controller are utilized to realize efficient path planning of the robot arm. Through the combination of the two, while ensuring the feasibility of the path, the energy consumption of the path can be significantly reduced, and the operation efficiency of the robot arm can be improved.

[0023] During the path planning process, the joint angles of the robot are constrained. When sampling, ensure that the sampled joint angles are within the physical limitations of the robot. When expanding the nodes, correct the angles that exceed the limits to satisfy the restricted angle constraints. This can effectively prevent the joint angles of the robot from exceeding the physical limits, ensure the safe operation of the robot, and improve the reliability of path planning.

[0024] In the sampling process of the RRT* algorithm, a target point sampling strategy is introduced to directly sample the target point with a certain probability, increasing the bias towards the target point in the path planning process, thereby increasing the probability of finding a feasible path, speeding up the path planning process, reducing the algorithm's search time, and improving the efficiency of path planning.

[0025] The LQR controller is used to calculate the optimal control input for each path segment, and the energy consumption of the path is calculated based on the control input. During the path planning process, energy consumption is used as an important basis for path selection, and path segments with lower energy consumption are given priority. This can effectively reduce the energy consumption during the operation of the robot arm and improve the endurance and operation economy of the robot arm.

[0026] The LQR controller uses dynamic normalized weights to calculate heuristic values, so that the algorithm can dynamically adjust the weights according to the distance and energy consumption of the current node during path planning, thereby better balancing the two factors of distance and energy consumption. In different work scenarios and task stages, it can adaptively optimize the path planning strategy and further improve the quality and efficiency of path planning.

[0027] A priority queue is introduced into the RRT* algorithm to sort the nodes according to the heuristic distance from the node to the target point and the energy consumption. The nodes that are closer to the target point and have lower energy consumption are expanded first, which improves the efficiency and quality of path planning, can quickly find a near-optimal path, reduce the search range of the algorithm, and improve the speed of path planning. BRIEF DESCRIPTION OF THE DRAWINGS

[0028] Figure 1 It is a schematic diagram of a low-energy consumption path planning method for a limited-angle six-degree-of-freedom manipulator based on an improved RRT* algorithm according to Embodiment 1 of the present invention; Figure 2 A schematic diagram of the joint angles of a robotic arm according to a low-energy consumption path planning method for a six-DOF robotic arm with a restricted angle based on an improved RRT* algorithm according to Embodiment 1 of the present invention; Figure 3 This is a line graph comparing energy consumption of a low-energy path planning method for a limited-angle six-degree-of-freedom manipulator based on an improved RRT* algorithm in Example 1 of the present invention; Figure 4 This is a schematic diagram of the path planning results of the low-energy consumption path planning method for a limited-angle six-degree-of-freedom manipulator based on the improved RRT* algorithm in Example 1 of the present invention; DETAILED DESCRIPTION

[0029] Hereinafter, a low-energy consumption path planning method for a constrained angle six-degree-of-freedom manipulator based on an improved RRT* algorithm of the present invention will be described in detail with reference to the accompanying drawings.

[0030] like Figure 1As shown, a low-energy consumption path planning method for a limited angle six-degree-of-freedom manipulator based on an improved RRT* algorithm is provided. The method is an improvement based on the RRT* algorithm. The path planning of the improved RRT* algorithm includes the following steps: S1: Initialize the workspace and parameters; define the boundary of the robot workspace and the robot DH parameters and determine the starting point S and the end point G, set the number of sampling points N, the step length s and the angle limit J of the robot joint as basic parameters, use the maximum number of iterations M as the termination condition, initialize the data structure, and use the Matplotlib library to create a 3D graphics environment; like Figure 2 As shown in the figure, the range of the joint angle limit J is: joint 1 [- π, π], joint 2 [- , ], joint three [-π, π], joint four [- , ], joint five [-π,π], joint six [- , ]; The data structure includes a random tree T, a priority queue H and an LQR controller; The robot arm DH parameters include the joint angle θ, joint offset d, connecting rod length a and connecting rod torsion angle α of each connecting rod; The parameters of the established six-DOF robot DH are shown in Table 1: Table 1 Among them, θ_i represents the joint angle, d_i is the joint offset, a_i is the connecting rod length, α_i represents the connecting rod torsion angle, and the DH transformation matrix is ​​constructed according to the four parameters of joint angle θ, joint offset d, connecting rod length a and connecting rod torsion angle α. The general expression of DH is: in, Represents the transformation matrix from the i-1th coordinate system to the i-th coordinate system.

[0031] Furthermore, the initialization of the LQR controller includes initializing parameters, calculating the P matrix, and calculating the gain matrix K. The parameters include the system state matrix A, the control matrix B, the state cost matrix Q, the control cost matrix R, and the time step dt. The initialized parameters provide a data basis for subsequent calculations.

[0032] Furthermore, based on the initialization parameters, the scipy.linalg.solve_continuous_are function is used to solve the continuous-time algebraic Riccati equation to obtain the P matrix. The P matrix and the control matrix B are then used to calculate the LQR gain matrix K. The calculation formula is: Among them, R −1 is the inverse matrix of the control cost matrix R, B T is the transposed matrix of the control matrix B, and P is the P matrix. According to the initialized parameters, the P matrix and the gain matrix K are obtained by calculation, which provides data support for the subsequent calculation of the control input.

[0033] S2: Generate random sampling points Pr; introduce target point sampling, generate sampling points Pr, perform constraint checks on sampling points Pr and obtain the spatial coordinates of sampling points Pr; The constraint check includes joint angle limit constraints; Furthermore, the target point sampling includes setting a fixed probability p, which is set to 0.1. When generating the sampling point Pr, a random number r in the interval [0,1] is first generated. If r≤p, the end point G is directly sampled as the target point. If r>p, the sampling point Pr is randomly generated in the state space of the robot. The target point sampling strategy increases the bias towards the target point, reduces the search time, increases the probability of finding a feasible path, and speeds up the path planning speed.

[0034] Furthermore, by comparing the joint angle corresponding to the sampling point Pr with the preset joint angle limit J, it is determined whether the range of the joint angle limit J is met. If it exceeds the range, the sampling point Pr is regenerated. During the sampling and node expansion process, the joint angle is strictly checked to ensure that it is within the physical limit of the robot arm, avoid exceeding the angle limit, improve the reliability of path planning, and ensure the safe operation of the robot arm.

[0035] S3: Path planning and node expansion; Find the node Pn closest to the sampling point Pr in the random tree T, expand from the nearest point Pn along the direction of the sampling point Pr and generate a new node Pnew. After the new node Pnew is generated, detect obstacles between the new node Pnew and the nearest point Pn. If there are obstacles, abandon Pnew and regenerate a random sampling point Pr. If not, add Pnew to the random tree T to obtain the current state x of the new node Pnew, and then dynamically update the image display. Furthermore, the 3D image displays drawn content including obstacles, starting points, target points, extended paths, and robotic arms, and the drawn content is updated in each iteration.

[0036] S4: LQR controller optimizes energy consumption; LQR controller is used to calculate the energy consumption e of the new node Pnew and directly calculate the Euclidean distance d between the new node Pnew and the end point G through the formula, and the heuristic value h is calculated through the heuristic function according to the energy consumption e and the Euclidean distance d; The heuristic function is a function that maps the relevant information of the current node into a numerical value, and is used to balance the path length and energy consumption.

[0037] Furthermore, the LQR controller calculates the control input u using the gain matrix K calculated by initialization in step S1 and the current state x of the new node Pnew obtained in step S3. The calculation formula of the control input u is: Among them, u is the control input, K is the gain matrix K, and x is the current state of the new node Pnew.

[0038] The LQR controller calculates the energy loss e according to the obtained control input u and the time step dt initialized in step S1. The calculation formula of the energy loss e is: Among them, e is the energy consumption, dt is the time step, u1 and u2 represent the control inputs of the starting state x1 and the target state x2, respectively, that is, the process of transferring from the current state to the next state.

[0039] The Euclidean distance d is calculated based on the coordinates of the new node Pnew and the coordinates of the end point G. The calculation formula of the Euclidean distance d is: Among them, d is the Euclidean distance, x Pnew ,y Pnew 、z Pnew are the spatial coordinates of the new node Pnew, x G ,y G and z G are the spatial coordinates of the end point G respectively.

[0040] Furthermore, the LQR controller calculates the weight coefficient by the normalization method, and uses the minimum and maximum value normalization to normalize the distance and energy consumption to between 0 and 1. The calculation formula of the normalization method is: Among them, d normalized and e normalized are the normalized Euclidean distance and energy consumption, min_d and max_d are the minimum and maximum distances observed so far, min_e and max_e are the minimum and maximum energy consumption observed so far, and the role of 1e - 6 is to prevent the denominator from being 0 and unable to be calculated in the actual program; Then, according to the normalized d normalized and e normalized Dynamically calculate weights. The weight calculation formula is: Among them, d_weight is the distance weight, e_weight is the energy weight, d normalized and e normalized They are the standardized Euclidean distance and energy consumption respectively. The role of 1e-6 is to prevent the denominator from being 0 and unable to be calculated in actual programs.

[0041] The LQR controller calculates the heuristic value h through the heuristic function according to the calculated energy consumption e and the Euclidean distance d. The final heuristic value h is the weighted sum of the distance and the energy consumption. The formula in the heuristic function is:

[0042] Among them, d_weight and e_weight are weight coefficients, h is the heuristic value, d is the Euclidean distance, and e is the energy consumption.

[0043] like Figure 3 As shown in the figure, the dotted line represents the energy consumption of the RRT* algorithm with the LQR controller, and the solid line represents the energy consumption of the RRT* algorithm without the LQR controller. In order to show the advantage of adding the LOR controller in reducing energy consumption, we conducted a comparative experiment at the dimensional level, and it can be concluded that the control energy consumed by the improved RRT* algorithm using the LOR controller is significantly reduced compared to the path planning using only the RRT* algorithm.

[0044] S5: Priority queue management node; add the new node Pnew added to the random tree T and its heuristic value h to the priority queue H; Furthermore, the priority queue H is implemented using a minimum heap data structure, and is sorted from small to large according to the heuristic value h, ensuring that the node extracted from the queue each time is the node with the smallest heuristic value h. The priority queue is sorted according to the size of the heuristic value, and the node with a smaller heuristic value will be processed first.

[0045] S6: Path optimization; extract the new node Pnew with the smallest heuristic value from the priority queue H, set a range with the new node Pnew as the center, traverse other nodes within the range and calculate the new path cost new_cost from the new node Pnew to these nearby nodes. If the new path cost is less than the path cost of the original node, update the parent node of these nodes to the new node Pnew, otherwise continue traversing, and then reconnect the affected nodes P around. The reconnection operation is to check whether the distance from the affected point to the starting point S can be reduced through the new node, and update the distance information from the relevant node to the starting point.

[0046] Furthermore, for the traversed nodes P, the path cost to reach them through the new node Pnew is recalculated and compared with the original path cost. If the new path cost is lower, the distance information from the relevant nodes to the starting point is updated, and the parent node of these nodes is updated to Pnew.

[0047] S7: Determine the termination condition; if the termination condition is met, stop the algorithm; if not, execute steps S2 to S7 in a loop; The termination condition includes the distance from the new node Pnew to the end point G and the maximum number of iterations M.

[0048] S8: Generate a path; when the termination condition is met, trace back from the end point G to the starting point S according to the parent-child relationship of the node P in the random tree T to generate a complete path.

[0049] like Figure 4 As shown in the figure, when the set termination condition is met, the random tree T has been constructed, and each node P in the tree stores the parent-child relationship information. Starting from the end point G, we can trace back to the parent node along the branches of the tree according to these parent-child relationships, and trace back to the starting point S. This process can connect the previously explored path fragments to form a complete path from the starting point to the end point.

[0050] The invention discloses a low-energy path planning method for a limited-angle six-degree-of-freedom manipulator based on an improved RRT* algorithm. The method first defines the workspace boundary of the manipulator, the DH parameters of the manipulator, determines the starting point S and the end point G, sets the number of sampling points N, the step length s, the joint angle limit J and other parameters, initializes the data structures such as the random tree T, the priority queue H and the LQR controller, and creates a 3D graphics environment. Then, a random sampling point Pr is generated according to the target point sampling method, and constraints such as joint angles are checked. During path planning and node expansion, obstacle detection is performed between the new node Pnew and the nearest point Pn. The LQR controller is used to calculate the energy consumption e of the new node and the Euclidean distance d from the new node Pnew to the end point G is directly calculated by the formula, and the heuristic value h is obtained by the heuristic function. The heuristic function uses dynamic normalized weights, normalizes the Euclidean distance d and energy consumption e, and then calculates the weights, and then obtains the weighted sum as the heuristic value h. The weights are adjusted in real time according to the Euclidean distance d and energy consumption e of each node P, so as to better balance the distance and energy consumption, realize dynamic optimization of the path, and further improve the quality and efficiency of path planning. The new node Pnew and its heuristic value h are added to the priority queue H. Then the node with the smallest heuristic value h is extracted from the priority queue H, and the parent node is re-searched in its vicinity and the surrounding nodes are reconnected to optimize the path. The termination condition is determined according to the distance from the new node Pnew to the end point G and the maximum number of iterations M. If it is satisfied, the complete path is generated by backtracking the parent-child relationship of the nodes in the random tree. This method combines the global planning ability of the RRT algorithm with the local optimal control ability of the LQR controller, constrains the joint angle, introduces the target point sampling strategy, and uses the priority queue, which can effectively reduce the path energy consumption, improve the operation safety of the robot arm and the path planning efficiency.

[0051] The above description is only a preferred example of the present application and is not intended to limit the present application. For those skilled in the art, the present application may have other optimization schemes and additional functions. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present application shall be included in the protection scope of the present application.

Claims

1. A low-energy consumption path planning method for a limited-angle six-degree-of-freedom manipulator based on an improved RRT* algorithm. The method is an improvement on the RRT* algorithm and is characterized by: The improved RRT* algorithm path planning includes the following steps: S1: Initialize the workspace and parameters; define the boundary of the robot workspace and the robot DH parameters and determine the starting point S and the end point G, set the number of sampling points N, the step length s and the angle limit J of the robot joint as basic parameters, use the maximum number of iterations M as the termination condition, initialize the data structure, and use the Matplotlib library to create a 3D graphics environment; The range of the joint angle limit J is: joint 1 [-π, π], joint 2 [- , ], joint three [-π, π], joint four [- , ], joint five [-π,π], joint six [- , ]; The data structure includes a random tree T, a priority queue H and an LQR controller; The robot arm DH parameters include the joint angle θ, joint offset d, connecting rod length a and connecting rod torsion angle α of each connecting rod; S2: Generate random sampling points Pr; introduce target point sampling, generate sampling points Pr, perform constraint checks on sampling points Pr and obtain the spatial coordinates of sampling points Pr; The constraint check includes joint angle limit constraints; S3: Path planning and node expansion; Find the node Pn closest to the sampling point Pr in the random tree T, expand from the nearest point Pn along the direction of the sampling point Pr and generate a new node Pnew. After the new node Pnew is generated, detect obstacles between the new node Pnew and the nearest point Pn. If there are obstacles, abandon Pnew and regenerate a random sampling point Pr. If not, add Pnew to the random tree T to obtain the current state x of the new node Pnew, and then dynamically update the image display. S4: LQR controller optimizes energy consumption; LQR controller is used to calculate the energy consumption e of the new node Pnew and directly calculate the Euclidean distance d between the new node Pnew and the end point G according to the formula, and the heuristic value h is calculated by the heuristic function according to the energy consumption e and the Euclidean distance d; The heuristic function is a function that maps the relevant information of the current node into a numerical value, which is used to balance the path length and energy consumption, wherein the weight calculation of the energy consumption e and the Euclidean distance d adopts a dynamic normalization method; S5: Priority queue management node; add the new node Pnew added to the random tree T and its heuristic value h to the priority queue H; S6: Path optimization; extract the new node Pnew with the smallest heuristic value from the priority queue H, set a range with the new node Pnew as the center, traverse other nodes within the range and calculate the new path cost new_cost from the new node Pnew to these nearby nodes. If the new path cost is less than the path cost of the original node, update the parent node of these nodes to the new node Pnew, otherwise continue traversing, and then reconnect the affected nodes P around. The reconnection operation is to check whether the distance from the affected point to the starting point S can be reduced through the new node, and update the distance information from the relevant node to the starting point; S7: Determine the termination condition; if the termination condition is met, stop the algorithm; if not, execute steps S2 to S7 in a loop; The termination condition includes the distance from the new node Pnew to the end point G and the maximum number of iterations M; S8: Generate a path; when the termination condition is met, trace back from the end point G to the starting point S according to the parent-child relationship of the node P in the random tree T to generate a complete path.

2. The low-energy consumption path planning method for a limited-angle six-degree-of-freedom manipulator based on an improved RRT* algorithm as claimed in claim 1, characterized in that: In step S1, the initialization of the LQR controller includes initializing parameters, calculating the P matrix, and calculating the gain matrix K. The parameters include the system state matrix A, the control matrix B, the state cost matrix Q, the control cost matrix R, and the time step dt.

3. The low-energy consumption path planning method for a limited-angle six-degree-of-freedom manipulator based on an improved RRT* algorithm as claimed in claim 2, characterized in that: Based on the initialization parameters, the scipy.linalg.solve_continuous_are function is used to solve the continuous-time algebraic Riccati equation to obtain the P matrix. The P matrix and the control matrix B are then used to calculate the LQR gain matrix K. The calculation formula is: Where K is the gain matrix K, R −1 is the inverse matrix of the control cost matrix R, B T is the transposed matrix of the control matrix B, and P is the P matrix.

4. The low-energy consumption path planning method for a limited-angle six-degree-of-freedom manipulator based on an improved RRT* algorithm as claimed in claim 1, characterized in that: In step S2, the target point sampling includes setting a fixed probability p, the probability p is set to 0.1, and when generating the sampling point Pr, first generate a random number r in the interval [0,1]. If r≤p, the end point G is directly sampled as the target point. If r>p, the sampling point Pr is randomly generated in the state space of the robot arm.

5. The low-energy consumption path planning method for a limited-angle six-degree-of-freedom manipulator based on an improved RRT* algorithm as claimed in claim 1, characterized in that: In step S2, by comparing the joint angle corresponding to the sampling point Pr with the preset joint angle limit J, it is determined whether the range of the joint angle limit J is met. If it exceeds the range, the sampling point Pr is regenerated.

6. The low-energy consumption path planning method for a limited-angle six-degree-of-freedom manipulator based on an improved RRT* algorithm as claimed in claim 1, characterized in that: In step S3, the 3D image displays the drawn content including obstacles, starting points, target points, extended paths, and robotic arms, and the drawn content is updated in each iteration.

7. The low-energy consumption path planning method for a limited-angle six-degree-of-freedom manipulator based on an improved RRT* algorithm as claimed in claim 1, characterized in that: In step S4, the LQR controller calculates the control input u using the gain matrix K initialized and calculated in step S1 and the current state x of the new node Pnew obtained in step S3. The calculation formula of the control input u is: Among them, u is the control input, K is the gain matrix K, and x is the current state of the new node Pnew; The LQR controller calculates the energy loss e according to the obtained control input u and the time step dt initialized in step S1. The calculation formula of the energy loss e is: Where e is the energy consumption, dt is the time step, u1 and u2 represent the control inputs of the starting state x1 and the target state x2, respectively, that is, the process of transferring from the current state to the next state; The Euclidean distance d is calculated based on the coordinates of the new node Pnew and the coordinates of the end point G. The calculation formula of the Euclidean distance d is: Among them, d is the Euclidean distance, x Pnew ,y Pnew 、z Pnew are the spatial coordinates of the new node Pnew, x G ,y G and z G are the spatial coordinates of the end point G respectively.

8. The low-energy consumption path planning method for a limited-angle six-degree-of-freedom manipulator based on an improved RRT* algorithm as claimed in claim 7, characterized in that: The LQR controller calculates the weight coefficient by the normalization method, and uses the minimum and maximum normalization method to normalize the Euclidean distance and energy consumption to between 0 and 1. The calculation formula of the normalization method is: Among them, d normalized and e normalized are the normalized Euclidean distance and energy consumption, min_d and max_d are the minimum and maximum distances observed so far, min_e and max_e are the minimum and maximum energy consumption observed so far, and the role of 1e - 6 is to prevent the denominator from being 0 and unable to be calculated in the actual program; Then, according to the normalized d normalized and e normalized Dynamically calculate weights. The weight calculation formula is: Among them, d_weight is the distance weight, e_weight is the energy weight, d normalized and e normalized They are the normalized Euclidean distance and energy consumption respectively. The role of 1e - 6 is to prevent the denominator from being 0 and unable to be calculated in the actual program. The LQR controller calculates the heuristic value h through the heuristic function according to the calculated energy consumption e and the Euclidean distance d. The final heuristic value h is the weighted sum of the distance and the energy consumption. The formula in the heuristic function is: Among them, d_weight and e_weight are weight coefficients, h is the heuristic value, d is the Euclidean distance, and e is the energy consumption.

9. The low-energy consumption path planning method for a limited-angle six-degree-of-freedom manipulator based on an improved RRT* algorithm as claimed in claim 1, characterized in that: In step S5, the priority queue H is implemented using a minimum heap data structure and is sorted from small to large according to the heuristic value h, ensuring that the node extracted from the queue each time is the node with the smallest heuristic value h.

10. The low-energy consumption path planning method for a limited-angle six-degree-of-freedom manipulator based on an improved RRT* algorithm as claimed in claim 1, characterized in that: In step S6, for the nodes P traversed around, the path cost to reach them through the new node Pnew is recalculated and compared with the original path cost. If the new path cost is lower, the distance information from the relevant nodes to the starting point is updated, and the parent node of these nodes is updated to Pnew.

Images