Smooth transition planning method for real-time obstacle avoidance track of arch dam cable crane

Abstract

The invention belongs to the field of hydraulic engineering, and relates to an arch dam cable crane real-time obstacle avoidance trajectory smooth transition planning method, which comprises the following steps: 1) initializing working initialization parameters of an arch dam cable crane; 2) constructing a two-dimensional grid environment map; 3) performing simplest geometric obstacle avoidance path planning based on the two-dimensional grid environment map to obtain path nodes; 4) optimizing the simplest geometric obstacle avoidance path plan to generate a trajectory cluster; 5) screening the trajectory clusters to form a preferred trajectory; 6) executing the optimal trajectory, and updating the two-dimensional grid environment map in combination with the collision risk in the event; and 7) in combination with the updated two-dimensional grid environment map, repeatedly executing the step 3), and constructing a real-time obstacle avoidance track smooth curve. The invention provides the smooth transition planning method for the real-time obstacle avoidance track of the arch dam cable crane, which can provide theoretical research and engineering application support for dynamic simulation and beforehand rehearsal of a cable crane hoisting operation process, real-time obstacle avoidance track result output and safe and efficient construction guidance of a construction process.

Description

Technical Field

[0001] The invention belongs to the field of water conservancy engineering, and relates to a real-time obstacle avoidance trajectory planning method for an arch dam cable crane, and in particular to a real-time obstacle avoidance trajectory smooth transition planning method for an arch dam cable crane. Background Art

[0002] Arch dams are often built in canyon areas with steep terrain. During the construction of arch dams, cable cranes are widely used in the construction of high arch dams due to their flexible layout, long service life, high efficiency, and wide control range. However, in the canyon area, the working environment accompanied by strong winds can cause the cable crane hoisting tank to swing violently during the lifting process. At the same time, with the advancement of the arch dam concrete pouring operation, it is very easy to collide with the surrounding working environment due to the influence of the cable crane's own movement characteristics and the dynamic changes of the construction scene. Avoiding collisions through emergency braking will reduce the working efficiency of the cable crane. Relying solely on the experience of the cable crane operator is inefficient and dangerous. By adopting the positioning monitoring system developed by the global positioning system, timely controlling the position and speed of the cable crane can effectively reduce the occurrence of accidents. However, the cable crane is often faced with the threat of dynamic and static obstacles during the lifting process. The strategy of emergency stopping and instantaneous change of the speed of the trolley and the hoisting rope to avoid obstacles can easily cause the planned obstacle avoidance trajectory to be difficult to take into account safety, efficiency, and stability at the same time. Summary of the invention

[0003] In order to solve the above-mentioned technical problems existing in the background technology, the present invention 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 cable crane hoisting operations, real-time obstacle avoidance trajectory result output, and safe and efficient construction guidance during the construction process.

[0004] In order to achieve the above object, the present invention adopts the following technical solution:

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

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

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

[0008] 3) Based on the two-dimensional grid environment map constructed in step 2), the simplest geometric obstacle avoidance path planning is performed to obtain the path nodes;

[0009] 4) Optimizing the simplest geometric obstacle avoidance path planning obtained in step 3) and generating trajectory clusters;

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

[0011] 6) executing the optimal trajectory of step 5), and updating the two-dimensional grid environment map obtained in step 2) in combination with the collision risk during the event;

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

[0013] Preferably, the initialization parameters in step 1) include the positions of the starting point and the ending point, the initial speed and acceleration, the time parameters, the constraints, the maximum horizontal and vertical lifting speeds of the cable crane, the minimum safe distance between the cable crane hoisting tank and the slope, the dam body and the ancillary facilities, the minimum safe distance between the cable crane and the tower crane and the warehouse surface pouring machinery, the height distance between the end point of the heavy tank lifting and the starting point of the empty tank dropping from the platform, the height of the starting point of the heavy tank alignment, the trajectory planning threshold σ and Collision detection time period and prediction time step;

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

[0015] The constraints include the operating speed range of the trolley, the speed range of the cable lifting, the acceleration range of the trolley and the acceleration range of the cable lifting, and the maximum operating speed of the trolley and the cable crane cable lifting;

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

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

[0018] The specific construction method of the two-dimensional grid environment map is: by looking at the arch dam orientation, setting the boundary position coordinates of the arch dam, and establishing a plane square grid map; expressing the obstacle area in the form of a grid, and accurately determining the boundary position size of the obstacle area;

[0019] The boundary position coordinates of the arch dam are based on the main tower as the origin, the horizontal transportation direction from the main tower of the cable car to the auxiliary tower as the X-axis, the horizontal upward direction of the arch dam elevation as the Z-axis, and the direction along the river as the Y-axis; the position size of the obstacle area boundary, that is, the size of the volume box of the concrete arch dam block, is the size parameter of the dam block using the volume box to describe the size of the spatial volume, including length, width, and height; each grid represents an area on the map, and the state of each grid state is divided into occupied, idle or unknown; each grid state is updated according to environmental data, and preferably, the specific method of updating is to perceive whether there is an obstacle in the grid of the environment in its corresponding two-dimensional grid environment map based on the perception system, and if there is an obstacle, the grid state is updated.

