CN108189031B

CN108189031B - Underwater robot motion path planning method and system

Info

Publication number: CN108189031B
Application number: CN201711443969.6A
Authority: CN
Inventors: 庄楠; 应佳伟; 陈展平; 何彩燕
Original assignee: Individual
Current assignee: Zhuang Nan
Priority date: 2017-12-27
Filing date: 2017-12-27
Publication date: 2021-02-19
Anticipated expiration: 2037-12-27
Also published as: CN108189031A

Abstract

The invention relates to a method and a system for planning a motion path of an underwater robot, which comprises the following steps: acquiring current state information of the underwater robot in real time; advancing to climb the wall based on the state information, and retreating to descend the wall after advancing to climb the wall; judging whether the underwater robot retreats from the wall or not based on the state information, if so, retreating successively for carrying out first steering, retreating again after the first steering, and carrying out second steering with the same angle as the first steering; retreating to climb the wall based on the state information, and advancing to descend the wall after retreating to climb the wall; and judging whether the underwater robot finishes advancing and getting off the wall or not based on the state information, if so, carrying out third steering in advance, advancing again after the third steering, and carrying out fourth steering with the same angle with the third steering direction. The method can ensure that the underwater robot can clean the swimming pool without dead angles. And because it is regular washing to the swimming pool, consequently can also avoid underwater robot cable conductor winding's problem.

Description

Underwater robot motion path planning method and system

Technical Field

The invention relates to the technical field of automatic cleaning, in particular to a method and a system for planning a motion path of an underwater robot.

Background

With the rapid development of national technological innovation and high-end manufacturing industry, the development of the robot industry is more and more highly concerned by countries in the world, and the main economic bodies raise the development of the robot industry into the national strategy at a dispute, and the strategy is taken as an important means for maintaining and regaining competitive advantages of the manufacturing industry. The intelligent household robots for meeting daily use have entered the lives of people, and various different types of intelligent household robots can replace manpower to perform various kinds of work.

An underwater robot, a cleaning device for cleaning private or public swimming pools. The use of underwater robots to clean private or public swimming pools can save manpower and is more thorough than manual cleaning of underwater robots, so that the use of underwater robots to replace manpower for swimming pool cleaning is a trend. In the traditional technology, the underwater robot intelligently identifies the included angle between the robot and the X axis, the motion track of the underwater robot in the swimming pool is unpredictable, and the motion of the underwater robot in the swimming pool in the traditional technology is random.

In the above-mentioned technical solution for cleaning the swimming pool by the underwater robot in the conventional technology, because only the included angle between the machine and one axis in the spatial coordinate system can be identified, and the movement is unpredictable, the cable of the machine is wound, and the unpredictable movement track may have a cleaning dead zone, which does not achieve the effect of cleaning the swimming pool.

Disclosure of Invention

Therefore, a method and a system for planning the motion path of the underwater robot are needed to solve the problems that the cable of the robot is wound, a cleaning dead zone occurs and the effect of cleaning a swimming pool cannot be achieved.

The invention provides a method for planning a motion path of an underwater robot, which comprises the following steps: acquiring current state information of the underwater robot in real time; controlling the underwater robot to advance and climb the wall based on the state information, and retreating and falling the wall after the underwater robot advances and climbs the wall; judging whether the underwater robot retreats to leave the wall or not based on the state information, if so, controlling the underwater robot to retreat firstly to perform first steering, and retreating again after the first steering to perform second steering with the same angle as the first steering direction; controlling the underwater robot to retreat and climb the wall based on the state information, and advancing to descend the wall after retreating and climbing; judging whether the underwater robot finishes advancing and getting off the wall or not based on the state information, if so, controlling the underwater robot to advance firstly and carry out third steering, and after the third steering, advancing again and carrying out fourth steering with the same angle as the third steering; the above steps are repeated and the number of round trips is increased by 1.

Further, the step of controlling the underwater robot to advance to climb the wall and retreat to climb the wall after advancing to climb the wall and the step of controlling the underwater robot to retreat to climb the wall based on the state information are specifically: judging whether the underwater robot is in a horizontal state or not based on the state information, and if the underwater robot is in the horizontal state, acquiring the current round-trip times; if the number of round trips is less than n times, controlling the underwater robot to move forwards/backwards; acquiring current forward/backward time, and comparing the forward/backward time with first preset time; if the forward/backward time is less than a first preset time, judging whether the underwater robot meets a side wall or not based on the state information, and if so, controlling the underwater robot to climb the wall forward/backward; acquiring current forward/backward wall climbing time, and comparing the forward/backward wall climbing time with second preset time; and if the advancing/retreating wall climbing time is not less than the second preset time, controlling the underwater robot to retreat/advance to lower the wall.

Further, when the number of round trips is increased to n, the number of round trips is reset to 1.

Further, whether the underwater robot is in a horizontal state or not is judged based on the state information, and if the underwater robot is in the horizontal state, the current round-trip times are obtained; if the number of round trips is less than n times, controlling the underwater robot to move forward/backward further comprises: and if the round-trip times are not less than n times, acquiring the information of the zone bit, controlling the underwater robot to turn for the fifth time based on the zone bit, and after the fifth turning, advancing/backing and adding 1 to the zone bit.

Further, whether the underwater robot is in a horizontal state or not is judged based on the state information, and if the underwater robot is in the horizontal state, the current round-trip times are obtained; if the number of round trips is less than n times, controlling the underwater robot to move forward/backward further comprises: and if the underwater robot is not in a horizontal state, controlling the underwater robot to retreat/advance to descend the wall and subtracting 1 from the number of times of round trip, judging whether the underwater robot finishes retreating/advancing to descend the wall or not based on the state information, if so, controlling the underwater robot to retreat/advance in sequence to carry out first/third steering, and retreating/advancing again after the first/third steering to carry out second/fourth steering with the same angle with the first/third steering.

Further, the fifth rotation based on the flag bit specifically is: the value range of the flag bit is more than or equal to 1 and less than or equal to 4; when the zone bit is equal to 1, the steering is carried out on the right when the vehicle moves backwards, the steering is not carried out when the vehicle moves forwards, and 1 is added to the zone bit; when the zone bit is equal to 2, the steering is carried out on the left when the vehicle moves backwards, the steering is not carried out when the vehicle moves forwards, and 1 is added to the zone bit; when the zone bit is equal to 3, the steering is not carried out when the vehicle moves backwards, the steering is carried out when the vehicle moves forwards, and 1 is added to the zone bit; when the zone bit is equal to 4, the steering is not carried out when the vehicle moves backwards, the steering is carried out on the left when the vehicle moves forwards, and 1 is added to the zone bit; and resetting the flag bit to 1 when the flag bit is larger than 4.

Further, the rotation angle of the first steering, the second steering, the third steering and the fourth steering is 45 °, and the rotation angle of the fifth steering is 90 °.

Further, if the forward/backward time is less than a first preset time, determining whether the underwater robot meets a side wall based on the state information, and if so, controlling the underwater robot to advance/backward climb the wall further comprises: if the forward/backward time is not less than the first preset time, the execution step judges whether the underwater robot finishes forward/backward wall falling or not based on the state information, if so, the underwater robot is controlled to advance/backward in advance, third/primary steering is carried out, and after the third/primary steering, the underwater robot advances/backward again, and fourth/secondary steering with the same angle as the third/primary steering is carried out.

Further, if the forward/backward time is less than a first preset time, determining whether the underwater robot meets a side wall based on the state information, and if so, controlling the underwater robot to advance/backward climb the wall further comprises: and if the forward/backward time is less than the first preset time and the side wall is not met, controlling the underwater robot to continue to move forward/backward.

Further, if the forward/backward wall climbing time is not less than the second preset time, the step of controlling the underwater robot to go backward/forward and get off the wall further includes: and if the advancing/retreating wall climbing time is less than the second preset time, controlling the underwater robot to continue advancing/retreating to climb the wall.

Further, advance climb the wall, retreat down the wall, retreat climb the wall and advance down the wall still include: acquiring current state information in real time; judging whether the current state of the underwater robot is vertical wall climbing/falling or not based on the state information; if the climbing/falling is vertical, controlling the underwater robot to continuously climb/fall the wall; and if the current state is not the vertical wall climbing/descending state, judging that the current state of the underwater robot is the wall winding state.

Further, when in a wall-surrounding state: acquiring current state information in real time; controlling the underwater robot to adjust the position of the body based on the state information; judging whether the underwater robot is successfully adjusted to vertical wall climbing/falling or not based on the state information; if the underwater robot is successfully adjusted to be vertical wall climbing/falling, controlling the underwater robot to retreat/advance to fall on the wall, judging whether the underwater robot finishes advancing to fall on the wall or not based on the state information, if so, controlling the underwater robot to advance firstly and carry out third steering, and after third steering, advancing again and carrying out fourth steering with the same angle as the third steering; and if the adjustment is not successful, controlling the underwater robot to give an alarm.

Further, the step of acquiring the current state information of the underwater robot in real time further comprises the following steps: presetting a first preset time, a second preset time, a maximum round-trip time n and an initial value of a flag bit.

The invention also provides a system for planning the motion path of the underwater robot, which comprises: the state acquisition module is used for acquiring the current state information of the underwater robot in real time; the wall climbing control module is used for controlling the underwater robot to advance/retreat to climb the wall based on the state information and retreat/advance to descend the wall after advancing/retreating to climb the wall; the steering control module is used for judging whether the underwater robot retreats from the wall or not based on the state information, if so, controlling the underwater robot to retreat firstly for carrying out first steering, and retreating again after the first steering for carrying out second steering with the same angle as the first steering direction; and judging whether the underwater robot finishes advancing and getting off the wall or not based on the state information, if so, controlling the underwater robot to advance firstly and carry out third steering, and after the third steering, advancing again and carrying out fourth steering with the same angle as the third steering direction.

Further, the wall climbing control module comprises: the judging module is used for judging whether the underwater robot meets the side wall or not based on the state information, and if not, controlling the underwater robot to move forwards/backwards; judging whether the forward/backward time is not less than the first preset time, if so, judging whether the underwater robot meets a side wall based on the state information, and if so, controlling the underwater robot to climb the wall forward/backward; judging whether the forward/backward wall climbing time is not less than the second preset time or not; and the lower wall control module is used for controlling the underwater robot to retreat/advance to lower the wall if the advancing/retreating wall climbing time is not less than the second preset time.

The underwater robot cleans the swimming pool wall on the movement path of the underwater robot, and controls the movement path of the underwater robot by acquiring the current state information, namely acquiring the included angles between the robot body and an XY plane, an XZ plane and a YZ plane. Firstly, judging whether the machine is in a horizontal state or not, if so, advancing to climb the wall for cleaning, vertically lowering the wall after reaching a second preset time, rotating twice after lowering the wall, adjusting the preset distance in the width direction of the machine, retreating to climb the wall after adjusting, advancing to lower the wall after reaching the second preset time, rotating twice again after lowering the wall, adjusting the preset distance in the width direction of the machine, and completing one-time back-and-forth cleaning. The swimming pool is cleaned in a reciprocating manner for multiple times without dead angles, and the problem of cable winding of an underwater robot is solved because the motion trail of the swimming pool is regular.

Drawings

Fig. 1 is a flowchart of a method for planning a motion path of an underwater robot according to a first embodiment of the present invention;

fig. 2 is a flowchart of a method for planning a motion path of an underwater robot according to a second embodiment of the present invention;

fig. 3 is a flowchart of a method for planning a motion path of an underwater robot according to a third embodiment of the present invention;

fig. 4 is a flowchart of a method for planning a motion path of an underwater robot according to a fourth embodiment of the present invention;

fig. 5 is a flowchart of a method for planning a motion path of an underwater robot according to a fifth embodiment of the present invention;

fig. 6 is a schematic diagram of a module relationship of an underwater robot motion path planning system according to an embodiment of the present invention;

fig. 7 is a schematic diagram of a module relationship of a climbing control module according to an embodiment of the present invention.

Reference numerals: 100 is a state acquisition module, 200 is a climbing control module, 210 is a judgment module, 220 is a lower wall control module, and 300 is a steering control module.

Detailed Description

In order to make the objects, technical solutions and advantages of the present invention more apparent, the present invention is described in further detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the invention.

The embodiment of the invention discloses a method for planning a motion path of an underwater robot. Through obtaining current state information, control underwater robot carries out earlier vertical horizontal regular washing again, and then can guarantee that underwater robot does not have the dead angle and washs the swimming pool. And because it is regular washing to the swimming pool, consequently can also avoid underwater robot's cable conductor winding's problem.

The horizontal states in this embodiment include: after the first steering, the second steering, the third steering, the fourth steering and the fifth steering, the horizontal state is as follows: the included angle between the underwater robot and the XY plane is smaller than a preset horizontal included angle, and the preset horizontal included angle is preferably 30 degrees in the embodiment; when the robot is lowered from the wall, the horizontal state is that the included angle between the robot body and the XY plane is smaller than or equal to a preset lower wall angle; when the robot is on the wall, the horizontal state is that the included angle between the robot body and the XY plane is smaller than or equal to the preset wall climbing angle.

Referring to fig. 1, fig. 1 is a flowchart of a method for planning a motion path of an underwater robot according to a first embodiment of the present invention.

As shown in fig. 1, a method for planning a moving path of an underwater robot may include the following steps S100 to S600, which are described as an example of a path planning method for an underwater cleaning robot to clean a swimming pool, wherein the cleaning path planning for the bottom of the swimming pool and the surrounding walls is involved.

Step S100: and acquiring the current state information of the underwater robot in real time.

