CN114995416A - Global path navigation method, device, terminal equipment and storage medium - Google Patents
Global path navigation method, device, terminal equipment and storage medium Download PDFInfo
- Publication number
- CN114995416A CN114995416A CN202210603086.1A CN202210603086A CN114995416A CN 114995416 A CN114995416 A CN 114995416A CN 202210603086 A CN202210603086 A CN 202210603086A CN 114995416 A CN114995416 A CN 114995416A
- Authority
- CN
- China
- Prior art keywords
- mobile robot
- path
- global path
- narrow channel
- point
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 57
- 238000003860 storage Methods 0.000 title claims abstract description 9
- 238000001514 detection method Methods 0.000 claims description 27
- 238000013507 mapping Methods 0.000 claims description 18
- 230000008569 process Effects 0.000 claims description 15
- 238000004590 computer program Methods 0.000 claims description 12
- 238000000926 separation method Methods 0.000 claims description 4
- 230000004888 barrier function Effects 0.000 claims description 3
- 230000001276 controlling effect Effects 0.000 claims 10
- 230000002596 correlated effect Effects 0.000 claims 1
- 238000004422 calculation algorithm Methods 0.000 description 15
- 238000010586 diagram Methods 0.000 description 6
- 230000006870 function Effects 0.000 description 6
- 230000003993 interaction Effects 0.000 description 6
- 238000010408 sweeping Methods 0.000 description 4
- 238000004519 manufacturing process Methods 0.000 description 3
- 238000004364 calculation method Methods 0.000 description 2
- 238000013500 data storage Methods 0.000 description 2
- 238000005259 measurement Methods 0.000 description 2
- 239000011664 nicotinic acid Substances 0.000 description 2
- 238000003825 pressing Methods 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 238000005070 sampling Methods 0.000 description 2
- 238000013528 artificial neural network Methods 0.000 description 1
- 230000003190 augmentative effect Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 238000009826 distribution Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000002068 genetic effect Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012216 screening Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
- G05D1/0238—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors
- G05D1/024—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors in combination with a laser
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0255—Control of position or course in two dimensions specially adapted to land vehicles using acoustic signals, e.g. ultra-sonic singals
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Aviation & Aerospace Engineering (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Optics & Photonics (AREA)
- Electromagnetism (AREA)
- Acoustics & Sound (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The application is applicable to the technical field of path planning, and provides a global path navigation method, a device, terminal equipment and a storage medium, wherein a global path from the current position of a mobile robot to a target position is planned based on a cost map, a grid in the cost map, which is within a preset expansion radius from an obstacle, has a first generation value, a grid outside the preset expansion radius from the obstacle has a second generation value which is smaller than the first generation value and is negatively related to the distance between the grid and the obstacle; controlling the mobile robot to move in the working environment along the global path; traversing a first path point within a first preset distance from the mobile robot along the global path; if a second path point with the cost value larger than a preset cost threshold value exists in the first path point, determining that the second path point is in a narrow channel; the mobile robot is controlled to move in the working environment based on the narrow channel, the narrow channel on the moving path of the mobile robot can be accurately detected, and therefore the narrow channel passing capacity of the mobile robot is improved.
Description
Technical Field
The present application belongs to the technical field of path planning, and in particular, to a global path navigation method, apparatus, terminal device, and storage medium.
Background
In some application environments of mobile robots, there are often a large number of narrow passages. The narrow passage throughput capability of a mobile robot directly determines its navigation flexibility. On one hand, the method for improving the narrow passage passing capacity of the mobile robot can start from reducing the size of the mobile robot, but the stability of the mobile robot during movement is weakened; on the other hand, the capacity of path planning and control algorithm of the mobile robot is improved, so that the mobile robot can be accurately controlled to safely pass through the narrow channel on the premise that the size of the mobile robot is allowed. Before the mobile robot is controlled to pass through the narrow channel, the narrow channel needs to be detected, and the detection precision of the narrow channel directly influences the narrow channel passing capacity of the mobile robot.
Disclosure of Invention
The embodiment of the application provides a global path navigation method, a global path navigation device, terminal equipment and a storage medium, which can accurately detect a narrow channel on a motion path of a mobile robot.
A first aspect of an embodiment of the present application provides a global path navigation method, including:
planning a global path from the current position of the mobile robot to a target position based on a cost map of the working environment, wherein a grid in a preset expansion radius away from an obstacle in the cost map has a first generation value, a grid outside the preset expansion radius away from the obstacle has a second generation value, and the second generation value is smaller than the first generation value and is inversely related to the distance between the grid and the obstacle;
controlling the mobile robot to move in the work environment along the global path;
in the process that the mobile robot moves in the working environment along the global path, traversing a first path point within a first preset distance from the mobile robot along the global path, and detecting a cost value of the first path point;
if a second path point with the cost value larger than a preset cost threshold exists in the first path point, determining that the second path point is in a narrow channel;
controlling the mobile robot to move in the work environment based on the narrow channel.
A second aspect of an embodiment of the present application provides a global path navigation apparatus, including:
the path planning unit is used for planning a global path from the current position of the mobile robot to a target position based on a cost map of a working environment, wherein a grid in a preset expansion radius away from an obstacle in the cost map has a first generation value, a grid outside the preset expansion radius away from the obstacle has a second generation value, and the second generation value is smaller than the first generation value and is negatively related to the distance between the grid and the obstacle;
a first motion control unit for controlling the mobile robot to move in the work environment along the global path;
the cost value detection unit is used for traversing a first path point within a first preset distance from the mobile robot along the global path in the process that the mobile robot moves in the working environment along the global path, and detecting the cost value of the first path point;
the narrow channel detection unit is used for determining that a second path point with a cost value larger than a preset cost threshold exists in the first path point;
a second motion control unit for controlling the mobile robot to move in the work environment based on the narrow passage.
A third aspect of embodiments of the present application provides a robot, including a processor and a computer program stored in the memory and executable on the processor, where the processor implements the steps of the global path navigation method according to the first aspect of embodiments of the present application when executing the computer program.
A fourth aspect of embodiments of the present application provides a computer-readable storage medium, which stores a computer program, and which, when executed by a processor, implements the steps of the global path navigation method according to the first aspect of embodiments of the present application.
According to the global path navigation method provided by the first aspect of the embodiment of the application, firstly, a global path from the current position of the mobile robot to the target position is planned based on a cost map, the mobile robot is controlled to move in an operation environment along the global path, a grid in the cost map, which is within a preset expansion radius from an obstacle, has a first generation value, a grid outside the preset expansion radius from the obstacle has a second generation value, and the second generation value is smaller than the first generation value and is negatively related to the distance between the grid and the obstacle; then, traversing first path points within a first preset distance from the mobile robot along the global path, and if second path points with cost values larger than a preset cost threshold exist in the first path points, determining that the second path points are in narrow channels; finally, the mobile robot is controlled to move in the working environment based on the narrow channel, and the narrow channel on the moving path of the mobile robot can be accurately detected, so that the narrow channel passing capacity of the mobile robot is improved.
It is to be understood that, for the beneficial effects of the second aspect to the fourth aspect, reference may be made to the relevant description in the first aspect, and details are not described herein again.
Drawings
In order to more clearly illustrate the technical solutions in the embodiments of the present application, the drawings needed to be used in the embodiments or the prior art descriptions will be briefly described below, and it is obvious that the drawings in the following description are only some embodiments of the present application, and it is obvious for those skilled in the art to obtain other drawings without creative efforts.
Fig. 1 is a first flowchart of a global path navigation method according to an embodiment of the present application;
fig. 2 is a second flowchart of a global path navigation method according to an embodiment of the present application;
fig. 3 is a third flowchart illustrating a global path navigation method according to an embodiment of the present application;
fig. 4 is a fourth flowchart illustrating a global path navigation method according to an embodiment of the present application;
fig. 5 is a fifth flowchart illustrating a global path navigation method according to an embodiment of the present application;
fig. 6 is a sixth flowchart of a global path navigation method according to an embodiment of the present application;
FIG. 7 is a schematic diagram of a detection block provided in an embodiment of the present application;
FIG. 8 is a schematic diagram of an image mapped with ranging data points according to an embodiment of the present disclosure
FIG. 9 is a schematic structural diagram of a global path navigation device according to an embodiment of the present application;
fig. 10 is a schematic structural diagram of a terminal device according to an embodiment of the present application.
Detailed Description
In the following description, for purposes of explanation and not limitation, specific details are set forth, such as particular system structures, techniques, etc. in order to provide a thorough understanding of the embodiments of the present application. It will be apparent, however, to one skilled in the art that the present application may be practiced in other embodiments that depart from these specific details. In other instances, detailed descriptions of well-known systems, devices, circuits, and methods are omitted so as not to obscure the description of the present application with unnecessary detail.
It will be understood that the terms "comprises" and/or "comprising," when used in this specification and the appended claims, specify the presence of stated features, integers, steps, operations, elements, and/or components, but do not preclude the presence or addition of one or more other features, integers, steps, operations, elements, components, and/or groups thereof.
It should also be understood that the term "and/or" as used in this specification and the appended claims refers to and includes any and all possible combinations of one or more of the associated listed items.
Furthermore, in the description of the present application and the appended claims, the terms "first," "second," "third," and the like are used for distinguishing between descriptions and not necessarily for describing or implying relative importance.
Reference throughout this specification to "one embodiment" or "some embodiments," or the like, means that a particular feature, structure, or characteristic described in connection with the embodiment is included in one or more embodiments of the present application. Thus, appearances of the phrases "in one embodiment," "in some embodiments," "in other embodiments," or the like, in various places throughout this specification are not necessarily all referring to the same embodiment, but rather "one or more but not all embodiments" unless specifically stated otherwise. The terms "comprising," "including," "having," and variations thereof mean "including, but not limited to," unless expressly specified otherwise.
The embodiment of the application provides a global path navigation method, which can be executed by a processor of a terminal device when running a corresponding computer program, and can improve the narrow channel passing capacity of a mobile robot by accurately detecting a narrow channel on a motion path of the mobile robot and controlling the mobile robot to move in a working environment based on the narrow channel.
In application, the terminal device may be a Mobile robot, or may be a computing device that can communicate with the Mobile robot, such as a (cloud) server, a Mobile phone, a tablet Computer, a wearable device, an Augmented Reality (AR)/Virtual Reality (VR) device, a notebook Computer, a super-Mobile Personal Computer (UMPC), a netbook, a Personal Digital Assistant (PDA), and the like, so as to control the Mobile robot. The mobile robot may be any type of robot with working and moving functions, such as a sweeping robot, a killing robot, a plant protection drone, an automated guided vehicle, etc.
As shown in fig. 1, the global path navigation method provided in the embodiment of the present application includes the following steps S100 to S105:
step S100, expanding the grid map of the working environment to generate a cost map, and entering step S101.
In application, the work environment is any geographical location area in which the mobile robot is currently located, working or waiting for working. The working environment is different according to different types of the mobile robot, for example, when the mobile robot is a sweeping robot, the working environment is a home place, an office place, a production place and the like where the sweeping robot is sweeping or is to be swept; when the mobile robot is a killing robot, the working environment is a home place, an office place, a production place and the like where the killing robot is killing or waiting to kill; when the mobile robot is a plant protection robot, the working environment is farmlands, gardens and the like where the plant protection robot is performing plant protection operation or is to perform plant protection operation; when the mobile robot is an automated guided vehicle, the working environment is a warehouse, a production place, or the like in which the automated guided vehicle is working or is to perform work.
In application, before generating a cost map, rasterizing an electronic map of a working environment to generate a raster map of the working environment; and traversing the grid map, assigning grids with the distance from the grid occupied by the obstacle within the preset expansion radius in the grid map as a first generation value, and assigning grids with the distance from the grid occupied by the obstacle outside the preset expansion radius in the grid map as a second generation value to generate a cost map. The first cost value may be set to a fixed value that is greater than the second cost value, which should be less than the first cost value and inversely related to the distance of the grid from the grid occupied by the obstacle. The distance between any two grids may be the straight-line distance between the geometric center points of the two grids.
In one embodiment, the calculation formula of the cost value of the grid in the cost map is as follows:
wherein m is 0 Represents a cost value, m 1 Representing the first cost value, p represents the distance of the grid from the obstacle, r represents the preset expansion radius, m 2 Representing the second generation value and k representing the constant coefficient.
In an application, the cost value of a grid may represent a gray value of the grid in an electronic map, or a probability that the grid is occupied by an obstacle. When the cost value represents the gray value of the grid, the gray value of the grid occupied by the obstacle is different from that of the grid not occupied by the obstacle in the cost map, so that the grids occupied by the obstacle and not occupied by the obstacle in the cost map can be obviously distinguished. The first generation value and the second generation value can be set by a user through a human-computer interaction device of the terminal equipment according to actual needs, for example, when the generation value represents a gray value and the gray value has a brightness value from 0% (white) to 100% (black) and a gray level from 0 (black) to 255 (white), the first generation value is a gray value with A brightness or a gray level, the second generation value is a gray value with B brightness or B gray level, B is more than or equal to 0% and less than or equal to C and less than or equal to A and less than or equal to 100%, B is in negative correlation with the distance of the grid from the grid occupied by the obstacle, a is more than or equal to 0% and less than or equal to C and less than or equal to 255, and B is in positive correlation with the distance of the grid from the grid occupied by the obstacle; or, when the cost value represents the probability, the probability that the first cost value is 100%, the probability that the second cost value is D, and D is more than or equal to 0% and less than 100%, and is inversely related to the distance of the grid from the grid occupied by the obstacle.
In application, the preset expansion radius may be set to be less than or equal to the radius of the mobile robot. When the mobile robot is non-circular, the radius of the mobile robot may be the radius of the smallest circumscribed circle of the mobile robot. In order to increase the throughput of the mobile robot through the narrow passage, the preset expansion radius may be set to be slightly lower than the radius of the mobile robot, for example, the range of the preset expansion radius is set to [ 90% of the radius of the mobile robot, 100% of the radius of the mobile robot ").
In application, the cost values after all grids in the cost map are assigned with values may be stored in a memory, for example, a data storage list may be preset in the memory, and the cost values after all grids in the cost map are assigned with values may be stored in the data storage list. The corresponding relation between the cost map and the corresponding operation environment can be established in the memory, so that when the mobile robot needs to be controlled to operate the operation environment in the follow-up process, the corresponding cost map can be quickly searched, and the calculation resources and the execution time of the processor are effectively saved. The corresponding relationship may be a mapping relationship, and may exist in a form of a corresponding relationship Table, and the corresponding relationship Table may be a Look-Up-Table (LUT), and may also exist in a form that a corresponding search result may be searched and output by using other input data.
In application, for any operation environment, the terminal device only needs to execute step S100 once, and generates and stores a cost map according to the grid map of the operation environment, so as to be used when planning a global path of the mobile robot moving from the current position to the target position of the operation environment based on the cost map in the following. If the working environment changes (for example, the number, position or size of obstacles in the working environment changes), the grid map of the working environment needs to be updated, and a new cost map is regenerated according to the updated grid map, so as to update the cost map.
S101, planning a global path from the current position of the mobile robot to a target position based on the cost map;
step S102, controlling the mobile robot to move in the working environment along the global path, and proceeding to step S103.
In the application, after a cost map is generated, a global path from the current position of the mobile robot to a target position in the cost map is planned based on a path planning method according to the actual operation requirement of an operation environment, then the global path is mapped to the real operation environment according to a scaling scale based on the cost map, the mobile robot is controlled to move in the operation environment along the global path based on the current pose of the mobile robot, so that the global operation of the operation environment is realized, and meanwhile, the real-time position of the mobile robot is marked in the cost map. The target position is the mapping of the work end point of the mobile robot in the work environment in the cost map.
In application, the path planning method suitable for the mobile robot can be a traditional path planning algorithm, a sampling-based path planning algorithm, an intelligent bionic algorithm and the like. The traditional path planning algorithm can be an A algorithm, a Dijkstra algorithm, a D algorithm, an artificial potential field method and the like, the sampling-based path planning algorithm can be a PRM algorithm, an RRT algorithm and the like, and the intelligent bionic path planning algorithm can be a neural network algorithm, an ant colony algorithm, a genetic algorithm and the like.
Step S103, traversing a first path point within a first preset distance from the mobile robot along the global path, detecting a cost value of the first path point, and entering step S104.
In the application, in the process that the mobile robot moves in the working environment along the global path, path points (called as first path points) with the distance between the mobile robot and the global path within a first preset distance are traversed in the cost map from the starting moment of the mobile robot moving in the working environment along the global path, and the cost values of the grids where all the traversed first path points are located are detected. Since each grid has been assigned as the first generation value or the second generation value in step S100, the detected cost value of the grid on which each first path point is located is the first generation value or the second generation value.
In the application, because the walls or the barriers on the two sides of the narrow channel belong to the obstacles in the grid map and the cost map, the detection of the narrow channel can be realized by detecting the grids occupied by the obstacles and the grids around the obstacles, and because the grids occupied by the obstacles and the grids around the obstacles are assigned with different cost values in step S100, the detection of the narrow channel can be realized by detecting the cost values of the grids occupied by the obstacles and the cost values of the grids around the obstacles.
In application, the first preset distance may be set according to actual needs, and the first preset distance should be set to be greater than or equal to the width of one grid, so that the terminal device traverses at least one grid at a time. In order to improve the detection accuracy of the narrow channel and facilitate subsequent control of the mobile robot to smoothly pass through or avoid the narrow channel, the terminal device may be set to detect at most one narrow channel at a time, and based on the setting, the first preset distance may be set to be less than or equal to a distance between two obstacles with a minimum distance in the grid map (or cost map), so as to avoid that the terminal device mistakenly recognizes that a plurality of narrow channels are detected at one time as detecting one narrow channel.
Step S104, if a second path point with the cost value larger than a preset cost threshold exists in the first path point, determining that the second path point is in a narrow channel, and entering step S105.
In application, since the narrow channel generally refers to a channel having a width smaller than, equal to, or slightly larger than the diameter (twice the radius) of the mobile robot, and the grids within the preset expansion radius of the obstacle (including the walls or the barriers on both sides of the narrow channel) have been assigned as the first generation value in step S100, and the grids outside the preset expansion radius of the obstacle have been assigned as the second generation value, the cost value of the grid where the path point in the narrow channel is located is necessarily equal to the first generation value or a larger second generation value, and accordingly, the preset cost threshold may be set to be larger than or equal to the third generation value, and the third generation value is equal to the second generation value of the grid where the distance to the obstacle is larger than the preset distance threshold. The preset distance threshold is equal to the preset expansion radius + E radius of the mobile robot, and the value range of E can be set to 0% -10%.
And S105, controlling the mobile robot to move in the working environment based on the narrow channel.
In application, when it is determined that a narrow channel exists within a first preset distance of a current position of the mobile robot when the mobile robot moves along a global path, corresponding measures can be taken according to the position of the narrow channel to control the mobile robot to move in a working environment, so that the mobile robot can smoothly pass through the narrow channel, or the global path is replanned to bypass the narrow channel.
As shown in fig. 2, in one embodiment, step S105 includes:
and step S201, controlling the mobile robot to reduce the movement speed to pass through the narrow channel.
In application, in order to enable the mobile robot to safely pass through the narrow passage and avoid collision between the mobile robot and obstacles on two sides of the narrow passage, the movement speed of the mobile robot can be reduced, so that the mobile robot can pass through the narrow passage at a low speed. Specifically, the current movement speed of the mobile robot may be obtained, when the current movement speed of the mobile robot is greater than a preset movement speed threshold, the current movement speed of the mobile robot is reduced to the preset movement speed threshold, and when the current movement speed of the mobile robot is less than or equal to the preset movement speed threshold, the current movement speed of the mobile robot is kept unchanged.
As shown in fig. 3, in one embodiment, step S105 includes the following steps S301 and S302:
step S301, replanning a new global path from the current position of the mobile robot to the target position, wherein the distance from a third path point of the narrow channel in the new global path to the central point of the narrow channel is smaller than a second preset distance, and the step S302 is entered;
and S302, controlling the mobile robot to move in the working environment along the new global path.
In application, when it is determined that a narrow channel exists within a first preset distance of a current position of the mobile robot when the mobile robot moves along the global path, a new global path from the current position of the mobile robot to a target position can be re-planned according to the position of the narrow channel, so that the mobile robot is prevented from colliding with obstacles on two sides of the narrow channel when passing through the narrow channel. Specifically, a local path (that is, a path where the second path point is located) that passes through the narrow channel in the global path may be adjusted, so that the adjusted local path (that is, a path where the third path point is located) passes through or is closer to the central point of the narrow channel, that is, a distance from the third path point to the central point of the narrow channel is smaller than a second preset distance, where the second preset distance may be set as a distance value that the third path point is closer to the central point of the narrow channel relative to the second path point according to actual needs, for example, the second preset distance is smaller than or equal to a distance between the second path point and the central point of the narrow channel. After the new global path is generated based on the third path point, the mobile robot is controlled to continue moving in the work environment along the new global path to continue working on the work environment.
As shown in fig. 4, in one embodiment, step S302 includes the following steps S401 and S402:
step S401, detecting a third path point which is within a third preset distance from the second path point and has the lowest cost value in the narrow channel, and entering step S402;
and S402, replanning a new global path from the current position of the mobile robot to the target position and passing through the third path point.
In application, in step S100, the grids in the preset expansion radius of the obstacles on the two sides of the narrow channel are assigned as the first generation value, the grids outside the preset expansion radius of the obstacles are assigned as the second generation value, and the second generation value is smaller than the first generation value and is negatively related to the distance between the grids and the obstacles, so that the distance between the second path point and the obstacles on the two sides of the narrow channel can be obtained according to the cost value of the second path point, and then the position of the second path point from the center point of the narrow channel can be obtained. Specifically, a grid within a third preset distance from the second path point in the cost map can be searched first; then, screening out the grids with the lowest cost value in the grids within a third preset distance from the second path point as third path points; and finally, replanning a new global path based on the third path point. The third preset distance may be set to be less than or equal to the maximum distance from the second path point to the obstacles on both sides of the narrow channel according to actual needs.
As shown in fig. 5, in an embodiment, the preset expansion radius is smaller than the radius of the mobile robot, and the step S105 includes the following steps S501 to S504:
step S501, acquiring the width of the narrow channel based on ranging data of a ranging sensor arranged on the mobile robot, and entering step S502;
step S502, if the width of the narrow channel is lower than a preset width threshold, adding a virtual obstacle at the position of the narrow channel in the cost map to generate a new cost map, and entering step S503;
step S503, replanning a new global path from the current position of the mobile robot to the target position and avoiding the narrow channel based on the new cost map, and entering step S504;
and step S504, controlling the mobile robot to move in the working environment along the new global path.
In application, if the preset expansion radius is set to be the same as the radius of the mobile robot, it is easy to determine that a detected part of narrow channels is a channel that cannot be passed through by the mobile robot in a cost map due to the influence of data noise, so that a virtual obstacle is added to the narrow channels that cannot be passed through, the narrow channels that cannot be passed through are blocked, a global path that enables the mobile robot to pass through the narrow channels cannot be planned, and therefore, the preset expansion radius can be set to be slightly lower than the radius of the mobile robot.
In application, if the preset expansion radius is set to be smaller than the radius of the mobile robot, the planned global path may pass through a narrow channel having a width smaller than or very close to the diameter of the mobile robot, and therefore, it is necessary to measure the actual width of the narrow channel by using a ranging sensor (e.g., a laser radar, an ultrasonic ranging sensor, an infrared ranging sensor, a depth camera (e.g., an RGBD camera), etc.) provided in the mobile robot, and if the width of the narrow channel is smaller than a preset width threshold, a virtual obstacle may be added at the position of the narrow channel in the cost map, so as to force the terminal device to re-plan a new global path avoiding the narrow channel. The preset width threshold may be set to be equal to or slightly larger than the diameter of the mobile robot according to actual needs, for example, the value range of the preset width threshold is set to [ 100% of the diameter of the mobile robot, 110% of the diameter of the mobile robot ].
As shown in fig. 6, in one embodiment, step S501 includes the following steps S601 to S607:
step S601, constructing a detection frame covering the position of the narrow channel by taking the position of the mobile robot in the working environment as a boundary point and the motion direction of the mobile robot in the working environment as a central axis direction, wherein the boundary or tangent line of the detection frame passing through the boundary point is perpendicular to the central axis, and the step S602 is executed;
step S602, generating an image of the area where the detection frame is located, and entering step S603;
step S603, mapping the global path to the image based on the current pose of the mobile robot, and entering step S604;
step S604, dividing the image into a first image area and a second image area by taking the global path as a separation line, and entering step S605;
step S605, mapping the distance measurement data of the mobile robot to the image, and entering step S606;
step S606, obtaining a distance value between each detection data in the first image region and each detection data in the second image region, and entering step S607;
step S607, acquiring the minimum distance value of all the distance values as the width of the narrow channel.
In application, the terminal device may first construct a detection frame covering the position of the narrow channel and having a suitable size according to the position of the narrow channel in the real operation environment;
then, according to ranging data covering the detection frame obtained by the ranging sensor, acquiring an image of an area where the detection frame is located at a preset resolution, wherein when the ranging sensor is a laser radar, an ultrasonic ranging sensor or an infrared ranging sensor, the ranging data is point cloud data, and the image is a point cloud image; when the ranging sensor is a depth camera, the ranging data is depth image data and the image is a depth image; the preset resolution may be set to be greater than or equal to the resolution of the cost map such that each grid corresponds to at least one point cloud data point in the point cloud data or at least one pixel point in the depth image;
then acquiring the current pose of the mobile robot, wherein the pose comprises the position coordinate and the pose of the mobile robot in an image coordinate system, the image coordinate system can be a two-dimensional coordinate system or a three-dimensional coordinate system, and correspondingly, the position coordinate can comprise a two-dimensional coordinate or a three-dimensional coordinate; establishing a mapping relation between a cost map coordinate system and an image coordinate system based on the current pose of the mobile robot, and mapping a global map to an image based on the mapping relation;
then, taking the global path mapped into the image as a separation line, dividing the image into two image areas, namely a first image area and a second image area, and mapping the ranging data to the image after the divided areas; or mapping the ranging data to the image, and then dividing the image into two image regions by using the global path mapped to the image as a separation line, that is, the execution sequence of steps S604 and S605 may be changed;
then respectively obtaining the distance value between the first ranging data point in the first image area and each ranging data point in the second image area, respectively obtaining the distance value between the second ranging data point in the first image area and each ranging data point in the second image area, … …, and so on; similarly, the distance value between the first ranging data point in the second image area and each ranging data point in the first image area may be obtained first, then the distance value between the second ranging data point in the second image area and each ranging data point in the first image area may be obtained, … …, and so on, and finally the distance value between each ranging data point in the first image area and each ranging data point in the second image area may be obtained; when the ranging sensor is a laser radar, an ultrasonic ranging sensor or an infrared ranging sensor, ranging data points are data points in the point cloud data; when the ranging sensor is a depth camera, the ranging data points are pixel points in the depth image data;
and finally, comparing the sizes of all the distance values, and taking the minimum distance value in all the distance values as the width of the narrow channel.
In application, the detection frame may be set to any regular shape according to actual needs, as long as the position of the narrow channel can be covered, for example, a rectangle, a circle, an ellipse, and the like.
As shown in fig. 7, a schematic diagram of a detection block is exemplarily shown; where 11 denotes a target position, 12 denotes a global path, 13 denotes a distance measurement data point, 14 denotes obstacles on both sides of a narrow passage, 15 denotes a detection frame, 16 denotes a mobile robot, X denotes a moving direction, and Y denotes a tangential or boundary direction.
As shown in fig. 8, a schematic diagram illustrating an image with range finding data points mapped thereto is exemplary; where 21 denotes an image, 22 denotes a global path, 23 denotes a ranging data point, 24 denotes a first image area, 25 denotes a second image area, and 26 denotes a mobile robot.
In one embodiment, step S603 includes:
establishing a mapping relation between the cost map coordinate system and the image coordinate system based on the current pose of the mobile robot;
mapping the global path to the image based on the mapping relationship.
In one embodiment, after step S103, the method further includes:
and if no second path point with the cost value larger than the preset cost threshold exists in the first path points, returning to execute the step S102.
In application, if there is no second path point with a cost value greater than the preset cost threshold in the first path point, it indicates that there is no narrow channel in the first preset distance from the mobile robot on the global path, at this time, the process may return to step S102, continue to control the mobile robot to move along the global path, and then continue to step S103.
In one embodiment, step S101 is preceded by:
and if receiving a control command for starting operation, entering a narrow channel mode.
In the application, a user may input a job start control instruction through a human-computer interaction device of the terminal device to control the mobile robot to enter a narrow channel mode according to the job start control instruction, and then perform step S101.
In one embodiment, step S105 is followed by:
and if the operation stopping control instruction is received, stopping the movement, and entering a shutdown mode, a standby mode or a charging mode.
In application, a user can input a job stopping control instruction through a human-computer interaction device of the terminal equipment to control the mobile robot to stop working on a working environment according to the job stopping control instruction, and after the working on the working environment is stopped, the mobile robot can stop moving and enter a shutdown mode, a standby mode or a charging mode. After stopping the work on the work environment, the cost map of the other work environment may be acquired, and then step S101 is executed to perform the work on the other work environment based on the global path navigation method.
In application, the human-computer interaction device of the terminal device may include at least one of an entity key, a touch sensor, a gesture recognition sensor, and a voice recognition unit, so that a user may input a movement control instruction in a corresponding touch manner, gesture control manner, or voice control manner. The physical keys and the touch sensor can be arranged at any position of the terminal device, such as a control panel. The touch manner of the physical key may be pressing or toggling. The touch manner of the touch sensor may be pressing or touching. The gesture recognition sensor may be disposed at any position outside the housing of the terminal device. The gesture for controlling the terminal device can be set by a user according to actual needs in a user-defined mode or default setting of the user when the user leaves a factory. The voice recognition unit may include a microphone and a voice recognition chip, or may include only a microphone and implement a voice recognition function by a processor of the terminal device. The voice for controlling the terminal device can be set by the user according to the actual needs in a user-defined way or by default when the user leaves the factory.
In application, step S201 and steps S301 and S302 may be executed simultaneously, and step S201 and steps S501 and S504 may also be executed simultaneously, that is, before and during the process of controlling the mobile robot to pass through the narrow passage, or before and during the process of controlling the mobile robot to avoid the narrow passage, the operation speed of the mobile robot may be reduced, so that the mobile robot may pass through or avoid the narrow passage at a low speed, and avoid collision with obstacles on both sides of the narrow passage.
It should be understood that, the sequence numbers of the steps in the foregoing embodiments do not imply an execution sequence, and the execution sequence of each process should be determined by functions and internal logic of the process, and should not constitute any limitation to the implementation process of the embodiments of the present application.
The embodiment of the present application further provides a global path navigation device, which is used for executing the method steps in the foregoing method embodiment. The device may be a virtual appliance (virtual application) in the terminal device, which is executed by a processor of the terminal device, or may be the terminal device itself.
As shown in fig. 9, the global path navigation device 1000 according to the embodiment of the present application includes:
the map generation unit 100 is configured to expand a grid map of a working environment to generate a cost map, and enter the path planning unit 101;
a path planning unit 101, configured to plan a global path from a current position of the mobile robot to a target position based on the cost map, and enter a first motion control unit 102;
a first motion control unit 102, configured to control the mobile robot to move in the working environment along the global path, and enter a cost value detection unit 103;
a cost value detection unit 103, configured to, in a process that the mobile robot moves in the working environment along the global path, traverse a first path point within a first preset distance from the mobile robot along the global path, detect a cost value of the first path point, and enter a narrow channel detection unit 104;
a narrow channel detection unit 104, configured to determine that a second path point with a cost value greater than a preset cost threshold exists in the first path point, and enter a second motion control unit 105, where the second path point is located in a narrow channel;
a second motion control unit 105 for controlling the mobile robot to move in the working environment based on the narrow channel.
In one embodiment, the global path navigation device further comprises:
and the returning unit is used for returning to the first motion control unit if a second path point with the cost value larger than a preset cost threshold value does not exist in the first path point.
In one embodiment, the global path navigation device further comprises:
and the starting unit is used for entering the narrow channel mode if receiving the operation starting control instruction.
In one embodiment, the global path navigation device further comprises:
and the stopping unit is used for stopping movement and entering a shutdown mode, a standby mode or a charging mode if receiving the operation stopping control instruction.
In application, each unit in the above apparatus may be a software program module, may be implemented by different logic circuits integrated in a processor or by a separate physical component connected to the processor, and may also be implemented by a plurality of distributed processors.
As shown in fig. 10, an embodiment of the present application further provides a terminal device 2000, including: at least one processor 201 (only one processor is shown in fig. 10), a memory 202, and a computer program 203 stored in the memory 202 and executable on the at least one processor 201, the steps in the various global path navigation method embodiments described above being implemented when the computer program 203 is executed by the processor 201.
In an application, the terminal device may include, but is not limited to, a processor and a memory, fig. 10 is merely an example of the terminal device, and does not constitute a limitation of the terminal device, and may include more or less components than those shown, or may combine some components, or different components, such as an input-output device, a network access device, and the like, and may further include a mobile component and a ranging sensor when the terminal device is a mobile robot. The input and output device may include the human-computer interaction device, and may further include a display screen for displaying the operating parameters of the terminal device. The network access device may include a communication module for the terminal device to communicate with the user terminal. The moving part can comprise a steering engine, a motor, a driver and the like for driving the joints of the mobile robot to move.
In an Application, the Processor may be a Central Processing Unit (CPU), and the Processor may be other general purpose Processor, a Digital Signal Processor (DSP), an Application Specific Integrated Circuit (ASIC), a Field Programmable Gate Array (FPGA) or other Programmable logic device, discrete Gate or transistor logic device, discrete hardware component, or the like. A general purpose processor may be a microprocessor or the processor may be any conventional processor or the like.
It should be noted that, for the information interaction, execution process, and other contents between the above-mentioned devices/modules, the specific functions and technical effects thereof are based on the same concept as those of the embodiment of the method of the present application, and reference may be made to the part of the embodiment of the method specifically, and details are not described here.
It is obvious to those skilled in the art that, for convenience and simplicity of description, the above division of each functional module is only used for illustration, and in practical applications, the above function distribution may be performed by different functional modules as required, that is, the internal structure of the apparatus is divided into different functional modules to perform all or part of the above described functions. Each functional module in the embodiments may be integrated in one processing module, or each module may exist alone physically, or two or more modules are integrated in one module, and the integrated module may be implemented in a form of hardware, or in a form of software functional module. In addition, specific names of the functional modules are only used for distinguishing one functional module from another, and are not used for limiting the protection scope of the application. The specific working process of the modules in the system may refer to the corresponding process in the foregoing method embodiment, and is not described herein again.
The embodiments of the present application further provide a computer-readable storage medium, in which a computer program is stored, and when the computer program is executed by a processor, the steps in the above-mentioned method embodiments can be implemented.
Embodiments of the present application provide a computer program product, which, when running on a terminal device, enables the terminal device to implement the steps in the above method embodiments.
The above-mentioned embodiments are only used for illustrating the technical solutions of the present application, and not for limiting the same; although the present application has been described in detail with reference to the foregoing embodiments, it should be understood by those of ordinary skill in the art that: the technical solutions described in the foregoing embodiments may still be modified, or some technical features may be equivalently replaced; such modifications and substitutions do not substantially depart from the spirit and scope of the embodiments of the present application and are intended to be included within the scope of the present application.
Claims (10)
1. A global path navigation method, comprising:
planning a global path from the current position of the mobile robot to a target position based on a cost map of the working environment, wherein a grid in a preset expansion radius away from an obstacle in the cost map has a first generation value, a grid outside the preset expansion radius away from the obstacle has a second generation value, and the second generation value is smaller than the first generation value and is inversely related to the distance between the grid and the obstacle;
controlling the mobile robot to move in the work environment along the global path;
traversing a first path point within a first preset distance from the mobile robot along the global path, and detecting a cost value of the first path point;
if a second path point with the cost value larger than a preset cost threshold exists in the first path point, determining that the second path point is in a narrow channel;
controlling the mobile robot to move in the work environment based on the narrow channel.
2. The global path navigation method of claim 1, wherein said controlling the mobile robot to move in the work environment based on the narrow channel comprises:
and controlling the mobile robot to reduce the movement speed through the narrow passage.
3. The global path navigation method of claim 1, wherein said controlling the mobile robot to move in the work environment based on the narrow channel comprises:
replanning a new global path from the current position of the mobile robot to a target position, wherein the distance from a third path point of the narrow channel in the new global path to the central point of the narrow channel is smaller than a second preset distance;
controlling the mobile robot to move in the work environment along the new global path.
4. The global path navigation method of claim 3, wherein said replanning a new global path from a current position to a target position of the mobile robot comprises:
detecting a third path point which is within a third preset distance from the second path point and has the lowest cost value in the narrow channel;
replanning a new global path from the current position of the mobile robot to the target position and through the third path point.
5. The global path navigation method of claim 1, wherein the preset inflation radius is smaller than a radius of the mobile robot;
the controlling the mobile robot to move in the work environment based on the narrow channel includes:
acquiring the width of the narrow channel based on ranging data of a ranging sensor arranged on the mobile robot;
if the width of the narrow channel is lower than a preset width threshold value, adding a virtual barrier at the position of the narrow channel in the cost map to generate a new cost map;
replanning a new global path from the current position of the mobile robot to the target position and avoiding the narrow channel based on the new cost map;
controlling the mobile robot to move in the work environment along the new global path.
6. The global path navigation method according to claim 5, wherein the obtaining the width of the narrow channel based on ranging data of a ranging sensor provided to the mobile robot includes:
constructing a detection frame covering the position of the narrow channel by taking the position of the mobile robot in the operation environment as a boundary point and the motion direction of the mobile robot in the operation environment as a central axis direction, wherein the boundary or tangent line of the detection frame passing through the boundary point is perpendicular to the central axis;
generating an image of the area where the detection frame is located;
mapping the global path to the image based on a current pose of the mobile robot;
dividing the image into a first image area and a second image area by taking the global path as a separation line;
mapping ranging data of the mobile robot to the image;
acquiring a distance value between each ranging data point in the first image area and each ranging data point in the second image area;
and acquiring the minimum distance value of all the distance values as the width of the narrow channel.
7. The global path navigation method of claim 6, wherein said mapping the global path to the image based on the current pose of the mobile robot comprises:
establishing a mapping relation between the cost map coordinate system and the image coordinate system based on the current pose of the mobile robot;
mapping the global path to the image based on the mapping relationship.
8. A global path navigation apparatus, comprising:
the path planning unit is used for planning a global path from the current position of the mobile robot to the target position based on the cost map of the working environment;
a first motion control unit for controlling the mobile robot to move in the work environment along the global path, the grid within a preset expansion radius from an obstacle in the cost map having a first generation value, the grid outside the preset expansion radius from the obstacle having a second generation value, the second generation value being less than the first generation value and negatively correlated with the distance of the grid from the obstacle;
the cost value detection unit is used for traversing a first path point within a first preset distance from the mobile robot along the global path in the process that the mobile robot moves in the working environment along the global path, and detecting the cost value of the first path point;
the narrow channel detection unit is used for determining that a second path point with a cost value larger than a preset cost threshold exists in the first path point;
a second motion control unit for controlling the mobile robot to move in the work environment based on the narrow passage.
9. A terminal device comprising a processor and a computer program stored in said memory and executable on said processor, said processor implementing the steps of the global path navigation method according to any of claims 1 to 7 when executing said computer program.
10. A computer-readable storage medium, characterized in that the computer-readable storage medium stores a computer program which, when executed by a processor, implements the steps of the global path navigation method according to any one of claims 1 to 7.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210603086.1A CN114995416A (en) | 2022-05-30 | 2022-05-30 | Global path navigation method, device, terminal equipment and storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210603086.1A CN114995416A (en) | 2022-05-30 | 2022-05-30 | Global path navigation method, device, terminal equipment and storage medium |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN114995416A true CN114995416A (en) | 2022-09-02 |
Family
ID=83031418
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210603086.1A Pending CN114995416A (en) | 2022-05-30 | 2022-05-30 | Global path navigation method, device, terminal equipment and storage medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114995416A (en) |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117021094A (en) * | 2023-08-20 | 2023-11-10 | 哈尔滨理工大学 | Path planning method of cutter changing robot of shield machine suitable for narrow space |
| CN117472067A (en) * | 2023-12-27 | 2024-01-30 | 江苏中科重德智能科技有限公司 | Robot narrow channel passing method and system based on multilayer grid map |
| WO2024114682A1 (en) * | 2022-12-02 | 2024-06-06 | 北京极智嘉科技股份有限公司 | Positioning method and apparatus, and computing device and storage medium |
-
2022
- 2022-05-30 CN CN202210603086.1A patent/CN114995416A/en active Pending
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2024114682A1 (en) * | 2022-12-02 | 2024-06-06 | 北京极智嘉科技股份有限公司 | Positioning method and apparatus, and computing device and storage medium |
| CN117021094A (en) * | 2023-08-20 | 2023-11-10 | 哈尔滨理工大学 | Path planning method of cutter changing robot of shield machine suitable for narrow space |
| CN117021094B (en) * | 2023-08-20 | 2024-04-26 | 哈尔滨理工大学 | Path planning method of cutter changing robot of shield machine suitable for narrow space |
| CN117472067A (en) * | 2023-12-27 | 2024-01-30 | 江苏中科重德智能科技有限公司 | Robot narrow channel passing method and system based on multilayer grid map |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN114995416A (en) | Global path navigation method, device, terminal equipment and storage medium | |
| US5570285A (en) | Method and apparatus for avoiding obstacles by a robot | |
| EP4102186A1 (en) | Method for constructing self-driving map and related device | |
| Castellanos et al. | Multisensor fusion for simultaneous localization and map building | |
| CN111813101B (en) | Robot path planning method, device, terminal equipment and storage medium | |
| JP2022160538A (en) | Collision detection method, device, electronic apparatus, storage medium, automatic driving vehicle, and computer program | |
| EP4296133A1 (en) | Intelligent driving method and apparatus, and storage medium and computer program | |
| CN112171675B (en) | Obstacle avoidance method and device for mobile robot, robot and storage medium | |
| CN113960995A (en) | Obstacle avoidance planning method, system and equipment | |
| CN112008718B (en) | Robot control method, system, storage medium and intelligent robot | |
| CN114219905A (en) | Map construction method and device, terminal equipment and storage medium | |
| CN114485673B (en) | Service robot crowd sensing navigation method and system based on deep reinforcement learning | |
| CN111546348A (en) | Robot position calibration method and position calibration system | |
| CN112925313B (en) | Avoidance processing method and device for robot, electronic equipment and medium | |
| KR20210037633A (en) | Method and apparatus for determining velocity of obstacle, electronic device, storage medium and program | |
| Nasti et al. | Obstacle avoidance during robot navigation in dynamic environment using fuzzy controller | |
| CN110007680B (en) | Robot obstacle avoidance algorithm based on topological relation | |
| CN115993828A (en) | Robot obstacle avoidance method, system, storage medium and electronic equipment | |
| CN113341975B (en) | Robot moving method based on variable objective function firefly optimized path | |
| CN112506203B (en) | Robot motion dynamic feedback method and system | |
| KR20220166784A (en) | Riding method, device, facility and storage medium based on autonomous driving | |
| CN114489050A (en) | Obstacle avoidance route control method, device, equipment and storage medium for straight line driving | |
| CN111862218A (en) | Computer equipment positioning method and device, computer equipment and storage medium | |
| CN118034281A (en) | Robot active obstacle avoidance control method based on dynamic environment and related device | |
| US20240215788A1 (en) | Collided position determination method, computer-readable storage medium, and robot |
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 |