CN108189031A - Underwater robot trajectory path planning method and system - Google Patents
Underwater robot trajectory path planning method and system Download PDFInfo
- Publication number
- CN108189031A CN108189031A CN201711443969.6A CN201711443969A CN108189031A CN 108189031 A CN108189031 A CN 108189031A CN 201711443969 A CN201711443969 A CN 201711443969A CN 108189031 A CN108189031 A CN 108189031A
- Authority
- CN
- China
- Prior art keywords
- wall
- underwater robot
- time
- status information
- backward
- 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
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Manipulator (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The present invention relates to a kind of underwater robot trajectory path planning method and system to include：Underwater robot current state information is obtained in real time；Wall is climbed based on status information advance, and advances and climbs wall under wall retreats later；Based on status information judge underwater robot whether after the completion of back off wall, if then priority setback row turns to for the first time, the first time turn to after retreat again, carry out second identical with first time steering direction opposite angles and turn to；Wall is climbed based on status information retrogressing, and wall under advancing after wall is climbed in retrogressing；Judge whether underwater robot completes the lower wall that advances based on status information, if then first advancing carries out third time steering, the third time is advanced again after turning to, and carries out four steering identical with third time steering direction opposite angles.The method of the present invention can ensure that underwater robot cleans swimming pool without dead angle.And since it is regular cleaning to swimming pool, it also is able to avoid the problem that underwater robot cable is wound.
Description
Technical field
The present invention relates to automation cleaning technology field, more particularly to a kind of underwater robot trajectory path planning method
And system.
Background technology
With national science and technology innovation and high-end manufacturing rapid development, Robot industry development is more and more each by the world
Development Robot industry is risen to national strategy, and in this, as holding and again by the highest attention of state, main economic body one after another
Obtain the important means of manufacturing industry competitive advantage.Wherein, people has been come into for meeting smart home robot used in everyday
Life, various types of smart home robot have been able to replace manually carry out various work.
Underwater robot, a kind of cleaning device for being used to clean private swimming pool or public swimming pool.Utilize underwater robot pair
Private swimming pool or public swimming pool, which carry out cleaning, can save manpower, and compared under manpower clean water robot cleaner it is more thorough
Bottom, therefore will certainly become a kind of trend using underwater robot come the cleaning that manpower is replaced to carry out swimming pool.In traditional technology, water
The angle of lower intelligent robot identification machine and X-axis, and its movement locus in swimming pool is uncertain, traditional technology
In movement of the underwater robot in swimming pool be random.
In the underwater robot of above-mentioned traditional technology carries out the clean technical solution of swimming pool, because machine can only be recognized
With the angle of an axis in space coordinates, and move be uncertain, so will result in machine cable line winding, and
And its uncertain movement locus is it is possible that cleaning dead zone, is not achieved the effect for cleaning swimming pool.
Invention content
Based on this, it is necessary to for the winding of machine cable line, cleaning dead zone occur, the effect that cleaning swimming pool is not achieved is asked
Topic, provides a kind of underwater robot trajectory path planning method and system.
The present invention provides a kind of underwater robot trajectory path planning methods to include step：The underwater machine is obtained in real time
The current status information of device people；The underwater robot advance is controlled to climb wall, and wall is climbed in advance based on the status information
Lower wall is retreated later；Based on the status information judge the underwater robot whether after the completion of back off wall, if so, control institute
It states underwater robot first to retreat, carries out first time steering, after the first time turns to, retreat again, carry out with turning to for the first time
Second identical of steering of direction opposite angles；The underwater robot retrogressing is controlled to climb wall based on the status information, and
Wall under advancing after wall is climbed in retrogressing；Judge whether the underwater robot completes the lower wall that advances based on the status information, if
Be that the underwater robot is then controlled first to advance, carry out third time steering, after the third time turns to, advance again, carry out with
The 4th time identical steering of third time steering direction opposite angles；Above step is repeated, and round-trip number is added 1.
Further, it is described the underwater robot advance to be controlled to climb wall, and wall is climbed in advance based on the status information
The underwater robot retrogressing is controlled to climb wall, and climb in retrogressing the step of wall and based on the status information under retreating later
It is specially the step of wall under advancing after wall：Judge whether the underwater robot is horizontality based on the status information,
If horizontality, current round-trip number is obtained；If round-trip number is less than n times, control the underwater robot advance/
It retreats；The current forward/backward time is obtained, the forward/backward time is compared with the first preset time；If advance/
Backoff time is less than the first preset time, then judges whether the underwater robot encounters side wall based on the status information, if
It encounters, then controls the underwater robot is forward/backward to climb wall；Obtain it is current it is forward/backward climbs the wall time, by it is described advance/
Retrogressing is climbed the wall time and is compared with the second preset time；If described, forward/backward to climb the wall time default not less than described second
During the time, then wall under the underwater robot back/forward is controlled.
Further, when the round-trip number increases to n, 1 is reset to the round-trip number.
Further, it is described to judge whether the underwater robot is horizontality based on the status information, if level
State obtains current round-trip number；If round-trip number is less than n times, control the underwater robot is forward/backward also to wrap
It includes：If round-trip number is not less than n times, zone bit information is obtained, the underwater robot the 5th is controlled based on the flag bit
Secondary steering, after described 5th time turns to, forward/backward and flag bit adds 1.
Further, it is described to judge whether the underwater robot is horizontality based on the status information, if level
State obtains current round-trip number；If round-trip number is less than n times, control the underwater robot is forward/backward also to wrap
It includes：If not horizontality, then control wall under the underwater robot back/forward and round-trip number subtracts 1, perform step and be based on
The status information judges whether the underwater robot completes wall under back/forward, if so, controlling the underwater robot
First back/forward, carries out first/turn to three times, described first/turn to three times after, back/forward again carries out and the first/tri-
The second/tetra- time identical steering of secondary steering direction opposite angles.
Further, the 5th time turn of the flag bit that is based on is specially：The value range of the flag bit for 1≤m≤
4；When the flag bit is equal to 1, right turn during retrogressing, when advance, does not turn to, and the flag bit adds 1；When the flag bit
During equal to 2, left steering during retrogressing, when advance, does not turn to, and the flag bit adds 1；When the flag bit is equal to 3, during retrogressing
It does not turn to, right turn during advance, and the flag bit adds 1；When the flag bit is equal to 4, when retrogressing, does not turn to, and when advance is left
It turns to, and the flag bit adds 1；When the flag bit is more than 4, flag bit is reset to 1.
Further, the first time turns to, turns to for second, third time turns to, the rotational angle of the 4th steering is
45 °, the rotational angle of the 5th steering is 90 °.
Further, if the forward/backward time is less than the first preset time, based on described in status information judgement
Whether underwater robot encounters side wall, if encountering, the underwater robot forward/backward the step of climbing wall is controlled to further include：
If the forward/backward time is not less than the first preset time, performs step and the underwater is judged based on the status information
Whether people completes forward/backward lower wall, if so, control the underwater robot first forward/backward, progress third/primary turn
It is forward/backward again after being turned to, the third/primary, carry out identical with a third/time steering direction opposite angles the
Four/secondary steering.
Further, if the forward/backward time is less than the first preset time, based on described in status information judgement
Whether underwater robot encounters side wall, if encountering, the underwater robot forward/backward the step of climbing wall is controlled to further include：
If the forward/backward time is less than the first preset time, and do not encounter side wall, then control the underwater robot move on/
It retreats.
Further, if it is described it is forward/backward climb the wall time not less than second preset time when, control described in
It is further included the step of wall under underwater robot back/forward：If it is described it is forward/backward climb the wall time less than described second it is default when
Between when, then the underwater robot is controlled to move on/retreat and climbs wall.
Further, wall under wall, retrogressing is climbed in the advance, wall is climbed in retrogressing and the lower wall that advances further includes：It obtains in real time current
Status information；Based on the status information, judge whether the underwater robot current state is to climb wall/lower wall vertically；If
To climb wall/lower wall vertically, then the underwater robot is controlled to continue to climb wall/lower wall；If not climbing wall/lower wall vertically, then judge
The underwater robot current state is around wall state.
Further, when in around wall state：Current status information is obtained in real time；Based on the status information, control
The underwater robot adjusts fuselage positions；Based on the status information, judge whether the underwater robot is successfully adjusted to
Climb wall/lower wall vertically；If being successfully adjusted to climb wall/lower wall vertically, wall under the underwater robot back/forward is controlled, is held
Row step judges whether the underwater robot completes the lower wall that advances based on the status information, if so, control is described underwater
Robot first advances, and carries out third time steering, after the third time turns to, advances again, carries out and third time steering direction phase
The 4th time identical steering of Reverse gantry；If being adjusted to climb wall/lower wall vertically not successfully, the underwater robot is controlled to send out police
Report.
Further, it is further included before described the step of obtaining the current status information of the underwater robot in real time：In advance
The initial value of first preset time, the second preset time, maximum round trip frequency n and flag bit is set.
The present invention also provides a kind of underwater robot trajectory path planning systems to include：State acquisition module, for reality
When obtain the current status information of the underwater robot；Wall control module is climbed, it is underwater for being based on the status information control
Robot is forward/backward to climb wall, and climbs after wall wall under back/forward forward/backward；Course changing control module, for base
In the status information judge the underwater robot whether after the completion of back off wall, if so, controlling the underwater robot first
It retreats, carries out first time steering, after the first time turns to, retreat again, carry out and first time steering direction opposite angles phase
Second same of steering；Judge whether the underwater robot completes the lower wall that advances based on the status information, if so, control
The underwater robot first advances, and carries out third time steering, after the third time turns to, advances again, turn with third time
Four steering identical to direction opposite angles.
Further, the wall control module of climbing includes：Judgment module, for be based on the status information judge it is described underwater
Whether robot encounters side wall, if not encountering, controls the underwater robot forward/backward；Judge described forward/backward
Whether the time is not less than first preset time, if the forward/backward time is less than the first preset time, based on the state
Information judges whether the underwater robot encounters side wall, if encountering, controls the underwater robot is forward/backward to climb wall；
Judge described forward/backward whether to climb the wall time not less than second preset time；Lower wall control module, if for before described
Into/retreat climb the wall time not less than second preset time when, then control wall under the underwater robot back/forward.
The underwater robot of the present invention cleans the wall of swimming pool on its motion path, is believed by obtaining current state
Angle between breath namely acquisition fuselage and X/Y plane, XZ planes, YZ planes, and then control the motion path of machine.Sentence first
It is disconnected current whether in horizontality, advance if in horizontality and climb wall cleaning, after reaching the second preset time, under vertical
Wall by rotating twice after lower wall, adjusts preset distance in machine-wide direction, is retreated again after adjustment and climb wall, it is pre- to reach second
If after the time, advance lower wall, again by rotating twice after lower wall, preset distance is adjusted in machine-wide direction, is completed primary
Round-trip cleaning.Reached by multiple round-trip cleaning and swimming pool cleaned without dead angle, due to its movement locus be it is regular, because
This also avoids the problem of cable winding of underwater robot.
Description of the drawings
Fig. 1 is the flow chart of a kind of underwater robot trajectory path planning method that first embodiment of the invention provides；
Fig. 2 is the flow chart of a kind of underwater robot trajectory path planning method that second embodiment of the invention provides；
Fig. 3 is the flow chart of a kind of underwater robot trajectory path planning method that third embodiment of the invention provides；
Fig. 4 is the flow chart of a kind of underwater robot trajectory path planning method that fourth embodiment of the invention provides；
Fig. 5 is the flow chart of a kind of underwater robot trajectory path planning method that fifth embodiment of the invention provides；
Fig. 6 is that a kind of module relationship of underwater robot trajectory path planning system provided in an embodiment of the present invention is illustrated
Figure；
Fig. 7 is the module relationship schematic diagram provided in an embodiment of the present invention for climbing wall control module.
Reference numeral：100 be state acquisition module, 200 for climb wall control module, 210 be judgment module, 220 be lower wall
Control module, 300 are course changing control module.
Specific embodiment
In order to make the purpose , technical scheme and advantage of the present invention be clearer, with reference to the accompanying drawings and embodiments, it is right
The present invention is further elaborated.It should be appreciated that the specific embodiments described herein are merely illustrative of the present invention, and
It is not used in the restriction present invention.
The embodiment of the invention discloses a kind of underwater robot trajectory path planning methods.Believed by obtaining current state
It ceases, control underwater robot carries out the regular cleaning of first longitudinal transverse direction again, and then underwater robot can be ensured without dead angle
Swimming pool is cleaned.And since it is regular cleaning to swimming pool, it also is able to avoid the electricity of underwater robot
The problem of cable is wound.
Horizontality includes in the present embodiment：Turn to, turn in first time for second, turning to for the third time, turning to for the 4th time,
Horizontality is after 5th steering：The angle of underwater robot and X/Y plane is less than preset level angle, and the present embodiment is preferred
Preset level angle be 30 °；In lower wall, horizontality is, the angle of fuselage and X/Y plane is less than or equal to sets corner in advance
Degree；In upper wall, horizontality is, the angle of fuselage and X/Y plane, which is less than or equal to preset, climbs corner degree.
Referring to Fig. 1, Fig. 1 is a kind of underwater robot trajectory path planning method that first embodiment of the invention provides
Flow chart.
As shown in Figure 1, a kind of underwater robot trajectory path planning method may comprise steps of S100 to S600, with
Lower step is introduced by taking the paths planning method that underwater cleaning robot cleans swimming pool as an example, is directed to swimming pool
The cleaning path of bottom and surrounding wall is planned.
Step S100：The current status information of the underwater robot is obtained in real time.
Specifically, angle of the status information between fuselage and X/Y plane, XZ planes, YZ planes.Wherein, XY is put down
Face, XZ planes, YZ planes for X-axis, Y-axis and Z axis in space coordinates two-by-two where plane, X/Y plane is X-axis and Y-axis institute
The plane of formation；XZ planes are X-axis and the plane where Z axis；The plane that YZ planes are formed by Y-axis and Z axis, and X/Y plane
It is parallel with swimming pool bottom surface.The device of current state information is wherein obtained as inertial sensor, can be 3-axis acceleration sensor,
The combination of gyroscope or the two.Underwater robot during exercise, the current status information of real-time update.
Status information in the present embodiment includes fuselage and the angle of tri- planes of XY, YZ and XZ.
Step S200：Underwater robot advance is controlled to climb wall based on the status information, and advance climb wall it
Retreat lower wall.
Specifically, described be based on including when the status information controls the underwater robot advance to climb wall：Based on described
Status information judges whether the underwater robot is horizontality, if horizontality, then controls the underwater robot water
It is flat to advance；It obtains present level to advance the time, horizontally advances the time by described and be compared with the first preset time；If before level
It is less than the first preset time into the time, then judges whether the underwater robot encounters side wall based on the status information, if meets
To side wall, then the underwater robot advance is controlled to climb wall according to status information；It obtains current advance and climbs the wall time, before described
It is compared into the wall time is climbed with the second preset time；If the advance climbs the wall time not less than second preset time,
The underwater robot is then controlled to retreat lower wall.
During robot horizontally advances under water, when robot encounters side wall, before continuing level due to underwater robot
Into fuselage can tilt, and fuselage gradually increases relative to the angle of X/Y plane until perpendicular to X/Y plane.Correspondingly, this implementation
Judge whether the underwater robot encounters side wall and include based on the status information in example：Judge the angle of fuselage and X/Y plane
Whether 60 ° are reached, if angle reaches 60 °, then it is assumed that encounter side wall.Otherwise it is assumed that side wall is encountered.
When the angle between underwater robot and X/Y plane reaches certain value, then underwater robot advance is controlled to climb wall.
Specifically, climb corner degree and compare the angle of underwater robot and X/Y plane with default, when more than it is default climb corner and spend when,
Then think that underwater robot encounters side wall, and underwater robot advance is controlled to climb wall, preferred default corner degree of climbing is 60 °, i.e.,
When with the angle of X/Y plane be more than 60 ° when control underwater robot advance climb wall.When wall operation second is climbed in underwater robot advance
After preset time, lower wall is retreated.Wherein the second preset time is the speed moved according to swimming pool height and underwater robot, in advance
The second preset time first set.Namely when underwater robot runs the second preset time in vertical direction with fixed speed
Afterwards, the distance advanced is slightly larger than the height of swimming pool.
Step S300：Based on the status information judge the underwater robot whether after the completion of back off wall, if so, control
It makes the underwater robot first to retreat, carries out first time steering, after the first time turns to, retreat again, carry out and first time
Second identical of steering of steering direction opposite angles.
Specifically, based on the status information judge the underwater robot whether after the completion of wall of backing off be：Judge current
Whether in horizontality, the angle of underwater robot and X/Y plane is compared with setting corner degree in advance, when less than default
Then it is horizontality when lower corner is spent, preferably sets corner degree in advance as 30 °, i.e., sentences when being less than 30 ° with the angle of X/Y plane
Disconnected underwater robot completes lower wall.Also just illustrate that underwater robot descends wall completely.When descending wall completely, then retreat, when after
When moving back third preset time, first time steering is carried out, third preset time is retreated again after turning to for the first time, carries out second later
Secondary steering.When judging to be not at horizontality, then continue to retreat lower wall.Wherein, it turns to and turns with what is turned to for the second time for the first time
Dynamic direction in machine-wide direction by underwater robot by turning to twice on the contrary, namely adjust preset distance, and described
The angle that first time turns to steering for the second time is rotated is identical, i.e., underwater robot still edge turns forward twice after turning to twice
Direction retreats, and it is 45 ° preferably to turn to the rotational angle turned to second for the first time.Wherein third preset time is according to underwater
The movement velocity of robot and the length of fuselage are pre-set, and third preset time can ensure the water after turning to twice
Lower robot can adjust preset distance in the direction of machine-wide.
Step S400：Underwater robot retrogressing is controlled to climb wall based on the status information, and retrogressing climb wall it
Advance lower wall afterwards.
Specifically, after underwater robot is adjusted preset distance in machine-wide direction by step S300, under water
The artificial horizontality of machine, i.e. angle between underwater robot and X/Y plane are less than preset level angle, at this time underwater
People's level retreats.
Specifically, the underwater robot retrogressing is controlled to climb wall, and after wall is climbed in retrogressing based on the status information
The lower wall that advances includes：Present level backoff time is obtained, the horizontal backoff time is compared with the first preset time；If
Horizontal backoff time is less than the first preset time, then judges whether the underwater robot encounters side based on the status information
Wall if encountering side wall, controls the underwater robot retrogressing to climb wall according to status information；It obtains present back and climbs the wall time,
The retrogressing climbed the wall time to be compared with the second preset time；If it is default not less than described second that the wall time is climbed in the retrogressing
During the time, then the lower wall of underwater robot advance is controlled.
Under water in the horizontal fallback procedures of robot, underwater robot level retreat, fuselage can tilt make fuselage relative to
The angle of X/Y plane gradually increases.Correspondingly, whether the underwater robot is judged based on the status information in the present embodiment
Side wall is encountered to include：Judge whether the angle of fuselage and X/Y plane reaches 60 °, if angle reaches 60 °, then it is assumed that encounter side wall.
Otherwise it is assumed that side wall is encountered.
When the angle between underwater robot and X/Y plane reaches certain value, then it is assumed that underwater robot encounters side wall simultaneously
Wall is climbed in control underwater robot retrogressing.Specifically, the angle of underwater robot and X/Y plane and default corner degree of climbing are carried out pair
Than, when more than it is default climb corner and spend when, then it is assumed that underwater robot encounters side wall and underwater robot retrogressing is controlled to climb wall, preferably
Default corner degree of climbing for 60 °, i.e., when being more than 60 ° with the angle of X/Y plane control underwater robot retrogressing climb wall.When underwater
After wall the second preset time of operation is climbed in robot retrogressing, advance lower wall.Wherein the second preset time be according to swimming pool height and
The speed of underwater robot movement, pre-set second preset time.Namely when underwater robot in vertical direction with solid
After fixed speed runs the second preset time, the distance advanced is slightly larger than the height of swimming pool.
Step S500：Judge whether the underwater robot completes the lower wall that advances based on the status information, if so, control
It makes the underwater robot first to advance, carries out third time steering, after the third time turns to, advance again, carry out and third time
The 4th time identical steering of steering direction opposite angles.
During wall under advance, underwater robot gradually leaves side wall and is contacted with swimming pool bottom surface, in this process under water
The fuselage of robot is gradually reduced relative to the angle of X/Y plane, and is horizontally advanced after horizontality is entered.Specifically, it is based on
The status information judges that the underwater robot lower wall of whether completing to advance is：Judge currently whether in horizontality, it will
The angle of underwater robot and X/Y plane is set corner degree and is compared with pre-, when less than it is pre- set corner and spend when, then be level
State preferably sets corner degree as 30 ° in advance, i.e., judges the completion of people's underwater robot when being less than 30 ° with the angle of X/Y plane
Lower wall.When descending wall completely, then advance, when horizontally advancing third preset time, carry out third time steering, turn to it for the third time
After readvance third preset time, carry out the 4th steering later.When judging to be not at horizontality, then move on down
Wall.Wherein, third time turns to the rotation direction with the 4th steering on the contrary, namely underwater robot exists by turning to twice
Machine-wide direction adjusts preset distance, and third time steering is identical with the 4th the rotated angle of steering, i.e., and two
For underwater robot still along forward direction advance is turned twice, preferred third time turns to the rotation with the 4th steering after secondary steering
Angle is 45 °.Wherein third preset time is pre-set according to the movement velocity of underwater robot and the length of fuselage,
Third preset time can ensure that underwater robot can adjust preset distance in the direction of machine-wide after turning to twice.
Step S600：Above step is repeated, and round-trip number is added 1.
Specifically, step S100 is performed again after fuselage preset distance has been adjusted, so as to ensure underwater robot energy
It is enough that the cleaning without dead angle is carried out to swimming pool.When performing step S100 again, round-trip number is added 1, is working as front for counting
To the round-trip wash number of underwater robot, when round-trip number increases to n, it is 1 to reset round-trip number.Wherein n is sets in advance
The maximum value of the round-trip number of statistics put.
Referring to Fig. 2, Fig. 2 is a kind of underwater robot trajectory path planning method that second embodiment of the invention provides
Flow chart.Second embodiment of the invention is the specific optimization to step S200 in first embodiment, is identical with the first embodiment
Step repeats no more in the present embodiment.
As shown in Fig. 2, second embodiment of the invention may comprise steps of S210 to step S250.
Step S210：Judge whether the underwater robot is horizontality based on the status information, if horizontal
State obtains current round-trip number；If round-trip number is less than n times, the underwater robot is controlled to advance.
Specifically, judge whether the underwater robot is horizontality based on the status information：It obtains in real time current
Status information, the angle based on underwater robot in current state information and X/Y plane judges whether current underwater robot is located
In horizontality, if the angle of current underwater robot and X/Y plane is less than preset level angle, underwater is artificially horizontal
State, control underwater robot advance, and preferred preset level angle is 30 °.If round-trip number is not less than n times, mark is obtained
Will position information controls the 5th steering of underwater robot based on the flag bit, after described 5th time turns to, advances and mark
Will position adds 1.If not horizontality, then control wall under the underwater robot back/forward and round-trip number subtracts 1, perform step
Rapid S300.Specifically, the value range of the flag bit is 1≤m≤4；When the flag bit is equal to 1, right turn during retrogressing,
It is not turned to during advance, and the flag bit adds 1；When the flag bit is equal to 2, left steering during retrogressing, when advance, does not turn to, and
The flag bit adds 1；When the flag bit is equal to 3, when retrogressing, does not turn to, right turn during advance, and the flag bit adds 1；
When the flag bit is equal to 4, when retrogressing, does not turn to, left steering during advance, and the flag bit adds 1；When the flag bit is big
When 4, flag bit is reset to 1.It is 90 ° that 5th time, which turns to preferred rotational angle,.It the value range of flag bit and takes not
The operation rules being carried out at the same time can adjust.
Step S220：The current advance time is obtained, the advance time is compared with the first preset time.
Specifically, the current advance time is：Underwater robot detects that starting forward time point for horizontality arrives
The duration of current point in time.The current time of advancing is compared with pre-set first preset time.Wherein first is default
Time is pre-set first preset time of length and width and underwater robot movement velocity according to swimming pool, based on institute
It can be that underwater robot selects optimal cleaning path to state the first preset time.
Step S230：If the time of advancing is less than the first preset time, the underwater machine is judged based on the status information
Whether device people encounters side wall, if encountering, the underwater robot advance is controlled to climb wall.
Specifically, the current advance time is compared with the first preset time, and based on the status information
Judge whether the underwater robot encounters side wall.Wherein encountering side wall is：During robot is forward/backward under water, water
Lower robot level is forward/backward, and fuselage can tilt, and fuselage gradually increases relative to the angle of X/Y plane.Correspondingly, this implementation
Judge whether the underwater robot encounters side wall and include based on the status information in example：Judge the angle of fuselage and X/Y plane
Whether 60 ° are reached, if angle reaches 60 °, then it is assumed that encounter side wall.Otherwise it is assumed that side wall is encountered.When advancing, the time is less than the
One preset time, and side wall is encountered, then it moves on；When advancing, the time is less than the first preset time, and encounters side wall, then before
Into climbing wall；When advancing, the time is not less than the first preset time, then performs step S300.
Step S240：It obtains current advance and climbs the wall time, the wall time is climbed into the advance and is compared with the second preset time
Compared with.
Specifically, the wall time is climbed in current advance is：It encounters side wall and starts to advance and climb time point of wall to current point in time
Duration.It current will advance and climb the wall time and compared with pre-set second preset time.Wherein the second preset time is
According to the speed that swimming pool height and underwater robot move, pre-set second preset time.Namely work as underwater
For people after vertical direction runs the second preset time with fixed speed, the distance advanced is no more than the height of swimming pool.
Step S250：If the advance climbs the wall time not less than second preset time, the underwater machine is controlled
Device people retreats lower wall.
Compared specifically, the current advance climbed the wall time with the second preset time, when it is described advance climb wall when
Between when being not less than second preset time, then retreat lower wall；When the advance climbs the wall time less than second preset time
When, then it moves on and climbs wall.
Referring to Fig. 3, Fig. 3 is a kind of underwater robot trajectory path planning method that third embodiment of the invention provides
Flow chart.Third embodiment of the invention is specific optimization to step S400 in first embodiment and second embodiment, with the
One embodiment and the identical step of second embodiment, repeat no more in the present embodiment.
As shown in figure 3, third embodiment of the invention may comprise steps of S410 to step S450.
Step S410：Judge whether the underwater robot is horizontality based on the status information, if horizontal
State obtains current round-trip number；If round-trip number is less than n times, the underwater robot is controlled to retreat.
Specifically, judge whether the underwater robot is horizontality based on the status information：It obtains in real time current
Status information, the angle based on underwater robot in current state information and X/Y plane judges whether current underwater robot is located
In horizontality, if the angle of current underwater robot and X/Y plane is less than preset level angle, underwater is artificially horizontal
State, control underwater robot retreat, and preferred preset level angle is 30 °.If round-trip number is not less than n times, mark is obtained
Will position information controls the 5th steering of underwater robot based on the flag bit, after described 5th time turns to, advances and mark
Will position adds 1.If not horizontality, then control wall under the underwater robot back/forward and round-trip number subtracts 1, perform step
Rapid S500.Specifically, the value range of the flag bit is 1≤m≤4；When the flag bit is equal to 1, right turn during retrogressing,
It is not turned to during advance, and the flag bit adds 1；When the flag bit is equal to 2, left steering during retrogressing, when advance, does not turn to, and
The flag bit adds 1；When the flag bit is equal to 3, when retrogressing, does not turn to, right turn during advance, and the flag bit adds 1；
When the flag bit is equal to 4, when retrogressing, does not turn to, left steering during advance, and the flag bit adds 1；When the flag bit is big
When 4, flag bit is reset to 1.It is 90 ° that 5th time, which turns to preferred rotational angle,.
Step S420：The present back time is obtained, the backoff time and the first preset time are compared.
Specifically, current backoff time is：Underwater robot detects that the time point for starting to retreat for horizontality arrives
The duration of current point in time.Present back time and pre-set first preset time are compared.Wherein first is default
Time is pre-set first preset time of length and width and underwater robot movement velocity according to swimming pool, based on institute
It can be that underwater robot selects optimal cleaning path to state the first preset time.
Step S430：If backoff time is less than the first preset time, the underwater machine is judged based on the status information
Whether device people encounters side wall, if encountering, the underwater robot retrogressing is controlled to climb wall.
Specifically, the present back time is compared with the first preset time, and based on the status information
Judge whether the underwater robot encounters side wall.Wherein encountering side wall is：During robot is forward/backward under water, water
Lower robot level is forward/backward, and fuselage can tilt, and fuselage gradually increases relative to the angle of X/Y plane.Correspondingly, this implementation
Judge whether the underwater robot encounters side wall and include based on the status information in example：Judge the angle of fuselage and X/Y plane
Whether 60 ° are reached, if angle reaches 60 °, then it is assumed that encounter side wall.Otherwise it is assumed that side wall is encountered.When backoff time is less than the
One preset time, and side wall is encountered, then it moves on；When backoff time is less than the first preset time, and encounter side wall, then before
Into climbing wall；When advancing, the time is not less than the first preset time, then performs step S500.
Step S440：It obtains present back and climbs the wall time, the wall time is climbed into the retrogressing and is compared with the second preset time
Compared with.
Specifically, the wall time is climbed in current retrogressing is：It encounters side wall and starts to retreat the time point for climbing wall to current point in time
Duration.Present back climbed the wall time to compare with pre-set second preset time.Wherein the second preset time is
According to the speed that swimming pool height and underwater robot move, pre-set second preset time.Namely work as underwater
For people after vertical direction runs the second preset time with fixed speed, the distance advanced is no more than the height of swimming pool.
Step S450：If the retrogressing climbs the wall time not less than second preset time, the underwater machine is controlled
Wall under device people advances.
It is compared specifically, the present back climbed the wall time with the second preset time, when wall is climbed in the retrogressing
Between when being not less than second preset time, then advance lower wall；When the retrogressing climbs the wall time less than second preset time
When, then it moves on and climbs wall.
Referring to Fig. 4, Fig. 4 is a kind of underwater robot trajectory path planning method that fourth embodiment of the invention provides
Flow chart.Fourth embodiment of the invention climbs wall tool under wall, advance for wall, retrogressing under advance in above-described embodiment is climbed wall, retreated
Body optimizes.
As shown in figure 4, fourth embodiment of the invention may comprise steps of S431 to step S434.
Step S431：Current status information is obtained in real time.
Specifically, it obtains when forebody and the angle information between X/Y plane, XZ planes, YZ planes.
Step S432：Based on the status information, judge the underwater robot current state whether be climb vertically wall/under
Wall.
Specifically, judging whether current state is to climb wall/lower wall vertically, that is, judge the angle when fuselage and X/Y plane
When being 90 °, fuselage whether there is angle with YZ planes.
Step S433：If wall/lower wall is climbed vertically, then the underwater robot is controlled to continue to climb wall/lower wall.
If specifically, when the angle of fuselage and X/Y plane is 90 °, there is no angles with YZ planes for fuselage, then continue
Climb wall/lower wall.
Step S434：If not climbing wall/lower wall vertically, then it is around wall state to judge the underwater robot current state.
If specifically, when the angle of fuselage and X/Y plane is 90 °, fuselage and YZ planes are there are angle, and angle
When the number of degrees are more than 25 °, then it is around wall state to judge current state.
Referring to Fig. 5, Fig. 5 is a kind of underwater robot trajectory path planning method that fifth embodiment of the invention provides
Flow chart.Fifth embodiment of the invention is around the specific optimization of wall state to judgement in above-described embodiment.
As shown in figure 5, fifth embodiment of the invention may comprise steps of S641 to step S645.
Step S641：Current status information is obtained in real time.
Specifically, it obtains when the angle information between forebody and X/Y plane, XZ planes, YZ planes.
Step S642：Based on the status information, the underwater robot adjustment fuselage positions are controlled.
Specifically, when around wall state, the angle of fuselage and X/Y plane is 90 °, and there are angles with YZ planes for fuselage.Its
Middle adjustment fuselage positions are, by the rotating speed of Motor Control underwater robot the right and left wheel, to form it into speed difference.When
Fuselage then controls underwater robot left-hand wheel in the left side of YZ planes, and when forming angle and angle with YZ planes and being more than 25 °
Rotating speed is more than right-hand wheel rotating speed, so as to be adjusted to zero with the angle of YZ planes；When fuselage is on the right side of YZ planes, and and YZ
When plane forms angle and angle and is more than 25 °, then underwater robot right-hand wheel rotating speed is controlled to be more than left-hand wheel rotating speed, so as to
Zero will be adjusted to the angle of YZ planes.So as to achieve the purpose that adjust fuselage positions.
Step S643：Based on the status information, judge the underwater robot whether be successfully adjusted to climb vertically wall/under
Wall.
Specifically, when adjusting completion, obtained again when forebody and the folder between X/Y plane, XZ planes, YZ planes
Angle information.Judge whether successfully to be adjusted to climb wall/lower wall vertically, that is, judge when the angle of fuselage and X/Y plane is 90 °,
Fuselage whether there is angle with YZ planes.
Step S644：If being successfully adjusted to climb wall/lower wall vertically, wall under the underwater robot back/forward is controlled,
Perform step S500.
If specifically, when the angle of forebody and X/Y plane is 90 °, angle is not present with YZ planes in fuselage,
When work(is adjusted to climb wall/lower wall vertically, then wall under back/forward, performs step S500.
Step S645：If being adjusted to climb wall/lower wall vertically not successfully, the underwater robot is controlled to send out alarm.
Specifically, when the angle of forebody and X/Y plane is 90 °, there are angles with YZ planes for fuselage, i.e., failed to adjust
Whole is to climb wall/lower wall vertically, then sends out alarm.It can be alarm music and/or light flash etc. wherein to send out alarm.Specifically
For buzzer and LED alarm lamps.
In the above-described embodiments, further include pre-set the first preset time, the second preset time, third preset time,
The initial value of maximum round trip frequency n and flag bit.
Fig. 6-7 is please referred to, Fig. 6 is a kind of mould of underwater robot trajectory path planning system provided in an embodiment of the present invention
Block relation schematic diagram；Fig. 7 is the module relationship schematic diagram provided in an embodiment of the present invention for climbing wall control module.
As shown in fig. 6-7, a kind of underwater robot trajectory path planning system includes：State acquisition module 100, for reality
When obtain the current status information of the underwater robot；Wall control module 200 is climbed, for being based on the status information control water
Lower robot is forward/backward to climb wall, and climbs after wall wall under back/forward forward/backward；Course changing control module 300 is used
In judged based on the status information underwater robot whether after the completion of back off wall, if so, controlling the underwater
People first retreats, and carries out first time steering, after the first time turns to, retreats again, carries out and first time steering direction phase dihedral
Spend second identical of steering；Judge whether the underwater robot completes the lower wall that advances based on the status information, if so,
The underwater robot is controlled first to advance, third time steering is carried out, after the third time turns to, advances again, progress and third
The 4th time identical steering of secondary steering direction opposite angles.
Specifically, the status information that state acquisition module 100 obtains is fuselage and X/Y plane, XZ planes, YZ planes
Between angle.Wherein, X/Y plane, XZ planes, YZ planes for X-axis, Y-axis and Z axis in space coordinates two-by-two where it is flat
Face, the plane that X/Y plane is formed by X-axis and Y-axis；XZ planes are X-axis and the plane where Z axis；YZ planes are Y-axis and Z axis institute
The plane of formation.And X/Y plane is parallel with horizontal plane.Wherein state acquisition module 100 is inertial sensor, can be three axis
The combination of acceleration transducer, gyroscope or the two.Underwater robot during exercise, the current status information of real-time update.
Wall control module 200 is climbed for during robot horizontally advances under water, underwater robot horizontally advances/after
It moves back, fuselage can tilt, and fuselage gradually increases relative to the angle of X/Y plane.Correspondingly, believed in the present embodiment based on the state
Breath judges whether the underwater robot encounters side wall and include：Judge whether the angle of fuselage and X/Y plane reaches 60 °, if angle
Reach 60 °, then it is assumed that encounter side wall.Otherwise it is assumed that side wall is encountered.When the angle between underwater robot and X/Y plane reaches
During certain value, then control underwater robot is forward/backward to climb wall.If specifically, encountering side wall, the underwater robot is controlled
Forward/backward wall of climbing includes：The angle of underwater robot and X/Y plane is climbed corner degree and compared with default, when more than default
When climbing corner and spending, then underwater robot advance is controlled to climb wall, preferred default corner degree of climbing is 60 °, i.e., when the angle with X/Y plane
Underwater robot is forward/backward climbs wall for control when degree is more than 60 °.When underwater robot is forward/backward, to climb wall operation second default
After time, wall under back/forward.Wherein the second preset time is the speed moved according to swimming pool height and underwater robot,
Pre-set second preset time.Namely when underwater robot is when vertical direction is default with fixed speed operation second
Between after, the distance advanced is slightly larger than the height of swimming pool.
Course changing control module 300 is used to judge whether the underwater robot backs off wall after the completion based on the status information
For：Judge currently whether be in horizontality, the angle of underwater robot and X/Y plane compared with setting corner degree in advance,
When less than it is pre- set corner and spend when, then be horizontality, it is 30 ° preferably to set corner degree in advance, i.e., when the angle with X/Y plane
Judge that underwater robot completes lower wall during less than 30 °.Also just illustrate that underwater robot descends wall completely.When descending wall completely,
It then retreats, when retreating third preset time, carries out first time steering, third preset time is retreated again after turning to for the first time,
It carries out turning to for second later.When judging to be not at horizontality, then continue to retreat lower wall.Wherein, it turns to and for the first time
The rotation direction of secondary steering in machine-wide direction by underwater robot by turning to twice on the contrary, namely adjust pre- spacing
From, and the angle that the first time turns to steering for the second time is rotated is identical, i.e., underwater robot still edge after turning to twice
Turn forward direction retrogressing twice, it is 45 ° preferably to turn to the rotational angle turned to second for the first time.When wherein third is default
Between be according to the movement velocity of underwater robot and the length of fuselage it is pre-set, third preset time can ensure two
Underwater robot can adjust preset distance in the direction of machine-wide after secondary steering.It is additionally operable to sentence based on the status information
The underwater robot that breaks lower wall of whether completing to advance is：Judge currently whether in horizontality, by underwater robot and XY
The angle of plane is set corner degree and is compared with pre-, when less than it is pre- set corner and spend when, then be horizontality, it is preferred default
Lower corner degree be 30 °, i.e., when with the angle of X/Y plane be less than 30 ° when judge people's underwater robot complete descend wall.When descending wall completely
When, then advance, when advance third preset time, third time steering is carried out, when the third that readvances after third time steering is default
Between, the 4th steering is carried out later.When judging to be not at horizontality, then Xia Qiang is moved on.Wherein, third time turn to
The rotation direction of 4th steering is on the contrary, namely predetermined in the adjustment of machine-wide direction by underwater robot by turning to twice
Distance, and the third time turns to identical with turning to rotated angle for the 4th time, i.e., and underwater robot is still after steering twice
Along forward direction advance is turned twice, it is 45 ° that preferred third time, which is turned to the rotational angle of the 4th steering,.Wherein third is pre-
If the time is pre-set according to the movement velocity of underwater robot and the length of fuselage, third preset time can ensure
Underwater robot can adjust preset distance in the direction of machine-wide after turning to twice.
Preferably, wall control module 200 is climbed to include：Judgment module 210 judges the water for being based on the status information
Whether lower robot encounters side wall, if not encountering, controls the underwater robot forward/backward；Judge it is described advance/after
Move back whether the time is not less than first preset time, if the forward/backward time is less than the first preset time, based on the shape
State information judges whether the underwater robot encounters side wall, if encountering, controls the underwater robot is forward/backward to climb
Wall；Judge described forward/backward whether to climb the wall time not less than second preset time；Lower wall control module 220, if for
It is described it is forward/backward climb the wall time not less than second preset time when, then control under the underwater robot back/forward
Wall.
Judgment module 210 is used to judge whether the underwater robot is horizontality based on the status information：In real time
Current status information is obtained, the angle based on underwater robot in current state information and X/Y plane judges current underwater
Whether people is in horizontality, if the angle of current underwater robot and X/Y plane is less than preset level angle, underwater
Artificial horizontality, control underwater robot advance, and preferred preset level angle is 30 °.
Judgment module 210 is additionally operable to compare the current time of advancing with pre-set first preset time.Wherein
When first preset time is according to the length and width of swimming pool and default underwater robot movement velocity pre-set first
Between, can be that underwater robot selects optimal cleaning path based on first preset time.By the current advance time
It is compared with the first preset time, and judges whether the underwater robot encounters side wall based on the status information.Its
In encounter side wall and be：During robot is forward/backward under water, underwater robot is horizontally advanced/is retreated, and fuselage can tilt,
Fuselage gradually increases relative to the angle of X/Y plane.Correspondingly, judged in the present embodiment based on the status information described underwater
Whether robot, which encounters side wall, includes：Judge whether the angle of fuselage and X/Y plane reaches 60 °, if angle reaches 60 °, then it is assumed that
Encounter side wall.Otherwise it is assumed that side wall is encountered.When advancing, the time is less than the first preset time, and side wall is encountered, then before continuing
Into；When advancing, the time is less than the first preset time, and encounter side wall, then advances and climb wall；It is default not less than first when the time of advancing
Time then performs step S300.It current will advance and climb the wall time and compared with pre-set second preset time.Wherein
Two preset times are the speed moved according to swimming pool height and underwater robot, pre-set second preset time.Also
It is when underwater robot is after vertical direction runs the second preset time with fixed speed, the distance advanced is no more than swimming pool
Height.
Lower wall control module 220 be used for when it is described it is forward/backward climb the wall time not less than second preset time when, then
Control wall under the underwater robot back/forward.
It should be noted that in the trajectory path planning method of the present invention, if meeting any one in following three conditions
It is a, think underwater robot whether in horizontality：
(a) after steering, the angle of fuselage and X/Y plane is less than horizontal sextant angle, and horizontal sextant angle is in the present embodiment
30°；
(b) when under by side wall to swimming pool bottom surface, the angle of fuselage and X/Y plane is less than lower corner degree, in the present embodiment under
Corner degree is 30 °；
(c) when on by swimming pool bottom surface to side wall, the angle of fuselage and X/Y plane, which is less than, climbs corner degree, is climbed in the present embodiment
Corner degree is 60 °.
Horizontal sextant angle, lower corner Du Heshang corners degree value between independently of each other, it is mutually incoherent, when practicing,
Can specific implementation be adjusted according to application demand on the basis of the judgment principle.
The underwater robot of the present invention cleans the wall of swimming pool on its motion path, is believed by obtaining current state
Angle between breath namely acquisition fuselage and X/Y plane, XZ planes, YZ planes, and then control the motion path of machine.Sentence first
It is disconnected current whether in horizontality, advance if in horizontality and climb wall cleaning, after reaching the second preset time, under vertical
Wall by rotating twice after lower wall, adjusts preset distance in machine-wide direction, is retreated again after adjustment and climb wall, it is pre- to reach second
If after the time, advance lower wall, again by rotating twice after lower wall, preset distance is adjusted in machine-wide direction, is completed primary
Round-trip cleaning.Reached by multiple round-trip cleaning and swimming pool cleaned without dead angle, due to its movement locus be it is regular, because
This also avoids the problem of cable winding of underwater robot.
Each technical characteristic of embodiment described above can be combined arbitrarily, to make description succinct, not to above-mentioned reality
It applies all possible combination of each technical characteristic in example to be all described, as long as however, the combination of these technical characteristics is not deposited
In contradiction, it is all considered to be the range of this specification record.
Embodiment described above only expresses the several embodiments of the present invention, and description is more specific and detailed, but simultaneously
It cannot therefore be construed as limiting the scope of the patent.It should be pointed out that those of ordinary skill in the art are come
It says, without departing from the inventive concept of the premise, various modifications and improvements can be made, these belong to the protection of the present invention
Range.Therefore, the protection domain of patent of the present invention should be determined by the appended claims.
Claims (15)
- A kind of 1. underwater robot trajectory path planning method, which is characterized in that including step：The current status information of the underwater robot is obtained in real time；Underwater robot advance is controlled to climb wall based on the status information, and wall under being retreated after wall is climbed in advance；Based on the status information judge the underwater robot whether after the completion of back off wall, if so, the control underwater machine Device people first retreats, and carries out first time steering, after the first time turns to, retreats again, carries out opposite with first time steering direction Second identical of steering of angle；Underwater robot retrogressing is controlled to climb wall based on the status information, and wall under advancing after wall is climbed in retrogressing；Judge whether the underwater robot completes the lower wall that advances based on the status information, if so, the control underwater machine Device people first advances, and carries out third time steering, after the third time turns to, advances again, carries out opposite with third time steering direction The 4th time identical steering of angle；Above step is repeated, and round-trip number is added 1.
- 2. according to the method described in claim 1, it is characterized in that, described control the underwater based on the status information People, which advances, climbs wall, and controls the underwater machine under retrogressing after wall is climbed in advance the step of wall and based on the status information Device people, which retreats, climbs wall, and is specially in the step of wall under advancing after wall is climbed in retrogressing：Judge whether the underwater robot is horizontality based on the status information, if horizontality, obtain current Round-trip number；If round-trip number is less than n times, control the underwater robot forward/backward；The current forward/backward time is obtained, the forward/backward time is compared with the first preset time；If the forward/backward time is less than the first preset time, whether the underwater robot is judged based on the status information Side wall is encountered, if encountering, controls the underwater robot is forward/backward to climb wall；Obtain it is current it is forward/backward climbs the wall time, forward/backward climb the wall time by described and be compared with the second preset time；If described forward/backward when climbing the wall time not less than second preset time, control the underwater robot retreat/ Advance lower wall.
- 3. according to the method described in claim 1, it is characterized in that, when the round-trip number increases to n, to the round-trip number Reset to 1.
- 4. according to the method described in claim 3, it is characterized in that, described judge the underwater based on the status information Whether people is horizontality, if horizontality, obtains current round-trip number；If round-trip number is less than n times, institute is controlled State that underwater robot is forward/backward to be further included：If round-trip number is not less than n times, zone bit information is obtained, the underwater robot the 5th is controlled based on the flag bit Secondary steering, after described 5th time turns to, forward/backward and flag bit adds 1.
- 5. according to the method described in claim 4, it is characterized in that, described judge the underwater based on the status information Whether people is horizontality, if horizontality, obtains current round-trip number；If round-trip number is less than n times, institute is controlled State that underwater robot is forward/backward to be further included：If not horizontality, then control wall under the underwater robot back/forward and round-trip number subtracts 1, perform step base Judge whether the underwater robot completes wall under back/forward in the status information, if so, controlling the underwater People elder generation back/forward, carries out first/turn to three times, described first/turn to three times after, back/forward again, carry out with first/ The second/tetra- time identical steering of steering direction opposite angles three times.
- 6. according to the method described in claim 4, it is characterized in that, the 5th time turn of the flag bit that is based on is specially：The value range of the flag bit is 1≤m≤4；When the flag bit is equal to 1, right turn during retrogressing, when advance, does not turn to, and the flag bit adds 1；When the flag bit is equal to 2, left steering during retrogressing, when advance, does not turn to, and the flag bit adds 1；When the flag bit is equal to 3, when retrogressing, does not turn to, right turn during advance, and the flag bit adds 1；When the flag bit is equal to 4, when retrogressing, does not turn to, left steering during advance, and the flag bit adds 1；When the flag bit is more than 4, flag bit is reset to 1.
- 7. according to the method described in claim 4, it is characterized in that,The first time turns to, turns to for second, third time turns to, the rotational angle of the 4th steering is 45 °, described 5th time The rotational angle of steering is 90 °.
- If 8. according to the method described in claim 2, it is characterized in that, the forward/backward time be less than the first preset time, Then judge whether the underwater robot encounters side wall based on the status information, if encountering, control the underwater robot Forward/backward the step of climbing wall, further includes：If the forward/backward time be not less than the first preset time, perform step be based on the status information judge it is described under water Whether robot completes forward/backward lower wall, if so, controlling the underwater robot first forward/backward, carries out third/mono- Secondary steering, it is forward/backward again after the third/primary steering, it carries out identical with a third/time steering direction opposite angles The 4th/secondary steering.
- If 9. according to the method described in claim 2, it is characterized in that, the forward/backward time be less than the first preset time, Then judge whether the underwater robot encounters side wall based on the status information, if encountering, control the underwater robot Forward/backward the step of climbing wall, further includes：If the forward/backward time is less than the first preset time, and does not encounter side wall, then before the underwater robot is controlled to continue Into/retreat.
- 10. if according to the method described in claim 2, it is characterized in that, described forward/backward climb the wall time not less than institute When stating the second preset time, then the step of controlling wall under the underwater robot back/forward, further includes：If described forward/backward when climbing the wall time less than second preset time, before the underwater robot is controlled to continue Into/retreat and climb wall.
- 11. according to the method described in claim 2, it is characterized in that, the advance climb wall, retreat lower wall, retreat climb wall and The lower wall that advances further includes：Current status information is obtained in real time；Based on the status information, judge whether the underwater robot current state is to climb wall/lower wall vertically；If wall/lower wall is climbed vertically, then the underwater robot is controlled to continue to climb wall/lower wall；If not climbing wall/lower wall vertically, then it is around wall state to judge the underwater robot current state.
- 12. according to the method for claim 11, which is characterized in that when in around wall state：Current status information is obtained in real time；Based on the status information, the underwater robot adjustment fuselage positions are controlled；Based on the status information, judge whether the underwater robot is successfully adjusted to climb wall/lower wall vertically；If being successfully adjusted to climb wall/lower wall vertically, wall under the underwater robot back/forward is controlled, step is performed and is based on The status information judges whether the underwater robot completes the lower wall that advances, if so, controlling the underwater robot previous Into progress third time steering after the third time turns to, is advanced, carried out identical with third time steering direction opposite angles again The 4th time steering；If being adjusted to climb wall/lower wall vertically not successfully, the underwater robot is controlled to send out alarm.
- 13. according to the method described in claim 1, it is characterized in that, described obtain the current shape of the underwater robot in real time It is further included before the step of state information：Pre-set the initial value of the first preset time, the second preset time, maximum round trip frequency n and flag bit.
- 14. a kind of underwater robot trajectory path planning system, which is characterized in that including：State acquisition module, for obtaining the current status information of the underwater robot in real time；Climb wall control module, status information control underwater robot is forward/backward to climb wall for being based on, and advance/ Wall under back/forward is climbed after wall in retrogressing；Course changing control module, for be based on the status information judge the underwater robot whether after the completion of back off wall, if so, The underwater robot is then controlled first to retreat, first time steering is carried out, after first time turns to, retreats again, is carried out and the Second identical of steering of steering direction opposite angles；Judge whether the underwater robot is complete based on the status information Into the lower wall that advances, if so, the underwater robot is controlled first to advance, third time steering is carried out, after the third time turns to, then Secondary advance carries out four steering identical with third time steering direction opposite angles.
- 15. system according to claim 14, which is characterized in that the wall control module of climbing includes：Judgment module judges whether the underwater robot encounters side wall for being based on the status information, if not encountering, Control the underwater robot forward/backward；Judge whether the forward/backward time is not less than first preset time, If the forward/backward time is less than the first preset time, judge whether the underwater robot encounters based on the status information Side wall if encountering, controls the underwater robot is forward/backward to climb wall；Judge described forward/backward whether not to climb the wall time Less than second preset time；Lower wall control module, if for it is described it is forward/backward climb the wall time not less than second preset time when, control institute State wall under underwater robot back/forward.
Priority Applications (3)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201711443969.6A CN108189031B (en) | 2017-12-27 | 2017-12-27 | Underwater robot motion path planning method and system |
| PCT/CN2018/115760 WO2019128521A1 (en) | 2017-12-27 | 2018-11-15 | Underwater cleaning robot and crawling method thereof, motion path planning method and system |
| CN201880083268.2A CN111757832B (en) | 2017-12-27 | 2018-11-15 | Underwater cleaning robot, crawling method thereof, and motion path planning method and system |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201711443969.6A CN108189031B (en) | 2017-12-27 | 2017-12-27 | Underwater robot motion path planning method and system |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN108189031A true CN108189031A (en) | 2018-06-22 |
| CN108189031B CN108189031B (en) | 2021-02-19 |
Family
ID=62584509
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201711443969.6A Active CN108189031B (en) | 2017-12-27 | 2017-12-27 | Underwater robot motion path planning method and system |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN108189031B (en) |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2019128521A1 (en) * | 2017-12-27 | 2019-07-04 | 杭州高越科技有限公司 | Underwater cleaning robot and crawling method thereof, motion path planning method and system |
| CN111757832A (en) * | 2017-12-27 | 2020-10-09 | 杭州高越科技有限公司 | Underwater cleaning robot, crawling method thereof, and motion path planning method and system |
| CN112294192A (en) * | 2019-08-01 | 2021-02-02 | 惠州拓邦电气技术有限公司 | Robot control method and robot |
| CN115542922A (en) * | 2022-11-02 | 2022-12-30 | 智橙动力(北京)科技有限公司 | Swimming pool cleaning robot, control method, electronic equipment and storage medium |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN1423025A (en) * | 2001-12-05 | 2003-06-11 | 温馨环保工程技术有限公司 | Self-walking type sweeping device and method thereof |
| CN101481957A (en) * | 2009-02-05 | 2009-07-15 | 付桂兰 | Remote control ordered swimming pool cleaning robot and ordered cleaning method thereof |
| CN201395961Y (en) * | 2009-05-21 | 2010-02-03 | 付桂兰 | Parallel-connection swimming pond automatic cleaning machine |
| CN101833333A (en) * | 2009-12-23 | 2010-09-15 | 湖南大学 | Unknown environment route planning method of underwater cleaning robot |
| EP3199725A1 (en) * | 2016-01-26 | 2017-08-02 | Maytronics Ltd. | Interactive pool cleaning robot |
| EP3249137A1 (en) * | 2016-05-25 | 2017-11-29 | Maytronics Ltd. | Pool cleaner with drive motor navigation capabilities |
-
2017
- 2017-12-27 CN CN201711443969.6A patent/CN108189031B/en active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN1423025A (en) * | 2001-12-05 | 2003-06-11 | 温馨环保工程技术有限公司 | Self-walking type sweeping device and method thereof |
| CN101481957A (en) * | 2009-02-05 | 2009-07-15 | 付桂兰 | Remote control ordered swimming pool cleaning robot and ordered cleaning method thereof |
| CN201395961Y (en) * | 2009-05-21 | 2010-02-03 | 付桂兰 | Parallel-connection swimming pond automatic cleaning machine |
| CN101833333A (en) * | 2009-12-23 | 2010-09-15 | 湖南大学 | Unknown environment route planning method of underwater cleaning robot |
| EP3199725A1 (en) * | 2016-01-26 | 2017-08-02 | Maytronics Ltd. | Interactive pool cleaning robot |
| EP3249137A1 (en) * | 2016-05-25 | 2017-11-29 | Maytronics Ltd. | Pool cleaner with drive motor navigation capabilities |
Cited By (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2019128521A1 (en) * | 2017-12-27 | 2019-07-04 | 杭州高越科技有限公司 | Underwater cleaning robot and crawling method thereof, motion path planning method and system |
| CN111757832A (en) * | 2017-12-27 | 2020-10-09 | 杭州高越科技有限公司 | Underwater cleaning robot, crawling method thereof, and motion path planning method and system |
| CN111757832B (en) * | 2017-12-27 | 2022-07-15 | 杭州高越科技有限公司 | Underwater cleaning robot, crawling method thereof, and motion path planning method and system |
| CN112294192A (en) * | 2019-08-01 | 2021-02-02 | 惠州拓邦电气技术有限公司 | Robot control method and robot |
| CN115542922A (en) * | 2022-11-02 | 2022-12-30 | 智橙动力(北京)科技有限公司 | Swimming pool cleaning robot, control method, electronic equipment and storage medium |
Also Published As
| Publication number | Publication date |
|---|---|
| CN108189031B (en) | 2021-02-19 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108189031A (en) | Underwater robot trajectory path planning method and system | |
| CN109984689B (en) | Cleaning robot and path optimization method thereof | |
| CN106444768B (en) | A kind of the welt traveling method and system of robot | |
| CN106970623A (en) | Intelligent cleaning device and its trellis paths operational method | |
| WO2005035909B1 (en) | Cordless pool cleaning robot | |
| CN108890666A (en) | A kind of robot welding welding seam tracking method and system | |
| CN106527438A (en) | Robot navigation control method and device | |
| CN108784534A (en) | A kind of artificial intelligence cleaning robot | |
| CN112198876B (en) | Map full-coverage sweeping modular control method suitable for sweeping robot | |
| CN112809665A (en) | Mechanical arm motion planning method based on improved RRT algorithm | |
| CN107421538A (en) | Navigation system and air navigation aid | |
| CN111880522A (en) | Novel autonomous assembly robot path planning autonomous navigation system and method | |
| US20240085919A1 (en) | Traversal Method and System, Robot, and Readable Storage Medium | |
| Lensgraf et al. | Droplet: Towards autonomous underwater assembly of modular structures | |
| CN113467445A (en) | Four-legged robot swing leg obstacle avoidance method based on vision and path planning | |
| CN107817720A (en) | A kind of steering gear control system and method | |
| CN111757832B (en) | Underwater cleaning robot, crawling method thereof, and motion path planning method and system | |
| CN116149314A (en) | Robot full-coverage operation method and device and robot | |
| WO2022166397A1 (en) | Method and apparatus for avoiding target object, storage medium and electronic apparatus | |
| CN109834711A (en) | A kind of quadruped robot motion control method, system and robot | |
| CN112932367B (en) | Fixed-point sweeping method of cleaning equipment and cleaning equipment | |
| CN115769994A (en) | Ground maintenance method based on robot and robot | |
| CN113998021A (en) | Bionic search and rescue robot and space self-deployment method | |
| CN114767014A (en) | Automatic planning method and system for cleaning path of platform basin of cleaning robot | |
| CN209091192U (en) | A kind of artificial intelligence cleaning robot |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| CB03 | Change of inventor or designer information |
Inventor after: Zhuang Nan Inventor after: Ying Jiawei Inventor after: Chen Zhanping Inventor after: He Caiyan Inventor before: Ying Jiawei Inventor before: Chen Zhanping Inventor before: He Caiyan |
|
| CB03 | Change of inventor or designer information | ||
| TA01 | Transfer of patent application right |
Effective date of registration: 20201009 Address after: Room 601, No.12, Yiheng Road, Nonglin Shanglu, Yuexiu District, Guangzhou City, Guangdong Province 510030 Applicant after: Zhuang Nan Address before: Yuhang District of Hangzhou city in Zhejiang province 311113 Qixian Village Building 9 Liangzhu Street Bridge (South and North Building 2 layer 1-2 layer) Applicant before: HANGZHOU GAOYUE TECHNOLOGY Co.,Ltd. |
|
| TA01 | Transfer of patent application right | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |