Method for planning continuous trajectories for excavator working devices

Abstract

The present application is suitable for the technical field of excavator operation, and provides a continuous trajectory planning method for the working device of an excavator. The present application adopts a regional excavation strategy, through explicit task decomposition, making the single excavation task more clear and controllable. This strategy helps to optimize the excavation path, reduce unnecessary repeated excavation, and thus improve the excavation efficiency; the TD3 algorithm based on deep reinforcement learning is introduced, the application of the algorithm improves the real-time and adaptability of trajectory planning, so that the excavator can quickly respond to various complex operation conditions and environmental changes, and ensure the efficient operation; the automatic trajectory planning is realized, which reduces the dependence on manual operation. Through automatic planning, not only the accuracy and stability of the operation are improved, but also the safety of the operation is further improved.

Description

TECHNICAL FIELD

[0001] The present application belongs to the technical field of excavator operation, and particularly relates to a continuous trajectory planning method for an excavator working device. BACKGROUND

[0002] Excavators are one of the indispensable equipment in engineering construction and are widely used in many fields such as mining, water conservancy, transportation and municipal administration. However, traditional excavators rely on manual operation and face many challenges: heavy construction tasks lead to high operating intensity; poor work site environment, such as mechanical vibration, high noise and dust; high-risk operation scenarios, such as landfill compaction, landslide recovery and post-disaster reconstruction; and shortage of skilled operators affecting production efficiency. These challenges highlight the urgent need to develop automated excavator technology to improve operation efficiency and safety.

[0003] In automated excavator technology, trajectory planning is a core link that directly relates to the realization of autonomous operation of the excavator and is a key link to improve operation performance. Currently, most methods are single-digging trajectory planning, which means that perception, planning and tracking need to be re-performed before each round of digging action cycle, greatly reducing operation efficiency and failing to meet the needs of real-time and continuity. Among them, commonly used trajectory planning methods based on curve interpolation, such as NURBS function interpolation, high-order polynomial interpolation and Bezier curve interpolation, have certain effects, but when dealing with complex conditions, complex mathematical models need to be established and intelligent optimization algorithms are used to optimize target parameters, which requires a large amount of computing resources, resulting in long planning time and difficulty in realizing real-time planning. Therefore, in order to meet the performance requirements of unmanned operation of excavators, there is an urgent need for a multi-objective real-time planning method that can quickly plan a set of continuous digging trajectories according to task requirements. Therefore, the present application proposes a continuous trajectory planning method for an excavator working device. SUMMARY

[0004] The present application aims to provide a continuous trajectory planning method for an excavator working device, which aims to solve the problems raised in the background.

[0005] To achieve the above-mentioned purpose, the present application provides the following technical solutions:

[0006] The continuous trajectory planning method for the excavator working device comprises the following steps:

[0007] Step S1, a simulation model of the excavator working device is established, including a kinematics model and a dynamics model of the excavator working device;

[0008] Step S2, a whole task decomposition digging strategy;

[0009] Step S3, the single-digging trajectory is planned from the joint space by using a curve interpolation method;

[0010] Step S4, the interpolation curve parameters are optimized by using a particle swarm optimization algorithm, and an optimized trajectory dataset is collected;

[0011] Step S5, the trajectory dataset in step S4 is trained by using a deep reinforcement learning TD3 algorithm;

[0012] Step S6, for the overall digging task, a continuous trajectory planning model is used to plan a continuous trajectory for the target area, and a complete digging trajectory of the target area is generated.

[0013] Further, in step S2, the overall trench digging task is in the form of regional digging, and the overall task is decomposed into three different types of trajectories, namely trapezoidal, hexagonal and parallelogram.

[0014] Further, the specific process of step S3 is as follows:

[0015] The 6th order polynomial is used to fit the joint angle change curve of the bucket tooth tip between the starting point and the ending point of the digging trajectory, and the rod i The 6th order polynomial interpolation function of the joint angle is expressed as:

[0016] Formula 1: ;

[0017] In the formula, the boom, the stick and the bucket are sequentially, the rod i The j order coefficient of the joint angle interpolation function, t denotes the time required to execute the trajectory;

