Three-dimensional path planning and tracking control system and method for tower crane hook

Abstract

The application discloses a three-dimensional path planning and tracking control system of a tower crane hook, which is formed by interconnection of four function modules, namely, a tower crane operation state detection module, a tower crane dynamics performance detection module, a hook three-dimensional path planning module and a hook path tracking control module. The tower crane operation state detection module provides data support for the tower crane dynamics performance detection module and the hook three-dimensional path planning module. The tower crane operation state detection module, the tower crane dynamics performance detection module and the hook three-dimensional path planning module jointly provide data support for the hook path tracking control module. The application further discloses a three-dimensional path planning and tracking control method of a tower crane hook. The application belongs to the technical field of intelligent control of tower cranes, solves the problems of high cost of environment modeling at a construction site, insufficient real-time performance and stable control of the tower crane in a large inertia state, and improves the working efficiency of the tower crane.

Description

TECHNICAL FIELD

[0001] The present application belongs to the technical field of tower crane hook tracking control, and relates to a three-dimensional path planning and tracking control system of a tower crane hook. BACKGROUND

[0002] The tower crane, hereinafter referred to as tower machine, is an important vertical transport tool in the construction industry. Due to high-altitude operation, narrow space and inconvenient activities, the working environment of the tower machine driver is very poor, and long-term tension work greatly affects the driver's mood, resulting in a large human safety hazard in the work of the tower machine driver. With the development of communication technology and video technology, in order to reduce the construction labor cost, the unmanned control of the tower machine has become a research hotspot in recent two years.

[0003] Construction scene perception and modeling is the premise of tower machine unmanned driving. On the one hand, due to the large working range of the tower machine, the environment modeling of the tower machine working scene needs to use laser radar, which is high in cost, and the construction site changes dynamically, so the real-time performance cannot meet the requirements of the site when laser radar is used for dynamic map construction. On the other hand, during the working process of the tower machine, the path of the hook is a three-dimensional space path, and the existing three-dimensional automatic path tracking control method has high algorithm complexity and high model precision requirement. However, due to the tower machine in various working conditions, such as different loads and different external wind speeds, the dynamics performance of the tower machine will be affected, and at the same time, the inertia of the tower machine is relatively large, so it is necessary to ensure the smooth transition of the motion state in the path tracking control to avoid the hard impact on the steel structure of the tower machine.

[0004] Therefore, the existing space three-dimensional path tracking algorithm cannot guarantee the smoothness of the tower machine motion and the reliability of the tracking, and a new method is needed to overcome the above technical problems. SUMMARY

[0005] The present application provides a three-dimensional path planning and tracking control system of a tower machine hook, which solves the problems of difficult environment modeling of a complex construction site, high cost of automatic path planning and insufficient real-time performance. On the other hand, it solves the problem of insufficient stability of the existing technology of tower machine automatic driving, which produces hard impact on the steel structure of the tower machine.

[0006] Another object of the present application is to provide a three-dimensional path planning and tracking control method of a tower machine hook, which solves the problems of high cost and insufficient real-time performance of the existing automatic path planning, and insufficient stability.

[0007] The technical scheme adopted by the application is that the three-dimensional path planning and tracking control system of the tower crane hook is formed by interconnection of four functional modules, namely, a tower crane operating state detection module, a tower crane dynamics performance detection module, a hook three-dimensional path planning module and a hook path tracking control module, the tower crane operating state detection module provides data support for the tower crane dynamics performance detection module and the hook three-dimensional path planning module, and the tower crane operating state detection module, the tower crane dynamics performance detection module and the hook three-dimensional path planning module jointly provide data support for the hook path tracking control module.

[0008] Another technical scheme adopted by the application is a three-dimensional path planning and tracking control method of a tower crane hook, which relies on the three-dimensional path planning and tracking control system of the tower crane hook, and the implementation process is as follows:

[0009] The tower crane operator performs an actual hoisting operation, and the tower crane operating state detection module acquires the position information of the hook in real time by using three sensors;

[0010] The tower crane dynamics performance detection module detects the dynamics performance of the three-dimensional movement of the tower crane by using the position information of the hook, and obtains the speed control threshold of slewing, luffing and hook lifting;

[0011] The hook three-dimensional path planning module records and stores the three-dimensional coordinates of all points in the path by using the three-dimensional coordinate values of the hook, and completes the path planning of the hook between two points;

[0012] The hook path tracking control module sends control instructions to the tower crane electric control system according to the hook path tracking process by using the tower crane dynamics performance parameters and all hook three-dimensional coordinates on the path, and completes the hook path tracking control.

[0013] The beneficial effects of the application are that the tower crane completes a hoisting operation between two points by the tower crane operator controlling the handle, realizes manual planning of the path of the hook, and on this basis, a hook path tracking process is designed in combination with the dynamics characteristics of the tower crane, three-dimensional automatic path tracking of the hook for the back-and-forth movement of the tower crane between position A (a loading point) and position B (an unloading point) is realized, and specifically: (1) according to the working characteristics of the tower crane on the construction site, the operator manually completes three-dimensional path planning between two points, which solves the problems of high modeling cost and insufficient real-time performance of the construction site environment; (2) the three-dimensional path tracking process of the hook based on dynamics is adopted, which realizes unmanned control of the two-point back-and-forth movement of the tower crane, solves the problem of stable control of the tower crane in a large inertia state, and has small tracking position error, thereby improving the working efficiency of the tower crane. BRIEF DESCRIPTION OF DRAWINGS

[0014] Figure 1 is a diagram of the relationship between the movement of the tower crane and the position of the hook in the method of the application;

[0015] Figure 2is a flow of detecting the dynamic characteristics of the slewing motion of the tower crane in the method of the application;

[0016] Figure 3 is a flow of detecting the dynamic characteristics of the luffing motion of the trolley of the tower crane in the method of the application;

[0017] Figure 4 is a flow of detecting the dynamic characteristics of the hoisting motion of the hook of the tower crane in the method of the application;

[0018] Figure 5 is a schematic diagram of the path planning of the tower crane in the method of the application;

[0019] Figure 6 is a flow of the hook path tracking control of the tower crane based on the dynamic characteristics in the method of the application;

[0020] Figure 7 is a functional module relationship block diagram used in the method of the application;

[0021] Figure 8 is the planned path of the hook of the tower crane from position A to position B in embodiment 1 of the method of the application;

[0022] Figure 9 is the tracking path of the hook from position A to position B in embodiment 1 of the method of the application, using tracking control mode 1;

[0023] Figure 10 is the tracking path of the hook from position B to position A in embodiment 2 of the method of the application, using tracking control mode 2;

[0024] Figure 11 is the path planning of the hook from position A to position B in embodiment 3 of the method of the application, using tracking control mode 1;

[0025] Figure 12 is the tracking path of the hook from position B to position A in embodiment 4 of the method of the application, using tracking control mode 2. DETAILED DESCRIPTION

[0026] The method of the application will be described in detail below with reference to the accompanying drawings and specific embodiments.

[0027] The design idea of the application is that the path planning of the hook of the tower crane between two points is completed by the tower crane driver manually operating to guide the hook to hoist once between the two points, and on this basis, the hook path tracking control method is designed based on the dynamic characteristics of the tower crane to realize the unmanned control of the hook between the two points in a complex environment, ensure the stability of the movement of the tower crane, and improve the working efficiency of the tower crane.

[0028] Reference Figure 7The three-dimensional path planning and tracking control system of the tower crane hook comprises four functional modules, namely, a tower crane operation state detection module, a tower crane dynamics performance detection module, a hook three-dimensional path planning module and a hook path tracking control module.

[0029] A. The tower crane operation state detection module.

[0030] According to the working principle of the tower crane, the tower crane controls the three-dimensional movement of the hoist arm rotation, the trolley amplitude variation and the hook lifting to control the running position of the hook in the three-dimensional space and complete the safe hoisting of materials.

[0031] Referring to Figure 1 , the tower crane operation state detection module is constructed in such a way that a three-dimensional coordinate system O-XYZ is established with the center O of the tower crane standard section as the origin, wherein X-O-Y is referred to as a two-dimensional ground coordinate system, the tower crane rotation angle θ t is the included angle between the vertical projection of the hoist arm and the X axis, the trolley amplitude r t is the distance between the amplitude trolley and the center (origin O) of the tower crane, and the hook height h t is the height of the hook relative to the ground.

[0032] According to the tower crane rotation angle θ t , the trolley amplitude r t and the hook height h t , the current three-dimensional space position D(θ t , r t , h t ) of the hook can be determined, and thus the data acquisition mode of the tower crane operation state detection module is to realize the real-time monitoring and data acquisition of the current position D(θ t , r t , h t ) of the tower crane hook by respectively installing a height sensor, a position sensor and an angle sensor on the tower crane, and to complete storage.

[0033] B. The tower crane dynamics performance detection module.

[0034] According to the working principle of the tower crane, the control of the three-dimensional motion of the slewing boom, the trolley luffing and the hook lifting of the tower crane all adopts frequency conversion speed control. Because the inertia of the tower crane is large and the motion speed response is slow, if the speed of the tower crane changes too fast, it will cause rigid impact on the structure of the tower crane or lead to unstable motion, therefore, the motion speed of the tower crane adopts a split speed regulation mode, and the speed gear adjustment needs to be based on the dynamics performance of the tower crane. Considering that the dynamics performance of the three-dimensional motion of the tower crane is different, and the influence of different motions on the tower crane is different, before the split control of the motion speed of the tower crane, the dynamics performance of different motions of the tower crane needs to be detected.

[0035] B1) Dynamics performance detection of the slewing motion of the tower crane.

[0036] The influence of the dynamics performance of the slewing motion of the tower crane mainly manifests that when the tower crane slews, if the slewing motion of the boom is suddenly started and stopped, the inertia of the boom will impact the steel structure of the tower crane.

[0037] The angular velocity of the slewing motion of the boom of the tower crane is set to three gears, which are ω θi , i = 1, 2, 3, wherein ω θ3 is the highest gear, at the same time, the counterclockwise slewing motion is determined as positive, and the clockwise slewing motion is determined as negative.

[0038] Referring to Figure 2 , the detection flow of the dynamics performance of the slewing motion of the tower crane is as follows:

[0039] B1.1) Confirm that the tower crane is under rated load;

[0040] B1.2) The tower crane slews at the highest speed ω θ3 stably;

[0041] B1.3) Real-time obtain the rotation angle θ t of the boom of the tower crane by using the tower crane running state detection module;

[0042] B1.4) The boom of the tower crane is suddenly stopped at the angle θ0, due to inertia, the boom of the tower crane continues to rotate until the boom reaches the angle θ1 and stops moving;

[0043] B1.5) Calculate the inertia angle θ = |θ1-θ0| of the highest slewing speed ω θ3 , and define the corresponding slewing speed control threshold θ3 of the highest slewing speed ω θ3 at the inertia angle θ position;

[0044] According to the above process, the corresponding slewing speed control threshold θ2 of the slewing speed ω θ2 is obtained, and the corresponding slewing speed control threshold θ1 of the slewing speed ω θ1 is obtained.

[0045] B2) The dynamic performance detection of the trolley amplitude variation movement of the tower crane.

[0046] The influence of the dynamic performance of the trolley amplitude variation movement of the tower crane mainly shows that the load under the tower crane steel rope will swing, resulting in unstable load movement, when the trolley amplitude variation movement of the tower crane suddenly stops. In order to reduce the swing of the load, according to the frequency conversion control principle of the tower crane, the frequency converter parameter "acceleration and deceleration time" can be adjusted, so as to adjust the slope of the frequency converter frequency change, so that the trolley amplitude variation speed change has a buffer process, thereby reducing the load swing caused by the trolley amplitude variation movement.

[0047] The trolley amplitude variation movement of the tower crane is set to three gears, respectively v ri , i = 1, 2, 3, v r3 is taken as the highest gear; at the same time, the outward amplitude variation direction of the trolley is determined as the positive direction, and the inward amplitude variation direction of the trolley is determined as the negative direction.

[0048] Referring to Figure 3 , the detection process of the dynamic performance of the trolley amplitude variation movement is as follows:

[0049] B2.1) Confirming that the trolley is under the rated load;

[0050] B2.2) Adjusting the "acceleration and deceleration time" parameter of the trolley amplitude variation frequency converter;

[0051] B2.3) The trolley moves stably at the highest amplitude variation speed v r3 ;

[0052] B2.4) Real-time obtaining the amplitude r t of the trolley by using the tower crane running state detection module;

[0053] B2.5) The trolley suddenly stops at the amplitude r0, and the trolley continues to move in the process of decreasing from the speed v r3 to stop, and the trolley stops moving when the trolley amplitude is r1; in this process, due to the inertia of the load, swing will be generated, if the load swings greatly in the air, return to step B2.2) to adjust the "acceleration and deceleration time" parameter of the frequency converter; if the load swing is small, confirm that the "acceleration and deceleration time" parameter of the frequency converter is reasonable, and execute step B2.6);

[0054] B2.6) Calculating the movement distance r = |r1-r0| of the trolley highest amplitude variation speed v r3 ; defining the movement distance r as the amplitude variation speed control threshold r3 corresponding to the trolley amplitude variation speed v r3 ;

[0055] According to the above process, the amplitude variation speed control threshold r2 of the trolley amplitude variation speed v r2 is detected, and the trolley amplitude variation speed vr1 amplitude speed control threshold value r1;

[0056] B3) Detection of the dynamic performance of the tower crane hook lifting movement.

[0057] The influence of the dynamic performance of the tower crane hook lifting movement mainly manifests in the sudden change of the tower crane hook lifting movement, which will cause the vibration of the tower crane jib in the vertical direction due to the load inertia. In order to reduce the vibration of the jib, according to the tower crane frequency conversion control principle, the frequency converter parameter "acceleration and deceleration time" can be adjusted, the slope of the frequency converter frequency change is adjusted, so that the hook lifting speed change has a buffer process, thereby reducing the jib vibration caused by the hook lifting movement.

[0058] The speed of the tower crane hook lifting movement is set to five gears, respectively v hi , i = 1, 2, 3, 4, 5, wherein v h5 as the highest gear, the upward movement direction of the hook is determined as the positive direction, and the downward movement direction of the hook is determined as the negative direction.

[0059] Referring to Figure 4 , the detection process of the dynamic performance of the tower crane hook lifting movement is as follows:

[0060] B3.1) Confirm that the hook is at the rated load;

[0061] B3.2) The hook is stopped at the height h0 position;

[0062] B3.3) Adjust the "acceleration and deceleration time" parameter of the hook lifting frequency converter;

[0063] B3.4) Real-time obtain the height h t of the hook by using the tower crane running state detection module;

[0064] B3.5) The hook is at the height h0 position, and the upward movement of the hook is suddenly started at the highest lifting speed v h5 , the hook speed rises from 0 to v h5 , the hook height reaches h1, and in this process, if the swing of the jib is large, return to step B3.2) to adjust the "acceleration and deceleration time" parameter of the lifting frequency converter; if the jib swing is small, confirm that the frequency converter parameter is reasonable, and execute step B3.6);

[0065] B3.6) Calculate the motion distance of the highest lifting speed v h5 of the hook: h = |h1-h0|; define the motion distance h as the hook lifting speed control threshold value h5 corresponding to the highest lifting speed v h5 of the hook;

[0066] According to the above method, the tower crane hook lifting speeds v h4 , vh3 ,v h2 ,v h1 Corresponding hook lifting speed control threshold h4, h3, h2 and h1.

[0067] C, hook three-dimensional path planning module.

[0068] When the tower crane needs to hoist materials between position A and position B multiple times, according to the driving experience and the environmental state, the tower crane driver controls the three-dimensional motion of the tower crane boom slewing, trolley luffing and hook lifting by operating the handle, and the tower crane completes a complete hoisting process of the hook from position A to position B with the optimal path recognized by the tower crane driver;

[0069] In this process, the hook forms a three-dimensional path in space, the tower crane operating state detection module collects the tower crane operating state data in real time through three related sensors (i.e. height sensor, position sensor and angle sensor), and records the continuous position p i (θ i ,r i ,h i ) of the hook in the process of the tower crane operation, so as to plan the three-dimensional path of the hook of the tower crane between position A and position B; at the same time, considering the particularity of the slewing motion of the tower crane, the direction of the slewing motion of the tower crane is recorded in the manual planning process of the path, and the tower crane slewing motion direction variable is F θ , then F θ =1 when the tower crane slews counterclockwise, and F θ =0 when the tower crane slews clockwise.

[0070] A material transportation of the tower crane includes three stages of lifting, stable moving and hook dropping, the lifting stage is the lifting motion of the hook, and the hook is away from the ground in height; the stable moving stage generally refers to that after the hook is lifted away from the ground, the three-dimensional motion of the tower crane boom slewing, trolley luffing and hook lifting is used to reach the target position above according to different target positions; the hook dropping stage is that the hook of the tower crane moves downward, and the hook reaches the height of the unloading area.

[0071] The dashed line in Figure 5 , Figure 5 is the driving path of the tower crane actively controlled by the driver, which is the three-dimensional planning path of the hook formed from position A to position B, wherein position A→position C1 is the lifting stage; position C1→position C2 is the stable moving stage; and position C2→position B is the hook dropping stage. Figure 5 The points p1, p2, p3, …, p i ,...,p n in Figure 5 , Figure 5 are specific position points recorded in the path planning, see Table 1, which shows the three-dimensional coordinates of each specific position point in the planning path; at the same time, the tower crane slewing motion direction variable Fθ = 1.

[0072] Table 1, the p of each position point collected by the hook three-dimensional path planning module n (θ n , r n , h n )

[0073]

[0074] D, hook path tracking control module.

[0075] After the hook three-dimensional path planning module is constructed, the hook path tracking control module determines the front tracking point by detecting the running state of the hook and the planned path in real time, calculates the position error (including the angle error, the amplitude error and the height error), and plans the three-dimensional motion direction and the speed gear of the hook based on the dynamics performance of the hook three-dimensional motion, and outputs the motion control signal to the tower crane electrical control system (which is the control system on the tower crane), and the tower crane electrical control system specifically controls the slewing of the tower crane jib, the amplitude change of the trolley and the lifting of the hook, so as to realize the three-dimensional path tracking motion of the tower crane hook under the unmanned control.

[0076] In the method of the application, the hook path tracking control module actually includes two modes, one is the tracking control output of the hook from position A to position B, which is called path tracking control mode 1; the second is the tracking control output of the hook from position B to position A, which is called path tracking control mode 2, and the paths of the two modes are the same but the motion directions are opposite, and the two working modes are respectively implemented in combination with the direction instructions (positive or negative) of the related actions.

[0077] Referring to Figure 6 , the flow of the hook path tracking control module of the application is as follows:

[0078] Step 1: initialize each module;

[0079] 1.1) start the tower crane running state detection module and the hook path tracking control module, and at the same time, start the communication between the hook path tracking control module and the tower crane electrical control system;

[0080] 1.2) the hook path tracking control module reads the parameters set by the hook three-dimensional path planning module, including: the data of the three-dimensional path position point p n (θ n , r n , h n ) and the total number of planned path tracking points n;

[0081] 1.3) The hook path tracking control module reads the dynamic performance parameters of the three-dimensional motion of the tower crane obtained by the tower crane dynamic performance detection module, including: the rotation speed control threshold θ1, θ2, θ3, the amplitude speed control threshold r1, r2, r3, the hook lifting speed control threshold h1, h2, h3, h4, h5;

[0082] 1.4) Set the hook path tracking space position error ε in the hook path tracking control module, and update the forward distance S of the tracking point dl .

[0083] Step 2: Determine the path tracking control mode;

[0084] The hook path tracking control module obtains the current position D(θ t , r t , h t ) of the hook, calculates the spatial distance |Dp1| of the current position and the p1 point of the three-dimensional path planning, and the spatial distance |Dp2| of the current position and the p n point;

[0085] If |Dp1| < |Dp2|, it is determined to belong to path tracking control mode 1, and the tracking target point is set to p n ; Otherwise, it is determined to belong to path tracking control mode 2, and the tracking target point is set to p1.

[0086] Step 3: Obtain the current position of the hook;

[0087] The hook three-dimensional path planning module uses three sensors installed on the tower crane to obtain the three-dimensional coordinate values D(θ t , r t , h t ) of the current position of the hook of the tower crane in real time.

[0088] Step 4: Calculate the position deviation of the hook three-dimensional path tracking;

[0089] Let P k (θ k , r k , h k ) be the position three-dimensional coordinates of the current tracking point, then the position deviation of the current position of the hook of the tower crane and the current tracking point in the hook path tracking control process is set as: angle deviation Δθ, amplitude deviation Δr and height deviation Δh, the expression is as follows:

[0090] Δθ = θ k - θ t ,

[0091] Δr = r k - r t ,

[0092] Δh = h k -h t .

[0093] Step 5: Three-dimensional path tracking control planning of tower crane hook based on dynamics;

[0094] The tower crane path tracking control planning is to determine the current rotation angular velocity ω θ , the trolley luffing motion speed v r , and the hook lifting speed v h ; at the same time, a positive or negative prefix is added in front of each speed value to clearly indicate the positive and negative directions of three-dimensional motion, which is explained as follows:

[0095] 5.1) The rotation motion planning of the tower crane jib is set as follows:

[0096] The angular velocity of the tower crane jib rotation motion is set to three gears, denoted as ω θi , i = 1, 2, 3, and ω θ3 is the highest gear; the direction of the tower crane jib rotation motion is determined by the variable F θ recorded in the path planning process, and the angular velocity gear of the rotation motion is set as follows:

[0097] If |Δθ| < θ1, set the current rotation angular velocity ω θ = 0;

[0098] If θ1 < |Δθ| < θ2, set the current rotation angular velocity ω θ = ω θ1 ;

[0099] If θ2 < |Δθ| < θ3, set the current rotation angular velocity ω θ = ω θ2 ;

[0100] If θ3 < |Δθ|, set the current rotation angular velocity ω θ = ω θ3 ;

[0101] Where θ1, θ2, θ3 are the rotation speed control thresholds corresponding to the three rotation angular velocity gears respectively.

[0102] 5.2) The trolley luffing motion planning of the tower crane is set as follows:

[0103] The tower crane trolley luffing motion is set to three gears, denoted as v ri , i = 1, 2, 3, and v r3As the highest gear, if the amplitude deviation Δr > 0, the tower trolley outwardly varies the amplitude, and the movement speed is positive; otherwise, the trolley varies the amplitude inwardly, and the movement speed is negative;

[0104] If Δr ≤ r1, the current trolley amplitude movement speed v is set to be 0, and the trolley amplitude movement stops; r

[0105] If r1 < Δr ≤ r2, the current trolley amplitude movement speed v is set to be v r ; r1

[0106] If r2 < Δr ≤ r3, the current trolley amplitude movement speed v is set to be v r ; r2

[0107] If r3 < Δr, the current trolley amplitude movement speed v is set to be v r ; r3

[0108] Wherein, r1, r2, r3 are the amplitude deviation thresholds corresponding to the trolley amplitude movement speed gears respectively.

[0109] 5.3) The lifting movement planning of the tower hook is set as follows:

[0110] The hook lifting movement speed is set to five gears, which are respectively denoted as v hi , i = 1, 2, 3, 4, 5, wherein v h5 is the highest gear; if the height deviation Δh > 0, the tower upwardly hooks, and the hook lifting movement speed is positive; otherwise, the tower downwardly falls the hook, and the hook lifting movement speed is negative;

[0111] If h5 < |Δh|, the current hook lifting movement speed v is set to be V h ; h5

[0112] If h4 < |Δh| < h5, the current hook lifting movement speed v is set to be V h ; h4

[0113] If h3 < |Δh| < h4, the current hook lifting movement speed v is set to be V h ; h3

[0114] If h2 < |Δh| < h3, the current hook lifting movement speed v is set to be V h ; h2

[0115] If h1 < |Δh| < h2, the current hook lifting movement speed v is set to be V h ​​​​​​​​= V h1 ;

[0116] If |Δh| < h1, the hook lifting movement speed v h = 0;

[0117] Wherein, h1, h2, h3, h4, h5 are the height deviation threshold values corresponding to the tower crane lifting movement speed gear respectively.

[0118] Step 6: Hook tracking control output;

[0119] The hook path tracking control module sends the tower crane movement speed and direction instructions planned in step 5 to the tower crane electrical control system, and the tower crane electrical control system controls the related driving devices of the tower crane three-dimensional movement to accurately complete the three-dimensional movement state of the jib slewing, trolley amplitude changing and hook lifting.

[0120] Step 7: Update the point to be tracked;

[0121] Considering the large inertia of the tower crane, in order to ensure the smoothness of the hook path tracking movement, the tower crane updates the point to be tracked in a larger look-ahead distance S dl ; as Figure 5 The two points marked with "X" in the planning path shown by the dashed line are respectively referred to as the current tracking point and the point to be tracked.

[0122] Suppose the total number of points recorded in the planning path (dashed line) is n, the current position coordinates of the hook are D(θ t , r t , h t ), and the current tracking point is p k (θ k , r k , h k ), then the updating method of the point to be tracked is as follows:

[0123] 7.1) In the path tracking control mode 1, if the current tracking point p k (θ k , r k , h k ) is the path tracking target point p n (θ n , r n , h n ), then the point to be tracked is still p n (θ n , r n , h n ); otherwise, calculate points p k+1 (θ k+1 , r k+1 , h k+1 ), …, p n (θn , r n , h n ) and the current position D(θ t , r t , h t ) of the hook, S i (i = k + 1,... n) is calculated as follows:

[0124]

[0125] The distance point S i with the minimum error to the forward distance S dl is determined as the point to be tracked.

[0126] 7.2) In the path tracking control mode 2, if the current tracking point p k (θ k , r k , h k ) is the path tracking target point p1(θ1, r1, h1), the point to be tracked is still p1(θ1, r1, h1); otherwise, the distances S k-1 (θ k-1 , r k-1 , h k-1 ),..., p1(θ1, r1, h n ) of the points p t (θ t , r t , h i ) and the current position D(θ t , r t , h t ) of the hook are calculated, and the distance calculation formula is as follows:

[0127]

[0128] Step 8: Determine whether the path tracking is completed.

[0129] The spatial distance DP between the current position D(θ t , r t , h t ) of the tower hook and the path tracking target point is calculated.

[0130] When the current state is the path tracking mode 1, the expression of the spatial distance DP is:

[0131]

[0132] When the current state is the path tracking mode 2, the expression of the spatial distance DP is:

[0133]

[0134] If |DP| < ε, ε is an error value, it indicates that the hook reaches the target position, the path tracking is completed, and the hook path tracking control process is exited this time;

[0135] Otherwise, return to step 3 to continue tracking.

[0136] Reference Figure 7 The three-dimensional path planning and tracking control method of the tower crane hook of the present application relies on the flow of the aforementioned four functional modules, namely the tower crane operating state detection module, the tower crane dynamics performance detection module, the hook three-dimensional path planning module, and the hook path tracking control module,

[0137] Firstly, the tower crane driver performs an actual hoisting operation, the hook travels a three-dimensional space path from position A to position B, the tower crane operating state detection module obtains the position information of the tower crane hook in real time by using the three sensors installed on the tower crane, and then the tower crane operating state detection module provides historical data and real-time data for the other three functional modules;

[0138] The tower crane dynamics performance detection module uses the position information of the tower crane hook obtained in real time by the tower crane operating state detection module, and detects the dynamics performance of the three-dimensional motion of the tower crane according to the flow of Figure 2 、 Figure 3 and Figure 4 , respectively, to obtain the performance parameters of the rotation speed control threshold θ1, θ2, θ3, the amplitude speed control threshold r1, r2, r3, and the hook lifting speed control threshold h1, h2, h3, h4, h5;

[0139] The hook three-dimensional path planning module uses the three-dimensional coordinate values D(θ t , r t , h t ) of the hook on the three-dimensional path obtained in real time by the tower crane operating state detection module, and records and stores the three-dimensional coordinates of all points in the path, thereby completing the path planning of the tower crane hook between two points;

