A real-time trajectory generation and tracking control method based on human-robot cooperation

Abstract

The application relates to the technical field of automation control, and particularly discloses a real-time trajectory generation and tracking control method based on man-machine cooperation, which comprises the following steps: dividing the operation trajectory of a working device of a excavator into a depth section and a trajectory section based on an operator input working mode, a target slope and a target depth; real-time solving of a target joint rotation angle of the trajectory section based on acquired current joint rotation angle information and in combination with a preset constraint condition; mapping the target joint rotation angle to a driving space of a hydraulic cylinder to obtain a target oil cylinder displacement; and calculating and outputting a final control quantity for driving a hydraulic system by using an incremental PID control algorithm based on the deviation between the target oil cylinder displacement and a current oil cylinder displacement. The operator input signal and sensor feedback are introduced into the trajectory generation process, so that the judgment of the operator under complex working conditions can be fully utilized, the trajectory precision and stability can be ensured through the algorithm, and efficient man-machine cooperation is realized.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of automation control technology, and in particular to a real-time trajectory generation and tracking control method based on human-machine cooperation. BACKGROUND

[0002] With the development of intelligent construction equipment, hydraulic excavators gradually evolve towards electric control and intelligence. Traditional excavators completely rely on the experience of operators to control the working device, which is flexible but has poor trajectory accuracy, low efficiency, and is heavily dependent on the skills of the operator. Although the full-automatic path planning method that has emerged in recent years can improve accuracy, it mostly relies on complex trajectory optimization and path interpolation algorithms, which have large computational loads and poor adaptability to environmental changes, and cannot meet the demand for real-time adjustment in the construction site. Some intelligent auxiliary control methods attempt to achieve semi-automatic operation by combining sensors and electric hydraulic systems, but due to the strong nonlinearity, strong coupling and parameter time-varying characteristics of the hydraulic system itself, these methods still have problems such as response lag, insufficient real-time performance and non-smooth trajectory in trajectory control, especially in typical flat ground and slope repair operation scenarios, the switching between trajectory segments and depth segments is unreasonable, which can easily cause impact and operation interruption. Therefore, how to realize lightweight and real-time trajectory generation while ensuring trajectory accuracy, and taking into account the participation and control of the operator, is a key problem that needs to be solved for the automatic control of electric hydraulic excavators.

[0003] Existing trajectory generation methods have obvious deficiencies in computational complexity, real-time performance, human-machine cooperation and trajectory switching smoothness. Firstly, traditional trajectory generation generally relies on offline planning or global optimization, which has large computational loads and is difficult to achieve real-time operation on embedded controllers, resulting in trajectory update lag and inability to respond to rapid changes in the construction site. Secondly, some methods directly interpolate trajectories in the Cartesian space without considering the kinematic characteristics and constraints of typical excavator operations, resulting in low efficiency in the trajectory generation process, and making it difficult to balance real-time performance and accuracy. Thirdly, existing trajectory generation methods generally adopt a fully automated approach, ignoring the value of operator input and lacking a human-machine cooperation mechanism, which results in a lack of timely adjustment in complex working conditions and insufficient operation flexibility and robustness. Fourthly, in flat ground and slope repair operations, depth segments and trajectory segments often need to be switched continuously, and traditional methods lack reasonable buffering and discrimination conditions, which can easily cause arm impact and vibration during switching, affecting operation smoothness and stability. SUMMARY

[0004] The present application aims to provide a real-time trajectory generation and tracking control method based on human-machine cooperation 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 method comprises:

[0007] dividing a working track of a excavator working device into a depth section and a track section based on an operator inputted working mode, a target slope and a target depth;

[0008] solving a target joint rotation angle of the track section in real time based on acquired current joint rotation angle information and in combination with preset constraint conditions, wherein the working mode is a linkage mode in which an operator controls a boom joint movement and a system cooperatively controls a stick joint and a bucket joint;

[0009] mapping the target joint rotation angle to a driving space of a hydraulic cylinder to obtain a target cylinder displacement;

