Autonomous exploration method and device of underwater robot

Abstract

The invention belongs to the field of underwater robot control, and particularly relates to an autonomous exploration method and device of an underwater robot. The invention provides a segmented waypoint exploration method combining floating and diving conditions aiming at the precision problem of an existing underwater vehicle in waypoint navigation. According to the method, waypoint information is issued through a GCS (Ground Control Station), the waypoint information comprises position coordinates of a target waypoint, and the underwater navigation distance and the course angle are calculated. Then, the underwater vehicle is controlled to dive to the specified depth, the course angle is adjusted to the target course, and inertial navigation is adopted for underwater navigation. And when preset conditions are met, for example, the estimated displacement of the underwater robot from the target waypoint is smaller than a threshold value, the displacement is increased along with time or the underwater navigation time exceeds a set threshold value, the underwater robot executes a specific underwater task and floats to the water surface. On the water surface, the robot uses GPS signals or other precise navigation means to update position information and navigates to the position near a target waypoint. And the upper computer continues to issue the next waypoint information, and the steps are circulated until the underwater vehicle reaches the last waypoint, so that the whole underwater exploration task is completed. According to the invention, the closed loop of a long-distance underwater exploration task is realized, and the navigation precision and reliability of the underwater robot are remarkably improved.

Description

Technical Field

[0001] The invention belongs to the field of underwater robot control and relates to an autonomous exploration method of an underwater robot. Background Art

[0002] Waypoints Navigation technology of Unmanned Aerial Vehicle (UAV) has been widely used in logistics distribution, inspection, surveying and mapping, pesticide spraying and other fields. In this technology, the UAV can fly along the path set by the host computer, which consists of a series of waypoints. The UAV automatically adjusts the flight attitude and speed according to the waypoint information set by the host computer, and flies to each waypoint in turn. During the flight, the UAV will use sensors such as the Global Positioning System (GPS) and the Inertial Navigation System (INS) to determine its own position and attitude.

[0003] In recent years, with the rapid development of Large Language Model (LLM) technology, its powerful natural language processing capabilities and thought chain decision-making capabilities have had a profound impact on the field of robotics. As an important branch of the robotics field, drones also benefit from the development of large model technology. For example, Microsoft's ChatGPT for Robotics project uses large model technology to give drones the ability to handle complex tasks, such as autonomous exploration and identification of goods along specific waypoint paths (such as Z-shaped folding paths), demonstrating the huge application potential of drones in logistics, warehousing and other fields.

[0004] However, in the field of remotely operated vehicles (ROVs) and autonomous underwater vehicles (AUVs), although these devices play an important role in ocean exploration, underwater construction, underwater archaeology, etc., intelligent autonomous exploration technology combined with large language models is still blank. A key technical difficulty is that the widely used GPS system cannot provide accurate positioning for underwater robots because it cannot receive electromagnetic wave signals in underwater environments. At the same time, navigation methods based on inertial navigation systems (INS), magnetic compasses, and Doppler velocity logs will produce linearly growing errors over time until the errors accumulate to an unacceptable level.

[0005] At present, many patent studies focus on the fusion of multiple navigation data, error estimation update, and parameter update in response to the underwater navigation accuracy problem. These patents mainly focus on the optimization of underwater navigation accuracy, and rarely involve the innovation of underwater autonomous exploration mode combined with waypoint technology. Therefore, there is an urgent need for a method that can solve the problem of underwater robot waypoint navigation accuracy, so as to enable a large language model to issue precision-controlled underwater navigation commands to the robot, thereby completing underwater exploration tasks over longer distances.

[0006] The following are the comparative patent search results related to this patent technology. These patents involve technical fields such as combined navigation and autonomous exploration of underwater robots to varying degrees, providing reference and comparison for the innovation and practicality of this invention.

[0007] "Autonomous Underwater Robot Combined Navigation System and Method", CN102829777A, the claims only emphasize that GPS "regularly readjusts" the inertial navigation system. "Regularly readjust" is not clearly stated, and can only be understood as surfacing at regular intervals. After the robot dives, the patent describes the use of GPS positioning position and speed as the initial value of the inertial navigation system, but does not specify when to dive. Therefore, the patent does not clearly define the specific conditions for the underwater robot to float and dive. This patent has clear provisions for the conditions for diving and floating, and protects this segmented waypoint underwater exploration mode with specific diving and floating conditions.

[0008] "A method for controlling the navigation of an underwater robot", CN118131798A, discloses another method for realizing the navigation control of an underwater robot. The patent adopts a mode combining the GPS of an unmanned ship and sonar for positioning the underwater robot. The robot always sails underwater and does not involve segmented waypoint navigation including sinking and floating actions. In addition, the patent uses a local path planning when encountering underwater obstacles, while this patent adopts a vertical floating strategy, so it does not involve the modification of the two-dimensional global navigation route. Summary of the invention

[0009] In order to solve the problem of waypoint navigation accuracy of underwater robots, so that the large language model can issue precision-controlled underwater navigation commands to the robot, the present invention proposes a method for underwater robot navigation control and task exploration, thereby realizing a closed loop of underwater exploration tasks over longer distances.

[0010] The present invention provides a method for navigation control and task exploration of an underwater robot, comprising: a host computer sends the next waypoint information; the host computer calculates the underwater navigation distance and heading angle, controls the robot to dive and adjusts the heading angle; the robot navigates underwater until a preset braking condition is met; the robot surfaces or directly surfaces after performing a specific task underwater; the robot updates position information on the water surface using a GPS signal and navigates to the vicinity of a waypoint using GPS; the host computer sends the next waypoint information, and the process is repeated until the robot reaches the last waypoint, thereby completing the entire underwater exploration task.

[0011] According to an embodiment of the present invention, the host computer exchanges information with the main body ROV or underwater robot through a cable. The host computer receives sensor data from the main body inertial navigation system, magnetic compass or Doppler log in real time, such as acceleration, speed, attitude and orientation, etc., for subsequent control and navigation calculations.

[0012] According to an embodiment of the present invention, the host computer sends the next waypoint information, each waypoint information can be longitude and latitude coordinates (usually based on the WGS84 coordinate system), or NED (North-East-Down) relative coordinates based on the exploration origin. If the waypoint uses NED relative displacement, it can be converted into longitude and latitude coordinates before subsequent navigation calculations.

[0013] According to an embodiment of the present invention, the host computer calculates the underwater navigation distance and the heading angle corresponding to the target waypoint, controls the underwater robot to dive to the specified depth and rotate to the target heading angle. Specifically, given the longitude and latitude coordinates of point A and point B, the great circle distance between the two points on the earth's spherical surface can be calculated by the Haversine formula, and then the heading angle from point A to point B can be calculated according to the Great Circle Bearing Formula. There is no order requirement for diving and rotating the heading angle, but it is generally chosen to dive to the specified depth first, and then control its rotation, so as to reduce the interference of the diving process on the heading angle.

[0014] According to an embodiment of the present invention, the robot keeps the underwater heading angle unchanged, uses inertial navigation, and navigates underwater until a preset condition is met. In this embodiment, the host computer estimates the relative displacement of the body during underwater navigation by integrating the acceleration of the body's inertial measurement unit (IMU). The power propulsion control of the robot during underwater navigation can be adjusted by a pulse width modulation signal (PWM). PWM can use a fixed amplitude or a more precise proportional integral differential control (PID control). When the inertial navigation meets the preset conditions, it brakes and floats to the surface. In this embodiment, the pre-set underwater braking conditions are: 1. The estimated displacement of the underwater robot from the target waypoint is less than a threshold; 2. The estimated displacement of the underwater robot from the target waypoint begins to increase over time; 3. The underwater navigation time exceeds the set threshold.

[0015] In special cases, when the robot is navigating underwater, if the radar detects an obstacle in front of it, the host computer can control the robot to float up until the obstacle in front is removed, and then control it to continue navigating underwater. The two-dimensional trajectory of the robot during underwater navigation does not change.

[0016] According to an embodiment of the present invention, after the robot completes braking while navigating underwater, it can perform a specific underwater task and then surface or directly surface. Specific underwater tasks include but are not limited to: picking up underwater garbage, static and dynamic tracking and observation of marine life, etc.

[0017] According to an embodiment of the present invention, after the robot floats to the surface, it uses precise navigation means such as GPS signals or optical positioning to update the position information and accurately navigate to the vicinity of the waypoint. In terms of specific physical implementation, the robot can use surface navigation or air flight to accurately navigate to a place close enough to the waypoint. When the robot enters the displacement error threshold range of the waypoint, it is determined that it has reached the waypoint, thereby realizing the correction of the underwater inertial navigation error. At this time, the host computer sends the next waypoint information.

[0018] The error of underwater inertial navigation is proportional to the distance and time of underwater navigation, so the upper limit of the error of a single underwater inertial navigation is:

[0019] By automatically controlling the descent and ascent of the drone through programming, this method can ensure that the error of underwater inertial navigation is less than the threshold each time. , so that the inertial navigation error of the entire exploration process is less than the threshold This is because, after reaching the target waypoint through precise navigation, the errors of each underwater navigation can be accumulated again.

[0020] The host computer controls the robot to navigate and explore underwater section by section by continuously sending target points, and finally completes the underwater exploration mission in the specified area. BRIEF DESCRIPTION OF THE DRAWINGS

[0021] For a more complete understanding of the present invention and its advantages, reference will now be made to the following description of the accompanying drawings, in which:

[0022] Figure 1 A general flow chart of autonomous exploration of an underwater robot provided by an embodiment of the present invention is shown.

[0023] Figure 2 A logical judgment diagram of the surfacing and diving of an underwater robot provided by an embodiment of the present invention is shown.

[0024] Figure 3 A schematic diagram of a mission route for autonomous exploration of an underwater robot provided by an embodiment of the present invention is shown.

[0025] The realization of the purpose, functional features and advantages of the present invention will be further explained in conjunction with embodiments and with reference to the accompanying drawings. DETAILED DESCRIPTION

[0026] The present application will be described in detail below with reference to the accompanying drawings and in combination with embodiments. It should be noted that the embodiments and features in the embodiments of the present application can be combined with each other without conflict.

[0027] This specification contains flow charts and block diagrams to assist understanding. It should be understood that some of the blocks or combinations thereof in these diagrams can be implemented in practice by computer programs. These programs can be designed to run on a processor of a general-purpose computer, a special-purpose computer, or other programmable data processing device, thereby creating a device capable of performing the functions / operations described in the diagrams.

[0028] Furthermore, the technical implementation of the present invention can be in the form of hardware or software (including firmware, microcode, etc.), or a combination of the two. In addition, the technology of the present invention can also exist in the form of a computer program product, which is stored on a computer-readable medium for use alone or in combination with an instruction execution system. In this context, a computer-readable medium is broadly defined as any medium that can carry, store, transmit, disseminate or transfer instructions. For example, this includes but is not limited to electronic, magnetic, optical, electromagnetic, infrared or semiconductor systems, devices, components or communication media. Specifically, examples of computer-readable media include magnetic storage devices (such as magnetic tapes, hard disk drives HDD), optical storage devices (such as optical disks CD-ROM), memory devices (such as random access memory RAM, flash memory), and wired or wireless communication links.

[0029] like Figure 1 As shown, the embodiment of the present invention provides a general process of autonomous exploration of an underwater robot, including S110 to S190.

[0030] S110, the host computer loads and runs the underwater exploration program, the underwater robot starts to explore, and sends the initial position to the host computer as the exploration origin. The host computer and the underwater robot can exchange information through physical cables using a specific message transmission protocol, such as MAVLink (Micro Air Vehicle Link). In this embodiment, the underwater robot or underwater drone ROV uses the open source software ArduSub to achieve autonomous navigation, depth, heading keeping, feedback stability control and other functions.

[0031] S120, the host computer program decides and sends a new waypoint location information. Specifically, the waypoint information can be a two-dimensional longitude and latitude coordinate (such as WGS84 coordinate system), or a relative coordinate based on the exploration origin (such as NED relative coordinate, in meters). Assume that the origin GPS coordinate is , the NED coordinates of the robot relative to the origin are , then the GPS coordinates of the robot are:

[0032] Where R = 6378137.0 is the radius of the Earth (in meters).

[0033] S130, the host computer program calculates the underwater navigation distance and the heading angle corresponding to the target waypoint. Specifically, the host computer can calculate the great circle distance d between the two points on the earth's spherical surface by the Haversine formula based on the current coordinates of the underwater robot and the longitude and latitude coordinates of the new waypoint:

[0034] in is the dimension value, Longitude value, It represents the difference in longitude or latitude between two points, where R is the average radius of the Earth (6371 km).

[0035] Then use the Great Circle Bearing Formula to calculate the heading angle \theta from the current position to the new waypoint (with true north as 0 degrees):

[0036] S140, the robot dives, adjusts and fixes the heading angle, and then navigates underwater based on inertial navigation. There is no order requirement for diving and rotating the heading angle, but it is generally chosen to dive first and then control its rotation, so as to reduce the interference of the diving process on the heading angle. The specific diving depth can be judged based on the image detected by the camera or radar. After diving to about 1-2 meters from the bottom of the water, the depth is fixed. Based on the MAVLink protocol, the upper computer controls the robot's forward movement and heading rotation by sending the pulse width modulation signal PMW value of the forward (Pitch) and rotation (Yaw) respectively. The simplest control is to use a fixed PWM value, and more precise control can adopt proportional integral differential control (PID control).

[0037] S150, the robot navigates underwater until the preset conditions are met, performs a specific underwater task, and floats to the surface. In this embodiment, the host computer estimates the relative displacement of the robot during underwater navigation by integrating the acceleration of the robot's inertial measurement unit. When the inertial navigation meets the preset conditions, it brakes, performs a specific underwater task, and floats to the surface. The execution of a specific underwater task is an optional operation, which depends on the decision command of the host computer. In this embodiment, the pre-set underwater braking conditions are: 1. The estimated displacement of the underwater robot from the target waypoint is less than a threshold; 2. The estimated displacement of the underwater robot from the target waypoint begins to increase over time; 3. The underwater navigation time exceeds the set threshold.

[0038] In particular, if the robot detects an obstacle in front of it during underwater navigation through radar, the host computer can control the robot to float up until the obstacle in front is removed, and then control it to continue to navigate underwater. During the whole process, the robot's navigation angle remains unchanged during underwater navigation, and the two-dimensional trajectory route does not change.

[0039] According to an embodiment of the present invention, after the robot completes braking while navigating underwater, it can perform specific underwater tasks according to the instructions sent by the host computer. Specific underwater tasks include but are not limited to: picking up underwater garbage, static and dynamic tracking and observation of marine life, etc.

[0040] S160, after the robot floats to the surface, it can use GPS signals or other precise navigation methods such as optical positioning to update the position information. In this embodiment, after the robot floats to the surface, the operation mode will automatically change from manual mode (Manual) to navigation mode (Guided), so that the robot can switch from manual navigation based on PWM to automatic precise navigation using GPS.

[0041] S170, the robot sails on the water surface or flies in the air based on GPS signals or optical positioning until it reaches the vicinity of the waypoint. In this embodiment, the robot adopts a vector thruster layout and can directly sail on the water surface after surfacing. If the robot has a quad-rotor or other flight structure device, it can also take off directly from the water surface, fly in the air to the vicinity of the two-dimensional coordinates of the waypoint, and then vertically land on the water surface.

[0042] S180, when the robot enters the displacement error threshold range of the two-dimensional coordinates of the waypoint, it is determined that the drone has reached the waypoint. At this time, if the host computer decides that it needs to go to the next exploration point, the host computer will send the next waypoint information, and the process will enter S120 again from S180.

[0043] S190: If the current waypoint is the exploration starting point, or the host computer decides that there is no need to go to the next exploration point, the process enters S190 from S180 and the exploration ends.

[0044] Figure 2 The logical judgment diagram of the underwater robot's ascent and descent is shown.

[0045] When the robot is in the underwater inertial navigation stage 201, the positioning starting point of this stage is the last updated two-dimensional position information of the GPS obtained when the robot starts to dive. During the entire underwater navigation stage, the host computer integrates the acceleration of the inertial measurement unit (IMU) to estimate the relative displacement of the body during underwater navigation (such as NED relative coordinates).

[0046] First, we estimate the robot's two-dimensional coordinate position underwater in real time based on the initial diving position and the NED relative coordinates (refer to the calculation formula of S120). Based on the coordinate position estimated by the inertial navigation and the target waypoint position, we can further estimate the remaining distance of the current underwater robot from the target waypoint. The robot brakes after the underwater inertial navigation meets the preset conditions, performs specific underwater tasks and floats to the surface.

[0047] Braking condition 1 corresponds to 202: determining whether the estimated remaining distance is less than a threshold.

[0048] Braking condition 2 corresponds to 203: determining whether the estimated remaining distance begins to increase over time.

[0049] Braking condition 3 corresponds to 204: determining whether the underwater navigation time exceeds a threshold.

[0050] As long as one of the braking conditions 202, 203 and 204 returns "yes", the robot starts braking and floats to the surface 205; otherwise, the robot continues underwater inertial navigation 201. After surfacing, the robot starts to use precise navigation to navigate to the exploration point 206. The host computer receives the GPS coordinate information of the robot in real time and determines the braking condition 207: whether the distance difference between the GPS coordinates of the robot and the GPS coordinates of the exploration point is less than the threshold. If the braking condition 207 returns "yes", the robot receives the next waypoint sent by the host computer, dives, and enters the next underwater inertial navigation 201 stage; if it returns "no", it continues to wait for the GPS navigation of ArduSub to execute 206.

