A positioning method and system for underwater robot

Abstract

The embodiment of the present invention discloses a positioning method and system for an underwater robot. The method comprises: obtaining a first reflection signal of a first sound wave signal emitted in front of an underwater robot body; obtaining a second reflection signal of a second sound wave signal emitted below the body; and locating the body and / or objects around the body according to the first reflection signal and the second reflection signal. The embodiment of the present invention can determine obstacles in the directions indicated by the four sonars and lock the underwater position of the underwater robot body, thereby improving the reliability of the underwater robot operating in water areas.

Description

Technical Field

[0001] The embodiments of the present invention relate to the field of robot technology, and in particular to a positioning method and system for an underwater robot. Background Art

[0002] With the increasing demand for underwater exploration and underwater operation reliability, the innovation and advancement of the functional technology of underwater robots as underwater operation devices has become a research hotspot today.

[0003] Most underwater robots use large sonar arrays for obstacle avoidance. However, large sonar arrays are expensive and large in size, and have a single obstacle avoidance function. This will affect the reliability of underwater robots operating in turbulent and complex waters, making it impossible for the robots to automatically adjust their movement posture quickly in turbulent waters when operating underwater, increasing the possibility of underwater robots losing control and colliding in dark waters. Summary of the invention

[0004] The present invention provides a positioning method and system for an underwater robot, which can accurately realize the positioning and environmental positioning of the underwater robot and improve the reliability of the underwater robot in the water area.

[0005] In a first aspect, an embodiment of the present invention provides a positioning method for an underwater robot, the method comprising:

[0006] Acquire a first reflection signal of a first sound wave signal emitted toward the front of the underwater robot body;

[0007] Acquire a second reflection signal of a second sound wave signal emitted toward the bottom of the machine body;

[0008] The machine body and / or objects around the machine body are positioned according to the first reflection signal and the second reflection signal.

[0009] In a second aspect, an embodiment of the present invention further provides a positioning device for an underwater robot, the device comprising:

[0010] A first acquisition unit, used to acquire a first reflection signal of a first sound wave signal emitted toward the front of the underwater robot body;

[0011] A second acquisition unit, used to acquire a second reflection signal of a second sound wave signal emitted toward the bottom of the machine body;

[0012] A positioning unit is used to locate the body and / or objects around the body according to the first reflection signal and the second reflection signal.

[0013] In a third aspect, an embodiment of the present invention further provides a computer device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein when the processor executes the program, a positioning method for an underwater robot as described in any one of the embodiments of the present invention is implemented.

[0014] In a fourth aspect, an embodiment of the present invention further provides a computer-readable storage medium having a computer program stored thereon, wherein the program, when executed by a processor, implements a positioning method for an underwater robot as described in any one of the embodiments of the present invention.

[0015] In a fifth aspect, an embodiment of the present invention further provides a positioning system for an underwater robot, the system comprising:

[0016] A first forward sonar, a second forward sonar, a first downward sonar, a second downward sonar, a first sonar chamber and a second sonar chamber; the first sonar chamber and the second sonar chamber are respectively installed on both sides of the underwater robot body; the first forward sonar and the first downward sonar are placed in the first sonar chamber, and the second forward sonar and the second downward sonar are placed in the second sonar chamber; the first forward sonar and the second forward sonar are placed in a direction parallel to the body direction, and their heads face the front of the body; the first downward sonar and the second downward sonar are placed in a direction perpendicular to the body direction, and their heads face the bottom of the body;

[0017] The first reflection signal of the first sound wave signal emitted by the first forward sonar and the second forward sonar toward the front of the aircraft body is sent to the aircraft body; the second reflection signal of the second sound wave signal emitted by the first downward sonar and the second downward sonar toward the bottom of the aircraft body is sent to the aircraft body, and the first reflection signal and the second reflection signal are used by the aircraft body to locate the aircraft body and / or objects around the aircraft body.

[0018] The embodiment of the present invention uses the first reflection signal of the first sound wave signal emitted in front of the body by two forward sonars and the second reflection signal of the second sound wave signal emitted below the body by two downward sonars to comprehensively calculate the positioning of the body and / or the positioning of objects around the body, thereby accurately realizing the positioning and environmental positioning of the underwater robot. Furthermore, since the positioning and environmental positioning of the underwater robot can be performed quickly and accurately, the movement posture of the underwater robot can be quickly adjusted even when operating in turbulent and complex waters, thereby avoiding the occurrence of abnormal situations such as loss of control during underwater operations. BRIEF DESCRIPTION OF THE DRAWINGS