[0018] The joint angular velocity i and the angular acceleration of the rod are obtained by derivation of formula 1:

[0019] Formula 2: ;

[0020] Formula 3: ;

[0021] The angular velocity and angular acceleration of each joint at the starting point and ending point of the trajectory of the bucket tooth tip are all set to 0; the unknown quantities of the 6th order polynomial interpolation function of each joint angle are , , and t , The design variable is set as an optimization problem.

[0022] Further, the specific process of step S4 is as follows:

[0023] The normalized digging time , the digging rate , and the work energy consumption are taken as objective functions, while constraint conditions are set, including physical model constraints, trajectory angle change direction constraints, and constraints on the angle between the back of the bucket tooth and the trajectory;

[0024] A mathematical model of the excavator working device digging trajectory optimization problem is established:

[0025] Formula 4: ;

[0026] In the formula, , and are the weights of , and , respectively, satisfying ; and represent the maximum and minimum threshold values of the joint angle ; and represent the threshold values of the joint angular velocity and the angular acceleration ; and are the power and torque required to drive the joint and overcome the digging resistance during material excavation; and are the maximum values of the power and torque allowed by the driving system; is the angle between the tangent of the bucket tooth tip trajectory and the horizontal plane; is the trajectory angle before a time step; is the threshold value of the angle , and the time step ;

[0027] The particle swarm optimization algorithm is used to optimize the trajectory parameters to determine the best solution, and the optimized trajectory joint motion information is collected as a data set.

[0028] Further, step S5 includes building a deep neural network model, an excavator material excavation physical environment model, and a reward function model.

[0029] First, build a deep neural network model: observe the state parameters related to the trajectory planning objective function and constraint conditions, including joint angles , joint angular velocities joint angle acceleration joint angle jerk time t excavation rate work energy consumption power total torque with joint angle jerk and its differential as the action parameter to ensure , , Smooth the planning curve and set the corresponding constraint conditions:

[0030] Equation 5: ;

[0031] In the formula, is the maximum joint angle jerk; is the differential of the maximum joint angle jerk;

[0032] The training of the TD3 continuous trajectory planning model requires two Critic networks and and an Actor network , wherein and have the same structure;

[0033] Secondly, a physical environment model of the excavator material excavation is built, which includes an input-output interface module, a calculation module and a visualization module.

[0034] Finally, a reward function model is built, and the process is as follows:

[0035] Define two termination conditions, normal termination T1 and penalty termination T2, and the single-step reward for the target is:

[0036] Equation 6: ;

[0037] In the formula, is the reward accumulation function for the target; is the normalized weighting function of all target state quantities; is the before a time step;

[0038] The function is expressed as follows:

[0039] Equation 7: ;

[0040] In the formula, , and are the weights of , and , is the digging rate , energy consumption and digging time t state quantity normalized weighted objective function, and are the normalized objective functions of bucket tooth tip height and posture ;

[0041] Sparse rewards are introduced, represented as:

[0042] Equation 8: ;

[0043] The total reward is:

[0044] Equation 9: ;

[0045] Finally, the total reward is used to complete the training of the target area task; during the training process, the input is the state parameter information, and the output is the trajectory joint motion information.

[0046] Compared with the prior art, the beneficial effects of the present application are:

[0047] The present application adopts a regional excavation strategy, through explicit task decomposition, making the single excavation task more clear and controllable. This strategy helps to optimize the excavation path, reduce unnecessary repeated excavation, and thus improve the excavation efficiency.

[0048] 2、The present application introduces a TD3 algorithm based on deep reinforcement learning, which improves the real-time and adaptability of trajectory planning, so that the excavator can quickly respond to various complex working conditions and environmental changes, and ensure efficient operation.

[0049] 3、The present application realizes automatic trajectory planning, which reduces the dependence on manual operation. Through automatic planning, not only the accuracy and stability of the operation are improved, but also the safety of the operation is further improved. BRIEF DESCRIPTION OF DRAWINGS

[0050] Figure 1 is the flowchart of the method of the present application.

[0051] Figure 2 is the continuous trajectory planning strategy diagram under the overall operation condition.

