Path planning method, control system and self-moving equipment

Abstract

The embodiment of the invention discloses a path planning method, a control system and self-moving equipment. The method comprises the following steps: determining a task starting point and a task ending point of a task to be executed by the mobile equipment; performing path planning for the task to obtain an initial path; sub-paths which do not meet preset constraint conditions in the initial path are determined; re-planning the sub-paths which do not meet the preset constraint condition to obtain re-planned sub-paths; and splicing the sub-paths meeting a preset constraint condition in the initial path and the re-planned sub-paths to obtain a task path. On one hand, automatic path planning is realized, manual participation is not needed, the efficiency is improved, and the labor cost is reduced; on the other hand, path planning is conducted on the sub-paths which do not meet the preset constraint conditions again in a segmentation mode, so that more reasonable path planning is provided, and the calculation efficiency is improved as much as possible.

Description

Technical Field

[0001] The present application relates to the field of intelligent control technology, and in particular to a path planning method, a control system and a self-moving device. Background Art

[0002] Systems that use autonomous devices such as AGVs (automated guided vehicles) have the advantages of being highly unmanned, automated, and intelligent, which improves production efficiency and operational levels for industries such as warehousing, manufacturing, and logistics. As one of the more typical scenarios, autonomous devices are often responsible for loading and unloading cargo containers such as container trucks and container ships, for example, moving cargo from a temporary storage area to a container truck compartment, or taking cargo out of a container truck compartment and moving it to a temporary storage area.

[0003] Since the space of cargo container transport vehicles is limited and the docking position and docking posture of cargo container transport vehicles are different each time, in order to avoid collision, reasonable path planning of self-moving equipment has become an urgent problem to be solved. Summary of the invention

[0004] In view of this, the present application provides a path planning method, a control system and a self-moving device, so as to facilitate more reasonable path planning for the self-moving device.

[0005] This application provides the following solutions:

[0006] In a first aspect, a path planning method is provided, which is executed by a processor, and the method includes:

[0007] Determine a task start point and a task end point of a task to be performed by the mobile device;

[0008] Performing path planning for the task to obtain an initial path;

[0009] Determine a sub-path in the initial path that does not meet the preset constraint condition;

[0010] Re-planning the sub-path that does not meet the preset constraint condition to obtain a re-planned sub-path;

[0011] The sub-paths in the initial path that meet the preset constraint conditions and the re-planned sub-paths are spliced ​​to obtain a task path.

[0012] Optionally, performing path planning for the task based on the planning map to obtain an initial path includes:

[0013] Starting from the task starting point, a path segment search is performed based on a map to obtain a sequence of path segments with the shortest path to the task end point as the initial path, and the range of the map at least covers the task starting point and the task end point.

[0014] Optionally, performing path planning for the task to obtain an initial path includes:

[0015] Starting from the task starting point, a path segment search is performed based on a map to obtain a path segment sequence with the shortest path to the task end point and conforming to the constraints of the first collision detection model as the initial path.

[0016] Optionally, the first collision detection model includes a circular collision detection model.

[0017] Optionally, the path segment sequence that complies with the circular collision detection model constraint includes:

[0018] The path segments in the path segment sequence do not intersect with the target contour, and the target contour is obtained by expanding the obstacle contour by a preset length, and the preset length is the maximum inscribed circle radius of the orthographic projection of the mobile device on the ground plane.

[0019] Optionally, after obtaining the replanned sub-path, the method further includes:

[0020] Determine whether all replanned sub-paths meet the preset constraints. If so, execute the step of splicing the sub-paths in the initial path that meet the preset constraints and the replanned sub-paths; otherwise, re-plan the sub-paths in the replanned sub-paths that do not meet the preset constraints.

[0021] Optionally, the sub-path that does not meet the preset constraint condition includes at least one of the following:

[0022] subpaths that do not conform to the kinematic constraints of the self-moving device;

[0023] Subpaths that do not meet the constraints of the second collision detection model;

[0024] The subpath where the task starting point is located, the difference between the direction of the subpath where the task starting point is located and the orientation of the self-mobile device at the task starting point exceeds a first angle threshold;

[0025] The sub-path where the task end point is located, the difference between the direction of the sub-path where the task end point is located and the target orientation of the self-moving device at the task end point exceeds the second angle threshold.

[0026] Optionally, before determining the sub-path in the initial path that does not meet the preset constraint condition, the method further includes:

[0027] Determine at least one segmentation point in the initial path, and segment the initial path at the position of the at least one segmentation point to obtain a plurality of sub-paths;

[0028] The segmentation points include at least one of narrow points and position points with a preset length interval, and the narrow points are position points in the initial path where the passage width is less than or equal to a width threshold.

[0029] Optionally, the second collision detection model comprises a capsule collision detection model.

[0030] Optionally, the subpath that does not meet the constraints of the capsule collision detection model is determined in the following manner:

[0031] When the self-moving device is simulated as a capsule shape moving on the sub-path, the capsule shape is composed of a rectangle and semicircles at both ends of the rectangle. It is determined whether the distance between the midline of the rectangle and the obstacle outline is greater than the maximum inscribed circle radius of the rectangle. If not, it is determined that the sub-path does not conform to the capsule detection model.

[0032] Optionally, before determining whether the distance between the midline of the rectangle and the obstacle outline is greater than the maximum inscribed circle radius of the rectangle, the method further includes:

[0033] It is determined whether the distance between the midpoint of the rectangle and the obstacle contour is greater than the radius of the circumscribed circle of the rectangle. If so, it is determined that the subpath conforms to the capsule detection model.

[0034] Optionally, determining whether a distance between a midline of the rectangle and an obstacle contour is greater than a maximum inscribed circle radius of the rectangle, and if not, determining that the subpath does not conform to the capsule detection model includes:

[0035] It is determined whether the midline of the rectangle intersects with the target contour. If so, it is determined that the subpath does not conform to the capsule collision detection model. The target contour is obtained by expanding the obstacle contour by a preset length, and the preset length is the maximum inscribed circle radius.

[0036] Optionally, re-planning the sub-path that does not meet the preset constraint condition to obtain the re-planned sub-path includes:

[0037] Starting from the starting point of the sub-path that does not meet the preset constraint condition, searching for path segments based on the map, obtaining a sequence of path segments that reaches the end point of the sub-path and has the minimum cost function value as the re-planned sub-path, wherein the range of the map at least covers the task starting point and the task end point;

[0038] The value of the cost function is obtained based on one or any combination of the total length of the sub-path, the change in orientation between path segments, the number of cusps, the distance between the path segment and the obstacle, and the change in the steering angle between the path segments, and the cusp is the switching position between forward and backward.

[0039] Optionally, during the search process, if the distance between the current position searched and the end point of the sub-path is less than or equal to a preset distance threshold, the current position and the end point of the sub-path are connected using an RS curve, and the searched path segment and the curve obtained by connecting using the RS curve are spliced ​​into a re-planned sub-path.

[0040] Optionally, the step of splicing the searched path segment and the curve obtained by connecting the RS curve into a re-planned sub-path includes:

[0041] If the use of the RS curve to connect the current position with the end point of the sub-path fails, then trace back a preset distance from the current position according to the searched path segment sequence, take the traced position as the current position, and proceed to the step of connecting the current position with the end point of the sub-path using the RS curve until the backtracking end condition is met;

[0042] If the use of the RS curve to connect the current position and the end point of the sub-path is successful, the searched path segment sequence and the curve obtained by connecting using the RS curve are spliced ​​into the re-planned sub-path.

[0043] Optionally, the preset forward tracing back distance includes:

[0044] The distance corresponding to a preset number of action nodes is traced back forward, where the action nodes are movement actions performed by the self-moving device on the searched path segment sequence.

[0045] Optionally, the backtracking end condition includes:

[0046] The total number of backtracked action nodes reaches the preset number threshold.

[0047] Optionally, the task is a handling task, and one of the task start point and the task end point is a storage location on a cargo container transportation vehicle;

[0048] The map is obtained after the mobile device scans and maps the cargo container transportation vehicle using the sensor carried by the mobile device.

[0049] Optionally, the processor is arranged in a control system, and the method further comprises:

[0050] The task path is sent to the self-mobile device, so that the self-mobile device performs the task based on the task path.

[0051] In a second aspect, a control system is provided, comprising a processor and a memory, the memory being used to store program instructions, and the processor executing the program instructions to implement any of the methods described in the first aspect above.

[0052] In a third aspect, a self-mobile device is provided, comprising a processor and a memory, the memory being used to store program instructions, and the processor executing the program instructions to implement any of the methods described in the first aspect above.

[0053] According to the specific embodiments provided in this application, this application discloses the following technical effects:

[0054] 1) After the initial path is obtained through path planning based on the task starting point and the task end point, the present application re-plans the path for the sub-paths that do not meet the preset constraints. On the one hand, it realizes automatic path planning without manual intervention, improves efficiency and reduces labor costs; on the other hand, it adopts a "segmented" approach to re-plan the sub-paths that do not meet the preset constraints, thereby providing a more reasonable path planning and maximizing computational efficiency.

[0055] 2) When generating the initial path, the present application only searches based on the shortest path that complies with the circular collision detection model constraints, thereby improving the efficiency of path planning and reducing the computational pressure while meeting basic safety requirements.

[0056] 3) This application fully considers multiple factors such as kinematic constraints, capsule collision detection model constraints, and orientation constraints to find unreasonable sub-paths for re-path planning, thereby more comprehensively and accurately ensuring the safety and rationality of the task path.

[0057] 4) When re-planning the path for a sub-path that does not meet the preset constraints, the present application considers multi-dimensional factors such as the total length, the change in orientation between path segments, the number of sharp points, the distance between the path segment and the obstacle, and the change in the steering angle between the path segments in the cost function, and achieves a balance between these multi-dimensional factors, thereby re-planning a smoother, more stable, safer and more efficient path.

[0058] 5) In the process of replanning the sub-path, the present application can combine the RS curve to perform path repair, thereby reducing the error caused by the discretization processing in the search process and improving the smoothness of the re-planned sub-path, so that the self-moving device can move more efficiently, smoothly and stably during the task execution.

[0059] 6) This application is applicable to the use of self-mobile equipment to perform transportation tasks of cargo container transportation vehicles. The docking position and docking posture of the cargo container transportation vehicles are somewhat different each time, and an efficient, safe and reasonable task path can also be generated.

[0060] Of course, any invention of the present application does not necessarily need to achieve all of the advantages described above at the same time. BRIEF DESCRIPTION OF THE DRAWINGS

[0061] In order to more clearly illustrate the embodiments of the present application or the technical solutions in the prior art, the drawings required for use in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0062] Figure 1 is a system architecture diagram applicable to the embodiments of the present application;

[0063] Figure 2 A flow chart of a path planning method provided in an embodiment of the present application;

[0064] Figure 3 A schematic diagram of a work area for performing a handling task provided in an embodiment of the present application;

[0065] Figure 4 A schematic diagram of simulating a mobile device as a capsule provided in an embodiment of the present application;

[0066] Figure 5 An example diagram of an initial path provided in an embodiment of the present application;

[0067] Figure 6 A schematic block diagram of an electronic device provided in an embodiment of the present application. DETAILED DESCRIPTION

[0068] The following will be combined with the drawings in the embodiments of the present application to clearly and completely describe the technical solutions in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, not all of the embodiments. All other embodiments obtained by ordinary technicians in this field based on the embodiments in the present application belong to the scope of protection of this application.

[0069] 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.

[0070] 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.

[0071] 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.

[0072] At present, there are mainly the following path planning methods for self-moving equipment:

[0073] 1) The method based on manual path drawing, that is, manually drawing each path from the starting point to the end point of the transportation. This method requires drawing paths separately according to the posture of different cargo container transportation tools and the conditions of the loading space. Obviously, it will consume a lot of manpower, be inefficient, costly, and strongly rely on manual experience, and have poor flexibility.

[0074] 2) Plan the path from the task start point to the task end point based on search algorithms such as A-Star. However, the path planned in this way may not be suitable for the movement of self-mobile devices in complex environments, and its rationality still needs to be improved.

[0075] In view of this, the present application provides a new idea. In order to facilitate the understanding of the present application, the system architecture on which the present application is based is first described. Figure 1 An exemplary system architecture to which the embodiments of the present application can be applied is shown. Figure 1 As shown in , the system architecture may include: a self-moving device and a self-moving device control system.

[0076] Among them, the self-moving device refers to a tool equipped with an electromagnetic or optical automatic guidance device, controlled by a computer, with its own power or power conversion device and capable of automatically moving along a specified path. In the embodiment of the present application, the self-moving device can be able to move and undertake certain tasks, such as transportation, handling (including picking up and unloading goods), etc. For example, the automatic moving device can be an AGV (Automated Guided Vehicle), a logistics robot, an intelligent forklift, etc.

[0077] In an embodiment of the present application, a self-mobile device is installed with an on-board software system, which has a SLAM (Simultaneous Localization and Mapping) function, a map generation function for generating a planning map based on a mapping result, a motion control function for controlling the movement and operation of the self-mobile device, etc.

[0078] The mobile device control system (RCS) can be set on the server side and is responsible for scheduling each mobile device, including task allocation, path planning, instruction delivery, etc. For details, please refer to the relevant records in the subsequent embodiments.

[0079] The above-mentioned self-mobile device control system (RCS) can be set up on an independent server, a server group, or a cloud server. A cloud server, also known as a cloud computing server or cloud host, is a host product in the cloud computing service system to solve the defects of difficult management and weak service scalability in traditional physical hosts and virtual private servers (VPS) services. In addition, the above-mentioned control system can also be set up on a computer terminal with strong computing power.

[0080] It should be understood that Figure 1 The number of self-propelled devices and self-propelled device control systems (RCS) in the embodiment is only illustrative. Any number of self-propelled devices and control systems may be provided according to implementation requirements.

[0081] Figure 2 The flow chart of the path planning method provided in the embodiment of the present application can be executed by a processor, and the processor can be set to Figure 1 The RCS in the system shown can also be set in a mobile device. Figure 2 As shown in , the method may include the following steps:

[0082] Step 201: Determine a task start point and a task end point of a task to be executed by a mobile device.

[0083] Step 203: Perform path planning for the task to obtain an initial path.

[0084] Step 205: Determine the sub-paths in the initial path that do not meet the preset constraint conditions.

[0085] Step 207: re-plan the sub-paths that do not meet the preset constraint conditions to obtain re-planned sub-paths.

[0086] Step 209: The sub-paths in the initial path that meet the preset constraint conditions and the re-planned sub-paths are spliced ​​to obtain the task path.

[0087] It can be seen from the above process that after the application obtains the initial path through path planning based on the task starting point and the task end point, the path is re-planned for the sub-paths that do not meet the preset constraints. On the one hand, automatic path planning is realized without human participation, which improves efficiency and reduces labor costs; on the other hand, a "segmented" approach is used to re-plan the sub-paths that do not meet the preset constraints, thereby providing a more reasonable path planning and maximizing computational efficiency.

[0088] The following describes in detail each step in the above process and the effects that can be further produced in combination with the embodiments. First, the above step 201, namely "determining the task start point and task end point of the task to be executed by the mobile device", is described in detail in combination with the embodiments.

[0089] The method provided in this application can realize path planning for any task. As one of the more typical scenarios, the self-moving device can move and transport goods between the temporary storage area of ​​the warehouse and the loading space of the cargo container transportation vehicle without human intervention, thereby completing loading / unloading.

[0090] For ease of understanding, combined Figure 3 The above scenario is briefly introduced. Figure 3 As shown in , when the self-moving equipment is not performing work, it is usually located in its own parking position, and the parking position can be a location such as a warehouse, a carport, etc. The cargo container transportation vehicle can be a truck, a cargo ship, a flying wing vehicle, etc. The cargo container transportation vehicle has a loading space, and the loading space can be a compartment, a platform, a rack, etc. The location where the goods are placed in the loading space is called a storage location (also called a cargo location). The cargo container transportation vehicle is usually docked at a designated location on the platform, and the bridge position between the platform and the cargo container transportation vehicle is usually used for the self-moving equipment to board the container transportation vehicle, for example, it can be a boarding bridge position, a boarding bridge position, etc. When the self-moving equipment performs the handling task, it is necessary to transport the goods in each storage location in the temporary storage area to the storage location of the cargo container transportation vehicle (i.e. loading) or to transport the goods in the storage location of the cargo container transportation vehicle to the storage location in the temporary storage area (i.e. unloading).

[0091] When the staff determines that the cargo container transport vehicle has docked at the platform position, the terminal device is triggered to send a task start instruction to RCS through physical or virtual buttons on the terminal device, or input instructions, etc. RCS generates a mapping task and sends the mapping task instruction to the self-mobile device. In the process of executing the mapping task, the self-mobile device uses its own sensors (for example, it can include radars of types such as lidar, millimeter wave radar, ToF radar, and can further include visual sensors such as cameras) to scan the outside and inside of the cargo container transport vehicle, and uses the scanned point cloud map to map it on the ground plane, and performs binarization processing to obtain a planning map, and sends the planning map to RCS. RCS generates a handling task based on the planning map and sends a handling task instruction to the self-mobile device. Among them, when RCS generates a handling task based on the planning map, it involves the planning of the handling path, and the method provided in the embodiment of the present application can be used to plan the handling path.