[0020] Preferably, the specific implementation method of the simplest geometric obstacle avoidance path planning in step 3) is: by facing the arch dam orientation, according to the constructed two-dimensional grid environment map, in the passable area, avoiding the obstacle area of ​​the two-dimensional grid environment map, treating the cable crane bucket as a two-dimensional plane moving particle, planning the simplest geometric obstacle avoidance path, and outputting the result in the form of a two-dimensional grid environment map node; the obstacle area is the obstacle entity in the two-dimensional grid environment map according to the minimum safety distance set by initialization outside the position coordinate area of ​​the two-dimensional grid environment map, and expanding the virtual collision boundary;

[0021] The path nodes are obtained by: a1) geometric obstacle avoidance path planning to obtain trajectory nodes; a2) optimizing trajectory path nodes.

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

[0023] The expression of the cost value is: F(n)=G(n)+H(n);

[0024] in:

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

[0026] H(n) represents the estimated cost from the current node n to the target point;

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

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

[0029] in:

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

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

[0032] a2.1) Determine redundant points from the path nodes obtained from the preliminary planned trajectory; if the three adjacent nodes in the previous and next sequences are collinear, perform a deletion operation to delete the middle node among the three points; if the minimum degree formed by the line connecting the previous 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 previous node and the subsequent node has no intersection with the obstacle area, then determine that the current point is a redundant point, and perform a deletion operation to delete the middle node among the three points; after each execution of the above operations, re-traverse the new nodes and update the path until all redundant points are removed;

[0033] a2.2) If the difference between the angle between the current node and the previous planned path and the angle between the subsequent node and the path is greater than the preset threshold 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 along the path normal direction, toward the favorable path direction, by ω grid units, that is, to reduce The direction of the angle is translated by ω grid units, and the new node is traversed again. After confirming that the path has no intersection with the impassable area, the path is updated; ω changes flexibly depending on the distance from the impassable area.

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

[0035] 4.1) The point-to-point trajectory speed planning of the cable car round-trip compound motion stage is carried out based on the reference position of the hanging pot; the reference position of the hanging pot refers to the position center of the centroid of the hanging pot; a path-speed smooth transition curve is used to fit between every two inflection points in the optimized trajectory path node, and the path-speed smooth transition curve combination function is:

[0036]

[0037] in:

[0038] t is the expected value of the time required to complete each segment of the movement in the expected completion time;

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

[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] in:

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

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

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

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

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

[0055] 4.2) According to the initially set 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 method of the screening is: first, the expected value of the lifting time of each section is set, and then the expected lifting time of each section is discretized, and an orthogonal test combination of expected time is generated, which is substituted into the parameterized trajectory equation one by one to generate a trajectory cluster, and finally a trajectory is selected for cable crane execution according to the trajectory constraints and performance evaluation indicators.

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

[0057] 5.1) Select a trajectory from the generated trajectory clusters based on trajectory constraints and a cost function to evaluate trajectory performance;

[0058] The trajectory cost function is:

[0059]

[0060] in:

[0061] m is the trajectory segment number;

[0062] h i is the corresponding duration;

[0063] di is the minimum safe distance between the cable crane tank and obstacles;

[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, that is, when 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 optimal trajectory obtained in step 5.1) into the motion parameters of the cable traction and hoisting mechanism:

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

[0068] 5.2.2) Convert into the motion parameters of the cable traction and hoisting mechanism; let the speed and acceleration of the trolley, hoisting rope and hanging tank be v c 、v l 、v b and a c 、a l 、a b The relationship between the motion parameters of the cable car, hoisting rope and hanging tank is expressed as:

[0069]

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

[0071] in:

[0072] β is the lifting angle of the trolley, β∈[-α, α], α<<90°. When the planned hanging tank speed transitions smoothly, the speed changes of the cable traction and hoisting mechanism also transition smoothly.

[0073] 5.2.3) The motion parameters of the cable traction and hoisting mechanism obtained in step 5.2.2) are used as the output of the optimal trajectory.

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

[0075] After planning the optimal global trajectory, execute the trajectory running event. In the event, perform collision detection with the initial set detection cycle time. When the detection assessment shows that there is a collision risk, immediately re-plan the path with the current point, that is, perform real-time smooth transition curve planning trajectory obstacle avoidance within the initial set prediction time step; within the prediction time step, add the identified obstacles to the two-dimensional grid environment map, and use the sequence trajectory path point at the warning moment as the starting point to reconstruct the path-speed smooth transition curve function;

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

