Tower crane trajectory planning method based on quintic polynomial and point constraint

Abstract

The invention discloses a tower crane trajectory planning method based on quintic polynomial point constraint, and belongs to the field of building construction automation. According to the method, a kinematics basis is constructed through system modeling, path point distribution is optimized through adaptive key point sampling, initial period time is determined by adopting constraint perception time distribution, non-zero passing point speed / acceleration boundary conditions are constructed through a weighted center difference method and a non-equal step length second-order difference method in combination with smooth filtering, and the non-zero passing point speed / acceleration boundary conditions are determined. And generating an improved quintic polynomial trajectory meeting Ccontinuity, correcting speed / acceleration overshoot through iterative time stretching, and selectively verifying load swing safety through a swing model. According to the method, the problem of low efficiency caused by a walking-stopping-walking mode in the prior art is solved, structural vibration caused by sudden acceleration change is remarkably reduced, the maximum swing angle of the load is reduced to 0.822 degrees from 12.317 degrees, the total task time is shortened by 12.9%, meanwhile, it is guaranteed that the track strictly meets hardware constraints, and the high efficiency, smoothness and safety of operation of the tower crane are improved.

Description

Technical Field

[0001] This invention relates to a heavy machinery control technology applied in the field of construction automation, specifically a trajectory generation and optimization method for the coordinated motion of a tower crane's three degrees of freedom (slewing, luffing, and hoisting). This method achieves smooth trajectory planning that satisfies strict dynamic constraints and possesses low jerk characteristics by constructing velocity and acceleration boundary conditions for continuous via-points, combined with adaptive keypoint sampling and iterative time stretching mechanisms.

[0002] Background Technology: With the advancement of the concepts of "Industry 4.0" and "Smart Construction," the automation and intelligent operation of tower cranes, as core vertical transportation equipment on construction sites, has become an inevitable trend in the industry. Tower cranes are mainly used for lifting precast components, steel structures, and large construction equipment. Their operating environment is complex, with heavy loads and high risks. Traditional tower crane operation relies on human experience, resulting in large efficiency fluctuations, high safety hazards, and easy fatigue. Therefore, realizing automatic trajectory planning and precise control of tower cranes is a key technology for improving construction efficiency and safety.

[0003] In the existing automated tower crane control technology system, the main task of the trajectory planning module is to generate a collision-free path from the starting point to the target point in the workspace and convert it into motion commands for each joint (slewing mechanism, luffing trolley, hoisting mechanism) that change over time.

[0004] Currently, the mainstream technical approach adopted by industry and academia is "Piecewise Polynomial Interpolation" as the basic scheme for tower crane trajectory planning. Because the various actuators (motors) of a tower crane are usually subject to strict velocity and acceleration constraints, and to ensure smooth motion, the quintic polynomial, with its six undetermined coefficients, can theoretically simultaneously satisfy the boundary conditions of position, velocity, and acceleration, and is therefore widely used in point-to-point trajectory generation.

[0005] Detailed Implementation (Example of Prior Art) Existing typical implementation schemes (hereinafter referred to as "ordinary fifth-degree polynomial schemes") typically include the following steps: Path point generation: First, a series of discrete path points are generated in Cartesian space using a geometric path planning algorithm (such as A* algorithm, RRT algorithm, or manual teaching), denoted as . .

[0006] Inverse kinematics solution: Transforming path points in Cartesian space into a coordinate sequence in tower crane joint space. Each of these points It includes three components: lifting height, luffing radius, and slewing angle [1].

[0007] Stop-and-Go Strategy: This is the most prominent feature in existing technologies. To simplify control logic and ensure safety, existing schemes typically perform stop-and-go interpolation for each path segment. arrive This is treated as an independent motion task. At each intermediate via-point, the planner forces the velocity and acceleration boundary conditions to be zero.

[0008] Mathematical expression: For the connection and The The segment trajectory, whose boundary conditions are set as follows: Time: Location ,speed acceleration ; Time: Location ,speed acceleration .

[0009] Time parameterization: based on the maximum allowable speed of the motor and maximum acceleration The shortest time required for each segment is calculated, typically using a trapezoidal velocity curve or simple extreme value estimation. This approach is discussed in detail in classic textbooks such as "Introduction to Robotics" (Craig). Its advantages include simplicity of implementation, low computational cost, and the ability to ensure that the robotic arm or hook can safely stop at any intermediate point, making it easy to handle unexpected situations.

[0010] Summary of the Invention The purpose of this invention is to overcome the shortcomings of the aforementioned background technology and provide a tower crane trajectory planning method based on improved fifth-order polynomial continuous via-point constraints. Specifically, this invention aims to achieve the following technical objectives: Achieving continuous and smooth motion throughout the entire process: By constructing non-zero via-point velocity and acceleration boundary conditions, the pauses at intermediate points are eliminated, achieving cross-segment trajectory... Continuity (position, velocity, and acceleration are all continuous), significantly shortening task execution time and improving operational efficiency. Jerk and structural impact suppression: By smoothing the derivatives at points, the rate of change of acceleration is reduced at the source, minimizing mechanical impact and vibration on the tower crane's metal structure. Active load sway suppression: Based on dynamic mechanisms, by reducing abrupt changes and peak values ​​of hook horizontal acceleration, energy input to the flexible rope-load system is reduced at the planning level, significantly decreasing process sway amplitude and residual sway at the endpoint, achieving highly efficient "stop-on-arrival" operation. Strictly ensuring the executability of hardware constraints: An iterative time-stretching mechanism is proposed to ensure that the generated trajectory strictly meets the maximum speed and maximum acceleration limits of each joint motor at any time, solving the dynamic over-limit problem under complex paths. Attached Figure Description

[0011] Figure 1 The flowchart of the improved fifth-order polynomial dynamics constrained trajectory planning method provided by the present invention is shown.

[0012] Figure 2 This is a schematic diagram of the trajectory planning parameters and result data structure provided by the present invention (opts / limits / traj).

[0013] Figure 3 The block diagram of the tower crane trajectory planning and dynamics analysis system provided for this invention is shown in main.m—DynamicsAnalysisV2.

[0014] Figure 4 The diagram showing the relationship between the three-degree-of-freedom kinematic model and the forward and inverse kinematics of the tower crane provided by this invention.

[0015] Figure 5 A comparison curve showing the improved smoothness of the five-times (continuous via points) and ordinary five-times (intermittent stops) trajectories. In this invention, a hierarchical trajectory planning framework integrating adaptive sampling, heuristic time allocation, continuous derivative construction, and iterative optimization is proposed. This scheme is implemented in the main control unit (such as an industrial PC or PLC) of the tower crane via computer software algorithms.

