CN116400711A - Path planning method, underwater robot, electronic device and storage medium - Google Patents
Path planning method, underwater robot, electronic device and storage medium Download PDFInfo
- Publication number
- CN116400711A CN116400711A CN202310658883.4A CN202310658883A CN116400711A CN 116400711 A CN116400711 A CN 116400711A CN 202310658883 A CN202310658883 A CN 202310658883A CN 116400711 A CN116400711 A CN 116400711A
- Authority
- CN
- China
- Prior art keywords
- underwater robot
- distance
- difference
- pool wall
- threshold 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.)
- Granted
Links
Images
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The application provides a path planning method, an underwater robot, electronic equipment and a storage medium, which are used in the field of the underwater robot and are used for solving the problems that the conventional underwater robot path planning is not intelligent and dead zones and repeated areas occur. The method comprises the following steps: acquiring position coordinates and azimuth angles of the underwater robot, a first distance from a current side pool wall and a second distance from the current front pool wall in real time; controlling the underwater robot to travel along the front side of the underwater robot, and keeping the first distance between the underwater robot and the current side pool wall equal to a first threshold value; controlling the underwater robot to rotate a first angle in a direction deviating from the current side pool wall under the condition that the second distance is smaller than a second threshold value; under the condition that the underwater robot returns to the initial point, determining that the underwater robot travels for one round, resetting the initial position, the first threshold value and the second threshold value of the underwater robot, and controlling the underwater robot to move to the reset initial position.
Description
Technical Field
The present invention relates to the field of underwater robots, and in particular, to a path planning method, an underwater robot, an electronic device, and a storage medium.
Background
The underwater robot is used for cleaning the private swimming pool or the public swimming pool, so that manpower can be saved, and compared with manpower cleaning, the underwater robot is more thorough in cleaning. Therefore, it is becoming a trend to replace manpower with underwater robots for pool cleaning.
For the cleaning robot, the path planning method mainly comprises a random path method, a path planning method of a regular swimming pool and the like, and the cleaning robot is required to adopt a proper path planning method according to different configured sensors, working environments and task characteristics.
However, the conventional underwater random path method is random in underwater movement, so that more cleaning dead zones and more repeated cleaning areas are most likely to occur, the cleaning efficiency is low, and the intelligentization of the robot cannot be embodied at all; the application range and the scene of the path planning method of the regular swimming pool are greatly limited, the method is only suitable for the swimming pool with a relatively regular shape, the shape of the swimming pool is in a thousand and monster in reality, the method cannot adapt to the development of the swimming pool and the requirements of users, and the problem of low efficiency exists.
The above information disclosed in the background section is only for enhancement of understanding of the background of the application and therefore it may contain information that does not form the prior art that is already known to a person of ordinary skill in the art.
Disclosure of Invention
In order to solve the above problems, the present application provides a path planning method, an underwater robot, an electronic device and a storage medium.
According to a first aspect of the present application, a path planning method for an underwater robot is provided, the method comprising:
(a) Acquiring an initial position coordinate and an initial azimuth angle of the underwater robot;
(b) Acquiring the position coordinates and azimuth angles of the underwater robot, a first distance between the underwater robot and a current side pool wall and a second distance between the underwater robot and a current front pool wall in real time;
(c) Controlling the underwater robot to travel along the front side of the underwater robot, and keeping the first distance between the underwater robot and the current side pool wall equal to a first threshold value;
(d) When the difference value between the position coordinates of the underwater robot obtained in real time and the initial position coordinates continuously decreases in a first time and is smaller than a third threshold value, determining that the underwater robot returns to an initial point and executing the step (f) under the condition that the difference value between the azimuth angle obtained in real time and the initial azimuth angle is smaller than a fourth threshold value;
(e) Controlling the underwater robot to rotate a first angle in a direction deviating from the current side pool wall and performing the step (c) if the second distance is smaller than a second threshold;
(f) Under the condition that the underwater robot returns to the initial point, determining that the underwater robot travels for one round, and judging whether the difference between the maximum value and the minimum value of the abscissa or the difference between any one of the maximum value and the minimum value of the ordinate in the position coordinates acquired in real time by the underwater robot in the travelling process of the round is smaller than a fifth threshold;
(g) And (c) resetting the initial position of the underwater robot, the first threshold value and the second threshold value under the condition that the difference between the maximum value and the minimum value of the abscissa or the difference between the maximum value and the minimum value of the ordinate is larger than a fifth threshold value, controlling the underwater robot to move to the reset initial position, and executing the step (a).
According to some embodiments, the underwater robot is controlled to stop traveling in a case where a difference between any one of the maximum value and the minimum value of the abscissa or the maximum value and the minimum value of the ordinate is equal to or less than a fifth threshold.
According to some embodiments, the acquiring, in real time, the position coordinates and the azimuth angle of the underwater robot, the first distance from the current side pool wall, and the second distance from the current front pool wall includes:
acquiring a first distance between the underwater robot and a current side pool wall and a second distance between the underwater robot and a current right-ahead pool wall through ultrasonic ranging of the underwater robot:
wherein, S1 is the first distance, S2 is the second distance, V is the sound velocity of the water, t0 is the time of transmitting sound wave signals, t1 is the time of receiving side pool wall reflected signals, and t2 is the time of receiving the right front pool wall reflected signals.
According to some embodiments, the difference between the position coordinates of the underwater robot and the initial position coordinates and the difference between the azimuth angle and the initial azimuth angle are calculated by:
the dz is the difference between the position coordinate of the underwater robot and the initial position coordinate, da is the difference between the azimuth angle and the initial azimuth angle, x1, y1 and a1 are the position coordinate and the azimuth angle of the underwater robot obtained in real time, and x0, y0 and a0 are the initial position coordinate and the initial azimuth angle of the underwater robot.
According to some embodiments, the resetting the initial position of the underwater robot, the first threshold value, and the second threshold value comprises:
moving the underwater robot a third distance in a direction deviating from the current side pool wall;
increasing the first threshold by a fourth distance;
and increasing the second threshold by a fifth distance.
According to a second aspect of the present application, there is provided an underwater robot for performing the method of any of the first aspects, the underwater robot comprising:
the ultrasonic ranging module is arranged at the side and right in front of the underwater robot, acquires the position coordinates of the underwater robot in real time, and acquires the initial position coordinates of the underwater robot from the first distance of the current side pool wall and the second distance of the current right front pool wall;
the control unit is used for controlling the underwater robot to travel along the front direction of the underwater robot and keeping the first distance between the underwater robot and the current side pool wall equal to a first threshold value; controlling the underwater robot to rotate a first angle in a direction deviating from the current side pool wall under the condition that the second distance is smaller than a second threshold value; determining that the underwater robot returns to an initial point when the difference value between the position coordinates of the underwater robot and the initial position coordinates obtained in real time continuously decreases for a first time and is smaller than a third threshold value; under the condition that the underwater robot returns to the initial point, determining that the underwater robot travels for one round, and judging whether the difference between the maximum value and the minimum value of the abscissa or the difference between any one of the maximum value and the minimum value of the ordinate in the position coordinates acquired in real time by the underwater robot in the travelling process of the round is smaller than a fifth threshold; controlling the underwater robot to stop traveling under the condition that the difference between any one of the maximum value and the minimum value of the abscissa or the maximum value and the minimum value of the ordinate is smaller than or equal to a fifth threshold value;
and the advancing unit is used for receiving the instruction of the control unit and controlling the underwater robot to advance along the right front.
According to some embodiments, the underwater robot further comprises a gyroscope for acquiring an azimuth angle of the underwater robot;
the control unit is further configured to determine whether a difference between an azimuth angle and the initial azimuth angle is smaller than a fourth threshold when a difference between the position coordinate of the underwater robot and the initial position coordinate acquired in real time continues to decrease for a first time, and the difference between the position coordinate of the underwater robot and the initial position coordinate is smaller than a third threshold; and determining that the underwater robot returns to an initial point in the case that the difference between the azimuth angle and the initial azimuth angle is less than a fourth threshold.
According to some embodiments, the control unit is further configured to reset the initial position of the underwater robot, the first threshold value, and the second threshold value, and control the underwater robot to move to the reset initial position, if the difference between the maximum value and the minimum value of the abscissa or the maximum value and the minimum value of the ordinate is greater than a fifth threshold value.
According to a third aspect of the present application, there is provided an electronic device comprising:
one or more processors;
a memory for storing one or more programs;
the one or more programs, when executed by the one or more processors, cause the one or more processors to perform the method of any of the first aspects.
According to a fourth aspect of the present application, there is provided a computer readable storage medium having stored thereon a computer program which, when executed by a processor, implements a method according to any of the first aspects.
The application provides a path planning method, an underwater robot, electronic equipment and a storage medium, wherein the method is based on an ultrasonic ranging module and is used for controlling the underwater robot to travel from the periphery to the center according to a method of loop path planning along a pool wall, so that the traveling efficiency of the underwater robot is improved, the repeated problems of dead zones and paths can be avoided, the path planning is not influenced by the shape of a pool, and the intelligentization of the underwater robot is reflected; and the underwater robot provided by the application can realize path planning only by using two ultrasonic ranging modules, so that the equipment cost is saved.
It is to be understood that both the foregoing general description and the following detailed description are exemplary and explanatory only and are not restrictive of the application.
Drawings
The above and other objects, features and advantages of the present application will become more apparent by describing in detail exemplary embodiments thereof with reference to the attached drawings. The drawings described below are only some of the embodiments of the present application and are not intended to limit the present application.
FIG. 1 illustrates a path planning method flow diagram for an underwater robot in accordance with an exemplary embodiment;
FIG. 2 illustrates a schematic diagram of a method for path planning of an underwater robot in accordance with an exemplary embodiment;
FIG. 3 illustrates a schematic view of an exemplary embodiment of an underwater robot;
fig. 4 shows a block diagram of an electronic device provided in the present application.
Detailed Description
Example embodiments will now be described more fully with reference to the accompanying drawings. However, the exemplary embodiments can be embodied in many forms and should not be construed as limited to the embodiments set forth herein; rather, these embodiments are provided so that this disclosure will be thorough and complete, and will fully convey the concept of the example embodiments to those skilled in the art. The same reference numerals in the drawings denote the same or similar parts, and thus a repetitive description thereof will be omitted.
The described features, structures, or characteristics may be combined in any suitable manner in one or more embodiments. In the following description, numerous specific details are provided to give a thorough understanding of embodiments of the disclosure. One skilled in the relevant art will recognize, however, that the disclosed aspects may be practiced without one or more of the specific details, or with other methods, components, materials, apparatus, etc. In these instances, well-known structures, methods, devices, implementations, materials, or operations are not shown or described in detail.
The flow diagrams depicted in the figures are exemplary only, and do not necessarily include all of the elements and operations/steps, nor must they be performed in the order described. For example, some operations/steps may be decomposed, and some operations/steps may be combined or partially combined, so that the order of actual execution may be changed according to actual situations.
The terms "first," "second," and the like in the description of the present application and in the above-described figures, are used for distinguishing between different objects and not for describing a particular sequential order. Furthermore, the terms "comprise" and "have," as well as any variations thereof, are intended to cover a non-exclusive inclusion. For example, a process, method, system, article, or apparatus that comprises a list of steps or elements is not limited to only those listed steps or elements but may include other steps or elements not listed or inherent to such process, method, article, or apparatus.
Those skilled in the art will appreciate that the drawings are schematic representations of example embodiments, and that the modules or flows in the drawings are not necessarily required to practice the present application, and therefore, should not be taken to limit the scope of the present application.
Fig. 1 shows a flow chart of a path planning method for an underwater robot according to an exemplary embodiment.
S101, acquiring initial position coordinates and initial azimuth angles of the underwater robot.
According to an example embodiment, the underwater robot acquires an initial position coordinate (x 0, y 0) and an initial azimuth angle a0.
S102, acquiring the position coordinates and azimuth angle of the underwater robot, the first distance between the underwater robot and the current side pool wall and the second distance between the underwater robot and the current front pool wall in real time.
According to an example embodiment, position coordinates (x 1, y 1) and azimuth angle a1 of the underwater robot are acquired in real time during traveling of the underwater robot.
According to an example embodiment, the underwater robot obtains a first distance S1 from the current side pool wall and a second distance S2 from the current front pool wall by ultrasonic ranging:
wherein S1 is a firstThe distance S2 is the second distance, V is the sound velocity in water, t0 is the time of transmitting the sound wave signal, t1 is the time of receiving the reflected signal of the side pool wall, and t2 is the time of receiving the reflected signal of the right front pool wall.
And S103, controlling the underwater robot to travel along the front side of the underwater robot, and keeping the first distance between the underwater robot and the current side pool wall equal to a first threshold value.
According to an example embodiment, a first threshold value of the underwater robot from the side pool wall is preset to be S01, the underwater robot is controlled to feed back the side distance in real time, the movement traveling angular speed of the underwater robot is adjusted to keep the first distance from the current side pool wall to be the first threshold value S01, and the underwater robot is controlled to travel along the front side of the underwater robot.
S104, judging whether the underwater robot returns to the initial position.
According to an example embodiment, position coordinates of the underwater robot are acquired in real time, and if the difference value between the position coordinates and the initial position coordinates continues to decrease for a first time and is smaller than a third threshold dz1, the process goes to S105; if the difference from the initial position coordinates does not last for the first time to decrease or is not smaller than the third threshold value, the process goes to S109.
According to an example embodiment, the difference between the position coordinates of the underwater robot and the initial position coordinates is calculated by:
wherein dz is the difference between the position coordinates of the underwater robot and the initial position coordinates, (x 1, y 1) is the position coordinates of the underwater robot obtained in real time, and (x 0, y 0) is the initial position coordinates of the underwater robot.
According to some embodiments, the first time may be set according to actual needs. For example, the first time is 2S, and if it is detected in real time that the difference between the position coordinates of the underwater robot and the initial position coordinates continuously decreases within 2S and is smaller than the third threshold dz1, the process proceeds to S105.
S105, judging whether the difference value between the azimuth angle and the initial azimuth angle is smaller than a fourth threshold value.
According to an example embodiment, an underwater robot is acquired in real timeUnder the condition that the difference value between the position coordinate and the initial position coordinate continuously decreases in the first time and is smaller than a third threshold value, calculating the difference value between the azimuth angle acquired in real time and the initial azimuth angle:
da is the difference between the azimuth angle and the initial azimuth angle, a1 is the azimuth angle of the underwater robot obtained in real time, and a0 is the initial azimuth angle of the underwater robot.
Under the condition that the difference da between the azimuth angle and the initial azimuth angle is smaller than a fourth threshold value da1, determining that the underwater robot returns to an initial point, and turning to S106; if the threshold value is equal to or greater than the fourth threshold value, the process proceeds to S109.
S106, judging whether the difference between any one of the maximum value and the minimum value of the horizontal coordinate or the maximum value and the minimum value of the vertical coordinate in the position coordinates acquired in real time in the running process of the underwater robot is smaller than a fifth threshold value.
According to an example embodiment, it is determined that the underwater robot travels one round in a case where the underwater robot returns to an initial point. According to the position coordinates of the underwater robot acquired in real time, selecting the maximum value x of the abscissa among the position coordinates max And a minimum value x min Maximum y of ordinate max And a minimum value y min And calculating the difference value between the maximum value and the minimum value of the abscissa, the difference value between the maximum value and the minimum value of the ordinate, and judging whether the difference between the maximum value and the minimum value of the abscissa or any one of the maximum value and the minimum value of the ordinate is smaller than a fifth threshold value.
According to an example embodiment, in case the maximum and minimum values of the coordinates, or the difference between the maximum and minimum values of the ordinate are both greater than a fifth threshold value, go to S107; if the difference between the maximum value and the minimum value on the abscissa or between the maximum value and the minimum value on the ordinate is equal to or smaller than the fifth threshold value, the process proceeds to S108.
S107, resetting the initial position, the first threshold and the second threshold of the underwater robot, and controlling the underwater robot to move to the reset initial position.
According to an example embodiment, after the underwater robot travels one round, moving the underwater robot a third distance in a direction deviating from the current side pool wall; increasing the first threshold by a fourth distance; the second threshold is increased by a fifth distance, as shown in fig. 2, the underwater robot is moved from the point A0 to the point B0, and the process proceeds to S103.
According to some embodiments, the fourth distance and the fifth distance may be the same.
S108, controlling the underwater robot to stop travelling.
According to an example embodiment, in case that a difference between any one of the maximum value and the minimum value of the abscissa or the maximum value and the minimum value of the ordinate is equal to or less than a fifth threshold value, it is verified that the underwater robot has reached the center of the pool, and the underwater robot is controlled to stop traveling.
S109, judging whether the second distance is smaller than a second threshold.
According to an example embodiment, a second threshold value of the underwater robot from the pool wall right in front is preset as S02, and if the acquired second distance is smaller than the second threshold value S02 during the traveling process of the underwater robot, the process goes to S110; if the acquired second distance is equal to or greater than the second threshold, the process goes to S103.
S110, controlling the underwater robot to rotate a first angle in a direction deviating from the current side pool wall.
According to an example embodiment, in case the second distance S2 of the underwater robot from the right ahead is smaller than the second threshold S02, the underwater robot is controlled to rotate a first angle in a direction deviating from the current side pool wall: after the underwater robot rotates by the first angle, the process goes to S103, that is, the distance between the underwater robot and the current side pool wall is still required to be controlled to be the first threshold value in the process of traveling.
According to some embodiments, the first angle may be set by itself.
Taking fig. 2 as an example, when the underwater robot travels to the point A1, the distance S2 from the tank wall right in front is smaller than the second threshold S02, and the underwater robot is controlled to deviate from the direction of the tank wall on the current side, i.e., to rotate 90 ° to the right.
The application provides a path planning method, an underwater robot, electronic equipment and a storage medium, wherein the method is based on an ultrasonic ranging module and is used for controlling the underwater robot to travel from the periphery to the center according to a method of loop path planning along a pool wall, so that the traveling efficiency of the underwater robot is improved, the occurrence of dead zones and repetition of paths can be avoided, the influence of the pool shape is avoided, and the intellectualization of the underwater robot is embodied.
Fig. 3 shows a schematic view of an exemplary embodiment of an underwater robot.
As shown in fig. 3, the underwater robot 30 includes two ultrasonic ranging modules 3011 and 3012, a control unit 302, a traveling unit 303, wherein:
the ultrasonic ranging module 3011 is arranged at the side of the underwater robot and used for acquiring a first distance S1 between the underwater robot and the current side pool wall, and the ultrasonic ranging module 3012 is arranged at the right front of the underwater robot and used for acquiring a second distance S2 between the underwater robot and the current right front pool wall.
The first distance S1 between the underwater robot and the current side pool wall and the second distance S2 between the underwater robot and the current front pool wall are calculated in the following way:
wherein S1 is the first distance, S2 is the second distance, V is the underwater sound velocity, t0 is the time of transmitting the sound wave signal, t1 is the time of receiving the reflected signal of the side pool wall, and t2 is the time of receiving the reflected signal of the right front pool wall.
According to an example embodiment, the ultrasonic ranging module 3011 and the ultrasonic ranging module 3012 are also used to obtain the position coordinates (x 1, y 1) and the initial position coordinates (x 0, y 0) of the underwater robot.
And a traveling unit 303 for receiving the instruction of the control unit 302 and controlling the underwater robot to travel along the straight ahead.
The control unit 302 is configured to, when controlling the underwater robot to travel along the front side of the underwater robot, keep the first distance S1 between the underwater robot and the current side pool wall equal to the first threshold S01, and the ultrasonic ranging module 3011 feeds back the side distance S1 in real time, and the control unit 302 controls the travel unit 303 to adjust the angular velocity of motion travel of the underwater robot so as to keep the first distance between the underwater robot and the current side pool wall at the first threshold S01 and control the underwater robot to travel along the front side of the underwater robot.
According to an example embodiment, in case that the second distance S2 acquired in real time by the ultrasonic ranging module 3012 is less than the second threshold S02, the control unit 302 outputs a control instruction to the traveling unit 303 to control the underwater robot to rotate by a first angle in a direction deviating from the current side pool wall.
According to an example embodiment, in case that the difference between the position coordinates (x 1, y 1) of the underwater robot and the initial position coordinates (x 0, y 0) acquired in real time by the ultrasonic ranging module 3011 and the ultrasonic ranging module 3012 continuously decreases for a first time and is smaller than the third threshold dz1, it is determined that the underwater robot returns to the initial point.
According to an exemplary embodiment, the underwater robot further comprises a gyroscope 304 for acquiring an initial azimuth a0 and an azimuth a1 of the underwater robot:
when the difference value between the position coordinate of the underwater robot and the initial position coordinate obtained in real time continuously decreases for a first time, the control unit 302 is further configured to determine whether the difference value between the azimuth angle and the initial azimuth angle is smaller than a fourth threshold value when the difference value between the position coordinate of the underwater robot and the initial position coordinate is smaller than a third threshold value; in the case where the difference between the azimuth angle a1 and the initial azimuth angle a0 is smaller than the fourth threshold value da1, it is determined that the underwater robot returns to the initial point.
In the case where the underwater robot returns to the initial point, the control unit 302 determines that the underwater robot travels one round and selects the maximum value x of the abscissa among the position coordinates of the underwater robot acquired in real time max And a minimum value x min Maximum y of ordinate max And a minimum value y min And calculating the difference between the maximum value and the minimum value of the abscissa and the difference between the maximum value and the minimum value of the ordinate. And judging whether the difference between any one of the maximum value and the minimum value of the horizontal coordinates or the maximum value and the minimum value of the vertical coordinates in the real-time acquired position coordinates of the underwater robot is smaller than a fifth threshold value in the advancing process of the underwater robot.
When the difference between the maximum value and the minimum value on the abscissa or between the maximum value and the minimum value on the ordinate is equal to or smaller than the fifth threshold, the control unit 302 outputs a control command to the traveling unit 303 to control the underwater robot to stop traveling.
In the case that the difference between the maximum value and the minimum value of the abscissa or the maximum value and the minimum value of the ordinate is greater than the fifth threshold, the control unit 302 resets the initial position of the underwater robot, and moves the underwater robot a third distance in a direction deviating from the current side pool wall; resetting the first threshold value and the second threshold value, and increasing the first threshold value by a fourth distance; increasing the second threshold by a fifth distance; and outputs a control instruction to the traveling unit 303 to control the underwater robot to move to the reset initial position.
According to some embodiments, the fourth distance and the fifth distance may be the same.
The application provides a path planning method, an underwater robot, electronic equipment and a storage medium, wherein the path planning method is based on ultrasonic ranging modules and is based on a method for planning a loop-shaped path along a pool wall, and the path planning can be realized by only using two ultrasonic ranging modules, so that the equipment cost is saved.
Fig. 4 shows a block diagram of an electronic device provided in the present application.
Referring to fig. 4, fig. 4 provides an electronic device 40, which includes a processor 401 and a memory 402. The memory 402 stores computer instructions that, when executed by the processor 401, cause the processor 401 to execute the computer instructions to implement the method and refinement as shown in fig. 1 and to transceive data via the communication module 403.
It should be understood that the above-described device embodiments are illustrative only and that the disclosed device may be implemented in other ways. For example, the division of the units/modules in the above embodiments is merely a logic function division, and there may be another division manner in actual implementation. For example, multiple units, modules, or components may be combined, or may be integrated into another system, or some features may be omitted or not performed.
In addition, unless specifically described, each functional unit/module in each embodiment of the present invention may be integrated into one unit/module, or each unit/module may exist alone physically, or two or more units/modules may be integrated together. The integrated units/modules described above may be implemented either in hardware or in software program modules.
The integrated units/modules, if implemented in hardware, may be digital circuits, analog circuits, etc. Physical implementations of hardware structures include, but are not limited to, transistors, memristors, and the like. The processor or chip may be any suitable hardware processor, such as CPU, GPU, FPGA, DSP and ASIC, etc., unless otherwise specified. The on-chip cache, off-chip Memory, memory may be any suitable magnetic or magneto-optical storage medium, such as resistive Random Access Memory RRAM (Resistive Random Access Memory), dynamic Random Access Memory DRAM (Dynamic Random Access Memory), static Random Access Memory SRAM (Static Random Access Memory), enhanced dynamic Random Access Memory EDRAM (Enhanced Dynamic Random Access Memory), high-Bandwidth Memory HBM (High-Bandwidth Memory), hybrid Memory cube HMC (Hybrid Memory Cube), and the like, unless otherwise indicated.
The integrated units/modules may be stored in a computer readable memory if implemented in the form of software program modules and sold or used as a stand-alone product. Based on such understanding, the technical solution of the present invention may be embodied in essence or a part contributing to the prior art or all or part of the technical solution in the form of a software product stored in a memory, comprising several instructions for causing a computer device (which may be a personal computer, a server or a network device, etc.) to perform all or part of the steps of the method according to the embodiments of the present disclosure. And the aforementioned memory includes: a U-disk, a Read-Only Memory (ROM), a random access Memory (RAM, random Access Memory), a removable hard disk, a magnetic disk, or an optical disk, or other various media capable of storing program codes.
Embodiments of the present application also provide a non-transitory computer storage medium storing a computer program that, when executed by a plurality of processors, causes the processors to perform the method and refinement as shown in fig. 1.
It should be clearly understood that this application describes how to make and use particular examples, but is not limited to any details of these examples. Rather, these principles can be applied to many other embodiments based on the teachings of the present disclosure.
Furthermore, it should be noted that the above-described figures are merely illustrative of the processes involved in the method according to the exemplary embodiments of the present application, and are not intended to be limiting. It will be readily appreciated that the processes shown in the above figures do not indicate or limit the temporal order of these processes. In addition, it is also readily understood that these processes may be performed synchronously or asynchronously, for example, among a plurality of modules.
Exemplary embodiments of the present application are specifically illustrated and described above. It is to be understood that this application is not limited to the details of construction, arrangement or method of implementation described herein; on the contrary, the application is intended to cover various modifications and equivalent arrangements included within the spirit and scope of the appended claims.
Claims (10)
1. A path planning method for an underwater robot, the method comprising:
(a) Acquiring an initial position coordinate and an initial azimuth angle of the underwater robot;
(b) Acquiring the position coordinates and azimuth angles of the underwater robot, a first distance between the underwater robot and a current side pool wall and a second distance between the underwater robot and a current front pool wall in real time;
(c) Controlling the underwater robot to travel along the front side of the underwater robot, and keeping the first distance between the underwater robot and the current side pool wall equal to a first threshold value;
(d) When the difference value between the position coordinates of the underwater robot obtained in real time and the initial position coordinates continuously decreases in a first time and is smaller than a third threshold value, determining that the underwater robot returns to an initial point and executing the step (f) under the condition that the difference value between the azimuth angle obtained in real time and the initial azimuth angle is smaller than a fourth threshold value;
(e) Controlling the underwater robot to rotate a first angle in a direction deviating from the current side pool wall and performing the step (c) if the second distance is smaller than a second threshold;
(f) Under the condition that the underwater robot returns to the initial point, determining that the underwater robot travels for one round, and judging whether the difference between the maximum value and the minimum value of the abscissa or the difference between any one of the maximum value and the minimum value of the ordinate in the position coordinates acquired in real time by the underwater robot in the travelling process of the round is smaller than a fifth threshold;
(g) And (c) resetting the initial position of the underwater robot, the first threshold value and the second threshold value under the condition that the difference between the maximum value and the minimum value of the abscissa or the difference between the maximum value and the minimum value of the ordinate is larger than a fifth threshold value, controlling the underwater robot to move to the reset initial position, and executing the step (a).
2. The path planning method according to claim 1, wherein the underwater robot is controlled to stop traveling in a case where a difference between any one of the maximum value and the minimum value of the abscissa or the maximum value and the minimum value of the ordinate is equal to or smaller than a fifth threshold value.
3. The path planning method according to claim 1, wherein the acquiring in real time the position coordinates and azimuth angle of the underwater robot, the first distance from the current side pool wall and the second distance from the current front pool wall includes:
acquiring a first distance between the underwater robot and a current side pool wall and a second distance between the underwater robot and a current right-ahead pool wall through ultrasonic ranging of the underwater robot:
wherein, S1 is the first distance, S2 is the second distance, V is the sound velocity of the water, t0 is the time of transmitting sound wave signals, t1 is the time of receiving side pool wall reflected signals, and t2 is the time of receiving the right front pool wall reflected signals.
4. The path planning method according to claim 1, wherein the difference between the position coordinates of the underwater robot and the initial position coordinates and the difference between the azimuth angle and the initial azimuth angle are calculated by:
the dz is the difference between the position coordinate of the underwater robot and the initial position coordinate, da is the difference between the azimuth angle and the initial azimuth angle, x1, y1 and a1 are the position coordinate and the azimuth angle of the underwater robot obtained in real time, and x0, y0 and a0 are the initial position coordinate and the initial azimuth angle of the underwater robot.
5. The path planning method according to claim 1, wherein the resetting the initial position of the underwater robot, the first threshold value, and the second threshold value includes:
moving the underwater robot a third distance in a direction deviating from the current side pool wall;
increasing the first threshold by a fourth distance;
and increasing the second threshold by a fifth distance.
6. An underwater robot for performing the method of any of claims 1-5, the underwater robot comprising:
the ultrasonic ranging module is arranged at the side and right in front of the underwater robot, acquires the position coordinates of the underwater robot in real time, and acquires the initial position coordinates of the underwater robot from the first distance of the current side pool wall and the second distance of the current right front pool wall;
the control unit is used for controlling the underwater robot to travel along the front direction of the underwater robot and keeping the first distance between the underwater robot and the current side pool wall equal to a first threshold value; controlling the underwater robot to rotate a first angle in a direction deviating from the current side pool wall under the condition that the second distance is smaller than a second threshold value; determining that the underwater robot returns to an initial point when the difference value between the position coordinates of the underwater robot and the initial position coordinates obtained in real time continuously decreases for a first time and is smaller than a third threshold value; under the condition that the underwater robot returns to the initial point, determining that the underwater robot travels for one round, and judging whether the difference between the maximum value and the minimum value of the abscissa or the difference between any one of the maximum value and the minimum value of the ordinate in the position coordinates acquired in real time by the underwater robot in the travelling process of the round is smaller than a fifth threshold; controlling the underwater robot to stop traveling under the condition that the difference between any one of the maximum value and the minimum value of the abscissa or the maximum value and the minimum value of the ordinate is smaller than or equal to a fifth threshold value;
and the advancing unit is used for receiving the instruction of the control unit and controlling the underwater robot to advance along the right front.
7. The underwater robot of claim 6 further comprising a gyroscope for acquiring an azimuth angle of the underwater robot;
the control unit is further configured to determine whether a difference between an azimuth angle and the initial azimuth angle is smaller than a fourth threshold when a difference between the position coordinate of the underwater robot and the initial position coordinate acquired in real time continues to decrease for a first time, and the difference between the position coordinate of the underwater robot and the initial position coordinate is smaller than a third threshold; and determining that the underwater robot returns to an initial point in the case that the difference between the azimuth angle and the initial azimuth angle is less than a fourth threshold.
8. The underwater robot of claim 6, wherein the control unit is further configured to reset the initial position of the underwater robot, the first threshold value, and the second threshold value, and control the underwater robot to move to the reset initial position, in a case where a difference between the maximum value and the minimum value of the abscissa or the maximum value and the minimum value of the ordinate is greater than a fifth threshold value.
9. An electronic device, comprising:
one or more processors;
a memory for storing one or more programs;
the one or more programs, when executed by the one or more processors, cause the one or more processors to perform the method of any of claims 1-5.
10. A computer readable storage medium, on which a computer program is stored, characterized in that the program, when being executed by a processor, implements the method according to any of claims 1-5.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310658883.4A CN116400711B (en) | 2023-06-06 | 2023-06-06 | Path planning method, underwater robot, electronic device and storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310658883.4A CN116400711B (en) | 2023-06-06 | 2023-06-06 | Path planning method, underwater robot, electronic device and storage medium |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116400711A true CN116400711A (en) | 2023-07-07 |
| CN116400711B CN116400711B (en) | 2023-10-03 |
Family
ID=87016437
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310658883.4A Active CN116400711B (en) | 2023-06-06 | 2023-06-06 | Path planning method, underwater robot, electronic device and storage medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116400711B (en) |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20040167667A1 (en) * | 2002-12-17 | 2004-08-26 | Goncalves Luis Filipe Domingues | Systems and methods for filtering potentially unreliable visual data for visual simultaneous localization and mapping |
| CN102083352A (en) * | 2008-04-24 | 2011-06-01 | 进展机器人有限公司 | Application of localization, positioning & navigation systems for robotic enabled mobile products |
| CN102799180A (en) * | 2012-07-26 | 2012-11-28 | 杭州高越科技有限公司 | Walking control method and device of cleaning machine |
| CN106708040A (en) * | 2016-12-09 | 2017-05-24 | 重庆长安汽车股份有限公司 | Sensor module of automatic driving system, automatic driving system and automatic driving method |
| CN114531903A (en) * | 2020-08-31 | 2022-05-24 | 苏州珊口智能科技有限公司 | Traveling control method and path planning method for mobile robot and mobile robot |
| CN115047893A (en) * | 2022-06-13 | 2022-09-13 | 深圳众清人居科技有限公司 | Underwater robot path planning method based on ultrasonic radar and underwater robot |
-
2023
- 2023-06-06 CN CN202310658883.4A patent/CN116400711B/en active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20040167667A1 (en) * | 2002-12-17 | 2004-08-26 | Goncalves Luis Filipe Domingues | Systems and methods for filtering potentially unreliable visual data for visual simultaneous localization and mapping |
| CN102083352A (en) * | 2008-04-24 | 2011-06-01 | 进展机器人有限公司 | Application of localization, positioning & navigation systems for robotic enabled mobile products |
| CN102799180A (en) * | 2012-07-26 | 2012-11-28 | 杭州高越科技有限公司 | Walking control method and device of cleaning machine |
| CN106708040A (en) * | 2016-12-09 | 2017-05-24 | 重庆长安汽车股份有限公司 | Sensor module of automatic driving system, automatic driving system and automatic driving method |
| CN114531903A (en) * | 2020-08-31 | 2022-05-24 | 苏州珊口智能科技有限公司 | Traveling control method and path planning method for mobile robot and mobile robot |
| CN115047893A (en) * | 2022-06-13 | 2022-09-13 | 深圳众清人居科技有限公司 | Underwater robot path planning method based on ultrasonic radar and underwater robot |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116400711B (en) | 2023-10-03 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107688342B (en) | The obstruction-avoiding control system and method for robot | |
| CN111026131B (en) | Expansion region determining method and device, robot and storage medium | |
| WO2006022827A2 (en) | System and method for adaptive path planning | |
| CN113341984A (en) | Robot path planning method and device based on improved RRT algorithm | |
| CN107990906B (en) | Method for determining path | |
| CN107966153A (en) | Submarine navigation device path planning algorithm | |
| US11373412B2 (en) | Obstacle map generating method and apparatus | |
| CN113778099A (en) | Unmanned ship path planning method based on NDT algorithm and Hybrid A algorithm | |
| CN116400711B (en) | Path planning method, underwater robot, electronic device and storage medium | |
| CN113139696B (en) | Trajectory prediction model construction method and trajectory prediction method and device | |
| CN112462403A (en) | Positioning method, positioning device, storage medium and electronic equipment | |
| CN114489060B (en) | Unmanned ship formation control method, terminal equipment and computer readable storage medium | |
| CN114459486A (en) | Robot and channel navigation method, device and storage medium thereof | |
| CN114003047A (en) | Path planning method for small unmanned ship | |
| KR20220044777A (en) | Point cloud data processing method and device | |
| Zhang et al. | A Robot Navigation System in Complex Terrain Based on Statistical Features of Point Clouds | |
| Stamford et al. | Pathfinding in partially explored games environments: The application of the A* Algorithm with occupancy grids in Unity3D | |
| CN115837921B (en) | Vehicle track collision detection method, device, equipment and storage medium | |
| Bayrak et al. | Formation preserving path finding in 3-D terrains | |
| CN117109597B (en) | Unmanned plane path planning method and device based on improved A star algorithm | |
| Yim et al. | Robust path planning for autonomous vehicle in position uncertainty | |
| JP2962747B2 (en) | Collision detection system | |
| Choi et al. | Obstacle avoidance using active window and flexible vector field with a laser range finder | |
| CN117032205A (en) | Automatic intelligent edge finding method and underwater robot | |
| CN116678411A (en) | Ship route planning method and device based on artificial potential field and storage medium |
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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |