A trajectory planning and control method for excavator arm based on double-layer optimization algorithm

Abstract

This invention relates to a trajectory planning and control method for a mining robot based on a two-layer optimization algorithm. The method constructs a two-layer optimization framework, including an outer optimization layer and an inner optimization layer. The outer layer optimizes the polynomial interpolation time of the trajectory to find the optimal trajectory, which serves as the input to the inner layer. The inner layer performs dynamic tracking control simulation on a reference trajectory, optimizing the control parameters to obtain a set of control parameters that minimizes the tracking error. The weighting coefficients of the outer layer trajectory optimization are then adjusted based on the simulation tracking error, forming a two-layer closed-loop optimization mechanism. This method not only integrates multiple influences such as trajectory time and impact for trajectory planning but also adjusts the weighting coefficients of trajectory planning by considering dynamic tracking characteristics, achieving a precise match between dynamic trajectory requirements and system tracking capabilities. Compared with existing technologies, this invention has advantages such as achieving a precise match between dynamic trajectory requirements and system tracking capabilities.

Description

Technical Field

[0001] This invention relates to the field of automatic control technology for excavating robotic arms in the engineering machinery industry, and in particular to a method for trajectory planning and control of excavating robotic arms based on a two-layer optimization algorithm. Background Technology

[0002] As core equipment in engineering construction and mining, excavating robotic arms are crucial for the efficiency and quality of operations, especially given the trend towards unmanned and autonomous operation in the construction machinery industry. Tracking control based on kinematic optimization to obtain a predetermined trajectory is a conventional technical approach for the automated operation of excavating robotic arms. Therefore, improving the tracking control accuracy of excavating robotic arms has become an important research direction in related fields.

[0003] Excavators are commonly used in construction for typical tasks such as roadbed leveling and pipeline trench excavation. These operations all place high demands on the flatness of the working surface (road surface, trench bottom, and slope). The current Chinese standard, "Standard for Acceptance of Construction Quality of Building Foundation Engineering" (GB50202-2018), stipulates that the flatness requirement for excavator leveling and trenching operations is that the vertical error of the working plane should not exceed ±50mm, which places high demands on the precision of automated excavator operations.

[0004] In traditional techniques, the motion trajectory of a robotic excavator is often optimized to minimize operation time and mechanical impact. Then, control parameters are optimized to improve the system's tracking response performance. However, robotic excavators typically use electro-hydraulic proportional control systems, which inherently suffer from slow dynamic response and low control precision. When only the trajectory is optimized, the resulting trajectory often has dynamic requirements exceeding the robotic arm system's dynamic tracking capabilities, leading to inaccurate trajectory tracking. Even subsequent optimization of control parameters cannot compensate for the trajectory planning deficiencies, making high-precision tracking impossible.

[0005] Chinese Patent 202110392530.5 discloses a robotic arm trajectory planning method based on an improved whale search method. Its core technical feature is the establishment of a time-optimal model, utilizing the improved whale search method to plan the robotic arm's operational trajectory and solve for the time-optimal trajectory. However, the optimal trajectory obtained by this time-only planning method can cause significant impact and vibration to the robotic arm during operation, affecting tracking and control accuracy.

[0006] Chinese patent 201910647190.9 provides an optimal trajectory planning method for intelligent hydraulic excavators. Its core technical feature lies in using a sequential quadratic programming method to solve for the time-optimal excavator operating trajectory, simultaneously achieving optimal time trajectory planning and the smoothest trajectory planning. However, this planning method does not consider the dynamic characteristics of the excavator's hydraulic system. The planned trajectory only represents the optimality in kinematic characteristics and is not the most suitable trajectory for high-precision tracking by the hydraulic excavator.

