A dynamic height control method for underwater robot based on DVL information

Abstract

The present invention proposes a method for dynamic altitude control of an underwater robot based on DVL information, which belongs to the field of underwater robot control technology. The method includes: estimating the horizontal distance and vertical water depth of the beam detection area according to the real-time information of two forward beams of DVL and the real-time inclination information of the underwater robot; fitting the historical horizontal distance and vertical water depth of the forward area by a quadratic polynomial, and generating an altitude trajectory in combination with the expected altitude; discretizing the fitted trajectory, and determining the preview distance in combination with the expected speed and the comprehensive curvature, and smoothly adjusting the preview distance by a moving average method; calculating the expected pitch angle of the underwater robot according to the longitudinal error between the preview point position and the current position, adjusting the inclination of the underwater robot in real time, and realizing accurate altitude path tracking. By implementing the method of the embodiment of the present invention, the underwater robot can be controlled to track the undulating navigation of the seabed with high precision and high safety, so as to improve its near-seabed altitude observation capability.

Description

Technical Field

[0001] The present invention relates to the technical field of underwater robot control, and in particular to a dynamic height-fixing control method for an underwater robot based on DVL information. Background Art

[0002] Underwater robots are widely used in marine resource exploration, marine scientific research, marine engineering and other fields. In many tasks, underwater robots need to observe or operate close to the seabed, which requires underwater robots to have high-precision altitude-fixed navigation capabilities.

[0003] Traditional methods of determining the height of underwater robots mainly rely on sonar or pressure sensors. Although sonar ranging can directly measure the distance to the seabed, it is affected by the speed of sound wave propagation and the seawater environment, and its accuracy is limited. Although pressure sensors can accurately measure water depth, they cannot directly reflect the relative distance to the seabed, especially when the seabed terrain is complex.

[0004] A Doppler velocimeter (DVL) is a device that uses the Doppler effect to measure the speed of an underwater robot relative to the seabed or water body. A DVL usually contains multiple acoustic beams and can measure speed and distance information in multiple directions simultaneously.

[0005] However, most of the current underwater robot navigation methods using DVL mainly focus on position estimation and track planning. There is still little research on achieving high-precision altitude navigation in complex seabed terrain. Especially in the case of large fluctuations in the seabed terrain, how to use the information provided by DVL to predict the terrain ahead and adjust the posture of the underwater robot accordingly to achieve safe and efficient altitude navigation is still a problem to be solved.

[0006] Therefore, a method is needed to make full use of the information provided by DVL to achieve high-precision and high-safety altitude navigation of underwater robots, so as to improve the operating ability of underwater robots in complex seabed environments. Summary of the invention

[0007] The purpose of the present invention is to provide a method for dynamic altitude control of an underwater robot based on DVL information to solve the problems existing in the prior art.

[0008] The method of the present invention comprises the following steps:

[0009] a) Estimate the horizontal distance and vertical water depth of the beam detection area based on the real-time information of the two forward beams of the DVL and the real-time inclination information of the underwater robot;

[0010] b) Use quadratic polynomial to fit the historical horizontal distance and vertical water depth of the forward area, and generate a fixed altitude trajectory in combination with the expected fixed altitude;

[0011] c) discretizing the fitted fixed-altitude trajectory;

[0012] d) Determine the preview distance based on the expected speed and comprehensive curvature, and smoothly adjust the preview distance by moving average;

[0013] e) The longitudinal error preview control strategy is adopted to adjust the inclination angle of the underwater robot in real time to achieve precise altitude path tracking.

[0014] The beneficial effects of the present invention are:

[0015] Using the real-time and historical information provided by DVL, the seabed terrain ahead can be effectively estimated, improving the underwater robot's ability to adapt to complex terrain;

[0016] The preview distance is dynamically adjusted based on the comprehensive curvature and expected speed, which enhances the robustness of the system.

[0017] By setting the preview point, the aircraft can respond to the terrain ahead in advance, thus improving the safety and stability of altitude-fixed navigation.

[0018] The inclination angle of the underwater robot is adjusted based on the longitudinal error between the current position and the preview point, thus achieving accurate tracking of the target at a fixed height. BRIEF DESCRIPTION OF THE DRAWINGS

[0019] The accompanying drawings are used to provide an understanding of the technical solution of the present disclosure and constitute a part of the specification. Together with the embodiments of the present disclosure, they are used to explain the technical solution of the present disclosure and do not constitute a limitation on the technical solution of the present disclosure.

[0020] Figure 1 Flow chart of the method of the present invention.

[0021] Figure 2 Schematic diagram of DVL forward beam detection.

