Cantilever crane path planning method, electronic equipment and computer storage medium

Abstract

The invention discloses a boom path planning method, electronic equipment and a computer storage medium. The method comprises the following steps: acquiring a starting point position and a target point position; performing random sampling of path points from the starting point position to the target point position; when it is detected that the sampling point collides with the obstacle, the sampling angle is adjusted according to the obstacle information for sampling again, so that the sampling point avoids the obstacle; and performing path fusion on a current planned path determined according to the sampling points searched at the time and a first optimal planned path determined according to the historical sampling points, and determining a target planned path. According to the technical scheme, the reasonability of path planning is improved, and the efficiency of path planning and the hoisting construction efficiency are improved.

Description

Technical Field

[0001] The present application relates to the field of path planning, and in particular to an arm path planning method, electronic equipment and computer storage medium. Background Art

[0002] In the development of intelligent and unmanned booms, automatic lifting is one of the mainstream directions of development, and the path planning of the boom is in a key position. In the related technology, the movement space of the boom of the lifting equipment has a wide range, and the path is repeatedly planned under the same starting point and target point conditions. The path obtained by the spatial search is diverse, resulting in unreasonable planning routes. In addition, the search range of path points in three-dimensional space is wide, and the point search direction diverges in all directions of the sphere, which is infinite. How to search for a feasible lifting path within a limited time is also a problem that technicians in this field need to solve urgently. Summary of the invention

[0003] The purpose of this application is to provide a boom path planning method, electronic equipment and computer storage medium to improve the rationality of path planning and enhance the efficiency of path planning and hoisting construction.

[0004] To achieve the above purpose, the technical solution of this application is implemented as follows:

[0005] In a first aspect, the present application provides a boom path planning method, the method comprising:

[0006] Get the starting point position and the target point position;

[0007] Starting from the starting point position, randomly sampling path points toward the target point position;

[0008] When it is detected that a sampling point collides with an obstacle, the sampling angle is adjusted according to the obstacle information and the sampling is performed again so that the sampling point avoids the obstacle;

[0009] The current planned path determined according to the sampling points of this search is merged with the first optimal planned path determined according to the historical sampling points to determine the target planned path.

[0010] As one implementation manner, before randomly sampling path points from the starting point to the target point, the method further includes:

[0011] Determine a spatial path search area according to the starting point position, the target point position, the obstacle information, and the joint obstacle avoidance strategy of the arm;

[0012] The step of performing random sampling of path points from the starting point to the target point includes:

[0013] Starting from the starting point position, random sampling of path points is performed in the spatial path search area toward the target point position.

[0014] As one implementation manner, determining a spatial path search area according to the starting point position, the target point position, the obstacle information, and the joint obstacle avoidance strategy of the arm includes:

[0015] Determine whether there is an obstacle between the starting point and the target point according to the obstacle information;

[0016] If there is no obstacle, determining a regular spatial area as the spatial path search area according to the starting point position and the target point position;

[0017] If there is an obstacle, a regular spatial area is determined as the initial search area according to the starting point position and the target point position, and the initial search area is constrained based on the joint obstacle avoidance strategy of the arm and the obstacle information to determine the spatial path search area.

[0018] As one implementation mode, the joint obstacle avoidance strategy includes a rotation obstacle avoidance strategy, a winch obstacle avoidance strategy, and a variable amplitude obstacle avoidance strategy. The joint obstacle avoidance strategy based on the boom and the obstacle information determines the spatial path search area, including:

[0019] When it is determined that the obstacle can be crossed according to the winch obstacle avoidance strategy and the obstacle information, determining the search direction to be upward;

[0020] When it is determined that the obstacle is insurmountable according to the winch obstacle avoidance strategy and the obstacle information, the search direction is determined according to the rotation obstacle avoidance strategy and the amplitude variation obstacle avoidance strategy;

[0021] The spatial path search area is determined according to the search direction.

[0022] As one implementation manner, determining the search direction according to the rotation obstacle avoidance strategy and the amplitude variation obstacle avoidance strategy includes:

[0023] According to the azimuth relationship between the target point position and the obstacle, determining the rotation search direction based on the rotation obstacle avoidance strategy;

[0024] If there is an obstacle in the rotation search direction, the variable amplitude search direction is determined based on the variable amplitude obstacle avoidance strategy according to the relationship between the distance from the target point position to the rotation center of the boom and the distance from the current search position to the rotation center of the boom.

[0025] As one implementation manner, when it is detected that the sampling point collides with an obstacle, adjusting the sampling angle according to the obstacle information and re-sampling so that the sampling point avoids the obstacle includes:

[0026] When it is detected that the sampling point collides with the obstacle, the angle required to rotate from the previous level sampling point to the target point position to avoid the obstacle is calculated according to the spatial coordinates, orientation and size of the bounding box of the obstacle;

[0027] Re-sampling is performed based on the angle so that the sampling points avoid obstacles in the spatial path search area.

[0028] As one implementation manner, when the sampling point is detected to collide with the obstacle, the angle required to rotate from the previous level sampling point to the target point position to avoid the obstacle is calculated according to the spatial coordinates, orientation and size of the bounding box of the obstacle, including at least one of the following:

[0029] According to the vertical distance and the horizontal edge distance from the previous level sampling point to the bounding box, calculate the angle that needs to be rotated in the horizontal plane to avoid the obstacle when sampling from the previous level sampling point to the target point position;

[0030] According to the vertical distance and the top edge distance from the previous sampling point to the bounding box, the angle required to rotate in the vertical plane to avoid the obstacle when sampling from the previous sampling point to the target point position is calculated.

[0031] As one implementation mode, the current planned path determined according to the sampling points of the current search is merged with the first optimal planned path determined according to the historical sampling points to determine the target planned path, including:

[0032] The first optimal planned path and the current planned path are merged to obtain a second optimal planned path, wherein the length of the second optimal planned path is less than or equal to the length of the first optimal planned path and the current planned path;

[0033] When the second optimal planning path meets the length convergence condition, the sampling is ended, and the second optimal planning path is used as the target planning path;

[0034] When the second optimal planned path does not meet the length convergence condition, return to the step of randomly sampling path points from the starting point position to the target point position.

[0035] As one implementation manner, the fusing the first optimal planned path and the current planned path to obtain a second optimal planned path includes:

[0036] Determine the midpoint of the line connecting the two sampling points whose distance between the first optimal planned path and the current planned path is less than a preset threshold;

[0037] The midpoint is set as the intersection of the first optimal planned path and the current planned path, and the two sampling points whose distance is less than a preset threshold are eliminated;

[0038] Obtaining a first subpath from the starting point to the intersection of the first optimal planned path, a second subpath from the intersection to the target point of the first optimal planned path, a third subpath from the starting point to the intersection of the current planned path, and a fourth subpath from the intersection to the target point of the current planned path;

[0039] The shorter path among the first subpath and the third subpath is merged with the shorter path among the second subpath and the fourth subpath to obtain the second optimal planned path.

[0040] As one implementation manner, after determining the target planning path, the method further includes:

[0041] Determine the target location according to the target planning path;

[0042] Performing closed-loop control on the movement position of the boom according to the current position of the boom and the target position;

[0043] According to the closed-loop control, output a target speed;

[0044] The movement speed of the boom is controlled in a closed loop according to the current speed of the boom and the target speed.

[0045] In a second aspect, the present application provides an electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein when the processor executes the computer program, the steps of the arm path planning method described in the first aspect are implemented.

[0046] In a third aspect, the present application provides a computer storage medium, in which a computer program is stored. When the computer program is executed by a processor, the steps of the arm path planning method described in the first aspect are implemented.

[0047] The present application provides a boom path planning method, electronic device and computer storage medium, the method comprising: obtaining the starting point position and the target point position; starting from the starting point position, randomly sampling the path points to the target point position; when it is detected that the sampling point collides with an obstacle, adjusting the sampling angle according to the obstacle information and re-sampling, so that the sampling point avoids the obstacle; the current planning path determined according to the sampling point searched this time is merged with the first optimal planning path determined according to the historical sampling point to determine the target planning path. The technical solution of the present application first obtains the starting point position and the target point position, and then randomly samples the path points from the starting point position to the target point position, and when it is detected that the sampling point collides with an obstacle, adjusting the sampling angle according to the obstacle information and re-sampling, so that the sampling point avoids the obstacle, and finally the current planning path determined according to the sampling point searched this time is merged with the first optimal planning path determined according to the historical sampling point to determine the target planning path, thereby improving the rationality of path planning, and improving the efficiency of path planning and hoisting construction efficiency. BRIEF DESCRIPTION OF THE DRAWINGS

[0048] In order to more clearly illustrate the technical solutions of the embodiments of the present application, 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 application 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.

[0049] Figure 1 A schematic diagram of a process flow of a boom path planning method provided in an embodiment of the present application;

[0050] Figure 2 A schematic diagram of a spatial path search area provided in an embodiment of the present application;

[0051] Figure 3 A schematic diagram of a spatial path search area constrained by winch motion characteristics provided in an embodiment of the present application;

[0052] Figure 4 A schematic diagram of a spatial path search area constrained based on variable amplitude motion characteristics provided in an embodiment of the present application;

[0053] Figure 5 A schematic diagram of the angles required to rotate in the horizontal plane to avoid obstacles when sampling from the previous level sampling point to the target point position provided in an embodiment of the present application;

[0054] Figure 6 A schematic diagram of path fusion provided in an embodiment of the present application;

[0055] Figure 7A schematic diagram of the structure of an electronic device provided in an embodiment of the present application. DETAILED DESCRIPTION

[0056] The specific embodiments of the present application will be described in detail below in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present application, rather than all of the embodiments. Based on the description of the present application, all other embodiments obtained by ordinary technicians in the field without making creative work are within the scope of protection of the present application.

[0057] It should be noted that, in this article, the terms "include", "comprises" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also includes other elements not explicitly listed, or also includes elements inherent to such process, method, article or device. In the absence of further restrictions, an element defined by the sentence "includes a ..." does not exclude the existence of other identical elements in the process, method, article or device including the element. In addition, components, features, and elements with the same name in different embodiments of the present application may have the same meaning or different meanings, and their specific meanings need to be determined by their explanation in the specific embodiment or further combined with the context of the specific embodiment.

