Intelligent positioning method and system for running orbit of underwater robot

Abstract

The invention discloses an intelligent positioning method and system for a running orbit of an underwater robot, and the method comprises the steps: obtaining the position data, posture data and underwater environment data of the underwater robot based on a sensor carried by the underwater robot; based on the position data and the attitude data of the underwater robot, constructing a motion model of the underwater robot, and predicting position and attitude changes of the robot under given control output; based on the data of the underwater environment where the underwater robot is located, extracting underwater landform feature data in the data, and constructing an underwater environment map model; and matching the motion model of the underwater robot with the underwater environment map model to locate the current position of the underwater robot. According to the method, the actual observation point of the underwater robot is predicted by introducing the underwater robot motion model of the control output formula, and the feature points of the map model of the underwater environment are matched, so that the accurate positioning of the current position of the robot is realized.

Description

Technical Field

[0001] The present invention relates to the field of robot positioning technology, and in particular to an intelligent positioning method and system for an underwater robot's running track. Background Art

[0002] Underwater robots have been widely used in marine scientific research, underwater archaeology, seabed resource development and military fields. However, the complex and changeable underwater environment poses a huge challenge to the underwater robots to achieve accurate positioning of their operating orbits. Traditional underwater robot orbit positioning methods mainly rely on GPS technology, but due to the complexity and uncertainty of the underwater environment, GPS signals are easily interfered and shielded, thus affecting the positioning accuracy; at the same time, the positioning accuracy of GPS technology is also limited by the quality and quantity of satellite signals.

[0003] The patent with publication number CN 113341421A discloses a positioning method and system for an underwater robot, which obtains the reflected signals of the sound wave signals emitted to the front and bottom of the underwater robot body, and locates the robot according to these reflected signals. However, the sound wave signals are also easily seriously affected in complex underwater environments. It is impossible to improve the positioning accuracy of the underwater robot by simply relying on the sound wave signals for positioning. The patent with publication number CN 116539045A discloses an underwater robot positioning method, an underwater robot, a storage medium and an electronic device. Through a preset map, the underwater robot moves along the pool wall at a reference distance, and the movement distance with the pool wall is obtained in real time. The position of the underwater robot is obtained by calculation to ensure the accuracy of navigation positioning. However, this method is based on the preset map for positioning, and it is highly dependent on the map information constructed and stored in advance. If there is a large difference between the actual working environment and the preset map, such as terrain changes, obstacle position changes, etc., the accuracy of positioning is seriously interfered; therefore, the existing technology is often subject to many restrictions in the face of the complexity and uncertainty of the unknown underwater environment, and thus cannot achieve accurate positioning of the underwater robot.

[0004] In view of this, there is an urgent need in the art for an intelligent positioning method and system for the running track of an underwater robot that can achieve precise positioning in a complex underwater environment. Summary of the invention

[0005] In order to overcome the defects of the above-mentioned prior art, the present invention provides a method and system for intelligently positioning the running track of an underwater robot, so as to solve the problem in the prior art that the underwater robot cannot achieve accurate positioning in a complex underwater environment.

[0006] In order to achieve the above object, the present invention adopts the following technical solutions:

[0007] In a first aspect, an embodiment of the present invention provides a method for intelligently positioning an underwater robot's running track, comprising the following steps:

[0008] S1: Based on the sensors carried by the underwater robot, the position data, posture data and underwater environment data of the underwater robot are obtained;

[0009] S2: Based on the position data and posture data of the underwater robot, a motion model of the underwater robot is constructed, and the position and posture changes of the robot under given control output are predicted;

[0010] S3: Based on the underwater environment data of the underwater robot, extract the underwater landform feature data and build an underwater environment map model;

[0011] S4: Combine the motion model of the underwater robot and the underwater environment map model to locate the current position of the underwater robot.

[0012] Optionally, the sensor in S1 includes a sonar sensor, a light scanner, and an inertial sensor;

[0013] The sonar sensor is used to calculate the position data of the underwater robot through the time difference of the sound waves going back and forth in the water; the position data is the distance between the underwater robot and the boundary of the water area;

[0014] The inertial sensor is used to measure the angular velocity and acceleration changes in different directions, and calculate the posture data of the robot according to the inertial sensor measurement values;

[0015] The light scanner is used to collect underwater environment data where the robot is located.

[0016] Optionally, a function expression of the position data:

[0017] d is =0.5*sv*T time

[0018] Where, d is is the distance data, sv is the speed of sound, T time It is double the time difference;

[0019] Functional expression of the posture data:

[0020] pitch=atan2(Y,sqrt(X 2 +Z 2 ))

[0021] roll=atan2(X,Z)

[0022] yaw=a tan 2(X,Y)

[0023] Where roll is the roll angle of the robot, pitch is the pitch angle of the robot, yaw is the yaw angle of the robot; atan2 is the inverse tangent value, Y is the Y-axis value of the inertial sensor, X is the X-axis value of the inertial sensor, and Z is the Z-axis value of the inertial sensor; sqrt is used to calculate the non-negative square root.

[0024] Optionally, S2 includes the following specific steps:

[0025] S201, taking the center of the underwater robot as the point of the coordinate system, determining the directions of three perpendicularly intersecting coordinate axes, and constructing a coordinate system; wherein the directions of the three coordinate axes are represented as x, y, and z respectively;

[0026] S202, based on the position data and attitude data of the underwater robot, obtaining the position coordinates and attitude angle of the underwater robot at the current moment;

[0027] S203, constructing a motion model of the underwater robot based on the position coordinates and posture angle of the underwater robot at the current moment;

[0028] The underwater robot motion model includes a position update model and a posture update model;

[0029] S204: Introduce a control output formula into the robot motion model to predict the position change and posture change of the robot under a given control output.

[0030] Optionally, the function expression of the location update model is:

[0031] x(t+Δt)=x(t)+v*cos(yaw)*Δt

[0032] y(t+Δt)=y(t)+v*sin(yaw)*Δt

[0033] z(t+Δt)=z(t)+v z *Δt

[0034] Where x(t) is the horizontal coordinate of the underwater robot at the current moment, y(t) is the vertical coordinate of the underwater robot at the current moment, and z(t) is the vertical coordinate of the underwater robot at the current moment; x(t+Δt) is the horizontal coordinate of the underwater robot after a time of Δt, y(t+Δt) is the vertical coordinate of the underwater robot after a time of Δt, and z(t+Δt) is the vertical coordinate of the underwater robot after a time of Δt; Δt is the time step of the underwater robot; v is the speed of the underwater robot at the current moment, v Z is the vertical speed of the underwater robot at the current moment; yaw is the yaw angle of the underwater robot at the current moment, cos(yaw) is the cosine value of the yaw angle, and sin(yaw) is the sine value of the yaw angle;

[0035] The functional expression of the posture update model is:

[0036] roll(t+Δt)=roll(t)+ang_v_x*Δt

[0037] pitch(t+Δt)=pitch(t)+ang_v_y*Δt

[0038] yaw(t+Δt)=yaw(t)+ang_v_z*Δt

[0039] Among them, roll(t) is the roll angle of the underwater robot at the current moment, pitch(t) is the pitch angle of the underwater robot at the current moment, and yaw(t) is the yaw angle of the underwater robot at the current moment; roll(t+Δt) is the roll angle of the underwater robot after a period of time Δt, pitch(t+Δt) is the pitch angle of the underwater robot after a period of time Δt, and yaw(t+Δt) is the yaw angle of the underwater robot after a period of time Δt; Δt is the time step of the underwater robot's rotation; ang_v_x is the angular velocity of the underwater robot rotating around the x-axis at the current moment, ang_v_y is the angular velocity of the underwater robot rotating around the y-axis at the current moment, and ang_v_z is the angular velocity of the underwater robot rotating around the z-axis at the current moment;

[0040] The control output formula is:

[0041] v k =f(v_c)

[0042] In the formula, v k is the control output; v_c is the linear velocity control value of the underwater robot at the current moment; f is the actual linear velocity of the underwater robot.

[0043] Optionally, S3 includes the following specific steps:

[0044] Based on the underwater environment data, extracting characteristic data describing the underwater landform;

[0045] The extracted feature data are combined to establish an underwater environment map model;

[0046] Divide the area of ​​the underwater environment map model into a number of groups of grid cells according to fixed sizes;

[0047] Calculate the value of the attribute of each grid cell, and store the calculated attribute value in the underwater environment map model;

[0048] The unit attributes include a height attribute, a normal attribute, and a color attribute, wherein the color attribute is used to represent different water depths through different colors;

[0049] Optionally, the function expression of the height attribute is:

[0050]

[0051] In the formula, H p is the height attribute of the grid cell; N is the number of points in the grid cell, h i is the height value of the i-th point in the grid unit. Specifically, each grid unit includes multiple points, and each point has a height value;

[0052] The function expression of the normal attribute is:

[0053]

[0054] Where N f is the normal property of the mesh unit, N i is the normal of the ith point in the grid cell, and N is the number of points in the grid cell;

[0055] The function expression of the color attribute is:

[0056]

[0057] C max =max(max(R1,R2,...,R N ),max(G1,G2,...,G N ),max(B1,B2,...,B N ))

[0058] C min =min(min(R1,R2,...,R N ),min(G1,G2,...,G N ),min(B1,B2,...,B N ))

[0059] In the formula, C j , C max , C min are the average color, maximum color, and minimum color respectively; R i , G i , B i are the R value, G value, and B value of the RGB value of the i-th point in the grid unit, and N is the number of points in the grid unit.

[0060] Optionally, S4 includes the following specific steps:

[0061] Obtain the actual observation point of the underwater robot through the motion model of the underwater robot;

[0062] Perform descriptor matching between the actual observation points of the underwater robot and the feature points of the underwater environment map to obtain the similarity of the feature points between the two;

[0063] The similarity of the feature points is compared with the preset similarity threshold. If the similarity of the feature points is greater than the similarity threshold, the two feature points are considered to match and the position of the underwater robot is determined. If the similarity of the feature points is less than the similarity threshold, the two feature points are considered to be mismatched, and the actual observation points are re-obtained and descriptor-matched with the feature points of the underwater environment map again.

[0064] The actual observation point includes the location data of the current location of the underwater robot; the underwater environment map feature point includes the unit attribute value in the underwater environment map.

[0065] Optionally, the function expression of the descriptor matching:

[0066]

[0067] Where D(p1,p2) is the similarity between the actual observation point and the feature point of the underwater environment map; p1 is the actual observation point; p2 is the feature point of the underwater environment map; d i is the descriptor of the ith feature.

[0068] In a second aspect, an embodiment of the present invention provides an intelligent positioning system for an underwater robot's running track, comprising:

[0069] A data acquisition module is used to obtain the position data, posture data and underwater environment data of the underwater robot based on the sensors carried by the underwater robot;

[0070] A model building module is used to build a motion model of the underwater robot based on the position data and posture data of the underwater robot, and predict the position and posture changes of the robot under a given control output;

[0071] A map generation module is used to extract underwater landform feature data based on the underwater environment data of the underwater robot and construct an underwater environment map model;

[0072] The position determination module is used to match the motion model of the underwater robot with the underwater environment map model to locate the current position of the underwater robot.

[0073] The beneficial effects brought by the embodiments provided by the present invention include:

[0074] The present invention predicts the actual observation point of the underwater robot by introducing an underwater robot motion model with a control output formula, and matches the feature points of the map model of its underwater environment to achieve accurate positioning of the robot's current position, thereby improving the accuracy and reliability of the underwater robot's underwater operations; the present invention achieves positioning by identifying natural feature points in the underwater landform, so that the underwater robot can also be positioned in a complex environment, thereby improving the flexibility and applicability of the underwater robot's positioning. BRIEF DESCRIPTION OF THE DRAWINGS

[0075] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative labor.

[0076] Figure 1 A schematic diagram of a process of intelligently positioning a running track of an underwater robot according to an embodiment of this specification is shown;

[0077] Figure 2 A structural block diagram of an intelligent positioning system for an underwater robot's running track according to an embodiment of the present specification is shown. DETAILED DESCRIPTION

[0078] In order to make the objectives, technical solutions and advantages of the present disclosure more clearly understood, the present disclosure is further described in detail below in combination with specific embodiments and with reference to the accompanying drawings.

[0079] However, it should be understood that these descriptions are exemplary only and are not intended to limit the scope of the present disclosure. In the following detailed description, for ease of explanation, many specific details are set forth to provide a comprehensive understanding of the embodiments of the present disclosure. However, it is apparent that one or more embodiments may also be implemented without these specific details. In addition, in the following description, the description of the known technology is omitted to avoid unnecessary confusion of the concepts of the present disclosure.

[0080] The terms used herein are only for describing specific embodiments and are not intended to limit the present disclosure. The term "comprising" used herein indicates the existence of features, steps, operations, but does not exclude the existence or addition of one or more other features. It should be noted that all terms (including technical and scientific terms) used herein have the meanings commonly understood by those skilled in the art, unless otherwise defined. It should be noted that the terms used here should be interpreted as having meanings consistent with the context of this specification, and should not be interpreted in an idealized or overly rigid manner.

[0081] Example 1

[0082] Figure 1 A schematic diagram showing a flow chart of an intelligent positioning method for an underwater robot's running track according to an embodiment of this specification

[0083] like Figure 1 As shown, an embodiment of the present invention provides a method for intelligently positioning the running track of an underwater robot, comprising the following steps:

[0084] S1: Based on the sensors carried by the underwater robot, the position data, posture data and underwater environment data of the underwater robot are obtained;

[0085] Exemplarily, the sensors in S1 include a sonar sensor, a light scanner, and an inertial sensor; wherein the inertial sensor includes a gyroscope and an accelerometer.

[0086] In some embodiments, S1 includes the following specific steps:

[0087] Based on the sonar signal emitted by the sonar sensor, the time it takes for the sound wave to go back and forth in the water is calculated by the time difference between sending and receiving the sound wave, and the position data of the underwater robot is calculated based on the time difference; wherein the position data is the distance between the underwater robot and the boundary of the water area;

[0088] The inertial sensor is used to measure the angular velocity and acceleration changes in different directions, and the influence of gravity, buoyancy, motion inertia and other factors on the inertial sensor measurement values ​​is eliminated. The robot's posture data is calculated based on the inertial sensor measurement values.

[0089] Collect underwater environment data of the robot through a light scanner;

[0090] The position data, attitude data and underwater environment data are cleaned and filtered to remove useless data and noise. Specifically, the function expression of the position data is:

[0091] d is =0.5*sv*T time

[0092] Where, d is is the distance data, sv is the speed of sound, T time It is double the time difference;

[0093] Specifically, the function expression of posture data is:

[0094] pitch=atan2(Y,sqrt(X 2 +Z 2 ))

[0095] roll=atan2(X,Z)

[0096] yaw=atan2(X,Y)

[0097] Where roll is the roll angle of the robot, pitch is the pitch angle of the robot, yaw is the yaw angle of the robot; atan2 is the inverse tangent value, Y is the Y-axis value of the inertial sensor, X is the X-axis value of the inertial sensor, and Z is the Z-axis value of the inertial sensor; sqrt is used to calculate the non-negative square root.

[0098] S2: Based on the position data and posture data of the underwater robot, a motion model of the underwater robot is constructed, and the position and posture changes of the robot under given control output are predicted;

[0099] In some embodiments, S2 includes the following specific steps:

[0100] S201, taking the center of the underwater robot as the point of the coordinate system, determining the directions of three perpendicularly intersecting coordinate axes, and constructing a coordinate system; wherein the directions of the three coordinate axes are represented as x, y, and z respectively;

[0101] S202, based on the position data and attitude data of the underwater robot, obtaining the position coordinates and attitude angle of the underwater robot at the current moment;

[0102] S203, based on the current position coordinates and attitude angle of the underwater robot, and according to the dynamic characteristics of the robot, construct an underwater robot motion model; wherein the underwater robot motion model includes a position update model and an attitude update model;

[0103] S204: Introduce a control output formula into the robot motion model to predict the position change and posture change of the robot under a given control output.

[0104] Specifically, the function expression of the position update model is:

[0105] x(t+Δt)=x(t)+v*cos(yaw)*Δt

[0106] y(t+Δt)=y(t)+v*sin(yaw)*Δt

[0107] z(t+Δt)=z(t)+v z *Δt