[0077] The specific implementation method of the fitting is: fit the dam block and the suspended tower crane with a cubic bounding box; fit the running tower crane with a cylinder; fit the hanging tank with a bounding sphere; detect whether the bounding box collides with the bounding sphere, and check whether the distance from the center of the sphere to the nearest point of the bounding box is less than or equal to the radius of the sphere; the minimum and maximum values ​​of the bounding box on the x, y, and z axes are respectively (x min ,y min , z min ) and (x max ,y max , z max ), the center of the enclosing sphere is C(x c ,y c , z c ), with a radius of r, calculate the closest point P(x) from the center of the sphere to the bounding box P ,y P , z P ),

[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 center of the sphere to the nearest point: If d≤r, the bounding sphere and the bounding box collide; otherwise, there is no collision; the bounding sphere collides with the bounding cylinder, and the unit vector u in the direction of the cylinder axis is calculated as (u x ,u y ,u z ), a point P0 (x0, y0, z0) on the axis, and the base radius r cy1 , calculate the closest point P1 from the center of the sphere to the axis, Where v = (x c -x0,y c -y0,z c -z0), calculate the distance from the center of the sphere to the nearest point, d = ||C-P1||, if d ≤ r+r cy1 , then 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, execute step 3) to obtain the nodes of the planned trajectory. Between the two inflection points, a single polynomial is fitted. The displacement curve equation formula is:

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

[0085] in:

[0086] t is the expected value of the time required to complete 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 end point, the real-time obstacle avoidance trajectory smooth transition planning of the single polynomial in the above formula is performed;

[0089] If a single polynomial does not meet the requirements of the current trajectory planning scenario, that is, there is an intermediate node between the collision warning occurrence point and the target end point, a two-segment polynomial is used, and the displacement curve equation group is:

[0090]

[0091] The coefficients of this polynomial are:

[0092]

[0093] in:

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

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

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

[0097] s0 and s, v s and v e 、a s and a e They 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 there are more than two segments, it is decomposed into a combination of multiple 1-2 segment polynomials; continue with step 3) to perform real-time obstacle avoidance during the process, otherwise terminate the planning and continue to execute the original planned trajectory.

[0100] The advantages of the present invention are:

[0101] The present invention provides a method for planning a smooth transition of a real-time obstacle avoidance trajectory for an arch dam cable crane, the method comprising: (1) initializing parameters; (2) constructing a two-dimensional grid environment map; (3) performing a minimal geometric obstacle avoidance path planning and obtaining path nodes; (4) performing an orthogonal test, optimizing a smooth transition trajectory, and generating a trajectory cluster; (5) selecting an optimal trajectory from the generated trajectory cluster, converting the motion parameters of the preferred trajectory scheme into motion parameters of the cable crane traction and hoisting mechanism, and executing the cable crane hoisting trajectory; (6) estimating the collision risk during the event and updating the two-dimensional grid map; (7) repeating step (3), constructing a real-time obstacle avoidance trajectory smooth curve, and executing real-time obstacle avoidance during the event. In view of the complex operating environment, narrow space, and many active entities of the cable crane during arch dam construction, the present invention is still in the initial exploration stage for the research on the safe hoisting trajectory of the cable crane; in addition, compared with other fields, the operation duration of the cable crane is long and the starting and ending points of each cycle are different, and its motion characteristics are significantly different from those of other mechanical or unmanned vehicles. Nowadays, real-time obstacle avoidance control measures generally adopt alarms when entering a dangerous area, stop when less than a safe distance, or instantaneously change speed and angle for control. Although the stopping method can simply and effectively prevent the collision between the moving entity and the obstacle, it reduces the working efficiency and increases the electromechanical loss. Therefore, when avoiding obstacles in real time, the present invention fully ensures that the obstacle avoidance path of the cable crane is smooth and the speed transition is smooth, taking into account the requirements of safety, efficiency and stability during the cable crane hoisting process. BRIEF DESCRIPTION OF THE DRAWINGS

[0102] Figure 1 It is a flow chart of the real-time obstacle avoidance trajectory smooth transition planning method for arch dam cable crane;

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

[0104] Figure 3 It is the conversion diagram of cable machine motion parameters;

[0105] Figure 4 It is the bounding box collision check;

[0106] Figure 5 This is a schematic diagram of the real-time obstacle avoidance trajectory planning of the cable car;

[0107] Figure 6 It is a sixteen-way graph;

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

[0109] The technical solution provided by the present invention is described in detail below in conjunction with the accompanying drawings:

[0110] See also Figure 1 The present invention provides a method for planning smooth transition of real-time obstacle avoidance trajectory of arch dam cable crane. Through the real-time perception system, spatial information such as the location and operation parameters of the construction cable crane is obtained. Through pre-planning and in-process planning, the parameter results of the planned trajectory are output to provide theoretical research and engineering application support for the construction process. The method includes the following steps:

[0111] Step 1: Set the initialization parameters, including the starting point and the end point, the initial speed and acceleration, the maximum expected time to complete the journey, the minimum expected time and the minimum unit of the expected completion time interval; the constraints include the trolley's running speed interval, the cable lifting speed interval, the trolley's running acceleration interval and the cable lifting acceleration interval, the maximum running speed of the trolley and the cable crane cable lifting, the constraints, the horizontal and vertical maximum lifting speed of the cable crane, the minimum safe distance between the cable crane hoisting tank and the slope, dam body and its ancillary facilities, the minimum safe distance between the cable crane and the tower crane and the warehouse surface casting machinery, the height distance between the heavy tank lifting end point and the empty tank dropping starting point and the platform, and the starting height of the heavy tank alignment; the trajectory planning threshold σ, Collision detection time period, prediction time step; grid map resolution / pixel. Among them, σ is the minimum angle threshold of the line segment connecting the current point and its adjacent points before and after in the planned trajectory node sequence; It is the minimum angle threshold of the difference between the angle between the current node and the previous planned path and the angle between the subsequent node and the path in the trajectory planning node sequence.

[0112] Step 2: A two-dimensional grid environment map is a map format that divides the environment into planar square areas (grids). It is also the spatial domain for cable crane concrete hoisting, including passable areas and obstacle areas. The obstacle area includes fixed obstacle areas and non-fixed obstacle areas according to the movement state of the obstacle. By looking directly at the arch dam orientation, the boundary position coordinates of the arch dam are set to establish a planar square grid map. The obstacle area is expressed in a grid form to accurately determine the boundary position size of the obstacle area. The boundary position coordinates of the arch dam are based on the main tower as the origin, the horizontal transportation direction from the cable crane main tower to the auxiliary tower as the X-axis, and the arch dam as the horizontal direction. The horizontal upward direction of the dam elevation is the Z axis, and the direction along the river is the Y axis; the dimensions are length, width, and height; by looking at the arch dam orientation, the boundary position coordinates of the arch dam are set to establish a plane square grid map; initialize a series of grids, each grid represents an area on the map, and the state of each grid is divided into "occupied", "free" or "unknown", and use sensors to collect environmental data. When sensor data arrives, the relevant grid state will be updated according to the data, and the obstacle area is expressed in the form of a grid to accurately determine the boundary position and size of the obstacle area, and expand the virtual collision boundary outside the obstacle area ( Figure 2 ).

[0113] Step 3: By looking squarely at the arch dam orientation, in the passable area of ​​the constructed two-dimensional grid environment map, with the purpose of avoiding the obstacle area of ​​the environment map, the cable crane bucket is regarded as a two-dimensional plane moving particle, and the simplest geometric obstacle avoidance path is planned, and the result is output in the form of a two-dimensional grid map node; the obstacle area, according to the minimum safety distance set by the initialization, continues to expand the virtual collision boundary in the obstacle entity of the two-dimensional grid environment map outside the coordinate area of ​​the environment map where it is located;