[0052] Figure 3The diagram shows the overall task breakdown; (a) Overall task breakdown, (b) First type of target area (trapezoidal), (c) Second type of target area (hexagonal), (d) Third type of target area (parallelogram). Detailed Implementation

[0053] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention.

[0054] The specific implementation of the present invention will be described in detail below with reference to specific embodiments.

[0055] One embodiment of the present invention provides a continuous trajectory planning method for an excavator working device, the flowchart of which is shown below. Figure 1 As shown, the specific steps include:

[0056] Step S1: Establish a simulation model of the excavator's working device;

[0057] Step S2: Overall task decomposition and mining strategy;

[0058] Step S3: Plan the single excavation trajectory from the joint space using the curve interpolation method;

[0059] Step S4: Use the particle swarm optimization algorithm to perform multi-objective optimization of the interpolation curve parameters and collect the optimized trajectory dataset;

[0060] Step S5: Train the trajectory dataset from step S4 using the deep reinforcement learning TD3 algorithm;

[0061] Step S6: For the overall excavation task, use the trained TD3 continuous trajectory planning model to perform continuous trajectory planning and generate the complete excavation trajectory of the target area.

[0062] (a) Regarding step S1;

[0063] The construction of a simulation model of the excavator's working device is the foundation for subsequent trajectory planning research, including the kinematic and dynamic models of the working device.

[0064] 1. Kinematic model:

[0065] A kinematic model of the excavator's working device is established using the DH parameter method, with the boom coordinate system as follows: The boom coordinate system is The bucket coordinate system is The bucket tooth tip coordinate system is The boom coordinate system is defined as the base coordinate system; the boom length is... , the arm rod length is , the bucket rod length is , the boom joint angle is , the bucket rod joint angle is , the bucket joint angle is , the bucket tooth tip posture angle is .

[0066] 1) Forward kinematics model;

[0067] The forward kinematics model of the working device, i.e. the conversion model from joint space to pose space, converts the joint angle information of the boom, bucket rod and bucket into the position and posture angle information of the bucket tooth tip , and the forward kinematics model of the working device is:

[0068] ;

[0069] In the formula, represents , represents , ; represents , represents ; represents , represents .

[0070] 2) Inverse kinematics model;

[0071] The inverse kinematics model of the working device is a conversion model from pose space to joint space, which can convert the pose information of the bucket tooth tip into the joint angle information of the boom, bucket rod and bucket , and the inverse kinematics model of the working device is:

[0072] ;

[0073] In which:

[0074] ;

[0075] ;

[0076] ;

[0077] ;

[0078] In the formula, represents the line connecting the origins of the boom-bucket rod coordinate system and the line connecting the origin of the boom-shovel coordinate system ; denotes the angle between the line connecting the origin of the boom-shovel coordinate system and the line connecting the origin of the boom-shovel tooth tip coordinate system ; denotes the angle between the line connecting the origin of the boom-shovel tooth tip coordinate system and the horizontal plane; denotes the angle between the line connecting the origin of the stick-boom coordinate system and the line connecting the origin of the stick-shovel coordinate system ; denotes the distance between the origin of the boom coordinate system and the origin of the shovel coordinate system; denotes the distance between the origin of the boom coordinate system and the origin of the shovel tooth tip coordinate system.

[0079] 2. Dynamic model:

[0080] 1) Lagrange dynamic model;

[0081] In the process of establishing the dynamic model, it is assumed that the positions of the centers of mass of the various rods are known and that the friction of the mechanical transmission is not considered, and the digging resistance in the operation process is considered, which can be converted into a matrix form:

[0082] ;

[0083] In the formula, is the inertia matrix; is the square of the velocity term, which is called the centripetal force influence matrix; is the gravity influence matrix; is the resistance moment of the material on the shovel in the process of digging operation; denotes the joint angle ; denotes the joint torque ; denotes the joint angular velocity ; denotes the joint angular acceleration .

[0084] 2) Digging resistance model;

[0085] In order to simplify the calculation, the digging resistance is decomposed into the tangential resistance and the normal resistance along the trajectory of the shovel tooth tip:

[0086] ;

[0087] In the formula, and These represent the specific drag coefficient and the excavation resistance coefficient, respectively. Indicates the width of the inner side of the bucket. This indicates the working depth of the bucket teeth.

[0088] (ii) Regarding step S2;

[0089] The overall trench excavation task was carried out in the form of regional excavation, using three different types of trajectories to decompose the overall task, such as... Figure 3 As shown in (a), the main parameters include the starting point. ,high Total area and the slope angles of the two inclined planes and The decomposed single-excavation target areas are divided into three types: trapezoidal, irregular hexagonal, and parallelogram-shaped, such as... Figure 3 As shown in (b), (c), and (d), the dashed lines represent the target area, and the solid lines correspond to different excavation trajectories.

[0090] (III) Regarding step S3;

[0091] During excavator operation, the angles of the boom, stick, and bucket joints are easier to measure than the position and posture of the bucket teeth. Therefore, the digging trajectory of the working device is planned from the joint space. A sixth-order high-order polynomial is used to fit the joint angle variation curve of the bucket teeth between the start and end points of the digging trajectory. i 6th-order polynomial interpolation function for joint angles Represented as:

[0092] Formula 1: ;

[0093] In the formula, In order: boom, stick, and bucket. Indicating rods i Joint angle interpolation function j coefficient of the secondary term, t This indicates the time required to complete the trajectory execution;

[0094] rod i joint angular velocity and angular acceleration Differentiating Equation 1, we get:

[0095] Formula 2: ;

[0096] Formula 3: ;

[0097] The angular velocity and angular acceleration of each joint at the starting and ending points of the bucket tooth trajectory are set to 0 to avoid excessive impact on the excavator hardware.

[0098] After the joint angle values at the start and end points of the excavation trajectory are given, the 6th order polynomial interpolation function of each joint angle has only 、 、 and t 4 independent unknowns, so is set as the design variable of the optimization problem.

[0099] (Four) about step S4;

[0100] According to the working characteristics and requirements of the excavator, the normalized excavation time , the excavation rate and the operation energy consumption are taken as the objective functions, and the constraint conditions are set, including the physical model (kinematics and dynamics model) constraint, the trajectory angle change direction constraint and the angle between the bucket tooth back and the trajectory constraint;

[0101] The mathematical model of the excavator working device excavation trajectory optimization problem is established:

[0102] Formula 4: ;

[0103] In the formula, 、 and are the weights of 、 and , respectively, satisfying ; and represent the maximum and minimum threshold values of the joint angle ; and represent the threshold values of the joint angular velocity and the angular acceleration ; and are the power and torque required to drive the joint and overcome the excavation resistance during material excavation; and are the maximum values of the power and torque allowed by the driving system; is the angle between the tangent of the bucket tooth tip trajectory and the horizontal plane; is the trajectory angle before a time step; is the threshold value of the angle , and the time step is ;

[0104] The particle swarm optimization algorithm is used to optimize the trajectory parameters to determine the best solution, and the optimized trajectory joint motion information is collected as a data set.

[0105] (V) regarding step S5;

[0106] In order to ensure the real-time and adaptability of trajectory planning, the TD3 algorithm of deep reinforcement learning is used to train the trajectory dataset, which specifically includes building a deep neural network model, excavator material excavation physical environment model and reward function model;

[0107] 1) neural network model;

[0108] The observed state parameters related to the trajectory planning objective function and constraint conditions include joint angle , joint angular velocity , joint angular acceleration , joint angular jerk , time t , digging rate , work energy consumption , power , total torque , with joint angular jerk and its differential as action parameters to ensure , , The smoothness of the planned curve, and set the corresponding constraint conditions:

[0109] Equation 5: ;

[0110] In the formula, is the maximum joint angular jerk; is the maximum joint angular jerk differential;

[0111] The training of the TD3 continuous trajectory planning model requires two Critic networks and and an Actor network , wherein and have the same structure;

[0112] 2) excavator material excavation physical environment model;

[0113] The physical environment of the TD3 continuous trajectory planning model includes input / output interface modules, calculation modules (kinematics, dynamics and reward function model) and visualization modules.

[0114] 3) reward function model;

[0115] The task objectives and constraint conditions of the TD3 continuous trajectory planning model are embodied in the form of reward function, which is used to evaluate the performance of the excavator working device when executing trajectory planning.

[0116] Define termination conditions:

[0117] ① Normal termination (T1): including the bucket tooth tip height above the ground a certain height, attitude angle too large, too long digging time, too large actual digging volume, too large energy consumption, etc.

[0118] ② Punishment termination (T2): including the joint angle, angular velocity, angular acceleration, angular jerk constraints, as well as power, torque, trajectory angle, tooth back trajectory angle and bucket tooth tip transverse velocity constraints.

[0119] Single-step reward for target is:

[0120] Equation 6: ;

[0121] In the formula, is the reward accumulation function for the target; is the normalized weighting function of all target state quantities; is the ;

[0122] Function is expressed as:

[0123] Equation 7: ;

[0124] In the formula, , and are the weights of , and , is the state quantity normalized weighting target function of the digging rate , energy consumption and digging time t , and are the normalized target functions of the bucket tooth tip height and attitude ;

[0125] Sparse reward is introduced and expressed as:

[0126] Equation 8: ;

[0127] The total reward is:

[0128] Equation 9: ;

[0129] Finally, the total reward to complete the target area task; during the training process, the input is state parameter information, and the output is trajectory joint motion information.

[0130] The continuous trajectory planning strategy of the excavator in the overall working condition is as shown in Figure 2 and is as follows:

[0131] (1) input the overall task requirements, including the position, slope, area and depth of the overall target area;

[0132] (2) complete the overall task decomposition according to the decomposition strategy;

[0133] (3) determine the type and parameters of the single target task;

[0134] (4) initialize the pose and other state information of the excavator;

[0135] (5) output the excavating trajectory using the TD3 continuous trajectory planning model;

[0136] (6) complete the current excavating task by controlling the joints through the drive cylinder;

[0137] (7) sequentially plan the trajectory for the decomposed target area and perform the excavating action until the excavating task for the entire area is completed.

[0138] By designing three different overall trench excavating conditions, the TD3 continuous trajectory planning model and the particle swarm optimization algorithm model are compared and tested, and the results are shown in Table 1:

[0139] Table 1 Comparison of trajectory planning performance of TD3 continuous trajectory planning model and particle swarm optimization model

[0140]

[0141] The test results of Table 1 show that in terms of real-time performance, the TD3 continuous trajectory planning model exhibits extremely high efficiency. For each subtask, the average planning calculation time of this model is less than 0.5s, which indicates that the planning real-time performance of this model is good, ensuring that the excavator can quickly respond and adjust the trajectory during the working process, thereby greatly improving the working efficiency.

[0142] In terms of adaptability, by comparing the target area excavation rate , it is found that the particle swarm optimization algorithm model appears twice in three working conditions The digging rate is less than 90%, among which the subtask 1 in working condition 1 has a digging rate of only 81.7%, and the planning time exceeds 60s. The TD3 continuous trajectory planning model maintains a digging rate of more than 90% in all subtasks, which fully illustrates the good adaptability of the model under different working conditions. In terms of overall operation efficiency, by comprehensively considering the digging time, digging rate and energy consumption normalization function and index, it is found that the performance of the TD3 continuous trajectory planning model under three working conditions is better than that of the particle swarm optimization algorithm model. This further proves the superiority of the TD3 continuous trajectory planning model in the unmanned operation environment of the excavator.

[0143] In summary, the TD3 continuous trajectory planning model has significant advantages in real-time performance, adaptability and overall operation efficiency, and provides strong support for the unmanned operation of the excavator.

[0144] The above is only the preferred embodiment of the present application, it should be noted that for those skilled in the art, without departing from the concept of the present application, a number of modifications and improvements can be made, which should be regarded as the protection scope of the present application, these will not affect the effect and practicality of the patent.

Claims