[0019] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings required for use in the embodiments are briefly introduced below. It should be understood that the following drawings only show certain embodiments of the present invention and therefore should not be regarded as limiting the scope. For ordinary technicians in this field, other related drawings can be obtained based on these drawings without creative work.

[0020] Figure 1 is a flow chart of a positioning method for an underwater robot in Embodiment 1 of the present invention;

[0021] Figure 2 is a flow chart of a positioning method for an underwater robot in Embodiment 2 of the present invention;

[0022] Figure 2A is a schematic diagram of the underwater robot body coordinate system XYZ established in the second embodiment of the present invention;

[0023] Figure 3 is a structural schematic diagram of a positioning device for an underwater robot in Embodiment 3 of the present invention;

[0024] Figure 4 It is a structural schematic diagram of a computer device in Embodiment 4 of the present invention;

[0025] Figure 5 is a structural schematic diagram of a positioning system for an underwater robot in Embodiment 5 of the present invention;

[0026] Figure 6 This is a schematic diagram of a sonar array wave generation system for an underwater robot in Embodiment 5 of the present invention.

[0027] In the figure:

[0028] 1. First forward sonar; 2. Second forward sonar; 3. First downward sonar; 4. Second downward sonar; 5. First sonar chamber; 6. Second sonar chamber; 7. Underwater robot body. DETAILED DESCRIPTION

[0029] The present invention will be further described in detail below in conjunction with the accompanying drawings and embodiments. It is to be understood that the specific embodiments described herein are only used to explain the present invention, rather than to limit the present invention. It should also be noted that, for ease of description, only parts related to the present invention, rather than all structures, are shown in the accompanying drawings.

[0030] Embodiment 1

[0031] Figure 1This is a flow chart of a positioning method for an underwater robot provided in the first embodiment of the present invention. This embodiment can be applied to the situation where the underwater robot senses obstacles and performs intelligent obstacle avoidance maneuvers and locks the underwater position of the underwater robot body when operating underwater. The method can be executed by the positioning device for the underwater robot in the embodiment of the present invention, and the device can be implemented in software and / or hardware, such as Figure 1 As shown, the method specifically comprises the following steps:

[0032] S110: Acquire a first reflection signal of a first sound wave signal emitted toward the front of the underwater robot body.

[0033] Among them, the front of the body refers to the front of the underwater robot body movement direction, and the body movement direction may include the front, back, top, bottom, left and right of the body; the first sound wave signal is the sound wave signal emitted by the first forward sonar and the second forward sonar. The sonar is an electronic device that uses the propagation characteristics of sound waves underwater to complete underwater detection and communication tasks through electroacoustic conversion and information processing, and can realize the emission and reception of sound wave signals; the first reflected signal is the sound wave signal received by the first forward sonar and the second forward sonar. For example, the first sound wave signal emitted by the first forward sonar and the second forward sonar is sent to an obstacle and then reflected back by the obstacle. The reflected sound wave signal is the first reflected signal.

[0034] S120: Acquire a second reflection signal of a second sound wave signal emitted toward the bottom of the machine body.

[0035] Among them, the below the body refers to the direction directly below the movement of the underwater robot body; the second sound wave signal is the sound wave signal emitted by the first downward sonar and the second downward sonar; the second reflected signal is the sound wave signal received by the first downward sonar and the second downward sonar. For example, the second sound wave signal emitted by the first downward sonar and the second downward sonar is sent to an obstacle and then reflected back by the obstacle. The reflected sound wave signal is the second reflected signal.

[0036] S130: Position the machine body and / or objects around the machine body according to the first reflection signal and the second reflection signal.

[0037] Among them, the objects around the body are objects that may exist around and above and below the body, such as obstacles that may exist in front of and below the underwater robot body; the positioning is to measure and determine the position of the object, for example, the position of the obstacle in front of the body can be calculated according to the first reflection signal, the position of the obstacle below the body can be calculated according to the second reflection signal, and the position of the body can be calculated based on the combination of the first reflection signal and the second reflection signal, and the position of the obstacles around the body and the position of the body are determined together.

[0038] The embodiment of the present invention uses the first reflection signal of the first sound wave signal emitted in front of the body by two forward sonars and the second reflection signal of the second sound wave signal emitted below the body by two downward sonars to comprehensively calculate the positioning of the body and / or the positioning of objects around the body, thereby accurately realizing the positioning and environmental positioning of the underwater robot. Furthermore, since the positioning and environmental positioning of the underwater robot can be performed quickly and accurately, the movement posture of the underwater robot can be quickly adjusted even when operating in turbulent and complex waters, thereby avoiding the occurrence of abnormal situations such as loss of control during underwater operations.

