An excavator and a trajectory optimization planning control system thereof

Abstract

This invention relates to the field of automated control of construction machinery, and discloses an excavator and its trajectory optimization planning and control system. The system includes a slewing platform, a boom rotatably connected to the center of the slewing platform, a stick rotatably connected to the center of the boom, and a bucket rotatably connected to one end of the stick. To drive the working device, a connecting rod is rotatably connected to the outer side of the stick, a rocker arm is rotatably connected to the center of the connecting rod, and one end of the rocker arm is rotatably connected to the outer side of the bucket. The actuator includes: a first hydraulic cylinder rotatably connected to the top of the slewing platform, with its output end rotatably connected to the outer side of the boom; and a second hydraulic cylinder rotatably connected to the outer side of the boom. This invention can flexibly balance operating efficiency and energy consumption according to different operational needs, and improve the smoothness of the motion trajectory and reduce mechanical impact through diagonal acceleration constraints, providing a reliable control foundation for the autonomous, efficient, and stable operation of the excavator.

Description

Technical Field

[0001] This invention relates to the field of automated control of construction machinery, specifically to an excavator and its trajectory optimization planning and control system. Background Technology

[0002] As a key piece of construction machinery, the automation and intelligentization of excavators represent a significant trend in the industry's development. Trajectory optimization planning is the core technology for enabling autonomous excavator operation; its goal is to generate an optimal motion trajectory from start to finish while satisfying various physical constraints.

[0003] However, existing excavator trajectory planning technologies still have many shortcomings. First, traditional trajectory optimization methods often employ a single, fixed optimization objective, such as simply pursuing the shortest possible operation time, while ignoring energy consumption. This makes the system inflexible when facing diverse actual working conditions. In scenarios where efficiency is not a priority, this method leads to unnecessary energy waste; conversely, if energy saving is the sole consideration, it cannot meet the requirements of high-intensity, fast-paced operations. Existing technologies lack an effective mechanism that can dynamically and collaboratively optimize the balance between operational efficiency and energy consumption based on actual needs.

[0004] Furthermore, during trajectory generation, the excavator's working device must strictly adhere to its own kinematic and dynamic constraints, such as the speed, acceleration, and torque limits of each joint. Many existing technologies, when dealing with these complex nonlinear constraints, fail to adequately consider the smoothness of motion, particularly neglecting the control of angular jerk. This results in abrupt acceleration changes in the generated trajectory, causing impacts and vibrations in the mechanical structure. This not only accelerates equipment aging and wear but also reduces trajectory tracking accuracy and operational stability.

[0005] At the algorithm level, some existing technologies rely on heuristic algorithms to find the optimal trajectory. Although such algorithms can provide feasible solutions in some cases, they generally suffer from inherent defects such as slow convergence speed and susceptibility to getting trapped in local optima. This results in the inability to guarantee the global optimality of the obtained trajectory, and the algorithms lack robustness, making it difficult to meet the requirements of high reliability and high precision autonomous operation. This constitutes a technical bottleneck in achieving truly efficient and stable autonomous control of excavators. Summary of the Invention

[0006] To address the shortcomings of existing technologies, this invention provides an excavator and its trajectory optimization planning and control system, which solves the problems of existing technologies having a single objective in trajectory planning and difficulty in coordinating and optimizing work efficiency and energy consumption according to actual working conditions.

[0007] To achieve the above objectives, the present invention provides the following technical solution:

[0008] The first aspect of this invention provides an excavator trajectory optimization planning and control system, comprising:

[0009] The model building module is used to build kinematic and dynamic models of the excavator's working device, which consists of the boom, stick, and bucket.

[0010] The optimization target setting module is used to establish a collaborative optimization objective function that includes energy consumption and efficiency;

[0011] The constraint processing module is used to set the kinematic and dynamic constraints of the excavator working device (i.e., boom, stick, and bucket, etc.), and to perform pseudo-constraint transformation processing on the path constraints to form unified boundary constraint conditions.

[0012] The trajectory planning and solving module is used to solve for the optimal trajectory based on the kinematic model and dynamic model, the cooperative optimization objective function and the boundary constraints.

[0013] The trajectory tracking control module is used to generate and output control commands to the actuators of the excavator's working device, namely cylinder one, cylinder two, and cylinder three, based on the optimal trajectory.

[0014] In one specific implementation, the model building module is specifically used to: establish a multi-rigid-body kinematic model of the excavator's working device using the Denavit-Hartenberg parametric method. (Joint coordinate systems) Relative to the previous joint coordinate system Transformation matrix It can be described by the following formula:

[0015] ;

[0016] In the formula: Joint coordinate system Relative to the previous joint coordinate system The homogeneous transformation matrix; Indicates the joint angle of rotation about the Z-axis ; Indicates the joint offset along the Z-axis. ; Indicates the length of the translational joint along the X-axis. ; Indicates the joint torsion angle about the X-axis. .

[0017] The rotation and translation transformation matrices are defined as follows:

[0018] ;

[0019] ;

[0020] ;

[0021] ;

[0022] In the formula: Joint angle The value of the cosine function; Joint angle The value of the sine function; Joint angle The negative value of the sine function; Joint torsion angle The value of the cosine function; Joint torsion angle The value of the sine function; Joint torsion angle The negative value of the sine function.

[0023] Based on the DH parameters, the forward and inverse kinematic equations of the excavator's end effector can be obtained, which are represented by the following set of formulas:

[0024] ;

[0025] In the formula: The three-dimensional coordinates of the excavator's end effector in the base coordinate system; This refers to the attitude angle of the excavator's end face; , They are respectively and abbreviation; They are respectively and abbreviation; They are respectively and abbreviation; The length of each joint; The height offset of the base; For the angles of each joint.

[0026] ;

[0027] In the formula: Given the coordinates of the end target position; Given the end target attitude angle; The angles of each joint to be solved; These are the length parameters of each joint; This is the joint offset of the first joint, which physically corresponds to the height of the excavator's rotating base; They are respectively The abbreviated form; for The abbreviated form; It is a four-quadrant arctangent function, which determines the quadrant of the angle based on the sign of the input parameter, thus obtaining a unique and correct angle value.

[0028] The model building module is also used to establish a dynamic model of the excavator's working device using the Lagrange equation, the general form of which is:

[0029] ;

[0030] In the formula: For joints The driving torque is generated by the corresponding hydraulic cylinder one, hydraulic cylinder two or hydraulic cylinder three through the hydraulic system; These are the joint angle, angular velocity, and angular acceleration vectors, respectively. Here is the system's inertia matrix; These are coefficients related to centrifugal force and Coriolis force. This is the term related to gravity. Let i be the actuator rotor inertia; This is the friction term.

[0031] Drive torque for excavator boom, stick, and bucket joints , , The specific expression is:

[0032] ;

[0033] ;

[0034] ;

[0035] In the formula: These refer to the masses of the boom, stick, and bucket, respectively. These are the moments of inertia of the boom, stick, and bucket about their respective centers of mass; These refer to the lengths of the boom and stick, respectively. These are the distances from the joint axes of the boom, stick, and bucket to their respective centers of mass; These are the geometric angles of the center of mass positions of the boom, stick, and bucket relative to the connecting rod axis, respectively. Gravitational acceleration constant; These are the relative rotation angles of the boom, stick, and bucket joints, respectively. The absolute angle of the pole is defined as... ; The sum of the angles between the boom and the bucket is defined as follows: ; The absolute angle of the bucket is defined as... ; These are the first derivative (angular velocity) and second derivative (angular acceleration) for the corresponding angles, for example, It is the square of the absolute angular velocity of the boom (5). It is the absolute angular acceleration of the bucket (9); These are the cosine function and the sine function, respectively. They are respectively and The abbreviated form of .

[0036] Furthermore, the energy consumption in the optimization target setting module is characterized by integrating the absolute value of the product of the driving torque and angular velocity of each joint (i.e., boom, stick, and bucket); the efficiency is characterized by the ratio of the effective work done in the digging process to the operating time. This collaborative optimization objective function comprehensively considers both the shortest operating time (efficiency target) and the lowest total energy consumption (energy consumption target), and its mathematical expression is as follows:

[0037] ;

[0038] In the formula: It is a minimization operator; This indicates that the expression within the parentheses is integrated along the entire geometric path; This represents the goal of optimizing task time; This represents the goal of optimizing energy consumption; For the normalized parameters of the geometric path, ; The square of the pseudo-velocity, i.e. ,in Path parameters Regarding time The first derivative; It is pseudo-acceleration, that is ; For joints Along the path The driving torque; This is a weighting coefficient used to balance time and energy consumption targets.

[0039] Specifically, the constraint processing module is used to concretize the kinematic and dynamic constraints of the excavator's working device into state variable constraints and control variable constraints. The state variable constraints include angle constraints, angular velocity constraints, and angular acceleration constraints for each joint (i.e., boom, stick, and bucket); the control variable constraints are the driving torque constraints for each joint. Their mathematical expression is as follows:

[0040] ;

[0041] In the formula: Joints In time Angle, angular velocity, and angular acceleration; For joints Angular limit; Joints The maximum angular velocity and maximum angular acceleration.

[0042] The constraint processing module also performs pseudo-constraint transformation on the path constraints during the mining process. Pseudo-path parameters are introduced, and the square of the pseudo-velocity is defined. and pseudo-acceleration The relationship between the two is Through this transformation, the original nonlinear angular velocity and angular acceleration constraints are converted into constraints concerning variables. and Linear constraints:

[0043] ;

[0044] In the formula: Joint angle For path parameters The first derivative.

[0045] Angular acceleration constraint transformation:

[0046] ;

[0047] In the formula: Joint angle For path parameters The second derivative of .

[0048] Preferably, to improve motion smoothness, a pseudo-constraint transformation of angular jerk (Jerk) is introduced:

[0049] ;

[0050] In the formula: The maximum angular acceleration of joint i; , Path parameters Regarding time First, second, and third derivatives; Joint angle For path parameters The third derivative of .

[0051] After the pseudo-constraint transformation process, the path constraint, the state variable constraint, and the control variable constraint together constitute a unified boundary constraint condition.

[0052] In one embodiment, the trajectory planning solution module embeds a pseudospectral method to transform the optimal trajectory solution problem into a nonlinear programming problem. After discretization, this problem ultimately involves solving for a set of variables. , The objective function is minimized while satisfying a series of algebraic constraints. The resulting optimization problem is as follows:

[0053] ;

[0054] In the formula: This represents the total number of discrete points along the path. For the index of discrete points; For discrete points and The pseudo-velocity squared value at the location; For the first The length of a discrete path segment; For the first The weighting coefficient of the segment; For joints In the The driving torque of the segment; For joints The normalized torque.

[0055] The constraints are:

[0056] ;

[0057] ;

[0058] ;

[0059] ;

[0060] ;

[0061] ;

[0062] ;

[0063] ;

[0064] In the formula: In the A discrete point along the path, applied to the joint. The driving torque; No. The discrete point and the first The midpoint path position between discrete points; These are the dynamic coefficients or functions related to the inertia term, centrifugal / Coriolis force term, and gravity term, respectively. In the The pseudo-acceleration at a discrete point, i.e. ; In the and The square of the pseudo-velocities at the midpoints of the discrete segments, i.e. ; These are the squared pseudo-velocities and pseudo-accelerations at the starting point of the path, respectively. These are the squared pseudo-velocities and pseudo-accelerations at the end of the path, respectively. These are the actual velocity and actual acceleration at the starting point of the path, respectively. These are the actual velocity and actual acceleration at the end of the path, respectively. The first and the The squared pseudo-velocities at discrete points; For the first Pseudo-acceleration at discrete points; For from the first Point to number Path step size of a point; No. The squared pseudo-velocities at discrete points; because The actual speed The square of is always a non-negative number; Path points The square of the pseudo-velocity at that point; joint Maximum permissible angular velocity; joint Angle For path parameters The first derivative, i.e. ; Path points The pseudo-acceleration at that point; joint Maximum permissible angular acceleration; Path points The square of the pseudo-velocity at that point; These are the joint angles For path parameters The first and second derivatives; Represents at discrete points and The pseudo-acceleration at the point affects the path parameters derivative Or its related quantities. The rate of change is directly related to the actual Jerk motion; for , indicating the first The path step size of the segment; They represent path parameters respectively Regarding time The third derivative The lower and upper limits; No. The actual speed of the point The entire expression transforms the complex constraints on Jerk into an algebraic inequality constraint on the pseudo-acceleration rate of change.

[0065] Preferably, the trajectory planning and solving module further includes a weighting coefficient allocation unit, used to assign weighting coefficients to energy consumption and efficiency in the collaborative optimization objective function. The allocation. By adjusting The value of can be used to obtain the optimal trajectory with different emphases (emphasis on time or emphasis on energy consumption) to control the coordinated movement of the boom, stick and bucket.

[0066] Furthermore, the trajectory tracking control module specifically includes: a status feedback unit, used to acquire the actual motion state of the excavator working device (i.e., boom, stick, and bucket) in real time; a deviation calculation unit, used to calculate the deviation between the actual motion state and the optimal trajectory; and a controller unit, used to generate the control command based on the deviation and output it to the actuators (cylinder one, cylinder two, and cylinder three) of the excavator working device to form a closed-loop control.

[0067] Specifically, the state feedback unit includes multiple angle sensors respectively installed at various joints of the excavator working device (such as the connection between the slewing platform and the boom, the connection between the boom and the stick, etc.); the angle sensors are used to detect the actual rotation angle of each joint in real time, and to obtain the actual motion state of the excavator working device.

[0068] A second aspect of the present invention provides an excavator, the structure of which includes: a slewing platform, a boom rotatably connected to the center of the slewing platform, a stick rotatably connected to the center of the boom, and a bucket rotatably connected to one end of the stick. To drive the working device, a connecting rod is rotatably connected to the outer side of the stick, a rocker arm is rotatably connected to the center of the connecting rod, and one end of the rocker arm is rotatably connected to the outer side of the bucket. The actuator includes: a first hydraulic cylinder rotatably connected to the top of the slewing platform, its output end rotatably connected to the outer side of the boom; a second hydraulic cylinder rotatably connected to the outer side of the boom, its output end rotatably connected to the end of the stick away from the bucket; and a third hydraulic cylinder rotatably connected to the outer side of the stick, its output end rotatably connected to the center of the connecting rod.

[0069] This invention provides an excavator and its trajectory optimization planning and control system. It has the following beneficial effects:

[0070] 1. This invention establishes a collaborative optimization objective function that includes energy consumption and efficiency, and introduces adjustable weighting coefficients. This allows for a trade-off between operation time and energy consumption under different operational requirements. For example, in situations where high efficiency is prioritized, the weighting coefficient can be increased to shorten operation time; conversely, in situations where energy consumption is critical, the weighting coefficient can be decreased to reduce energy consumption. This effectively adapts to various real-world operational scenarios, enhancing the system's flexibility and practicality.

[0071] 2. This invention employs a pseudo-constraint transformation technique to convert the original nonlinear angular velocity, angular acceleration, and even angular jerk constraints into linear boundary constraints concerning pseudo-velocities and pseudo-accelerations. This approach not only simplifies the problem-solving process but, more importantly, effectively suppresses abrupt changes in velocity and acceleration along the trajectory by constraining the angular jerk (Jerk), resulting in a smoother motion trajectory for the excavator's working device, reducing impact and vibration on the mechanical structure, and simultaneously improving trajectory tracking accuracy.

[0072] 3. This invention transforms the complex trajectory optimization problem into a structured nonlinear programming problem and solves it using mature numerical optimization algorithms such as the pseudospectral method. This method ensures the convergence and robustness of the algorithm, enabling it to find the optimal solution quickly and stably. It avoids the local optima or non-convergence problems that occur in traditional heuristic algorithms, providing a reliable control foundation for the autonomous operation of excavators. Attached Figure Description

[0073] Figure 1 This is a modular framework diagram of the system of the present invention;

[0074] Figure 2 This is a modular framework diagram of the system of the present invention;

[0075] Figure 3 This is a flowchart of the optimization problem-solving process of the present invention;

[0076] Figure 4 This is a closed-loop control framework diagram of the trajectory tracking control module of the present invention.

[0077] The components include: 1. Slewing platform; 2. Boom; 3. Hydraulic cylinder one; 4. Hydraulic cylinder two; 5. Stick; 6. Hydraulic cylinder three; 7. Connecting rod; 8. Rocker arm; 9. Bucket.

[0078] 100. Model building module; 200. Optimization target setting module; 300. Constraint processing module; 400. Trajectory planning and solving module; 500. Trajectory tracking and control module. Detailed Implementation

[0079] The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0080] Please refer to the appendix. Figure 1 This invention provides an excavator, comprising a slewing platform 1, characterized in that a boom 2 is rotatably connected to the middle of the slewing platform 1, a stick 5 is rotatably connected to the middle of the boom 2, a bucket 9 is rotatably connected to one end of the stick 5, a connecting rod 7 is rotatably connected to the outer side of the stick 5, a rocker arm 8 is rotatably connected to the middle of the connecting rod 7, one end of the rocker arm 8 is rotatably connected to the outer side of the bucket 9, a hydraulic cylinder 3 is rotatably connected to the top of the slewing platform 1, the output end of the hydraulic cylinder 3 is rotatably connected to the outer side of the boom 2, a hydraulic cylinder 4 is rotatably connected to the outer side of the boom 2, the output end of the hydraulic cylinder 4 is rotatably connected to the end of the stick 5 away from the bucket 9, and a hydraulic cylinder 6 is rotatably connected to the outer side of the stick 5, the output end of the hydraulic cylinder 6 is rotatably connected to the middle of the connecting rod 7.

[0081] In one specific embodiment, the main degrees of freedom of the working device are constituted by the series hinge of the slewing platform 1, boom 2, stick 5, and bucket 9. Simultaneously, precise and powerful hydraulic drive control is achieved through hydraulic cylinders 3 and 4, and a linkage-rocker mechanism driven by cylinder 6, respectively. In conjunction with connecting rod 7 and rocker arm 8, the excavator can perform a series of complex coordinated actions such as boom raising, boom extension, bucket retraction, and slewing, meeting various engineering operation requirements.

[0082] See attached document Figure 2 The present invention provides an excavator trajectory optimization planning and control system, which may include: a model building module 100, an optimization target setting module 200, a constraint condition processing module 300, a trajectory planning solution module 400, and a trajectory tracking control module 500.

[0083] The model building module 100 is configured to establish kinematic and dynamic models of the excavator's working device based on preset structural parameters. These kinematic and dynamic models provide the mathematical foundation for subsequent optimization calculations.

[0084] The optimization objective setting module 200, connected to the model building module 100, is configured to construct a collaborative optimization objective function that includes operation time and energy consumption based on a preset optimization objective. A specific form of the collaborative optimization objective function is shown in the following formula:

[0085] ;

[0086] In the formula: It is a minimization operator; This indicates that the expression within the parentheses is integrated along the entire geometric path; This represents the goal of optimizing task time; This represents the goal of optimizing energy consumption; For the normalized parameters of the geometric path, ; The square of the pseudo-velocity, i.e. ,in Path parameters Regarding time The first derivative; For joints Along the path The driving torque; This is a weighting coefficient used to balance time and energy consumption targets.

[0087] The constraint processing module 300, connected to the model building module 100, is configured to receive preset physical constraints on each joint of the excavator's working device and convert these physical constraints into pseudo-velocities. and pseudo-acceleration The algebraic constraints. The physical constraints include the angle limits, angular velocity limits, angular acceleration limits, and angular jerk limits for each joint.

[0088] The trajectory planning and solving module 400 is connected to both the optimization objective setting module 200 and the constraint processing module 300. This module is configured to combine the objective function output by the optimization objective setting module 200 and the algebraic constraints output by the constraint processing module 300 into a nonlinear programming problem. Subsequently, the trajectory planning and solving module 400 solves the nonlinear programming problem by executing a preset numerical optimization algorithm (e.g., a sequential quadratic programming algorithm) to obtain the optimal pseudo-velocity along the entire path. and optimal pseudo-acceleration The function curve.

[0089] A trajectory tracking control module 500 is connected to a trajectory planning and solving module 400. This module 500 is configured to receive optimal pseudo-velocity and pseudo-acceleration function curves and, based on these, inversely calculate the joint angles, angular velocities, and angular acceleration commands of each joint of the working device over time. These commands are then output to the excavator's underlying controller to drive the working device in motion.

[0090] In one specific embodiment of the present invention, the system's workflow is as follows:

[0091] The system first receives a preset work path. The model building module 100 calls the pre-stored excavator DH parameters and mass inertia parameters to generate a set of kinematic and dynamic equations describing the excavator's motion and mechanical characteristics.

[0092] Subsequently, the target setting module 200 optimizes the target setting module based on the input weight coefficients. A collaborative optimization objective function is constructed. Simultaneously, the constraint processing module 300 transforms preset joint motion limits (e.g., maximum angular velocity, maximum angular acceleration) into a series of pseudo-velocities. and pseudo-acceleration Algebraic inequalities.

[0093] The trajectory planning and solving module 400 integrates the above objective function and algebraic inequalities into a standard nonlinear programming problem, and performs the solution operation to obtain a set of discrete, optimal pseudovelocities. and pseudo-acceleration sequence.

[0094] Finally, the trajectory tracking control module 500 uses the optimal sequence and numerical integration to reconstruct the motion commands of each joint over time, and sends the motion command sequence to the hydraulic control unit of the excavator, thereby completing a trajectory tracking control with energy efficiency synergy optimization.

[0095] In one specific embodiment of the present invention, the model building module 100 first performs kinematic modeling on the excavator working device to establish a mapping relationship between joint space and Cartesian space.

[0096] To accurately describe the relative positions and attitude relationships between the joints of the excavator's working device, this embodiment employs the Denavit-Hartenberg parametric method. According to this method, a coordinate system is fixed for each joint. coordinate system Relative to joint coordinate system The transformation relationship can be obtained through joint length. Joint torsion angle Joint offset and joint angle These four parameters are used to uniquely determine the identity.

[0097] coordinate system Relative to coordinate system The pose transformation can be represented by a 4x4 homogeneous transformation matrix. The transformation is composed of four basic transformations, and the specific order of operations is shown in the following formulas:

[0098] ;

[0099] In the formula: Represents rotational transformation; Represents translation transformation; Joint coordinate system Relative to the previous joint coordinate system The homogeneous transformation matrix; Indicates the joint angle of rotation about the Z-axis ; Indicates the joint offset along the Z-axis. ; Indicates the length of the translational joint along the X-axis. ; Indicates the joint torsion angle about the X-axis. .

[0100] Around Axis rotation Rotation transformation matrix:

[0101] ;

[0102] along Axis translation Translation transformation matrix:

[0103] ;

[0104] along Axis translation Translation transformation matrix:

[0105] ;

[0106] Rotate around the X-axis Rotation transformation matrix:

[0107] ;

[0108] In the formula: Joint angle The value of the cosine function; Joint angle The value of the sine function; Joint angle The negative value of the sine function; Joint torsion angle The value of the cosine function; Joint torsion angle The value of the sine function; Joint torsion angle The negative value of the sine function.

[0109] By performing the matrix multiplication operation shown in the formula, the joint can be obtained. To the joint The specific expression for the homogeneous transformation matrix is ​​shown in the formula:

[0110] ;

[0111] In the formula: .

[0112] Homogeneous transformation matrix It can be represented as a block matrix, as shown in the formula:

[0113] ;

[0114] In the formula: Let be a 3x3 rotation matrix representing the coordinate system. Relative to coordinate system The posture; A 3x1 position vector represents the coordinate system. The origin of the coordinate system The position in the middle.

[0115] Any point in the coordinate system Position vector in It can be achieved by left multiplying by the homogeneous transformation matrix Transform to coordinate system In the formula, as shown:

[0116] ;

[0117] In the formula: Let be the position vector of the point in the coordinate system {i+1}.

[0118] From the base coordinate system To the end effector bucket 9 coordinate system All joint transformation matrices are multiplied sequentially, i.e. This allows the establishment of a forward kinematic model of the excavator's working device. For a A specific example of the forward kinematic equations of a working device with degrees of freedom is shown in the formula:

[0119] ;

[0120] In the formula: The three-dimensional coordinates of the excavator's end effector in the base coordinate system; This refers to the attitude angle of the excavator's end face; , They are respectively and abbreviation; They are respectively and abbreviation; They are respectively and abbreviation; The length of each joint; The height offset of the base; For the angles of each joint.

[0121] Inverse kinematics is based on the given end effector pose ( The corresponding joint angles can be obtained by solving the problem. The process of solving this problem can be carried out using algebraic or geometric methods, and the specific form of a set of solutions is shown in the formula:

[0122] ;

[0123] In the formula: Given the coordinates of the end target position; Given the end target attitude angle; For the angles of each joint; These are the length parameters of each joint; They are respectively The abbreviated form; for The abbreviated form; This is a four-quadrant arctangent function, which determines the quadrant of the angle based on the sign of the input parameters, thus obtaining a unique and correct angle value. Through this kinematic model, precise bidirectional calculations between joint variables and end-effector pose can be achieved.

[0124] After establishing the kinematic model, the model building module 100 further performs dynamic modeling on the excavator's working device. The dynamic model describes the relationship between the motion state of the working device (joint angles, angular velocity, angular acceleration) and the driving torque, and is the basis for energy consumption calculation and trajectory optimization. This embodiment uses the Lagrange method for dynamic modeling.

[0125] For a having The general dynamic equations for a series-joint robotic arm system are shown in the following formula:

[0126] ;

[0127] In the formula: For joints The driving torque; These are the joint angle, angular velocity, and angular acceleration vectors, respectively. Here is the system's inertia matrix; These are coefficients related to centrifugal force and Coriolis force. This is the term related to gravity. Let i be the actuator rotor inertia; This is the friction term.

[0128] The coefficients in the above dynamic equations can be derived using the Newton-Euler method or the Lagrange method. Among them, the inertia matrix... Centrifugal force and Coriolis force coefficients and gravity terms The specific calculation expression is shown in the formula:

[0129] ;

[0130] In the formula: Represents the trace operation of a matrix; From the base coordinate system to link 7 Homogeneous transformation matrix of the coordinate system; Link 7 The generalized inertial tensor matrix; Link 7 The quality; It is the vector of gravitational acceleration; Link 7 The center of mass is at link 7 Position vector in coordinate system; Link 7 The product of inertia and moment of inertia about its center of mass; Link 7 The center of mass is at link 7 Coordinates in a coordinate system.

[0131] Based on the aforementioned general dynamic equations and combined with the specific structural parameters of the excavator's working device (taking boom 2, stick 5, and bucket 9 as an example with three degrees of freedom), detailed expressions for the driving torque of each joint can be derived. These expressions clarify the coupling relationship between the movements of each joint and the influence of external loads (such as gravity) on the drive system. The driving torques of the bucket 9 joint, stick 5 joint, and boom 2 joint are given respectively. , , The expanded form:

[0132] Drive torque of the 9th joint of the bucket :

[0133] ;

[0134] Driving torque of joint 5 of the boom :

[0135] ;

[0136] Driving torque of boom joint 2 :

[0137] ;

[0138] In the formula: These are the masses of boom 2, stick 5, and bucket 9, respectively. These are the moments of inertia of the boom 2, stick 5, and bucket 9 about their respective centers of mass; These are the lengths of boom 2 and stick 5, respectively; These are the distances from the joint axes of boom 2, stick 5, and bucket 9 to their respective centers of mass; These are the geometric angles of the center of mass positions of boom 2, stick 5, and bucket 9 relative to the axis of connecting rod 7, respectively. Gravitational acceleration constant; These are the relative rotation angles of the joints of boom 2, stick 5, and bucket 9, respectively. The absolute angle of pole 5 is defined as follows: ; The sum of the angles between the boom 5 and the bucket 9 is defined as follows: ; The absolute angle of bucket 9 is defined as... ; These are the first derivative (angular velocity) and second derivative (angular acceleration) for the corresponding angles, for example, It is the square of the absolute angular velocity of the boom 5. It is the absolute angular acceleration of bucket 9; These are the cosine function and the sine function, respectively. They are respectively and The abbreviated form of .

[0139] Based on the above dynamic model, the system can determine the joint motion state at any given time ( The required driving torque for each joint is accurately calculated. The calculation results serve as the direct basis for subsequent energy consumption assessments and optimization solutions.

[0140] In one specific embodiment of the present invention, the construction and solution of the trajectory optimization problem are collaboratively completed by the optimization target setting module 200, the constraint condition processing module 300, and the trajectory planning and solving module 400. This section first describes the function and implementation of the optimization target setting module 200.

[0141] The function of the optimization objective setting module 200 is to transform the two qualitatively described optimization objectives of high efficiency and low energy consumption into a precise mathematical expression that can be numerically calculated, namely, the co-optimization objective function. This optimization objective setting module 200 first uses path parameters to define the two independent physical quantities of time and energy consumption. To provide a unified representation.

[0142] Regarding the total duration of the task. This can be achieved by using time infinitesimal elements The integral is obtained throughout the entire motion. This is achieved by introducing path velocity. The time infinitesimal element can be represented as Therefore, the objective of minimizing job time can be expressed in integral form as shown in the first part of the formula:

[0143] ;

[0144] In the formula: Total duration The upper and lower limits of the integral are 0 and 1 after normalization.

[0145] An indirect measure of energy consumption is the integral of the power loss of the drive system during motion. In a motor drive system, this energy loss is approximately proportional to the square of the drive torque. Therefore, minimizing energy consumption can be expressed as minimizing the integral of the square of the joint drive torque over time. Similarly, through... By substituting variables, the objective can be expressed in integral form as shown in the second part of the formula:

[0146] ;

[0147] In the formula: For the end at the path point Time joint The driving torque.

[0148] For ease of subsequent processing, a state variable is defined: the square of the pseudo-velocity. Therefore, path speed Substituting this definition, the two optimization objectives mentioned above are unified under the variable. Down.

[0149] The optimization target setting module 200 uses a weighting coefficient to assign weights to the two sub-targets mentioned above. By applying linear weights, a single, collaborative optimization objective function is formed, as shown in the formula:

[0150] ;

[0151] In the formula: It is a minimization operator; This indicates that the expression within the parentheses is integrated along the entire geometric path; This represents the goal of optimizing task time; This represents the goal of optimizing energy consumption; For the normalized parameters of the geometric path, ; The square of the pseudo-velocity, i.e. ,in Path parameters Regarding time The first derivative; For joints Along the path The driving torque; This is a weighting coefficient used to balance time and energy consumption targets.

[0152] when At that time, it degenerates into an objective that only minimizes time. At that time, it degenerates into the objective of simply minimizing energy consumption. When the value is between (0,1), the objective function optimizes both time and energy consumption.

[0153] In another embodiment, the objective function can also take other forms, for example, by directly optimizing the mechanical work and total stroke in the joint space, as shown in the formula:

[0154] ;

[0155] In the formula: For the objective function / cost function; For minimization operators; For joint trajectory; Total exercise duration; For time step.

[0156] Finally, the optimization target setting module 200 outputs the completed objective function, which is in continuous form as shown in the formula, to the trajectory planning and solving module 400 as the evaluation basis for subsequent numerical optimization.

[0157] In one specific embodiment of the present invention, the constraint processing module 300 functions to transform the kinematic and dynamic constraints that the excavator's working device must physically adhere to into a mathematical form that can be directly processed by the trajectory planning and solving module 400. This transformation process is a key step connecting the physical model and numerical optimization.

[0158] First, the system receives a set of preset time-based parameters. The physical constraints are the independent variables, which limit the range of motion and capabilities of each joint. Their specific form is shown in the formula:

[0159] ;

[0160] In the formula: The first The joint angle, angular velocity, and angular acceleration of each joint as a function of time; The first The lower and upper limits of the angle of each joint; For the first The maximum permissible angular velocity of each joint; For the first The maximum permissible angular acceleration of each joint.

[0161] In addition to the constraints mentioned above, this embodiment may also include constraints on diagonal jerk (Jerk) to ensure smoothness of motion.

[0162] Since the optimization problem is based on path parameters Constructed, therefore the above regarding time must be included. The differential constraints are transformed into constraints on the path parameters. Algebraic constraints. Therefore, a pseudo-velocity is introduced. and pseudo-acceleration Define two new state variables: the square of the pseudo-velocity. and pseudo-acceleration The relationship between these two state variables can be established by differentiation, as shown in the formula:

[0163] ;

[0164] Based on the chain rule, joint angular velocity and angular acceleration It can be represented as path parameters Functions:

[0165] ;

[0166] ;

[0167] In the formula: and These are the path parameters for the joint angles. The first and second derivatives can be calculated once the path is determined.

[0168] Substituting the above relationship into the angular velocity constraint After transformation, a value about The upper limit constraint, its derivation process and results are shown in the formula:

[0169] ;

[0170] ;

[0171] ;

[0172] ;

[0173] Similarly, substituting the above relationship into the angular acceleration constraint After transformation, a value about The upper and lower bound constraints, which are simultaneously with Related. The derivation process and results are shown in the formula:

[0174] ;

[0175] ;

[0176] ;

[0177] To further improve the smoothness of the trajectory, angular acceleration can be added. Apply constraints. The derivative of angular acceleration with respect to time is... The transformation form of its constraints is shown in the formula:

[0178] ;

[0179] Finally, the constraint processing module 300 transforms all the original, time-dependent differential constraints into a set of state variables. and Algebraic inequality constraints. Together with boundary conditions at the start and end points of the path (e.g., and the physical limitations of the drive system (e.g., Together, these constitute the complete set of constraints for the optimization problem to be solved, as shown in part of the formula:

[0180] ;

[0181] ;

[0182] ;

[0183] ;

[0184] ;

[0185] ;

[0186] ;

[0187] The set of constraints is then output to the trajectory planning solver module 400.

[0188] See attached document Figure 3The trajectory planning and solving module 400 receives the continuous objective function constructed by the optimization objective setting module 200 and the continuous constraint set transformed by the constraint processing module 300, and transforms it into a discrete nonlinear programming problem that can be numerically calculated.

[0189] The first step in this transformation process is to normalize the continuous path parameters. Discretize the interval. Given subintervals of equal or unequal length, thus obtaining Discrete path nodes ,in The length of each subinterval is .

[0190] Subsequently, the continuous state function and Discretized into a vector of decision variables of finite dimensions and .

[0191] The trajectory planning and solving module 400 performs numerical approximation on the continuous integral term of the objective function. This is done within each sub-interval. The internal calculation assumes linear interpolation, as shown in the formula:

[0192] ;

[0193] The trajectory planning and solving module 400 can transform continuous integrals into discrete summations. A specific discretized objective function is shown in the formula:

[0194] ;

[0195] In the formula: , Path nodes and The pseudo-velocity squared value at the location; For the first The length of each sub-interval; In the first The weight coefficients set on each sub-interval can vary with the path. In the first The first subinterval calculated within the _ ... The driving torque of each joint; For the first The rated torque of each joint or a preset normalized constant.

[0196] Simultaneously, continuous constraints must also be satisfied at each discrete node. The trajectory planning solution module 400 discretizes differential equation constraints and algebraic inequality constraints. For example, using the forward Euler method or higher-order numerical methods, the differential relations are discretized. This is transformed into an algebraic equality constraint. Ultimately, a complete set of discretized constraints is formed, as shown in the formula:

[0197] Boundary conditions: ;

[0198] State transition equation: ;

[0199] Physical feasibility: ;

[0200] Speed ​​constraints: ;

[0201] Acceleration constraints: ;

[0202] Torque constraint: ;

[0203] ;

[0204] In the formula, torque The calculation of the midpoint of a subinterval To improve calculation accuracy; These are the coefficients of inertia, centrifugal force / Corst force, and gravity term calculated at that point, respectively.

[0205] At this point, the trajectory planning solver module 400 has successfully transformed the original dynamic optimization problem into a standard, large-scale nonlinear programming (NLP) problem. The goal of this NLP problem is to find the decision variable vector. and This minimizes the objective function while satisfying all equality and inequality constraints in the constraint set.

[0206] Finally, the trajectory planning and solving module 400 calls a preset numerical optimization solver, such as the Sequence Quadratic Programming (SQP) algorithm or the Interior Point Method (IPM), to solve the NLP problem. Through iterative calculations, the solver ultimately outputs a set of optimal discrete pseudo-velocities and pseudo-acceleration sequences. This sequence represents the optimal trajectory planning result.

[0207] See attached document Figure 4 To illustrate the implementation process of the trajectory optimization planning and control system provided by the present invention, this section provides a specific embodiment.

[0208] In this embodiment, a task is set up for an excavator's working device to perform point-to-point motion in Cartesian space. This task requires the end effector (the tip of the bucket tooth 9) of the working device to start from a preset position. Move to a preset target pose .

[0209] Task path definition: starting pose The coordinates and attitude angles in the base coordinate system {0} are defined as follows: Target pose The coordinates and attitude angles in the base coordinate system {0} are defined as follows: The motion path of the end effector is defined as a connection. and A straight line. This path is normalized by the path parameters. Describe it.

[0210] Excavator Model Parameter Initialization: This embodiment uses a three-degree-of-freedom excavator working device model. The joint DH parameters are defined by the formula, and the specific values ​​are shown in the table below.

[0211]

[0212] Among them, joint length , .

[0213] Based on the dynamic model, the initial values ​​of the dynamic parameters (mass, center of mass position, moment of inertia) of each joint are shown in the table below:

[0214]

[0215] Physical constraint setting: Based on the formula, the physical motion limits are set for the joints of the excavator's boom 2, stick 5, and bucket 9. These constraints will be used by the constraint processing module 300 to generate an algebraic constraint set. Specific values ​​are shown in the table below:

[0216]

[0217] Optimization parameter settings: In the optimization target setting module 200, an objective function is constructed according to the formula. In this embodiment, to achieve a balanced optimization between operation time and energy consumption, the weighting coefficients are... The value is set to 0.5. In the trajectory planning and solving module 400, the normalized path is used for numerical solution. Discretize it into N=100 nodes.

[0218] All of the above parameters are input into the control system of the present invention as initial conditions for performing subsequent trajectory optimization calculations.

[0219] After completing the scenario setting and parameter initialization, the system of this invention begins to perform trajectory optimization calculations. The trajectory planning and solving module 400 receives the previously defined path, model parameters, and constraints, and constructs a specific, large-scale nonlinear programming (NLP) problem based on these.

[0220] The objective function of this NLP problem is in discrete form, where the weight coefficients are... In this embodiment, the value is uniformly set to 0.5. The constraint set of this NLP problem is in the discrete form of the formula, where all parameters (such as...) are... All of them are replaced with the given specific values.

[0221] The trajectory planning solution module 400 then calls the built-in Sequence Quadratic Programming (SQP) solver to iteratively solve the NLP problem. The goal of the solution process is to find a set of optimal discrete decision variable sequences. .

[0222] After the solution is completed, the system obtains the weight coefficients. The optimal pseudo-velocity square curve under (equilibrium mode) is derived from... A smooth start, reaching a peak in the middle of the path, followed by a smooth deceleration. This fully satisfies the velocity boundary conditions at the start and end points of the path.

[0223] The trajectory tracking control module 500 receives this optimal... The sequence is used to solve for the total motion time through numerical integration. And the movement commands of each joint over time, including joint angles. Joint angular velocity and joint angular acceleration .

[0224] In balanced mode, the angular velocities of the three joints: boom 2, stick 5, and bucket 9. The curve shows how the trajectory changes over time. Throughout the entire motion, the angular velocity of all joints did not exceed the set speed limit. This indicates that the trajectory generated by the system of this invention strictly adheres to the preset physical constraints.

[0225] Meanwhile, the trajectory tracking control module 500 utilizes a dynamic model to determine the joint motion state. Calculate the corresponding joint driving torque These torques reflect the driving output required to overcome inertia, centrifugal force, Coriolis force, and gravity during motion.

[0226] To further illustrate the beneficial effects of the technical solution of the present invention, the system sets weighting coefficients while keeping all other parameters unchanged. (Time-priority mode) and (Energy priority mode) and re-execute optimization calculations.

[0227] when At this point, the objective function degenerates into minimizing the operation time. The system generates a pseudo-velocity square curve. This pseudo-velocity square curve reaches and maintains a higher velocity plateau allowed by physical constraints more quickly and for a longer period, ultimately resulting in the shortest calculated total motion time. However, this mode results in larger peak and rate of change of driving torque, leading to higher total energy consumption.

[0228] when At this point, the objective function degenerates into minimizing energy consumption. The system generates a pseudo-velocity square curve. This pseudo-velocity square curve is generally smoother, with a lower peak velocity, avoiding abrupt acceleration and deceleration. The final calculated total motion time is the longest, but its driving torque curve is smoother, the peak torque is lower, and the total energy consumption is the lowest among the three modes.

[0229] Through comparative analysis, this embodiment verifies that:

[0230] The system of this invention can automatically generate a feasible optimal trajectory that satisfies all physical constraints based on a given task and constraints.

[0231] By adjusting the single weight coefficient Users can easily balance the two mutually constraining objectives of shortest time and lowest energy consumption to obtain customized motion trajectories that meet different operational needs. In this embodiment... The setting is that, within an acceptable time range, energy consumption is reduced, achieving synergistic optimization of time and energy.

[0232] Finally, the trajectory tracking control module 500 will select the mode (e.g., The joint motion command sequence generated under the balanced mode is output to the excavator's bottom controller to drive cylinder 3, cylinder 4 and cylinder 6, so that the boom 2, stick 5 and bucket 9 can accurately reproduce the optimized trajectory.

Claims

1. A trajectory optimization planning and control system for excavators, characterized in that, include: The model building module is used to build the kinematic and dynamic models of the working device consisting of boom (2), stick (5) and bucket (9); The optimization target setting module is used to establish a collaborative optimization objective function that includes energy consumption and efficiency; The constraint processing module is used to set the kinematic and dynamic constraints of the boom (2), stick (5) and bucket (9) of the excavator, and to perform pseudo-constraint transformation on the path constraints to form unified boundary constraints. The trajectory planning and solving module is used to solve for the optimal trajectory based on the kinematic model and dynamic model, the cooperative optimization objective function and the boundary constraints. The trajectory tracking control module is used to generate and output control commands to the first (3), second (4) and third (6) cylinders of the excavator working device according to the optimal trajectory. The model building module is specifically used for: The Denavit-Hartenberg parametric method was used to establish a multi-rigid-body kinematic model of the excavator's working device; A dynamic model of the excavator's working device was established using the Lagrange equation. The energy consumption in the optimization target setting module is characterized by integrating the absolute value of the product of the driving torque and angular velocity of each joint; the efficiency is characterized by the ratio of the effective work of the excavation process to the operation time. The excavator includes a slewing platform (1), a boom (2) rotatably connected to the middle of the slewing platform (1), a stick (5) rotatably connected to the middle of the boom (2), a bucket (9) rotatably connected to one end of the stick (5), a connecting rod (7) rotatably connected to the outer side of the stick (5), a rocker arm (8) rotatably connected to the middle of the connecting rod (7), one end of the rocker arm (8) rotatably connected to the outer side of the bucket (9), a hydraulic cylinder (3) rotatably connected to the top of the slewing platform (1), the output end of the hydraulic cylinder (3) rotatably connected to the outer side of the boom (2), a hydraulic cylinder (4) rotatably connected to the outer side of the boom (2), the output end of the hydraulic cylinder (4) rotatably connected to the end of the stick (5) away from the bucket (9), a hydraulic cylinder (6) rotatably connected to the outer side of the stick (5), and the output end of the hydraulic cylinder (6) rotatably connected to the middle of the connecting rod (7).

2. The excavator trajectory optimization planning and control system according to claim 1, characterized in that, The constraint processing module is specifically used for: The kinematic and dynamic constraints of the excavator working device are specified as state variable constraints and control variable constraints; The path constraints in the mining process are transformed into pseudo-constraints, which together with the state variable constraints and control variable constraints constitute the boundary constraint conditions.

3. The excavator trajectory optimization planning and control system according to claim 2, characterized in that, The state variable constraints include angle constraints, angular velocity constraints, and angular acceleration constraints for each joint; the control variable constraints are the driving torque constraints for each joint.

4. The excavator trajectory optimization planning and control system according to claim 1, characterized in that, The trajectory planning and solving module incorporates a pseudospectral method to transform the optimal trajectory problem into a nonlinear programming problem for solution.

5. The excavator trajectory optimization planning and control system according to claim 1, characterized in that, The trajectory planning and solving module also includes: The weighting coefficient allocation unit is used to allocate weighting coefficients to energy consumption and efficiency in the collaborative optimization objective function.

6. The excavator trajectory optimization planning and control system according to claim 1, characterized in that, The trajectory tracking control module specifically includes: The status feedback unit is used to acquire the actual motion status of the excavator's working device in real time; A deviation calculation unit is used to calculate the deviation between the actual motion state and the optimal trajectory; The controller unit is used to generate the control command based on the deviation and output it to the actuator of the excavator working device.

7. The excavator trajectory optimization planning and control system according to claim 6, characterized in that, The status feedback unit includes: Multiple angle sensors are respectively installed at each joint of the excavator's working device; The angle sensor is used to detect the actual rotation angle of each joint in real time, and to obtain the actual motion state of the excavator working device.

Images