[0007] Chinese patent 202411190384.8 discloses a parameter optimization method for valve positioning controllers based on genetic fusion. Its core technical feature is the optimization of control parameters using a genetic fusion algorithm to control the opening of the pneumatic regulating valve with the optimal control quantity, thereby shortening the adjustment time and improving control stability and efficiency. However, controller performance optimization has an upper limit. If the dynamic requirements of the reference trajectory are too high, even with optimal control parameters, the system's dynamic response capability cannot meet the dynamic requirements of the trajectory, making high-precision tracking difficult to achieve.

[0008] Existing technologies lack a collaborative mechanism for trajectory optimization and control parameter optimization, and fail to consider the matching between dynamic trajectory requirements and system tracking capabilities, resulting in insufficient tracking accuracy of the excavating robot during automatic control, which affects operational stability and reliability. Summary of the Invention

[0009] The purpose of this invention is to provide a method for trajectory planning and control of excavating robotic arms based on a two-layer optimization algorithm, in order to achieve collaborative optimization of trajectory and control parameters and enable the trajectory to adapt to the dynamic tracking capability of the system.

[0010] The objective of this invention can be achieved through the following technical solutions: A method for trajectory planning and control of a mining robot based on a two-layer optimization algorithm, comprising the following steps: Step 1: Construct the electromechanical-hydraulic dynamics model of the excavating robot arm; Step 2: Confirm the work scenario and operating conditions, and solve for the work path points based on the dynamic model; Step 3: Based on the working path points, use the comprehensive cost function to perform trajectory outer-layer optimization to obtain the reference trajectory; Step 4: Optimize the reference trajectory with the goal of minimizing the maximum absolute tracking error at the bucket end. Perform inner-layer optimization with the goal of minimizing the maximum absolute tracking error at the bucket end to obtain the optimal control parameters. Step 5: If the error between the current control parameters and the optimal control parameters is greater than the threshold, adjust the weight coefficient of the comprehensive cost function and return to step 3; otherwise, use the current reference trajectory as the reference trajectory result and the current optimal control parameters as the control parameter result. Step 6: Control the excavating robot arm based on the reference trajectory results and control parameter results.

[0011] Furthermore, the electromechanical-hydraulic dynamic model for constructing the excavating robotic arm is specifically as follows: Step 1.1: Based on the Lagrange equation, establish the relationship between the torque, angle, angular velocity, and angular acceleration of each joint of the excavating robot arm; Step 1.2: Map the load onto the joint hydraulic cylinder to establish the electromechanical-hydraulic dynamics model of the excavating robot arm; Step 1.3: Select the electro-hydraulic proportional system control algorithm and determine the initial values ​​of the control parameters.

[0012] Furthermore, the comprehensive cost function is: in, f 1 、f 2 are the time cost function and the impact cost function, respectively; w 1 、w 2 represents the weighting coefficients of the time cost function and the impact cost function, satisfying... w 1 +w 2 = 1; β 1 、β 2 is a coefficient that brings the time cost function and the impact cost function into the same quantitative range.

[0013] Furthermore, the time cost function is: Where T is the total operation time; Δ t i m+1 is the interpolation time between two key points; m+1 is the number of interpolation points.

[0014] Furthermore, the impact cost function is: in, j =1, 2, and 3 represent the boom, stick, and bucket joints, respectively. Indicates joint j angular acceleration.

[0015] Furthermore, the specific steps for obtaining the reference trajectory by using a comprehensive cost function to optimize the outer layer of the trajectory based on the working path points are as follows: Step 3.1: Perform trajectory planning using polynomials, optimize the interpolation time using an optimization algorithm, and determine the initial values ​​of the comprehensive cost function and weight coefficients for trajectory optimization. Step 3.2: Iteratively calculate the comprehensive cost function. When the comprehensive cost function converges, obtain the optimized trajectory interpolation time. Based on the optimized optimal trajectory interpolation time Polynomial interpolation is performed to obtain the reference trajectory of each joint. .