[0039] Embodiment 2

[0040] Figure 2 This is a flow chart of a positioning method for an underwater robot in Embodiment 2 of the present invention. This embodiment is a further improvement on the positioning method for an underwater robot in the above embodiment. The method in this embodiment specifically includes the following steps:

[0041] S210, respectively calculating and measuring the distances between four sonars (a first forward sonar, a second forward sonar, a first downward sonar, and a second downward sonar) and an obstacle.

[0042] Among them, the distance measured between the four sonars and the obstacle can be calculated by multiplying the one-way time of the sound wave signal transmission and the transmission speed of the sound wave in the water; the one-way time of the sound wave signal transmission is half of the sum of the time from the sound wave signal emitted by the sonar to the obstacle and the time from the sound wave signal being reflected back until it is received by the sonar; the transmission speed of the sound wave in the water is 1500m / s.

[0043] S220, establishing a body coordinate system XYZ of the underwater robot, wherein the detection centerline directions of the first forward sonar and the second forward sonar are along the X-axis, and the detection centerline directions of the first downward sonar and the second downward sonar are along the Z-axis.

[0044] For example, the underwater robot body can be compared to a human head, where the X-axis represents the front-to-back direction of the human head, the Y-axis represents the left-to-right direction of the human head, and the Z-axis represents the up-down direction of the human head; Figure 2A As shown in the figure, D1 and D2 respectively represent the distances between the first forward sonar and the second forward sonar and the obstacle in front of the body, and D3 and D4 respectively represent the distances between the first downward sonar and the second downward sonar and the obstacle below the body.

[0045] S230, calculating the distance state between the underwater robot body and the obstacle, and obtaining the stable distance of the body in the X-axis and Z-axis directions.

[0046] Among them, the current distance between each sonar of the underwater robot body and the obstacle calculated by step S210 constitutes the current distance state matrix; the body can be compared to a rigid body, which will change during the actual underwater operation, and is not a completely static state, similar to the fixed-point rotation of a rigid body. At this time, it is necessary to actually introduce a body coordinate system change matrix, that is, the three Euler angle changes of the body around the XYZ axis. The three Euler angles are a group of three independent angular parameters used to determine the position of the fixed-point rotation body. The three Euler angle changes will affect the changes in the current measured distance state matrix. The transformation of these Euler angles can be represented by the system state transfer matrix, which is used to transfer the current distance state to the next state. The corresponding angle change data information can be obtained through the posture sensor in the body; when the body is operating underwater, it cannot be guaranteed that it is fixed and it may produce A certain small movement may occur, which may be forward and backward movement, left and right movement, or up and down movement. At this time, this small movement of the body will produce a certain deviation when measuring the distance, and the measured distance deviation can be obtained by the sonar in the body; the measured amount of the distance deviation needs to be estimated, and it can be filtered by different methods. For example, the Kalman filter equation can be introduced in this algorithm process to optimize the measured distance deviation; the distance state between the underwater robot body and the obstacle can be calculated by combining the current distance state matrix, the system state transfer matrix and the measured distance deviation after the optimization process, and then the stable distance of the body in the X-axis and Z-axis directions is obtained to achieve fixed distance and fixed height; the fixed distance refers to the stable distance between the body and the obstacle in front along the X-axis direction; the fixed height refers to the stable distance between the body and the obstacle below along the Z-axis direction.

[0047] S240, respectively calculating and measuring radial velocities between four sonars (a first forward sonar, a second forward sonar, a first downward sonar, and a second downward sonar) and the obstacle.

[0048] Among them, the radial velocity between the sonar and the obstacle can be calculated by the wavelength of the sonar transmitted signal and the Doppler frequency shift; the wavelength of the sonar transmitted signal can be calculated by the transmission speed of sound waves in water and the signal frequency of the sonar transmitted signal; the Doppler frequency shift is the difference between the signal frequency transmitted by the sonar and the signal frequency received by the sonar; the signal frequency transmitted by the sonar is a known parameter of the sonar, which is determined according to the model of the sonar used in the underwater robot, and the signal frequency transmitted by the sonar contacts the obstacle and is reflected back, and the frequency of the reflected sound wave signal will produce a certain loss, so there is a certain difference between the signal frequency transmitted by the sonar and the signal frequency received, which is the Doppler frequency shift.