[0010] based on a deviation between the target cylinder displacement and a current cylinder displacement, using an incremental PID control algorithm to calculate and output a final control quantity for driving a hydraulic system.

[0011] As a further scheme of the present application, the step of solving the target joint rotation angle of the track section in real time based on the acquired current joint rotation angle information and in combination with the preset constraint conditions specifically comprises: setting the constraint conditions as follows when the working mode is the two-linkage mode:

[0012] ;

[0013] wherein φ is a set target slope angle, is an initial bucket rotation angle, is a current stick rotation angle, (x st ,y st ) is an initial coordinate of a bucket tip, ( , ) is a target bucket tip coordinate;

[0014] solving a target boom rotation angle in real time based on the constraint conditions .

[0015] As a further scheme of the present application, the step of solving the target joint rotation angle of the track section in real time based on the acquired current joint rotation angle information and in combination with the preset constraint conditions specifically comprises: setting the constraint conditions as follows when the working mode is the three-linkage mode:

[0016] ;

[0017] wherein φ is a set target slope angle, ksi st is an initial value of an angle between the bucket and a horizontal line, is a current stick rotation angle, (x st ,y st ) is an initial coordinate of a bucket tip, (x ob3 ,yob3 () represents the coordinates of the target beacon tip;

[0018] The target boom rotation angle is solved in real time based on the second constraint condition. Turning at the target bucket ;

[0019] As a further embodiment of the present invention, the formula for calculating the target bucket rotation angle is as follows:

[0020]

[0021] As a further embodiment of the present invention, the step of calculating and outputting the final control quantity for driving the hydraulic system based on the deviation between the target cylinder displacement and the current cylinder displacement using an incremental PID control algorithm specifically includes:

[0022] Calculate the control deviation for the current sampling period: Where x(k) is the displacement of the hydraulic cylinder, The target cylinder displacement.

[0023] Calculate the deviation increment: ;

[0024] Calculate the output change of incremental PID control: Where Δμ(k) is the varying output control quantity, and K p ,K d ,K i These are the three control coefficient matrices for incremental PID control;

[0025] Calculate the final control value: , where u bi The basic control variable is μ(k), which is the control variable of the final output, including the pump current and valve current of the final output.

[0026] Compared with existing technologies, the advantages of this invention are as follows: By dividing trajectory generation into depth segments and trajectory segments, and utilizing constraints for rapid inverse solving, this invention ensures concise and efficient trajectory generation calculations, significantly improving real-time performance. Simultaneously, by introducing operator input signals and sensor feedback into the trajectory generation process, it can fully utilize the operator's judgment under complex working conditions, while also ensuring trajectory accuracy and stability through algorithms, achieving efficient human-machine collaboration. Attached Figure Description

[0027] To more clearly illustrate the technical solutions in the embodiments of the present invention, the accompanying drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the accompanying drawings described below are only some embodiments of the present invention.

[0028] Figure 1This invention provides a trajectory generation process for a real-time trajectory generation and tracking control method based on human-machine collaboration.

[0029] Figure 2 The trajectory tracking control process provided in the embodiments of the present invention.

[0030] Figure 3 This is a time variation diagram of the joint target rotation angle under the two-linkage working mode provided in an embodiment of the present invention.

[0031] Figure 4 The RTB simulation trajectory diagram is provided in the two-linkage working mode of the present invention.

[0032] Figure 5 The diagram shows the time variation of the joint target rotation angle under the three-linkage working mode provided in the embodiment of the present invention.

[0033] Figure 6 The RTB simulation trajectory diagram is provided in the three-linkage working mode of the present invention. Detailed Implementation

[0034] To make the technical problems to be solved, the technical solutions, and the beneficial effects of the present invention clearer, the present 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 of the present invention and are not intended to limit the present invention.

[0035] In this embodiment of the invention, a real-time trajectory generation and tracking control method based on human-machine collaboration is provided, the method comprising:

[0036] The method includes:

[0037] Based on the operator-inputted working mode, target slope, and target depth, the working trajectory of the excavator's working device is divided into depth segments and trajectory segments;

[0038] Based on the acquired current joint rotation angle information and combined with preset constraints, the target joint rotation angle of the trajectory segment is solved in real time. The working mode is a linkage mode in which the operator controls the movement of the boom joint and the system coordinates the control of the linkage between the boom joint and the bucket joint.

[0039] The target joint angle is mapped to the driving space of the hydraulic cylinder to obtain the target cylinder displacement;

[0040] Based on the deviation between the target cylinder displacement and the current cylinder displacement, an incremental PID control algorithm is used to calculate and output the final control quantity used to drive the hydraulic system.

[0041] The step of solving the target joint angle of the trajectory segment in real time based on the acquired current joint angle information and combined with preset constraints, in the two-linkage mode, mainly uses sensors to extract the excavator's pose in the trajectory solution of the electro-hydraulic excavator. The two-linkage mode maintains the initial value of the bucket angle. The stick handle remains unchanged; it is controlled by the operator, while the stick angle is determined based on real-time feedback from the stick IMU. The target angle of the boom is determined by inverse kinematics. To meet the needs of leveling or slope repair work.

[0042] Therefore, the constraints that can be set for the trajectory segment are:

[0043] ;

[0044] Where φ is the target slope angle set; when φ is 0°, it indicates operation on flat ground. The initial angle of the bucket, For the current stick angle, (x) st ,y st ) is the initial coordinate of the beacon tip, ( , The third constraint represents the target bucket tip coordinates in the two-linkage working mode, which must meet the coordinate requirements for linear operation. , );

[0045] The target boom rotation angle is solved in real time based on the aforementioned constraints. .

[0046] The relationship between the bucket tip coordinates (x, y) and the joint rotation angles q1, q2, q3 is as follows:

[0047] ;

[0048] ;

[0049] (x st ,y st The initial coordinates of the beacon tip can be obtained by adjusting the initial attitude angle (q1). st ,q2 st ,q3 st Substituting into the above equation to solve, similarly ( , It can also be done through (q1) st ,q2 st ,q3 st Please provide a solution.

[0050] Let the intermediate variables A2, B2, M2, and δ2 be:

[0051] ;

[0052] ;

[0053] ;

[0054] ;

[0055] The target boom rotation angle can then be calculated. for:

[0056] ;

[0057] When the target depth is not 0, the bucket tip should enter the depth segment first and then the trajectory segment. Since the trajectory segment accounts for a small proportion and takes up less time in actual operation, if the target boom angle is calculated in this case, the boom will face the switching between lifting and lowering in a very short time during control, causing a serious impact. Therefore, the target boom angle is not calculated in the depth segment.

[0058] Therefore, the trajectory of the two-linkage depth segment does not need to be solved separately, but is achieved simply by the operator pulling the stick. In the stated depth segment, keeping the boom joint at its initial rotation angle, the condition for transitioning from the depth segment to the trajectory segment is:

[0059] ;

[0060] That is, the current coordinates of the bucket tip are below the set target slope line.

[0061] The three-linkage working mode also involves the operator pulling the stick handle, and the stick angle is adjusted in real time via IMU feedback. Real-time reverse engineering of the boom target rotation angle under the three-linkage working mode and the target corner of the bucket This is to ensure that leveling and slope repair operations can be carried out, while maintaining the angle ksi between the bucket and the horizontal line.

[0062] Based on the acquired current joint angle information and combined with preset constraints, the step of solving the target joint angle of the trajectory segment in real time specifically includes, when the working mode is the three-linkage mode: setting the constraints as follows:

[0063] ;

[0064]

[0065] Where φ is the target slope angle set, and ksi st This is the initial value of the angle between the bucket and the horizontal line. For the current stick angle, (x) st ,yst (x) represents the initial coordinates of the beacon tip. ob3 ,y ob3 () represents the coordinates of the target beacon tip;

[0066] The target boom rotation angle is solved in real time based on the second constraint condition. Turning at the target bucket ;

[0067] Similarly, let the intermediate variables A3, B3, M3, and δ3 be:

[0068] ;

[0069] ;

[0070] ;

[0071] ;

[0072] Finally, the real-time target rotation angle is obtained. , :

[0073] ;

[0074] ;

[0075] Similar to the two-linkage method, when the depth setting of the three-linkage working mode is not 0, q1=q1 is also maintained in the depth segment. st However, because the three-linkage mechanism needs to keep the ksi angle constant, the target angle of the bucket needs to be calculated in real time during the depth segment. The constraints at this point are:

[0076] ;

[0077] ;

[0078] In addition, because the three-linkage working mode is greatly affected by the initial value and slope, when calculating the target, if the angle value of a target exceeds the upper and lower limits of the allowable angle, it should still be allowed to continue working. If the angle of a target reaches the limit, it should be temporarily maintained. Time makes This is because the ksi angle cannot be maintained due to the restrictions imposed on it; when the ksi angle is different from the initial... The difference exceeds a certain limit, for example At that point, real-time target solving will cease, and a message indicating job completion will be displayed.

[0079] The step of mapping the target joint angle to the drive space of the hydraulic cylinder to obtain the target cylinder displacement specifically includes:

[0080] Calculate the boom cylinder displacement LEF by solving the boom rotation angle q1:

[0081] ;

[0082] Calculate the boom cylinder displacement LGH by boom rotation angle q2:

[0083] ;

[0084] ;

[0085] Calculation of bucket cylinder displacement LIJ for bucket rotation angle q3:

[0086] ;

[0087] ;

[0088] ;

[0089] ;

[0090] ;

[0091] ;

[0092] ;

[0093] Therefore, the initial joint rotation angle (q1) can be used to determine the initial joint angle. st ,q2 st ,q3 st Calculate the initial cylinder displacement (LEF) st ,LGH st ,LIJ st Similarly, the current cylinder displacement (LEF) can also be calculated from the joint rotation angle. cur ,LGH cur ,LIJ cur ) and target cylinder displacement (LEF) ob ,LGH ob ,LIJ ob Since the actual control variable is a discrete variable, the current cylinder displacement and the target cylinder displacement should be expressed as:

[0094] ;

[0095] ;

[0096] Where k represents the current sampling period number, and LGH is not in the matrix because it only controls the boom and bucket.

[0097] For electro-hydraulic excavators, the process typically involves pulling a handle to generate pilot pressure. Based on this pilot pressure, pump and valve currents are calculated to control the flow rate of the variable displacement pump and the opening of the main hydraulic valve. This, in turn, controls the flow rate to each hydraulic cylinder, thus controlling the cylinder's movement speed. Therefore, the input control quantities should be the pump current and valve current.

[0098] Based on the target speed of the hydraulic cylinder, the base pump current and valve current required to reach the target speed can be calculated.

[0099] First, the basic target flow rate Q can be calculated. bi Matrix, Q bi =(Q biboom Q bibucket ), where Q biboom Q is the flow rate required for the boom. bibucket Required flow rate for the bucket:

[0100] ;

[0101] Where k work This is a working condition correction coefficient matrix, related to the working conditions of the linkages controlled by the hydraulic cylinder. For example, when the boom is rising, it needs to overcome gravity and load inertia, so this coefficient will be larger than when the boom is falling. k cyl The actuator correction coefficient matrix is ​​related to the load characteristics of the hydraulic cylinder. A eff Let v be the effective area matrix of the hydraulic cylinder. ob The target cylinder velocity matrix is ​​obtained through the target cylinder displacement. The result is obtained by performing a difference operation.

[0102] Further calculation of the required pump current matrix I pump To drive the pump to output a matching flow rate, the mapping relationship between the pump current and the target flow rate is as follows:

[0103] ;

[0104] Where a p This is the conversion factor matrix between pump flow rate and current, obtained from pump calibration. Specifically, it represents the conversion relationship between the pump's maximum / minimum flow rate and maximum / minimum pump current, also obtained from pump calibration. k pump This is the pump correction coefficient matrix, used to compensate for the effects of pressure, speed, and oil temperature on pump efficiency. Generally, under high pressure and high load conditions, its value is larger to ensure sufficient flow.

[0105] Basic pilot pressure P bi Matrix and basic valve current I vbiMatrix calculations also employ this mapping relationship:

[0106] ;

[0107] ;

[0108] Where a pp β is the conversion coefficient matrix between pump current and pilot pressure. pp This is the bias term matrix, used to compensate for the zero-point offset of the pilot pressure; a vp Here is the conversion coefficient matrix from pilot pressure to valve current, k v This is the valve correction coefficient matrix, used to compensate for the nonlinear characteristics of the valve under different temperature, wear, and flow ranges.

[0109] After calculating the pump current, pilot pressure, and valve current, this invention further introduces an incremental PID control algorithm to perform closed-loop correction on the actual motion state of the hydraulic actuator. This control method can effectively suppress the nonlinear characteristics and parameter uncertainties in the hydraulic system, ensuring that the working device can operate strictly according to the target trajectory.

[0110] The step of calculating and outputting the final control quantity for driving the hydraulic system based on the deviation between the target cylinder displacement and the current cylinder displacement using an incremental PID control algorithm specifically includes:

[0111] Calculate the control deviation for the current sampling period: Where x(k) is the displacement of the hydraulic cylinder, The target cylinder displacement.

[0112] Calculate the deviation increment: ;

[0113] Calculate the output change of incremental PID control: Where Δμ(k) is the varying output control quantity, and K p ,K d ,K i These are the three control coefficient matrices for incremental PID control;

[0114] Calculate the final control value: , where u bi The basic control variable is μ(k), which is the control variable of the final output, including the pump current and valve current of the final output.

[0115] To ensure the bucket tip can achieve a linear operating trajectory, the hydraulic cylinder displacement should be adjusted. (i.e., the target values ​​of LAB, LBC, and LCD in the trajectory generation section) are used as the target value, and the difference between it and the current displacement x(k) is used as the control deviation e. k :

[0116] ;

[0117] As an actuator, the dynamics of a hydraulic cylinder are influenced by valve control nonlinearity, hydraulic fluid compressibility, friction, and external load disturbances, exhibiting strong nonlinearity, strong coupling, and time-varying parameters. In this context, traditional position-based PID control directly applies the parameters e... k Using position deviation increments as control variables can easily lead to problems such as hysteresis, large overshoot, and inability to eliminate steady-state errors. Incremental PID control, however, addresses these issues by controlling the position deviation increment ∆e. k As a control variable, only the error change between adjacent time points is calculated, without relying on the absolute error integral. This avoids abrupt changes in the control signal caused by nonlinearity, thereby improving system stability. In some embedded controllers, computational resources are limited. Positional PID requires calculating a large range of integrals, resulting in high storage pressure; while incremental PID only involves differential operations and a limited historical deviation, with low computational complexity, making it highly suitable for real-time control scenarios. The output control variable of the incremental PID in this invention can be expressed by the following formula:

[0118] ;

[0119] ;

[0120] ;

[0121] Where Δμ(k) is the varying output control quantity, K p ,K d ,K i These are the three control coefficient matrices for incremental PID control, u bi For the previously calculated base value (I) pbi ,I vbi ), μ(k) is the final output control quantity [I p (k),I v [(k)], which refers to the final output pump current and valve current.

[0122] This invention utilizes lightweight modeling and segmented design to divide trajectory generation into depth and trajectory segments, and employs constraint conditions for rapid inverse kinematics, ensuring concise and efficient trajectory generation calculations and significantly improving real-time performance. Simultaneously, operator input signals and sensor feedback are incorporated into the trajectory generation process, fully leveraging the operator's judgment under complex working conditions while ensuring trajectory accuracy and stability through algorithms, achieving efficient human-machine collaboration. Furthermore, this invention adds discrimination conditions and buffering mechanisms during the switching between depth and trajectory segments, effectively avoiding the impact caused by frequent boom raising and lowering, ensuring the continuity and smoothness of the operation. Through these improvements, this invention enables efficient and stable real-time trajectory generation in an embedded environment, significantly improving the operational accuracy and efficiency of electro-hydraulic excavators in leveling and slope repair operations.

[0123] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A real-time trajectory generation and tracking control method based on human-machine collaboration, characterized in that, The method includes: Based on the operator-inputted working mode, target slope, and target depth, the working trajectory of the excavator's working device is divided into depth segments and trajectory segments; Based on the acquired current joint rotation angle information and combined with preset constraints, the target joint rotation angle of the trajectory segment is solved in real time. The working mode is a linkage mode in which the operator controls the movement of the boom joint and the system coordinates the control of the linkage between the boom joint and the bucket joint. The target joint angle is mapped to the driving space of the hydraulic cylinder to obtain the target cylinder displacement; Based on the deviation between the target cylinder displacement and the current cylinder displacement, an incremental PID control algorithm is used to calculate and output the final control quantity used to drive the hydraulic system.

2. The real-time trajectory generation and tracking control method based on human-machine collaboration as described in claim 1, characterized in that, The step of solving the target joint angle of the trajectory segment in real time based on the acquired current joint angle information and in combination with preset constraints, specifically includes the following when the working mode is the two-linkage mode: setting the constraints as follows: ； Where φ is the target slope angle set. The initial angle of the bucket, For the current stick angle, (x) st ,y st ) is the initial coordinate of the beacon tip, ( , () represents the coordinates of the target beacon tip; The target boom rotation angle is solved in real time based on the aforementioned constraints. .

3. The real-time trajectory generation and tracking control method based on human-machine collaboration as described in claim 1, characterized in that, Based on the acquired current joint angle information and combined with preset constraints, the step of solving the target joint angle of the trajectory segment in real time specifically includes, when the working mode is the three-linkage mode: setting the constraints as follows: ； Where φ is the target slope angle set, and ksi st This is the initial value of the angle between the bucket and the horizontal line. For the current stick angle, (x) st ,y st (x) represents the initial coordinates of the beacon tip. ob3 ,y ob3 () represents the coordinates of the target beacon tip; The target boom angle is solved in real time based on the second constraint condition. Turning at the target bucket .

4. The real-time trajectory generation and tracking control method based on human-machine collaboration as described in claim 2, characterized in that, When the target depth is not zero, the boom joint is kept at the initial rotation angle within the depth range. The condition for switching from the depth segment to the trajectory segment is: the current coordinates of the bucket tip are below the set target slope line.

5. The real-time trajectory generation and tracking control method based on human-machine collaboration as described in claim 3, characterized in that, When the target depth is not zero, the constraint condition is set as follows for the depth segment: ； It also calculates the target bucket angle in the depth range in real time.

6. The real-time trajectory generation and tracking control method based on human-machine collaboration as described in claim 5, characterized in that, The formula for calculating the target bucket rotation angle is: 。 7. The real-time trajectory generation and tracking control method based on human-machine collaboration as described in claim 1, characterized in that, The step of calculating and outputting the final control quantity for driving the hydraulic system based on the deviation between the target cylinder displacement and the current cylinder displacement using an incremental PID control algorithm specifically includes: Calculate the control deviation for the current sampling period: Where x(k) is the displacement of the hydraulic cylinder, The target cylinder displacement; Calculate the deviation increment: ; Calculate the output change of incremental PID control: Where Δμ(k) is the varying output control quantity, and K p ,K d ,K i These are the three control coefficient matrices for incremental PID; Calculate the final control value: , where u bi The basic control variable is μ(k), which is the final output control variable, including the final output pump current and valve current.

Images