[0058] It should be understood that, although the terms first, second, third, etc. may be used to describe various information in this article, these information should not be limited to these terms. These terms are only used to distinguish the same type of information from each other. For example, without departing from the scope of this article, the first information may also be referred to as the second information, and similarly, the second information may also be referred to as the first information. Depending on the context, the word "if" as used herein can be interpreted as "at the time of" or "when" or "in response to determination". Furthermore, as used in this article, the singular forms "one", "one" and "the" are intended to also include plural forms, unless there is an opposite indication in the context. It should be further understood that the terms "comprising" and "including" indicate the presence of the described features, steps, operations, elements, components, projects, kinds, and / or groups, but do not exclude the presence, occurrence or addition of one or more other features, steps, operations, elements, components, projects, kinds, and / or groups. The terms "or" and "and / or" used herein are interpreted as inclusive, or mean any one or any combination. Thus, “A, B, or C” or “A, B and / or C” means “any of the following: A; B; C; A and B; A and C; B and C; A, B, and C.” An exception to this definition will occur only when a combination of elements, functions, steps, or operations are inherently mutually exclusive in some manner.

[0059] It should be understood that, although the various steps in the flowchart in the embodiment of the present application are displayed in sequence according to the indication of the arrows, these steps are not necessarily performed in sequence according to the order indicated by the arrows. Unless there is a clear explanation in this article, the execution of these steps does not have a strict order restriction, and it can be performed in other orders. Moreover, at least a portion of the steps in the figure may include a plurality of sub-steps or a plurality of stages, and these sub-steps or stages are not necessarily performed at the same time, but can be performed at different times, and their execution order is not necessarily performed in sequence, but can be performed in turn or alternately with other steps or at least a portion of the sub-steps or stages of other steps.

[0060] It should be noted that, in this article, step codes such as S101, S102, etc. are used for the purpose of expressing the corresponding content more clearly and concisely, and do not constitute a substantial limitation on the sequence. When implementing the step, those skilled in the art may execute S102 first and then S101, etc., but these should all be within the scope of protection of this application.

[0061] It should be understood that the specific embodiments described herein are only used to explain the present application and are not used to limit the present application.

[0062] See also Figure 1 , is a flow chart of a boom path planning method provided in an embodiment of the present application. Figure 1 As shown, the arm path planning method of the present application includes the following steps:

[0063] Step S101: Obtain the starting point position and the target point position;

[0064] Step S102: Starting from the starting point position, randomly sampling the path points towards the target point position;

[0065] Step S103: when it is detected that the sampling point collides with an obstacle, the sampling angle is adjusted according to the obstacle information and the sampling is performed again so that the sampling point avoids the obstacle;

[0066] Step S104: The current planned path determined according to the sampling points of this search is merged with the first optimal planned path determined according to the historical sampling points to determine the target planned path.

[0067] The technical solution of the present application first obtains the starting point position and the target point position, and then randomly samples the path points from the starting point position to the target point position. When it is detected that the sampling point collides with an obstacle, the sampling angle is adjusted according to the obstacle information and the sampling is re-performed to make the sampling point avoid the obstacle. Finally, the current planned path determined according to the sampling points of this search is merged with the first optimal planned path determined according to the historical sampling points to determine the target planned path, thereby improving the rationality of the path planning and improving the efficiency of the path planning and the lifting construction efficiency.

[0068] In one embodiment, before randomly sampling the path points from the starting point to the target point, the method further includes:

[0069] Determine the spatial path search area based on the starting point position, target point position, obstacle information, and the arm's joint obstacle avoidance strategy;

[0070] Starting from the starting point, random sampling of path points is performed toward the target point, including:

[0071] Starting from the starting point position, random sampling of path points is performed in the spatial path search area toward the target point position.

[0072] In one embodiment, the spatial path search area is determined according to the starting point position, the target point position, the obstacle information and the joint obstacle avoidance strategy of the arm, including:

[0073] Determine whether there is an obstacle between the starting point and the target point based on the obstacle information;

[0074] If there is no obstacle, a regular spatial area is determined as the spatial path search area according to the starting point position and the target point position;

[0075] If there are obstacles, a regular spatial area is determined as the initial search area according to the starting point position and the target point position. Based on the arm's joint obstacle avoidance strategy and obstacle information, the initial search area is constrained to determine the spatial path search area.

[0076] In one embodiment, the joint obstacle avoidance strategy includes a rotation obstacle avoidance strategy, a winch obstacle avoidance strategy, and a luffing obstacle avoidance strategy. Based on the joint obstacle avoidance strategy of the boom and obstacle information, a spatial path search area is determined, including:

[0077] When it is determined that the obstacle can be crossed based on the winch obstacle avoidance strategy and the obstacle information, the search direction is determined to be upward;

[0078] When the obstacle is judged to be insurmountable based on the winch obstacle avoidance strategy and obstacle information, the search direction is determined based on the rotation obstacle avoidance strategy and the amplitude variation obstacle avoidance strategy;

[0079] Determine the spatial path search area according to the search direction.

[0080] In one embodiment, determining the search direction according to the slewing obstacle avoidance strategy and the luffing obstacle avoidance strategy includes:

[0081] Based on the slewing obstacle avoidance strategy, determine the slewing search direction according to the azimuth relationship between the target point position and the obstacle;

[0082] In the slewing search direction, if there is an obstacle, then based on the luffing obstacle avoidance strategy, determine the luffing search direction according to the magnitude relationship between the distance from the target point position to the slewing center of the boom and the distance from the current search position to the slewing center of the boom.

[0083] Here, according to the obstacle information, the starting point position, and the target point position, the construction scene type is divided into two cases: with obstacle interference and without obstacle interference. According to the two cases of whether there is obstacle interference, the execution joint movement and the spatial path search area are constrained and limited to avoid the large-scale invalid growth of path points, and improve the efficiency of path search, the rationality of path planning, and the hoisting construction efficiency.

[0084] Specifically, when there is no obstacle between the starting point position and the target point position, a regular area can be determined as the spatial path search area. This regular area can be a spatial area of any shape. For example, it can be an ellipsoidal area with the starting point and the target point as the foci, or it can be a cuboid area containing the starting point and the target point. In this embodiment, the ellipsoidal area is taken as an example. As Figure 2 shown, first, take the hoisting point position (i.e., the starting point position) and the target point position as the foci. According to the hoisting point position coordinates (x a , y a , z a ) and the target point position coordinates (x b , y b , z b ), calculate the focal length F of the ellipsoid to be searched, that is: Then calculate the major semi-axis a1 - k*F of the ellipsoid, where k is a coefficient set according to the distance between the hoisting point position and the target point position. For example, 1.1 < k < 1.6. Then, according to the relationship formula a1 2 - b1 2 = F 2 calculate the minor semi-axis of the ellipsoid and the middle semi-axis 0 ≤ c1 ≤ h 臂头高 / 2, where h 臂头高 represents the maximum height from the ground to the head winch of the boom. According to the standard equation of the ellipsoid (In the basis, a1>b1>0, a1>c1>0), the calculated ellipsoid area is determined to be the spatial path search area in the scene without obstacle interference.

[0085] When there is an obstacle between the lifting point and the target point, the spatial path search area in the above-mentioned obstacle-free interference scenario is used as the initial spatial path search area. Under the condition of avoiding the reverse motion of the execution joints (winch mechanism, slewing mechanism, and luffing mechanism) as much as possible, the initial spatial path search area is processed to avoid obstacles based on the motion characteristics of the execution joints of the boom, so as to achieve the purpose of further constraining the spatial path search area.

[0086] Specifically, let the obstacle height be H 障碍物 , the maximum lifting height of the boom winch is H, the height of the sling and the load is h, and the current height of the load relative to the vehicle plane is H 吊物 , the height safety threshold is h 阈值 , when H 障碍物 <H-h-H 吊物 -h 阈值 When the boom hoisting winch can cross over the obstacle, the boom hoisting winch search is planned in the spatial path search area according to the hoisting motion characteristics of the boom. The spatial path search area is constrained to be the area facing the front and upward of the obstacle, such as Figure 3 As shown, the diameter and height of the upward search columnar area can be adaptively set according to actual application requirements such as obstacle information and current hanging object information.

[0087] When H 障碍物 ≥HhH 吊物 -h 阈值 When the boom hoisting winch cannot cross over the obstacle, the boom is planned to rotate in the spatial path search area according to the swing motion characteristics of the boom, and search around the left and right sides facing the obstacle (assuming that the obstacles can be passed through), and judge whether the target point is on the left or right side facing the obstacle (which can be determined according to the horizontal distance and direction). If it is on the left side, the swing search direction is determined to search in the left area facing the obstacle, and the spatial path search area is constrained to the left area facing the obstacle; if it is on the right side, the swing search direction is determined to search in the right area facing the obstacle, and the spatial path search area is constrained to the right area facing the obstacle. Here, the minimum height of the search area is ≥ the height of the target point to prevent collision.

[0088] If there is an obstacle in the rotation search direction, that is, an obstacle is encountered when searching the area to the left of the obstacle or the area to the right of the obstacle, then the distance from the target point position to the boom's rotation center and the distance from the current search position to the boom's rotation center are used to determine whether the target point is in a direction away from the boom's rotation center or in a direction close to the boom's rotation center. If the distance from the target point position to the boom's rotation center is greater than the distance from the current search position to the boom's rotation center, then the target point is determined to be in a direction away from the boom's rotation center, and the amplitude search direction is determined to be a falling amplitude search, and the spatial path search area is constrained to an area away from the boom's rotation center; if the distance from the target point position to the boom's rotation center is less than the distance from the current search position to the boom's rotation center, then the target point is determined to be in a direction close to the boom's rotation center, and the amplitude search direction is determined to be a starting amplitude search, and the spatial path search area is constrained to an area close to the boom's rotation center. Figure 4 As shown in the figure, O is the rotation center of the boom, b is the target point position, and c is the current search position. It can be clearly seen that the distance from point O to point b is greater than the distance from point O to point c. Therefore, it is determined that the target point is in the direction away from the rotation center of the boom, and the amplitude search direction is determined to be a drop amplitude search, and the spatial path search area is constrained to be an area away from the rotation center of the boom. In particular, if the amplitude required for the drop amplitude search is greater than the maximum amplitude of the current working condition of the boom, the amplitude search direction needs to be replanned.

[0089] When H 障碍物 ≥HhH 吊物 -h 阈值 That is, when the boom hoisting winch cannot cross over the obstacle and the width of the area between adjacent obstacles is less than the sum of the maximum width of the current hoisted object and the preset width threshold, it indicates that there is no spatial path search area.

[0090] In this way, according to the distribution of obstacles in the construction scene and the actual obstacle avoidance strategy based on the arm's execution joint motion characteristics, the constraints of the spatial path search area are set. On the one hand, the lifting path planning is consistent with the joint obstacle avoidance strategy, which improves the rationality of the path planning. On the other hand, the search for path points has a priori search direction, which improves the efficiency of path planning and lifting construction efficiency.

[0091] In one embodiment, when it is detected that a sampling point collides with an obstacle, the sampling angle is adjusted according to the obstacle information and the sampling is performed again so that the sampling point avoids the obstacle, including:

[0092] When a collision between a sampling point and an obstacle is detected, the angle required to rotate from the previous sampling point to the target point to avoid the obstacle is calculated based on the spatial coordinates, orientation, and size of the bounding box of the obstacle.

[0093] Resample based on angle so that the sampling points avoid obstacles within the spatial path search area.

[0094] In one embodiment, when a collision between a sampling point and an obstacle is detected, the angle required to rotate from the previous level sampling point to the target point position to avoid the obstacle is calculated based on the spatial coordinates, orientation, and size of the bounding box of the obstacle, including at least one of the following:

[0095] According to the vertical distance and horizontal edge distance from the previous sampling point to the bounding box, calculate the angle required to rotate in the horizontal plane to avoid obstacles when sampling from the previous sampling point to the target point position;

[0096] According to the vertical distance from the previous sampling point to the bounding box and the distance to the top edge, the angle required to rotate in the vertical plane to avoid obstacles when sampling from the previous sampling point to the target point is calculated.

[0097] Optionally, the RRT (Rapid Random Tree) algorithm can be used for random sampling. The RRT algorithm is an efficient path planning algorithm that randomly samples a state in the entire state space and guides the tree to expand to the state. When a new sampling point is expanded using the RRT algorithm, the RRT algorithm calculates the distance d from the sampling point to the target point. If d is greater than the set threshold r, the algorithm continues to search for the target point, otherwise the search ends and the path point generation is finally completed.

[0098] This embodiment improves the RRT algorithm, and improves the growth trend and collision interference detection of the sampling point search under the constraint condition of the spatial path search area based on the obstacle avoidance strategy.

[0099] According to the initially set spatial path search area, starting from the starting point position, random sampling is performed in the spatial path search area to determine the sampling point. In order to prevent the sampling point from being invalid, a circular verification area for the sampling point can be set. Only one sampling point needs to be determined in each circular verification area to reduce the computational complexity of path planning and improve the efficiency of path planning. Here, the determined sampling point is taken as the center of the circle, and the radius r is preset according to the actual application requirements to determine whether the sampling point is in the verification area. If the sampling point is detected in the verification area, it is confirmed as an invalid growth point and removed, otherwise it is retained to ensure that the sampling point can grow to the target point position based on the preset prior area. In this way, the invalid growth point is eliminated, the direction search of the random sampling point can be avoided, the computational cost caused by the invalid growth of the sampling point is reduced, and the efficiency of path planning is improved.

[0100] Since only the direction of the path search is constrained according to the obstacle information when determining the spatial path search area for obstacle avoidance, and there is no constraint on how to accurately avoid obstacles, when random sampling is performed in the spatial path search area, there may still be a situation where the sampling point collides with the obstacle, or the subpath between two adjacent sampling points is interfered by the obstacle. If an obstacle is encountered during random sampling in the spatial path search area, in order to pass through the adjacent obstacles and pass through the narrow safety area, when the random growth point detection collides with the obstacle, random sampling is not performed directly in the search area, but according to the spatial position point coordinates, orientation and length, width and height size information of the bounding box of the obstacle. obs =(Λ(x0,y0,z0,L0,W0,H0), B(x1,y1,z1,L1,W1,H1),...,N(x n ,y n , z n , L n , W n , H n ))(O obs is a set of obstacles), the angle θ that needs to be rotated is calculated towards the target point at the previous level sampling point, and re-sampling is performed based on the angle θ so that the sampling point avoids obstacles in the spatial path search area. Here, some bounding box algorithm results will contain both orientation and size information, such as OBB bounding box, and some algorithms will not contain orientation, such as AABB bounding box, so the orientation can be determined based on the vertex coordinates of the bounding box, or it can be obtained through the bounding box information. Figure 5 As shown, q i Indicates the previous level sampling point, qtarge t represents the target point, the black square represents the obstacle bounding box, the dotted line represents the xOy plane, and θ represents the angle of rotation in the horizontal plane to avoid obstacles when sampling from the previous sampling point to the target point.

[0101] When the sampling point is made to go around the obstacle laterally, the angle θ can be calculated according to the following formula:

[0102]

[0103] Among them, d i Indicates the lateral edge distance from the previous sampling point to the obstacle bounding box, T i It represents the vertical distance from the previous sampling point to the obstacle bounding box, and ε represents the safety distance threshold.

[0104] When the sampling point is made to span around the obstacle, the angle θ can be calculated according to the following formula:

[0105]

[0106] Among them, h i Indicates the distance from the previous sampling point to the top edge of the obstacle bounding box, T i It represents the vertical distance from the previous sampling point to the obstacle bounding box, and ε represents the safety distance threshold.

[0107] In one embodiment, the current planned path determined according to the sampling points of the current search is merged with the first optimal planned path determined according to the historical sampling points to determine the target planned path, including:

[0108] The first optimal planned path and the current planned path are merged to obtain a second optimal planned path, wherein the length of the second optimal planned path is less than or equal to the length of the first optimal planned path and the current planned path;

[0109] When the second optimal planning path meets the length convergence condition, the sampling is ended, and the second optimal planning path is used as the target planning path;

[0110] When the second optimal planned path does not meet the length convergence condition, return to the step of randomly sampling path points from the starting point position to the target point position.

[0111] In one implementation, the first optimal planned path and the current planned path are merged to obtain a second optimal planned path, including:

[0112] Determine the midpoint of a line connecting two sampling points whose distances between the first optimal planned path and the current planned path are less than a preset threshold;

[0113] The midpoint is set as the intersection of the first optimal planned path and the current planned path, and two sampling points whose distance is less than a preset threshold are eliminated;

[0114] Obtain a first subpath from the starting point to the intersection of the first optimal planned path, a second subpath from the intersection to the target point of the first optimal planned path, a third subpath from the starting point to the intersection of the current planned path, and a fourth subpath from the intersection to the target point of the current planned path;

[0115] The shorter path among the first subpath and the third subpath is merged with the shorter path among the second subpath and the fourth subpath to obtain a second optimal planned path.

[0116] Here, path fusion refers to fusing multiple planned paths together to obtain a better path that is shorter than the two planned paths. In each path search iteration, the current planned path is fused with the current optimal path (i.e., the first optimal planned path) to obtain a better planned path. After obtaining the current optimal path, perform another search, and continue to fuse the planned path of this search with the current optimal path to obtain a new optimal path, and so on. When the current optimal path meets the length convergence condition, the sampling is ended, and the current optimal planned path is used as the target planned path; when the current optimal path does not meet the length convergence condition, it is further determined whether the conditions for ending sampling are met. If so, the sampling is ended. If not, the step of randomly sampling the path points from the starting point to the target point is returned, wherein the conditions for ending sampling include the sampling time reaching the preset time and the number of searched paths reaching the preset number, i.e., forced exit of sampling to ensure the efficiency of sampling.

[0117] Path fusion may specifically include the following steps:

[0118] Step 1: Calculate the intersection of the first optimal planned path and the current planned path, and divide the two paths into multiple sub-paths. Figure 6 As shown, assuming that the first optimal planning path (P optimal ) and the current planned path (P new ) do not completely overlap. If the distance between the points q1 and q2 corresponding to the two paths is less than the preset threshold, they are considered to overlap and the midpoint q mid Set to the intersection of the first optimal planned path and the current planned path.

[0119] Step 2: Select a subpath from the first optimal planning path and the current planning path to connect each intersection. Figure 6 As shown, P new Connect start and q near The subpath ratio of P optimal Connect start and q near The subpath is short, so use P new The subpaths of q start and q near Similarly, P optimal Connect near and q goal The subpath ratio of P new Connect near and q goal The subpath is short, so use P optimal The subpaths of q near and q goal , where q startrepresents the starting point of the planned path, q goal represents the end point of the planned path. In this way, two planned paths can be merged into a shorter path than the original path. By continuously merging the current optimal path with the newly generated random path, an optimal path can be quickly obtained.

[0120] In one embodiment, after determining the target planned path, it further includes:

[0121] Determine the target position according to the target planned path;

[0122] Perform closed-loop control on the movement position of the boom according to the current position and the target position of the boom;

[0123] Output the target speed according to the closed-loop control;

[0124] Perform closed-loop control on the movement speed of the boom according to the current speed and the target speed of the boom.

[0125] Here, according to the target planned path, the target planned path includes the time information corresponding to the movement position of the boom, that is, a curve with time on the X-axis and position on the Y-axis.

[0126] Send the target position of the boom, that is, the path point, to the controller at a certain frequency. The controller performs closed-loop control on the movement position of the boom with the target position and the current position feedback by the encoder. The position closed-loop control will output a target speed. The controller performs closed-loop control on the movement speed of the boom with the target speed and the current speed feedback by the encoder.

[0127] The speed closed-loop control will output a current control value. This current control value is processed by the acceleration and deceleration ramp, emergency stop module, and speed grading module in the controller and finally input to the current loop of the controller. The actual current of the current loop is Ir, and the target current is Ia. The current loop performs closed-loop control with Ir and Ia. When |Ir - Ia| < X, output Ir to make the execution joint move, so that the boom moves along the ideal path.

[0128] In this way, by adopting the speed and position double closed-loop control strategy, the execution joint of the boom tracks the speed and acceleration more smoothly, and the position control is more accurate, realizing the accurate and smooth tracking control of the path.

[0129] Based on the same inventive concept as the foregoing embodiments, an embodiment of the present application provides an electronic device, as Figure 7 shown, the electronic device includes: a processor 110 and a memory 111 for storing a computer program that can run on the processor 110; wherein, Figure 7The processor 110 shown in the figure is not used to indicate that the number of the processor 110 is one, but is only used to indicate the positional relationship of the processor 110 relative to other devices. In actual applications, the number of the processor 110 may be one or more; similarly, Figure 7 The memory 111 shown in the figure has the same meaning, that is, it is only used to refer to the position relationship of the memory 111 relative to other devices. In practical applications, the number of memories 111 can be one or more. The processor 110 is used to implement the arm path planning method when running the computer program.

[0130] The electronic device may also include: at least one network interface 112. The various components in the electronic device are coupled together via a bus system 113. It is understood that the bus system 113 is used to achieve connection and communication between these components. In addition to the data bus, the bus system 113 also includes a power bus, a control bus, and a status signal bus. However, for the sake of clarity, in Figure 7 Various buses are labeled as bus system 113 .

[0131] The memory 111 may be a volatile memory or a nonvolatile memory, or may include both volatile and nonvolatile memories. The nonvolatile memory may be a read-only memory (ROM), a programmable read-only memory (PROM), an erasable programmable read-only memory (EPROM), an electrically erasable programmable read-only memory (EEPROM), a ferromagnetic random access memory (FRAM), a flash memory, a magnetic surface memory, an optical disk, or a compact disc read-only memory (CD-ROM); the magnetic surface memory may be a disk memory or a tape memory. The volatile memory may be a random access memory (RAM), which is used as an external cache. By way of example and not limitation, many forms of RAM are available, such as static random access memory (SRAM), synchronous static random access memory (SSRAM), dynamic random access memory (DRAM), synchronous dynamic random access memory (SDRAM), double data rate synchronous dynamic random access memory (DDRSDRAM), enhanced synchronous dynamic random access memory (ESDRAM), synchronous link dynamic random access memory (SLDRAM, SyncLink Dynamic Random Access Memory), and direct RAM bus random access memory (DRRAM, Direct Rambus Random Access Memory).The memory 111 described in the embodiments of the present application is intended to include but is not limited to these and any other suitable types of memories.

[0132] The memory 111 in the embodiment of the present application is used to store various types of data to support the operation of the electronic device. Examples of these data include: any computer program used to operate on the electronic device, such as an operating system and an application; contact data; phone book data; messages; pictures; videos, etc. Among them, the operating system includes various system programs, such as a framework layer, a core library layer, a driver layer, etc., which are used to implement various basic services and handle hardware-based tasks. The application program may include various applications, such as a media player (Media Player), a browser (Browser), etc., for implementing various application services. Here, the program that implements the method of the embodiment of the present application may be included in the application program.

[0133] Based on the same inventive concept as the above-mentioned embodiment, this embodiment further provides a computer storage medium, in which a computer program is stored. The computer storage medium may be a ferromagnetic random access memory (FRAM), a read-only memory (ROM), a programmable read-only memory (PROM), an erasable programmable read-only memory (EPROM), an electrically erasable programmable read-only memory (EEPROM), a flash memory, a magnetic surface memory, an optical disc, or a compact disc read-only memory (CD-ROM); it may also be various devices including one or any combination of the above-mentioned memories, such as a mobile phone, a computer, a tablet device, a personal digital assistant, etc. When the computer program stored in the computer storage medium is executed by the processor, the above-mentioned arm path planning method is implemented. For the specific steps implemented when the computer program is executed by the processor, please refer to Figure 1 The description of the illustrated embodiment will not be repeated here.

[0134] The technical features of the above-described embodiments may be arbitrarily combined. To make the description concise, not all possible combinations of the technical features in the above-described embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0135] In this document, the terms "comprises," "comprising," or any other variations thereof, are intended to cover a non-exclusive inclusion of elements other than those listed and may also include additional elements not expressly listed.

[0136] The above is only a specific implementation of the present application, but the protection scope of the present application is not limited thereto. Any person skilled in the art who is familiar with the present technical field can easily think of changes or substitutions within the technical scope disclosed in the present application, which should be included in the protection scope of the present application. Therefore, the protection scope of the present application shall be subject to the attached claims.

Claims

1. A boom path planning method, characterized in that: The method comprises: Get the starting point position and the target point position; Starting from the starting point position, randomly sampling path points toward the target point position; When it is detected that a sampling point collides with an obstacle, the sampling angle is adjusted according to the obstacle information and the sampling is performed again so that the sampling point avoids the obstacle; The current planned path determined according to the sampling points of this search is merged with the first optimal planned path determined according to the historical sampling points to determine the target planned path.

2. The boom path planning method according to claim 1, characterized in that: Before randomly sampling the path points from the starting point to the target point, the method further includes: Determine a spatial path search area according to the starting point position, the target point position, the obstacle information, and the joint obstacle avoidance strategy of the arm; The step of performing random sampling of path points from the starting point to the target point includes: Starting from the starting point position, random sampling of path points is performed in the spatial path search area toward the target point position.

3. The boom path planning method according to claim 2, characterized in that: The determining of the spatial path search area according to the starting point position, the target point position, the obstacle information and the joint obstacle avoidance strategy of the arm comprises: Determine whether there is an obstacle between the starting point and the target point according to the obstacle information; If there is no obstacle, determining a regular spatial area as the spatial path search area according to the starting point position and the target point position; If there is an obstacle, a regular spatial area is determined as the initial search area according to the starting point position and the target point position, and the initial search area is constrained based on the joint obstacle avoidance strategy of the arm and the obstacle information to determine the spatial path search area.

4. The boom path planning method according to claim 3, characterized in that: The joint obstacle avoidance strategy includes a rotation obstacle avoidance strategy, a winch obstacle avoidance strategy, and a variable amplitude obstacle avoidance strategy. The joint obstacle avoidance strategy based on the boom and the obstacle information constrains the initial search area to determine the space path search area, including: When it is determined that the obstacle can be crossed according to the winch obstacle avoidance strategy and the obstacle information, determining the search direction to be upward; When it is determined that the obstacle is insurmountable according to the winch obstacle avoidance strategy and the obstacle information, the search direction is determined according to the rotation obstacle avoidance strategy and the amplitude variation obstacle avoidance strategy; The spatial path search area is determined according to the search direction.

5. The boom path planning method according to claim 4, wherein the step of determining the search direction according to the rotation obstacle avoidance strategy and the amplitude variation obstacle avoidance strategy comprises: According to the azimuth relationship between the target point position and the obstacle, determining the rotation search direction based on the rotation obstacle avoidance strategy; If there is an obstacle in the rotation search direction, the variable amplitude search direction is determined based on the variable amplitude obstacle avoidance strategy according to the relationship between the distance from the target point position to the rotation center of the boom and the distance from the current search position to the rotation center of the boom.

6. The boom path planning method according to claim 1, characterized in that: When it is detected that the sampling point collides with an obstacle, adjusting the sampling angle according to the obstacle information and re-sampling so that the sampling point avoids the obstacle includes: When it is detected that the sampling point collides with the obstacle, the angle required to rotate from the previous level sampling point to the target point position to avoid the obstacle is calculated according to the spatial coordinates, orientation and size of the bounding box of the obstacle; Re-sampling is performed based on the angle so that the sampling points avoid obstacles in the spatial path search area.

7. The boom path planning method according to claim 6, characterized in that: When the collision between the sampling point and the obstacle is detected, the angle required to rotate from the previous sampling point to the target point position to avoid the obstacle is calculated according to the spatial coordinates, orientation and size of the bounding box of the obstacle, including at least one of the following: According to the vertical distance and the horizontal edge distance from the previous level sampling point to the bounding box, calculate the angle that needs to be rotated in the horizontal plane to avoid the obstacle when sampling from the previous level sampling point to the target point position; According to the vertical distance and the top edge distance from the previous sampling point to the bounding box, the angle required to rotate in the vertical plane to avoid the obstacle when sampling from the previous sampling point to the target point position is calculated.

8. The boom path planning method according to claim 1, characterized in that: The step of fusing the current planned path determined according to the sampling points of the current search with the first optimal planned path determined according to the historical sampling points to determine the target planned path includes: The first optimal planned path and the current planned path are merged to obtain a second optimal planned path, wherein the length of the second optimal planned path is less than or equal to the length of the first optimal planned path and the current planned path; When the second optimal planning path meets the length convergence condition, the sampling is ended, and the second optimal planning path is used as the target planning path; When the second optimal planned path does not meet the length convergence condition, return to the step of randomly sampling path points from the starting point position to the target point position.

9. The boom path planning method according to claim 8, characterized in that: The fusing the first optimal planned path and the current planned path to obtain a second optimal planned path includes: Determine the midpoint of the line connecting two sampling points whose distance between the first optimal planned path and the current planned path is less than a preset threshold; The midpoint is set as the intersection of the first optimal planned path and the current planned path, and the two sampling points whose distance is less than a preset threshold are eliminated; Obtaining a first subpath from the starting point to the intersection of the first optimal planned path, a second subpath from the intersection to the target point of the first optimal planned path, a third subpath from the starting point to the intersection of the current planned path, and a fourth subpath from the intersection to the target point of the current planned path; The shorter path among the first subpath and the third subpath is merged with the shorter path among the second subpath and the fourth subpath to obtain the second optimal planned path.

10. The boom path planning method according to claim 8, characterized in that: After determining the target planning path, the method further includes: Determine the target location according to the target planning path; Performing closed-loop control on the movement position of the boom according to the current position of the boom and the target position; According to the closed-loop control, output a target speed; The movement speed of the boom is controlled in a closed loop according to the current speed of the boom and the target speed.

11. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein: When the processor executes the computer program, the steps of the boom path planning method according to any one of claims 1 to 10 are implemented.

12. A computer storage medium storing a computer program, characterized in that: When the computer program is executed by a processor, the steps of the boom path planning method according to any one of claims 1 to 10 are implemented.

Images