[0140] The hook path tracking control module uses the tower crane dynamics performance parameters provided by the tower crane dynamics performance detection module and the three-dimensional coordinates of the hook on the three-dimensional path provided by the hook three-dimensional path planning module, according to the hook path tracking flow shown in Figure 6 , issues control instructions to the tower crane electrical control system to control the three-dimensional motion state of the tower crane, and realizes the path tracking control of the hook.

[0141] Experimental verification

[0142] A construction site uses a tower machine CCT331, the length of the tower machine's lifting arm is 70m, the angle sensor of the tower machine's rotation, the position sensor of the trolley's amplitude variation and the position sensor of the hook's height are installed on the tower machine, and the current position D(θ t , t , t ) of the tower machine's hook is obtained in real time.

[0143] Firstly, according to the tower machine dynamics performance detection module of the method of the present application, the tower machine dynamics performance, i.e. the speed gear position and position deviation threshold value of three-dimensional motion, is obtained according to the flow of steps Figure 2 , Figure 3 and Figure 4 , as shown in Table 2, Table 3 and Table 4, wherein the angular velocity unit is r.p.m, the angle unit is degree, the speed unit is m / min and the distance unit is m.

[0144] Table 2, the rotation angular velocity gear position and angle deviation threshold value of the tower machine's lifting arm

[0145] Swing parking Swing 1st gear Swing 2nd gear Swing 3rd gear <![CDATA[ω θ ]]> 0 0.24 0.48 0.78 Δθ 5 30 90 >90

[0146] Table 3, the speed gear position and amplitude deviation threshold value of the trolley's amplitude variation

[0147] Luff parking Luff 1st gear Luff 2nd gear Luff 3rd gear v r ]]> 0 6 28 55 Δr 1 6 20 >20

[0148] Table 4, the lifting speed and corresponding distance deviation threshold value of the tower machine

[0149] Hoist 1st gear Hoist 2nd gear Hoist 3rd gear Hoist 4th gear Hoist 5th gear v h ]]> 1.5 6 30 50 70 Δh 0.2 0.4 0.6 0.8 1.0

[0150] Based on the above basic data, the following embodiment is specifically implemented by using the method of the present application.

[0151] Embodiment 1

[0152] Referring to Figure 8 , the height of the tower machine is 70m, and the tower machine needs to hoist the steel bars from the position A(30, 20, 0) of the ground steel bar material area to the point B(-30, 30, 30) of the building work surface.

[0153] According to the above method steps, the three-dimensional path tracking control of the tower machine's hook from the position A to the position B is completed, i.e. tracking control mode 1 is adopted.

[0154] Supposing that the hook-off-ground judgment threshold value h g is 5m, the movement starting point of the hook is the position A(30, 20, 0) in Figure 8 , according to the tower machine's on-site safety work specification, the hook path setting process is as follows: firstly, the hook is lifted at low speed (from Table 3, the first gear v h = 30m / min), at this time, the rotation movement and the amplitude variation movement are stopped ωθ =0, v r =0; When the tower crane hook reaches a low-speed lifting height of 5 meters, the hook reaches Figure 8 Then, the crane driver controls the hook to move to the target position by linking the crane's slewing, luffing and lifting movements according to the current position of the hook and the target position. Figure 8 Position P2 (-30, 30, 35) in the tower crane; finally, the tower crane drops the hook at a low speed (see Table 3, using the first gear v h =30m / min) until the hook reaches the target point. Figure 8 Position B(-30,30,30) in the.

[0155] When the crane driver completes the above hook movement, the crane's operating status is detected in real time through the crane's rotation angle sensor, the trolley's amplitude position sensor, and the hook's height position sensor. The hook position is recorded according to an execution cycle of 1 second to obtain the three-dimensional planning path of the crane's hook from position A to position B. Figure 8 The dotted line in the figure is the planned path from position A to position B. The path records a total of 562 planned points, which record the tower crane rotation direction variable F. θ =1;

[0156] according to Figure 6 As shown, the current position D(θ t , r t , h t ) and the position error (Δθ, Δr, Δh) of the point to be tracked on the planned path, according to Figure 6 Step 5.1) and Table 1, determine the rotation speed ω θ Generally, during the hoisting process, the direction of the slewing motion of the tower crane remains unchanged. According to the slewing motion direction F recorded in the path planning, θ =1, then Figure 8 The dotted line tracks the process from position A to position B, and the rotation motion is counterclockwise. Similarly, according to Figure 6 Step 5.2) and Table 2 determine the amplitude motion speed v r , determine the lifting speed v according to step 5.3) and Table 3 h , send control instructions to the tower crane electrical control system to control the three-dimensional motion state of the tower crane.

[0157] Assume that the hook tracking error is ε = 2 meters and the forward sight distance S dl =5 meters; press Figure 6The shown hook path tracking control path moves from position A to position B, and the tower hook path tracking control execution period is 1 second. When all the 526 position points in the path are tracked and the distance between the hook position and the position B of the end point of the path is less than or equal to the final tracking error, the planning path tracking control is completed. Figure 9 The dashed line from position A to position B represents the planning path, and the solid line represents the movement path of the hook from position A to position B in the unmanned operation. The arrow on the solid line represents the direction of path tracking.

[0158] It can be seen that, by using the method of the present application, the maximum distance error between the actual movement path of the tower hook and the planning path is 3.19 meters, and the average distance error is 2.16 meters, which fully meets the requirements of the technical specification.

[0159] Example 2

[0160] Based on the same scenario as Figure 8 , the tower height is 70 meters, and during the operation of the tower, the tower moves back and forth between the ground material area position A (30, 20, 0) and the building work surface position B (-30, 30, 30) to transport steel bars.

[0161] According to the method steps described above, the three-dimensional path tracking control of the tower hook from position B to position A is completed in this embodiment 2, that is, tracking control mode 2 is used, and the specific description is as follows:

[0162] As Figure 8 shown by the dashed line between positions A and B, the path planning is performed. According to Figure 6 shown, the position error (Δθ, Δr, Δh) between the current position D (θ t , r t , h t ) of the tower hook and the position to be tracked on the planning path is calculated in real time. According to Figure 6 step 5.1) and Table 1, the rotation movement speed ω θ is determined. Since the path planning method is opposite to the tracking direction, the rotation movement direction F θ = 0, and the inverse time rotation is performed. Similarly, according to Figure 6 step 5.2) and Table 2, the luffing movement speed v r is determined, and according to step 5.3) and Table 3, the hoisting movement speed v h is determined.

[0163] The tower electrical control system controls the three-dimensional movement state of the tower according to the control instructions. It is assumed that the hook tracking error is 2 meters, and the forward distance is set to 5 meters. According to Figure 6The tower crane hook path tracking control execution cycle is 1 second from position B to position A. When all 526 position tracking points in the path are completed, and the distance between the hook position and the path end point A is less than or equal to the final tracking error, the path tracking control process is determined to be completed. Figure 10 In the figure, the dashed line represents the planned path between position A and position B, the solid line represents the path of the tower crane hook moving from position B to position A in the unmanned operation, and the arrow on the solid line represents the direction of path tracking.

[0164] It can be seen that the maximum distance error between the actual movement path of the tower crane hook and the planned path is 3.48 meters, and the average distance error is 1.73 meters.

[0165] Example 3

[0166] Referring to Figure 11 , the tower crane height is 70 meters, and the tower crane needs to reach position B(-30, 30, 30) from position A(-20, -30, 0) during operation.

[0167] According to the method steps described above, the three-dimensional path tracking control of the tower crane hook from position A to position B is completed in this embodiment 3, that is, tracking control mode 1 is used, and the specific description is as follows:

[0168] Let the hook off the ground threshold h g = 5m, according to the tower crane site safety work specification, the hook completes a manual control hoisting operation from position A to position B under the manual control of the tower crane driver, in this process, the three operating state parameters of the tower crane are detected in real time through the tower crane operating state detection module, and the position information of the hook at each tracking point is recorded, and the three-dimensional planned path of the hook from position A to position B is obtained.

[0169] Figure 11 The dashed line in the figure is the planned path from position A to position B, which records a total of 562 tracking points, and records the tower crane rotation direction variable F θ = 0. Figure 11 The solid line in the figure is the tracking movement path of the hook from position A to position B, and the arrow on the solid line represents the direction of path tracking.

[0170] From Figure 11 It can be seen that the maximum distance error between the actual movement path of the tower crane hook and the planned path is 3.14 meters, and the average distance error is 1.91 meters.

[0171] Example 4

[0172] Similarly based on Figure 11, tower height 70 meters, tower working process, the tower needs to reach A(-20, -30, 0) position from B(-30, 30, 30) position.

[0173] According to the foregoing method steps, this embodiment 4 completes the three-dimensional path tracking control of the tower hook from position B to position A, that is, tracking control mode 2 is adopted, and specific description is as follows:

[0174] As Figure 12 The dashed line in the figure represents the path planning between position A and position B in embodiment 3, and according to Figure 6 The position error (Δθ, Δr, Δh) between the current position D(θ t , r t , h t ) of the tower hook and the position to be tracked on the planned path is calculated in real time, according to Figure 6 Step 5.1) and Table 1, the rotating motion speed ω θ is determined, and since the path planning method is opposite to the tracking direction, the rotating motion direction F θ =1; Similarly, according to step 5.2) and Table 2 of Figure 6 The amplitude motion speed v r is determined, and according to step 5.3) and Table 3, the lifting motion speed v h is determined.

[0175] The tower electrical control system controls the three-dimensional motion state of the tower according to the control instruction. It is assumed that the hook tracking error is 2 meters, and the forward-looking distance is set to 5 meters; according to Figure 6 The tower hook path tracking control execution cycle is 1 second from position B to position A, and when all 526 position tracking points in the path are completed, the distance between the hook position and the path endpoint A is less than or equal to the final tracking error, the path tracking control process is completed. Figure 12 In the figure, the dashed line represents the planned path between position A and position B, the solid line represents the path of the tower hook moving from position B to position A without human operation, and the arrow on the solid line represents the direction of path tracking.

[0176] It can be seen that by using the method of the present application, the maximum distance error between the actual motion path of the tower hook and the planned path is 3.43 meters, and the average distance error is 1.66 meters.

[0177] Embodiment 5

[0178] The tower height is 70 meters, and the tower needs to reach B(-30, 30, 30) position from A(60, 20, 0) position during the working process of the tower. According to the foregoing method steps, this embodiment 5 completes the three-dimensional path tracking control of the tower hook from position A to position B, that is, tracking control mode 1 is adopted, and specific description is as follows:

[0179] Set the hook off the ground to determine the threshold h g = 5m, according to the tower crane site safety work specification, under the manual control of the tower crane driver, the hook completes a manual control hoisting operation from position A to position B, in the process, through the angle sensor of the tower crane rotation state, the position sensor of the trolley amplitude state and the position sensor of the hook height state, the three operating state parameters of the tower crane are detected in real time, and the position information of the hook at each tracking point is recorded, and the three-dimensional path of the tower crane hook from position A to position B is planned. The path records 442 tracking points, records the tower crane rotation movement direction variable F θ = 1. On this basis, the tower crane hook path tracking control module based on the dynamics characteristics of the method of the application, the maximum distance error between the actual motion path of the tower crane hook and the planned path is 4.39 meters, and the average distance error is 2.19 meters.

[0180] Example 6

[0181] The tower crane height is 70 meters, and during the operation of the tower crane, the tower crane needs to reach position A (60, 20, 0) from position B (-30, 30, 30). According to the method steps described above, this embodiment 6 completes the three-dimensional path tracking control of the tower crane hook from position B to position A, that is, tracking control mode 2 is adopted, and the specific description is as follows:

[0182] The path planning between position A and position B obtained in embodiment 5 is obtained, and then according to Figure 6 , the position error (Δθ, Δr, Δh) between the current position D (θ t , r t , h t ) of the tower crane hook and the position to be tracked on the planned path is calculated in real time, according to Figure 6 step 5.1) and table 1, the rotation movement speed ω θ is determined, and since the path planning method is opposite to the tracking direction, the rotation movement direction F θ = 0; similarly, according to step 5.2) of Figure 6 and table 2, the amplitude movement speed v r is determined, and according to step 5.3) and table 3, the lifting movement speed v h is determined.

[0183] The tower crane electrical control system controls the three-dimensional motion state of the tower crane according to the control command. Set the hook tracking error to 2 meters, and the forward-looking distance to 5 meters; according to Figure 6The tower crane hook path tracking control execution cycle is 1 second from position B to position A, when the 442 position tracking points in the path are all completed tracking, when the distance between the hook position and the path end point A is less than or equal to the final tracking error, the path tracking control process is judged to be completed. Using the method of the application, the maximum distance error between the actual movement path of the tower crane hook and the planned path is 3.76 meters, and the average distance error is 1.61 meters.

Claims

1. The three-dimensional path planning and tracking control system for the tower crane hook is characterized by: It is composed of four interconnected functional modules, namely the tower crane operation status detection module, the tower crane dynamic performance detection module, the hook three-dimensional path planning module, and the hook path tracking control module. The tower crane operation status detection module provides data support for the tower crane dynamic performance detection module and the hook three-dimensional path planning module, and the tower crane operation status detection module, the tower crane dynamic performance detection module, and the hook three-dimensional path planning module jointly provide data support to the hook path tracking control module; The construction method of the tower crane operation status detection module is to establish a three-dimensional coordinate system O-XYZ with the center O of the tower crane standard section as the origin, where XOY is called the ground two-dimensional coordinate system, and the tower crane rotation angle θ t is the angle between the vertical projection of the boom and the X-axis, and the trolley amplitude r t is the distance between the luffing trolley and the center point O of the tower crane, and the hook height h t is the height of the hook relative to the ground; The data acquisition method of the tower crane operation status detection module is to install a height sensor, a position sensor and an angle sensor on the tower crane to realize the current position D (θ t , r t , h t ) real-time monitoring and data collection, and complete storage; The three-dimensional path planning module of the hook is operated by the tower crane operator through the handle to control the three-dimensional movement of the tower crane boom rotation, the trolley luffing and the hook lifting. The tower crane completes a complete lifting process of the hook from position A to position B along the path selected by the tower crane operator. During this process, the hook forms a three-dimensional path in space. The tower crane operation status detection module collects the tower crane operation status data in real time through three related sensors and records the continuous position p of the hook during the operation of the tower crane in real time. i (θ i ,r i ,h i ), thereby planning the three-dimensional spatial path of the tower crane hook between position A and position B; at the same time, considering the particularity of the tower crane's rotation movement, the direction of the tower crane's rotation movement is recorded during the manual path planning process, and the tower crane's rotation movement direction variable is set as F θ , then when the tower crane rotates counterclockwise, F θ =1, when the tower crane rotates clockwise F θ =0; The hook path tracking control module determines the forward tracking point through real-time detection of the hook's operating status and planned path, calculates the position error, and plans the hook's three-dimensional motion direction and speed gear based on the dynamic performance of the hook's three-dimensional motion and adopts a motion control method based on the tower crane's dynamic characteristics. The motion control signal is then output to the tower crane's electrical control system to achieve three-dimensional path tracking motion of the tower crane's hook under unmanned control.