[0049] S250, converting the radial velocities of the four sonars into velocities in the body coordinate system; and then converting the velocities in the body coordinate system into velocities in the earth coordinate system.

[0050] Among them, through step S240, the radial velocities between the four sonars and the obstacles can be calculated respectively, and then the radial velocities of the four sonars are formed into a column vector matrix, and the velocity in the body coordinate system can be calculated after transposition; the velocity in the body coordinate system is converted into the velocity in the earth coordinate system, and the velocity in the body coordinate system needs to be calculated through a transposed matrix, and the transposed matrix is ​​a matrix obtained by the angle transformation of the Euler angle in the body, and the data information of the transposed matrix and the system state transfer matrix described in the above step S230 are obtained through the same attitude sensor in the body; in the algorithm process, the step S230 and the step S250 are performed simultaneously.

[0051] S260, calculating the speed state between the underwater robot body and the obstacle, and obtaining the stable speed of the body in the Y-axis direction.

[0052] Among them, the speed state between the underwater robot body and the obstacle can be calculated by combining the current distance state matrix, the system state transfer matrix and the measured speed deviation after the optimization process, and then the stable speed of the body in the Y-axis direction is obtained; the current speed state matrix can be formed by the current speed between each sonar of the body and the obstacle calculated by step S250; the system state transfer matrix is ​​used to transfer the current speed state to the next state, which is the same as the system state transfer matrix in step S230; the measured speed deviation also means that when the underwater robot is operating underwater, it cannot be guaranteed to be fixed, and it may produce certain small movements, which may be forward and backward, left and right, or up and down. At this time, this small movement will produce a certain deviation in the measurement speed. The measured distance deviation can be obtained by the sonar in the body. In this algorithm process, the Kalman filter equation is also used to optimize the measurement speed deviation.

[0053] S270: Determine the fixed-point control of the machine in the XYZ three-dimensional space according to the determined stable distances of the machine in the X-axis and Z-axis directions and the stable speed of the machine in the Y-axis direction.

[0054] Among them, the stable distance of the body in the X-axis and Z-axis directions and the stable speed of the body in the Y-axis direction are determined, and the fixed-point control of the body in the XYZ three-dimensional space is determined by combining the stable distance and the stable speed. These three steps are carried out simultaneously in the motion control algorithm process.

[0055] The technical solution of this embodiment, by receiving the reflected sound wave data of four sonars and combining it with the built-in motion algorithm, determines the obstacles in the directions indicated by the four sonars and the position of the underwater robot body, and accurately realizes the positioning and environmental positioning of the underwater robot. Furthermore, since the positioning and environmental positioning of the underwater robot can be performed quickly and accurately, the motion posture of the underwater robot can be quickly adjusted even when operating in turbulent and complex waters, thereby avoiding the occurrence of abnormal situations such as loss of control of underwater operations, and improving the reliability of the underwater robot in turbulent and complex waters.

[0056] Embodiment 3

[0057] Figure 3 This is a schematic diagram of the structure of a positioning device for an underwater robot provided in Embodiment 3 of the present invention. The device can be implemented in software and / or hardware, and can be integrated in any device that provides a positioning function for an underwater robot, such as Figure 3 As shown, the device specifically includes: a first acquisition unit 310, a second acquisition unit 320 and a positioning unit 330.

[0058] The first acquisition unit 310 is used to acquire a first reflection signal of a first sound wave signal emitted to the front of the underwater robot body;

[0059] A second acquisition unit 320 is used to acquire a second reflection signal of a second sound wave signal emitted toward the bottom of the machine body;

[0060] The positioning unit 330 is used to locate the body and / or objects around the body according to the first reflection signal and the second reflection signal.

[0061] Optionally, the positioning unit is specifically used for:

[0062] Locating an obstacle in front of the machine body according to the first reflected signal; and

[0063] An obstacle below the machine body is located according to the second reflected signal.

[0064] Optionally, the positioning unit is further used for:

[0065] The body is positioned according to the first reflection signal and the second reflection signal.

[0066] A positioning device for an underwater robot provided in an embodiment of the present invention can execute the method provided in any embodiment of the present invention, and has functional modules and beneficial effects corresponding to the execution method.

[0067] The embodiment of the present invention uses the first reflection signal of the first sound wave signal emitted in front of the body by two forward sonars and the second reflection signal of the second sound wave signal emitted below the body by two downward sonars to comprehensively calculate the positioning of the body and / or the positioning of objects around the body, thereby accurately realizing the positioning and environmental positioning of the underwater robot. Furthermore, since the positioning and environmental positioning of the underwater robot can be performed quickly and accurately, the movement posture of the underwater robot can be quickly adjusted even when operating in turbulent and complex waters, thereby avoiding the occurrence of abnormal situations such as loss of control during underwater operations.