[0108] Where x(t) is the horizontal coordinate of the underwater robot at the current moment, y(t) is the vertical coordinate of the underwater robot at the current moment, and z(t) is the vertical coordinate of the underwater robot at the current moment; x(t+Δt) is the horizontal coordinate of the underwater robot after a time of Δt, y(t+Δt) is the vertical coordinate of the underwater robot after a time of Δt, and z(t+Δt) is the vertical coordinate of the underwater robot after a time of Δt; Δt is the time step of the underwater robot; v is the speed of the underwater robot at the current moment, v Z is the vertical speed of the underwater robot at the current moment; yaw is the yaw angle of the underwater robot at the current moment, cos(yaw) is the cosine value of the yaw angle, and sin(yaw) is the sine value of the yaw angle;

[0109] Specifically, the function expression of posture update is:

[0110] roll(t+Δt)=roll(t)+ang_v_x*Δt

[0111] pitch(t+Δt)=pitch(t)+ang_v_y*Δt

[0112] yaw(t+Δt)=yaw(t)+ang_v_z*Δt

[0113] Among them, roll(t) is the roll angle of the underwater robot at the current moment, pitch(t) is the pitch angle of the underwater robot at the current moment, and yaw(t) is the yaw angle of the underwater robot at the current moment; roll(t+Δt) is the roll angle of the underwater robot after a period of time Δt, pitch(t+Δt) is the pitch angle of the underwater robot after a period of time Δt, and yaw(t+Δt) is the yaw angle of the underwater robot after a period of time Δt; Δt is the time step of the underwater robot's rotation; ang_v_x is the angular velocity of the underwater robot rotating around the x-axis at the current moment, ang_v_y is the angular velocity of the underwater robot rotating around the y-axis at the current moment, and ang_v_z is the angular velocity of the underwater robot rotating around the z-axis at the current moment;

[0114] Specifically, the control output formula is:

[0115] v k =f(v_c)

[0116] In the formula, v k is the control output; v_c is the linear velocity control value of the underwater robot at the current moment; f is the actual linear velocity of the underwater robot.

[0117] S3: Based on the underwater environment data of the underwater robot, extract the underwater landform feature data and build an underwater environment map model;

[0118] In some embodiments, S3 includes the following specific steps:

[0119] Based on the underwater environment data, extracting characteristic data describing the underwater landform;

[0120] The extracted feature data are combined to establish an underwater environment map model;

[0121] Divide the area of ​​the underwater environment map model into a number of groups of grid cells according to fixed sizes;

[0122] Calculate the value of the attribute of each grid cell and store the calculated attribute value in the map model;

[0123] Exemplarily, the unit attributes include a height attribute, a normal attribute, and a color attribute; wherein the color attribute is used to represent different water depths by different colors; it should be noted that the size of each grid unit can be flexibly set according to application requirements.

[0124] Specifically, the function expression of the height attribute is:

[0125]

[0126] In the formula, H p is the height attribute of the grid cell; N is the number of points in the grid cell, h i is the height value of the i-th point in the grid unit. Specifically, each grid unit includes multiple points, and each point has a height value;

[0127] The function expression of the height value is:

[0128] H max =max(h1,h2,...,h N )

[0129] H min =min(h1,h2,...,h N )

[0130] In the formula, H max and H min are the maximum and minimum height values ​​in the grid unit respectively; max and min are the maximum value function and the minimum value function respectively, h N is the height value of the point within the grid cell;

[0131] Specifically, the function expression of the normal attribute is:

[0132]

[0133] Where N f is the normal property of the mesh unit, Ni is the normal of the ith point in the grid cell, and N is the number of points in the grid cell;

[0134] Specifically, the function expression of the color attribute is:

[0135]

[0136] C max =max(max(R1,R2,...,R N ),max(G1,G2,...,G N ),max(B1,B2,...,B N ))

[0137] C min =min(min(R1,R2,...,R N ),min(G1,G2,...,G N ),min(B1,B2,...,B N ))

[0138] In the formula, C j , C max , C min are the average color, maximum color, and minimum color respectively; R i , G i , B i are the R value, G value, and B value of the RGB value of the i-th point in the grid unit, and N is the number of points in the grid unit.

[0139] In some embodiments, the gridded underwater environment map model is displayed in a three-dimensional visual environment, and the user can observe the underwater environment map model from different angles and perspectives through operations such as zooming, rotating, and translating, so as to better plan the route of the underwater robot.

[0140] S4: Combine the motion model of the underwater robot with the underwater environment map model to locate the current position of the robot.

[0141] In some embodiments, S4 includes the following specific steps:

[0142] Through the motion model of the underwater robot, the actual observation feature points of the underwater robot are obtained;