2. The three-dimensional path planning and tracking control system for a tower crane hook according to claim 1, characterized in that: The tower crane dynamic performance detection module needs to detect the dynamic performance of different movements of the tower crane before performing step-by-step control of the tower crane's movement speed. It is divided into the following three aspects: B1) Dynamic performance test of tower crane rotation motion, The angular velocity of the tower crane arm rotation motion is set to three gears, namely ω θi , i=1,2,3, where ω θ3 As the highest gear, at the same time, the counterclockwise rotation movement is determined as the positive direction, and the counterclockwise rotation movement is determined as the negative direction. The detection process of the dynamic characteristics of the tower crane's rotary motion is as follows: B1.1) Confirm that the tower crane is under rated load; B1.2) The tower crane runs at the highest speed ω θ3 Smooth rotation; B1.3) Use the tower crane operation status detection module to obtain the rotation angle θ of the tower crane boom in real time t ; B1.4) The tower crane boom suddenly stops rotating at angle θ0. Due to inertia, the tower crane boom continues to rotate until it reaches angle θ1. B1.5) Calculate the maximum rotation speed ω θ3 The inertia angle is: θ = |θ1-θ0|; the maximum rotation speed ω at the inertia angle θ is defined θ3 The corresponding rotation speed control threshold θ3; According to the above process, the tower crane rotation speed ω is obtained θ2 The corresponding rotation speed control threshold θ2 is obtained to obtain the tower crane rotation speed ω θ1 The corresponding rotation speed control threshold θ1; B2) Dynamic performance test of tower crane trolley luffing motion, The tower crane trolley has three gears for variable amplitude motion, namely v ri ,i=1,2,3,v r3 As the highest gear; at the same time, the direction of the trolley changing the luffing outward is determined as the positive direction, and the direction of the trolley changing the luffing inward is determined as the negative direction. The detection process of the dynamic characteristics of the trolley's variable amplitude motion is as follows: B2.1) Confirm that the trolley is under rated load; B2.2) Adjust the "acceleration and deceleration time" parameters of the trolley variable speed inverter; B2.3) The trolley moves at the highest variable speed v r3 smooth movement; B2.4) Use the tower crane operation status detection module to obtain the trolley amplitude r in real time t ; B2.5) The car suddenly stops at amplitude r0, and the car goes from speed v r3 The trolley will continue to move as it descends to a stop, and will stop when the amplitude reaches r1. During this process, the load will swing due to inertia. If the load swings significantly in the air, return to step B2.2) and adjust the inverter's "acceleration and deceleration time" parameters. If the load swings slightly, confirm that the inverter's "acceleration and deceleration time" parameters are reasonable and proceed to step B2.6). B2.6) Calculate the maximum amplitude change speed v of the trolley r3 The movement distance: r = |r1-r0|; the movement distance r is defined as the maximum speed v of the trolley r3 The corresponding amplitude speed control threshold r3; According to the above process, the amplitude change speed control threshold r2 of the trolley amplitude change speed vr2 is detected, and the amplitude change speed v of the trolley is detected. r1 The amplitude speed control threshold r1; B3) Dynamic performance test of tower crane hook lifting motion, The speed of the tower crane's hook lifting movement is set to five gears, namely v hi , i=1,2,3,4,5, where v h5 As the highest gear, the upward movement direction of the hook is determined as the positive direction, and the downward movement direction of the hook is determined as the negative direction. The detection process of the dynamic characteristics of the tower crane hook lifting motion is as follows: B3.1) Confirm that the hook is under rated load; B3.2) The hook stops at height h0; B3.3) Adjust the "acceleration and deceleration time" parameters of the hook lifting inverter; B3.4) Use the tower crane operation status detection module to obtain the hook height h in real time t ; B3.5) The hook is at height h0 and is suddenly lifted at the maximum speed v h5 Start the hook to move upward, and the hook speed increases from 0 to v h5 , the hook height reaches h1. During this process, if the boom swings significantly, return to step B3.2) and adjust the "acceleration / deceleration time" parameters of the lifting inverter. If the boom swings slightly, confirm that the inverter parameters are reasonable and proceed to step B3.6). B3.6) Calculate the maximum lifting speed v of the hook h5 Movement distance: h = |h1-h0|; the movement distance h is defined as the maximum speed v of the hook lifting movement h5 The corresponding hook lifting speed control threshold h5; According to the above method, the lifting speed v of the tower crane hook is detected respectively. h4 ,v h3 ,v h2 ,v h1 The corresponding hook lifting speed control thresholds are h4, h3, h2 and h1.

3. The three-dimensional path planning and tracking control system for a tower crane hook according to claim 1, characterized in that: The hook path tracking control module includes two modes. One is the tracking control output of the hook from position A to position B, called path tracking control mode 1; the second is the tracking control output of the hook from position B to position A, called path tracking control mode 2. The paths of the two modes are the same but the movement directions are opposite. The two working modes are implemented differently in combination with the direction instructions of the relevant actions.