[0068] Embodiment 4

[0069] Figure 4 This is a schematic diagram of the structure of a computer device in Embodiment 4 of the present invention. Figure 4 A block diagram of an exemplary computer device 12 suitable for use in implementing embodiments of the present invention is shown. Figure 4 The computer device 12 shown is only an example and should not bring any limitation to the functions and scope of use of the embodiments of the present invention.

[0070] like Figure 4As shown, the computer device 12 is in the form of a general-purpose computing device. The components of the computer device 12 may include, but are not limited to: one or more processors or processing units 16, a system memory 28, and a bus 18 that connects various system components (including the system memory 28 and the processing unit 16).

[0071] Bus 18 represents one or more of several types of bus structures, including a memory bus or memory controller, a peripheral bus, an accelerated graphics port, a processor or a local bus using any of a variety of bus architectures. By way of example, these architectures include, but are not limited to, an Industry Standard Architecture (ISA) bus, a Micro Channel Architecture (MAC) bus, an Enhanced ISA bus, a Video Electronics Standards Association (VESA) local bus, and a Peripheral Component Interconnect (PCI) bus.

[0072] The computer device 12 typically includes a variety of computer system readable media. These media can be any available media that can be accessed by the computer device 12, including volatile and non-volatile media, removable and non-removable media.

[0073] The system memory 28 may include computer system readable media in the form of volatile memory, such as random access memory (RAM) 30 and / or cache memory 32. The computer device 12 may further include other removable / non-removable, volatile / non-volatile computer system storage media. By way of example only, the storage system 34 may be used to read and write non-removable, non-volatile magnetic media ( Figure 4 not shown, usually called a "hard drive"). Although Figure 4 Not shown in the figure, a disk drive for reading and writing to a removable non-volatile disk (e.g., a "floppy disk"), and an optical disk drive for reading and writing to a removable non-volatile optical disk (e.g., a CD-ROM, a DVD-ROM, or other optical media) may be provided. In these cases, each drive may be connected to the bus 18 via one or more data medium interfaces. The memory 28 may include at least one program product having a set (e.g., at least one) of program modules that are configured to perform the functions of the various embodiments of the present invention.

[0074] A program / utility 40 having a set (at least one) of program modules 42 may be stored, for example, in the memory 28. Such program modules 42 include, but are not limited to, an operating system, one or more application programs, other program modules, and program data, each of which or some combination may include an implementation of a network environment. The program modules 42 generally perform the functions and / or methods of the embodiments described herein.

[0075] The computer device 12 may also communicate with one or more external devices 14 (e.g., keyboard, pointing device, display 24, etc.), may also communicate with one or more devices that enable a user to interact with the computer device 12, and / or communicate with any device that enables the computer device 12 to communicate with one or more other computing devices (e.g., network card, modem, etc.). Such communication may be performed through an input / output (I / O) interface 22. In addition, the computer device 12 in this embodiment, the display 24 does not exist as an independent individual, but is embedded in the mirror surface, and when the display surface of the display 24 is not displayed, the display surface of the display 24 and the mirror surface are visually integrated. In addition, the computer device 12 may also communicate with one or more networks (e.g., local area network (LAN), wide area network (WAN) and / or public network, such as the Internet) through a network adapter 20. As shown in the figure, the network adapter 20 communicates with other modules of the computer device 12 through a bus 18. It should be understood that although not shown in the figures, other hardware and / or software modules may be used in conjunction with the computer device 12, including but not limited to: microcode, device drivers, redundant processing units, external disk drive arrays, RAID systems, tape drives, and data backup storage systems.

[0076] The processing unit 16 executes various functional applications and data processing by running the program stored in the system memory 28, for example, implementing the positioning method for an underwater robot provided in an embodiment of the present invention: obtaining a first reflection signal of a first sound wave signal emitted in front of the underwater robot body; obtaining a second reflection signal of a second sound wave signal emitted below the body; and positioning the body and / or objects around the body according to the first reflection signal and the second reflection signal.

[0077] Embodiment 5

[0078] Embodiment 5 of the present invention provides a computer-readable storage medium having a computer program stored thereon. When the program is executed by a processor, the positioning method for an underwater robot provided in all the embodiments of the invention of the present application is implemented: obtaining a first reflection signal of a first sound wave signal emitted in front of a body of the underwater robot; obtaining a second reflection signal of a second sound wave signal emitted below the body; and locating the body and / or objects around the body according to the first reflection signal and the second reflection signal.

[0079] Any combination of one or more computer-readable media may be used. A computer-readable medium may be a computer-readable signal medium or a computer-readable storage medium. A computer-readable storage medium may be, for example, but not limited to, an electrical, magnetic, optical, electromagnetic, infrared, or semiconductor system, device or device, or any combination thereof. More specific examples (a non-exhaustive list) of computer-readable storage media include: an electrical connection with one or more wires, a portable computer disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination thereof. In this document, a computer-readable storage medium may be any tangible medium containing or storing a program that may be used by or in conjunction with an instruction execution system, device, or device.

[0080] Computer-readable signal media may include a data signal propagated in baseband or as part of a carrier wave, which carries a computer-readable program code. Such propagated data signals may take a variety of forms, including, but not limited to, electromagnetic signals, optical signals, or any suitable combination of the above. Computer-readable signal media may also be any computer-readable medium other than a computer-readable storage medium, which may send, propagate, or transmit a program for use by or in conjunction with an instruction execution system, apparatus, or device.

[0081] Program code embodied on a computer readable medium may be transmitted using any appropriate medium, including but not limited to wireless, wireline, optical fiber cable, RF, etc., or any suitable combination of the foregoing.

[0082] Computer program code for performing the operations of the present invention may be written in one or more programming languages ​​or a combination thereof, including object-oriented programming languages ​​such as Java, Smalltalk, C++, and conventional procedural programming languages ​​such as "C" or similar programming languages. The program code may be executed entirely on the user's computer, partially on the user's computer, as a separate software package, partially on the user's computer and partially on a remote computer, or entirely on a remote computer or server. In the case of a remote computer, the remote computer may be connected to the user's computer through any type of network, including a local area network (LAN) or a wide area network (WAN), or may be connected to an external computer (e.g., via the Internet using an Internet service provider).

[0083] Embodiment 6

[0084] Figure 5 FIG. 1 is a schematic diagram of a positioning system for an underwater robot in Embodiment 6 of the present invention.

[0085] Figure 6 This is a schematic diagram of a sonar array wave transmission of a positioning system for an underwater robot in Example 5 of the present invention. As shown in the figure, the system includes: a first forward sonar 1, a second forward sonar 2, a first downward sonar 3, a second downward sonar 4, a first sonar chamber 5 and a second sonar chamber 6.

[0086] Among them, the first sonar chamber 5 and the second sonar chamber 6 are respectively installed on both sides of the underwater robot body 7; the first forward sonar 1 and the first downward sonar 3 are placed in the first sonar chamber 5, and the second forward sonar 2 and the second downward sonar 4 are placed in the second sonar chamber 6; the placement direction of the first forward sonar 1 and the second forward sonar 2 is parallel to the direction of the body 7, and the heads are facing the front of the body 7; the placement direction of the first downward sonar 3 and the second downward sonar 4 is perpendicular to the direction of the body 7, and the heads are facing the bottom of the body 7.

[0087] The first forward sonar 1 is installed in the first sonar chamber 5, and the second forward sonar 2 is installed in the first sonar chamber 6, and the placement direction of the two sonars is parallel to the direction of the body 7, and their heads are both facing the front of the body 7. Specifically, the detection centerline of the first forward sonar 1 forms an angle with the horizontal central axis of the body 7, which is the detection angle α1 of the first forward sonar 1, and the detection centerline of the second forward sonar 2 forms an angle with the horizontal central axis of the body 7, which is the detection angle α2 of the second forward sonar 2.

[0088] The detection center lines of the first forward sonar 1 and the second forward sonar 2 are respectively at the same angle with the horizontal center axis of the body, that is, α1 and α2 are symmetrically set, and the set angles are the same, and the angle range is greater than or equal to 0 degrees and less than or equal to 45 degrees; for example, if the detection angle α1 of the first forward sonar 1 is set to 10°, then the detection angle α2 of the symmetrical second forward sonar 2 should also be set to 10°. The first forward sonar 1 and the second forward sonar 2 are placed on both sides of the lower part of the body 7; further, the first forward sonar 1 and the second forward sonar 2 are symmetrically placed on both sides of the body 7, and when measuring and determining the obstacle in front of the body 7, the measurement consistency of the distance between the two sonars and the obstacle in front can be guaranteed, so that the measurement result is accurate.