1. A continuous trajectory planning method for an excavator working device, characterized in that, Includes the following steps: Step S1: Establish a simulation model of the excavator's working device, including the kinematic and dynamic models of the excavator's working device; Step S2: Overall task decomposition and mining strategy; Step S3: Plan the single excavation trajectory from the joint space using the curve interpolation method; Step S4: Use the particle swarm optimization algorithm to perform multi-objective optimization of the interpolation curve parameters and collect the optimized trajectory dataset; Step S5: Train the trajectory dataset from step S4 using the deep reinforcement learning TD3 algorithm; Step S6: For the overall excavation task, use the trained TD3 continuous trajectory planning model to perform continuous trajectory planning and generate the complete excavation trajectory of the target area. Step S5 includes building a deep neural network model, an excavator material excavation physical environment model, and a reward function model; First, a deep neural network model is built: This involves observing the state parameters related to the objective function and constraints of trajectory planning, including joint angles. Joint angular velocity Joint angular acceleration Joint angular acceleration ,time t , mining rate Energy consumption during operation ,power Total torque , with joint angle acceleration and its differential To ensure the motion parameters , , Smooth the curve according to the plan and set the corresponding constraints: ； In the formula, The maximum angular acceleration of the joint; The differential of the accelerometer at the maximum joint angle; Training the TD3 continuous trajectory planning model requires two Critic networks. and and an Actor network ,in and They have the same structure; Secondly, a physical environment model for excavator material excavation is constructed, which includes an input / output interface module, a calculation module, and a visualization module. Finally, the reward function model is built, as follows: Define two termination conditions: normal termination T1 and penalty termination T2, for the single-step reward of the target. for: ； In the formula, For the cumulative reward function targeting the objective; The normalized weighted function for all target state variables; It is a time step before ; function It is expressed as follows: ； In the formula, , and They are respectively , and The weight, For mining rate Energy consumption and excavation time t The state variable normalized weighted objective function, and These are the heights of the bucket teeth. and posture The normalized objective function; Introducing sparse rewards , is represented as: ； Total Rewards for: ； The final total reward To complete the training for the target area task; During training, the input is state parameter information, and the output is trajectory joint motion information.

2. The continuous trajectory planning method for excavator working device according to claim 1, characterized in that, In step S2, the overall trench excavation task is carried out in the form of regional excavation, and the overall task is decomposed using three different types of trajectories, namely trapezoidal, irregular hexagonal, and parallelogram.

3. The continuous trajectory planning method for an excavator working device according to claim 1, characterized in that, The specific process of step S3 is as follows: The joint angle variation curve of the bucket tooth tip between the start and end points of the digging trajectory was fitted using a sixth-order higher-order polynomial. i 6th-order polynomial interpolation function for joint angles Represented as: ； In the formula, In order: boom, stick, and bucket. Indicating rods i Joint angle interpolation function j coefficient of the secondary term, t This indicates the time required to complete the trajectory execution; rod i joint angular velocity and angular acceleration Differentiating from Equation 1, we get: ； ； Set the angular velocity and angular acceleration of each joint at the starting and ending points of the bucket tooth trajectory to 0; determine the unknowns of the sixth-order polynomial interpolation function for each joint angle as follows: , , and t ,Will Let them be the design variables for the optimization problem.

4. The continuous trajectory planning method for an excavator working device according to claim 1, characterized in that, The specific process of step S4 is as follows: Normalized mining time , mining rate and operating energy consumption As the objective function, constraints are also set, including physical model constraints, trajectory angle change direction constraints, and bucket tooth back and trajectory angle constraints. Establish a mathematical model for the excavator working device digging trajectory optimization problem: ； In the formula, , and They are respectively , and The weights satisfy ; and These represent the angles of each joint. Maximum and minimum thresholds; and Representing the angular velocities of each joint and angular acceleration The threshold; and These are the power and torque required to drive the joint rotation and overcome digging resistance during the material excavation process, respectively. and These are the maximum allowable power and torque of the drive system, respectively; The angle between the tangent of the bucket tooth tip trajectory and the horizontal plane; The trajectory angle one time step ago; Angle Threshold, time step ; The particle swarm optimization algorithm is used to find the optimal path and determine the trajectory parameters. The optimal solution was determined, and the optimized trajectory joint motion information was collected as a dataset.

Images