[0092] Therefore, in this scenario, at least one of the task start point and the task end point is a storage location on the cargo container transportation vehicle. For example, for a loading task, the task start point is a storage location in the temporary storage area, and the task end point is a storage location on the cargo container transportation vehicle; for an unloading task, the task start point is a storage location on the cargo container transportation vehicle, and the task end point is a storage location in the temporary storage area.

[0093] In addition to the above-mentioned typical handling task scenarios, the embodiments of the present application are also applicable to other task scenarios.

[0094] The above step 203, namely "performing path planning for the task to obtain an initial path", is described in detail below in conjunction with an embodiment.

[0095] As one feasible method, it is possible to start from the task starting point, search for path segments based on a map, and obtain a sequence of path segments with the shortest path to the task end point and conforming to the constraints of the first collision detection model as the initial path, wherein the range of the above map at least covers the task starting point and the task end point.

[0096] When searching for path segments, a path search algorithm such as A-Star may be used.

[0097] When performing path planning in this step, a first collision detection model that does not consider constraints such as orientation and kinematics can be used for simple restrictions, so the planning speed is fast. For example, the first collision detection model can use a circular collision detection model.

[0098] In view of the fact that the A-Star algorithm is a mature path search algorithm, it will not be described in detail here. The circular collision detection model involved is described. The initial path searched in the embodiment of the present application is essentially composed of a sequence of path segments, and each path segment in the sequence of path segments needs to meet the constraints of the circular collision detection model to avoid collision with obstacles. For a self-moving device, the positive projection on the ground plane is a rectangle, and the self-moving device is simulated as the rectangle. Then, the self-moving device does not collide with obstacles during movement, which is equivalent to expanding the obstacle contour by a preset length to obtain the target contour. The preset length is the radius of the maximum inscribed circle of the self-moving device, wherein the maximum inscribed circle of the rectangle refers to a circle that is tangent to the two opposite long sides of the rectangle. If the path segments in the path segment sequence do not intersect with the target contour, the self-moving device will not collide with the obstacle when moving on the path segment.

[0099] It should be noted that the purpose of considering the circular collision detection model in the initial path is to make a rough restriction when the orientation of the self-moving device is not considered, so that an initial path can be obtained efficiently. In addition to the circular collision detection model, the circular collision detection model can also be not used, or other collision detection models can be used as the first collision detection model for constraint.

[0100] After the initial path is obtained, the initial path can be further smoothed, wherein the smoothing method may include: inflection point rounding, curve fitting, filter smoothing, curve interpolation, and solving the smooth path using the gradient descent method.

[0101] The above step 205, ie, "determining a sub-path in the initial path that does not meet the preset constraint condition", is described in detail below in conjunction with an embodiment.

[0102] This step essentially requires finding an "unreasonable" sub-path in the initial path. In the embodiment of the present application, the initial path is first segmented, that is, at least one segmentation point in the initial path is determined, and the initial path is segmented at the position of the segmentation point to obtain multiple sub-paths.

[0103] As one of the feasible segmentation methods, segmentation may be performed based on narrow points in the initial path, wherein the narrow points are positions in the initial path where the passage width is less than or equal to a first width threshold, and the first width threshold is usually determined according to the size of the self-moving device.

[0104] As another feasible segmentation method, segmentation can be performed based on the entry and exit points of the narrow segment in the initial path. The narrow segment is a path segment in the initial path whose passage width is less than or equal to a second width threshold, which is usually determined according to the size of the self-moving device. Figure 5As shown in , assuming that there is a narrow section (such as a narrow passage), the directions of the entrance position and the exit position of the narrow section are constrained, so the entrance position and the exit position of the narrow section can be used as segmentation points to segment the initial path.

[0105] As another feasible segmentation method, the initial path may be segmented based on preset length intervals, that is, position points of preset length intervals in the initial path are used as segmentation points to obtain sub-paths.

[0106] The three achievable segmentation methods mentioned above may be used one by one, or all of them may be used to segment the initial path.

[0107] The sub-paths that do not meet the preset constraints may include but are not limited to the following:

[0108] The first type: subpaths that do not comply with the kinematic constraints of the self-moving device.

[0109] Kinematic constraints refer to the physical limitations that a self-moving device needs to meet while moving along a path, such as speed, acceleration, and steering capabilities.

[0110] For example, whether the turning angle of the sub-path matches the steering capability of the self-moving device. If the turning radius of the sub-path is too small and exceeds the steering capability of the self-moving device, the sub-path does not meet the kinematic constraints of the self-moving device and is an unreasonable sub-path.

[0111] The second type: subpaths that do not meet the constraints of the second collision detection model.

[0112] In the embodiment of the present application, the second collision detection model is more restrictive than the first collision detection model, and generally further considers the orientation of the self-moving device.

[0113] The second collision detection model may adopt a capsule collision detection model. Compared with the circular collision detection model, the capsule collision detection model further considers the orientation of the self-moving device and is a more stringent constraint. When considering the constraints of the capsule collision detection model, the self-moving device may be simulated as a capsule shape moving on a subpath, such as Figure 4As shown in , the capsule shape is composed of a rectangle 41 and semicircles 42 at both ends of the rectangle. Rectangle 41 is a rectangular area obtained by projecting the self-moving device 40 on the ground plane, and the diameters of the two semicircles 42 are equal to the length of the short side of the rectangle 41. The center line 43 of the rectangle along the long side of the rectangle 41 can be understood as the orientation of the self-moving device 40 on the map plane. Therefore, it can be determined whether the distance between the center line 43 of the rectangle 41 and the obstacle outline is greater than the radius of the maximum inscribed circle 44 of the rectangle, where the radius of the maximum inscribed circle 44 refers to a circle that is tangent to both opposite long sides of the rectangle 41. If not, it is determined that the subpath does not conform to the capsule detection model, which means that the self-moving device 40 will collide with an obstacle when moving on the subpath.

[0114] As one of the feasible ways, when judging whether the distance between the center line of rectangle 41 and the obstacle contour is greater than the radius of the maximum inscribed circle 44 of the rectangle, it is possible to judge whether the center line 43 of rectangle 41 intersects with the target contour, which is obtained by expanding the obstacle contour by a preset length, and the preset length is the radius of the maximum inscribed circle 44 of the self-moving device 40. If the center line 43 of rectangle 41 intersects with the target contour, the subpath does not conform to the capsule collision detection model, which means that the self-moving device 40 will collide with the obstacle when moving on the subpath, and the subpath is unreasonable.

[0115] As one of the more preferred detection methods, when the capsule collision detection model is used for detection in the embodiment of the present application, it can first be detected whether the distance between the midpoint of the rectangle 41 and the obstacle is greater than the radius of the circumscribed circle 45 of the rectangle 41. The circumscribed circle 45 of the rectangle 41 can be a circle formed by taking the midpoint of the line connecting the two directional wheels of the self-moving device as the center and the straight-line distance between the midpoint and the farthest point on the rectangle from the center as the radius. If the distance between the midpoint of the rectangle 41 and the obstacle is greater than the radius of the circumscribed circle 45 of the rectangle 41, it can be directly determined that the self-moving device 40 will not collide with the obstacle, and the sub-path is reasonable. Otherwise, it can be further detected whether the distance between the midline 43 of the rectangle 41 and the outline of the obstacle is greater than the radius of the maximum inscribed circle 44 of the rectangle 41. If not, it is determined that the self-moving device 40 will collide with the obstacle, and the sub-path is unreasonable; if the distance between the midline 43 of the rectangle 41 and the outline of the obstacle is greater than the radius of the maximum inscribed circle 44 of the rectangle 41, it is determined that the self-moving device 40 will not collide with the obstacle, and the sub-path is reasonable.

[0116] The third type: a sub-path where the task starting point is located, wherein the difference between the direction of the sub-path where the task starting point is located and the orientation of the self-mobile device at the task starting point exceeds a first angle threshold.

[0117] That is, if the difference between the sub-path direction at the task starting point and the orientation of the mobile device at the task starting point exceeds the first angle threshold, the sub-path at the task starting point is determined to be a sub-path that does not meet the preset constraint condition.

[0118] If the orientation of the sub-path at the task starting point is different from the orientation of the self-moving device at the task starting point, it means that the self-moving device needs to adjust its orientation in order to be able to travel normally on the sub-path where the task starting point is located. However, in some cases it is difficult for the self-moving device to adjust its orientation on the spot. Therefore, it can be considered that the sub-path at the task starting point in this case is unreasonable.

[0119] The first angle threshold may be 0 or a value greater than 0.

[0120] Fourth type: a sub-path where the task endpoint is located, where the difference between the direction of the sub-path where the task endpoint is located and the target orientation of the self-mobile device at the task endpoint exceeds the second angle threshold.

[0121] That is, if the difference between the sub-path direction where the task end point is located and the target orientation of the self-mobile device at the task end point exceeds the second angle threshold, the sub-path where the task end point is located is determined to be a sub-path that does not meet the preset constraint condition.

[0122] If the orientation of the sub-path at the task endpoint is different from the target orientation of the self-moving device at the task endpoint (for example, an orientation suitable for moving goods to a target storage location in the loading space of a cargo container transport vehicle), it means that the self-moving device needs to adjust its orientation to be the target orientation at the task endpoint. However, in some cases it is difficult for the self-moving device to adjust its orientation in situ. Therefore, it can be considered that the sub-path at the task endpoint in this case is unreasonable.

[0123] The second angle threshold may be 0 or a value greater than 0.

[0124] by Figure 5Taking the initial path shown as an example, the initial path from the task start point to the task end point is constituted by the black path segment L1 in the figure. The initial path can be segmented according to the preset length interval, and because the middle part has a narrow section with a passage width less than or equal to the width threshold due to obstacles, the passage space is narrow, so it can be segmented at the entrance and exit of the narrow section. After segmentation, multiple sub-paths are obtained. The red arrow 510 indicates the direction of the self-moving device at the task start point, the orange arrow 520 indicates the direction of the initial path, and the blue arrow 530 indicates the target direction of the self-moving device at the task end point. The difference between the direction of the sub-path where the task start point is located and the direction of the self-moving device at the task start point exceeds the first angle threshold, so the sub-path where the task start point is located is determined to be a sub-segment that does not meet the preset constraints. The difference between the direction of the sub-path where the task end point is located and the target direction of the self-moving device at the task end point exceeds the second angle threshold, so the sub-path where the task end point is located is determined to be a sub-segment that does not meet the preset constraints.

[0125] The above step 207, namely "re-planning the path for the sub-path that does not meet the preset constraint condition to obtain the re-planned sub-path", is described in detail below in conjunction with the embodiments.

[0126] In this step, the path planning is re-performed for each sub-path that does not meet the preset constraints. That is, for each sub-path that does not meet the preset constraints, starting from the starting point of the sub-path, a search for path segments is performed based on the map to obtain a sequence of path segments that reaches the end point of the sub-path and has the smallest cost function value as the re-planned sub-path.

[0127] When searching for path segments as described above, the Hybrid A-Star algorithm can be used. The Hybrid A-Star algorithm is an improved path planning algorithm that is based on the traditional A-Star algorithm but adds consideration of the kinematic constraints of the self-moving device. Specifically, the Hybrid A-Star algorithm adds the dimension of the heading angle, expanding the search space from two dimensions to three dimensions. This means that when searching for path segments in the planning map, not only the position but also the direction needs to be considered, so as to better meet the kinematic constraints of the self-moving device.

[0128] Since the Hybrid A-Star algorithm adds the heading angle dimension, the amount of calculation is large, especially in complex environments, more computing resources are required. However, in the embodiment of the present application, the Hybrid A-Star algorithm is only used to re-plan the path for the sub-paths that are determined to not meet the preset constraints. While ensuring the overall rationality and safety of the path, the consumption of computing resources is reduced as much as possible to ensure computing efficiency.

[0129] In the embodiments of the present application, the Hybrid A-Star algorithm is further improved.