[0089] The first forward sonar 1 and the second forward sonar 2 can be used to realize the function of transmitting and receiving sound wave signals, specifically, a first sound wave signal is emitted to the front of the body 7, and the first sound wave signal will be reflected when it contacts the obstacle in front. Then, the first forward sonar 1 and the second forward sonar 2 receive the first reflected signal reflected by the obstacle and transmit it to the body 7. After that, the existence of the obstacle in front can be judged according to the received sound wave signal, and the distance, direction, speed and other parameters of the obstacle in front can be measured or estimated.

[0090] Similarly, the first downward sonar 3 is installed in the first sonar chamber 5, and the second downward sonar 4 is installed in the first sonar chamber 6, and the placement direction of the two sonars is perpendicular to the direction of the body 7, and their heads are both facing the bottom of the body 7. Specifically, the detection centerline of the first downward sonar 3 forms an angle with the vertical center axis of the body 7, which is the detection angle α3 of the first downward sonar 3, and the detection centerline of the second downward sonar 4 forms an angle with the vertical center axis of the body 7, which is the detection angle α4 of the second downward sonar 4.

[0091] The detection center lines of the first downward sonar 3 and the second downward sonar 4 are respectively at the same angle with the vertical center axis of the body, that is, α3 and α4 are symmetrically set, and the set angles are the same, and the angle range is greater than or equal to 0 degrees and less than or equal to 45 degrees; for example, if the detection angle α3 of the first downward sonar 3 is set to 0°, then the detection angle α4 of the symmetrical second forward sonar 2 should also be set to 0°. The first downward sonar 3 and the second downward sonar 4 are placed on both sides of the lower part of the body 7. Further, the first downward sonar 3 and the second downward sonar 4 are symmetrically placed on both sides of the body 7. When measuring and determining the obstacles under the body 7, the measurement consistency of the distances between the two sonars and the obstacles below can be guaranteed, so that the measurement results are accurate.

[0092] The first downward sonar 3 and the second downward sonar 4 can also be used to realize the function of transmitting and receiving sound wave signals, specifically, to transmit a second sound wave signal to the bottom of the body 7, and the second sound wave signal will be reflected when it contacts the obstacle below, and then the first downward sonar 3 and the second downward sonar 4 will receive the second reflected signal reflected back by the obstacle and transmit it to the body 7, and the existence of the obstacle below can be judged according to the received sound wave signal, and the distance, direction, speed and other parameters of the obstacle below can be measured or estimated.

[0093] Exemplarily, each sonar in the underwater robot in the embodiment of the present invention is installed on the sonar compartment of the body 7 at a fixed angle, wherein the two forward sonars are symmetrically arranged, and there is a fixed detection angle, and the two downward sonars are also symmetrically arranged, and there is also a fixed detection angle, and the fixed detection angles set for the sonars in different underwater robots are different. For example, a No. 1 underwater robot is produced, and its two forward sonar detection angles, namely, α1 and α2, are fixedly set to 10°, and the two downward sonar detection angles, namely, α3 and α4, are fixedly set to 5°; a No. 2 underwater robot is produced, and its two forward sonar detection angles, namely, α1 and α2, are fixedly set to 0°, and the two downward sonar detection angles, namely, α3 and α4, are fixedly set to 20°; and so on, according to the setting of the detection angle, various types of underwater robots can be produced, but the detection angle ranges set by them are all greater than or equal to 0 degrees and less than or equal to 45 degrees.

[0094] The combination of the first reflection signal and the second reflection signal can be used to realize the function of locking the underwater position of the body 7. Specifically, according to the first reflection signal and the second reflection signal, and in combination with the provided motion control algorithm, the distance between the body 7 and the obstacles in front and below can be calculated, so as to determine the position of the obstacles and realize intelligent obstacle avoidance. At the same time, the position of the body 7 can also be locked according to the position of the obstacles in front and below, so as to realize the underwater position locking of the body 7.

[0095] The first forward sonar 1, the second forward sonar 2, the first downward sonar 3 and the second downward sonar 4 are connected to the interior of the body 7 by concealed lines for signal data transmission.

[0096] The technical solution of this embodiment transmits sound waves and receives reflected waves through four micro-sonars in the horizontal direction and vertical downward direction, so as to sense obstacles in the directions indicated by the four sonars and changes in the position of the underwater robot in the water, so as to accurately realize the positioning and environmental positioning of the underwater robot. Furthermore, since the positioning and environmental positioning of the underwater robot can be carried out quickly and accurately, the movement posture of the underwater robot can be quickly adjusted even when operating in turbulent and complex waters, thereby avoiding the occurrence of abnormal situations such as loss of control during underwater operations.

