Tower crane low-altitude path planning method, device and equipment based on configuration space

Abstract

The invention provides a tower crane low-altitude path planning method, device and equipment based on configuration space, and relates to the technical field of path planning. According to the method, a configuration space composed of the variable-amplitude freedom degree of the tower crane, the rotation freedom degree of the tower crane and the self-rotation freedom degree of a hoisted object is constructed, an obstacle bounding box is generated and expanded and mapped to the configuration space, dangerous nodes are identified through quadtree division and collision detection, adjacent plane nodes are connected, and an optimal planning path is obtained through planning by using an A * algorithm. And the tower crane is operated to run according to the optimal planning path. The problems that in traditional path planning, the path is complex, operability is low, and self-rotation of the hoisted object is not considered can be solved, calculation efficiency and path precision are improved, operation cost is reduced, and the method is suitable for hoisting tasks with rotation requirements for the hoisting point and the target point.

Description

Technical Field

[0001] The present invention relates to the technical field of collision detection and path planning, and in particular to a method, device and equipment for low-altitude path planning of a tower crane based on configuration space. Background Art

[0002] In the field of construction, tower crane path planning is an important part of ensuring safe and efficient lifting operations. Traditionally, tower crane operators rely on experience and visual judgment to perform lifting tasks, which is cumbersome and prone to errors.

[0003] With the development of technology, researchers have proposed a variety of path planning methods to optimize the lifting process, but these methods still have significant shortcomings. For example, the rapid exploration random tree and its variants search non-convex high-dimensional space by randomly constructing space-filling trees. Although they can effectively cover complex spaces, they lack constraints on the search direction, resulting in complex and difficult-to-execute paths. Evolutionary algorithms such as genetic algorithms, particle swarm optimization, and simulated annealing reflect the researcher's preferences by establishing optimization objectives and weights, but these methods have high requirements for computing performance and are difficult to be widely used when high-performance equipment is limited on construction sites. In addition, since these algorithms are sensitive to the initial state, small changes may lead to significantly different results, making them less interpretable and repeatable, and difficult to meet the requirements for stability and accuracy in lifting practice. The Dijkstra and A* algorithms achieve optimization by recursively searching for the minimum cost path, but the paths they plan are mostly in the form of broken lines with many nodes, which leads to frequent swinging of the hook, increasing the risk of collision and energy consumption. At the same time, the path is incompatible with the rotation and amplitude movement characteristics of the tower crane, making it difficult for operators to operate according to the planned path. More importantly, existing methods mainly focus on the degrees of freedom of the tower crane itself, such as rotation, lifting and luffing, but ignore the possibility of self-rotation of the hoisted object at low altitude. This not only increases the operating cost, but also fails to meet the task requirements of having specific requirements on the rotation angle of the hoisted object between the starting position and the target position.

[0004] In view of this, the applicant filed this application after studying the existing technology. Summary of the invention

[0005] The present invention aims to provide a tower crane low-altitude path planning method, device and equipment based on configuration space, so as to solve the problems existing in the existing tower crane low-altitude path planning technology, such as complex path, low operability, high calculation cost and failure to consider the degree of freedom of self-rotation of the suspended object. The present invention comprehensively considers the motion characteristics of the tower crane, the possibility of self-rotation of the suspended object and the calculation efficiency, so as to realize efficient, accurate and highly operable path planning.

[0006] In order to solve the above technical problems, the present invention is implemented through the following technical solutions: A tower crane low-altitude path planning method based on configuration space, comprising: S1, obtain relevant data of the tower crane, the hoisted object and obstacles; S2, construct the three-dimensional configuration space of the tower crane's low-altitude lifting motion according to the tower crane's amplitude degree of freedom, rotation degree of freedom and self-rotation degree of freedom of the hoisted object; S3, generating an OBB bounding box of the suspended object and the obstacle in Cartesian space, and expanding the bounding box of the obstacle according to the self-rotation angle and size of the suspended object to obtain an expanded obstacle; S4, mapping the suspended object, the extended obstacle and the pseudo-obstacle to the three-dimensional configuration space; wherein the pseudo-obstacle is the range of the amplitude degree of freedom and the rotation degree of freedom of the tower crane; S5, performing quadtree partitioning on each plane of the mapped three-dimensional configuration space, and performing recursive collision detection between the quadtree and the extended obstacle to identify collision sub-bodies, until a partition termination condition is reached; S6, connecting the sub-volumes of the same level of adjacent planes and not at the vertices of the collision sub-volume with straight lines to generate a three-dimensional grid; S7, according to the three-dimensional grid, use the A* algorithm to perform path planning to obtain the optimal planned path, and operate the tower crane according to the optimal planned path.

[0007] Preferably, S3 specifically includes: Set the object to be hoisted The length is W, the width is H, and the vertex coordinates of the bounding box of the hanging object OBB are for: ; ; in, For the suspended object The vertex coordinates of the OBB bounding box; is the rotation matrix of the suspended object, is the current rotation angle of the suspended object; is the coordinate of the center of mass of the suspended object; The vertex coordinates of each obstacle OBB bounding box for: ; ; in, The vertex coordinates of the obstacle OBB bounding box; is the rotation matrix of the obstacle; is the direction angle of the current obstacle; , is the coordinate of the center of mass of the obstacle; , are the length and width of the obstacle respectively; After the OBB bounding box of the suspended object and the obstacle is generated, the outer boundary of the OBB bounding box of the obstacle is expanded according to the self-rotation angle of the suspended object and the vertices of the bounding box. The expansion expression is: ; in, When the self-rotation angle of the suspended object is When , the vertex coordinates of the expanded obstacle in the Cartesian coordinate system; is the Minkowski sum operation; , Respectively represent the point coordinates of the obstacle and the suspended object; , are the x and y values ​​of the vertices of the extended obstacle in the Cartesian coordinate system, respectively.