[0114] The path node acquisition method is: a1) geometric obstacle avoidance path planning, obtain trajectory nodes; a2) optimize trajectory path nodes. Among them, in method a1), the specific implementation method of geometric obstacle avoidance path planning and obtaining trajectory nodes is: starting from the two-dimensional grid map to determine the starting point, according to the directed graph construction rules; the directed graph construction rules include: (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 overlap or partially overlap; (4) starting from a node, there are at most 24 directed outgoing edges; (5) starting from a node, the longest directed edge in the scattered point queue cannot cross more than 3 rows or 3 columns; (6) starting from the starting node, when constructing a directed edge to the end node, if rules 1 to 5 are met, the one with the closest distance is given priority.

[0115] In the process of searching for nodes in the obstacle avoidance path, a 16-way node search is used (such as Figure 6As shown in the figure, based on the current node, the cost value F(n) from the current node to the 16 neighboring nodes of the current point is measured to determine the next node; the cost value F(n) is: F(n) = G(n) + H(n); where G(n) represents the actual cost from the starting node to the current node n, and H(n) represents the estimated cost from the current node n to the target point; Euclidean distance is used to calculate G(n) and H(n),

[0116]

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

[0118] In method a2), the acquired trajectory nodes are optimized to determine redundant points; if the three adjacent nodes in the previous and next sequences are collinear, a deletion operation is performed to delete the middle node among the three points; if the minimum degree formed by the line connecting the previous 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 previous node and the subsequent node has no intersection with the obstacle area, then the current point is determined to be a redundant point, and a deletion operation is performed to delete the middle node among the three points. After each execution of the above operation, the new nodes are traversed again and the path is updated until all redundant points are removed. Secondly, if the difference between the angle between the current node and the previous planned path and the angle between the subsequent node and the path is greater than the preset threshold 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 along the path normal direction, toward the favorable path direction, by ω grid units, that is, to reduce The direction of the angle is translated by ω grid units, and the new node is traversed again. After confirming that the path has no intersection with the impassable area, the path is updated; ω changes flexibly depending on the distance from the impassable area.

[0119] Step 4: Conduct orthogonal experiments to optimize the smooth transition trajectory and generate a specific implementation method of the trajectory cluster: use the reference position of the hanging pot (without considering the swing of the hanging pot) to perform point-to-point trajectory speed planning in the cable car round-trip compound motion stage; the hanging pot reference position refers to the position center of the hanging pot centroid; fit the optimized trajectory path node with a path-speed 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 time to complete each segment of the expected completion time, see Figure 7 ; a ij is the jth coefficient of the displacement curve of the i-th segment. The coefficients are constructed according to the initial starting point, end point, velocity range, and acceleration range. The test scheme matrix is ​​generated to generate a series of orthogonal experimental combinations, and finally multiple curve functions about time t are formed. According to the state constraints of each time period, the coefficients are:

[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 is the displacement curve of the i-th segment, a ij is the jth coefficient of the i-th displacement curve, h i is the displacement time of the i-th segment, 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 entire curve, respectively, v mis the set maximum running speed; then according to the initially set expected completion time interval and the minimum expected time unit, the eligible obstacle avoidance trajectory cluster is screened out.

[0127] Step 5: In the generated trajectory cluster, select a trajectory based on the trajectory constraints and the cost function for evaluating trajectory performance; the trajectory cost function is, Among them, m is the number of trajectory segments, h i is the corresponding duration, d is the minimum safe distance between the cable crane bucket and the obstacle, and D is the minimum distance between the trajectory point and the obstacle. ω is the weight coefficient. When d i -D min ≤0, ω1+ω2=1, and ω1>0, ω2>0; when d i -D min >0, that is, when the distance between the planned trajectory and the obstacle is less than the safe distance, ω1=ω2, and a large value is assigned to them as a penalty. In terms of other constraints, the above formula should also satisfy the maximum speed and acceleration constraints. When solving the constraint index, the time domain is discretized into N equal parts, and the constraint index is solved at N+1 discrete points; the constraint conditions are as follows: |S ′ |≤v m ;|S ″ |≤a m , where a m is the maximum acceleration limit, v m is the maximum speed limit; finally, the motion parameters of the optimal trajectory scheme are converted into the motion parameters of the cable traction and lifting mechanism; first, the speed of the base position of the hanging pot (without considering the swing of the hanging pot) is planned, and then it is relatively convenient to convert it into the motion parameters of the cable traction and lifting mechanism. Taking the cable system in a two-dimensional plane as an example, the speed and acceleration of the trolley, the lifting rope, and the hanging pot are v c 、v l 、v b and a c 、a l 、a b The relationship between the motion parameters of the cable car, hoisting rope and hanging tank can be expressed as:

[0128]

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

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

[0131] Step 6: In the execution trajectory operation event, the real-time collision warning system performs collision detection with the initial setting detection cycle time (taking 0.5s as an example). When the detection assessment shows that there is a collision risk, the path is immediately replanned with the current point, that is, the real-time smooth transition curve planning trajectory obstacle avoidance is performed within the initial setting prediction time step; the smooth transition curve planning trajectory obstacle avoidance is to perform real-time avoidance when encountering the real-time perception system to issue 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 used as the starting point to reconstruct the path-speed smooth transition curve function; the in-process collision risk assessment is detected by using a bounding box, the cable crane hanging pot is fitted with a bounding ball of its own size, and the radius of the bounding ball is expanded according to the set safety distance and swing, and the dynamic obstacle adopts a bounding box of different shapes according to the range of motion, and the bounding box size is also expanded at a safe distance; the dynamic obstacle is a tower crane and other objects with dynamically changing positions at the construction site; the collision detection selects the spatial coordinates when the collision occurs between the hanging pot and the construction entity, and determines whether a collision occurs by the position relationship between the fitting bodies ( Figure 4 ). The hanging tank is fitted with a bounding sphere, the dam block and the suspended tower crane are fitted with a cubic bounding box, and the running tower crane is fitted with a cylinder. Check whether the bounding box collides with the bounding sphere, and check whether the distance from the center of the sphere to the nearest point of the bounding box is less than or equal to the radius of the sphere; 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 enclosing sphere is C(x c ,y c , z c ), with a radius of r, calculate the closest point P(x) from the center of the sphere to the bounding box P ,y P , z P ),

[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 center of the sphere to the nearest point: If d≤r, the bounding sphere and the bounding box collide; otherwise, there is no collision; the bounding sphere collides with the bounding cylinder, and the unit vector u in the direction of the cylinder axis is calculated as (u x ,u y ,u z ), a point P0 (x0, y0, z0) on the axis, and the base radius r cy1 , calculate the closest point P1 from the center of the sphere to the axis, Where v = (x c -x0,y c -y0,z c -z0), calculate the distance from the center of the sphere to the nearest point, d = ||C-P1||, if d ≤ r+r cy1 , then the enclosing sphere collides with the enclosing cylinder, otherwise no collision occurs. When the real-time assessment shows that there is a risk of collision, the path is immediately replanned, and the planned trajectory of the real-time smooth transition curve is performed within the initial set prediction time step to avoid obstacles; the identified obstacles are added to the two-dimensional grid map, the relevant grid states are updated, and the path point of a certain time series trajectory in the future is used as the starting point to reconstruct the path-speed smooth transition curve function.

[0136] Step 7: Repeat step 3, optimize the nodes, and obtain the nodes of the real-time planning trajectory. When there is no intermediate node between the collision warning point and the target end point, a single polynomial real-time obstacle avoidance trajectory smooth transition planning is performed. With a single polynomial fitting, 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 constraints of the real-time trajectory do not satisfy the single polynomial fitting, that is, there is an intermediate node between the collision warning occurrence point and the target end point, then two polynomials are used, and the displacement curve equation group is,;

[0141]

[0142] The coefficients of this polynomial are:

[0143]

[0144] In the above formula, s iis the displacement curve of the i-th segment, b ij is the jth coefficient of the i-th displacement curve, h i is the displacement time of the i-th segment, 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 entire curve, respectively, v m is the set maximum running speed; if there are more than two segments, 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 evaluating trajectory performance, and the method is the same as step 3; a real-time obstacle avoidance trajectory smooth curve is constructed, and real-time obstacle avoidance is performed during the process ( Figure 5 ).

Claims

1. A method for real-time obstacle avoidance trajectory smooth transition planning for an arch dam cable car, characterized in that: The arch dam cable crane real-time obstacle avoidance trajectory smooth transition planning method comprises the following steps: 1) Initialize the working parameters of the arch dam cable crane; 2) Construct a two-dimensional grid environment map; 3) Based on the two-dimensional grid environment map constructed in step 2), the simplest geometric obstacle avoidance path planning is performed to obtain the path nodes; 4) Optimizing the simplest geometric obstacle avoidance path planning obtained in step 3) and generating trajectory clusters; 5) screening the trajectory clusters generated in step 4) and forming the 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 collision risk during the event; 7) Combined with the two-dimensional grid environment map updated in step 6), repeat step 3) to construct a real-time obstacle avoidance trajectory smooth curve.

2. The arch dam cable crane real-time obstacle avoidance trajectory smooth transition planning method according to claim 1 is characterized by: The initialization parameters in step 1) include the positions of the starting point and the ending point, the initial speed and acceleration, the time parameters, the constraints, the maximum horizontal and vertical lifting speeds of the cable crane, the minimum safe distance between the cable crane and the slope, the dam body and the ancillary facilities, the minimum safe distance between the cable crane and the tower crane and the warehouse surface casting machinery, the height distance between the end point of the heavy tank lifting and the starting point of the empty tank dropping from the platform, the height of the heavy tank alignment starting point, the trajectory planning threshold σ and Collision detection time period and prediction time step; The time parameters include the maximum expected time to complete the journey, the minimum expected time and the minimum unit of the expected completion time interval; The constraints include the operating speed range of the trolley, the speed range of the cable lifting, the acceleration range of the trolley and the acceleration range of the cable lifting, and the maximum operating speed of the trolley and the cable crane cable lifting; The σ is the minimum angle threshold of the angle between the current point and the line segments connecting the adjacent points before and after the current point in the planned trajectory node sequence; It is the minimum angle threshold of the difference between the angle between the current node and the previous planned path and the angle between the subsequent node and the path in the trajectory planning node sequence.

3. The arch dam cable crane real-time obstacle avoidance trajectory smooth transition planning method according to claim 2 is characterized by: The step 2) specifically includes: the two-dimensional grid environment map is a map that divides the environment into planar square areas or square grids, which is the spatial domain of the cable crane lifting concrete; the two-dimensional grid environment map includes a passable area and an obstacle area; the obstacle area is determined according to the motion state of the obstacle, and the obstacle area includes a fixed obstacle area and a non-fixed obstacle area; The specific construction method of the two-dimensional grid environment map is: by looking at the arch dam orientation, setting the boundary position coordinates of the arch dam, and establishing a plane square grid map; expressing the obstacle area in the form of a grid, and accurately determining the boundary position size of the obstacle area; The boundary position coordinates of the arch dam are based on the main tower as the origin, the horizontal transportation direction from the main tower of the cable car to the auxiliary tower as the X-axis, the horizontal upward direction of the arch dam elevation as the Z-axis, and the direction along the river as the Y-axis; the position size of the obstacle area boundary, that is, the size of the volume box of the concrete arch dam block, is the size parameter of the dam block using the volume box to describe the size of the spatial volume, including length, width, and height; each grid represents an area on the map, and the state of each grid state is divided into occupied, idle or unknown; each grid state is updated according to environmental data, and preferably, the specific method of updating is to perceive whether there is an obstacle in the grid of the environment in its corresponding two-dimensional grid environment map based on the perception system, and if there is an obstacle, the grid state is updated.

4. The arch dam cable crane real-time obstacle avoidance trajectory smooth transition planning method according to claim 3 is characterized by: The specific implementation method of the simplest geometric obstacle avoidance path planning in step 3) is: by facing the arch dam orientation, according to the constructed two-dimensional grid environment map, in the passable area, avoiding the obstacle area of ​​the two-dimensional grid environment map, treating the cable crane bucket as a two-dimensional plane moving particle, planning the simplest geometric obstacle avoidance path, and outputting the result in the form of a two-dimensional grid environment map node; the obstacle area is the obstacle entity in the two-dimensional grid environment map according to the minimum safety distance set by initialization outside the position coordinate area of ​​the two-dimensional grid environment map, and expanding the virtual collision boundary; The path nodes are obtained by: a1) geometric obstacle avoidance path planning to obtain trajectory nodes; a2) optimizing trajectory path nodes.

5. The arch dam cable crane real-time obstacle avoidance trajectory smooth transition planning method according to claim 4 is characterized by: The specific implementation method of a1) is as follows: starting from the two-dimensional grid environment map, determining the starting point, and measuring the cost value F(n) from the current node to the neighboring nodes in 16 directions adjacent to the current node from the initial node, until the desired end point, and obtaining the geometric obstacle avoidance path node; The expression of the cost value is: F(n)=G(n)+H(n); in: G(n) represents the actual cost from the starting node to the current node n; H(n) represents the estimated cost from the current node n to the target point; The expression of G(n) is: The expression of H(n) is: in: (x n ,y n )、(x start ,y start ) and (x goal ,y goal ) are the center coordinates of the grid where the starting point, current position and target point are located, respectively.

6. The arch dam cable crane real-time obstacle avoidance trajectory smooth transition planning method according to claim 4, characterized in that: The specific implementation of a2) is as follows: a2.1) Determine redundant points from the path nodes obtained from the preliminary planned trajectory; if the three adjacent nodes in the previous and next sequences are collinear, perform a deletion operation to delete the middle node among the three points; if the minimum degree formed by the line connecting the previous 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 previous node and the subsequent node has no intersection with the obstacle area, then determine that the current point is a redundant point, and perform a deletion operation to delete the middle node among the three points; after each execution of the above operations, re-traverse the new nodes and update the path until all redundant points are removed; a2.2) If the difference between the angle between the current node and the previous planned path and the angle between the subsequent node and the path is greater than the preset threshold 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 along the path normal direction, toward the favorable path direction, by ω grid units, that is, to reduce The direction of the angle is translated by ω grid units, and the new node is traversed again. After confirming that the path has no intersection with the impassable area, the path is updated; ω changes flexibly depending on the distance from the impassable area.

7. The arch dam cable crane real-time obstacle avoidance trajectory smooth transition planning method according to claim 5 or 6, characterized in that: The specific implementation of step 4) is as follows: 4.1) The point-to-point trajectory speed planning of the cable car round-trip compound motion stage is carried out based on the reference position of the hanging pot; the reference position of the hanging pot refers to the position center of the centroid of the hanging pot; a path-speed smooth transition curve is used to fit between every two inflection points in the optimized trajectory path node, and the path-speed smooth transition curve combination function is: in: t is the expected value of the time required to complete each segment of the movement in the expected completion time; The coefficients in the trajectory smooth transition control curve combination function are: a 10 =s0 and 11 =in s a 12 =a s / 2 and 21 =in m a 30 =a 20 +a 21 h2 and 31 =in m a 32 ＝0 in: s i is the displacement curve of the i-th segment; a ij is the jth coefficient of the i-th displacement curve; h i is the displacement time of the i-th segment; s0 and s, v s and v e 、a s and a e They are the displacement, velocity and acceleration at the beginning and end of the whole curve respectively; v m is the set maximum operating speed; 4.2) According to the initially set 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 method of the screening is: first, the expected value of the lifting time of each section is set, and then the expected lifting time of each section is discretized, and an orthogonal test combination of expected time is generated, which is substituted into the parameterized trajectory equation one by one to generate a trajectory cluster, and finally a trajectory is selected for cable crane execution according to the trajectory constraints and performance evaluation indicators.

8. The arch dam cable crane real-time obstacle avoidance trajectory smooth transition planning method according to claim 7, characterized in that: The specific implementation of step 5) is as follows: 5.1) Select a trajectory from the generated trajectory clusters based on trajectory constraints and a cost function to evaluate trajectory performance; The trajectory cost function is: in: m is the trajectory segment number; h i is the corresponding duration; di is the minimum safe distance between the cable crane tank and obstacles; D min is the minimum distance between the trajectory point and the obstacle; ω is the weight coefficient, when d i -D min ≤0, ω1+ω2=1, and ω1>0, ω2>0; when d i -D min >0, that is, when the distance between the planned trajectory and the obstacle is less than the safety distance, ω1=ω2; 5.2) Convert the motion parameters of the optimal trajectory obtained in step 5.1) into the motion parameters of the cable traction and hoisting mechanism: 5.2.1) Plan the speed of the hanging tank reference position; 5.2.2) Convert into the motion parameters of the cable traction and hoisting mechanism; let the speed and acceleration of the trolley, hoisting rope and hanging tank be v c 、v l 、v b and a c 、a l 、a b The relationship between the motion parameters of the cable car, hoisting rope and hanging tank is expressed as: v l =v bx (1-tanβ) in: β is the lifting angle of the trolley, β∈[-α, α], α<<90°. When the planned hanging tank speed transitions smoothly, the speed changes of the cable traction and hoisting mechanism also transition smoothly. 5.2.3) The motion parameters of the cable traction and hoisting mechanism obtained in step 5.2.2) are used as the output of the optimal trajectory.

9. The arch dam cable crane real-time obstacle avoidance trajectory smooth transition planning method according to claim 8, characterized in that: The specific implementation of step 6) is as follows: After planning the optimal global trajectory, execute the trajectory running event. In the event, perform collision detection with the initial set detection cycle time. When the detection assessment shows that there is a collision risk, immediately re-plan the path with the current point, that is, perform real-time smooth transition curve planning trajectory obstacle avoidance within the initial set prediction time step; Within the prediction time step, the identified obstacles are added to the two-dimensional grid environment map, and the path point of the sequence trajectory at the warning moment is used as the starting point to reconstruct the path-speed smooth transition curve function; The collision detection is to select the spatial coordinates when the hanging tank collides with the construction entity, and determine whether a collision occurs by the positional relationship between the fitting bodies; The specific implementation method of the fitting is: fit the dam block and the suspended tower crane with a cubic bounding box; fit the running tower crane with a cylinder; fit the hanging tank with a bounding sphere; detect whether the bounding box collides with the bounding sphere, and check whether the distance from the center of the sphere to the nearest point of the bounding box is less than or equal to the radius of the sphere; the minimum and maximum values ​​of the bounding box on the x, y, and z axes are respectively (x min ,y min , z min ) and (x max ,y max , z max ), the center of the enclosing sphere is C(x c ,y c , z c ), with a radius of r, calculate the closest point P(x) from the center of the sphere to the bounding box P ,y P , z P ), x P =max(x min ,min(x c ,x max )) and P =max(y min ,min(and c ,and max )) With P =max(from min ,min(of c ,With max )) Calculate the distance from the center of the sphere to the nearest point: If d≤r, the bounding sphere and the bounding box collide; otherwise, there is no collision; the bounding sphere collides with the bounding cylinder, and the unit vector u in the direction of the cylinder axis is calculated as (u x ,u y ,u z ), a point P0 (x0, y0, z0) on the axis, and the base radius r cy1 , calculate the closest point P1 from the center of the sphere to the axis, Where v = (x c -x0,y c -y0,z c -z0), calculate the distance from the center of the sphere to the nearest point, d = ||C-P1||, if d ≤ r+r cy1 , then the enclosing sphere collides with the enclosing cylinder, otherwise, no collision occurs.

10. The arch dam cable crane real-time obstacle avoidance trajectory smooth transition planning method according to claim 9, characterized in that: The specific implementation of step 7) is as follows: With the starting point set after collision detection, execute step 3) to obtain the nodes of the planned trajectory. Between the two inflection points, a single polynomial is fitted. The displacement curve equation formula is: <h2 style=";text-align:left;direction:ltr">S = b0 + b1t + b2t<h2 style=";text-align:left;direction:ltr"> 2 <h2 style=";text-align:left;direction:ltr"> +b3t<h2 style=";text-align:left;direction:ltr"> 3 <h2 style=";text-align:left;direction:ltr"> +b4t<h2 style=";text-align:left;direction:ltr"> 4 <h2 style=";text-align:left;direction:ltr"> +b5t<h2 style=";text-align:left;direction:ltr"> 5 in: t is the expected value of the time required to complete each segment of the expected completion 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 end point, the real-time obstacle avoidance trajectory smooth transition planning of the single polynomial in the above formula is performed; If a single polynomial does not meet the requirements of the current trajectory planning scenario, that is, there is an intermediate node between the collision warning occurrence point and the target end point, a two-segment polynomial is used, and the displacement curve equation group is: The coefficients of this polynomial are: in: s i is the displacement curve of the i-th segment; For b ij is the jth coefficient of the i-th curve; h i is the displacement time of the i-th segment; s0 and s, v s and v e 、a s and a e They are the displacement, velocity and 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, it is decomposed into a combination of multiple 1-2 segment polynomials; continue with step 3) to perform real-time obstacle avoidance during the process, otherwise terminate the planning and continue to execute the original planned trajectory.

Images