[0143] Perform descriptor matching on the feature points actually observed by the underwater robot and the feature points on the underwater environment map to obtain the similarity between the feature points;

[0144] The similarity of the feature points is compared with the preset similarity threshold. If the similarity of the feature points is greater than the similarity threshold, the two feature points are considered to match and the position of the underwater robot is determined. If the similarity of the feature points is less than the similarity threshold, the two feature points are considered to be mismatched, and the actual observed feature points are re-obtained and descriptor-matched with the feature points of the underwater environment map again.

[0145] The actual observation point includes the height value and water depth of the current position of the underwater robot; the underwater environment map feature point includes the height attribute and color attribute in the underwater environment map;

[0146] Specifically, the function expression of the descriptor matching is:

[0147]

[0148] Where D(p1,p2) is the similarity between the actual observed feature point and the underwater environment map feature point; p1 is the actual observed feature point; p2 is the underwater environment map feature point; d i is the descriptor of the ith feature.

[0149] Example 2

[0150] Figure 2 A structural block diagram of an intelligent positioning system for an underwater robot's running track according to an embodiment of the present specification is shown.

[0151] like Figure 2 As shown, an embodiment of the present invention provides an intelligent positioning system 100 for an underwater robot's running track, comprising:

[0152] The data acquisition module 101 is used to obtain the position data, posture data and underwater environment data of the underwater robot based on the sensors carried by the underwater robot;

[0153] A model building module 102 is used to build a motion model of the underwater robot based on the position data and posture data of the underwater robot, and predict the position and posture changes of the robot under a given control output;

[0154] A map generation module 103 is used to extract underwater landform feature data based on the underwater environment data of the underwater robot and construct an underwater environment map model;

[0155] The position determination module 104 is used to combine the motion model of the underwater robot with the underwater environment map model to locate the current position of the robot.

[0156] Example 3

[0157] As shown in the figure, an embodiment of the present invention provides a readable storage medium storing a computer program, wherein the computer program is called by a processor to implement:

[0158] The steps of a method for intelligently positioning the running track of an underwater robot are as described in Example 1.

[0159] It should be noted that the computer-readable storage medium of the present embodiment may be a computer-readable signal medium or a computer-readable storage medium or any combination of the above two. The 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 of the above. More specific examples of computer-readable storage media may include, but are not limited to: 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 of the above.

[0160] In this embodiment, the computer-readable storage medium may be any tangible medium containing or storing a program that can be used by or in combination with an instruction execution system, device, or device. In this embodiment, the computer-readable signal medium may include a data signal propagated in a baseband or as part of a carrier wave, which carries a computer-readable program. This propagated data signal may take a variety of forms, including but not limited to electromagnetic signals, optical signals, or any suitable combination of the above. The computer-readable signal medium may also be any computer-readable storage medium other than a computer-readable storage medium, which may send, propagate, or transmit a program used by or in combination with an instruction execution system, device, or device. The computer program contained on the computer-readable storage medium may be transmitted using any suitable medium, including but not limited to: wires, optical cables, RF (radio frequency), etc., or any suitable combination of the above.

[0161] The computer readable storage medium can be written in one or more programming languages ​​or a combination thereof to execute the computer program of the present embodiment, and the programming language includes an object-oriented programming language, such as Python, Java, C++, and also includes a conventional procedural programming language, such as C language or similar programming languages. The program can be executed entirely on the user's computer, partially on the user's computer, as an independent software package, partially on the user's computer and partially on a remote computer, or completely on a remote computer or server. In the case of a remote computer, the remote computer can 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 can be connected to an external computer (e.g., using an Internet service provider to connect through the Internet).

[0162] In summary, the present invention can realize high-precision positioning of the underwater robot's operating position in a complex and changeable underwater environment by analyzing and comparing the similarity between the underwater robot's location and the natural feature points of the underwater landform, ensuring the accuracy and reliability of the underwater robot in performing tasks, and expanding the application scope and operating depth of the underwater robot. The present invention can not only obtain and process the three-dimensional information of the underwater environment in real time by carrying a sonar sensor, but also identify and track feature points through the reflection characteristics of the sonar signal, thereby realizing high-precision position estimation, so that the positioning system can maintain good working performance under different water qualities, different lighting conditions and different water depths, greatly improving the flexibility and applicability of the system. The high-precision positioning information provided by the present invention can provide a solid foundation for the autonomous navigation and intelligent decision-making of the underwater robot. The underwater robot can plan the optimal path according to the real-time position information, combined with the task requirements and environmental characteristics, and perform complex tasks such as obstacle avoidance, exploration, and monitoring, thereby improving the intelligence level and efficiency of underwater operations.

