CN111121812B - Path optimization method, electronic device and storage medium - Google Patents
Path optimization method, electronic device and storage medium Download PDFInfo
- Publication number
- CN111121812B CN111121812B CN201911420881.1A CN201911420881A CN111121812B CN 111121812 B CN111121812 B CN 111121812B CN 201911420881 A CN201911420881 A CN 201911420881A CN 111121812 B CN111121812 B CN 111121812B
- Authority
- CN
- China
- Prior art keywords
- point
- path
- distance
- robot
- preset value
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/34—Route searching; Route guidance
- G01C21/3446—Details of route searching algorithms, e.g. Dijkstra, A*, arc-flags, using precalculated routes
Abstract
The embodiment of the invention relates to the field of robots and discloses a path optimization method, electronic equipment and a storage medium. In some embodiments of the present application, a method for path optimization includes: acquiring a path planned based on a navigation map of the robot; aiming at the track points on the path, respectively carrying out the following operations so as to optimize the path: determining the position information of the central point of the expansion area of the barrier closest to the track point and the first distance from the track point to the central point; determining a first ellipse boundary which takes the central point as the center, takes the first preset value as the length of a long axis and takes the second preset value as the length of a short axis, wherein the direction of the long axis is parallel to the advancing direction of the track point; determining a second distance from the intersection point of the straight line where the central point and the track point are located and the first elliptical boundary to the central point according to the first elliptical boundary; and optimizing the pose of the robot at the track point according to the first distance, the second distance and a preset barrier cost function. In this embodiment, the optimized path is made smoother.
Description
Technical Field
The embodiment of the invention relates to the field of robots, in particular to a path optimization method, electronic equipment and a storage medium.
Background
With the rapid development of computer technology, sensor technology and artificial intelligence, the autonomous navigation technology of the robot has also made great progress. However, in a complex external environment, the mobile robot inevitably encounters an obstacle during autonomous navigation. Among the path planning algorithms common in the present stage, the optimization-based local path planning algorithm is a large research direction. Among them, local planning (teb local planer) is the most widely used.
However, the inventors found that at least the following problems exist in the prior art: the path generated by the optimized local path planning algorithm is often not smooth enough and does not meet the design requirements, and especially when the moving direction of the robot is very close to the obstacle, the robot is often not very safe.
Disclosure of Invention
An object of the embodiments of the present invention is to provide a path optimization method, an electronic device, and a storage medium, so that a planned trajectory is smoother, better meets design requirements, and is safer.
In order to solve the above technical problem, an embodiment of the present invention provides a path optimization method applied to a robot, including: acquiring a path planned based on a navigation map of the robot; aiming at the track points on the path, respectively carrying out the following operations so as to optimize the path: determining the position information of the central point of the expansion area of the barrier closest to the track point and the first distance from the track point to the central point; determining a first ellipse boundary which takes the central point as the center, takes the first preset value as the length of a long axis and takes the second preset value as the length of a short axis, wherein the direction of the long axis is parallel to the advancing direction of the track point; determining a second distance from the intersection point of the straight line where the central point and the track point are located and the first elliptical boundary to the central point according to the first elliptical boundary; optimizing the pose of the robot at the track point according to the first distance, the second distance and a preset barrier cost function; the obstacle cost function indicates a constrained relationship of the first distance, the second distance, and the obstacle cost of the trajectory point.
An embodiment of the present invention also provides an electronic device, including: at least one processor; and a memory communicatively coupled to the at least one processor; wherein the memory stores instructions executable by the at least one processor to enable the at least one processor to perform the path optimization method as set forth in the above embodiments.
Embodiments of the present invention also provide a computer-readable storage medium storing a computer program, which when executed by a processor implements the path optimization method mentioned in the above embodiments.
Compared with the prior art, in the optimization process, the path is optimized by the oval boundary taking the central point of the expansion area of the barrier as the center, so that the optimized path is closer to an oval, and the optimized path is smoother and safer. In addition, compared with the route which is approximately circular, the route which is approximately elliptical has smaller transverse span, is easier to pass through narrower roads, has shorter route and has higher robot motion efficiency.
In addition, the obstacle cost function is: formula a:
where error represents the obstacle cost, weight represents the weight of the obstacle cost function, obsdistDenotes a first distance, avodistRepresenting the second distance.
In addition, the determining of the first elliptical boundary which takes the central point as the center, takes the first preset value as the length of the long axis, takes the second preset value as the length of the short axis, and the direction of the long axis is parallel to the advancing direction of the track point specifically comprises the following steps: acquiring the traveling direction of the robot at the track point; according to the advancing direction, establishing a rectangular coordinate system which takes the track point as an original point, the advancing direction as a long axis direction and the direction vertical to the advancing direction as a short axis direction; constructing a second ellipse boundary which takes the original point as the center, the first preset value as the length of the long axis and the second preset value as the length of the short axis according to the first preset value and the second preset value; and translating the second elliptical boundary according to the position information of the central point and the position information of the track point so as to overlap the center of the second elliptical boundary with the central point to obtain a first elliptical boundary.
In addition, the determining of the first elliptical boundary which takes the central point as the center, takes the first preset value as the length of the long axis, takes the second preset value as the length of the short axis, and the direction of the long axis is parallel to the advancing direction of the track point specifically comprises the following steps: acquiring the traveling direction of the robot at the track point; rotating the second elliptical boundary according to the advancing direction and the major axis direction of the second elliptical boundary of the obstacle, so that the major axis direction of the rotated second elliptical boundary is the same as the advancing direction; the second elliptical boundary of the obstacle is an elliptical boundary which takes the center point of the expansion area of the obstacle as the center, takes the first preset value as the length of the long axis and takes the second preset value as the length of the short axis; and taking the rotated second ellipse boundary as the first ellipse boundary.
In addition, the first preset value is larger than the length of the robot, and the second preset value is larger than the width of the robot.
In addition, according to the first distance, the second distance and a preset barrier cost function, the pose of the robot at the track point is optimized, and the method specifically comprises the following steps: and optimizing the pose of the track point by adopting a Leibberg Marquard LM algorithm according to the first distance, the second distance and the barrier cost function.
In addition, before performing subsequent operations on the trace points on the path, the path optimization method further includes: it is determined that an obstacle exists in a navigation map of the robot.
In addition, after the path planned based on the navigation map of the robot is acquired, the path optimization method further includes: and optimizing the path based on the shortest path optimization algorithm.
Drawings
One or more embodiments are illustrated by way of example in the accompanying drawings, which correspond to the figures in which like reference numerals refer to similar elements and which are not to scale unless otherwise specified.
FIG. 1 is a flow chart of a path optimization method of a first embodiment of the present invention;
FIG. 2 is a schematic diagram of a path planned based on a navigation map according to a first embodiment of the present invention;
3 a-3 e are schematic diagrams of the pose optimization process of track point A of the first embodiment of the invention;
4 a-4 e are schematic diagrams of the pose optimization process of trace point B of the first embodiment of the invention;
FIG. 5 is a schematic diagram of the optimized path of the first embodiment of the present invention;
FIG. 6 is a schematic diagram of a path planned by the prior art;
FIG. 7 is a flow chart of a path optimization method of a second embodiment of the present invention;
fig. 8 is a schematic structural view of a path optimizing apparatus according to a third embodiment of the present invention;
fig. 9 is a schematic structural diagram of an electronic apparatus according to a fourth embodiment of the present invention;
fig. 10 is a schematic configuration diagram of a robot according to a fifth embodiment of the present invention.
Detailed Description
In order to make the objects, technical solutions and advantages of the embodiments of the present invention more apparent, embodiments of the present invention will be described in detail below with reference to the accompanying drawings. However, it will be appreciated by those of ordinary skill in the art that numerous technical details are set forth in order to provide a better understanding of the present application in various embodiments of the present invention. However, the technical solution claimed in the present application can be implemented without these technical details and various changes and modifications based on the following embodiments.
Unless the context clearly requires otherwise, throughout the description and the claims, the words "comprise", "comprising", and the like are to be construed in an inclusive sense as opposed to an exclusive or exhaustive sense; that is, what is meant is "including, but not limited to".
In the description of the present disclosure, it is to be understood that the terms "first," "second," and the like are used for descriptive purposes only and are not to be construed as indicating or implying relative importance. In addition, in the description of the present disclosure, "a plurality" means two or more unless otherwise specified.
The first embodiment of the present invention relates to a path optimization method applied to a robot, which is applied to an electronic device, such as a robot, or other terminal or server communicatively connected to the robot. As shown in fig. 1, the path optimization method further includes:
step 101: and acquiring a path planned based on the navigation map of the robot.
Specifically, the electronic device may self-generate a route based on the navigation map, the start location information, and the target location information. The electronic device may also acquire a route generated by another device from the outside based on the navigation map, the start position information, and the target position information, and the manner of acquiring the route is not limited herein.
It should be noted that, as those skilled in the art can understand, the process of planning a route based on the navigation map, the start position information and the target position information is not the focus of the embodiment of the present invention, and the specific route generation method is not limited herein, and those skilled in the art may generate a route by referring to other related data.
Step 102: aiming at the track points on the path, respectively carrying out the following operations so as to optimize the path: determining the position information of the central point of the expansion area of the barrier closest to the track point and the first distance from the track point to the central point; determining a first ellipse boundary which takes the central point as the center, takes the first preset value as the length of a long axis and takes the second preset value as the length of a short axis, wherein the direction of the long axis is parallel to the advancing direction of the track point; determining a second distance from the intersection point of the straight line where the central point and the track point are located and the first elliptical boundary to the central point according to the first elliptical boundary; and optimizing the pose of the robot at the track point according to the first distance, the second distance and a preset barrier cost function.
Specifically, the obstacle cost function indicates a constrained relationship of the first distance, the second distance, and the obstacle cost of the trajectory point. The pose of the robot at the track point is optimized by the electronic equipment based on the obstacle cost obtained by the robot relative to the elliptical boundary of the obstacle, and the optimized pose enables the optimized path of the robot to approach the ellipse. Compared with the path planned by the prior art, the path which approaches to the ellipse is smoother and safer. In addition, compared with the route which is approximately circular, the route which is approximately elliptical has smaller transverse span, is easier to pass through narrower roads, has shorter route and has higher robot motion efficiency.
In one embodiment, the position information of the center point of the expanded area of the obstacle is coordinate information of the center point of the expanded area of the obstacle. And the electronic equipment determines the central point of the expansion area of the barrier closest to the track point, the coordinate information of the central point and the distance from the central point to the track point based on the navigation map.
In one embodiment, the obstacle cost function is:
formula a:
where error represents the obstacle cost, weight represents the weight of the obstacle cost function, obsdistDenotes a first distance, avodistRepresenting the second distance. Wherein, the weight can be set according to the project requirement of the developer, for example, if the project has higher importance degree to the barrier, the weight is set to be larger,and if the importance degree of the project on the barrier is lower, setting smaller weight.
It is worth mentioning that when the second estimate is larger than the first distance, it is indicated that the track point is located within the first elliptical boundary, and in this case, the pose of the track point is optimized based on the obstacle cost of the track point to reduce the distance between the robot and the central point.
It should be noted that, in practical applications, the first distance, the second distance and the obstacle cost of the track point may be other constraint relations besides the formula a, for example, when obsdist<avodistThen, error is weight (avo)dist-obsdist)2，obsdist≥avodistWhen the error is equal to 0, this embodiment is merely an example, and does not limit a specific constraint relationship between the first distance, the second distance, and the obstacle cost of the track point.
In one embodiment, the electronic device optimizes the pose of the trace point using a Levenberg-Marquardt (LM) algorithm based on the first distance, the second distance, and the obstacle cost function.
In one embodiment, the electronic device in determining the first ellipse boundary includes: acquiring the traveling direction of the robot at the track point; according to the advancing direction, establishing a rectangular coordinate system which takes the track point as an original point, the advancing direction as a long axis direction and the direction vertical to the advancing direction as a short axis direction; constructing a second ellipse boundary which takes the original point as the center, the first preset value as the length of the long axis and the second preset value as the length of the short axis according to the first preset value and the second preset value; and translating the second elliptical boundary according to the position information of the central point and the position information of the track point so as to overlap the center of the second elliptical boundary with the central point to obtain a first elliptical boundary.
In one embodiment, the process for the electronic device to translate the second ellipse boundary is as follows: the electronic equipment determines the translation direction and translation distance of the second ellipse boundary according to the relative position relationship between the track point and the central point; according to the direction and distance of translationAnd moving the second elliptical boundary horizontally to obtain a first elliptical boundary. For example, if the coordinates of the track point are (a)1，b1) The center point is (a)2，b2) Then the translation direction is arctan [ (b)2-b1)/(a2-a1)]The translation distance is
Thus, a second ellipse boundary is placed along the arctan [ (b)2-b1)/(a2-a1)]Direction translation
And (4) units.
Optionally, after the first elliptical boundary is obtained by translation, the coordinates of the center of the first elliptical boundary are calculated, whether the distance between the center of the first elliptical boundary and the center point is smaller than a distance threshold is judged, if yes, the translation process is completed, and if not, the first elliptical boundary is translated again based on the distance.
For example, the path of the robot 10 planned based on the navigation map is shown in fig. 2. The electronic equipment respectively carries out pose optimization aiming at the track point A and the track point B, and the central point of the expansion area of the barrier closest to the track point A and the track point B is a point C. For the track point a, determining a distance between the track point a and the central point C, i.e. a first distance (obs _ dist1) corresponding to the track point a, as shown in fig. 3 a; according to the advancing direction of the robot at the track point A, establishing a rectangular coordinate system which takes the track point A as an origin, the advancing direction of the track point A as a long axis direction (x axis direction in the figure) and the direction perpendicular to the advancing direction of the track point A as a short axis direction (y axis direction in the figure), as shown in FIG. 3 b; according to the first preset value and the second preset value, constructing a second elliptical boundary corresponding to a track point A which takes the original point as the center, the first preset value as the length of the long axis and the second preset value as the length of the short axis, as shown in fig. 3 c; determining an ellipse equation corresponding to a second ellipse boundary corresponding to the track point A; translating the second elliptical boundary corresponding to the track point A according to the position information of the central point C, so that the center of the second elliptical boundary corresponding to the track point A is overlapped with the central point C to obtain a first elliptical boundary corresponding to the track point A, and determining an elliptical equation corresponding to the first elliptical boundary corresponding to the track point A based on an elliptical equation corresponding to the second elliptical boundary corresponding to the track point A as shown in FIG. 3 d; making a straight line L1 passing through the track point A and the central point C, and determining a straight line equation of the straight line L1 according to the position information of the track point A and the position information of the central point C; based on a straight line equation of the straight line L1 and an ellipse equation corresponding to the first ellipse boundary corresponding to the track point A, determining position information of an intersection point D1 between the first ellipse boundary corresponding to the track point A and the straight line L1; from the position information of D1, and the position information of center point C, the distance between D1 and center point C, i.e., the second distance (avo _ dist1) corresponding to track point a, is determined, as shown in fig. 3 e. And bringing a first distance corresponding to the track point A and a second distance corresponding to the track point A into an obstacle cost function, and optimizing the pose of the robot at the track point A by adopting an LM algorithm according to the obstacle cost function.
For the track point B, determining a distance between the track point B and the central point C, i.e. a first distance (obs _ dist2) corresponding to the track point B, as shown in fig. 4 a; according to the advancing direction of the robot at the track point B, establishing a rectangular coordinate system which takes the track point B as an origin, the advancing direction of the track point B as a long axis direction (x axis direction in the figure) and the direction perpendicular to the advancing direction of the track point B as a short axis direction (y axis direction in the figure), as shown in FIG. 4B; according to the first preset value and the second preset value, constructing a second elliptical boundary corresponding to a track point B which takes the original point as the center, the first preset value as the length of the long axis and the second preset value as the length of the short axis, as shown in fig. 4 c; determining an ellipse equation corresponding to a second ellipse boundary corresponding to the track point B; translating the second elliptical boundary corresponding to the track point B according to the position information of the central point C, so that the center of the second elliptical boundary corresponding to the track point B is overlapped with the central point C to obtain a first elliptical boundary corresponding to the track point B, and determining an elliptical equation corresponding to the first elliptical boundary corresponding to the track point B based on the elliptical equation corresponding to the second elliptical boundary corresponding to the track point B as shown in FIG. 4 d; making a straight line L2 passing through the track point B and the central point C, and determining a straight line equation of the straight line L2 according to the position information of the track point B and the position information of the central point C; based on a straight line equation of the straight line L2 and an ellipse equation corresponding to the first ellipse boundary corresponding to the track point B, determining position information of an intersection point D2 between the first ellipse boundary corresponding to the track point B and the straight line L2; from the position information of D2, and the position information of center point C, the distance between D2 and center point C, i.e., the second distance (avo _ dist2) corresponding to track point B, is determined, as shown in fig. 4 e. And bringing a first distance corresponding to the track point B and a second distance corresponding to the track point B into the barrier cost function, and optimizing the pose of the robot at the track point B by adopting an LM algorithm according to the barrier cost function. Finally, the optimized path is shown in fig. 5. As can be seen from fig. 5, the optimized path is smoother than the original path, and better meets the design requirements. In addition, compared with the path (such as fig. 6) obtained based on the optimized local path planning algorithm, in the path shown in fig. 6, the moving direction of the robot is towards the obstacle, and the closest position is very close to the obstacle, which is often unsafe for the obstacle and the robot. In the path optimization method proposed in the present disclosure, the optimized path is shown in fig. 5, the moving direction of the robot is farther from the obstacle, and the robot moves more safely. The path obtained by the optimization mode ensures the safety of the robot, and compared with the path obtained by planning by taking the first preset value as the diameter, the path has smaller transverse span and is easier to pass through a narrower road; the path obtained by the optimization mode is shorter, and the robot motion efficiency is higher.
In another embodiment, the electronic device determines the first ellipse boundary as follows: acquiring the traveling direction of the robot at the track point; rotating the second elliptical boundary according to the advancing direction and the major axis direction of the second elliptical boundary of the obstacle, so that the major axis direction of the rotated second elliptical boundary is the same as the advancing direction; the second elliptical boundary of the obstacle is an elliptical boundary which takes the center point of the expansion area of the obstacle as the center, takes the first preset value as the length of the long axis and takes the second preset value as the length of the short axis; and taking the rotated second ellipse boundary as the first ellipse boundary. Specifically, the electronic device may store the second elliptical boundary of each obstacle in advance, or, after determining the obstacle closest to the trajectory point, construct the second elliptical boundary of the obstacle closest to the trajectory point based on a preset construction rule. The preset construction rule may be to establish a rectangular coordinate system with the center of the expansion region of the obstacle as an origin, the first direction as a long axis direction, and the second direction perpendicular to the first direction as a short axis direction; and constructing an elliptic equation by taking the central point of the expansion area of the obstacle as the center, taking the first preset value as the length of the long axis and taking the second preset value as the length of the short axis, wherein the elliptic boundary corresponding to the elliptic equation is the second elliptic boundary of the obstacle. The electronic device rotates the second elliptical boundary such that the long axis direction of the rotated second elliptical boundary is the same as the direction of travel, based on the direction of travel and the long axis direction of the second elliptical boundary of the obstacle. For example, if the angle between the major axis direction of the second elliptical boundary of the obstacle and the traveling direction is 45 degrees, the second elliptical boundary is rotated by 45 degrees in the traveling direction to obtain the first elliptical boundary.
In one embodiment, the first preset value is greater than the length of the robot and the second preset value is greater than the width of the robot. The specific values of the first preset value and the second preset value may be determined by referring to a preset safety distance between the robot and the obstacle, for example, if the robot has a length of 6 meters and a width of 4 meters, and the safety distance is 4 meters, the first preset value may be 8 meters, and the second preset value is 6 meters.
It is worth mentioning that since the major axis and the minor axis of the elliptical boundary centered on the center point of the expansion area of the obstacle are determined based on the size information of the robot, the influence of the size of the robot on the collision with the obstacle is fully considered, and the probability of the robot colliding with the obstacle is further reduced.
It should be noted that, as can be understood by those skilled in the art, in practical applications, the first preset value and the second preset value may be set as needed, and are only exemplified here.
The above description is only for illustrative purposes and does not limit the technical aspects of the present invention.
Compared with the prior art, in the path optimization method provided by the embodiment, in the optimization process, the path is optimized by using the elliptical boundary with the central point of the expansion area of the obstacle as the center, so that the optimized path is closer to an ellipse, and the optimized path is smoother and safer. In addition, compared with the route which is approximately circular, the route which is approximately elliptical has smaller transverse span, is easier to pass through narrower roads, has shorter route and has higher robot motion efficiency.
A second embodiment of the present invention relates to a path optimization method. The embodiment is further improved on the basis of the first embodiment, and the specific improvements are as follows: other related steps are added before step 102.
Specifically, as shown in fig. 7, the present embodiment includes steps 201 to 204, where steps 201 and 204 are substantially the same as steps 101 and 102 in the first embodiment, respectively, and are not repeated here. The following mainly introduces the differences:
step 201: and acquiring a path planned based on the navigation map of the robot.
Step 202: it is determined that an obstacle exists in a navigation map of the robot.
In particular, the robot may be provided with detection means for the presence of obstacles in the navigation map of the robot.
For example, the navigation map of the robot is a local map, the robot is a forward-shifting rear-drive robot with a laser sensor and an ultrasonic sensor, but the laser sensor and the ultrasonic sensor detect that an obstacle enters the local map of the robot, the robot performs subsequent operations by using an obstacle cost function, the obstacle cost of each track point of the path is reduced by changing the pose of each track point, and the planned path is optimized.
It should be noted that those skilled in the art can understand that in practical applications, other sensors may also be used to detect an obstacle, and this embodiment is merely an example.
Step 203: and optimizing the path based on the shortest path optimization algorithm.
Specifically, the electronic device optimizes the path based on the shortest path optimization algorithm, and then optimizes the path based on the barrier cost function.
It should be noted that, as will be understood by those skilled in the art, in this embodiment, the path optimization process is described by taking an example of optimizing a path based on a shortest path optimization algorithm before performing path optimization based on an obstacle cost function, and in practical application, the electronic device may also optimize a path based on other optimization algorithms, which is only exemplified in this embodiment.
It is worth mentioning that the electronic device optimizes the path of the robot based on various optimization algorithms, so that the path obtained through final optimization better meets the project requirements.
It should be noted that step 202 and step 203 are not necessarily performed, and step 202 and step 203 may be selectively performed, or either one of step 202 and step 203 may be performed.
It is worth mentioning that when the path is optimized based on other path optimization algorithms and then optimized based on the barrier cost function, the finally obtained path approaches to an ellipse, and the influence of other path optimization algorithms on the path form is avoided.
Step 204: aiming at the track points on the path, respectively carrying out the following operations so as to optimize the path: determining the position information of the central point of the expansion area of the barrier closest to the track point and the first distance from the track point to the central point; determining a first ellipse boundary which takes the central point as the center, takes the first preset value as the length of a long axis and takes the second preset value as the length of a short axis, wherein the direction of the long axis is parallel to the advancing direction of the track point; determining a second distance from the intersection point of the straight line where the central point and the track point are located and the first elliptical boundary to the central point according to the first elliptical boundary; and optimizing the pose of the robot at the track point according to the first distance, the second distance and a preset barrier cost function.
In this embodiment, for clarity of description, step 204 is set as a step subsequent to step 203. However, those skilled in the art can understand that in practical applications, step 204 only needs to be set after step 202, and step 204 can be set as any step after step 202 as needed, and this embodiment is not limited in this embodiment.
The above description is only for illustrative purposes and does not limit the technical aspects of the present invention.
Compared with the prior art, the path optimization method provided by the embodiment determines that the obstacle exists in the navigation map of the robot before optimization, and avoids resource waste caused by operation of the path optimization method when the obstacle does not exist in the navigation map. In the optimization process, the path is optimized by the oval boundary taking the central point of the expansion area of the barrier as the center, so that the optimized path is closer to an oval, and the optimized path is smoother and safer. In addition, compared with the route which is approximately circular, the route which is approximately elliptical has smaller transverse span, is easier to pass through narrower roads, has shorter route and has higher robot motion efficiency.
The steps of the above methods are divided for clarity, and the implementation may be combined into one step or split some steps, and the steps are divided into multiple steps, so long as the same logical relationship is included, which are all within the protection scope of the present patent; it is within the scope of the patent to add insignificant modifications to the algorithms or processes or to introduce insignificant design changes to the core design without changing the algorithms or processes.
A third embodiment of the present invention relates to a path optimizing apparatus, as shown in fig. 8, including: an acquisition module 301 and an optimization module 302. The acquisition module 301 is used for acquiring a path planned based on a navigation map of the robot; the optimization module 302 is configured to perform the following operations for the track points on the path, respectively, so as to optimize the path: determining the position information of the central point of the expansion area of the barrier closest to the track point and the first distance from the track point to the central point; determining a first ellipse boundary which takes the central point as the center, takes the first preset value as the length of a long axis and takes the second preset value as the length of a short axis, wherein the direction of the long axis is parallel to the advancing direction of the track point; determining a second distance from the intersection point of the straight line where the central point and the track point are located and the first elliptical boundary to the central point according to the first elliptical boundary; optimizing the pose of the robot at the track point according to the first distance, the second distance and a preset barrier cost function; the obstacle cost function indicates a constrained relationship of the first distance, the second distance, and the obstacle cost of the trajectory point.
It should be understood that this embodiment is a system example corresponding to the first embodiment, and may be implemented in cooperation with the first embodiment. The related technical details mentioned in the first embodiment are still valid in this embodiment, and are not described herein again in order to reduce repetition. Accordingly, the related-art details mentioned in the present embodiment can also be applied to the first embodiment.
It should be noted that each module referred to in this embodiment is a logical module, and in practical applications, one logical unit may be one physical unit, may be a part of one physical unit, and may be implemented by a combination of multiple physical units. In addition, in order to highlight the innovative part of the present invention, elements that are not so closely related to solving the technical problems proposed by the present invention are not introduced in the present embodiment, but this does not indicate that other elements are not present in the present embodiment.
A fourth embodiment of the present invention relates to an electronic apparatus, as shown in fig. 9, including: at least one processor 401; and a memory 402 communicatively coupled to the at least one processor 401; the memory 402 stores instructions executable by the at least one processor 401, and the instructions are executed by the at least one processor 401, so that the at least one processor 401 can execute the path optimization method according to the above embodiments.
A fifth embodiment of the present invention relates to a robot, as shown in fig. 10, including: at least one processor 501; and a memory 502 communicatively coupled to the at least one processor 501; wherein the memory 502 stores instructions executable by the at least one processor 501, the instructions being executable by the at least one processor 501 to enable the at least one processor 501 to perform the path optimization method as mentioned in the above embodiments.
The electronic device includes: one or more processors and memory, one processor being exemplified in fig. 9 and 10. The processor and the memory may be connected by a bus or other means, and the bus connection is exemplified in fig. 9 and 10. The memory, which is a non-volatile computer-readable storage medium, may be used to store non-volatile software programs, non-volatile computer-executable programs, and modules. The processor executes various functional applications of the device and data processing by executing nonvolatile software programs, instructions, and modules stored in the memory, that is, implements the path optimization method described above.
The memory may include a storage program area and a storage data area, wherein the storage program area may store an operating system, an application program required for at least one function; the storage data area may store a list of options, etc. Further, the memory may include high speed random access memory, and may also include non-volatile memory, such as at least one magnetic disk storage device, flash memory device, or other non-volatile solid state storage device. In some embodiments, the memory optionally includes memory located remotely from the processor, and these remote memories may be connected to the external device via a network. Examples of such networks include, but are not limited to, the internet, intranets, local area networks, mobile communication networks, and combinations thereof.
One or more modules are stored in the memory and, when executed by the one or more processors, perform the path optimization method of any of the method embodiments described above.
The electronic device or the robot may execute the method provided by the embodiments of the present application, and have functional modules and beneficial effects corresponding to the execution method, and reference may be made to the method provided by the embodiments of the present application for technical details that are not described in detail in the embodiments of the present application.
A sixth embodiment of the present invention relates to a computer-readable storage medium storing a computer program. The computer program realizes the above-described method embodiments when executed by a processor.
That is, as can be understood by those skilled in the art, all or part of the steps in the method according to the above embodiments may be implemented by a program instructing related hardware, where the program is stored in a storage medium and includes several instructions to enable a device (which may be a single chip, a chip, or the like) or a processor (processor) to execute all or part of the steps in the method according to the embodiments of the present application. And the aforementioned storage medium includes: a U-disk, a removable hard disk, a Read-Only Memory (ROM), a Random Access Memory (RAM), a magnetic disk or an optical disk, and other various media capable of storing program codes.
It will be understood by those of ordinary skill in the art that the foregoing embodiments are specific examples for carrying out the invention, and that various changes in form and details may be made therein without departing from the spirit and scope of the invention in practice.
Claims (10)
1. A path optimization method applied to a robot, comprising:
acquiring a path planned based on a navigation map of the robot;
for the track points on the path, respectively optimizing the path by:
determining position information of a central point of an expansion area of the barrier closest to the track point and a first distance from the track point to the central point;
determining a first elliptical boundary which takes the central point as a center, takes a first preset value as the length of a long axis and takes a second preset value as the length of a short axis, wherein the direction of the long axis is parallel to the advancing direction of the track point;
according to the first ellipse boundary, determining a second distance from the center point to the center point by the intersection point of the straight line where the center point and the track point are located and the first ellipse boundary;
optimizing the pose of the robot at the track point according to the first distance, the second distance and a preset barrier cost function; the obstacle cost function indicates a constraint relationship of the first distance, the second distance and the obstacle cost of the track point; the obstacle cost function is:
formula a:
where error represents the obstacle cost, weight represents the weight of the obstacle cost function, obsdistDenotes a first distance, avodistRepresenting the second distance.
2. The method according to claim 1, wherein the determining a first ellipse boundary with the center point as a center, the first preset value as a long axis length, the second preset value as a short axis length, and the long axis direction parallel to the traveling direction of the track point specifically includes:
acquiring the traveling direction of the robot at the track point;
according to the advancing direction, establishing a rectangular coordinate system which takes the track point as an origin, the advancing direction as a long axis direction and a direction perpendicular to the advancing direction as a short axis direction;
according to the first preset value and the second preset value, constructing a second elliptical boundary which takes the origin as the center, the first preset value as the length of the long axis and the second preset value as the length of the short axis;
and translating the second elliptical boundary according to the position information of the central point and the position information of the track point so as to enable the center of the second elliptical boundary to be overlapped with the central point to obtain the first elliptical boundary.
3. The method according to claim 1, wherein the determining a first ellipse boundary with the center point as a center, the first preset value as a long axis length, the second preset value as a short axis length, and the long axis direction parallel to the traveling direction of the track point specifically includes:
acquiring the traveling direction of the robot at the track point;
rotating a second elliptical boundary of the obstacle according to the advancing direction and the major axis direction of the second elliptical boundary so that the major axis direction of the rotated second elliptical boundary is the same as the advancing direction; the second elliptical boundary of the obstacle is an elliptical boundary which takes the center point of the expansion area of the obstacle as the center, takes the first preset value as the length of the long axis and takes the second preset value as the length of the short axis;
and taking the rotated second ellipse boundary as the first ellipse boundary.
4. The path optimization method according to claim 2 or 3, wherein the first preset value is greater than the length of the robot and the second preset value is greater than the width of the robot.
5. The path optimization method according to claim 1, wherein the optimizing the pose of the robot at the track point according to the first distance, the second distance, and a preset obstacle cost function specifically comprises:
and optimizing the pose of the track point by adopting a Leibberg Marquard LM algorithm according to the first distance, the second distance and the barrier cost function.
6. The path optimization method according to claim 1, wherein before performing the subsequent operations on the track points on the path, respectively, the path optimization method further comprises:
determining that an obstacle exists in a navigation map of the robot.
7. The path optimization method according to claim 1, wherein after the obtaining of the path planned based on the navigation map of the robot, the path optimization method further comprises:
and optimizing the path based on a shortest path optimization algorithm.
8. An electronic device, comprising: at least one processor; and the number of the first and second groups,
a memory communicatively coupled to the at least one processor; wherein the memory stores instructions executable by the at least one processor to enable the at least one processor to perform the path optimization method of any one of claims 1 to 7.
9. A robot, comprising: at least one processor; and the number of the first and second groups,
a memory communicatively coupled to the at least one processor; wherein the memory stores instructions executable by the at least one processor to enable the at least one processor to perform the path optimization method of any one of claims 1 to 7.
10. A computer-readable storage medium, storing a computer program, wherein the computer program, when executed by a processor, implements the path optimization method of any one of claims 1 to 7.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911420881.1A CN111121812B (en) | 2019-12-31 | 2019-12-31 | Path optimization method, electronic device and storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911420881.1A CN111121812B (en) | 2019-12-31 | 2019-12-31 | Path optimization method, electronic device and storage medium |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN111121812A CN111121812A (en) | 2020-05-08 |
| CN111121812B true CN111121812B (en) | 2022-04-08 |
Family
ID=70507536
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201911420881.1A Active CN111121812B (en) | 2019-12-31 | 2019-12-31 | Path optimization method, electronic device and storage medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111121812B (en) |
Families Citing this family (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112256039B (en) * | 2020-11-05 | 2023-05-26 | 深圳市锐曼智能装备有限公司 | Cleaning robot control system and cleaning route generation method |
| CN114791730A (en) * | 2021-01-26 | 2022-07-26 | 深圳市普渡科技有限公司 | Robot path planning method, robot and medium |
| CN112987724B (en) * | 2021-02-04 | 2023-05-02 | 京东科技信息技术有限公司 | Path optimization method, path optimization device, robot and storage medium |
| CN114353801B (en) * | 2021-12-31 | 2024-02-27 | 未岚大陆(北京)科技有限公司 | Self-mobile device and navigation method thereof |
| CN114740849B (en) * | 2022-04-07 | 2023-07-04 | 哈尔滨工业大学（深圳） | Mobile robot autonomous navigation method and device based on pedestrian walking decision rule |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2011258039A (en) * | 2010-06-10 | 2011-12-22 | Panasonic Corp | Position detector |
| CN106774329A (en) * | 2016-12-29 | 2017-05-31 | 大连理工大学 | A kind of robot path planning method based on oval tangent line construction |
| CN106774347A (en) * | 2017-02-24 | 2017-05-31 | 安科智慧城市技术（中国）有限公司 | Robot path planning method, device and robot under indoor dynamic environment |
| CN108664020A (en) * | 2018-04-11 | 2018-10-16 | 上海大学 | A kind of unmanned boat dynamic obstacle avoidance algorithm based on Speed Obstacles method and dynamic window method |
| CN110399807A (en) * | 2019-07-04 | 2019-11-01 | 深圳前海达闼云端智能科技有限公司 | Detect method, apparatus, readable storage medium storing program for executing and the electronic equipment of ground obstacle |
Family Cites Families (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109375625B (en) * | 2018-11-12 | 2021-06-01 | 智慧航海（青岛）科技有限公司 | Intelligent ship path planning method based on rapid search genetic algorithm |
| CN110470308B (en) * | 2019-09-06 | 2021-09-24 | 北京云迹科技有限公司 | Obstacle avoidance system and method |
-
2019
- 2019-12-31 CN CN201911420881.1A patent/CN111121812B/en active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2011258039A (en) * | 2010-06-10 | 2011-12-22 | Panasonic Corp | Position detector |
| CN106774329A (en) * | 2016-12-29 | 2017-05-31 | 大连理工大学 | A kind of robot path planning method based on oval tangent line construction |
| CN106774347A (en) * | 2017-02-24 | 2017-05-31 | 安科智慧城市技术（中国）有限公司 | Robot path planning method, device and robot under indoor dynamic environment |
| CN108664020A (en) * | 2018-04-11 | 2018-10-16 | 上海大学 | A kind of unmanned boat dynamic obstacle avoidance algorithm based on Speed Obstacles method and dynamic window method |
| CN110399807A (en) * | 2019-07-04 | 2019-11-01 | 深圳前海达闼云端智能科技有限公司 | Detect method, apparatus, readable storage medium storing program for executing and the electronic equipment of ground obstacle |
Non-Patent Citations (1)
| Title |
|---|
| 基于贝叶斯概率估计的智能电动车;盛鹏程 等;《中国公路学报》;20190630;第32卷(第6期);第96-104页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN111121812A (en) | 2020-05-08 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111121812B (en) | Path optimization method, electronic device and storage medium | |
| EP3805073B1 (en) | Automated vehicular lane changing method and apparatus | |
| CN109214248B (en) | Method and device for identifying laser point cloud data of unmanned vehicle | |
| US11281229B2 (en) | Method and apparatus for outputting obstacle information | |
| KR102395283B1 (en) | Apparatus for controlling automatic driving, system having the same and method thereof | |
| CN111413721B (en) | Vehicle positioning method, device, controller, intelligent vehicle and system | |
| US20220410939A1 (en) | Collision detection method, electronic device, and medium | |
| CN109916421B (en) | Path planning method and device | |
| CN111142530B (en) | Method for determining running track of robot, robot and storage medium | |
| CN113188562B (en) | Path planning method and device for travelable area, electronic equipment and storage medium | |
| US20220144265A1 (en) | Moving Track Prediction Method and Apparatus | |
| CN112327329B (en) | Obstacle avoidance method, target device, and storage medium | |
| CN109341698B (en) | Path selection method and device for mobile robot | |
| CN111680747A (en) | Method and apparatus for closed loop detection of occupancy grid subgraph | |
| KR20180092960A (en) | High-speed search randomization Feedback-based motion planning | |
| EP4172707A1 (en) | An edge computing based path planning system for agv with intelligent deviation correction algorithm | |
| CN113467476A (en) | Non-collision detection rapid stochastic tree global path planning method considering corner constraint | |
| US11383704B2 (en) | Enhanced vehicle operation | |
| US11403779B2 (en) | Methods, apparatuses, systems, and storage media for loading visual localization maps | |
| JP2018206038A (en) | Point group data processing device, mobile robot, mobile robot system, and point group data processing method | |
| CN116279596B (en) | Vehicle control method, apparatus, electronic device, and computer-readable medium | |
| CN115981311A (en) | Path planning method and system for complex pipeline maintenance robot | |
| CN115855095A (en) | Autonomous navigation method and device and electronic equipment | |
| CN115562290A (en) | Robot path planning method based on A-star penalty control optimization algorithm | |
| EP4205084A1 (en) | Multi-agent map generation |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| TA01 | Transfer of patent application right |
Effective date of registration: 20210209 Address after: 200245 2nd floor, building 2, no.1508, Kunyang Road, Minhang District, Shanghai Applicant after: Dalu Robot Co.,Ltd. Address before: 518000 Room 201, building A, No. 1, Qian Wan Road, Qianhai Shenzhen Hong Kong cooperation zone, Shenzhen, Guangdong (Shenzhen Qianhai business secretary Co., Ltd.) Applicant before: Shenzhen Qianhaida Yunyun Intelligent Technology Co.,Ltd. |
|
| TA01 | Transfer of patent application right | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant | ||
| CP03 | Change of name, title or address |
Address after: 200245 Building 8, No. 207, Zhongqing Road, Minhang District, Shanghai Patentee after: Dayu robot Co.,Ltd. Address before: 200245 2nd floor, building 2, no.1508, Kunyang Road, Minhang District, Shanghai Patentee before: Dalu Robot Co.,Ltd. |
|
| CP03 | Change of name, title or address |