[0130] As one of the improvements, the resolution of the state space used by the Hybrid A-Star algorithm is improved. Since the higher the resolution of the state space, the greater the computational complexity. In order to strike a balance between path quality and computational efficiency, in the application scenario of the embodiment of the present application, a centimeter-level resolution can be used in the two-dimensional dimension of the position, for example, a resolution of 0.05 meters, that is, 0.05 meters is used as the grid length. A path segment is obtained by using a search step of meters. A lower resolution is used in the heading dimension, such as 90°, which can also be extended to 45°, etc.

[0131] Accordingly, with the above resolution, the action space of the self-moving device can be discretized, for example, using six atomic actions: straight forward meters, turn left 90°, turn right 90°, and the reverse actions of these three atomic actions.

[0132] The value of the cost function is obtained based on one or any combination of the total length of the sub-path, the change in orientation between path segments, the number of cusps, the distance between the path segment and the obstacle, and the change in the steering angle between the path segments, and the cusp is the switching position between forward and backward.

[0133] As another improvement, a new cost function is adopted for the Hybrid A-Star algorithm, which is obtained based on one or any combination of the total length of the sub-paths, the change in orientation between path segments, the number of sharp points, the distance between the path segments and the obstacles, and the change in the steering angle between the path segments.

[0134] As an example, the cost function J can be defined as follows:

[0135] J=w 1 ·J length +w 2 ·J obstacle +w 3 ·J turn_angle +w 4 ·J steer_angle +w 5 ·J cusp (1)

[0136] Among them, w 1 、w 2 、w 3 、w 4 and w 5 is the weighting coefficient, which can be an empirical value or an experimental value.

[0137] J length is a cost item related to the length of the subpath, which encourages planning of the shortest possible subpath. In addition, considering that the self-moving device has a slow speed when reversing, a higher penalty can be imposed on the length of the reversing path to avoid unnecessary reversing paths. For example, the following can be used:

[0138] J length =Σd i (2)

[0139] d i It indicates the moving distance corresponding to each search step and the length of each path segment. The moving distance corresponding to reversing can be negative.

[0140] J obstacle is a cost term related to the distance between the path segment and the obstacle. This cost term is used to ensure that the sub-path maintains a safe distance from the obstacle, thereby enhancing the robustness of the path. For example, it can be used:

[0141] J obstacle =Σc i (3)

[0142] c i represents the distance cost between each path segment and the obstacle. If the distance between path segment i and the obstacle is greater than the safety distance, then c i Take a positive value; otherwise c i Takes negative value.

[0143] J turn_angle is a cost term related to the change in orientation between path segments. This cost term is used to penalize paths with large turning angles, thereby enhancing the smoothness of the path. For example, the following can be used:

[0144] J turn_angle =Σ(θ i+1 -θ i ) 2 (4)

[0145] θ i represents the angle between path segment i and the reference direction. All path segments use the same reference direction, so θ i+1 -θ i Indicates the change in orientation between adjacent path segments.

[0146] J steer_angle is the cost term for the change in steering angle between path segments. This cost term is used to penalize paths with excessive steering angle changes to reduce excessive steering wheel rotation of the self-driving vehicle and improve driving stability. For example, the following can be used:

[0147] J steer_angle =Σ(φ i+1 -φi ) 2 (5)

[0148] φ i is the rudder angle corresponding to path segment i.

[0149] J cusp is a cost item related to the number of cusps. The so-called cusps refer to the switching positions between forward and reverse, that is, the positions where the self-moving device shifts gears (from forward gear to reverse gear, or from reverse gear to forward gear). This cost item is used to penalize the path where the self-moving device shifts gears too many times. For example, it can be used:

[0150] J cusp =N cusp (6)

[0151] N cusp is the number of cusps in the subpath.

[0152] As another improvement, in order to reduce the error caused by discretization, the embodiment of the present application can repair the sub-path based on the HybridA-Star algorithm in combination with the RS curve. During the search process, if the distance between the searched current position and the end point of the sub-path (for example, the Euclidean distance can be used) is less than or equal to the preset distance threshold, the RS curve is used to connect the current position and the end point of the sub-path, and the searched path segment and the curve obtained by connecting using the RS curve are spliced ​​into the re-planned sub-path.

[0153] The RS curve (Reeds-Shepp curve) is developed based on the Dubins curve. It allows the vehicle to move forward and backward, so that in some cases a shorter path can be found than the Dubins curve. The RS curve is composed of arcs and straight lines, which can meet the curvature constraints in vehicle kinematics. There are relatively mature algorithms and software for calculating RS curves. In the embodiments of the present application, the existing RS curve algorithm can be used or the corresponding software interface can be called to realize the RS curve connecting two positions.

[0154] If the RS curve fails to connect the current position with the end point of the sub-path, trace back a preset distance from the current position according to the searched path segment sequence, use the traced position as the current position, and continue to try to connect the current position with the end point of the sub-path using the RS curve until the backtracking end condition is met.

[0155] The preset forward tracing back distance may include: tracing back the distance corresponding to a preset number of action nodes, where the action node is a movement action performed by the mobile device on the searched path segment sequence, and the movement action may be one of the six atomic actions mentioned above. The distance corresponding to one action node may be traced back each time, or the distance corresponding to multiple action nodes may be traced back each time.

[0156] If the RS curve is used to successfully connect the current position and the end point of the sub-path, the searched path segment sequence and the curve obtained by connecting the RS curve are concatenated into the re-planned sub-path.

[0157] In an embodiment of the present application, the above-mentioned backtracking end condition may include that the total number of backtracked action nodes reaches a preset number threshold. If the number of backtracked action nodes reaches the preset number threshold, the RS curve can be abandoned for the current sub-path, and the Hybrid A-Star algorithm can be used to search the path to the end of the sub-path.

[0158] Furthermore, the embodiment of the present application can also optimize the RS curve obtained by connection, and filter out RS curves containing sharp points or forming double arcs.

[0159] The above step 209, namely "joining the sub-paths meeting the preset constraint conditions and the re-planned sub-paths in the initial path to obtain the task path", is described in detail below in conjunction with an embodiment.

[0160] As one of the feasible ways, the sub-paths in the initial path that meet the preset constraints and the re-planned sub-paths can be directly spliced ​​to obtain the task path.

[0161] However, in this implementation, it is possible that the replanned sub-paths are still unreasonable. Therefore, the embodiment of the present application provides another more preferred implementation method, namely, determining whether the replanned sub-paths all meet the preset constraints. If so, executing the step of splicing the sub-paths in the initial path that meet the preset constraints and the replanned sub-paths; otherwise, re-planning the sub-paths in the replanned sub-paths that do not meet the preset constraints until all the replanned sub-paths meet the preset constraints, or the number of replanning reaches a preset number threshold.

[0162] As one of the application scenarios, the above task is a handling task, and one of the task starting point and the task end point is a storage location on a cargo container transportation vehicle. After obtaining the task path, the task path can be carried in the corresponding task instruction and sent to the self-moving device. For example, in the handling task scenario, the information of the task path generated for the handling task is carried in the handling task instruction, which is sent by the RCS to the self-moving device, and the self-moving device moves according to the task path and performs the corresponding handling task.

[0163] The above is a description of a specific embodiment of the specification. Other embodiments are within the scope of the appended claims. In some cases, the actions or steps recorded in the claims can be performed in an order different from that in the embodiments and still achieve the desired results. In addition, the processes depicted in the drawings do not necessarily require the specific order or continuous order shown to achieve the desired results. In some embodiments, multitasking and parallel processing are also possible or may be advantageous.

[0164] It should be noted that the user information (including but not limited to user device information, user personal information, etc.) and data (including but not limited to data used for analysis, stored data, displayed data, etc.) involved in this application are all information and data authorized by the user or fully authorized by all parties, and the collection, use and processing of relevant data must comply with the relevant laws, regulations and standards of the relevant countries and regions, and provide corresponding operation entrances for users to choose to authorize or refuse.

[0165] In addition, an embodiment of the present application further provides a computer-readable storage medium on which a computer program is stored, and when the program is executed by a processor, the steps of any one of the methods in the aforementioned method embodiments are implemented.

[0166] An embodiment of the present application also provides a control system, including: a processor, wherein the processor executes the steps of any one of the methods described in the aforementioned method embodiments.

[0167] An embodiment of the present application also provides a self-mobile device, including: a processor, wherein the processor executes the steps of any one of the methods described in the aforementioned method embodiments.

[0168] The present application also provides a computer program product, including a computer program, which implements the steps of any one of the methods in the aforementioned method embodiments when executed by a processor.

[0169] in, Figure 6 The schematic diagram of the composition of the electronic device (which may be the above-mentioned control system or self-moving device) is shown as an example. Figure 6 As shown in , the electronic device 600 includes: a memory 620 and a processor 610.

[0170] The memory 620 is used to store program instructions.

[0171] The processor 610 is coupled to the memory 620 and is configured to read program instructions stored in the memory 620 to execute the steps of any one of the methods described in the foregoing method embodiments.

[0172] In some embodiments, the processor 610 may be one or more processors, and may be implemented by a general-purpose CPU, a microprocessor, an application-specific integrated circuit (ASIC), or one or more integrated circuits, etc., to execute the above program instructions to implement the technical solution provided in this application.

[0173] The memory 620 can be implemented in the form of ROM (Read Only Memory), RAM (Random Access Memory), static storage device, dynamic storage device, etc.

[0174] It should be noted that, although the above electronic device only shows the memory 620 and the processor 610, etc., in the specific implementation process, the electronic device may also include other components necessary for normal operation. In addition, it can be understood by those skilled in the art that the above electronic device may also only include the components necessary for implementing the solution of the present application, and does not necessarily include all the components shown in the figure.

[0175] Each embodiment in this specification is described in a progressive manner, and the same or similar parts between the embodiments can be referred to each other, and each embodiment focuses on the differences from other embodiments. In particular, for the system and device embodiments, since they are basically similar to the method embodiments, the description is relatively simple, and the relevant parts can be referred to the partial description of the method embodiments. The system and device embodiments described above are merely schematic, wherein the units described as separate components may or may not be physically separated, and the components displayed as units may or may not be physical units, that is, they may be located in one place, or they may be distributed on multiple network units. Some or all of the modules may be selected according to actual needs to achieve the purpose of the scheme of this embodiment. Ordinary technicians in this field can understand and implement it without paying creative labor.

[0176] The technical solution provided by the present application is described in detail above. The principle and implementation method of the present application are described in detail using specific examples. The description of the above embodiments is only used to help understand the method and core idea of ​​the present application. At the same time, for those skilled in the art, according to the idea of ​​the present application, there will be changes in the specific implementation method and application scope. In summary, the content of this specification should not be understood as limiting the present application.

Claims

1. A path planning method, executed by a processor, characterized in that: The method comprises: Determine a task start point and a task end point of a task to be performed by the mobile device; Performing path planning for the task to obtain an initial path; Determine a sub-path in the initial path that does not meet the preset constraint condition; Re-planning the sub-path that does not meet the preset constraint condition to obtain a re-planned sub-path; The sub-paths in the initial path that meet the preset constraint conditions and the re-planned sub-paths are spliced ​​to obtain a task path.

2. The method according to claim 1, characterized in that The performing path planning for the task to obtain an initial path comprises: Starting from the task starting point, a path segment search is performed based on a map to obtain a sequence of path segments with the shortest path to the task end point as the initial path, and the range of the map at least covers the task starting point and the task end point.

3. The method according to claim 1, characterized in that The performing path planning for the task to obtain an initial path comprises: Starting from the task starting point, a path segment search is performed based on a map to obtain a path segment sequence with the shortest path to the task end point and conforming to the constraints of the first collision detection model as the initial path.

4. The method according to claim 3, characterized in that The first collision detection model includes a circular collision detection model.

5. The method according to claim 4, characterized in that The path segment sequences that meet the constraints of the circular collision detection model include: The path segments in the path segment sequence do not intersect with the target contour, and the target contour is obtained by expanding the obstacle contour by a preset length, and the preset length is the radius of the maximum inscribed circle of the orthographic projection of the mobile device on the ground plane.

6. The method according to claim 1, characterized in that After obtaining the replanned sub-path, the method further includes: Determine whether all replanned sub-paths meet the preset constraints. If so, execute the step of splicing the sub-paths in the initial path that meet the preset constraints and the replanned sub-paths; otherwise, re-plan the sub-paths in the replanned sub-paths that do not meet the preset constraints.

7. The method according to claim 1 or 6, characterized in that: The sub-path that does not meet the preset constraint condition includes at least one of the following: subpaths that do not conform to the kinematic constraints of the self-moving device; Subpaths that do not meet the constraints of the second collision detection model; The subpath where the task starting point is located, the difference between the direction of the subpath where the task starting point is located and the orientation of the self-mobile device at the task starting point exceeds a first angle threshold; The sub-path where the task end point is located has a sub-path where the difference between the direction of the sub-path where the task end point is located and the target orientation of the self-moving device at the task end point exceeds a second angle threshold.

8. The method according to claim 1, characterized in that Before determining the sub-path in the initial path that does not meet the preset constraint condition, the method further includes: Determine at least one segmentation point in the initial path, and segment the initial path at the position of the at least one segmentation point to obtain a plurality of sub-paths; The segmentation points include at least one of a narrow point, an entrance position point and an exit position point of a narrow segment, and a position point at a preset length interval. The narrow point is a position point in the initial path where the passage width is less than or equal to a first width threshold, and the narrow segment is a path segment in the initial path where the passage width is less than or equal to a second width threshold.

9. The method according to claim 7, characterized in that: The second collision detection model includes a capsule collision detection model.

10. The method according to claim 9, characterized in that Subpaths that do not meet the constraints of the capsule collision detection model are determined as follows: When the self-moving device is simulated as a capsule shape moving on the sub-path, the capsule shape is composed of a rectangle and semicircles at both ends of the rectangle. It is determined whether the distance between the midline of the rectangle and the obstacle outline is greater than the radius of the maximum inscribed circle of the rectangle. If not, it is determined that the sub-path does not conform to the capsule detection model.

11. The method according to claim 10, characterized in that Before determining whether the distance between the midline of the rectangle and the obstacle outline is greater than the maximum inscribed circle radius of the rectangle, the method further includes: It is determined whether the distance between the midpoint of the rectangle and the obstacle contour is greater than the radius of the circumscribed circle of the rectangle. If so, it is determined that the subpath conforms to the capsule detection model.

12. The method according to claim 10, characterized in that Determining whether the distance between the center line of the rectangle and the obstacle outline is greater than the maximum inscribed circle radius of the rectangle, and if not, determining that the subpath does not conform to the capsule detection model includes: It is determined whether the midline of the rectangle intersects with the target contour. If so, it is determined that the subpath does not conform to the capsule collision detection model. The target contour is obtained by expanding the obstacle contour by a preset length, and the preset length is the maximum inscribed circle radius.

13. The method according to claim 1, characterized in that Re-planning the sub-path that does not meet the preset constraint condition, and obtaining the re-planned sub-path includes: Starting from the starting point of the sub-path that does not meet the preset constraint condition, searching for path segments based on the map, obtaining a sequence of path segments that reaches the end point of the sub-path and has the minimum cost function value as the re-planned sub-path, wherein the range of the map at least covers the task starting point and the task end point; The value of the cost function is obtained based on one or any combination of the total length of the sub-path, the change in orientation between path segments, the number of cusps, the distance between the path segment and the obstacle, and the change in the steering angle between the path segments, and the cusp is the switching position between forward and backward.

14. The method according to claim 13, characterized in that During the search process, if the distance between the current position searched and the end point of the sub-path is less than or equal to a preset distance threshold, the current position and the end point of the sub-path are connected using an RS curve, and the searched path segment and the curve obtained by connecting using the RS curve are spliced ​​into a re-planned sub-path.

15. The method according to claim 14, characterized in that The step of splicing the searched path segment and the curve obtained by connecting the RS curve into a re-planned sub-path includes: If the use of the RS curve to connect the current position with the end point of the sub-path fails, then trace back a preset distance from the current position according to the searched path segment sequence, take the traced position as the current position, and proceed to the step of connecting the current position with the end point of the sub-path using the RS curve until the backtracking end condition is met; If the use of the RS curve to connect the current position and the end point of the sub-path is successful, the searched path segment sequence and the curve obtained by connecting using the RS curve are spliced ​​into the re-planned sub-path.

16. The method according to claim 15, characterized in that The preset forward tracing back distance includes: a distance corresponding to a preset number of action nodes, where the action nodes are movement actions performed by the self-mobile device on the searched path segment sequence; The backtracking end condition includes: the total number of backtracked action nodes reaches a preset number threshold.

17. The method according to claim 2, 3 or 13, characterized in that The task is a handling task, and one of the task start point and the task end point is a storage location on a cargo container transportation vehicle; The map is obtained after the mobile device scans and maps the cargo container transportation vehicle using the sensor carried by the mobile device.

18. The method according to claim 1, characterized in that The processor is disposed in a control system, and the method further comprises: The task path is sent to the self-mobile device, so that the self-mobile device performs the task based on the task path.

19. A control system, characterized in that: The system comprises a processor and a memory, wherein the memory is used to store program instructions, and the processor executes the program instructions to implement the method according to any one of claims 1 to 18.

20. A self-propelled device, characterized in that: The system comprises a processor and a memory, wherein the memory is used to store program instructions, and the processor executes the program instructions to implement the method according to any one of claims 1 to 18.

Images