[0163] The above description is only a preferred embodiment of the present invention. It is obvious to those skilled in the art that the present invention is not limited to the details of the above exemplary embodiments, and the present invention can be implemented in other specific forms without departing from the spirit or basic features of the present invention. Therefore, no matter from which point of view, the embodiments should be regarded as exemplary and non-restrictive. The scope of the present invention is defined by the appended claims rather than the above description, and it is intended that all changes falling within the meaning and scope of the equivalent elements of the claims are included in the present invention.

Claims

1. A method for intelligently positioning the running track of an underwater robot, characterized in that: The following steps are involved: S1: Based on the sensors carried by the underwater robot, the position data, posture data and underwater environment data of the underwater robot are obtained; S2: Based on the position data and posture data of the underwater robot, a motion model of the underwater robot is constructed, and the position and posture changes of the robot under given control output are predicted; S3: Based on the underwater environment data of the underwater robot, extract the underwater landform feature data and build an underwater environment map model; S4: Match the motion model of the underwater robot with the underwater environment map model to locate the current position of the underwater robot.

2. The method according to claim 1, characterized in that The sensors in S1 include sonar sensors, light scanners, and inertial sensors; The sonar sensor is used to calculate the position data of the underwater robot through the time difference of the sound waves going back and forth in the water; the position data is the distance between the underwater robot and the boundary of the water area; The inertial sensor is used to measure the angular velocity and acceleration changes in different directions, and calculate the posture data of the robot according to the inertial sensor measurement values; The light scanner is used to collect underwater environment data where the robot is located.

3. The method according to claim 1, characterized in that Functional expression of the position data: d is =0.5*sv*T time Where, d is is the distance data, sv is the speed of sound, T time It is double the time difference; Functional expression of the posture data: pitch=atan2(Y,sqrt(X 2 +Z 2 )) roll=atan2(X,Z) yaw=a tan 2(X,Y) Where roll is the roll angle of the robot, pitch is the pitch angle of the robot, yaw is the yaw angle of the robot; atan2 is the inverse tangent value, Y is the Y-axis value of the inertial sensor, X is the X-axis value of the inertial sensor, and Z is the Z-axis value of the inertial sensor; sqrt is used to calculate the non-negative square root.

4. The method according to claim 1, characterized in that: The S2 comprises the following specific steps: S201, taking the center of the underwater robot as the point of the coordinate system, determining the directions of three perpendicularly intersecting coordinate axes, and constructing a coordinate system; wherein the directions of the three coordinate axes are represented as x, y, and z respectively; S202, based on the position data and attitude data of the underwater robot, obtaining the position coordinates and attitude angle of the underwater robot at the current moment; S203, constructing a motion model of the underwater robot based on the position coordinates and posture angle of the underwater robot at the current moment; The underwater robot motion model includes a position update model and a posture update model; S204: Introduce a control output formula into the robot motion model to predict the position change and posture change of the robot under a given control output.

5. The method according to claim 4, characterized in that The function expression of the position update model is: x(t+Δt)=x(t)+v*cos(yaw)*Δt y(t+Δt)=y(t)+v*sin(yaw)*Δt z(t+Δt)=z(t)+v z *Δt Where x(t) is the horizontal coordinate of the underwater robot at the current moment, y(t) is the vertical coordinate of the underwater robot at the current moment, and z(t) is the vertical coordinate of the underwater robot at the current moment; x(t+Δt) is the horizontal coordinate of the underwater robot after a time of Δt, y(t+Δt) is the vertical coordinate of the underwater robot after a time of Δt, and z(t+Δt) is the vertical coordinate of the underwater robot after a time of Δt; Δt is the time step of the underwater robot; v is the speed of the underwater robot at the current moment, v Z is the vertical speed of the underwater robot at the current moment; yaw is the yaw angle of the underwater robot at the current moment, cos(yaw) is the cosine value of the yaw angle, and sin(yaw) is the sine value of the yaw angle; The functional expression of the posture update formula is: roll(t+Δt)=roll(t)+ang_v_x*Δt pitch(t+Δt)=pitch(t)+ang_v_y*Δt yaw(t+Δt)=yaw(t)+ang_v_z*Δt Among them, roll(t) is the roll angle of the underwater robot at the current moment, pitch(t) is the pitch angle of the underwater robot at the current moment, and yaw(t) is the yaw angle of the underwater robot at the current moment; roll(t+Δt) is the roll angle of the underwater robot after a period of time Δt, pitch(t+Δt) is the pitch angle of the underwater robot after a period of time Δt, and yaw(t+Δt) is the yaw angle of the underwater robot after a period of time Δt; Δt is the time step of the underwater robot's rotation; ang_v_x is the angular velocity of the underwater robot rotating around the x-axis at the current moment, ang_v_y is the angular velocity of the underwater robot rotating around the y-axis at the current moment, and ang_v_z is the angular velocity of the underwater robot rotating around the z-axis at the current moment; The control output formula is: v k =f(v_c) In the formula, v k is the control output; v_c is the linear velocity control value of the underwater robot at the current moment; f is the actual linear velocity of the underwater robot.

6. The method according to claim 1, characterized in that The S3 comprises the following specific steps: Based on the underwater environment data, extracting characteristic data describing the underwater landform; The extracted feature data are combined to establish an underwater environment map model; Divide the area of ​​the underwater environment map model into a number of groups of grid cells according to fixed sizes; Calculate the value of the attribute of each grid cell, and store the calculated attribute value in the underwater environment map model; The unit attributes include a height attribute, a normal attribute, and a color attribute, and the color attribute is used to represent different water depths through different colors.

7. The method according to claim 6, characterized in that The function expression of the height attribute is: In the formula, H p is the height attribute of the grid cell; N is the number of points in the grid cell, h i is the height value of the i-th point in the grid unit. Specifically, each grid unit includes multiple points, and each point has a height value; The function expression of the normal attribute is: Where N f is the normal property of the mesh unit, N i is the normal of the ith point in the grid cell, and N is the number of points in the grid cell; The function expression of the color attribute is: C max =max(max(R1,R2,...,R N ),max(G1,G2,...,G N ),max(B1,B2,...,B N )) C min =min(min(R1,R2,...,R N ),min(G1,G2,...,G N ),min(B1,B2,…,B N )) In the formula, C j , C max , C min are the average color, maximum color, and minimum color respectively; R i , G i , B i are the R value, G value, and B value of the RGB value of the i-th point in the grid unit, and N is the number of points in the grid unit.

8. The method according to claim 1, characterized in that The S4 comprises the following specific steps: Obtain the actual observation point of the underwater robot through the motion model of the underwater robot; Perform descriptor matching between the actual observation points of the underwater robot and the feature points of the underwater environment map to obtain the similarity of the feature points between the two; The similarity of the feature points is compared with the preset similarity threshold. If the similarity of the feature points is greater than the similarity threshold, the two feature points are considered to match and the position of the underwater robot is determined. If the similarity of the feature points is less than the similarity threshold, the two feature points are considered to be mismatched, and the actual observation points are re-obtained and descriptor-matched with the feature points of the underwater environment map again. The actual observation point includes the location data of the current location of the underwater robot; the underwater environment map feature point includes the unit attribute value in the underwater environment map.

9. The method according to claim 8, characterized in that The function expression of the descriptor matching is: Where D(p1,p2) is the similarity between the actual observation point and the feature point of the underwater environment map; p1 is the actual observation point; p2 is the feature point of the underwater environment map; d i is the descriptor of the ith feature.

10. An intelligent positioning system for the running track of an underwater robot, characterized in that: include: A data acquisition module is used to obtain the position data, posture data and underwater environment data of the underwater robot based on the sensors carried by the underwater robot; A model building module is used to build a motion model of the underwater robot based on the position data and posture data of the underwater robot, and predict the position and posture changes of the robot under a given control output; A map generation module is used to extract underwater landform feature data based on the underwater environment data of the underwater robot and construct an underwater environment map model; The position determination module is used to match the motion model of the underwater robot with the underwater environment map model to locate the current position of the underwater robot.

Images