[0016] Furthermore, the reference trajectory is optimized with the goal of minimizing the maximum absolute tracking error at the bucket end. The specific steps for inner-layer optimization to obtain the optimal control parameters with the goal of minimizing the maximum absolute tracking error at the bucket end are as follows: Step 4.1: Use the joint reference trajectories obtained in Step 3.2. As a reference input for dynamic tracking control simulation; Step 4.2: Use an optimization algorithm to optimize the control parameters and determine the objective function for optimizing the control parameters as the maximum absolute tracking error at the bucket end. Step 4.3: Perform optimization iteration. When the maximum absolute tracking error converges, the optimal control parameters are obtained.

[0017] Furthermore, after confirming the work scenario and operating conditions, the specific steps for solving the work path points based on the dynamic model are as follows: Step 2.1: Confirm that the working condition is leveling, use the excavating robotic arm to level the sand in front, and determine the error accuracy requirements of the operation. Step 2.2: Based on the working conditions, determine the start point, path point, and end point of the excavator's bucket end. Step 2.3: Calculate the joint angle based on the starting point, path point and end point of the bucket end. The starting point, path point and end point and the joint angle together are used as the working path point.

[0018] Furthermore, the initial point, path points, and termination point are solved using forward and inverse kinematics.

[0019] Furthermore, the control parameters are the proportional-integral-derivative coefficients of the PID controller, the QR matrix of the LQR controller, or the sliding surface parameters of the sliding mode control.

[0020] Compared with the prior art, the present invention has the following beneficial effects: 1. A two-layer optimization framework is adopted. The outer layer optimizes to ensure the optimality of the trajectory, while the inner layer optimizes the control parameters for the trajectory. This solves the problem of mismatch between the dynamic requirements of the trajectory and the system's tracking capability in a single optimization scheme, and significantly improves the trajectory tracking accuracy.

[0021] 2. Introduce a tracking error feedback adjustment mechanism to iteratively adjust the weight coefficients of the outer trajectory optimization to achieve closed-loop optimization.

[0022] 3. The optimization objectives are clearly defined, taking into account the three core requirements of operation time, joint impact, and tracking error. It ensures the operating efficiency of the excavating robot arm, reduces joint impact and extends equipment lifespan, and improves trajectory tracking accuracy. It is suitable for scenarios with high control precision requirements, such as engineering construction and mining, and has strong practicality.

[0023] 4. The core optimization process is implemented based on the optimization algorithm logic, which can be effectively adapted to the excavating robot arm, and the optimization results are easy to implement in engineering. Attached Figure Description

[0024] Figure 1 This is a schematic diagram illustrating the structure and operation of the excavating robotic arm in an embodiment. Figure 2 This is a flowchart of the method of the present invention. The markings in the diagram are as follows: 1-Hydraulic cylinder, 2-Joint angle sensor, 3-Boom, 4-Stick, 5-Bucket, 6-Target working path, 7-Working error boundary. Detailed Implementation

[0025] The present invention will now be described in detail with reference to the accompanying drawings and specific embodiments. These embodiments are based on the technical solution of the present invention and provide detailed implementation methods and specific operating procedures. However, the scope of protection of the present invention is not limited to the following embodiments.

[0026] This invention relates to a trajectory planning and control method for excavating manipulators based on a two-layer optimization algorithm. This method constructs a two-layer optimization framework, including an outer optimization layer (trajectory optimization) and an inner optimization layer (control parameter optimization). The outer layer optimizes the polynomial interpolation time of the trajectory to find the optimal trajectory, which serves as the input to the inner layer. The inner layer performs dynamic tracking control simulation on a reference trajectory to optimize the control parameters, obtaining a set of control parameters that minimizes the tracking error. Then, based on the simulation tracking error, the weight coefficients of the outer layer trajectory optimization are adjusted, forming a two-layer closed-loop optimization mechanism. This method not only comprehensively considers multiple influences such as trajectory time and impact during trajectory planning but also adjusts the weight coefficients of trajectory planning by considering dynamic tracking characteristics, achieving a precise match between dynamic trajectory requirements and system tracking capabilities. This invention can effectively improve the trajectory tracking accuracy of excavating manipulators and meet the application requirements of high-precision operation conditions.