[0022] Figure 3 Schematic diagram of underwater robot preview point selection and tilt angle adjustment. DETAILED DESCRIPTION

[0023] The present disclosure describes multiple embodiments, but the description is exemplary rather than restrictive, and it is obvious to those skilled in the art that there may be more embodiments and implementations within the scope of the embodiments described in the present disclosure. Although many possible feature combinations are shown in the drawings and discussed in the specific embodiments, many other combinations of the disclosed features are also possible.

[0024] The specific implementation modes of the present invention are described in detail below with reference to the accompanying drawings.

[0025] like Figure 1 As shown, the embodiment of the present application provides a method for controlling a dynamic height of an underwater robot based on DVL information, the method comprising the following steps:

[0026] Step 1: Get DVL information

[0027] 1.1 Working Principle of DVL

[0028] A Doppler velocimeter (DVL) is an acoustic device that measures the speed of an underwater robot relative to the seabed or water body by emitting and receiving sound waves. Its four measurement beams can effectively detect the distance to the bottom of the water or other reflectors and return to the device. In the embodiment of the present application, the vertical angle of each beam distance is 30°.

[0029] 1.2 Data Collection

[0030] DVL transmits sound waves at a fixed frequency (such as 2Hz) and receives echo signals. Each measurement can obtain the previous two beam length information. and .

[0031] 1.3 Calculation of horizontal distance and vertical water depth

[0032] like Figure 2 As shown, the fixed beam angle of DVL is 30°, and the inclination information of the current underwater robot AUV relative to the horizontal , counterclockwise is positive, calculate the horizontal distance and vertical water depth , the formula is: , .

[0033] Step 2: Seafloor topography fitting

[0034] 2.1 Data Preprocessing

[0035] Outlier detection and filtering are performed on the seafloor topography data obtained by DVL to remove possible noise points.

[0036] In one example, median filtering or Kalman filtering can be used. In addition, median filtering is used to select the window value. , for each point Find the total points (if on the boundary, use a partial window), calculate the median of these points, and use it as the new ,For the points near the beginning or end of the data set, methods such as symmetric filling and ,repeating boundary points can be used.

[0037] 2.2 Quadratic polynomial fitting

[0038] Use the least squares method to fit a quadratic polynomial to the preprocessed data, that is, find a quadratic polynomial (in, is the vertical water depth, is the horizontal distance, , , is the fitting coefficient), so that the polynomial fits the above data points as much as possible, that is, minimizes the following objective function:

[0039]

[0040] Expand the objective function and construct the matrix equation: ,in is the height vector, is the coefficient matrix, that is, the parameter vector to be solved , is the design matrix, solve for the coefficients That is, the fitted quadratic polynomial is obtained.

[0041] Further, the fitting effect is evaluated

[0042] Calculate the root mean square error (RMSE) between the fitted curve and the actual data points. If the RMSE exceeds the preset threshold, consider increasing the polynomial order or using piecewise fitting.

[0043] Step 3: Trajectory Generation

[0044] Underwater robots usually navigate at a fixed altitude, and the altitude does not change. The method to generate a fixed-height trajectory with the fitted quadratic polynomial is: ,in, is the height of the generated fixed-altitude trajectory, is the horizontal distance. The expected trajectory is the trajectory that the underwater robot expects to track.

[0045] Step 4: Trajectory discretization

[0046] The method of discretizing the fitted trajectory is to convert the continuous trajectory function Sampling at equally spaced horizontal distance points to obtain a discrete trajectory point set ,in , is the number of sampling points. The discretized trajectory points can be referred to Figure 3 The black dots in the

[0047] Step 5: Determine the pre-aiming distance

[0048] 5.1 Curvature calculation

[0049] Curvature is a geometric quantity that describes the curvature of a curve. For a plane curve, curvature geometrically reflects the bending speed of the curve at a certain point.

[0050] Specifically, the greater the curvature, the more curved the curve is; the smaller the curvature, the flatter the curve is. The formula for calculating the curvature of each discrete path point is: .

[0051] Comprehensive curvature , can describe the smoothness of this trajectory. If it is smaller, it is relatively flat, and the pre-aiming distance can be appropriately enlarged. The path is determined by the weighted average of the curvature of each path point after discretization, and the formula is: , For the The curvature of the path points, is the corresponding weight.

[0052] 5.2 Preview distance determination

[0053] Preview distance and expected speed Proportional to the combined curvature Inversely proportional to the current preview distance , the formula is: ,in, is the proportionality constant, A small positive number used to prevent the preview distance from being too long when the curvature is too small.

