Arch dam cable machine real-time obstacle avoidance trajectory smooth transition planning method

Abstract

The present application belongs to the field of hydraulic engineering, and relates to a real-time obstacle avoidance trajectory smooth transition planning method for an arch dam cable crane, comprising: 1) initializing the working initialization parameters of the arch dam cable crane; 2) constructing a two-dimensional grid environment map; 3) obtaining path nodes based on the two-dimensional grid environment map for simple geometric obstacle avoidance path planning; 4) optimizing the simple geometric obstacle avoidance path planning to generate a trajectory cluster; 5) screening the trajectory cluster to form an optimized trajectory; 6) executing the optimized trajectory and updating the two-dimensional grid environment map in combination with the collision risk in the process; and 7) repeating step 3) to construct a real-time obstacle avoidance trajectory smooth curve in combination with the updated two-dimensional grid environment map. The present application provides a real-time obstacle avoidance trajectory smooth transition planning method for an arch dam cable crane, which can provide theoretical research and engineering application support for dynamic simulation and pre-rehearsal of the cable crane hoisting operation process, real-time obstacle avoidance trajectory result output, and safe and efficient construction guidance for the construction process.

Description

TECHNICAL FIELD

[0001] The present application belongs to the field of hydraulic engineering, and relates to a real-time obstacle avoidance trajectory planning method for an arch dam cable crane, in particular to a real-time obstacle avoidance trajectory smooth transition planning method for an arch dam cable crane. BACKGROUND

[0002] The arch dam is often built in a steep valley area. In the arch dam construction process, the cable crane is widely used in the construction of high arch dams due to its flexible arrangement, long service period, high efficiency, wide control range and other characteristics. However, in the valley area with strong wind, the cable crane may swing violently during the lifting process. At the same time, with the advancement of the arch dam concrete layering and block pouring operation, the cable crane is prone to collision with the surrounding working environment due to the influence of the movement characteristics of the cable crane itself and the dynamic changes of the construction scene. The collision can be avoided by emergency braking, but it will also reduce the working efficiency of the cable crane. Relying solely on the experience of the cable crane operator is inefficient and dangerous. The timely control of the position and speed of the cable crane through the positioning monitoring system developed by the global positioning system can effectively reduce accidents. However, the cable crane often faces dynamic and static obstacles during the lifting process. The strategy of emergency stop and instantaneous change of the speed of the trolley and the lifting rope for obstacle avoidance may cause the planned obstacle avoidance trajectory to be difficult to simultaneously consider safety, efficiency and smoothness. SUMMARY

[0003] In order to solve the above technical problems in the background art, the present application provides a real-time obstacle avoidance trajectory smooth transition planning method for an arch dam cable crane, which can provide theoretical research and engineering application support for dynamic simulation and pre-rehearsal of the cable crane lifting operation process, real-time obstacle avoidance trajectory result output, and safe and efficient construction guidance during the construction process.

[0004] In order to achieve the above purpose, the present application adopts the following technical solutions:

[0005] A real-time obstacle avoidance trajectory smooth transition planning method for an arch dam cable crane, characterized in that the real-time obstacle avoidance trajectory smooth transition planning method for an arch dam cable crane comprises the following steps:

[0006] 1) initializing the working parameters of the arch dam cable crane;

[0007] 2) constructing a two-dimensional grid environment map;

[0008] 3) performing simple geometric obstacle avoidance path planning based on the two-dimensional grid environment map constructed in step 2) to obtain path nodes;

[0009] 4) optimizing the simple geometric obstacle avoidance path planning obtained in step 3) and simultaneously generating a trajectory cluster;

[0010] 5) screening the trajectory cluster generated in step 4) and forming an optimal trajectory;

[0011] 6) Execute the preferred trajectory of step 5), update the two-dimensional grid environment map obtained in step 2) in combination with the in-process collision risk;

[0012] 7) Repeat step 3) to construct a real-time obstacle avoidance trajectory smooth curve in combination with the two-dimensional grid environment map updated in step 6).

[0013] Preferably, the initialization parameters in step 1) include the position of the starting point and the position of the ending point, the initial speed and acceleration, the time parameter, the constraint condition, the maximum horizontal and vertical lifting speed of the cable crane, the minimum safety distance of the cable crane from the slope, the dam body and the auxiliary facilities, the minimum safety distance of the cable crane from the tower crane and the warehouse pouring machine, the height distance of the heavy tank lifting endpoint and the empty tank falling starting point from the platform, the heavy tank alignment starting point height, the trajectory planning threshold σ and the collision detection time period and the prediction time step;

[0014] The time parameter includes the maximum expected time to complete the journey, the minimum expected time and the minimum unit of the expected completion time interval;

[0015] The constraint condition includes the running speed interval of the trolley, the lifting speed interval of the cable, the running acceleration interval of the trolley and the lifting acceleration interval of the cable, and the maximum running speed of the trolley and the cable lifting;

[0016] The σ is the minimum angle threshold of the angle formed by the line segment connecting the current point and the adjacent points before and after the current point in the planned trajectory node sequence; and the is the minimum angle threshold of the difference between the included angle formed by the current node and the previous planned path and the included angle formed by the subsequent node and the path in the trajectory planning node sequence.

[0017] Preferably, step 2) is specifically: the two-dimensional grid environment map is a map that divides the environment into a planar square region or a square grid, which is the spatial domain of the cable crane lifting concrete; the two-dimensional grid environment map includes a passable region and an obstacle region; the obstacle region is determined according to the motion state of the obstacle, and the obstacle region includes a fixed obstacle region and a non-fixed obstacle region;

[0018] The specific construction method of the two-dimensional grid environment map is: through the front view of the arch dam orientation, set the boundary position coordinates of the arch dam, and establish a planar square grid map; express the obstacle region in the form of a grid, and accurately determine the boundary position and size of the obstacle region;

[0019] The boundary position coordinates of the arch dam are taken as the main tower as the origin, the horizontal transportation direction of the cable machine main tower to the auxiliary tower as the X axis, the horizontal upward direction of the arch dam elevation as the Z axis, and the downstream direction of the river as the Y axis. The position and size of the obstacle region boundary, i.e. the size of the volume box of the concrete arch dam block, are the size parameters for describing the spatial volume size of the volume box of the dam block, including length, width, and height. Each grid represents a region on the map, and the state of each grid state is divided into occupied, idle, or unknown. Each grid state is updated according to the environmental data. Preferably, the specific way of updating is to determine whether there is an obstacle in the corresponding two-dimensional grid environment map according to the perception system, and if there is an obstacle, the grid state is updated.

[0020] Preferably, the specific implementation of the step 3) is to determine the orientation of the arch dam, to plan the simplest geometric obstacle avoidance path in the passable region by avoiding the obstacle region of the two-dimensional grid environment map, to regard the cable machine tank as a two-dimensional plane moving point, and to output the result in the form of a two-dimensional grid environment map node. The obstacle region is the virtual collision boundary extended from the obstacle entity of the two-dimensional grid environment map according to the minimum safety distance initialized at the periphery of the position coordinate region of the two-dimensional grid environment map.

[0021] The path node is obtained in the following way: a1) geometric obstacle avoidance path planning, and a2) optimization of the trajectory path node.

[0022] Preferably, the specific implementation of a1) is to determine the starting point from the two-dimensional grid environment map, to determine the next node from the initial node according to the directed graph construction rule by measuring the generation value F(n) of the current node to the neighborhood nodes in 16 directions adjacent to the current node, and to obtain the geometric obstacle avoidance path node until the desired end point.

[0023] The expression of the generation value is F(n) = G(n) + H(n).

[0024] Wherein:

[0025] G(n) represents the actual cost from the starting node to the current node n.

[0026] H(n) represents the cost estimate value from the current node n to the target point.

[0027] The expression of G(n) is:

[0028] The expression of H(n) is:

[0029] Wherein:

[0030] (x n , y n ), (x start , y start ) and (x goal , y goal ) are the center coordinates of the starting point, the current position and the target point, respectively.

[0031] Preferably, the specific implementation of a2) is:

[0032] a2.1) Determine the redundant point from the path node obtained from the preliminary planned trajectory; if the three nodes in the adjacent front and rear sequence are collinear, perform the deletion operation to delete the middle node in the three points; if the minimum degree formed by the connecting line of the front sequence node and the middle node and the connecting line of the middle node is greater than the threshold value σ and less than 180°, and the connecting line of the front sequence node and the subsequent node has no intersection with the obstacle area, it is determined that the current point is a redundant point, and the deletion operation is performed to delete the middle node in the three points; after each execution of the above operation, the new node is re-traversed, and the path is updated until all redundant points are removed;

[0033] a2.2) If the difference between the angle formed by the current node and the front sequence planning path and the angle formed by the subsequent node and the path is greater than a predetermined threshold , it is determined that the node deviates too much from the path, and a new node needs to be added at the midpoint of the two node positions, and the new node is translated by ω grid units in the direction of the path normal towards the favorable path direction, i.e. in the direction of reducing by ω grid units, the new node is re-traversed, the path is updated after confirming that the path has no intersection with the impassable area; ω is automatically changed according to the distance from the impassable area.

[0034] Preferably, the specific implementation of step 4) is:

[0035] 4.1) The point-to-point trajectory velocity planning of the cable machine return composite motion stage is carried out based on the reference position of the hanging tank; the reference position of the hanging tank refers to the position center of the center of gravity of the hanging tank; each two inflection points in the optimized trajectory path node are fitted with a path-velocity smooth transition curve, and the combination function of the path-velocity smooth transition curve is:

[0036]

[0037] Wherein:

[0038] t is the expected value of the completion time of each segment of the motion;

[0039] The coefficients in the combination function of the trajectory smooth transition control curve are respectively:

[0040] a 10 = s0

[0041] a 11 = v s

[0042] a 12 = a s / 2

[0043]

[0044] a 21 = v m

[0045] a 30 = a 20 + a 21 h2

[0046] a 31 = v m

[0047] a 32 = 0

[0048]

[0049] wherein:

[0050] s i is the displacement curve of the i-th segment;

[0051] a ij is the j-th coefficient of the displacement curve of the i-th segment;

[0052] h i is the displacement time consumption of the i-th segment;

[0053] s0and s, v s and v e , a s and a e are the displacement, velocity and acceleration at the initial and final time of the whole curve, respectively;

[0054] v m is the set maximum running speed;

[0055] 4.2) According to the initial setting expected completion time interval, according to the minimum expected time unit, the obstacle avoidance trajectory cluster that meets the requirements is screened out; preferably, the specific implementation manner of the screening is: first, set the expected value of the hoisting time of each segment, then discretize the expected hoisting time of each segment, and generate an expected time orthogonal test combination, which is substituted into the parameterized trajectory equation to generate a trajectory cluster one by one, and finally, according to the trajectory constraint and performance evaluation index, one trajectory is selected for the cable crane to execute.

[0056] Preferably, the specific implementation manner of the step 5) is:

[0057] 5.1) In the generated trajectory cluster, a trajectory is preferred according to the trajectory constraints and the cost function for establishing the trajectory performance;

[0058] The trajectory cost function is:

[0059]

[0060] Wherein:

[0061] m is the trajectory segment number sequence;

[0062] h i is the corresponding time length;

[0063] di is the minimum safety distance between the cable crane and the obstacle;

[0064] D min is the minimum distance between the trajectory point and the obstacle;

[0065] ω is the weight coefficient, when d i -D min ≤0, ω1+ω2=1, and ω1>0, ω2>0; when d i -D min >0, i.e. the distance between the planned trajectory and the obstacle is less than the safety distance, ω1=ω2;

[0066] 5.2) Convert the motion parameters of the preferred trajectory obtained in step 5.1) into the motion parameters of the cable crane and the lifting mechanism:

[0067] 5.2.1) Plan the speed of the reference position of the ladle;

[0068] 5.2.2) Convert into the motion parameters of the cable crane and the lifting mechanism; let the speeds and accelerations of the trolley, the lifting rope, and the ladle be v c , v l , v b , and a c , a l , a b , respectively, and the relationship of the motion parameters of the cable crane trolley, the lifting rope, and the ladle is expressed as:

[0069]

[0070] v l = v bx (1-tanβ)

[0071] Wherein:

[0072] β is the trolley motion elevation angle, β∈[-α, α], α<<90°, when the planned ladle speed is smoothly transitioned, the speed change of the cable crane and the lifting mechanism is also smoothly transitioned;

[0073] 5.2.3) Take the motion parameters of the cable crane traction and lifting mechanism obtained in step 5.2.2) as the output of the preferred trajectory.

[0074] Preferably, the specific implementation of step 6) is:

[0075] After the preferred global trajectory is planned, a trajectory running event is executed, in which a detection period time is set to detect collision, and when there is a collision risk, the path is re-planned with the current point as the starting point, that is, the trajectory is planned to avoid obstacles in real time within the initialization setting prediction time step. Within the prediction time step, the identified obstacles are added to the two-dimensional grid environment map, and the sequence trajectory path point at the moment of warning is taken as the starting point to reconstruct the path-velocity smooth transition curve function;

[0076] The collision detection is to select the spatial coordinates when the crane tank collides with the construction entity, and to determine whether a collision occurs by fitting the position relationship between the bodies.

[0077] The specific implementation of the fitting is: the dam block and the stopped tower crane are fitted with a bounding box, the running tower crane is fitted with a cylinder, and the crane tank is fitted with a bounding sphere. Whether the bounding box and the bounding sphere collide is detected, and whether the distance from the sphere center to the nearest point of the bounding box is less than or equal to the sphere radius is checked. The minimum and maximum values of the bounding box on the x, y, and z axes are (x min , y min , z min ) and (x max , y max , z max ), the center of the bounding sphere is C(x c , y c , z c ), and the radius is r. The nearest point P(x P , y P , z P ) from the sphere center to the bounding box is calculated,

[0078] x P = max(x min , min(x c , x max ))

[0079] y P = max(y min , min(y c , y max ))

[0080] z P = max(z min , min(z c , zmax ))

[0081] Calculate the distance from the sphere center to the nearest point: If d≤r, the enclosing sphere and the enclosing box collide; otherwise, they do not collide; the enclosing sphere collides with the enclosing cylinder, and the unit vector u=(u x , u y , u z ) in the direction of the cylinder axis, a point P0(x0, y0, z0) on the axis, the bottom radius r cy1 , calculate the nearest point P1 of the sphere center to the axis, where v=(x c -x0, y c -y0, z c -z0), calculate the distance from the sphere center to the nearest point, d=||C-P1||, if d≤r+r cy1 , the enclosing sphere collides with the enclosing cylinder, otherwise, no collision occurs.

[0082] Preferably, the specific implementation of step 7) is:

[0083] With the starting point set after collision detection, step 3) is performed to obtain the nodes of the planned trajectory, and between two inflection points, a single polynomial is fitted, and the displacement curve equation is:

[0084] S=b0+b1t+b2t 2 +b3t 3 +b4t 4 +b5t 5

[0085] Where:

[0086] t is the expected value of each segment of the expected completion time; the coefficients of the polynomial are:

[0087]

[0088] In the above formula, when there is no intermediate node between the collision warning occurrence point and the target endpoint, the single polynomial in the above formula is used for real-time obstacle avoidance trajectory smoothing transition planning;

[0089] If the single polynomial does not meet the current trajectory planning scene requirements, i.e., there are intermediate nodes between the collision warning occurrence point and the target endpoint, two polynomials are used, and the displacement curve equation set is:

[0090]

[0091] The coefficients of the polynomial are:

[0092]

[0093] wherein:

[0094] s i is the displacement curve of the i-th segment;

[0095] b ij is the j-th coefficient of the i-th segment curve;

[0096] h i is the displacement time consumption of the i-th segment;

[0097] s0 and s, v s and v e , a s and a e are the displacement, velocity and acceleration at the beginning and end of the whole curve, respectively;

[0098] v m is the set maximum operating speed;

[0099] If more than two segments occur, they are decomposed into a combination of multiple 1-2 segment polynomials; continue step 3) to perform real-time obstacle avoidance, otherwise terminate the planning and continue to execute the original planning trajectory.

[0100] The advantages of the present application are:

[0101] The present application provides a real-time obstacle avoidance trajectory smooth transition planning method for arch dam cable machines, which comprises the following steps: (1) initializing parameters; (2) constructing a two-dimensional grid environment map; (3) performing the simplest geometric obstacle avoidance path planning to obtain path nodes; (4) performing orthogonal test to optimize the smooth transition trajectory and generate a trajectory cluster; (5) selecting the optimal trajectory from the generated trajectory cluster, converting the motion parameters of the preferred trajectory scheme into the motion parameters of the cable machine traction and lifting mechanism, and executing the cable machine hoisting trajectory; (6) estimating the collision risk in the process, updating the two-dimensional grid map; (7) repeating step (3) to construct a real-time obstacle avoidance trajectory smooth curve and perform real-time obstacle avoidance in the process. The present application is based on the fact that the running environment of the cable machine in arch dam construction is complex, the space is narrow, and there are many moving entities. At present, the research on safe hoisting trajectory of the cable machine is still in the initial exploration stage. In addition, compared with other fields, the running duration of the cable machine is long and the starting point and ending point of each cycle are different, and its motion characteristics are significantly different from other machines or unmanned vehicles. Today, real-time obstacle avoidance control measures generally use alarm when entering a dangerous area, stop or instantaneously change speed and angle when the safety distance is less than the safety distance. Although the stop mode can simply and effectively prevent the mutual collision between the moving entity and the obstacle, it reduces the work efficiency and increases the mechanical and electrical loss. Therefore, in the present application, the smoothness of the cable machine obstacle avoidance path and the smooth transition of the speed are fully guaranteed during real-time obstacle avoidance, and the safety, efficiency and smoothness requirements during the cable machine hoisting process are considered. BRIEF DESCRIPTION OF DRAWINGS

[0102] Figure 1 is an arch dam cable machine real-time obstacle avoidance trajectory smooth transition planning method flow chart;

[0103] Figure 2 is an arch dam two-dimensional grid environment map and the simplest geometric obstacle avoidance path planning;

[0104] Figure 3 is a cable machine motion parameter conversion diagram;

[0105] Figure 4 is a bounding box collision detection;

[0106] Figure 5 is a cable machine real-time obstacle avoidance trajectory planning schematic diagram;

[0107] Figure 6 is a sixteen-way diagram;

[0108] Figure 7 is a trajectory smooth transition control curve combination function curve diagram. DETAILED DESCRIPTION

[0109] The technical solutions provided by the present application will be described in detail below with reference to the accompanying drawings:

[0110] Referring to Figure 1 , the present application provides an arch dam cable machine real-time obstacle avoidance trajectory smooth transition planning method, which acquires spatial information such as construction cable machine position and running parameters through a real-time sensing system, outputs the parameter results of the planned trajectory through pre-planning and in-process planning, and provides theoretical research and engineering application support for the construction process. The method comprises the following steps:

[0111] Step 1: Set the initialization parameters, including the starting point and the ending point, the initial speed and acceleration, the maximum value of the expected time to complete the journey, the minimum value of the expected time and the minimum unit of the expected time interval; the constraint conditions include the running speed interval of the trolley, the speed interval of the cable lifting, the running acceleration interval of the trolley and the acceleration interval of the cable lifting, the maximum running speed of the trolley and the cable lifting, the constraint condition, the maximum lifting speed of the cable machine horizontally and vertically, the minimum safety distance between the cable machine and the tank and the dam body and its auxiliary facilities, the minimum safety distance between the cable machine and the tower crane and the warehouse pouring machine, the height distance between the heavy tank lifting endpoint and the empty tank falling tank starting point from the platform, and the heavy tank positioning starting point height; the trajectory planning threshold σ, the collision detection time period, the prediction time step, and the grid map resolution / pixel. Wherein, σ is the minimum angle threshold of the angle formed by the line segment connecting the current point and its adjacent points before and after in the planned trajectory node sequence; is the minimum angle threshold of the difference between the included angle of the current node and the previous planning path and the included angle of the subsequent node and the path in the trajectory planning node sequence.

[0112] Step 2: The two-dimensional grid environment map is a kind of map form that divides the environment into planar square regions (grids), and is also the spatial domain of the cable crane for hoisting concrete, including passable regions and obstacle regions; the obstacle regions include fixed obstacle regions and non-fixed obstacle regions according to the movement state of the obstacles; the boundary position coordinates of the arch dam are set through the front view of the arch dam orientation to establish a planar square grid map; the obstacle region is expressed in the form of a grid to accurately determine the boundary position size of the obstacle region; the boundary position coordinates of the arch dam are taken as the origin with the main tower, the horizontal transportation direction of the main tower to the auxiliary tower as the X axis, the horizontal upward direction of the arch dam elevation as the Z axis, and the downstream direction of the river as the Y axis; the size is the length, width and height size; the boundary position coordinates of the arch dam are set through the front view of the arch dam orientation to establish a planar square grid map; a series of grids are initialized, each grid representing a region on the map, and the state of each grid state is divided into "occupied", "idle" or "unknown"; the sensor is used to collect environmental data, and when the sensor data arrives, the relevant grid state will be updated according to the data; the obstacle region is expressed in the form of a grid to accurately determine the boundary position size of the obstacle region, and a virtual collision boundary is expanded around the obstacle region Figure 2

[0113] Step 3: In the passable region of the two-dimensional grid environment map that has been constructed, the cable crane tank is regarded as a two-dimensional planar moving point for the purpose of avoiding the obstacle region of the environment map, the simplest geometric obstacle avoidance path is planned, and the result is output in the form of two-dimensional grid map nodes; the obstacle region, according to the minimum safety distance set by the initialization, continues to expand the virtual collision boundary around the obstacle entity in the two-dimensional grid environment map at the position coordinate region of the obstacle region in the environment map;

[0114] The path node acquisition method is: a1) geometric obstacle avoidance path planning to obtain trajectory nodes; a2) optimization of trajectory path nodes. In method a1), the specific implementation of geometric obstacle avoidance path planning to obtain trajectory nodes is: starting from the starting point determined from the two-dimensional grid map, according to the directed graph construction rule; the directed graph construction rule includes: (1) there is at most a single directed edge between any two nodes; (2) any two directed edges do not intersect; (3) any two directed edges do not coincide or partially coincide; (4) there are at most 24 directed outgoing edges from a node; (5) the longest directed edge in the cross-row or cross-column of the scattered point queue from a node cannot exceed 3 rows or 3 columns; (6) from the starting node, when the directed edge is constructed to the termination node, the distance is close, and the priority is given to the closer one.

[0115] In the obstacle avoidance path node search process, 16-direction node search is adopted (such as Figure 6 ​The next node is determined based on the current node, the generation value F(n) of the 16 neighboring nodes of the current node to the current node (as shown in the figure), the generation value F(n) is F(n)=G(n)+H(n), wherein G(n) represents the actual cost from the starting node to the current node n, and H(n) represents the cost estimation value from the current node n to the target point; the Euclidean distance is used to calculate G(n) and H(n),

[0116]

[0117] (x n , y n ), (x start , y start ), (x goal , y goal ) are respectively the center coordinates of the starting point, the current position and the target point.

[0118] In mode a2), the obtained trajectory node is optimized, and a redundant point is determined; if three nodes in sequence are collinear, a deletion operation is performed to delete the middle node of the three nodes; if the minimum degree formed by the connecting line of the previous node and the middle node and the connecting line of the middle node is greater than a threshold σ and less than 180°, and the connecting line of the previous node and the subsequent node has no intersection with the obstacle area, it is determined that the current point is a redundant point, and a deletion operation is performed to delete the middle node of the three nodes. After each execution of the above operation, the new node is traversed again, and the path is updated until all redundant points are removed. Secondly, if the difference between the angle formed by the current node and the previous planned path and the angle formed by the subsequent node and the path is greater than a preset threshold , it is determined that the node deviates from the path too much, a new node is added at the midpoint of the two node positions, and the new node is translated by ω grid units in the direction of the path normal towards the favorable path direction, that is, the direction of reducing is translated by ω grid units, the new node is traversed again, the path is updated after confirming that the path has no intersection with the impassable area; ω is automatically changed according to the distance from the impassable area.

[0119] Step 4: orthogonal test is performed to optimize the smooth transition trajectory, and a specific implementation manner of generating a trajectory cluster is as follows: the point-to-point trajectory speed planning of the cable crane return composite motion stage is performed based on the reference position of the hanging tank (without considering the swinging of the hanging tank); the reference position of the hanging tank refers to the position center of the centroid of the hanging tank; the optimized trajectory path node is fitted with a path-velocity smooth transition curve, and the trajectory smooth transition control curve combination function is S1(t)=a 10 +a 11 t+a 12 t 2 +a 13 t 3 +a 14t 4 S2(t)=a 20 +a 21 t; S3(t)=a 30 +a 31 t+a 32 t 2 +a 33 t 3 +a 34 t 4 +a 35 t 5 Where: t is the expected value of the completion time for each segment of the expected completion time, see [reference]. Figure 7 ;a ij Let be the j-th coefficient of the i-th displacement curve. Based on the initialized start point, end point, velocity range, and acceleration range, an experimental scheme matrix is ​​constructed, generating a series of orthogonal experimental combinations, ultimately forming multiple curve functions with respect to time t. According to the state constraints at each time period, the coefficients are as follows:

[0120] a 10 =s0;a 11 =v s ;a 12 =a s / 2;

[0121]

[0122] a 21 =v m

[0123] a 30 =a 20 +a 21 h2;a 31 =v m ;a 32 =0

[0124]

[0125]

[0126] In the above formula, s i Let a be the displacement curve of the i-th segment. ij h is the j-th coefficient of the i-th displacement curve segment. i Let s0 and s, v be the time taken for the i-th displacement segment. s and v e a s and a e These represent the displacement, velocity, and acceleration at the beginning and end of the entire curve, respectively. mThe maximum running speed is set; then, based on the initial expected completion time interval, and according to the minimum expected time unit, the cluster of obstacle avoidance trajectories that meet the requirements are selected.

[0127] Step 5: From the generated trajectory family, select the best trajectory based on trajectory constraints and a cost function for evaluating trajectory performance; the trajectory cost function is, Where m is the trajectory segment number, h i Where d represents the corresponding duration, d is the minimum safe distance between the cable car and the obstacle, and D is the minimum distance between the trajectory point and the obstacle. ω is the weighting coefficient, which is applied when d... i -D min When d ≤ 0, ω1 + ω2 = 1, and ω1 > 0, ω2 > 0; when d i -D min When ω > 0, meaning the planned trajectory is less than the safe distance from the obstacle, ω1 = ω2, and a large value is assigned to them as a penalty. Regarding other constraints, the above formula should also satisfy maximum speed and acceleration constraints. When solving for the constraint indices, the time domain is discretized into N equal parts, and the constraint indices are solved at N+1 discrete points; the constraint conditions are as follows: |S ′ |≤v m ;|S ″ |≤a m In the formula, a m For the maximum acceleration limit, v m The maximum speed limit is set; finally, the motion parameters of the optimized trajectory scheme are converted into the motion parameters of the cable crane traction and hoisting mechanism; firstly, the velocity of the reference position of the hoisting tank (without considering the swing of the hoisting tank) is planned, and then the conversion into the motion parameters of the cable crane traction and hoisting mechanism is relatively convenient. Taking the cable crane system in a two-dimensional plane as an example, let the velocity and acceleration of the trolley, hoisting rope, and hoisting tank be v respectively. c v l v b and a c a l a b The relationship between the motion parameters of the cableway trolley, the hoisting rope, and the suspended tank can be expressed as:

[0128]

[0129] v l =v bx (1-tanβ)

[0130] In the above formula, β is the lift angle of the trolley, β∈[-α, α], α<<90°. When the planned speed of the hoisting tank transitions smoothly, the speed changes of the cable crane traction and hoisting mechanism also transition smoothly. Figure 3 ).

[0131] In the trajectory running event, the real-time collision warning system detects collision in the initial setting detection period (for example, 0.5s), and replans the path with the current point when there is a collision risk, that is, the planning trajectory obstacle avoidance of the smooth transition curve is performed within the initial setting prediction time step; the planning trajectory obstacle avoidance of the smooth transition curve is to avoid obstacles in real time when the real-time perception system issues an obstacle avoidance instruction; within the prediction time step, the identified obstacles are added to the two-dimensional grid map, and the sequence trajectory path point at the warning moment is taken as the starting point to reconstruct the path-velocity smooth transition curve function; the in-process collision risk assessment is performed by using the bounding box for detection, the crane tank is fitted with a self-size bounding sphere, and the bounding sphere radius is expanded according to the set safety distance and swing; the dynamic obstacle is fitted with a bounding box of different shapes according to the motion range amplitude, and the bounding box size is also expanded according to the safety distance; the dynamic obstacle is a dynamic object such as a tower crane in the construction site; the collision detection selects the spatial coordinates when the tank and the construction entity collide, and judges whether the collision occurs through the position relationship between the fitted bodies Figure 4 ). The tank is fitted with a bounding sphere, the dam block and the stopped tower crane are fitted with a cubic bounding box, and the running tower crane is fitted with a cylinder. It is detected whether the bounding box and the bounding sphere collide, and it is checked whether the distance from the sphere center to the nearest point of the bounding box is less than or equal to the sphere radius; the minimum and maximum values of the bounding box on the x, y and z axes are (x min , y min , z min ) and (x max , y max , z max ), the center of the bounding sphere is C(x c , y c , z c ), and the radius is r; the nearest point P(x P , y P , z P ) of the sphere center to the bounding box is calculated,

[0132] x P = max(x min , min(x c , x max ));

[0133] y P = max(y min , min(y c , y max ));

[0134] z P = max(z min , min(z c , z max))

[0135] Calculate the distance from the sphere center to the nearest point: If d≤r, the enclosing sphere and the enclosing box collide; otherwise, they do not collide; the enclosing sphere collides with the enclosing cylinder, and the unit vector u=(u x , u y , u z ) in the direction of the cylinder axis, a point P0(x0, y0, z0) on the axis, a bottom radius r cy1 , calculate the nearest point P1 of the sphere center to the axis, where v=(x c -x0, y c -y0, z c -z0), calculate the distance from the sphere center to the nearest point, d=||C-P1||, if d≤r+r cy1 , the enclosing sphere collides with the enclosing cylinder, otherwise no collision occurs. When real-time evaluation of collision risk exists, immediately re-plan the path, and plan the trajectory obstacle avoidance of the real-time smooth transition curve within the initialization setting prediction time step; add the identified obstacle to the two-dimensional grid map, update the related grid state, and take the future trajectory path point as the starting point to reconstruct the path-velocity smooth transition curve function.

[0136] Step 7: Repeat step 3 to optimize the node and obtain the real-time planning trajectory node. When there is no intermediate node between the collision warning occurrence point and the target endpoint, single polynomial real-time obstacle avoidance trajectory smoothing transition planning is performed. A single polynomial fitting is used, and the displacement curve equation formula is:

[0137] S=b0+b1t+b2t 2 +b3t 3 +b4t 4 +b5t 5

[0138] The coefficients of this polynomial are:

[0139]

[0140] If the constraint condition of the real-time trajectory does not satisfy single polynomial fitting, i.e., there is an intermediate node between the collision warning occurrence point and the target endpoint, two-section polynomials are used, and the displacement curve equation set is:

[0141]

[0142] The coefficients of this polynomial are:

[0143]

[0144] In the above formula, s iis the i-th segment displacement curve, b ij is the j-th coefficient of the i-th segment displacement curve, h i is the i-th segment displacement time, s0and s, v s and v e , a s and a e are the displacement, velocity, acceleration of the whole curve at the beginning and end, v m is the set maximum running speed; if more than two segments occur, it is decomposed into a combination of multiple 1-2 segment polynomials; finally, in the generated trajectory cluster, a trajectory is selected according to the trajectory constraints and the cost function for establishing the trajectory performance, the method is step 3; construct a real-time obstacle avoidance trajectory smooth curve, execute real-time obstacle avoidance in the process Figure 5 ).

Claims

1. A method for real-time obstacle avoidance trajectory smooth transition planning of an arch dam cable crane, characterized in that: The arch dam cable machine real-time obstacle avoidance trajectory smooth transition planning method comprises the following steps: 1) initializing the arch dam cable machine working parameters; 2) constructing a two-dimensional grid environment map; 3) performing simple geometric obstacle avoidance path planning based on the two-dimensional grid environment map obtained in step 2) to obtain path nodes; 4) optimizing the simple geometric obstacle avoidance path planning obtained in step 3) and simultaneously generating a trajectory cluster; the specific implementation of step 4) is as follows: 4.1) performing point-to-point trajectory speed planning for the cable machine return composite motion stage based on the hoist tank reference position; the hoist tank reference position refers to the position center of the hoist tank centroid; fitting each two inflection points in the optimized trajectory path node with a path-velocity smooth transition curve, and the path-velocity smooth transition curve combination function is as follows: wherein: t is the expected completion time of each segment of the expected completion time; the coefficients in the trajectory smooth transition control curve combination function are as follows: a 10 = s0 a 11 = v s a 12 = a s / 2 a 21 = v m a 30 = a 20 + a 21 h2 a 31 = v m a 32 ＝0 wherein: s i is the displacement curve for the i-th segment; a ij is the jth coefficient of the ith displacement curve; h i is the time taken for the i-th segment displacement; s0 and s, v s and v e , a s and a e are displacement, velocity, acceleration at the beginning and end of the whole curve, respectively v m is the set maximum operating speed; 4.2) selecting the obstacle avoidance trajectory cluster according to the initial setting of the expected completion time interval and the minimum expected time unit; the specific implementation of step 4.2) is as follows: first, setting the expected value of the hoisting time of each segment, then discretizing the expected hoisting time of each segment and generating an expected time orthogonal test combination, and then substituting the trajectory cluster into the parameterized trajectory equation one by one, and finally selecting an optimal trajectory according to the trajectory constraints and performance evaluation indexes for the cable machine to execute; 5) selecting the trajectory cluster generated in step 4) and forming an optimal trajectory; 6) executing the optimal trajectory of step 5) and updating the two-dimensional grid environment map obtained in step 2) in combination with the in-process collision risk; 7) repeating step 3) to construct a real-time obstacle avoidance trajectory smooth curve in combination with the two-dimensional grid environment map updated in step 6).

2. The real-time obstacle avoidance trajectory smooth transition planning method for an arch dam cable crane according to claim 1, characterized in that: The initialization parameters in the step 1) include the position of the starting point and the position of the ending point, initial speed and acceleration, time parameters, constraint conditions, maximum hoisting speed of the cable crane horizontally and vertically, minimum safety distance of the cable crane hoisting tank from the slope, dam body and auxiliary facilities, minimum safety distance of the cable crane from the tower crane, warehouse surface pouring machinery, height distance of the heavy tank lifting ending point and the empty tank falling starting point from the platform, heavy tank alignment starting point height, trajectory planning threshold σ and Collision detection time period and prediction time step; The time parameters include the maximum expected time, the minimum expected time, and the minimum unit of the expected completion time interval; The constraint conditions include the running speed interval of the trolley, the speed interval of the cable lifting, the running acceleration interval of the trolley, and the acceleration interval of the cable lifting, and the maximum running speed of the trolley and the cable lifting; The σ is a minimum angle threshold of an angle formed by a line connecting a current point and a point adjacent to the current point in a sequence of trajectory planning nodes; and the is a minimum angle threshold of a difference between an angle formed by a current node and a previous planning path and an angle formed by a subsequent node and a path in a sequence of trajectory planning nodes.

3. The real-time obstacle avoidance trajectory smooth transition planning method for an arch dam cable crane according to claim 2, characterized in that: Step 2) is specifically: the two-dimensional grid environment map is a map that divides the environment into a planar square region or a square grid, which is the spatial domain of the cable hoisting concrete; the two-dimensional grid environment map includes passable regions and obstacle regions; the obstacle regions are determined according to the motion state of the obstacles, and the obstacle regions include fixed obstacle regions and non-fixed obstacle regions; The specific construction method of the two-dimensional grid environment map is: setting the boundary position coordinates of the arch dam through the front view of the arch dam to establish a planar square grid map; expressing the obstacle regions in the form of a grid to accurately determine the boundary position and size of the obstacle regions. The boundary position coordinates of the arch dam are taken as the main tower as the origin, the horizontal transportation direction of the cable crane main tower to the auxiliary tower as the X axis, the horizontal upward direction of the arch dam elevation as the Z axis, and the downstream direction of the river as the Y axis. The position and size of the obstacle region boundary, i.e. the size of the volume box of the concrete arch dam block, is the size parameter for describing the spatial volume size of the block with a volume box, including length, width, and height. Each grid represents a region on the map, and the state of each grid state is divided into occupied, idle, or unknown. The state of each grid state is updated according to the environmental data.

4. The real-time obstacle avoidance trajectory smooth transition planning method for an arch dam cable crane according to claim 3, characterized in that: The specific way of updating is to determine whether there is an obstacle in the grid in the corresponding two-dimensional grid environment map according to the perception system's perception of the environment, and if there is an obstacle, the grid state is updated.

5. The real-time obstacle avoidance trajectory smooth transition planning method for an arch dam cable crane according to claim 3 or 4, characterized in that: The specific implementation of the step 3) is to determine the orientation of the arch dam, construct a two-dimensional grid environment map, avoid the obstacle region of the two-dimensional grid environment map in the passable region, regard the cable crane as a two-dimensional plane moving point, plan a simple geometric obstacle avoidance path, and output the result in the form of a two-dimensional grid environment map node. The obstacle region is a virtual collision boundary formed by continuously expanding the obstacle entity in the two-dimensional grid environment map according to the minimum safety distance set by the initialization outside the position coordinate region of the two-dimensional grid environment map. The path node is obtained in the following way: a1) geometric obstacle avoidance path planning to obtain the trajectory node; and a2) optimization of the trajectory path node.

6. The real-time obstacle avoidance trajectory smooth transition planning method for an arch dam cable crane according to claim 5, characterized in that: The specific implementation of a1) is to determine the starting point from the two-dimensional grid environment map, measure the generation value F(n) of the current node to the adjacent 16 nodes in the neighborhood of the current node from the initial node according to the directed graph construction rule, determine the next node until the desired terminal point, and obtain the geometric obstacle avoidance path node. The expression of the generation value is F(n) = G(n) + H(n). Wherein: G(n) represents the actual cost from the starting node to the current node n; H(n) represents the cost estimate value from the current node n to the target point. The expression of G(n) is: The expression of H(n) is: The specific implementation of a2) is as follows: (x n , y n ), (x start , y start ) and (x goal , y goal ) are the center coordinates of the grids where the start point, the current position and the target point are located, respectively.

7. The real-time obstacle avoidance trajectory smooth transition planning method for an arch dam cable crane according to claim 5, characterized in that: a2.1) determine the redundant point from the path node obtained from the preliminary planning; if the three nodes in sequence are collinear, perform a deletion operation to delete the middle node; if the minimum degree of the line connecting the front node and the middle node and the line connecting the middle node is greater than the threshold σ and less than 180°, and the line connecting the front node and the subsequent node has no intersection with the obstacle region, it is determined that the current point is a redundant point, and a deletion operation is performed to delete the middle node. After each execution of the above operation, the new node is traversed again, and the path is updated until all redundant points are removed. The specific implementation of step 5) is as follows: a2.2) If the difference between the angle formed by the current node and the preceding planned path and the angle formed by the subsequent node and the path is greater than a preset threshold then it is determined that the node deviates too much from the path and needs to be corrected, a new node is added at the midpoint of the two node positions, and the new node is translated by ω grid units in the direction of the path normal towards the favorable path direction, i.e. to reduce the angle The angle is reduced. The new node is re-traversed, the path is updated after confirming that the path has no intersection with the impassable area, and ω is automatically changed according to the distance from the impassable area.

8. The real-time obstacle avoidance trajectory smooth transition planning method for an arch dam cable crane according to claim 6 or 7, characterized in that: 5.1) in the generated trajectory cluster, a trajectory is selected according to the trajectory constraint and the cost function for evaluating the performance of the trajectory; The trajectory cost function is as follows: Wherein: m is the trajectory segment number; di is the minimum safety distance between the cable crane and the obstacle; h i is the corresponding duration; 5.2) convert the motion parameters of the selected trajectory in step 5.1) into the motion parameters of the cable crane traction and lifting mechanism: D min is the minimum distance of the trajectory point to the obstacle; ω is a weight coefficient, when d i -D min When d i -D min When d > 0, i.e., the distance between the planned trajectory and the obstacle is less than the safety distance, ω1= ω2. ​ 5.2.1) Plan the velocity of the ladle reference position; 5.2.2) Conversion into movement parameters of the cable machine traction and hoisting mechanism; let the speed and acceleration of the trolley, hoisting rope and suspended tank be v c , v l , v b , and a c , a l , a b , respectively; the relationship of the movement parameters of the cable machine trolley, hoisting rope and suspended tank is expressed by: v l = v bx (1 - tan β) Wherein: β is the trolley motion angle of ascent, β ∈ [-α, α], α << 90°, when the planned ladle velocity is smoothly transitioned, the speed change of the cable machine traction and lifting mechanism is also smoothly transitioned; 5.2.3) Take the motion parameters of the cable machine traction and lifting mechanism obtained in step 5.2.2) as the output of the preferred trajectory.

9. The real-time obstacle avoidance trajectory smooth transition planning method for an arch dam cable crane according to claim 8, characterized in that: The specific implementation of step 6) is: After planning the preferred global trajectory, execute the trajectory running event, and in the event, initialize the detection period time to perform collision detection, when there is a collision risk in the detection evaluation, immediately re-plan the path with the current point, that is, within the initialization setting prediction time step, plan the real-time smooth transition curve trajectory obstacle avoidance; Within the prediction time step, add the identified obstacles to the two-dimensional grid environment map, and take the pre-warning moment sequence trajectory path point as the starting point to reconstruct the path-velocity smooth transition curve function; The collision detection is to select the spatial coordinates when the ladle collides with the construction entity, and to judge whether a collision occurs by fitting the position relationship between the bodies; The specific implementation of the fitting is: fitting the dam block and the stopped tower crane with a cubic bounding box; fitting the running tower crane with a cylinder; fitting the hanging tank with a bounding sphere; detecting whether the bounding box and the bounding sphere collide, checking whether the nearest point distance from the sphere center to the bounding box is less than or equal to the sphere radius; the minimum and maximum values of the bounding box on the x, y, z axes are (x min , y min , z min ) and (x max , y max , z max ), the center of the bounding sphere is C(x c , y c , z c ), and the radius is r, the nearest point P(x P , y P , z P ) from the sphere center to the bounding box is calculated, x P = max(x min , min(x c , x max )) y P = max(y min , min(y c , y max )) z P = max(z min , min(z c , z max )) Compute the distance from the sphere center to the closest point: If d ≤ r, the enclosing sphere and the enclosing box collide; otherwise, they do not collide; the enclosing sphere collides with the enclosing cylinder, compute the unit vector u = (u x , u y , u z ) in the direction of the cylinder axis, a point P0(x0, y0, z0) on the axis, the bottom radius r cy1 , compute the closest point P1to the axis from the sphere center, where v = (x c -x0, y c -y0, z c -z0), compute the distance from the sphere center to the closest point, d = ||C - P1||, if d ≤ r + r cy1 , the enclosing sphere collides with the enclosing cylinder, otherwise, no collision occurs.

10. The real-time obstacle avoidance trajectory smooth transition planning method for an arch dam cable crane according to claim 9, characterized in that: The specific implementation of step 7) is: With the starting point set after collision detection, execute step 3) to obtain the nodes of the planned trajectory, and between two inflection points, fit a single polynomial, and the displacement curve equation formula is: S = b0+ b1t + b2t 2 + b3t 3 + b4t 4 + b5t 5 Wherein: t is the expected completion time of each segment of the expected value of the motion time; the coefficients of the polynomial are: In the above formula, when there is no intermediate node between the collision warning occurrence point and the target endpoint, the single polynomial in the above formula is used for real-time obstacle avoidance trajectory smooth transition planning; If a single polynomial does not meet the current trajectory planning scene requirements, that is, there are intermediate nodes between the collision warning occurrence point and the target endpoint, then a two-segment polynomial is used, and the displacement curve equation group is: The coefficients of the polynomial are: Wherein: s i is the i-th segment displacement curve; b ij is the jth coefficient of the ith curve segment; h i is the time taken for the i-th segment displacement; s0 and s, v s and v e , a s and a e are displacement, velocity, acceleration at the beginning and end of the whole curve, respectively v m is the set maximum operating speed; If there are more than two segments, they are decomposed into a combination of multiple 1-2 segment polynomials; continue step 3) to execute real-time obstacle avoidance, otherwise terminate the planning and continue to execute the original planned trajectory.

Images