4. The three-dimensional path planning and tracking control system for a tower crane hook according to claim 3, characterized in that: The process of the hook path tracking control module is as follows: Step 1: Initialize each module; 1.1) Start the tower crane operation status detection module and the hook path tracking control module. At the same time, start the communication between the hook path tracking control module and the tower crane electrical control system; 1.2) The hook path tracking control module reads the parameters set by the hook 3D path planning module, including: 3D path position point p n (θ n ,r n ,h n ) data, the total number of tracking points in the planned path is n; 1.3) The hook path tracking control module reads the three-dimensional dynamic performance parameters of the tower crane obtained by the tower crane dynamic performance detection module, including: slewing speed control thresholds θ1, θ2, θ3, luffing speed control thresholds r1, r2, r3, and hook lifting speed control thresholds h1, h2, h3, h4, and h5; 1.4) Set the hook path tracking spatial position error ε in the hook path tracking control module and update the foresight distance S of the tracking point dl ; Step 2: Determine the path tracking control mode; The hook path tracking control module obtains the current position D(θ t , r t , h t ), calculate the spatial distance |Dp1| between the current position and the point p1 of the three-dimensional path planning, as well as the spatial distance between the current position and p n The spatial distance of the points |Dp2|; If |Dp1|<|Dp2|, it is determined to belong to path tracking control mode 1, and the tracking target point is set to p n ; Otherwise, it is determined to belong to path tracking control mode 2, and the tracking target point is set to p1; Step 3: Get the current position of the hook; The hook 3D path planning module uses three sensors installed on the tower crane to obtain the 3D coordinate value D (θ t , r t , h t ); Step 4: Calculate the position deviation of the hook's three-dimensional path tracking; Let P k (θ k , r k , h k ) is the three-dimensional coordinate of the current tracking point. During the hook path tracking control process, the position deviation between the current position of the tower crane hook and the current tracking point is set as: angle deviation Δθ, amplitude deviation Δr and height deviation Δh, which are expressed as follows: Δθ=θ k -θ t , Δr=r k -r t , Δh=h k -h t ; Step 5: Dynamics-based 3D path tracking control planning of the tower crane hook; Step 6: Hook tracking control output; The hook path tracking control module sends the tower crane movement speed and direction instructions planned in step 5 to the tower crane electrical control system. The tower crane electrical control system then controls the relevant drive equipment of the tower crane's three-dimensional movement to accurately complete the three-dimensional movement states of the boom rotation, trolley luffing, and hook lifting; Step 7: Update the points to be tracked; Step 8: Determine whether the path tracking is completed; Calculate the current position D(θ t , r t , h t ) and the spatial distance DP from the path tracking target point, The current state is path tracking mode 1, and the spatial distance DP expression is: The current state is path tracking mode 2, and the spatial distance DP expression is: If |DP|<ε, where ε is the error value, it means that the hook has reached the target position, the path tracking is completed, and the hook path tracking control process is exited; Otherwise, return to step 3 to continue tracking.

5. The three-dimensional path planning and tracking control system for a tower crane hook according to claim 4, characterized in that: In step 5, the specific process is: The tower crane path tracking control planning is to determine the current angular velocity ω of the rotary motion θ , trolley variable amplitude motion speed v r , hook lifting speed v h At the same time, add a positive or negative sign prefix before each velocity value to clarify the positive and negative directions of the three-dimensional motion. The specific instructions are as follows: 5.1) The slewing motion planning of the tower crane boom is set as follows: The angular velocity of the tower crane arm rotation motion is set to three gears, which are denoted as ω θi , i=1,2,3, where ω θ3 As the highest gear; during the path tracking process, the direction of the slewing motion of the tower crane arm is determined by the variable F recorded during the path planning process. θ The angular velocity gear setting of rotary motion has the following situations: If |Δθ|<θ1, set the current rotational motion angular velocity ω θ =0; If θ1 < |Δθ| < θ2, set the current angular velocity ω of the rotational motion θ = ω θ1 ; If θ2 < |Δθ| < θ3, set the current rotational angular velocity ω θ = ω θ2 ; If θ3<|Δθ|, set the current rotational motion angular velocity ω θ =ω θ3 ; Among them, θ1, θ2, and θ3 are the rotation speed control thresholds corresponding to the three rotational motion angular velocity gears respectively; 5.2) The tower crane trolley luffing motion planning is set as follows: The tower crane trolley variable motion is set to three gears, which are respectively denoted as v ri , i=1,2,3, where v r3 As the highest gear, if the amplitude deviation Δr>0, the tower crane trolley changes the amplitude outward and the movement speed is positive; otherwise, the trolley changes the amplitude inward and the movement speed is negative; If Δr≤r1, set the current trolley amplitude movement speed v r =0, the trolley’s luffing motion stops; If r1<Δr≤r2, set the current trolley amplitude movement speed v r =v r1 ; If r2<Δr≤r3, set the current trolley amplitude movement speed v r =v r2 ; If r3<Δr, set the current trolley amplitude movement speed v r =v r3 ; Among them, r1, r2, and r3 are the amplitude deviation thresholds corresponding to the luffing speed gears of the tower crane trolley respectively; 5.3) The lifting motion planning of the tower crane hook is set as follows: The speed of the hook lifting movement is set to five gears, which are respectively recorded as v hi , i=1,2,3,4,5, where v h5 As the highest gear; if the height deviation Δh>0, the tower crane will lift the hook upwards, and the hook lifting speed is positive; otherwise, the tower crane will drop the hook downwards, and the hook lifting speed is negative; If h5<|Δh|, set the current hook lifting speed v h =v h5 ; If h4 < |Δh| < h5, set the current hook hoisting speed v h = v h4 ; If h3 < |Δh| < h4, set the current lifting speed v of the hook h = v h3 ; If h2 < |Δh| < h3, set the current hook hoisting speed v h = v h2 ; If h1 < |Δh| < h2, set the current hook lifting speed v h = v h1 ; If |Δh| h =0;​ Among them, h1, h2, h3, h4, and h5 are the height deviation thresholds corresponding to the lifting speed gears of the tower crane.

6. The three-dimensional path planning and tracking control system for a tower crane hook according to claim 4, characterized in that: In step 7, the specific process is: The tower crane updates the tracking point by using a larger forward sight distance S dl To determine; suppose the total number of path points recorded in the planning process is n, and the current position coordinates of the hook are D(θ t , r t , h t ), the current tracking point is p k (θ k ,r k ,h k ), then the update method of the point to be tracked is as follows: 7.1) In path tracking control mode 1, if the current tracking point p k (θ k ,r k ,h k ) is the path tracking target point p n (θ n ,r n ,h n ), then the point to be tracked is still p n (θ n ,r n ,h n ); otherwise calculate the point p separately k+1 (θ k+1 ,r k+1 ,h k+1 ),…,p n (θ n ,r n ,h n ) and the current position of the hook D(θ t , r t , h t ) distance S i (i=k+1,…n), the distance calculation formula is as follows: Set the distance S i Center and front sight distance S dl The distance point with the smallest error is identified as the point to be tracked; 7.2) In path tracking control mode 2, if the current tracking point p k (θ k ,r k ,h k ) is the path tracking target point p1(θ1,r1,h1), then the point to be tracked is still p1(θ1,r1,h1); otherwise, calculate the point p k-1 (θ k-1 ,r k-1 ,h k-1 ),…,p1(θ1,r1,h n ) and the current position of the hook D(θ t , r t , h t ) distance S i (i=1,…k-1), the distance is calculated as follows:

7. A three-dimensional path planning and tracking control method for a tower crane hook, which relies on the three-dimensional path planning and tracking control system for a tower crane hook according to any one of claims 1 to 4, and is characterized in that: The implementation process is as follows: When the crane operator performs an actual lifting operation, the crane operation status detection module uses three sensors to obtain the hook position information in real time; The tower crane dynamics performance detection module uses the hook position information to detect the dynamics performance of the tower crane's three-dimensional motion and obtain the speed control thresholds for rotation, luffing, and hook lifting; The hook 3D path planning module uses the 3D coordinate values ​​of the hook to record and store the 3D coordinates of all points in the path, completing the path planning of the hook between two points; The hook path tracking control module uses the tower crane's dynamic performance parameters and the three-dimensional coordinates of all hooks on the path to send control instructions to the tower crane's electrical control system according to the hook path tracking process to complete the hook path tracking control.

Images