[0027] Figure 1 This is a schematic diagram of the excavating robotic arm structure and working scenario in an embodiment of the method of the present invention; Figure 2 This is a flowchart of the method procedure of the present invention; combined with Figure 2 The present invention provides a method for trajectory planning and control of a mining robot based on a two-layer optimization algorithm, comprising the following steps: Step 1: Construct the electromechanical-hydraulic dynamics model of the excavating robot arm.

[0028] Step 1.1: Based on the Lagrange equation, establish the relationship between the torque, angle, angular velocity, and angular acceleration of each joint of the excavating robot arm.

[0029] Step 1.2: Map the load onto the joint hydraulic cylinder to establish the dynamic model of the electro-hydraulic control system.

[0030] Step 1.3: Select the electro-hydraulic proportional system control algorithm and determine the initial values ​​of the control parameters.

[0031] Step 2: Confirm the work scenario and working conditions, and solve for the work path points.

[0032] Step 2.1: Confirm that the working condition is leveling, use the excavating robotic arm to level the sand in front, and determine the error accuracy requirements for the operation.

[0033] Step 2.2: Based on the working conditions, determine the start point, path point, and end point of the excavator's bucket end.

[0034] Step 2.3: Calculate the joint angle based on the starting point, path point and ending point of the bucket end.

[0035] Step 3: Optimize the trajectory based on the comprehensive cost function to generate a reference trajectory (outer layer optimization).

[0036] Step 3.1: Use polynomials for trajectory planning, use optimization algorithms to optimize interpolation time, determine the comprehensive cost function of trajectory optimization as shown in Equation (1), and set the initial values ​​of weight coefficients.

[0037] Step 3.2: Initialize the optimization algorithm parameters and iterate. When the comprehensive cost function described in equation (1) converges to the minimum value, the optimized trajectory interpolation time is obtained. Polynomial interpolation is then performed to obtain the reference trajectory for each joint. .

[0038] Step 4: With the goal of minimizing the maximum absolute tracking error at the bucket end, perform inner-layer optimization to obtain the optimal control parameters (inner-layer optimization).

[0039] Step 4.1: Use the joint reference trajectories obtained in Step 3.2. As a reference input for dynamic tracking control simulation.

[0040] Step 4.2: Use an optimization algorithm to optimize the control parameters and determine the objective function for optimizing the control parameters as the maximum absolute tracking error at the bucket end.

[0041] Step 4.3: Initialize the optimization algorithm parameters and perform optimization iterations. When the maximum absolute tracking error converges to the minimum value, the optimal control parameters are obtained.

[0042] Step 5: Adjust the weighting coefficients based on the maximum absolute tracking error.

[0043] Step 5.1, Weighting coefficients of the comprehensive cost function w 1 、w 2. Adjustment based on the maximum absolute tracking error of dynamic tracking control simulation.

[0044] Step 5.2: After adjusting the weighting coefficients, repeat the trajectory optimization and control parameter optimization steps. The optimal joint trajectory is obtained when the maximum absolute tracking error at the bucket end meets the error accuracy requirements of the operating condition and converges to its minimum value. With optimal control parameters.

[0045] Step 6: Trajectory Tracking Control. The final optimized trajectory and control parameters are imported into the excavator arm controller. The electro-hydraulic proportional control system outputs flow to drive the excavator arm to track the reference trajectory. The actual bucket end-point tracking error is within the operational error range, meeting the requirements for high-precision operation.

[0046] The optimization algorithm parameters of this invention can be adjusted according to the number of joints of the robotic arm and the requirements of the operation scenario to balance optimization efficiency and optimization accuracy; the boundary range of the trajectory parameters needs to be set according to the physical structural constraints of the excavation robotic arm (joint rotation range, maximum angular velocity, maximum angular acceleration) to avoid exceeding the mechanical limits; the working condition accuracy requirements can be set according to the specific operation scenario.

[0047] Compared with the prior art, the present invention has the following beneficial effects: 1. A two-layer optimization framework is adopted. The outer layer optimizes to ensure the optimality of the trajectory, while the inner layer optimizes the control parameters for the trajectory. This solves the problem of mismatch between the dynamic requirements of the trajectory and the system's tracking capability in a single optimization scheme, and significantly improves the trajectory tracking accuracy.

[0048] 2. Introduce a tracking error feedback adjustment mechanism to iteratively adjust the weight coefficients of the outer trajectory optimization to achieve closed-loop optimization.

[0049] 3. The optimization objectives are clearly defined, taking into account the three core requirements of operation time, joint impact, and tracking error. It ensures the operating efficiency of the excavating robot arm, reduces joint impact and extends equipment lifespan, and improves trajectory tracking accuracy. It is suitable for scenarios with high control precision requirements, such as engineering construction and mining, and has strong practicality.

[0050] 4. The core optimization process is implemented based on the optimization algorithm logic, which can be effectively adapted to the excavating robot arm, and the optimization results are easy to implement in engineering.

[0051] In this invention, the first step, the body dynamics, is established based on the Lagrange equations, and the load is mapped onto the hydraulic cylinder to obtain the dynamic model of the electromechanical-hydraulic system. The second step solves for the initial point, path point, and termination point using forward and inverse kinematics. The third step's comprehensive cost function is... in, f 1 、f 2 are the time cost function and the impact cost function, respectively; w 1 、w 2 represents the weighting coefficients of the time cost function and the impact cost function, satisfying... w 1 +w 2 = 1; β 1 、β 2 is a coefficient that brings the time cost function and the impact cost function into the same quantitative range. The weighting coefficient is initially set to... w 1 = w 2 = 0.5.

[0052] The time cost function and the impact cost function are as follows: Where T is the total operation time; Δ t i m+1 is the interpolation time between two key points; m+1 is the number of interpolation points. j =1, 2, and 3 represent the boom, stick, and bucket joints, respectively. Indicates joint j angular acceleration.

[0053] The objective function for the fourth step is the maximum absolute tracking error of the bucket end in the dynamic simulation.

[0054] The weighting coefficients in the fifth step are adjusted based on the maximum absolute tracking error at the bucket end of the dynamic simulation.

[0055] Trajectory planning and control methods need to be tested in conjunction with hardware devices such as robotic arms, computers, and workbenches.

[0056] Forward and inverse kinematics are based on DH modeling, and the initial point, termination point and path point obtained are all in the joint space.

[0057] Joint space is better able to satisfy dynamic and kinematic constraints than Cartesian space.

[0058] The control parameters are the proportional-integral-derivative coefficients of PID control, the QR matrix of LQR control, or the sliding surface parameters of sliding mode control.

[0059] If the aforementioned functions are implemented as software functional units and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of this invention, or the part that contributes to the prior art, or a part of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of this invention. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

Claims

1. A method for trajectory planning and control of a mining robot based on a two-layer optimization algorithm, characterized in that, The method includes the following steps: Step 1: Construct the electromechanical-hydraulic dynamics model of the excavating robot arm; Step 2: Confirm the work scenario and operating conditions, and solve for the work path points based on the dynamic model; Step 3: Based on the working path points, use the comprehensive cost function to perform trajectory outer-layer optimization to obtain the reference trajectory; Step 4: Optimize the reference trajectory with the goal of minimizing the maximum absolute tracking error at the bucket end. Perform inner-layer optimization with the goal of minimizing the maximum absolute tracking error at the bucket end to obtain the optimal control parameters. Step 5: If the error between the current control parameters and the optimal control parameters is greater than the threshold, adjust the weight coefficient of the comprehensive cost function and return to step 3; otherwise, use the current reference trajectory as the reference trajectory result and the current optimal control parameters as the control parameter result. Step 6: Control the excavating robot arm based on the reference trajectory results and control parameter results.

2. The method for trajectory planning and control of a mining robot based on a two-layer optimization algorithm according to claim 1, characterized in that, The electromechanical-hydraulic dynamics model of the excavating robot arm is specifically constructed as follows: Step 1.1: Based on the Lagrange equation, establish the relationship between the torque, angle, angular velocity, and angular acceleration of each joint of the excavating robot arm; Step 1.2: Map the load onto the joint hydraulic cylinder to establish the electromechanical-hydraulic dynamics model of the excavating robot arm; Step 1.3: Select the electro-hydraulic proportional system control algorithm and determine the initial values ​​of the control parameters.

3. The method for trajectory planning and control of a mining robot based on a two-layer optimization algorithm according to claim 2, characterized in that, The comprehensive cost function is: in, f 1 、f 2 are the time cost function and the impact cost function, respectively; w 1 、w 2 represents the weighting coefficients of the time cost function and the impact cost function, satisfying... w 1 +w 2 = 1; β 1 、β 2 is a coefficient that brings the time cost function and the impact cost function into the same quantitative range.

4. The method for trajectory planning and control of a mining robot based on a two-layer optimization algorithm according to claim 3, characterized in that, The time cost function is: Where T is the total operation time; Δ t i m+1 is the interpolation time between two key points; m+1 is the number of interpolation points.

5. The method for trajectory planning and control of a mining robot based on a two-layer optimization algorithm according to claim 4, characterized in that, The impact cost function is: in, j =1, 2, and 3 represent the boom, stick, and bucket joints, respectively. Indicates joint j angular acceleration.

6. The method for trajectory planning and control of a mining robot based on a two-layer optimization algorithm according to claim 5, characterized in that, The specific steps for obtaining the reference trajectory by using a comprehensive cost function based on the working path points to optimize the outer layer of the trajectory are as follows: Step 3.1: Perform trajectory planning using polynomials, optimize the interpolation time using an optimization algorithm, and determine the initial values ​​of the comprehensive cost function and weight coefficients for trajectory optimization. Step 3.2: Iteratively calculate the comprehensive cost function. When the comprehensive cost function converges, obtain the optimized trajectory interpolation time. Based on the optimized optimal trajectory interpolation time Polynomial interpolation is performed to obtain the reference trajectory of each joint. .

7. The method for trajectory planning and control of a mining robot based on a two-layer optimization algorithm according to claim 6, characterized in that, The specific steps for optimizing the reference trajectory with the goal of minimizing the maximum absolute tracking error at the bucket end, and for performing inner-layer optimization with the goal of minimizing the maximum absolute tracking error at the bucket end, to obtain the optimal control parameters are as follows: Step 4.1: Use the joint reference trajectories obtained in Step 3.

2. As a reference input for dynamic tracking control simulation; Step 4.2: Use an optimization algorithm to optimize the control parameters and determine the objective function for optimizing the control parameters as the maximum absolute tracking error at the bucket end. Step 4.3: Perform optimization iteration. When the maximum absolute tracking error converges, the optimal control parameters are obtained.

8. The method for trajectory planning and control of a mining robot based on a two-layer optimization algorithm according to claim 1, characterized in that, The specific steps for determining the work scenario and operating conditions, and solving for the work path points based on the dynamic model, are as follows: Step 2.1: Confirm that the working condition is leveling, use the excavating robotic arm to level the sand in front, and determine the error accuracy requirements of the operation. Step 2.2: Based on the working conditions, determine the start point, path point, and end point of the excavator's bucket end. Step 2.3: Calculate the joint angle based on the starting point, path point and end point of the bucket end. The starting point, path point and end point and the joint angle together are used as the working path point.

9. The method for trajectory planning and control of a mining robot based on a two-layer optimization algorithm according to claim 8, characterized in that, The initial point, path points, and termination point are solved using forward and inverse kinematics.

10. The method for trajectory planning and control of a mining robot based on a two-layer optimization algorithm according to claim 1, characterized in that, The control parameters are the proportional-integral-derivative coefficients of PID control, the QR matrix of LQR control, or the sliding surface parameters of sliding mode control.

Images