Specifically, the state information is included angles between the body and XY, XZ, and YZ planes. The X-axis, the Y-axis and the Y-axis are planes in which the X-axis, the Y-axis and the Z-axis are located in pairs in a space coordinate system, and the X-axis and the Y-axis form a plane; the XZ plane is a plane where the X axis and the Z axis are located; the YZ plane is the plane formed by the Y axis and the Z axis, and the XY plane is parallel to the bottom surface of the pool. The device for acquiring the current state information is an inertial sensor, which can be a three-axis acceleration sensor, a gyroscope or a combination of the two. And when the underwater robot moves, updating the current state information in real time.

The state information in this embodiment includes the included angles between the body and the three planes XY, YZ, and XZ.

Step S200: and controlling the underwater robot to advance and climb the wall based on the state information, and retreating and falling the wall after the underwater robot advances and climbs the wall.

Specifically, the controlling the underwater robot to advance and climb the wall based on the state information includes: judging whether the underwater robot is in a horizontal state or not based on the state information, and if so, controlling the underwater robot to horizontally advance; acquiring current horizontal forward time, and comparing the horizontal forward time with first preset time; if the horizontal advancing time is shorter than a first preset time, judging whether the underwater robot meets a side wall or not based on the state information, and if so, controlling the underwater robot to advance and climb the wall according to the state information; acquiring current advancing wall climbing time, and comparing the advancing wall climbing time with second preset time; and if the forward wall climbing time is not less than the second preset time, controlling the underwater robot to retreat and lower the wall.

In the horizontal advancing process of the underwater robot, when the robot meets the side wall, the body can incline because the underwater robot continues to advance horizontally, and the included angle of the body relative to the XY plane is gradually increased until the body is perpendicular to the XY plane. Correspondingly, in this embodiment, determining whether the underwater robot meets the side wall based on the state information includes: and judging whether the included angle between the body and the XY plane reaches 60 degrees or not, and if the included angle reaches 60 degrees, determining that the side wall is encountered. Otherwise, the sidewall is deemed not encountered.

And when the included angle between the underwater robot and the XY plane reaches a certain value, controlling the underwater robot to advance and climb the wall. Specifically, the angle of the underwater robot and the XY plane is compared with a preset climbing angle, when the angle is larger than the preset climbing angle, the underwater robot is considered to encounter the side wall, the underwater robot is controlled to advance to climb the wall, the preferable preset climbing angle is 60 degrees, and when the angle of the underwater robot and the XY plane is larger than 60 degrees, the underwater robot is controlled to advance to climb the wall. And when the underwater robot moves forwards and climbs the wall for a second preset time, retreating and falling the wall. The second preset time is preset second preset time according to the height of the swimming pool and the movement speed of the underwater robot. That is, after the underwater robot is operated at a fixed speed in the vertical direction for a second preset time, the traveled distance is slightly greater than the height of the swimming pool.

Step S300: and judging whether the underwater robot retreats to leave the wall or not based on the state information, if so, controlling the underwater robot to retreat firstly to perform first steering, and retreating again after the first steering to perform second steering with the same angle as the first steering direction.

Specifically, the step of judging whether the underwater robot retreats from the wall after finishing the retreating based on the state information is as follows: and judging whether the underwater robot is in a horizontal state at present, comparing the angle between the underwater robot and the XY plane with a preset wall descending angle, and judging that the underwater robot finishes the wall descending when the angle is smaller than the preset wall descending angle, wherein the preferable preset wall descending angle is 30 degrees, namely when the angle with the XY plane is smaller than 30 degrees. Which indicates that the underwater robot has completely fallen off the wall. And when the vehicle completely runs off the wall, the vehicle backs off, when the vehicle backs off for a third preset time, the vehicle turns for the first time, and then backs off for the third preset time after the vehicle turns for the first time, and then turns for the second time. And when the horizontal state is judged not to be in, the wall is continuously retreated. The underwater robot is turned for the first time and the second time in opposite directions, namely, the underwater robot is adjusted by a preset distance in the width direction of the underwater robot through two turns, and the angles of the first turn and the second turn are the same, namely, the underwater robot is still retreated in the direction before the two turns after the two turns, and preferably, the turning angle of the first turn and the second turn is 45 degrees. The third preset time is preset according to the movement speed of the underwater robot and the length of the machine body, and the third preset time can ensure that the underwater robot can adjust the preset distance in the width direction of the machine after two turns.

Step S400: and controlling the underwater robot to retreat and climb the wall based on the state information, and advancing to descend the wall after retreating and climbing.

Specifically, after the underwater robot is adjusted by the predetermined distance in the machine width direction in step S300, the underwater robot is in a horizontal state, that is, an included angle between the underwater robot and the XY plane is smaller than a preset horizontal angle, and at this time, the underwater robot moves horizontally backward.

Specifically, controlling the underwater robot to retreat and climb the wall based on the state information, and advancing to the lower wall after retreating and climbing the wall comprises: acquiring current horizontal back-off time, and comparing the horizontal back-off time with first preset time; if the horizontal retreating time is less than a first preset time, judging whether the underwater robot meets a side wall or not based on the state information, and if the underwater robot meets the side wall, controlling the underwater robot to retreat and climb the wall according to the state information; obtaining the current wall climbing retreating time, and comparing the wall climbing retreating time with a second preset time; and if the retreating wall climbing time is not less than the second preset time, controlling the underwater robot to advance to lower the wall.

In the process of horizontally retreating the underwater robot, the underwater robot horizontally retreats, and the machine body can incline to gradually increase the included angle of the machine body relative to the XY plane. Correspondingly, in this embodiment, determining whether the underwater robot meets the side wall based on the state information includes: and judging whether the included angle between the body and the XY plane reaches 60 degrees or not, and if the included angle reaches 60 degrees, determining that the side wall is encountered. Otherwise, the sidewall is deemed not encountered.

When the included angle between the underwater robot and the XY plane reaches a certain value, the underwater robot is considered to meet the side wall and is controlled to retreat and climb the wall. Specifically, the angle of the underwater robot and the XY plane is compared with a preset climbing angle, when the angle is larger than the preset climbing angle, the underwater robot is considered to encounter the side wall and control the underwater robot to retreat and climb, the preferable preset climbing angle is 60 degrees, and when the angle of the underwater robot and the XY plane is larger than 60 degrees, the underwater robot is controlled to retreat and climb. And when the underwater robot retreats and climbs the wall for a second preset time, the underwater robot advances to descend the wall. The second preset time is preset second preset time according to the height of the swimming pool and the movement speed of the underwater robot. That is, after the underwater robot is operated at a fixed speed in the vertical direction for a second preset time, the traveled distance is slightly greater than the height of the swimming pool.

Step S500: and judging whether the underwater robot finishes advancing and getting off the wall or not based on the state information, if so, controlling the underwater robot to advance firstly and carry out third steering, and after the third steering, advancing again and carrying out fourth steering with the same angle as the third steering direction.