[0008] Preferably, S4 is specifically: After the obstacle bounding box is expanded, the suspended object, the expanded obstacle and the pseudo obstacle are mapped into the three-dimensional configuration space; The suspended object is mapped to a point P in the three-dimensional configuration space, expressed as: ; Where D is the luffing degree of freedom of the tower crane; is the rotational freedom of the tower crane; is the rotational freedom of the suspended object; Each discrete surface of the extended obstacle in the three-dimensional configuration space is mapped into a convex polygon, and the expression is: ; in, Mapping the vertex coordinates of each plane in the configuration space to the extended obstacle, connecting the vertices to form the extended obstacle in the configuration space; Indicates existence; is the vertex coordinates of the extended obstacle in the Cartesian coordinate system; , are the x-value and y-value of the vertex of the extended obstacle in the Cartesian coordinate system respectively; Next, the pseudo-obstacle is mapped to the configuration space and represented as a rectangle or square; the pseudo-obstacle is located on each plane of the configuration space. The position of is the same, and its vertex coordinate in the configuration space is expressed as K, which is determined by the amplitude range and the rotation range.

[0009] Preferably, S5 is specifically: The rotation angle of each suspended object Corresponding to a two-dimensional plane , each plane The center point is initialized as the root node of a quadtree, and the coverage of the quadtree is the tower crane's variable degree of freedom D and the tower crane's rotational degree of freedom The value range of Each plane Perform quadtree partitioning to create new sub-bodies until the partition termination condition is reached; The shape of each sub-body is square or rectangular, and the shapes of sub-bodies at the same level are consistent; In each plane The quadtree is used to detect collisions with the extended obstacles, the collision sub-bodies that collide with the obstacles are identified, and the vertices of the identified collision sub-bodies are stored in the collision risk node set N to avoid the planned route passing through N.

[0010] Preferably, the division termination condition is that the side length of the divided sub-body or the recursive level reaches a set threshold.

[0011] Preferably, in the configuration space, when the vertices of the same level sub-volumes of adjacent planes that are not in the collision sub-volume are connected by straight lines to generate a three-dimensional grid, the adjacent planes in the three-dimensional configuration space are connected. and The connection relationship is established between nodes. The connection must meet the following requirements: The two vertices of the connection are not in the collision risk node set N, and the rotation angle difference is in is the preset discrete rotation step size; Make the lifting point or target point of the tower crane on the three-dimensional grid or connect with the vertex of the three-dimensional grid; The edge of each three-dimensional grid is a possible planning path, and it is guaranteed that the planned path can reach the lifting point or the target point.

[0012] Preferably, the method further comprises: when the lifting point or the target point of the tower crane is not on the three-dimensional grid, connecting the lifting point or the target point of the tower crane with the vertices of the three-dimensional grid by an optimal connection path; the optimal connection path is: Identify the smallest sub-volume where the lifting point or target point is located and the vertex of the extended obstacle in the sub-volume Or the vertex K of the pseudo obstacle; The four vertices of the smallest identified sub-body are used as the starting point of the path, the lifting point of the tower crane or the target point is the end point of the path, and the vertices of the obstacle are expanded. The vertex K of the pseudo-obstacle is the path node, and the Dijkstra algorithm is used to plan an optimal connection path from the vertex of the smallest sub-body to the lifting point or the target point.

[0013] Preferably, in the process of obtaining the optimal planning path; The total cost function of the A* algorithm It is expressed as: ; in, is the path cost function; is the heuristic function; Path cost function The expression is: ; ; ; ; in, is the distance difference between the current node and the previous node in the amplitude variation direction; , are the amplitude values ​​of the previous node and the current node respectively; is the angular difference between the current node and the previous node in the rotation direction; , They are the rotation angle values ​​of the previous node and the current node respectively; It is the angle difference between the current node and the previous node in the self-rotation of the suspended object; , They are the self-rotation angle values ​​of the suspended object at the previous node and the current node respectively; , , They are the amplitude variation weight, rotation weight, and self-rotation weight of the suspended object; Heuristic function The expression is: ; in, It is the amplitude variation value of the lifting point or target point; is the amplitude value of the current node n; It is the angle difference between the lifting point or target point and the current node in the rotation direction; The self-rotation angle value of the hoisting object at the lifting point or target point; is the self-rotation angle value of the suspended object at the current node n; When the total cost function When it is minimum, it is the optimal planning path.

[0014] The present invention also provides a tower crane low-altitude path planning device based on configuration space, comprising: An acquisition unit, used to acquire relevant data of the tower crane, the hoisted object and the obstacles; A three-dimensional configuration space construction unit is used to construct a three-dimensional configuration space of the tower crane's low-altitude hoisting motion according to the tower crane's amplitude degree of freedom, rotation degree of freedom and the self-rotation degree of freedom of the hoisted object; The extended obstacle unit is used to generate the OBB bounding box of the suspended object and the obstacle in the Cartesian space, and expand the bounding box of the obstacle according to the self-rotation angle and size of the suspended object to obtain the extended obstacle; A mapping unit, used for mapping the hoisted object, the extended obstacle and the pseudo-obstacle to the three-dimensional configuration space; wherein the pseudo-obstacle is the range of the amplitude degree of freedom and the rotation degree of freedom of the tower crane; A quadtree partitioning unit is used to perform quadtree partitioning on each plane of the mapped three-dimensional configuration space, and to perform recursive collision detection between the quadtree and the extended obstacle to identify collision sub-bodies until a partition termination condition is reached; A three-dimensional grid unit, used to connect the vertices of the same level sub-volumes of adjacent planes and not at the colliding sub-volumes with straight lines to generate a three-dimensional grid; The optimal planning path unit is used to perform path planning based on the three-dimensional grid using the A* algorithm to obtain the optimal planning path, and operate the tower crane according to the optimal planning path.

[0015] The present invention also provides a tower crane low-altitude path planning device based on configuration space, including a processor and a memory, wherein the memory stores a computer program, and the computer program can be executed by the processor to implement a tower crane low-altitude path planning method based on configuration space as described above.

[0016] The present invention also provides a computer-readable storage medium, on which computer-readable instructions are stored. When the computer-readable instructions are executed by a processor of a device where the computer-readable storage medium is located, a tower crane low-altitude path planning method based on a configuration space as described above is implemented.

[0017] In summary, compared with the prior art, the present invention has the following beneficial effects: The invention constructs a three-dimensional configuration space and introduces the self-rotation freedom of the hoisted object. The planned path conforms to the kinematic characteristics of the tower crane, is easy for the operator to execute, and has stronger operability. At the same time, the method of the invention fully considers the self-rotation possibility of the hoisted object, is suitable for tasks that require the hoisting of the hoisted object and the target point to rotate, and reduces the hoisting cost.

[0018] The present invention effectively reduces the collision risk and energy consumption in the hoisting process through collision detection and path optimization, improves safety, and solves the problems existing in the existing tower crane low-altitude path planning technology, such as complex paths, low operability, high calculation costs, and failure to consider the rotational freedom of the hoisted object. It provides an efficient, accurate and safe tower crane low-altitude path planning solution. BRIEF DESCRIPTION OF THE DRAWINGS

[0019] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings required for use in the embodiments will be briefly introduced below. It should be understood that the following drawings only show certain embodiments of the present invention and therefore should not be regarded as limiting the scope. For ordinary technicians in this field, other related drawings can be obtained based on these drawings without paying creative work.

[0020] Figure 1 A schematic diagram of a tower crane low-altitude path planning method based on configuration space provided in Example 1.

[0021] Figure 2 This is an example diagram of the result after the obstacle bounding box provided in Example 1 is expanded, wherein: Figure 2 (a) is the result diagram after expansion into an octagon. Figure 2 (b) is the resulting graph after expansion into a quadrilateral.

[0022] Figure 3 A schematic diagram of the configuration space and three-dimensional grid construction based on quadtree partitioning provided in Example 1.

[0023] Figure 4 A schematic diagram of a tower crane low-altitude path planning device based on configuration space provided in Example 2.

[0024] The present invention is further described in detail below in conjunction with the accompanying drawings and specific embodiments. DETAILED DESCRIPTION

[0025] In order to make the purpose, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present invention. Therefore, the following detailed description of the embodiments of the present invention provided in the drawings is not intended to limit the scope of the invention claimed for protection, but merely represents the selected embodiments of the present invention. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present invention.

[0026] Embodiment 1 Embodiment 1 of the present invention provides a tower crane low-altitude path planning method based on configuration space, which can be implemented by a tower crane low-altitude path planning device based on configuration space (hereinafter referred to as path planning device), and in particular, executed by one or more processors in the path planning device.

[0027] In this embodiment, the path planning device may be an electronic device equipped with a processor, which carries a computer program of the low-altitude path planning method of the tower crane based on the configuration space and the computer program can be executed, such as a computer, a smart phone, a smart tablet, a workstation, etc., which is not limited here.

[0028] In this embodiment, the configuration space is an abstract spatial concept, in which each point represents a specific configuration of a robot (such as a tower crane). Configuration refers to a set of independent variables required to fully describe the position and posture of the robot in space. For example, for a robot with n degrees of freedom, the configuration space is an n-dimensional space, and each point in the space is determined by n coordinate values, which correspond to the positions or angles of each joint of the robot.

[0029] Quadtree: recursively subdivides space into four quadrants or regions, each node is a leaf node or has four leaf nodes. Each node corresponds to a specific two-dimensional region. When the data in the region meets certain conditions (such as the number of data exceeds the threshold, the data distribution in the region is uneven, etc.), the region will be divided into four sub-regions, each of which corresponds to a child node.

[0030] A recursive algorithm is a method in which a function or algorithm calls itself. In programming, recursive algorithms are mainly used to solve tasks that can be broken down into smaller, identical problems.

[0031] The A* algorithm is an efficient path search and graph traversal algorithm, which is widely used in game design, robot path planning, network routing and other fields. It finds the shortest path from the starting point to the end point by combining heuristic evaluation and actual cost.

[0032] Path planning: The sequence of points or curves connecting the starting point and the end point is called a path, and the strategy that constitutes the path is called path planning.

[0033] like Figure 1 As shown, a tower crane low-altitude path planning method based on configuration space includes steps S1 to S7.

[0034] S1, obtain the relevant data of the tower crane, the hoisted object and the obstacles.

[0035] In this embodiment, the application scenario is the low-altitude hoisting movement of the tower crane, so this method does not consider the lifting freedom and height attributes of the tower crane. The tower crane's amplitude freedom D and rotation freedom can be obtained through laser radar or other sensors. , lifting point location, target point location, size of the object being hoisted, rotational freedom , center of mass coordinates, obstacle size, position, center of mass coordinates, orientation angle, rotation angle and other data for subsequent path planning.

[0036] S2, construct the three-dimensional configuration space of the tower crane's low-altitude lifting movement based on the tower crane's amplitude degree of freedom, rotation degree of freedom and the self-rotation degree of freedom of the hoisted object.

[0037] In this embodiment, a three-dimensional configuration space is first constructed, and its three axes are the tower crane's amplitude freedom D, rotation freedom and the rotational freedom of the suspended object .

[0038] Specifically, the range of the variable amplitude degree of freedom D is , ],in, Indicates the minimum amplitude, Indicates the maximum amplitude; Rotational freedom The value range is ; Rotational freedom of the suspended object The discrete angle set is represented as: ; Wherein, k is any integer; Represents an integer; It is a preset discrete rotation step, which is set by the user according to actual needs.

[0039] The tower crane's amplitude degree of freedom and rotational degree of freedom are continuously changing, forming a continuous plane. Because the self-rotation of the hoisted object is generally performed manually using a rope, not a machine rotation, it is not suitable for small rotations, such as 1 degree, and it is not realistic. Therefore, the self-rotation axis of the hoisted object is discrete, and the discrete step length is determined by the user. Too small discrete step length will result in a lot of calculation time, and too large discrete step length will result in the planned path being inaccurate. Therefore, the configuration space is composed of multiple parallel planes stacked together, and each plane Corresponding to a specific rotation angle of the suspended object These planes form the basic structure for subsequent path planning.

[0040] S3, generating an OBB bounding box of the suspended object and the obstacle in Cartesian space, and expanding the bounding box of the obstacle according to the rotation angle and size of the suspended object to obtain an expanded obstacle.

[0041] Next, an OBB bounding box is generated for the suspended object and the obstacle in Cartesian space, and the bounding box of the obstacle is expanded.

[0042] Attach Figure 2 For example, suppose the object to be hoisted is The length is W, the width is H, and the vertex coordinates of the bounding box of the hanging object OBB are for: ; ; in, For the suspended object The vertex coordinates of the OBB bounding box; is the rotation matrix of the suspended object, is the current rotation angle of the suspended object; is the coordinate of the center of mass of the suspended object.

[0043] For each obstacle OBB bounding box vertex coordinates for: ; ; in, The vertex coordinates of the obstacle OBB bounding box; is the rotation matrix of the obstacle; is the direction angle of the current obstacle; , is the coordinate of the center of mass of the obstacle; , are the length and width of the obstacle respectively.

[0044] After the OBB bounding box of the suspended object and the obstacle is generated, the outer boundary of the OBB bounding box of the obstacle is expanded. The expansion method is related to the rotation angle of the suspended object and the vertices of the bounding box. The expansion is performed through the Minkowski and operation. The specific method is as follows: ; in, When the self-rotation angle of the suspended object is When , the vertex coordinates of the expanded obstacle in the Cartesian coordinate system; is the Minkowski sum operation; , Respectively represent the point coordinates of the obstacle and the suspended object; , are the x and y values ​​of the vertices of the extended obstacle in the Cartesian coordinate system, respectively.

[0045] The Minkowski Sum operation is an operation defined in Euclidean space and is used to describe the combination of two point sets. Every point and point set in Add up each point in the , and the new point set finally formed is their Minkowski sum.

[0046] The bounding box of the expanded obstacle is an eight-sided convex polygon with eight vertices, as shown in the figure below. Figure 2 In the special case, when the obstacle is parallel to the OBB edge of the suspended object, the bounding box of the extended obstacle is a four-vertex four-sided convex polygon, as shown in the figure. Figure 2 As shown in (b), the bounding box of the expanded obstacle can accurately describe the geometric relationship between the obstacle and the suspended object, thereby improving the accuracy of path planning.

[0047] S4, mapping the suspended object, the extended obstacle and the pseudo-obstacle to the three-dimensional configuration space; wherein the pseudo-obstacle is the range of the tower crane's amplitude degree of freedom and rotation degree of freedom.

[0048] After the obstacle bounding box is expanded, the suspended object, the expanded obstacle and the pseudo obstacle are mapped into the configuration space. The suspended object is mapped into a point P in the configuration space, which is expressed as: ; Extend obstacles in each discrete plane is mapped into a convex polygon, as shown in the attached Figure 3 As shown by the black polygon in , the mapping method is: ; in, To map the vertex coordinates of the extended obstacle on each plane of the configuration space, the extended obstacle in the configuration space is formed by connecting the vertices; Indicates existence; is the vertex coordinates of the extended obstacle in the Cartesian coordinate system; , are the x and y values ​​of the vertices of the extended obstacle in the Cartesian coordinate system, respectively.

[0049] During the hoisting process, due to the rotation and luffing movement, even if the hoisted object does not collide with the obstacle, the tower crane's boom and rope may collide with the obstacle. That is, the tower crane's luffing and rotation range are constrained, and they are mapped into the configuration space as pseudo-obstacles, mapped into a rectangle or square, as shown in the figure below. Figure 3 The pseudo obstacle is shown as the red quadrilateral in the configuration space. The position of is the same, and its vertex coordinate in the configuration space is expressed as K (determined by the amplitude range and the rotation range).

[0050] S5, performing quadtree partitioning on each plane of the mapped three-dimensional configuration space, and performing recursive collision detection between the quadtree and the extended obstacle to identify collision sub-bodies, until a partition termination condition is reached.