[0051] like Figure 3 As shown, an embodiment of the present invention provides a schematic diagram of a mission route for autonomous exploration of an underwater robot.

[0052] The host computer program sets the initial coordinate position of the underwater robot as the origin, and the exploration range is an ocean area 120 meters long and 80 meters wide. The host computer first designs a "zigzag" waypoint route and stores the waypoint information in a certain computer data structure, such as a stack structure.

[0053] The relative position of the first waypoint sent by the host computer is (0, 20). From the figure, we can see the trajectory information of the robot diving (position marked as a dot), navigating underwater, surfacing (position marked as a dot), and navigating on the surface to the waypoint (position marked as a dot). The trajectory of the robot's underwater inertial navigation is represented by a dotted line, and the trajectory of GPS navigation after surfacing is represented by a solid line.

[0054] When the robot was navigating underwater from waypoint (0, 20) to (40, 20), the robot detected an underwater garbage (marked by a triangle in the figure) and a coral reef (marked by a star in the figure) nearby through the sonar radar and camera. After the host computer estimated the relative position of the underwater garbage to be (34, 30) and the relative position of the coral reef to be (38, 15), it inserted these two coordinate information into the data structure storing the waypoints in sequence according to the priority of the underwater tasks. In this way, after reaching the waypoint (40, 20), the robot will go to (34, 30) and (38, 15) in turn, perform the corresponding garbage picking and photo taking tasks, and then return to the original zigzag route to continue exploring.

[0055] The entire exploration process ends after the robot has completed all zigzag waypoints.

Claims

1. An autonomous exploration method for an underwater robot, characterized in that: The following steps are involved: Step A: The host computer sends the next waypoint information, which includes the location coordinates of the target waypoint; Step B: The host computer calculates the underwater navigation distance and heading angle according to the current waypoint position and the target waypoint position, controls the underwater robot to dive to the specified depth and adjusts the heading angle to the target heading; Step C: The underwater robot maintains the heading angle unchanged and navigates underwater based on inertial navigation until a preset braking condition is met; Step D: The underwater robot directly floats to the surface after performing a specific underwater task; Step E: The underwater robot updates its position information on the water surface using GPS signals or other precise navigation methods, and navigates to the vicinity of the target waypoint; Step F: The host computer sends the next waypoint information, and steps A to E are repeated until the underwater robot reaches the last waypoint, thus completing the entire underwater exploration mission.

2. The autonomous exploration method of an underwater robot according to claim 1, characterized in that: The preset braking conditions in step C include: the estimated displacement of the underwater robot from the target waypoint is less than a threshold; or the estimated displacement of the underwater robot from the target waypoint begins to increase over time; or the underwater navigation time exceeds a set threshold.

3. The autonomous exploration method for an underwater vehicle according to claim 1, characterized in that: The specific underwater tasks in step D include underwater garbage picking up, static and dynamic tracking and observation of marine life, etc.

4. The autonomous exploration method of an underwater robot according to claim 1, characterized in that: The precise navigation to the target waypoint in step E can be physically realized by surface navigation or air flight.

5. The autonomous exploration method of an underwater robot according to claim 1, characterized in that: The host computer automatically controls the diving and surfacing of the underwater robot through programming, ensuring that the error of each underwater inertial navigation is less than the preset threshold, thereby achieving the controllability of the inertial navigation error during the entire exploration process.

6. The autonomous exploration method of an underwater robot according to claim 1, characterized in that: The host computer continuously sends target waypoints to control the underwater robot to navigate and explore underwater section by section, and finally complete the underwater exploration task in the specified area.

7. The autonomous exploration method of an underwater robot according to claim 1, characterized in that: The host computer exchanges information with the underwater robot through cables, receives real-time sensor data from the underwater robot, and performs subsequent control and navigation calculations. The sensor data includes acceleration, speed, attitude and orientation, etc.

8. A device for implementing the autonomous exploration method of an underwater robot according to any one of claims 1 to 7, characterized in that: It includes a host computer and an underwater robot. The host computer is used to send waypoint information, calculate underwater navigation distance and heading angle, receive and process real-time sensor data of the underwater robot, and control the diving and heading adjustment of the underwater robot; the underwater robot is used to receive instructions from the host computer, perform inertial navigation and underwater navigation, execute specific underwater tasks, surface and use GPS signals to update position information.

Images