In the process of advancing to lower the wall, the underwater robot gradually leaves the side wall to be in contact with the bottom surface of the swimming pool, in the process, the included angle of the body of the underwater robot relative to the XY plane is gradually reduced, and the underwater robot horizontally advances after entering a horizontal state. Specifically, whether the underwater robot finishes advancing and leaves the wall or not is judged based on the state information as follows: and judging whether the underwater robot is in a horizontal state at present, comparing the angle between the underwater robot and the XY plane with a preset wall descending angle, and judging that the underwater robot finishes the wall descending when the angle is smaller than the preset wall descending angle, wherein the preferable preset wall descending angle is 30 degrees, namely when the angle with the XY plane is smaller than 30 degrees. And when the vehicle completely runs off the wall, the vehicle moves forwards, when the vehicle horizontally moves forwards for a third preset time, steering for the third time is carried out, the vehicle moves forwards for the third preset time after steering for the third time, and then steering for the fourth time is carried out. And when the position is judged not to be in the horizontal state, continuing to move to the lower wall. Wherein the third steering is opposite to the fourth steering in rotation direction, i.e. the underwater robot is adjusted by the predetermined distance in the width direction of the underwater robot through two steering, and the third steering and the fourth steering are at the same rotation angle, i.e. the underwater robot still advances in the direction before the two steering after the two steering, and the rotation angle of the third steering and the fourth steering is preferably 45 °. The third preset time is preset according to the movement speed of the underwater robot and the length of the machine body, and the third preset time can ensure that the underwater robot can adjust the preset distance in the width direction of the machine after two turns.

Step S600: the above steps are repeated and the number of round trips is increased by 1.

Specifically, step S100 is executed again after the predetermined distance of the body is adjusted, so as to ensure that the underwater robot can clean the swimming pool without dead angles. When step S100 is performed again, the number of round trips is increased by 1 for counting the number of round trips for cleaning the underwater robot in the current direction, and when the number of round trips is increased to n, the number of round trips is reset to 1. Where n is the maximum value of the preset statistical round trip times.

Referring to fig. 2, fig. 2 is a flowchart of a method for planning a motion path of an underwater robot according to a second embodiment of the present invention. The second embodiment of the present invention is a specific optimization of step S200 in the first embodiment, and the steps identical to those in the first embodiment are not described again in this embodiment.

As shown in fig. 2, the second embodiment of the present invention may include the following steps S210 to S250.

Step S210: judging whether the underwater robot is in a horizontal state or not based on the state information, and if the underwater robot is in the horizontal state, acquiring the current round-trip times; and if the round-trip times are less than n times, controlling the underwater robot to advance.

Specifically, whether the underwater robot is in a horizontal state is judged based on the state information: acquiring current state information in real time, judging whether the current underwater robot is in a horizontal state or not based on an included angle between the current underwater robot and an XY plane in the current state information, and if the included angle between the current underwater robot and the XY plane is smaller than a preset horizontal angle, controlling the underwater robot to move forward by the underwater robot in the horizontal state, wherein the preset horizontal angle is preferably 30 degrees. And if the round-trip times are not less than n times, acquiring the information of the zone bit, controlling the underwater robot to turn for the fifth time based on the zone bit, and after the fifth turning, advancing and adding 1 to the zone bit. If the underwater robot is not in the horizontal state, the underwater robot is controlled to retreat/advance to the lower wall, the number of times of the reciprocating is reduced by 1, and the step S300 is executed. Specifically, the value range of the flag bit is that m is more than or equal to 1 and less than or equal to 4; when the zone bit is equal to 1, the steering is carried out on the right when the vehicle moves backwards, the steering is not carried out when the vehicle moves forwards, and 1 is added to the zone bit; when the zone bit is equal to 2, the steering is carried out on the left when the vehicle moves backwards, the steering is not carried out when the vehicle moves forwards, and 1 is added to the zone bit; when the zone bit is equal to 3, the steering is not carried out when the vehicle moves backwards, the steering is carried out when the vehicle moves forwards, and 1 is added to the zone bit; when the zone bit is equal to 4, the steering is not carried out when the vehicle moves backwards, the steering is carried out on the left when the vehicle moves forwards, and 1 is added to the zone bit; and resetting the flag bit to 1 when the flag bit is larger than 4. The preferred angle of rotation for the fifth turn is 90 °. The value range of the zone bit and the operation rules which are not performed simultaneously can be adjusted.

Step S220: and acquiring the current forward time, and comparing the forward time with a first preset time.

Specifically, the current advance time is: and the underwater robot detects the time from the time point of starting to move ahead in the horizontal state to the current time point. And comparing the current advancing time with a preset first preset time. The first preset time is preset according to the length and the width of the swimming pool and the movement speed of the underwater robot, and an optimal cleaning path can be selected for the underwater robot based on the first preset time.

Step S230: and if the advancing time is less than the first preset time, judging whether the underwater robot meets the side wall or not based on the state information, and if so, controlling the underwater robot to advance and climb the wall.

Specifically, the current forward time is compared with a first preset time, and whether the underwater robot meets a side wall or not is judged based on the state information. Wherein the side walls are encountered: in the advancing/retreating process of the underwater robot, the underwater robot horizontally advances/retreats, the machine body inclines, and the included angle of the machine body relative to the XY plane is gradually increased. Correspondingly, in this embodiment, determining whether the underwater robot meets the side wall based on the state information includes: and judging whether the included angle between the body and the XY plane reaches 60 degrees or not, and if the included angle reaches 60 degrees, determining that the side wall is encountered. Otherwise, the sidewall is deemed not encountered. When the forward time is less than the first preset time and does not meet the side wall, continuing to advance; when the current advancing time is less than the first preset time and meets the side wall, advancing to climb the wall; if the current time is not less than the first preset time, step S300 is executed.

Step S240: and acquiring the current advancing wall climbing time, and comparing the advancing wall climbing time with second preset time.

Specifically, the current forward wall climbing time is as follows: the time from the time point when the side wall starts to advance and climb the wall to the current time point is met. And comparing the current advancing wall climbing time with a preset second preset time. The second preset time is preset second preset time according to the height of the swimming pool and the movement speed of the underwater robot. That is, after the underwater robot is operated at a fixed speed in the vertical direction for a second preset time, the traveled distance does not exceed the height of the swimming pool.

Step S250: and if the forward wall climbing time is not less than the second preset time, controlling the underwater robot to retreat and lower the wall.

Specifically, the current advancing wall climbing time is compared with a second preset time, and when the advancing wall climbing time is not less than the second preset time, the wall is retreated; and when the advancing wall climbing time is less than the second preset time, continuing advancing the wall climbing.

Referring to fig. 3, fig. 3 is a flowchart of a method for planning a motion path of an underwater robot according to a third embodiment of the present invention. The third embodiment of the present invention is a specific optimization of step S400 in the first and second embodiments, and the same steps as those in the first and second embodiments are not repeated in this embodiment.

As shown in fig. 3, the third embodiment of the present invention may include the following steps S410 to S450.

Step S410: judging whether the underwater robot is in a horizontal state or not based on the state information, and if the underwater robot is in the horizontal state, acquiring the current round-trip times; and if the round-trip times are less than n times, controlling the underwater robot to retreat.

Specifically, whether the underwater robot is in a horizontal state is judged based on the state information: acquiring current state information in real time, judging whether the current underwater robot is in a horizontal state or not based on an included angle between the current underwater robot and an XY plane in the current state information, and if the included angle between the current underwater robot and the XY plane is smaller than a preset horizontal angle, controlling the underwater robot to retreat by the underwater robot in the horizontal state, wherein the preset horizontal angle is preferably 30 degrees. And if the round-trip times are not less than n times, acquiring the information of the zone bit, controlling the underwater robot to turn for the fifth time based on the zone bit, and after the fifth turning, advancing and adding 1 to the zone bit. If the underwater robot is not in the horizontal state, controlling the underwater robot to retreat/advance to descend to the wall and subtracting 1 from the number of round trips, and executing the step S500. Specifically, the value range of the flag bit is that m is more than or equal to 1 and less than or equal to 4; when the zone bit is equal to 1, the steering is carried out on the right when the vehicle moves backwards, the steering is not carried out when the vehicle moves forwards, and 1 is added to the zone bit; when the zone bit is equal to 2, the steering is carried out on the left when the vehicle moves backwards, the steering is not carried out when the vehicle moves forwards, and 1 is added to the zone bit; when the zone bit is equal to 3, the steering is not carried out when the vehicle moves backwards, the steering is carried out when the vehicle moves forwards, and 1 is added to the zone bit; when the zone bit is equal to 4, the steering is not carried out when the vehicle moves backwards, the steering is carried out on the left when the vehicle moves forwards, and 1 is added to the zone bit; and resetting the flag bit to 1 when the flag bit is larger than 4. The preferred angle of rotation for the fifth turn is 90 °.

Step S420: and obtaining the current back-off time, and comparing the back-off time with a first preset time.

Specifically, the current backoff time is: and the underwater robot detects the time from the time point when the underwater robot starts to retreat in the horizontal state to the current time point. And comparing the current back-off time with a preset first preset time. The first preset time is preset according to the length and the width of the swimming pool and the movement speed of the underwater robot, and an optimal cleaning path can be selected for the underwater robot based on the first preset time.

Step S430: and if the retreating time is less than the first preset time, judging whether the underwater robot meets the side wall or not based on the state information, and if so, controlling the underwater robot to retreat and climb the wall.

Specifically, the current back-off time is compared with a first preset time, and whether the underwater robot meets a side wall or not is judged based on the state information. Wherein the side walls are encountered: in the advancing/retreating process of the underwater robot, the underwater robot horizontally advances/retreats, the machine body inclines, and the included angle of the machine body relative to the XY plane is gradually increased. Correspondingly, in this embodiment, determining whether the underwater robot meets the side wall based on the state information includes: and judging whether the included angle between the body and the XY plane reaches 60 degrees or not, and if the included angle reaches 60 degrees, determining that the side wall is encountered. Otherwise, the sidewall is deemed not encountered. When the retreating time is less than the first preset time and the side wall is not met, continuing to advance; when the retreating time is less than the first preset time and the side wall is met, advancing to climb the wall; if the current time is not less than the first preset time, step S500 is executed.

Step S440: and obtaining the current retreating wall climbing time, and comparing the retreating wall climbing time with second preset time.

Specifically, the current retreat wall climbing time is as follows: when the side wall starts to retreat from the time point of climbing the wall to the current time point, the time length is obtained. And comparing the current retreating wall climbing time with a preset second preset time. The second preset time is preset second preset time according to the height of the swimming pool and the movement speed of the underwater robot. That is, after the underwater robot is operated at a fixed speed in the vertical direction for a second preset time, the traveled distance does not exceed the height of the swimming pool.

Step S450: and if the retreating wall climbing time is not less than the second preset time, controlling the underwater robot to advance to lower the wall.

Specifically, the current retreat wall climbing time is compared with a second preset time, and when the retreat wall climbing time is not less than the second preset time, the user moves forward to lower the wall; and when the retreating wall climbing time is less than the second preset time, continuing to advance to climb the wall.

Referring to fig. 4, fig. 4 is a flowchart of a method for planning a motion path of an underwater robot according to a fourth embodiment of the present invention. The fourth embodiment of the present invention is specifically optimized for forward climbing, backward lowering, backward climbing, and forward lowering in the above embodiments.

As shown in fig. 4, the fourth embodiment of the present invention may include the following steps S431 to S434.

Step S431: and acquiring the current state information in real time.

Specifically, the information of included angles between the current body and the XY plane, the XZ plane, and the YZ plane is obtained.

Step S432: and judging whether the current state of the underwater robot is vertical wall climbing/falling or not based on the state information.

Specifically, whether the current state is vertical wall climbing/descending is judged, that is, whether an included angle exists between the machine body and a YZ plane is judged when the included angle between the machine body and the XY plane is 90 degrees.

Step S433: and if the climbing/falling is vertical, controlling the underwater robot to continuously climb/fall the wall.

Specifically, if the included angle between the body and the XY plane is 90 °, the body and the YZ plane do not have an included angle, and the climbing/lowering of the wall is continued.

Step S434: and if the current state is not the vertical wall climbing/descending state, judging that the current state of the underwater robot is the wall winding state.

Specifically, if the included angle between the body and the XY plane is 90 °, the body and the YZ plane have an included angle, and the degree of the included angle is greater than 25 °, it is determined that the current state is the wall-winding state.

Referring to fig. 5, fig. 5 is a flowchart of a method for planning a motion path of an underwater robot according to a fifth embodiment of the present invention. The fifth embodiment of the present invention is a specific optimization of the determination of the wall-surrounding state in the above embodiments.

As shown in fig. 5, the fifth embodiment of the present invention may include the following steps S641 to S645.

Step S641: and acquiring the current state information in real time.

Specifically, information of included angles between the current body and an XY plane, an XZ plane and a YZ plane is obtained.

Step S642: and controlling the underwater robot to adjust the position of the body based on the state information.

Specifically, when the robot is in a wall-surrounding state, the included angle between the robot body and the XY plane is 90 degrees, and the included angle between the robot body and the YZ plane exists. The position of the machine body is adjusted to control the rotating speed of the wheels on the left side and the right side of the underwater robot through the motor, so that a speed difference is formed. When the robot body is positioned on the left side of a YZ plane, and an included angle is formed between the robot body and the YZ plane and is more than 25 degrees, controlling the rotating speed of a left wheel of the underwater robot to be more than that of a right wheel, so that the included angle between the robot body and the YZ plane is adjusted to be zero; when the robot body is arranged on the right side of the YZ plane, an included angle is formed between the robot body and the YZ plane and is larger than 25 degrees, the rotating speed of the wheels on the right side of the underwater robot is controlled to be larger than that of the wheels on the left side, and therefore the included angle between the robot body and the YZ plane is adjusted to be zero. Thereby achieving the purpose of adjusting the position of the machine body.

Step S643: and judging whether the underwater robot is successfully adjusted to vertical wall climbing/falling based on the state information.

Specifically, when the adjustment is completed, the information of the included angle between the current body and the XY plane, the XZ plane, and the YZ plane is obtained again. And judging whether the adjustment is successful to be vertical wall climbing/descending, namely judging whether the included angle between the machine body and the YZ plane is 90 degrees or not.

Step S644: and if the adjustment is successful, controlling the underwater robot to retreat/advance to lower the wall, and executing the step S500.

Specifically, if the included angle between the front body and the XY plane is 90 °, the body and the YZ plane do not have an included angle, that is, if the successful adjustment is vertical wall climbing/lowering, the body retreats/advances to lower the wall, and step S500 is executed.

Step S645: and if the adjustment is not successful, controlling the underwater robot to give an alarm.

Specifically, when the included angle between the front body and the XY plane is 90 °, the included angle between the body and the YZ plane exists, that is, the included angle is not successfully adjusted to vertical wall climbing/falling, and an alarm is given. Wherein the alarm can be a warning music and/or a light flashing and the like. In particular to a buzzer and an LED alarm indicator lamp.

In the above embodiment, the method further includes presetting a first preset time, a second preset time, a third preset time, a maximum round-trip time n, and an initial value of the flag bit.

Referring to fig. 6-7, fig. 6 is a schematic diagram illustrating a module relationship of an underwater robot motion path planning system according to an embodiment of the present invention; fig. 7 is a schematic diagram of a module relationship of a climbing control module according to an embodiment of the present invention.

As shown in fig. 6 to 7, a motion path planning system for an underwater robot includes: the state acquisition module 100 is configured to acquire current state information of the underwater robot in real time; a climbing control module 200 for controlling the underwater robot to advance/retreat to climb the wall based on the state information, and retreat/advance to descend the wall after advancing/retreating to climb the wall; the steering control module 300 is configured to determine whether the underwater robot retreats from the wall or not based on the state information, if yes, control the underwater robot to retreat first to perform first steering, retreat again after the first steering, and perform second steering with an angle opposite to the first steering direction; and judging whether the underwater robot finishes advancing and getting off the wall or not based on the state information, if so, controlling the underwater robot to advance firstly and carry out third steering, and after the third steering, advancing again and carrying out fourth steering with the same angle as the third steering direction.

Specifically, the state information acquired by the state acquiring module 100 is an included angle between the body and an XY plane, an XZ plane, and a YZ plane. The X-axis, the Y-axis and the Y-axis are planes in which the X-axis, the Y-axis and the Z-axis are located in pairs in a space coordinate system, and the X-axis and the Y-axis form a plane; the XZ plane is a plane where the X axis and the Z axis are located; the YZ plane is a plane formed by the Y axis and the Z axis. And the XY plane is parallel to the horizontal plane. The state acquisition module 100 is an inertial sensor, and may be a three-axis acceleration sensor, a gyroscope, or a combination of the two. And when the underwater robot moves, updating the current state information in real time.

The wall climbing control module 200 is used for enabling the underwater robot to horizontally advance/retreat in the horizontal advancing process of the underwater robot, enabling the body to incline and enabling the included angle of the body relative to the XY plane to gradually increase. Correspondingly, in this embodiment, determining whether the underwater robot meets the side wall based on the state information includes: and judging whether the included angle between the body and the XY plane reaches 60 degrees or not, and if the included angle reaches 60 degrees, determining that the side wall is encountered. Otherwise, the sidewall is deemed not encountered. And when the included angle between the underwater robot and the XY plane reaches a certain value, controlling the underwater robot to move forward/backward to climb the wall. Specifically, if meet the side wall, then control underwater robot advances/retreat climbing includes: the angle of the underwater robot and the XY plane is compared with a preset wall climbing angle, when the angle is larger than the preset wall climbing angle, the underwater robot is controlled to advance and climb the wall, the preferred preset wall climbing angle is 60 degrees, namely, when the angle with the XY plane is larger than 60 degrees, the underwater robot is controlled to advance/retreat and climb the wall. And when the underwater robot moves forwards/backwards to climb the wall for a second preset time, retreating/advancing to descend the wall. The second preset time is preset second preset time according to the height of the swimming pool and the movement speed of the underwater robot. That is, after the underwater robot is operated at a fixed speed in the vertical direction for a second preset time, the traveled distance is slightly greater than the height of the swimming pool.

The steering control module 300 is configured to determine whether the underwater robot is retreated from the wall based on the state information: and judging whether the underwater robot is in a horizontal state at present, comparing the angle between the underwater robot and the XY plane with a preset wall descending angle, and judging that the underwater robot finishes the wall descending when the angle is smaller than the preset wall descending angle, wherein the preferable preset wall descending angle is 30 degrees, namely when the angle with the XY plane is smaller than 30 degrees. Which indicates that the underwater robot has completely fallen off the wall. And when the vehicle completely runs off the wall, the vehicle backs off, when the vehicle backs off for a third preset time, the vehicle turns for the first time, and then backs off for the third preset time after the vehicle turns for the first time, and then turns for the second time. And when the horizontal state is judged not to be in, the wall is continuously retreated. The underwater robot is turned for the first time and the second time in opposite directions, namely, the underwater robot is adjusted by a preset distance in the width direction of the underwater robot through two turns, and the angles of the first turn and the second turn are the same, namely, the underwater robot is still retreated in the direction before the two turns after the two turns, and preferably, the turning angle of the first turn and the second turn is 45 degrees. The third preset time is preset according to the movement speed of the underwater robot and the length of the machine body, and the third preset time can ensure that the underwater robot can adjust the preset distance in the width direction of the machine after two turns. And the controller is also used for judging whether the underwater robot finishes advancing and leaves the wall based on the state information: and judging whether the underwater robot is in a horizontal state at present, comparing the angle between the underwater robot and the XY plane with a preset wall descending angle, and judging that the underwater robot finishes the wall descending when the angle is smaller than the preset wall descending angle, wherein the preferable preset wall descending angle is 30 degrees, namely when the angle with the XY plane is smaller than 30 degrees. And when the vehicle completely runs off the wall, advancing, carrying out steering for the third time when the vehicle advances for the third preset time, advancing for the third preset time after the steering for the third time, and then carrying out steering for the fourth time. And when the position is judged not to be in the horizontal state, continuing to move to the lower wall. Wherein the third steering is opposite to the fourth steering in rotation direction, i.e. the underwater robot is adjusted by the predetermined distance in the width direction of the underwater robot through two steering, and the third steering and the fourth steering are at the same rotation angle, i.e. the underwater robot still advances in the direction before the two steering after the two steering, and the rotation angle of the third steering and the fourth steering is preferably 45 °. The third preset time is preset according to the movement speed of the underwater robot and the length of the machine body, and the third preset time can ensure that the underwater robot can adjust the preset distance in the width direction of the machine after two turns.

Preferably, the climbing control module 200 includes: the judging module 210 is configured to judge whether the underwater robot meets a side wall based on the state information, and if not, control the underwater robot to move forward/backward; judging whether the forward/backward time is not less than the first preset time, if so, judging whether the underwater robot meets a side wall based on the state information, and if so, controlling the underwater robot to climb the wall forward/backward; judging whether the forward/backward wall climbing time is not less than the second preset time or not; and the lower wall control module 220 is configured to control the underwater robot to retreat/advance to lower the wall if the advancing/retreating wall climbing time is not less than the second preset time.

The judging module 210 is configured to judge whether the underwater robot is in a horizontal state based on the state information: acquiring current state information in real time, judging whether the current underwater robot is in a horizontal state or not based on an included angle between the current underwater robot and an XY plane in the current state information, and if the included angle between the current underwater robot and the XY plane is smaller than a preset horizontal angle, controlling the underwater robot to move forward by the underwater robot in the horizontal state, wherein the preset horizontal angle is preferably 30 degrees.

The determining module 210 is further configured to compare the current forward time with a preset first preset time. The first preset time is preset according to the length and the width of the swimming pool and the movement speed of the underwater robot, and an optimal cleaning path can be selected for the underwater robot based on the first preset time. And comparing the current forward time with a first preset time, and judging whether the underwater robot meets a side wall or not based on the state information. Wherein the side walls are encountered: in the advancing/retreating process of the underwater robot, the underwater robot horizontally advances/retreats, the machine body inclines, and the included angle of the machine body relative to the XY plane is gradually increased. Correspondingly, in this embodiment, determining whether the underwater robot meets the side wall based on the state information includes: and judging whether the included angle between the body and the XY plane reaches 60 degrees or not, and if the included angle reaches 60 degrees, determining that the side wall is encountered. Otherwise, the sidewall is deemed not encountered. When the forward time is less than the first preset time and does not meet the side wall, continuing to advance; when the current advancing time is less than the first preset time and meets the side wall, advancing to climb the wall; if the current time is not less than the first preset time, step S300 is executed. And comparing the current advancing wall climbing time with a preset second preset time. The second preset time is preset second preset time according to the height of the swimming pool and the movement speed of the underwater robot. That is, after the underwater robot is operated at a fixed speed in the vertical direction for a second preset time, the traveled distance does not exceed the height of the swimming pool.

The lower wall control module 220 is configured to control the underwater robot to retreat/advance to lower the wall when the advancing/retreating wall climbing time is not less than the second preset time.

In the motion path planning method of the present invention, it is determined whether the underwater robot is in a horizontal state if any one of the following three conditions is satisfied:

(a) after the steering, the included angle between the machine body and the XY plane is smaller than a horizontal included angle, and the horizontal included angle is 30 degrees in the embodiment;

(b) when the side wall is arranged below the bottom surface of the swimming pool, the included angle between the machine body and the XY plane is smaller than the lower wall angle, and the lower wall angle is 30 degrees in the embodiment;

(c) when the side wall is reached from the bottom surface of the swimming pool, the included angle between the body and the XY plane is smaller than the climbing angle, which is 60 ° in this embodiment.

The values of the horizontal included angle, the lower wall angle and the upper wall angle are independent and incoherent, and when the method is applied, the specific implementation scheme can be adjusted according to application requirements on the basis of the judgment principle.

The technical features of the embodiments described above may be arbitrarily combined, and for the sake of brevity, all possible combinations of the technical features in the embodiments described above are not described, but should be considered as being within the scope of the present specification as long as there is no contradiction between the combinations of the technical features.

The above-mentioned embodiments only express several embodiments of the present invention, and the description thereof is more specific and detailed, but not construed as limiting the scope of the invention. It should be noted that, for a person skilled in the art, several variations and modifications can be made without departing from the inventive concept, which falls within the scope of the present invention. Therefore, the protection scope of the present patent shall be subject to the appended claims.

Claims

1. A method for planning a motion path of an underwater robot is characterized by comprising the following steps:

acquiring current state information of the underwater robot in real time; the state information is the included angle between the machine body and an XY plane, an XZ plane and a YZ plane;

controlling the underwater robot to advance and climb the wall based on the state information, and retreating and falling the wall after the underwater robot advances and climbs the wall;

judging whether the underwater robot retreats to leave the wall or not based on the state information, if so, controlling the underwater robot to retreat firstly to perform first steering, and retreating again after the first steering to perform second steering with the same angle as the first steering direction;

controlling the underwater robot to retreat and climb the wall based on the state information, and advancing to descend the wall after retreating and climbing;

judging whether the underwater robot finishes advancing and getting off the wall or not based on the state information, if so, controlling the underwater robot to advance firstly and carry out third steering, and after the third steering, advancing again and carrying out fourth steering with the same angle as the third steering;

repeating the steps, and adding 1 to the round trip times;

the step of controlling the underwater robot to advance and climb the wall and retreat and climb the wall after advancing and climbing the wall based on the state information and the step of controlling the underwater robot to retreat and climb the wall based on the state information, wherein the step of advancing and descending the wall after retreating and climbing the wall is specifically as follows:

judging whether the underwater robot is in a horizontal state or not based on the state information, and if the underwater robot is in the horizontal state, acquiring the current round-trip times; if the number of round trips is less than n times, controlling the underwater robot to move forwards/backwards;

acquiring current forward/backward time, and comparing the forward/backward time with first preset time;

if the forward/backward time is less than a first preset time, judging whether the underwater robot meets a side wall or not based on the state information, and if so, controlling the underwater robot to climb the wall forward/backward;

acquiring current forward/backward wall climbing time, and comparing the forward/backward wall climbing time with second preset time;

if the advancing/retreating wall climbing time is not less than the second preset time, controlling the underwater robot to retreat/advance to lower the wall;

the horizontal state includes: after the first steering, the second steering, the third steering, the fourth steering and the fifth steering, the horizontal state is as follows: the included angle between the underwater robot and the XY plane is smaller than a preset horizontal included angle which is 30 degrees; when the robot is lowered from the wall, the horizontal state is that the included angle between the robot body and the XY plane is smaller than or equal to a preset lower wall angle; when the robot is on the wall, the horizontal state is that the included angle between the robot body and the XY plane is smaller than or equal to the preset wall climbing angle.

2. The method of claim 1, wherein the number of round trips is reset to 1 when the number of round trips is increased to n.

3. The method according to claim 2, wherein the underwater robot is judged whether to be in a horizontal state or not based on the state information, and if so, the current round trip times are acquired; if the number of round trips is less than n times, controlling the underwater robot to move forward/backward further comprises:

and if the round-trip times are not less than n times, acquiring the information of the zone bit, controlling the underwater robot to turn for the fifth time based on the zone bit, and after the fifth turning, advancing/backing and adding 1 to the zone bit.

4. The method according to claim 3, wherein the underwater robot is judged whether to be in a horizontal state or not based on the state information, and if so, the current round trip times are acquired; if the number of round trips is less than n times, controlling the underwater robot to move forward/backward further comprises:

and if the underwater robot is not in a horizontal state, controlling the underwater robot to retreat/advance to descend the wall and subtracting 1 from the number of times of round trip, judging whether the underwater robot finishes retreating/advancing to descend the wall or not based on the state information, if so, controlling the underwater robot to retreat/advance in sequence to carry out first/third steering, and retreating/advancing again after the first/third steering to carry out second/fourth steering with the same angle with the first/third steering.

5. The method according to claim 3, wherein the fifth steering based on the flag bit is specifically:

the value range of the flag bit is more than or equal to 1 and less than or equal to 4;

when the zone bit is equal to 1, the steering is carried out on the right when the vehicle moves backwards, the steering is not carried out when the vehicle moves forwards, and 1 is added to the zone bit;

when the zone bit is equal to 2, the steering is carried out on the left when the vehicle moves backwards, the steering is not carried out when the vehicle moves forwards, and 1 is added to the zone bit;

when the zone bit is equal to 3, the steering is not carried out when the vehicle moves backwards, the steering is carried out when the vehicle moves forwards, and 1 is added to the zone bit;

when the zone bit is equal to 4, the steering is not carried out when the vehicle moves backwards, the steering is carried out on the left when the vehicle moves forwards, and 1 is added to the zone bit;

and resetting the flag bit to 1 when the flag bit is larger than 4.

6. The method of claim 3,

the rotation angle of the first steering, the second steering, the third steering and the fourth steering is 45 degrees, and the rotation angle of the fifth steering is 90 degrees.

7. The method of claim 1, wherein if the forward/backward time is less than a first preset time, determining whether the underwater robot meets a side wall based on the state information, and if so, controlling the underwater robot to move forward/backward to climb the wall further comprises:

if the forward/backward time is not less than the first preset time, the execution step judges whether the underwater robot finishes forward/backward wall falling or not based on the state information, if so, the underwater robot is controlled to advance/backward in advance, third/primary steering is carried out, and after the third/primary steering, the underwater robot advances/backward again, and fourth/secondary steering with the same angle as the third/primary steering is carried out.

8. The method of claim 1, wherein if the forward/backward time is less than a first preset time, determining whether the underwater robot meets a side wall based on the state information, and if so, controlling the underwater robot to move forward/backward to climb the wall further comprises:

and if the forward/backward time is less than the first preset time and the side wall is not met, controlling the underwater robot to continue to move forward/backward.

9. The method of claim 1, wherein the step of controlling the underwater robot to retreat/advance down the wall if the advancing/retreating climbing time is not less than the second preset time further comprises:

and if the advancing/retreating wall climbing time is less than the second preset time, controlling the underwater robot to continue advancing/retreating to climb the wall.

10. The method of claim 1, wherein the forward climbing, the backward lower wall, the backward climbing, and the forward lower wall further comprise:

acquiring current state information in real time;

judging whether the current state of the underwater robot is vertical wall climbing/falling or not based on the state information;

if the climbing/falling is vertical, controlling the underwater robot to continuously climb/fall the wall;

and if the current state is not the vertical wall climbing/descending state, judging that the current state of the underwater robot is the wall winding state.

11. The method of claim 10, wherein when in a wall-surrounding condition:

acquiring current state information in real time;

controlling the underwater robot to adjust the position of the body based on the state information;

judging whether the underwater robot is successfully adjusted to vertical wall climbing/falling or not based on the state information;

if the underwater robot is successfully adjusted to be vertical wall climbing/falling, controlling the underwater robot to retreat/advance to fall on the wall, judging whether the underwater robot finishes advancing to fall on the wall or not based on the state information, if so, controlling the underwater robot to advance firstly and carry out third steering, and after third steering, advancing again and carrying out fourth steering with the same angle as the third steering;

and if the adjustment is not successful, controlling the underwater robot to give an alarm.

12. The method of claim 1, wherein the step of obtaining current status information of the underwater robot in real time further comprises:

presetting a first preset time, a second preset time, a maximum round-trip time n and an initial value of a flag bit.

13. An underwater robot motion path planning system, comprising:

the state acquisition module is used for acquiring the current state information of the underwater robot in real time; the state information is the included angle between the machine body and an XY plane, an XZ plane and a YZ plane;

the wall climbing control module is used for controlling the underwater robot to advance/retreat to climb the wall based on the state information and retreat/advance to descend the wall after advancing/retreating to climb the wall;

the steering control module is used for judging whether the underwater robot retreats from the wall or not based on the state information, if so, controlling the underwater robot to retreat firstly for carrying out first steering, and retreating again after the first steering for carrying out second steering with the same angle as the first steering direction; judging whether the underwater robot finishes advancing and getting off the wall or not based on the state information, if so, controlling the underwater robot to advance firstly and carry out third steering, and after the third steering, advancing again and carrying out fourth steering with the same angle as the third steering;

the steering control module is also used for repeating the steps and adding 1 to the round-trip times; the step of controlling the underwater robot to advance and climb the wall and retreat and climb the wall after advancing and climbing the wall based on the state information and the step of controlling the underwater robot to retreat and climb the wall based on the state information, wherein the step of advancing and descending the wall after retreating and climbing the wall is specifically as follows: judging whether the underwater robot is in a horizontal state or not based on the state information, and if the underwater robot is in the horizontal state, acquiring the current round-trip times; if the number of round trips is less than n times, controlling the underwater robot to move forwards/backwards; acquiring current forward/backward time, and comparing the forward/backward time with first preset time; if the forward/backward time is less than a first preset time, judging whether the underwater robot meets a side wall or not based on the state information, and if so, controlling the underwater robot to climb the wall forward/backward; acquiring current forward/backward wall climbing time, and comparing the forward/backward wall climbing time with second preset time; if the advancing/retreating wall climbing time is not less than the second preset time, controlling the underwater robot to retreat/advance to lower the wall;

14. The system of claim 13, wherein the wall climbing control module comprises:

the judging module is used for judging whether the underwater robot meets the side wall or not based on the state information, and if not, controlling the underwater robot to move forwards/backwards; judging whether the forward/backward time is not less than a first preset time, if so, judging whether the underwater robot meets a side wall or not based on the state information, and if so, controlling the underwater robot to climb the wall forwards/backwards; judging whether the forward/backward wall climbing time is not less than a second preset time or not;

and the lower wall control module is used for controlling the underwater robot to retreat/advance to lower the wall if the advancing/retreating wall climbing time is not less than second preset time.