[0051] In the configuration space, the rotation angle of each suspended object Corresponding to a two-dimensional plane , each plane The center point is initialized as the root node of a quadtree. Initialize the root node of the quadtree, covering the tower crane's variable degree of freedom D and the tower crane's rotational degree of freedom The value range of is calculated, and new sub-bodies are recursively divided until the division termination condition is reached, such as setting the division termination threshold (the sub-body side length is less than 0.5m or the recursive level exceeds 5 levels). The shape of each sub-body is square or rectangular, and the shapes of sub-bodies at the same level are consistent. The outer frame of the sub-body after division is the possible lifting route, and the sub-body vertices are path nodes. The introduction of the quadtree structure can significantly reduce the computational complexity of path planning while ensuring the accuracy of the path.

[0052] In each plane The quadtree and the extended obstacle are recursively collided to detect collision, the collision sub-body colliding with the obstacle is identified, and the vertices of the identified collision sub-body are stored in the collision risk node set N to avoid the planned route passing through N.

[0053] S6, connecting the vertices of the same level sub-volumes of adjacent planes that are not in the collision sub-volume with straight lines to generate a three-dimensional grid.

[0054] In the configuration space, when the vertices of the same level sub-volumes of adjacent planes that are not in the collision sub-volume are connected by straight lines to generate a three-dimensional mesh, such as Figure 3 As shown, the adjacent self-rotation angle planes in the three-dimensional configuration are and (i.e. the rotation angle of the object being hoisted and The nodes of the corresponding adjacent planes are connected to form a three-dimensional grid.

[0055] When connecting, the following conditions must be met: the vertices at both ends of the connection are not in the collision risk node set N, and the rotation angle difference is ;in is the preset discrete rotation step size.

[0056] The lifting point or target point of the tower crane is placed on the three-dimensional grid or connected to the vertices of the three-dimensional grid. The edge of each three-dimensional grid is a possible planning path, and it is guaranteed that the planned path can reach the lifting point or target point.

[0057] In actual operation, when the lifting point or target point of the tower crane is not on the three-dimensional grid, the lifting point or target point of the tower crane is connected to the vertices of the three-dimensional grid by the optimal connection path, specifically: Identify the smallest sub-body where the lifting point or target point of the tower crane is located and the extended obstacle or pseudo-obstacle vertex inside it, and use the Dijkstra algorithm to plan the optimal connection path from the vertex of the smallest sub-body to the lifting point or target point. Figure 3 The green dotted line in the figure shows a schematic diagram of the 3D grid path. Local path planning is used to ensure the integrity and flexibility of the path while reducing the computational complexity.

[0058] In this embodiment, the Dijkstra algorithm is a classic algorithm for calculating the single-source shortest path, which is applicable to weighted directed graphs and requires that the weights of all edges are non-negative. The algorithm gradually expands the shortest path from the starting point to other nodes and eventually finds the shortest path from the starting point to all other nodes.

[0059] S7, according to the three-dimensional grid, use the A* algorithm to perform path planning to obtain the optimal planned path, and operate the tower crane according to the optimal planned path.

[0060] The path planning problem of a tower crane is to find a collision-free, low-cost route connecting a starting point to a target point in a Cartesian coordinate system, which is converted into finding a route connecting a starting point to a target point through a three-dimensional grid in a configuration space. The calculation amount of the invention is small and uncomplicated, and the planned path conforms to the movement characteristics of the tower crane, which is convenient for the operator to operate, and the planned path also takes into account the possibility of self-rotation of the hoisted object.

[0061] In particular, the path planning stage uses the A* algorithm, a heuristic search algorithm for graph search and path planning, which combines the advantages of the greedy algorithm and the Dijkstra algorithm, and can significantly improve the search efficiency while ensuring the optimal solution. The A* algorithm evaluates the total cost function of each node. To select the optimal planning path. Total cost function Including the path cost function g(n) and the heuristic function h(n), defined as follows: Total cost function It is expressed as: ; in, is the path cost function, i.e., the actual cost (path length) from the starting point to the current node; is the heuristic function, that is, the estimated cost from the current node to the end point (lifting point or target point).

[0062] Path cost function The expression is: ; ; ; ; in, is the distance difference between the current node and the previous node in the amplitude variation direction; , are the amplitude values ​​of the previous node and the current node respectively; is the angular difference between the current node and the previous node in the rotation direction; , They are the rotation angle values ​​of the previous node and the current node respectively; It is the angle difference between the current node and the previous node in the self-rotation of the suspended object; , They are the self-rotation angle values ​​of the suspended object at the previous node and the current node respectively; , , They are the amplitude variation weight, rotation weight, and self-rotation weight of the suspended object, such as the amplitude variation weight = 0.4, the rotation weight = 0.3, and the self-rotation weight = 0.3. Experimental verification has shown that the path cost can be balanced.

[0063] Heuristic function The expression is: ; in, It is the amplitude variation value of the lifting point or target point; is the amplitude value of the current node n; It is the angle difference between the lifting point or target point and the current node in the rotation direction; The self-rotation angle value of the hoisting object at the lifting point or target point; It is the self-rotation angle value of the suspended object at the current node n.

[0064] Finally, the path with the smallest f(n) is selected as the planning result.

[0065] Through the A* algorithm, the global optimality and operability of the path are achieved.

[0066] Through the above-mentioned implementation steps, the technical effect of the method of the present invention is reflected. Through the three-dimensional configuration space constructed by the present invention combined with the self-rotation freedom of the hoisted object, the planned path conforms to the kinematic characteristics of the tower crane, is convenient for the operator to execute, and has stronger operability. Compared with path planning in a Cartesian coordinate system, this method reduces the amount of calculation and memory resource usage, and improves calculation efficiency. In addition, this method fully considers the possibility of self-rotation of the hoisted object, is suitable for tasks that require rotation between the lifting point and the target point of the hoisted object, and reduces the cost of hoisting. Through collision detection and path optimization, the collision risk and energy consumption during the hoisting process are effectively reduced, and safety is improved.

[0067] In a construction scene, for example, a large construction site needs to use a tower crane to transport construction materials from the lifting point to the target point, and the target point has clear requirements for the self-rotation angle of the material. According to the traditional method, the operator needs to rely on experience to make manual adjustments, which easily leads to problems such as complex paths, difficult operations, and high safety risks. After adopting the method of the present invention, the three-dimensional configuration space is first constructed according to the parameters of the tower crane, and the OBB bounding box of the hoisted object and the obstacle is generated, and the bounding box of the obstacle is expanded to accurately describe the geometric relationship. Subsequently, a three-dimensional grid path is generated through quadtree partitioning and collision detection to ensure the continuity and feasibility of the path. Finally, the A* algorithm is used to plan the optimal path, and the operator only needs to execute according to the planned path. In this process, the operation of the tower crane is more precise, the path is simpler, the hoisting efficiency is significantly improved, and the difficulty of operation and safety risks are reduced.

[0068] In summary, compared with the prior art, the present invention has the following beneficial effects: The invention solves the problem that the traditional method does not consider the self-rotation freedom degree of the suspended object by constructing a three-dimensional configuration space with the tower crane's amplitude variation freedom degree, rotation freedom degree and the self-rotation freedom degree of the suspended object as axes.

[0069] The present invention introduces an OBB bounding box expansion mechanism, and accurately describes the geometric relationship between the obstacle and the suspended object by expanding the bounding box of the obstacle, thereby improving the accuracy of path planning.

[0070] The present invention uses a quadtree structure to divide the configuration space and combines a recursive collision detection algorithm to significantly improve the efficiency and accuracy of path planning.

[0071] The present invention adopts the A* algorithm in the path planning stage, and ensures the optimality and operability of the path by defining the path cost function and the heuristic function.

[0072] The method of the present invention avoids the path complexity problem caused by too many nodes in the traditional method by establishing a quadtree structure in each discrete plane and performing collision detection. At the same time, by connecting the nodes of adjacent planes to form a three-dimensional grid, the continuity and feasibility of the path between different self-rotation angles are ensured. In addition, when the lifting point or the target point is not on the three-dimensional grid, the integrity and flexibility of the path planning are guaranteed by identifying the minimum sub-body and using the Dijkstra algorithm to plan the local path.

[0073] Embodiment 2 like Figure 4 As shown, the second embodiment of the present invention further provides a tower crane low-altitude path planning device based on configuration space, comprising: An acquisition unit, used to acquire relevant data of the tower crane, the hoisted object and the obstacles; A three-dimensional configuration space construction unit is used to construct a three-dimensional configuration space of the tower crane's low-altitude hoisting motion according to the tower crane's amplitude degree of freedom, rotation degree of freedom and the self-rotation degree of freedom of the hoisted object; The extended obstacle unit is used to generate the OBB bounding box of the suspended object and the obstacle in the Cartesian space, and expand the bounding box of the obstacle according to the self-rotation angle and size of the suspended object to obtain the extended obstacle; A mapping unit, used for mapping the hoisted object, the extended obstacle and the pseudo-obstacle to the three-dimensional configuration space; wherein the pseudo-obstacle is the range of the amplitude degree of freedom and the rotation degree of freedom of the tower crane; A quadtree partitioning unit is used to perform quadtree partitioning on each plane of the mapped three-dimensional configuration space, and to perform recursive collision detection between the quadtree and the extended obstacle to identify collision sub-bodies until a partition termination condition is reached; A three-dimensional grid unit, used to connect the vertices of the same level sub-volumes of adjacent planes and not at the colliding sub-volumes with straight lines to generate a three-dimensional grid; The optimal planning path unit is used to perform path planning based on the three-dimensional grid using the A* algorithm to obtain the optimal planning path, and operate the tower crane according to the optimal planning path.

[0074] Embodiment 3 The third embodiment of the present invention also provides a low-altitude path planning device for a tower crane based on a configuration space, which includes a memory and a processor, wherein a computer program is stored in the memory, and the computer program can be executed by the processor to implement the low-altitude path planning method for a tower crane based on a configuration space as described above.

[0075] Embodiment 4 The fourth embodiment of the present invention also provides a computer-readable storage medium, on which computer-readable instructions are stored. When the computer-readable instructions are executed by a processor of a device where the computer-readable storage medium is located, the tower crane low-altitude path planning method based on the configuration space as described above is implemented.

[0076] In several embodiments provided in the embodiments of the present invention, it should be understood that the disclosed apparatus and method can also be implemented in other ways. The apparatus and method embodiments described above are merely schematic. For example, the flowcharts in the accompanying drawings show the possible architecture, functions and operations of the apparatus, method and computer program product according to multiple embodiments of the present invention. In this regard, each box in the flowchart or block diagram can represent a module, a program segment or a part of a code, and the module, program segment or a part of the code contains one or more executable instructions for implementing the specified logical function. It should also be noted that in some alternative implementations, the functions marked in the box can also occur in a different order from the order marked in the accompanying drawings. For example, two consecutive boxes can actually be executed substantially in parallel, and they can sometimes be executed in the opposite order, depending on the functions involved. It should also be noted that each box in the block diagram and / or flow chart, and the combination of boxes in the block diagram and / or flow chart can be implemented with a dedicated hardware-based system that performs a specified function or action, or can be implemented with a combination of dedicated hardware and computer instructions.

[0077] In addition, the functional modules in the various embodiments of the present invention may be integrated together to form an independent part, or each module may exist independently, or two or more modules may be integrated to form an independent part.

[0078] If the function is implemented in the form of a software function module and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention is essentially or the part that contributes to the prior art or the part of the technical solution can be embodied in the form of a software product, which is stored in a storage medium and includes several instructions to enable a computer device (which can be a personal computer, an electronic device, or a network device, etc.) to perform all or part of the steps of the method described in each embodiment of the present invention. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), disk or optical disk and other media that can store program code. It should be noted that in this article, the term "include", "include" or any other variant thereof is intended to cover non-exclusive inclusion, so that the process, method, article or device including a series of elements includes not only those elements, but also includes other elements that are not explicitly listed, or also includes elements inherent to such process, method, article or device. Without more constraints, an element defined by the phrase "comprising a..." does not exclude the existence of other identical elements in the process, method, article or apparatus comprising the element.

[0079] The terms used in the embodiments of the present invention are only for the purpose of describing specific embodiments, and are not intended to limit the present invention. The singular forms "a", "said" and "the" used in the embodiments of the present invention and the appended claims are also intended to include plural forms, unless the context clearly indicates other meanings.

[0080] It should be understood that the term "and / or" used in this article is only a description of the association relationship of associated objects, indicating that there can be three relationships. For example, A and / or B can represent: A exists alone, A and B exist at the same time, and B exists alone. In addition, the character " / " in this article generally indicates that the associated objects before and after are in an "or" relationship.

[0081] The word "if" as used herein may be interpreted as "at the time of" or "when" or "in response to determining" or "in response to detecting", depending on the context. Similarly, the phrases "if it is determined" or "if (stated condition or event) is detected" may be interpreted as "when it is determined" or "in response to determining" or "when detecting (stated condition or event)" or "in response to detecting (stated condition or event)", depending on the context.

[0082] The "first\second" mentioned in the embodiments is only to distinguish similar objects, and does not represent a specific order for the objects. It is understandable that the "first\second" can be interchanged with the specific order or sequence where permitted. It should be understood that the objects distinguished by "first\second" can be interchanged where appropriate, so that the embodiments described herein can be implemented in an order other than those illustrated or described herein.

[0083] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A tower crane low-altitude path planning method based on configuration space, characterized in that: include: S1, obtain relevant data of the tower crane, the hoisted object and obstacles; S2, constructing the three-dimensional configuration space of the tower crane's low-altitude lifting motion according to the tower crane's amplitude degree of freedom, rotation degree of freedom and self-rotation degree of freedom of the hoisted object; S3, generating an OBB bounding box of the suspended object and the obstacle in Cartesian space, and expanding the bounding box of the obstacle according to the self-rotation angle and size of the suspended object to obtain an expanded obstacle; S4, mapping the suspended object, the extended obstacle and the pseudo-obstacle to the three-dimensional configuration space; wherein the pseudo-obstacle is the range of the amplitude degree of freedom and the rotation degree of freedom of the tower crane; S5, performing quadtree partitioning on each plane of the mapped three-dimensional configuration space, and performing recursive collision detection between the quadtree and the extended obstacle to identify collision sub-bodies, until a partition termination condition is reached; S6, connecting the sub-volumes of the same level of adjacent planes and not at the vertices of the collision sub-volume with straight lines to generate a three-dimensional grid; S7, according to the three-dimensional grid, use the A* algorithm to perform path planning to obtain the optimal planned path, and operate the tower crane according to the optimal planned path.

2. A tower crane low-altitude path planning method based on configuration space according to claim 1, characterized in that , the S3 is specifically: Set the object to be hoisted The length is W, the width is H, and the vertex coordinates of the bounding box of the hanging object OBB are for: ； ； in, For the suspended object The vertex coordinates of the OBB bounding box; is the rotation matrix of the suspended object, is the current rotation angle of the suspended object; is the coordinate of the center of mass of the suspended object; The vertex coordinates of each obstacle OBB bounding box for: ； ； in, The vertex coordinates of the obstacle OBB bounding box; is the rotation matrix of the obstacle; is the direction angle of the current obstacle; , is the coordinate of the center of mass of the obstacle; , are the length and width of the obstacle respectively; After the OBB bounding box of the suspended object and the obstacle is generated, the outer boundary of the OBB bounding box of the obstacle is expanded according to the self-rotation angle of the suspended object and the vertices of the bounding box. The expansion expression is: ； in, When the self-rotation angle of the suspended object is When , the vertex coordinates of the expanded obstacle in the Cartesian coordinate system; is the Minkowski sum operation; , Respectively represent the point coordinates of the obstacle and the suspended object; , are the x and y values ​​of the vertices of the extended obstacle in the Cartesian coordinate system, respectively.

3. The tower crane low-altitude path planning method based on configuration space according to claim 1 is characterized in that , the S4 is specifically: After the obstacle bounding box is expanded, the suspended object, the expanded obstacle and the pseudo obstacle are mapped into the three-dimensional configuration space; The suspended object is mapped to a point P in the three-dimensional configuration space, expressed as: ； Where D is the luffing degree of freedom of the tower crane; is the rotational freedom of the tower crane; is the rotational freedom of the suspended object; Each discrete surface of the extended obstacle in the three-dimensional configuration space is mapped into a convex polygon, and the expression is: ； in, Mapping the vertex coordinates of each plane in the configuration space to the extended obstacle, connecting the vertices to form the extended obstacle in the configuration space; Indicates existence; is the vertex coordinates of the extended obstacle in the Cartesian coordinate system; , are the x-value and y-value of the vertex of the extended obstacle in the Cartesian coordinate system respectively; Next, the pseudo-obstacle is mapped to the configuration space and represented as a rectangle or square; the pseudo-obstacle is located on each plane of the configuration space. The position of is the same, and its vertex coordinate in the configuration space is expressed as K, which is determined by the amplitude range and the rotation range.

4. A tower crane low-altitude path planning method based on configuration space according to claim 2, characterized in that , the S5 is specifically: The rotation angle of each suspended object Corresponding to a two-dimensional plane , each plane The center point is initialized as the root node of a quadtree, and the coverage of the quadtree is the tower crane's variable degree of freedom D and the tower crane's rotational degree of freedom The value range of Each plane Perform quadtree partitioning to create new sub-bodies until the partition termination condition is reached; The shape of each sub-body is square or rectangular, and the shapes of sub-bodies at the same level are consistent; In each plane The quadtree is used to detect collisions with the extended obstacles, the collision sub-bodies that collide with the obstacles are identified, and the vertices of the identified collision sub-bodies are stored in the collision risk node set N to avoid the planned route passing through N.

5. A tower crane low-altitude path planning method based on configuration space according to claim 4, characterized in that ,The termination condition of the division is that the side length of the divided sub-body or the recursive level reaches the set threshold.

6. A tower crane low-altitude path planning method based on configuration space according to claim 4, characterized in that In the configuration space, when the vertices of the same level of the adjacent planes that are not in the collision sub-body are connected by straight lines to generate a three-dimensional grid, the adjacent planes in the three-dimensional configuration space are and The connection relationship is established between nodes. The connection must meet the following requirements: The two vertices of the connection are not in the collision risk node set N, and the rotation angle difference is ;in is the preset discrete rotation step size; Make the lifting point or target point of the tower crane on the three-dimensional grid or connect with the vertex of the three-dimensional grid; The edge of each three-dimensional grid is a possible planning path, and it is guaranteed that the planned path can reach the lifting point or the target point.

7. A tower crane low-altitude path planning method based on configuration space according to claim 6, characterized in that , also includes, when the lifting point or target point of the tower crane is not on the three-dimensional grid, connecting the lifting point or target point of the tower crane with the vertices of the three-dimensional grid by an optimal connection path; the optimal connection path is: Identify the smallest sub-volume where the lifting point or target point is located and the vertex of the extended obstacle in the sub-volume Or the vertex K of the pseudo obstacle; The four vertices of the smallest identified sub-body are used as the starting point of the path, the lifting point of the tower crane or the target point is the end point of the path, and the vertices of the obstacle are expanded. The vertex K of the pseudo-obstacle is the path node, and the Dijkstra algorithm is used to plan an optimal connection path from the vertex of the smallest sub-body to the lifting point or the target point.

8. The method for tower crane low-altitude path planning based on configuration space according to claim 1 is characterized in that ,In the process of obtaining the optimal planning path: The total cost function of the A* algorithm It is expressed as: ； in, is the path cost function; is the heuristic function; Path cost function The expression is: ； ； ； ； in, is the distance difference between the current node and the previous node in the amplitude variation direction; , are the amplitude values ​​of the previous node and the current node respectively; is the angular difference between the current node and the previous node in the rotation direction; , They are the rotation angle values ​​of the previous node and the current node respectively; It is the angle difference between the current node and the previous node in the self-rotation of the suspended object; , They are the self-rotation angle values ​​of the suspended object at the previous node and the current node respectively; , , They are the amplitude variation weight, rotation weight, and self-rotation weight of the suspended object; Heuristic function The expression is: ； in, It is the amplitude variation value of the lifting point or target point; is the amplitude value of the current node n; It is the angle difference between the lifting point or target point and the current node in the rotation direction; The self-rotation angle value of the hoisting object at the lifting point or target point; is the self-rotation angle value of the suspended object at the current node n; When the total cost function When it is minimum, it is the optimal planning path.

9. A tower crane low-altitude path planning device based on configuration space, characterized in that: include: An acquisition unit, used to acquire relevant data of the tower crane, the hoisted object and the obstacles; A three-dimensional configuration space construction unit is used to construct a three-dimensional configuration space of the tower crane's low-altitude hoisting motion according to the tower crane's amplitude degree of freedom, rotation degree of freedom and the self-rotation degree of freedom of the hoisted object; The extended obstacle unit is used to generate the OBB bounding box of the suspended object and the obstacle in the Cartesian space, and expand the bounding box of the obstacle according to the self-rotation angle and size of the suspended object to obtain the extended obstacle; A mapping unit, used for mapping the hoisted object, the extended obstacle and the pseudo-obstacle to the three-dimensional configuration space; wherein the pseudo-obstacle is the range of the amplitude degree of freedom and the rotation degree of freedom of the tower crane; A quadtree partitioning unit is used to perform quadtree partitioning on each plane of the mapped three-dimensional configuration space, and to perform recursive collision detection between the quadtree and the extended obstacle to identify collision sub-bodies until a partition termination condition is reached; A three-dimensional grid unit, used to connect the vertices of the same level sub-volumes of adjacent planes and not at the colliding sub-volumes with straight lines to generate a three-dimensional grid; The optimal planning path unit is used to perform path planning based on the three-dimensional grid using the A* algorithm to obtain the optimal planning path, and operate the tower crane according to the optimal planning path.

10. A tower crane low-altitude path planning device based on configuration space, characterized in that: It comprises a processor and a memory, wherein the memory stores a computer program, and the computer program can be executed by the processor to implement a tower crane low-altitude path planning method based on a configuration space as described in any one of claims 1-8.

Images