[0054] 5.3 Preview distance smoothing

[0055] Use the moving average algorithm to smooth the current preview distance to obtain the updated preview distance , the formula is: in, is the preview distance calculated based on the current curvature and speed. is the last pre-aiming distance, is the smoothing factor, between 0 and 1.

[0056] Step 6: Tilt Control

[0057] 6.1 Determine the preview point

[0058] like Figure 3 As shown, according to the updated preview distance, the discrete trajectory point set is traversed , select the point with the smallest Euclidean distance as the preview point .

[0059] 6.2 Tangent direction angle calculation

[0060] like Figure 3 As shown, the tangent direction angle of the path Preview point Under the premise of quadratic fitting, the angle between the tangent direction at the current position and the horizontal axis is calculated as follows: .

[0061] 6.3 Calculation of longitudinal error

[0062] like Figure 3 As shown, the longitudinal error between the current position (x, h) of the underwater robot and the preview point , which can describe the distance of the underwater robot relative to the tangent path, is calculated as follows: .

[0063] 6.4 Calculation of expected tracking angle

[0064] Get the preview heading angle based on the updated preview distance , combined with the longitudinal error , calculate the expected tracking inclination of the underwater robot , the control law is: in, is the proportional control gain of the longitudinal error.

[0065] The embodiment of the present application may also include the following steps:

[0066] Real-time monitoring and adjustment of steps to adapt to complex seabed environmental changes;

[0067] as well as,

[0068] The step of dynamically adjusting the control parameters according to the motion state and environmental characteristics of the underwater robot, including adjusting the proportional constant in the preview distance calculation and smoothing factor , and the gain in longitudinal error control .

[0069] Through the above detailed steps, the embodiment of the present application can realize accurate dynamic altitude control of the underwater robot. The method comprehensively utilizes DVL information, seabed terrain fitting, trajectory planning and adaptive control technologies, has high accuracy and robustness, and can adapt to complex seabed environments.

[0070] Obviously, the above embodiments of the present invention are merely examples for clearly illustrating the present invention, and are not limitations on the embodiments of the present invention. For those skilled in the art, other different forms of changes or modifications can be made based on the above description. It is impossible to list all the embodiments here. All obvious changes or modifications derived from the technical solution of the present invention are still within the protection scope of the present invention.

Claims

1. A method for dynamic height control of an underwater robot based on DVL information, characterized in that: The following steps are involved: a) Estimate the horizontal distance and vertical water depth of the beam detection area based on the real-time information of the two forward beams of the DVL and the real-time inclination information of the underwater robot; b) Use quadratic polynomial to fit the historical horizontal distance and vertical water depth of the forward area, and generate a fixed altitude trajectory in combination with the expected fixed altitude; c) discretizing the fitted fixed-altitude trajectory; d) Determine the preview distance based on the expected speed and comprehensive curvature, and smoothly adjust the preview distance by moving average; e) Using the preview control strategy of longitudinal error, the inclination angle of the underwater robot is adjusted in real time to achieve precise altitude path tracking; in: In step b), the quadratic polynomial fitting method adopts the least squares method; Step c) specifically comprises: sampling the continuous trajectory function at equally spaced horizontal distance points to obtain a discrete trajectory point set; The comprehensive curvature in step d) is determined by calculating the weighted average of the curvatures of the first few path points after discretization; The preview distance in step d) is calculated as follows: ； in is the proportionality constant, is the expected speed, is the comprehensive curvature, A small positive number, used to prevent the preview distance from being too long when the curvature is too small; The preview control strategy described in step e) is specifically: According to the preview point determined by the preview distance, the current position and the path tangent direction, the longitudinal error between the current position of the underwater robot and the preview point is calculated; According to the heading angle determined by the preview distance, the expected inclination angle of the underwater robot is calculated in combination with the longitudinal error; The preview point determined according to the preview distance is specifically: According to the updated preview distance, traverse the discrete trajectory point set and select the point with the smallest Euclidean distance as the preview point; The path tangent direction is described by a tangent direction angle, wherein the tangent direction angle is the angle between the tangent direction of the preview point at the current position and the horizontal axis; The longitudinal error is calculated as follows: ； Among them is The vertical coordinate of the preview point, Set high expectations. is the horizontal coordinate of the preview point, is the horizontal distance, is the tangent direction angle.

2. The method according to claim 1, characterized in that The horizontal distance and vertical water depth of the estimated beam detection area in step a) are specifically: Combining the real-time inclination information of the underwater robot and the lengths of the two beams of the DVL, the distance of the beam detection point is decomposed into horizontal distance and vertical water depth.

Images