[0016] 1. System Modeling and Kinematic Foundations First, a kinematic model of the tower crane is established as the mathematical basis for trajectory planning. A world coordinate system is defined. Tower crane base coordinate system Let the position of the hook in the workspace be... Define joint space variables. ,in: • Lifting height ( (axis coordinates, unit: meters) • : Amplitude radius ( (Unit: meters) • Rotation angle ( (Unit: radians).

[0017] Inverse kinematic mapping (IK):

[0018] in The coordinates are for the tower crane base. This model converts the path in Cartesian space into control variables for each actuator.

[0019] 2. Step 1: Adaptive Keypoint Sampling. To address the problem of unreasonable sampling point distribution in the background technology, this invention designs an adaptive sampling strategy.

[0020] Input: The original dense path points generated by the geometry path planner . Processing logic: Curvature recognition: Calculate the local rotation angle of each path point. Set a threshold (e.g., 30 degrees) Any point whose turning angle exceeds the threshold is marked as an "inflection point" and must be retained to maintain the path geometry.

[0021] Minimum Spacing Constraint: Introducing a minimum arc length spacing parameter While preserving the inflection point, check the arc distance between adjacent key points. If the distance between two points is less than... Redundant points are then eliminated based on the weight of the turning angle. This step is to prevent "Jerk spikes" caused by pathpoints being too close together.

[0022] Arc length completion: In long straight segments of the path, uniform interpolation is performed according to the set maximum spacing to ensure the resolution of trajectory control.

[0023] Output: Optimized sparse keypoint sequence ,in [1].

[0024] Step Two: Constraint-Aware Time Allocation Before generating the polynomial, it is necessary to allocate time for each trajectory segment ( arrive Assign initial time This invention abandons simple equal distribution and adopts a heuristic distribution method based on physical limits.

[0025] Single-axis limit: for each segment Each joint Calculate the theoretical shortest time under trapezoidal velocity programming:

[0026] (Note: This is a simplified estimate; actual implementation includes consideration of acceleration time.)

[0027] The "barrel effect" integration: The baseline time for this segment is determined by the slowest joint.

[0028] in (e.g., 1.2) is the safety factor.

[0029] Global scaling: If the user specifies the total task time. Then scale each segment proportionally. This ensures that the total time meets the requirements and that each segment is not less than the physical limit. This step ensures that the initial time allocation conforms to the mechanical characteristics, significantly reducing the number of iterations required for subsequent optimization.

[0030] Step 3: Continuous Derivative Construction – Core Innovation: This is the key difference between this invention and existing technologies. To achieve… Continuity (position, velocity, acceleration continuity) must be achieved through each intermediate point. ( Calculate a reasonable "crossing speed" And "Acceleration Through Time" Instead of setting it to zero.

[0031] Specific calculation method: Velocity estimation (weighted central difference method): estimating the direction and magnitude of the velocity at the midpoint based on the displacement trends of the preceding and following segments. The physical meaning of this formula is: if the path passes through the point... If the line remains straight, the velocity remains constant; if there is a turning point, the velocity vector will transition naturally.

[0032] Clipping: It must be ensured that the estimated value does not exceed the motor's maximum speed.

[0033] Acceleration estimation (non-uniform step size second-order difference): Estimating acceleration using the velocity difference between the preceding and following segments: Let the average velocity of the preceding segment be... average speed in the latter part .

[0034]

[0035] This formula reflects the rate of change of the velocity vector.

[0036] Amplitude limiting: This also needs to be limited to the maximum acceleration range.

[0037] Lightweight Smoothing: Because difference calculations are sensitive to noise, direct use may cause derivative jitter. This invention applies a lightweight smoothing filter to the calculated sequence. and Apply three-point moving average filtering:

[0038] This step greatly smooths out the changes in the control quantity and reduces Jerk from the source [1].

[0039] Step 4: Improved Fifth-Order Polynomial Trajectory Generation Based on the non-zero boundary conditions calculated above, for each segment... Construct a quintic polynomial curve:

[0040] Boundary condition equations: Construct a linear system of equations and solve for the coefficients. : • ,

[0041] • (Note: Here) Not for ) • (Note: Here) Not for Due to the End condition of segment Strictly equal to the first The starting conditions of the segment ensure that the trajectory achieves a smooth and uninterrupted transition when passing through the transit point.

[0042] Step 5: Iterative Time Stretching. Since the derivative in Step 3 is based on heuristic estimation, the generated fifth-degree polynomial curve may experience velocity or acceleration "overshoot" in the middle of the segment, exceeding hardware limitations. Therefore, this invention designs a closed-loop feedback correction mechanism.

[0043] Process Logic: Global scan: After generating the entire trajectory, scan at high resolution (e.g., ... Discretized scanning to calculate the speed utilization rate of the entire process. and acceleration utilization rate .

[0044] Peak detection: Identifying the global maximum utilization and .

[0045] Judgment and correction: If and If the trajectory is valid, the result will be output.

[0046] If any indicator exceeds the standard ( If the stretching factor is calculated, then the stretching factor can be calculated. :

[0047] Time update: update the time of all segments Unified multiplication ( ).

[0048] Regeneration: Return to step four and recalculate the polynomial coefficients with the new time. This iterative process usually converges within 2-3 iterations, which ensures strict safety and avoids the high computational overhead of traditional optimization algorithms, making it suitable for real-time operation in embedded systems [1].

[0049] Step Six: Verification Based on the Swing Model (Optional) To further ensure hoisting safety, this invention introduces a time-varying rope length small-angle forced pendulum model for pre-simulation.

[0050]

[0051] in This is the system's natural frequency. The trajectory generated by the system's calculations... Input the model to predict residual oscillations. If the predicted value exceeds a safety threshold (e.g., 0.5 degrees), the time stretching factor is actively increased. To sacrifice a little efficiency in exchange for absolute safety.

[0052] Key points of the invention: 1. Point-based continuous derivative estimation method: Unlike existing technologies that force the velocity / acceleration at a point to zero, this invention proposes a combined algorithm of "differential estimation-amplitude limiting-smoothing" to construct non-zero boundary conditions that conform to physical constraints. This technical feature enables trajectory estimation. 1. Continuous operation, elimination of pauses, and reduction of Jerk. 2. Iterative time stretching mechanism based on constraint feedback: This invention does not rely on complex nonlinear programming (NLP) solvers, but instead adjusts the time parameters in reverse by detecting the peak velocity / acceleration ratio after trajectory generation. This "generation-detection-correction" closed-loop mechanism ensures that the trajectory will not cause motor overload under any complex working conditions with extremely low computational cost. 3. Adaptive sparse sampling that preserves geometric features: This invention introduces dual constraints of "minimum arc length spacing" and "curvature threshold" in the sampling stage. This solves the problem of mathematical acceleration singularities (Spikes) caused by excessively dense sampling points in narrow spaces or sharp turns, ensuring the numerical stability of the algorithm. 4. Anti-sway strategy with embedded dynamic mechanism: This invention does not use an external anti-sway controller (such as input shaping or PID feedback), but instead utilizes the low Jerk characteristics of an improved fifth-order polynomial to cut off the transmission of high-frequency energy to the oscillating system from the source of the trajectory. This "passive" anti-sway does not rely on precise load mass parameters and has stronger robustness.

[0053] The invention achieves significantly better results than existing technologies in terms of efficiency, smoothness, safety, and constraint satisfaction through the above-described technical solution. The following comparison is illustrated using specific simulation experimental data (data from comparative tests under uniform experimental settings).

[0054] 1. Significantly improved operating efficiency: By eliminating the interruptions at intermediate transit points, tower cranes can maintain continuous movement.

[0055] • Comparative data: Under the same test path, the total task time using the present invention (improved fifth-degree polynomial) is 297.00 seconds, while the time using the prior art (ordinary fifth-degree polynomial) is 340.79 seconds.

[0056] • Results: The operation time was reduced by approximately 43.8 seconds, and efficiency was increased by 12.9%. This means that on a real construction site, several more hoisting cycles can be completed each day, significantly accelerating the construction progress.

[0057] 2. Significantly improved motion smoothness (low Jerk): This invention effectively suppresses drastic changes in acceleration and protects the mechanical structure.

[0058] • Comparison data: Jerk peak ( The value was reduced from 0.057 in the prior art to 0.022, a decrease of 61%.

[0059] Jerk Energy The energy level decreased from 3.066e-2 to 4.473e-3, a reduction of nearly 6.85 times.

[0060] • Effects: The extremely low Jerk energy means a significant reduction in the vibration amplitude of the tower crane boom, an extension of the fatigue life of the wire rope, and a smoother operation.

[0061] 3. The load sway is greatly suppressed, which is the most prominent safety effect of this invention.

[0062] • Comparison data: Maximum swing angle during the process ( The temperature dropped sharply from 12.317 degrees in the existing technology to 0.822 degrees.

[0063] Residual pendulum angle at the end point ( The temperature dropped from 1.696 degrees to 0.164 degrees.

[0064] • Effect: A 12-degree oscillation is considered a dangerous condition in actual engineering and could potentially lead to a collision; while a 0.8-degree oscillation is almost invisible to the naked eye. This invention achieves a "stable, accurate, and fast" control effect, allowing operators to directly perform unloading operations without waiting for the oscillation to decay at the endpoint.

[0065] 4. Constraints are used more rationally. • Comparative data: Speed ​​utilization rate of existing technologies ( The speed often reaches 1.0 (full load), which easily triggers an overspeed alarm; while the speed utilization rate of the present invention is maintained at around 0.941.

[0066] • Effect: With a speed margin of approximately 6%, the system is more robust to sudden wind loads or voltage fluctuations, making it less prone to downtime.

[0067] Table 1: Comparison of Performance Indicators between the Invention and Existing Technologies

Claims

1. A method for planning the trajectory of a tower crane based on a fifth-order polynomial via point constraints, characterized in that, Includes the following steps: System Modeling and Kinematic Foundation Construction: Establish the world coordinate system, base coordinate system, and kinematic model of the tower crane, and define the hook position variable P(t)=[x(t),y(t),z(t)] in Cartesian space. T With the joint space variable q(t) = [q1(t), q2(t), q3(t)] T The Cartesian space path is converted into joint space control variables through inverse kinematic mapping, where q1 is the lifting height, q2 is the luffing radius, and q3 is the turning angle. Adaptive keypoint sampling: Input the original dense path points Praw generated by the geometric path planner, mark inflection points by curvature identification, remove redundant points based on minimum arc length spacing, and complete the long straight segment arc length, output the optimized sparse keypoint sequence Q1, Q2, ..., QM; Constraint-aware time allocation: For the trajectory between each adjacent key point, the shortest theoretical time for a single axis is calculated based on the physical limits of the speed and acceleration of each joint motor. The reference time is determined according to the barrel effect and a safety factor is introduced. The initial time allocation for each trajectory segment is completed by scaling the overall task time requirement. Construction of continuous transit point derivatives: The transit velocity of intermediate transit points is estimated by weighted central difference method, and the transit acceleration is estimated by non-equal step size second difference method. After the estimated values ​​are subjected to amplitude limiting processing, three-point moving average filtering is applied for smoothing to obtain non-zero transit point velocity and acceleration boundary conditions. Improved quintic polynomial trajectory generation: Based on the non-zero boundary conditions obtained in step 4, a quintic polynomial equation is constructed for each trajectory segment. The polynomial coefficients are solved through a system of linear equations to achieve continuity of position, velocity, and acceleration across trajectory segments (Ci). 2 (continuity) Iterative time stretching: Discretize the entire trajectory generated by scanning, calculate the velocity utilization and acceleration utilization, detect the global maximum utilization, if overshoot exists, calculate the stretching factor and update the time of each trajectory segment, return to step 5 to regenerate the trajectory, until all hardware constraints are met.

2. The method according to claim 1, characterized in that, The specific implementation of curvature recognition in step 2 is as follows: calculate the local rotation angle α of each path point. k Set the corner threshold α thr =30°, mark points with turning angles exceeding this threshold as inflection points and forcibly retain them.

3. The method according to claim 1, characterized in that, The minimum arc length spacing constraint mentioned in step 2 is implemented by introducing a minimum arc length spacing parameter s. min While retaining the inflection point, remove adjacent intervals less than s. min Redundant points are interpolated uniformly at the set maximum spacing for long straight segments.

4. The method according to claim 1, characterized in that, The formula for calculating the shortest time in the single-axis theory mentioned in step 3 is: T min,i,j ≈ / v lim，j Where qj,i is the joint coordinate of the j-th joint at the i-th key point, and vlim,j is the maximum speed limit of the j-th joint; the formula for calculating the reference time is: T base,i =max j = 1,2,3 (T min,i,j )×λ where λ>1 is the safety factor, which takes the value of 1.

2.

5. The method according to claim 1, characterized in that, The formula for the three-point moving average filter mentioned in step 4 is: x i new =0.25x i 1+0.5x i +0.25x i+1 Where x i x represents the velocity or acceleration value before filtering. i new This is the filtered value.

6. The method according to claim 1, characterized in that, The formulas for the clipping process described in step 4 are as follows: vi = clip(v est,i , v lim ,v lim ) ai=clip(a est,i , a lim ,a lim )where v lim a is the maximum speed of the motor. lim The maximum acceleration of the motor is clip( ) is a limiting function that constrains the input value to a specified upper and lower limit range.

Images