[0097] Note that the above are only preferred embodiments of the present invention and the technical principles used. Those skilled in the art will understand that the present invention is not limited to the specific embodiments described herein, and that various obvious changes, readjustments and substitutions can be made by those skilled in the art without departing from the scope of protection of the present invention. Therefore, although the present invention has been described in more detail through the above embodiments, the present invention is not limited to the above embodiments, and may include more other equivalent embodiments without departing from the concept of the present invention, and the scope of the present invention is determined by the scope of the appended claims.

Claims

1. A positioning method for an underwater robot, characterized in that: The method comprises: Acquire a first reflection signal of a first sound wave signal emitted toward the front of the underwater robot body; wherein the first reflection signal is a sound wave signal received by the first forward sonar and the second forward sonar; Acquire a second reflected signal of a second sound wave signal emitted toward the bottom of the body; wherein the second reflected signal is a sound wave signal received by the first downward sonar and the second downward sonar; Positioning the machine body and / or objects around the machine body according to the first reflection signal and the second reflection signal includes: respectively calculating and measuring the distances from the first forward sonar, the second forward sonar, the first downward sonar, and the second downward sonar to obstacles; Establishing a body coordinate system XYZ, wherein the detection centerline directions of the first forward sonar and the second forward sonar are along the X-axis, and the detection centerline directions of the first downward sonar and the second downward sonar are along the Z-axis; Calculating the distance state between the machine body and the obstacle to obtain the stable distance of the machine body in the X-axis and Z-axis directions; respectively calculating and measuring radial velocities between the first forward sonar, the second forward sonar, the first downward sonar, the second downward sonar and the obstacle; Convert the radial velocities of the four sonars into velocities in the body coordinate system; Converting the velocity in the body coordinate system into the velocity in the earth coordinate system; Calculating the speed state between the body and the obstacle to obtain the stable speed of the body in the Y-axis direction; The fixed-point control of the machine body in the XYZ three-dimensional space is determined according to the stable distances of the machine body in the X-axis and Z-axis directions and the stable speed of the machine body in the Y-axis direction.

2. The method according to claim 1, characterized in that The positioning of the machine body and / or objects around the machine body according to the first reflection signal and the second reflection signal includes: Locating an obstacle in front of the machine body according to the first reflected signal; and An obstacle below the machine body is located according to the second reflected signal.

3. The method according to claim 1, characterized in that The positioning of the machine body and / or objects around the machine body according to the first reflection signal and the second reflection signal includes: The body is positioned according to the first reflection signal and the second reflection signal.

4. A positioning device for an underwater robot, characterized in that: The device comprises: A first acquisition unit is used to acquire a first reflection signal of a first sound wave signal emitted to the front of the underwater robot body; the first reflection signal is a sound wave signal received by the first forward sonar and the second forward sonar; A second acquisition unit is used to acquire a second reflection signal of a second sound wave signal emitted toward the bottom of the body; the second reflection signal is a sound wave signal received by the first downward sonar and the second downward sonar; a positioning unit, configured to locate the machine body and / or objects around the machine body according to the first reflection signal and the second reflection signal; The positioning unit is specifically used to calculate and measure the distances between the first forward sonar, the second forward sonar, the first downward sonar and the second downward sonar and obstacles respectively; establish a body coordinate system XYZ, wherein the detection centerline directions of the first forward sonar and the second forward sonar are along the X axis, and the detection centerline directions of the first downward sonar and the second downward sonar are along the Z axis; The method comprises the steps of: calculating the distance state between the aircraft body and the obstacle to obtain the stable distance of the aircraft body in the X-axis and Z-axis directions; respectively calculating and measuring the radial speeds between the first forward sonar, the second forward sonar, the first downward sonar and the second downward sonar and the obstacle; converting the radial speeds of the four sonars into speeds in the aircraft body coordinate system; converting the speed in the aircraft body coordinate system into speeds in the earth coordinate system; calculating the speed state between the aircraft body and the obstacle to obtain the stable speed of the aircraft body in the Y-axis direction; and determining the fixed-point control of the aircraft body in the XYZ three-dimensional space according to the stable distances of the aircraft body in the X-axis and Z-axis directions and the stable speed of the aircraft body in the Y-axis direction.

5. A computer device comprising a memory, a processor and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the program, the positioning method for an underwater robot as described in any one of claims 1-3 is implemented.

6. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the program is executed by a processor, the positioning method for an underwater robot as described in any one of claims 1 to 3 is implemented.

Images