Robot path processing method, nonvolatile readable storage medium and robot
By adjusting and smoothing the robot path, the problem of insufficient path safety and smoothness in the prior art is solved, and higher navigation security and smoothness are achieved.
Patent Information
- Application Number
- CN202311139233.5
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2023-09-05
- Publication Date
- 2025-03-14
AI Technical Summary
The existing robot path planning algorithms have shortcomings in terms of security and smoothness, which leads to the robot being prone to collision with obstacles and the path is not smooth, affecting navigation smoothness.
After generating the original path, the path is adjusted to meet the preset security conditions, ensuring that the sum of the costs of the secure path does not exceed the sum of the costs of the original path, and smoothing is performed on this basis to obtain a smooth path that meets the preset smoothing conditions.
Improves path safety, enables the robot to walk closer to the expansion edge of obstacles, reduces collision risks, and improves the smoothness and efficiency of walking through smoothing.
Smart Images
Figure CN119618212A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the technical field of robots, and in particular to a path processing method of a robot, a non-volatile readable storage medium and a robot. Background Art
[0002] The robot provided by the related technology can perform path planning based on the graph search path planning algorithm and the cost map to obtain the shortest path. The related technology mainly focuses on shortening the path length. The planned path does not stick to the expanded edge of the obstacle at the corner, causing the robot to walk without sticking to the expanded edge of the obstacle, and it is easy to collide with surrounding obstacles, thus increasing the navigation risk of the robot. In addition, the planned path has sharp corners and the path is not smooth, which affects the navigation fluency of the robot. Summary of the invention
[0003] One purpose of an embodiment of the present invention is to provide a robot path processing method, a non-volatile readable storage medium and a robot to solve the technical problem that the path of the related technology is insufficient in security and smoothness.
[0004] In a first aspect, an embodiment of the present invention provides a path processing method of a robot, comprising:
[0005] Generate an original path according to a preset path planning algorithm and a preset cost map, wherein, in the obstacle expansion area of the cost map, the cost value of a position farther from the obstacle area is smaller, and the cost value of a position closer to the obstacle area is larger;
[0006] Adjusting the original path to obtain a safe path that meets a preset safety condition, wherein the sum of the costs of the safe path is less than or equal to the sum of the costs of the original path;
[0007] The safety path is smoothed to obtain a smooth path that meets a preset smoothing condition.
[0008] Optionally, adjusting the original path to obtain a safe path that meets a preset safety condition includes:
[0009] Determine the intermediate path points in sequence according to the original path, wherein the original path includes a first path point, a last path point, and a plurality of intermediate path points between the first path point and the last path point;
[0010] Determining a minimum cost position corresponding to the intermediate path point;
[0011] The position of the intermediate path point is set to the minimum cost position to obtain an adjusted intermediate path point, wherein the head path point, each of the adjusted intermediate path points and the terminal path point can form a safe path.
[0012] Optionally, determining the minimum cost position corresponding to the intermediate path point includes:
[0013] Sampling a plurality of candidate path points according to the intermediate path points;
[0014] Determine the candidate path point with the minimum cost value among the plurality of candidate path points as the target path point;
[0015] The minimum cost position corresponding to the intermediate path point is determined according to the cost value of the target path point and the cost value of the intermediate path point.
[0016] Optionally, sampling a plurality of candidate path points according to the intermediate path points comprises:
[0017] Determine two first path points adjacent to the middle path point and a midpoint of a first connecting line passing through the two first path points;
[0018] Determine a first moving direction and a second moving direction centered on the midpoint, wherein the first moving direction is opposite to the second moving direction;
[0019] Taking the midpoint as a starting point, a preset number of candidate path points are sampled in the first moving direction and the second moving direction respectively.
[0020] Optionally, the original path is configured with a coordinate system, and determining the first moving direction and the second moving direction centered on the midpoint comprises:
[0021] Determining a target angle between the first connecting line and a specified axis of the coordinate system;
[0022] A first ray and a second ray passing through the midpoint are determined according to the target angle, wherein the first ray and the second ray are both perpendicular to the first line, and the direction of the first ray is the first moving direction, and the direction of the second ray is the second moving direction.
[0023] Optionally, the determining the minimum cost position corresponding to the intermediate path point according to the cost value of the target path point and the cost value of the intermediate path point comprises:
[0024] If the cost value of the target path point is less than the cost value of the intermediate path point, determining the position of the target path point as the minimum cost position;
[0025] If the cost value of the target path point is greater than or equal to the cost value of the intermediate path point, the position of the intermediate path point is determined to be the minimum cost position.
[0026] Optionally, the safe path includes a head end path point, each adjusted intermediate path point and a terminal path point, and smoothing the safe path to obtain a smooth path that meets a preset smoothing condition includes:
[0027] Determine a second path point and a third path point adjacent to the adjusted intermediate path point, wherein the second path point and the third path point can form a second connecting line;
[0028] Determining a first total weighted cost according to the second path point, the third path point and the adjusted intermediate path point;
[0029] Determine a projection point where the adjusted intermediate path point is projected onto the second connecting line;
[0030] Determine a second total weighted cost value according to the second path point, the third path point and the projection point;
[0031] A smoothing point is determined according to the first total weighted cost value and the second total weighted cost value, and the head end path point, each of the smoothing points and the terminal path point can form a safe path.
[0032] Optionally, determining the first total weighted cost according to the second path point, the third path point and the adjusted intermediate path point includes:
[0033] Determining a first distance between the second path point and the adjusted intermediate path point, and a second distance between the third path point and the adjusted intermediate path point;
[0034] Calculating a first weighted cost value according to the first distance and the cost value of the second path point;
[0035] Calculating a second weighted cost value according to the second distance and the cost value of the third path point;
[0036] The adjusted first total weighted cost value of the intermediate path point is determined according to the first weighted cost value and the second weighted cost value.
[0037] Optionally, determining the second total weighted cost value according to the second path point, the third path point and the projection point includes:
[0038] Determine a third distance between the second path point and the projection point, and a fourth distance between the third path point and the projection point;
[0039] Calculating a third weighted cost value according to the third distance and the cost value of the second path point;
[0040] Calculating a fourth weighted cost value according to the fourth distance and the cost value of the third path point;
[0041] A second total weighted cost value is determined according to the third weighted cost value and the fourth weighted cost value.
[0042] Optionally, determining a smoothing point according to the first total weighted cost value and the second total weighted cost value includes:
[0043] Determine whether the first total weighted cost value is greater than the second total weighted cost value, and whether none of the projection points coincides with the second path point and the third path point;
[0044] If so, determining that the projection point is a smooth point;
[0045] If not, the adjusted intermediate path point is determined to be a smooth point.
[0046] In a second aspect, an embodiment of the present invention provides a non-volatile readable storage medium, characterized in that the non-volatile readable storage medium stores computer executable instructions, and the computer executable instructions are used to enable a robot to execute the above-mentioned robot path processing method.
[0047] In a third aspect, an embodiment of the present invention provides a robot, comprising:
[0048] at least one processor; and,
[0049] a memory communicatively connected to the at least one processor; wherein,
[0050] The memory stores instructions that can be executed by the at least one processor, and the instructions are executed by the at least one processor so that the at least one processor can execute the above-mentioned robot path processing method.
[0051] In the path processing method of the robot provided in the embodiment of the present invention, the original path is generated according to the preset path planning algorithm and the preset cost map, wherein, in the obstacle expansion area of the cost map, the cost value of the position farther from the obstacle area is smaller, and the cost value of the position closer to the obstacle area is larger, the original path is adjusted to obtain a safe path that meets the preset safety conditions, the sum of the costs of the safe path is less than or equal to the sum of the costs of the original path, and the safe path is smoothed to obtain a smooth path that meets the preset smoothing conditions. This embodiment makes adjustments based on the pre-planned original path to obtain a safe path whose sum of costs is less than or equal to the sum of the costs of the original path, which can improve the safety of the path and facilitate the path to be close to the expansion edge of the obstacle. Next, this embodiment performs smoothing processing on the basis of the safe path to obtain a smooth path, which will enable the robot to walk on the smooth path without multiple turns, which is conducive to improving the smoothness and efficiency of walking. BRIEF DESCRIPTION OF THE DRAWINGS
[0052] One or more embodiments are exemplarily described by pictures in the corresponding drawings, and these exemplified descriptions do not constitute limitations on the embodiments. Elements with the same reference numerals in the drawings represent similar elements, and unless otherwise stated, the figures in the drawings do not constitute proportional limitations.
[0053] Figure 1 A schematic flow chart of a path processing method for a robot provided by an embodiment of the present invention;
[0054] Figure 2 A schematic diagram of sampling candidate path points based on midpoints provided by an embodiment of the present invention;
[0055] Figure 3 A first schematic diagram of a robot provided in an embodiment of the present invention walking using an original path and a smooth path;
[0056] Figure 4 A second schematic diagram of a robot provided in an embodiment of the present invention walking using an original path and a smooth path;
[0057] Figure 5 A schematic diagram of the structure of a path processing device of a robot provided by an embodiment of the present invention;
[0058] Figure 6 A schematic diagram of the circuit structure of a robot provided in an embodiment of the present invention. DETAILED DESCRIPTION
[0059] In order to make the purpose, technical scheme and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present invention.
[0060] It should be noted that, if there is no conflict, the various features in the embodiments of the present invention can be combined with each other, all within the scope of protection of the present invention. In addition, although the functional modules are divided in the device schematic diagram and the logical order is shown in the flow chart, in some cases, the steps shown or described can be performed in a different order from the module division in the device or the flow chart. Furthermore, the words "first", "second", "third", etc. used in the present invention do not limit the data and execution order, but only distinguish the same items or similar items with basically the same functions and effects.
[0061] The present invention provides a method for processing a robot path. Figure 1 ,The path processing method of the robot includes the following steps:
[0062] S11: Generate an original path according to a preset path planning algorithm and a preset cost map.
[0063] In this step, the cost map is a map that adds navigation auxiliary information to the original map to facilitate the robot's path planning and navigation. The original map is a map generated by the robot according to a preset map construction algorithm. Among them, the cost map is a rasterized map, and the cost map is configured with cost values corresponding to each grid. In the cost map, this embodiment can expand the obstacle area of the cost map according to a preset expansion radius to obtain an obstacle expansion area.
[0064] In the obstacle expansion area of the cost map, the farther away from the obstacle area, the smaller the cost value, and the closer to the obstacle area, the larger the cost value. The grid beyond the preset expansion radius will not be expanded. The obstacle area is the area occupied by the obstacle on the cost map, where the obstacle area can be represented by the grid corresponding to the obstacle. The cost value can be customized by the designer based on engineering experience, such as the cost value of 0 / 1 / 254 / 255. The robot can walk close to the edge of the obstacle expansion area, which can not only avoid collision with obstacles, but also effectively clean the gaps in the obstacle area.
[0065] The preset path planning algorithm includes a graph search path planning algorithm, and the graph search path planning algorithm includes an A* algorithm, a Dijkstra algorithm, etc. In this embodiment, a path is planned on a preset cost map according to the preset path planning algorithm, thereby obtaining an original path.
[0066] S12: Adjust the original path to obtain a safe path that meets a preset safety condition, and the total cost of the safe path is less than or equal to the total cost of the original path.
[0067] In this step, the total cost of the safe path is the sum of the cost values of each path point on the safe path, and the total cost of the original path is the sum of the cost values of each path point on the original path. Since the total cost of the safe path is less than or equal to the total cost of the original path, the security of the safe path will be higher than that of the original path.
[0068] As mentioned above, within the obstacle expansion area, the farther away from the obstacle area, the smaller the cost value, and the closer to the obstacle area, the larger the cost value. Therefore, relatively speaking, the safe path will be easier to get close to the edge of the obstacle expansion area than the original path. In this way, when the robot walks along the safe path, it is easier for the robot to walk close to the edge of the obstacle expansion area, thereby reducing the probability of collision with surrounding obstacles and improving the safety of the robot's walking.
[0069] S13: Smoothing the safe path to obtain a smooth path that meets a preset smoothing condition.
[0070] In this step, smoothing refers to the operation of reducing the curvature of the safety path. This embodiment smoothes the safety path according to a preset smooth path algorithm or a customized smooth path algorithm to obtain a smooth path. This will enable the robot to walk along the smooth path without having to turn multiple times, which is beneficial to improving the smoothness and efficiency of walking.
[0071] It can be understood that this embodiment is compatible with both safety processing and smoothing processing based on the pre-planned original path, which can ensure that the final smooth path not only meets the smoothness requirements but also meets the safety requirements, which is conducive to the robot being able to walk more reliably, safely and efficiently.
[0072] In some embodiments, adjusting the original path to obtain a safe path that meets preset safety conditions includes the following steps: determining intermediate path points in sequence according to the original path, the original path including a starting path point, an end path point, and multiple intermediate path points between the starting path point and the end path point, determining the minimum cost position corresponding to the intermediate path point, setting the position of the intermediate path point to the minimum cost position, and obtaining adjusted intermediate path points, wherein the starting path point, each adjusted intermediate path point, and the end path point can form a safe path.
[0073] The minimum cost position is the position corresponding to the minimum cost value among the positions around the intermediate path point, and the adjusted intermediate path point is the intermediate path point whose original position is replaced by the minimum cost position.
[0074] For example, the path point set P of the original path = {p 0 ,p 1 ,p 2 ,...,p i ,...,p n-2 ,p n-1}, where p 0 is the first path point, p n-1 is the end path point, p i is the i-th intermediate path point. The first path point is the first path point of the original path, and the last path point is the last path point of the original path. i The position can be expressed as p i (x i ,y i ). When the i-th intermediate path point p i The corresponding minimum cost position is (x i_min ,y i_min ), then the minimum cost position (x i_min ,y i_min ) replaces the i-th intermediate path point p i The position p i (x i ,y i ), and get the adjusted intermediate path point p i (x i_min ,y i_min ), so that the cost value of the adjusted intermediate path point can be the smallest cost value among its surrounding points, thereby making the total cost of the safe path less than or equal to the total cost of the original path.
[0075] In some embodiments, determining the minimum cost position corresponding to the intermediate path point includes the following steps: sampling multiple candidate path points based on the intermediate path point, determining the candidate path point with the minimum cost value among the multiple candidate path points as the target path point, and determining the minimum cost position corresponding to the intermediate path point based on the cost value of the target path point and the cost value of the intermediate path point.
[0076] The candidate path points are path points sampled around the middle path point, wherein the present embodiment may sample multiple candidate path points in any suitable direction within a preset range centered on the middle path point.
[0077] In some embodiments, sampling multiple candidate path points based on the intermediate path point includes the following steps: determining two first path points adjacent to the intermediate path point and the midpoint of a first connecting line passing through the two first path points, determining a first moving direction and a second moving direction centered on the midpoint, wherein the first moving direction is opposite to the second moving direction, and sampling a preset number of candidate path points in the first moving direction and the second moving direction respectively with the midpoint as the starting point.
[0078] The sampling method may be a uniform sampling method or a random sampling method. In some embodiments, sampling a preset number of candidate path points in the first moving direction and the second moving direction respectively with the midpoint as the starting point includes: sampling a preset number of candidate path points along the first moving direction according to a preset sampling step length with the midpoint as the starting point, and sampling a preset number of candidate path points along the second moving direction according to a preset sampling step length with the midpoint as the starting point.
[0079] For example, see Figure 2 , and the intermediate path point p i Two adjacent first path points are first path points p i-1 and the first path point p i+1 , passing the first path point p i-1 and the first path point p i+1 The midpoint of the first connecting line is p _tmp , the first moving direction centered at the midpoint is The second moving direction centered on the midpoint is This embodiment samples m candidate path points along the first moving direction according to a preset sampling step length of l. This embodiment samples m candidate path points along the second moving direction according to a preset sampling step length of l, so that 2m candidate path points can be obtained.
[0080] This embodiment can take the midpoint corresponding to the middle path point as the center and sample in the opposite first moving direction and second moving direction, so as to accurately and reliably find candidate path points with a smaller cost value or a minimum cost value than the middle path point, and then replace the middle path point with the candidate path point with the minimum cost value, so as to reliably and accurately minimize the cost and adjust the original path to obtain a safe path.
[0081] In some embodiments, the original path is configured with a coordinate system, and determining the first moving direction and the second moving direction centered on the midpoint includes the following steps: determining a target angle between the first line and a specified axis of the coordinate system, and determining a first ray and a second ray passing through the midpoint based on the target angle, wherein the first ray and the second ray are both perpendicular to the first line, and the direction of the first ray is the first moving direction, and the direction of the second ray is the second moving direction.
[0082] When the positive x-axis of the coordinate system is from left to right and the positive y-axis is from bottom to top, the specified axis is the positive x-axis, and the target angle θ is the angle between the first line and the positive x-axis. When the positive y-axis of the coordinate system is from left to right and the positive x-axis is from bottom to top, the specified axis is the positive y-axis, and the target angle θ is the angle between the first line and the positive y-axis.
[0083] Determining the first ray passing through the midpoint according to the target angle includes the following steps: subtracting 90 degrees from the target angle to obtain a first angle, wherein the first angle is the angle between the first ray and the specified axis, and determining the first ray according to the first angle.
[0084] Determining a second ray passing through a midpoint according to a target angle includes the following steps: adding the target angle to 90 degrees to obtain a second angle, wherein the second angle is an angle between the second ray and a specified axis, and determining the second ray according to the second angle.
[0085] When the positive x-axis of the coordinate system is directed from left to right and the positive y-axis is directed from bottom to top, the coordinates of the candidate path points are as follows:
[0086] α=θ-90
[0087] p j x = p _tmp ·x+(cosα)*l
[0088] p j y = p _tmp y+(sinα)*l
[0089] j=1,2,...,m
[0090] Among them, the midpoint p _tmp The coordinates of the jth candidate path point are (x, y), the preset sampling step is l, and the coordinates of the jth candidate path point are (xj ,y j ).
[0091] Determining the candidate path point with the minimum cost value as the target path point among the multiple candidate path points includes: arranging the cost values of the multiple candidate path points according to a preset size arrangement order, and selecting the candidate path point with the minimum cost value as the target path point. The preset size arrangement order is an order from small to large cost values or an order from large to small cost values.
[0092] Determining the minimum cost position corresponding to the intermediate path point based on the cost value of the target path point and the cost value of the intermediate path point includes the following steps: if the cost value of the target path point is less than the cost value of the intermediate path point, then determining the position of the target path point as the minimum cost position; if the cost value of the target path point is greater than or equal to the cost value of the intermediate path point, then determining the position of the intermediate path point as the minimum cost position.
[0093] When the cost value of the target path point is less than the cost value of the intermediate path point, it means that the target path point is farther away from the obstacle than the intermediate path point. Therefore, this embodiment selects the position of the target path point as the minimum cost position. When the cost value of the target path point is greater than the cost value of the intermediate path point, it means that the target path point is closer to the obstacle than the intermediate path point. Therefore, this embodiment selects the position of the intermediate path point as the minimum cost position, that is, the position of the intermediate path point does not need to be changed.
[0094] In some embodiments, the safe path includes a first path point, each adjusted intermediate path point and a terminal path point, and smoothing the safe path to obtain a smooth path that meets a preset smoothing condition includes the following steps:
[0095] S131: Determine a second path point and a third path point adjacent to the adjusted intermediate path point, and the second path point and the third path point can form a second connection line.
[0096] S132: Determine a first total weighted cost according to the second path point, the third path point, and the adjusted intermediate path point.
[0097] S133: Determine the projection point of the adjusted intermediate path point onto the second connecting line.
[0098] S134: Determine a second total weighted cost value according to the second path point, the third path point and the projection point.
[0099] S135: Determine a smoothing point according to the first total weighted cost value and the second total weighted cost value. The first end path point, each smoothing point and the end path point can form a safe path.
[0100] In S131, determining the second path point and the third path point adjacent to the adjusted intermediate path point includes: determining the adjusted intermediate path point in sequence in the safe path, selecting a path point in front of the adjusted intermediate path point as the second path point, and a path point behind the adjusted intermediate path point as the third path point.
[0101] For example, the path point set of the safe path is P' = {p 0 ,p' 1 ,p' 2 ,...,p' i ,...,p' n-2 ,p n-1}, where p' i is the i-th adjusted intermediate path point. In this embodiment, the adjusted intermediate path point p' is first selected 1 , where the adjusted intermediate path point p' 1 The adjacent second path point is p 0 , the third path point is p' 2 In this embodiment, the adjusted intermediate path point p' 1 After performing the smoothing operation, select the adjusted intermediate path point p' 2 , and the adjusted intermediate path point p' 2 The adjacent second path point is p' 1 , the third path point is p' 3 , and so on, I will not go into details here.
[0102] In S132, this embodiment can comprehensively calculate the first total weighted cost value by combining the second path point, the third path point and the adjusted intermediate path point. The first total weighted cost value can comprehensively reflect the movement cost effect of the second path point and the third path point relative to the adjusted intermediate path point.
[0103] In some embodiments, determining a first total weighted cost value based on a second path point, a third path point, and an adjusted intermediate path point includes the following steps: determining a first distance between the second path point and the adjusted intermediate path point, and a second distance between the third path point and the adjusted intermediate path point, calculating a first weighted cost value based on the first distance and the cost value of the second path point, calculating a second weighted cost value based on the second distance and the cost value of the third path point, and determining a first total weighted cost value of the adjusted intermediate path point based on the first weighted cost value and the second weighted cost value.
[0104] Determining the first distance between the second path point and the adjusted intermediate path point includes: calculating the first distance according to the coordinates of the second path point in the coordinate system and the coordinates of the adjusted intermediate path point.
[0105] Determining the second distance between the third path point and the adjusted intermediate path point includes: calculating the second distance based on the coordinates of the third path point in the coordinate system and the coordinates of the adjusted intermediate path point.
[0106] Calculating the first weighted cost value according to the first distance and the cost value of the second path point includes: multiplying the first distance by the cost value of the second path point to obtain the first weighted cost value.
[0107] Calculating the second weighted cost value according to the second distance and the cost value of the third path point includes: multiplying the second distance by the cost value of the third path point to obtain the second weighted cost value.
[0108] Determining the first total weighted cost value of the adjusted intermediate path point according to the first weighted cost value and the second weighted cost value includes: adding the first weighted cost value and the second weighted cost value to obtain the first total weighted cost value of the adjusted intermediate path point.
[0109] For example, the present embodiment calculates the first total weighted cost of the adjusted intermediate path point according to the following formula, as shown below: c=l 1 c 1 +l 2 c 2 , where c is the first total weighted cost, l 1 is the first distance, c 1 is the cost value of the second path point, l 2 is the second distance, c 2 The present embodiment can calculate the adjusted first total weighted cost of the intermediate path points by combining the path length and the cost of the path points, thereby improving the path smoothing effect.
[0110] In S133, determining the projection point of the adjusted intermediate path point onto the second line includes: drawing a perpendicular line through the adjusted intermediate path point and perpendicular to the second line, and determining the intersection of the perpendicular line and the second line as the projection point.
[0111] In S134, as described above, this embodiment can comprehensively calculate the second path point, the third path point and the projection point to calculate the second total weighted cost value, and the second total weighted cost value can comprehensively reflect the movement cost effect of the second path point and the third path point relative to the projection point.
[0112] In some embodiments, determining a second total weighted cost value based on a second path point, a third path point, and a projection point includes the following steps: determining a third distance between the second path point and the projection point, and a fourth distance between the third path point and the projection point, calculating a third weighted cost value based on the third distance and the cost value of the second path point, calculating a fourth weighted cost value based on the fourth distance and the cost value of the third path point, and determining a second total weighted cost value based on the third weighted cost value and the fourth weighted cost value.
[0113] Determining the third distance between the second path point and the projection point includes: calculating the third distance according to the coordinates of the second path point in the coordinate system and the coordinates of the projection point.
[0114] Determining the fourth distance between the third path point and the projection point includes: calculating the fourth distance according to the coordinates of the third path point in the coordinate system and the coordinates of the projection point.
[0115] Calculating the third weighted cost value according to the third distance and the cost value of the second path point includes: multiplying the third distance by the cost value of the second path point to obtain the third weighted cost value.
[0116] Calculating the fourth weighted cost value according to the second distance and the cost value of the third path point includes: multiplying the second distance by the cost value of the third path point to obtain the fourth weighted cost value.
[0117] Determining the second total weighted cost value according to the third weighted cost value and the fourth weighted cost value includes: adding the third weighted cost value and the fourth weighted cost value to obtain the second total weighted cost value.
[0118] For example, the present embodiment calculates the adjusted first total weighted cost of the intermediate path point according to the following formula, as shown below: c'=l 3 c 1 +l 4 c 2 , where c' is the second total weighted cost, l 3 is the third distance, c 1 is the cost value of the second path point, l 4 is the fourth distance, c 2 is the cost value of the third path point.
[0119] In S135, determining the smooth point based on the first total weighted cost value and the second total weighted cost value includes: judging whether the first total weighted cost value is greater than the second total weighted cost value, and whether the projection point does not overlap with the second path point and the third path point; if so, determining that the projection point is a smooth point; if not, determining that the adjusted intermediate path point is a smooth point.
[0120] This embodiment fully integrates the path cost value and the path distance to select the smoothing point, so that the smoothing point can be moved in the direction of the path, thereby reducing the curvature of the safe path, improving the smoothing effect and ensuring the safety of the smoothed path. In addition, compared with path smoothing algorithms such as the B-spline curve smoothing algorithm, the smoothing operation provided by this embodiment is relatively simple, has lower requirements on the robot's computing power, and has better real-time performance.
[0121] This embodiment combines Figure 3 and Figure 4 The technical effects of the robot's path processing method are explained as follows:
[0122] See also Figure 3 The related art obtains the original path 31 according to the graph search path algorithm. The original path is close to the obstacle area 32 at the turn, but does not effectively close to the edge of the obstacle expansion area 33, so it is easy to collide with the obstacle. After being processed by the robot path processing method provided by each of the above embodiments, the smooth path 34 can effectively close to the edge of the obstacle expansion area 33.
[0123] See also Figure 4 The related technology obtains the original path 31 according to the graph search path algorithm. The original path 31 has a relatively sharp corner 35. However, after being processed by the path processing method of the robot provided in the above embodiments, the corner 35 is flattened in the smooth path 34. In this way, when the robot walks along the smooth path 34, it does not need to turn at the corner 35, thereby improving the walking smoothness and walking efficiency.
[0124] It should be noted that, in each of the above-mentioned embodiments, there is not necessarily a certain order between the above-mentioned steps. A person skilled in the art can understand, based on the description of the embodiments of the present invention, that in different embodiments, the above-mentioned steps may have different execution orders, that is, they may be executed in parallel, may be executed interchangeably, and so on.
[0125] As another aspect of the present invention, the present invention provides a path processing device for a robot, wherein the path processing device for the robot may be a software module, wherein the software module includes a plurality of instructions stored in a memory, and a processor may access the memory, call the instructions for execution, so as to complete the path processing method for the robot described in each of the above embodiments.
[0126] In some embodiments, the path processing device of the robot can also be constructed by hardware devices, for example, the path processing device of the robot can be constructed by one or more chips, and each chip can work in coordination with each other to complete the path processing method of the robot described in each of the above embodiments. For another example, the path processing device of the robot can also be constructed by various logic devices, such as a general-purpose processor, a digital signal processor (DSP), an application-specific integrated circuit (ASIC), a field programmable gate array (FPGA), a single-chip microcomputer, an ARM (Acorn RISC Machine) or other programmable logic device, discrete gate or transistor logic, discrete hardware components, or any combination of these components.
[0127] See also Figure 5 The robot's path processing device 500 includes an original path planning module 51 , a safe path processing module 52 and a smooth path processing module 53 .
[0128] The original path planning module 51 is used to generate the original path according to the preset path planning algorithm and the preset cost map, wherein, in the obstacle expansion area of the cost map, the cost value of the position farther from the obstacle area is smaller, and the cost value of the position closer to the obstacle area is larger. The safe path processing module 52 is used to adjust the original path to obtain a safe path that meets the preset safety conditions, and the sum of the costs of the safe path is less than or equal to the sum of the costs of the original path. The smooth path processing module 53 is used to smooth the safe path to obtain a smooth path that meets the preset smoothing conditions.
[0129] This embodiment makes adjustments based on the pre-planned original path to obtain a safe path whose total cost is less than or equal to the total cost of the original path, which can improve the safety of the path and help the path to be close to the expansion edge of the obstacle. Next, this embodiment performs smoothing processing on the basis of the safe path to obtain a smooth path, which will make the robot walk on the smooth path without multiple turns, which is conducive to improving the walking fluency and walking efficiency.
[0130] In some embodiments, the safe path processing module 52 is specifically used to: determine the intermediate path points in sequence according to the original path, the original path includes a starting path point, a terminal path point and multiple intermediate path points between the starting path point and the terminal path point, determine the minimum cost position corresponding to the intermediate path point, set the position of the intermediate path point to the minimum cost position, and obtain the adjusted intermediate path point, wherein the starting path point, each adjusted intermediate path point and the terminal path point can form a safe path.
[0131] In some embodiments, the safe path processing module 52 is also specifically used to: sample multiple candidate path points based on the intermediate path point, determine the candidate path point with the minimum cost value among the multiple candidate path points as the target path point, and determine the minimum cost position corresponding to the intermediate path point based on the cost value of the target path point and the cost value of the intermediate path point.
[0132] In some embodiments, the safe path processing module 52 is also specifically used to: determine the midpoint of two first path points adjacent to the intermediate path point and the first connecting line passing through the two first path points, determine a first moving direction and a second moving direction centered on the midpoint, wherein the first moving direction is opposite to the second moving direction, and takes the midpoint as the starting point, and samples a preset number of candidate path points in the first moving direction and the second moving direction, respectively.
[0133] In some embodiments, the original path is configured with a coordinate system, and the safe path processing module 52 is also specifically used to: determine the target angle between the first line and the specified axis of the coordinate system, and determine the first ray and the second ray passing through the midpoint according to the target angle, wherein the first ray and the second ray are both perpendicular to the first line, and the direction of the first ray is the first moving direction, and the direction of the second ray is the second moving direction.
[0134] In some embodiments, the safe path processing module 52 is also specifically used to: if the cost value of the target path point is less than the cost value of the intermediate path point, then determine the position of the target path point as the minimum cost position; if the cost value of the target path point is greater than or equal to the cost value of the intermediate path point, then determine the position of the intermediate path point as the minimum cost position.
[0135] In some embodiments, the safe path includes a starting path point, each adjusted intermediate path point and a terminal path point, and the smooth path processing module 53 is specifically used to: determine a second path point and a third path point adjacent to the adjusted intermediate path point, the second path point and the third path point can form a second line, determine a first total weighted cost value according to the second path point, the third path point and the adjusted intermediate path point, determine a projection point of the adjusted intermediate path point onto the second line, determine a second total weighted cost value according to the second path point, the third path point and the projection point, determine a smoothing point according to the first total weighted cost value and the second total weighted cost value, and the starting path point, each smoothing point and the terminal path point can form a safe path.
[0136] In some embodiments, the smooth path processing module 53 is also specifically used to: determine a first distance between the second path point and the adjusted intermediate path point, and a second distance between the third path point and the adjusted intermediate path point, calculate a first weighted cost value based on the first distance and the cost value of the second path point, calculate a second weighted cost value based on the second distance and the cost value of the third path point, and determine a first total weighted cost value of the adjusted intermediate path point based on the first weighted cost value and the second weighted cost value.
[0137] In some embodiments, the smooth path processing module 53 is also specifically used to: determine a third distance between the second path point and the projection point, and a fourth distance between the third path point and the projection point, calculate a third weighted cost value based on the third distance and the cost value of the second path point, calculate a fourth weighted cost value based on the fourth distance and the cost value of the third path point, and determine a second total weighted cost value based on the third weighted cost value and the fourth weighted cost value.
[0138] In some embodiments, the smooth path processing module 53 is also specifically used to determine whether the first total weighted cost value is greater than the second total weighted cost value, and whether the projection points do not overlap with the second path point and the third path point. If so, the projection point is determined to be a smooth point; if not, the adjusted intermediate path point is determined to be a smooth point.
[0139] It should be noted that the path processing device of the robot can execute the path processing method of the robot provided in the embodiment of the present invention, and has the corresponding functional modules and beneficial effects of the execution method. For technical details not fully described in the embodiment of the path processing device of the robot, please refer to the path processing method of the robot provided in the embodiment of the present invention.
[0140] See also Figure 6 , Figure 6 Schematic diagram of a circuit structure of a robot provided by an embodiment of the present invention. Figure 6 As shown, the robot 600 includes one or more processors 61 and a memory 62. Figure 6 A processor 61 is taken as an example.
[0141] The processor 61 and the memory 62 may be connected via a bus or other means. Figure 6 The example of connecting through bus is taken in the following.
[0142] The memory 62 is a non-volatile computer-readable storage medium that can be used to store non-volatile software programs, non-volatile computer executable programs and modules, such as program instructions / modules corresponding to the path processing method of the robot in the embodiment of the present invention. The processor 61 executes various functional applications and data processing of the path processing device of the robot by running the non-volatile software programs, instructions and modules stored in the memory 62, that is, realizes the path processing method of the robot provided in the above method embodiment and the functions of each module or unit in the above device embodiment.
[0143] The memory 62 may include a high-speed random access memory, and may also include a non-volatile memory, such as at least one disk storage device, a flash memory device, or other non-volatile solid-state storage devices. In some embodiments, the memory 62 may optionally include a memory remotely arranged relative to the processor 61, and these remote memories may be connected to the processor 61 via a network. Examples of the above-mentioned network include, but are not limited to, the Internet, an intranet, a local area network, a mobile communication network, and combinations thereof.
[0144] The program instructions / modules are stored in the memory 62, and when executed by the one or more processors 61, the path processing method of the robot in any of the above method embodiments is executed.
[0145] An embodiment of the present invention further provides a non-volatile computer storage medium, wherein the computer storage medium stores computer executable instructions, and the computer executable instructions are executed by one or more processors, such as Figure 6 A processor 61 in the embodiment may enable the one or more processors to execute the path processing method of the robot in any of the above method embodiments.
[0146] An embodiment of the present invention also provides a computer program product, which includes a computer program stored on a non-volatile computer-readable storage medium, and the computer program includes program instructions. When the program instructions are executed by a robot, the robot executes any one of the robot path processing methods described.
[0147] The above described device or equipment embodiments are merely illustrative, wherein the unit modules described as separate components may or may not be physically separated, and the components displayed as module units may or may not be physical units, that is, they may be located in one place, or may be distributed on multiple network module units. Some or all of the modules may be selected according to actual needs to achieve the purpose of the present embodiment.
[0148] Through the description of the above implementation methods, those skilled in the art can clearly understand that each implementation method can be implemented by means of software plus a general hardware platform, and of course, by hardware. Based on this understanding, the above technical solution is essentially or the part that contributes to the relevant technology can be embodied in the form of a software product, and the computer software product can be stored in a computer-readable storage medium, such as ROM / RAM, a disk, an optical disk, etc., including a number of instructions for a computer device (which can be a personal computer, a server, or a network device, etc.) to execute the methods described in each embodiment or some parts of the embodiments.
[0149] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit them. Under the concept of the present invention, the technical features in the above embodiments or different embodiments may also be combined, the steps may be implemented in any order, and there are many other changes in different aspects of the present invention as described above, which are not provided in detail for the sake of simplicity. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they may still modify the technical solutions described in the aforementioned embodiments, or make equivalent replacements for some of the technical features therein. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the scope of the technical solutions of the embodiments of the present invention.
Claims
1. A robot path processing method, characterized in that: include: Generate an original path according to a preset path planning algorithm and a preset cost map, wherein, in the obstacle expansion area of the cost map, the cost value of a position farther from the obstacle area is smaller, and the cost value of a position closer to the obstacle area is larger; Adjusting the original path to obtain a safe path that meets a preset safety condition, wherein the sum of the costs of the safe path is less than or equal to the sum of the costs of the original path; The safety path is smoothed to obtain a smooth path that meets a preset smoothing condition.
2. The method according to claim 1, characterized in that The adjusting the original path to obtain a safe path that meets a preset safety condition includes: Determine the intermediate path points in sequence according to the original path, wherein the original path includes a first path point, a last path point, and a plurality of intermediate path points between the first path point and the last path point; Determining a minimum cost position corresponding to the intermediate path point; The position of the intermediate path point is set to the minimum cost position to obtain an adjusted intermediate path point, wherein the head end path point, each of the adjusted intermediate path points and the terminal path point can form a safe path.
3. The method according to claim 2, characterized in that The determining of the minimum cost position corresponding to the intermediate path point comprises: Sampling a plurality of candidate path points according to the intermediate path points; Determine the candidate path point with the minimum cost value among the plurality of candidate path points as the target path point; The minimum cost position corresponding to the intermediate path point is determined according to the cost value of the target path point and the cost value of the intermediate path point.
4. The method according to claim 3, characterized in that The sampling of a plurality of candidate path points according to the intermediate path points comprises: Determine two first path points adjacent to the middle path point and a midpoint of a first connecting line passing through the two first path points; Determine a first moving direction and a second moving direction centered on the midpoint, wherein the first moving direction is opposite to the second moving direction; Taking the midpoint as a starting point, a preset number of candidate path points are sampled in the first moving direction and the second moving direction respectively.
5. The method according to claim 4, characterized in that The original path is configured with a coordinate system, and determining the first moving direction and the second moving direction centered on the midpoint includes: Determining a target angle between the first connecting line and a specified axis of the coordinate system; A first ray and a second ray passing through the midpoint are determined according to the target angle, wherein the first ray and the second ray are both perpendicular to the first line, and the direction of the first ray is the first moving direction, and the direction of the second ray is the second moving direction.
6. The method according to claim 3, characterized in that The step of determining the minimum cost position corresponding to the intermediate path point according to the cost value of the target path point and the cost value of the intermediate path point comprises: If the cost value of the target path point is less than the cost value of the intermediate path point, determining the position of the target path point as the minimum cost position; If the cost value of the target path point is greater than or equal to the cost value of the intermediate path point, the position of the intermediate path point is determined to be the minimum cost position.
7. The method according to any one of claims 1 to 6, characterized in that: The safe path includes a head end path point, each adjusted intermediate path point and a terminal path point, and smoothing the safe path to obtain a smooth path that meets a preset smoothing condition includes: Determine a second path point and a third path point adjacent to the adjusted intermediate path point, wherein the second path point and the third path point can form a second connecting line; Determining a first total weighted cost according to the second path point, the third path point and the adjusted intermediate path point; Determine a projection point where the adjusted intermediate path point is projected onto the second connecting line; Determine a second total weighted cost value according to the second path point, the third path point and the projection point; A smoothing point is determined according to the first total weighted cost value and the second total weighted cost value, and the head end path point, each of the smoothing points and the terminal path point can form a safe path.
8. The method according to claim 7, characterized in that The determining of the first total weighted cost value according to the second path point, the third path point and the adjusted intermediate path point comprises: Determining a first distance between the second path point and the adjusted intermediate path point, and a second distance between the third path point and the adjusted intermediate path point; Calculating a first weighted cost value according to the first distance and the cost value of the second path point; Calculating a second weighted cost value according to the second distance and the cost value of the third path point; The adjusted first total weighted cost value of the intermediate path point is determined according to the first weighted cost value and the second weighted cost value.
9. The method according to claim 7, characterized in that: The determining of the second total weighted cost value according to the second path point, the third path point and the projection point comprises: Determine a third distance between the second path point and the projection point, and a fourth distance between the third path point and the projection point; Calculating a third weighted cost value according to the third distance and the cost value of the second path point; Calculating a fourth weighted cost value according to the fourth distance and the cost value of the third path point; A second total weighted cost value is determined according to the third weighted cost value and the fourth weighted cost value.
10. The method according to claim 7, characterized in that The determining of the smoothing point according to the first total weighted cost value and the second total weighted cost value comprises: Determine whether the first total weighted cost value is greater than the second total weighted cost value, and whether none of the projection points overlaps with the second path point and the third path point; If so, determining that the projection point is a smooth point; If not, the adjusted intermediate path point is determined to be a smooth point.
11. A non-volatile readable storage medium, characterized in that: The non-volatile readable storage medium stores computer-executable instructions, and the computer-executable instructions are used to enable a robot to execute the path processing method of a robot as described in any one of claims 1 to 10.
12. A robot, characterized in that: include: at least one processor; as well as, a memory communicatively connected to the at least one processor; wherein, The memory stores instructions that can be executed by the at least one processor, and the instructions are executed by the at least one processor so that the at least one processor can execute the path processing method of the robot according to any one of claims 1 to 10.