CN-122018516-A - Stable navigation control method, system, equipment and medium for autonomous underwater robot

CN122018516ACN 122018516 ACN122018516 ACN 122018516ACN-122018516-A

Abstract

The application relates to the technical field of autonomous underwater robot navigation control, and discloses a stable navigation control method, system, equipment and medium for an autonomous underwater robot. The autonomous underwater robot can realize navigation control of stable navigation attitude while keeping the autonomous underwater robot in the optimal height range through the cooperative work of modules such as a depth gauge, an altimeter, a Doppler velocimeter, a single-beam forward-looking sonar, a navigation attitude measuring instrument and a central controller which are carried by the autonomous underwater robot. By adopting self-adaptive sampling frequency adjustment, gradient estimation of a dynamic sliding window and navigation attitude control based on terrain gradient compensation, the problems of difficult terrain tracking, unstable bottom height and the like caused by weak mobility of the under-actuated autonomous underwater robot and large sensor measurement noise under complex fluctuation terrain are solved, and the method is low in hardware cost, high in control precision and strong in robustness, and can be widely applied to various underwater sweeping operation scenes.

Inventors

LU XIAOTING
LI YUYANG
YU JIANCHENG
WANG BING
QIAO JIANAN
WANG ZHENYU
HU FENG
ZHAO BAODE
WANG FUHAI

Assignees

中国科学院沈阳自动化研究所

Dates

Publication Date: 20260512
Application Date: 20260415

Claims (10)

1. A stable navigational control method of an autonomous underwater robot, comprising: Acquiring depth data, off-bottom height data, navigational speed data, navigational attitude angle data and forward looking distance data of each sampling point; Determining the terrain relief height and the accumulated forward sailing distance of each sampling point and the terrain relief height and the accumulated forward sailing distance of the front detection point according to the depth data, the off-bottom height data, the navigational speed data, the navigational attitude angle data and the forward looking distance data; Estimating the terrain gradient result of the sampling point at the current moment according to the terrain fluctuation height and the accumulated forward navigation distance of each sampling point and the terrain fluctuation height and the accumulated forward navigation distance of the front detection point; Predicting the predicted off-bottom height of the future preset duration according to the terrain gradient result of the sampling point at the current moment, off-bottom height data of the sampling point at the current moment, navigational speed data of the sampling point at the current moment and navigational attitude angle data of the sampling point at the current moment; If the predicted off-bottom height exceeds the optimal height range of the autonomous underwater robot, determining a pitching control angle according to the target off-bottom height of the optimal height range, off-bottom height data of the sampling point at the current moment, a preset proportionality coefficient and a terrain gradient result of the sampling point at the current moment, and continuously controlling the autonomous underwater robot to navigate according to the pitching control angle; If the predicted off-bottom height does not exceed the optimal height range of the autonomous underwater robot, continuing to control the autonomous underwater robot to navigate according to the navigation attitude angle data of the sampling point at the current moment.
2. The method for controlling stable navigation of an autonomous underwater vehicle according to claim 1, wherein the acquiring depth data, off-bottom height data, navigational speed data, navigational attitude angle data, and forward looking distance data of each sampling point comprises: acquiring depth data continuously acquired according to sampling frequency based on depth gauge carried by autonomous underwater robot ; Acquiring off-bottom height data continuously acquired by an altimeter carried by an autonomous underwater robot according to sampling frequency ; Acquiring navigational speed data continuously acquired according to sampling frequency based on Doppler velocimeter carried by autonomous underwater robot ; Acquiring navigation attitude angle data continuously acquired by a navigation attitude measuring instrument carried by an autonomous underwater robot according to sampling frequency ; Acquiring single-beam forward looking sonar continuously acquired forward looking distance data according to sampling frequency based on autonomous underwater robot ; Wherein, the The sampling point at the current moment is indicated, A previous sampling time node representing the sampling point at the current time, The number of sampling points is indicated, Depth data representing the sampling point at the current time, Off-bottom height data representing the sampling point at the current time, The navigational speed data representing the sampling point at the current moment, The voyage angle data representing the sampling point at the current moment, And the forward looking distance data of the sampling point at the current moment is represented, and the sampling frequency is adaptively adjusted according to the navigational speed data of the autonomous underwater robot and the terrain gradient result.
3. The stable voyage control method of an autonomous underwater robot according to claim 2, wherein the sampling frequency is adaptively adjusted according to voyage data and a terrain gradient result of the autonomous underwater robot, comprising: calculating an initial sampling time interval between a sampling point at the next time and a sampling point at the previous time based on the following formula according to the navigational speed data of the sampling point at the previous time and a preset correlation coefficient; , Wherein, the Representing sample point index and , Representing the preset correlation coefficient(s) of the signal, Represent the first The navigational speed data of the individual sampling points, Represent the first The sampling point is the first An initial sampling time interval of the sampling points; If the absolute value of the terrain gradient result of the sampling point at the previous moment does not exceed the preset gradient threshold value, taking the initial sampling time interval as a sampling time interval, and determining the sampling point at the next moment; if the absolute value of the terrain gradient result of the sampling point at the previous moment exceeds a preset gradient threshold value, calculating the updated sampling time interval between the sampling point at the next moment and the sampling point at the previous moment based on the following formula; , Wherein, the Indicating the preset multiplying power and , Represent the first The sampling point is the first Updating sampling time intervals of the sampling points; Taking the updated sampling time interval as the sampling time interval, and determining the sampling point at the next moment.
4. The stable voyage control method of an autonomous underwater vehicle according to claim 2, wherein the determining of the terrain relief height and the accumulated forward voyage distance for each sampling point and the terrain relief height and the accumulated forward voyage distance for the forward detection point based on the depth data, the off-bottom height data, the voyage speed data, the voyage angle data, and the forward viewing distance data includes: according to the depth data, the off-bottom height data and the preset false full sea depth, calculating the relief height of the terrain of each sampling point based on the following formula; , Wherein, the Representing a predetermined false full sea depth and , Represent the first The relief height of the topography at the individual sampling points, Represent the first Depth data of the individual sample points, Represent the first Off-bottom height data of the sampling points; Calculating the topography relief height of a front detection point based on the following formula according to the depth data of the sampling point at the current moment, the voyage angle data of the sampling point at the current moment, the forward looking distance data of the sampling point at the current moment, the preset false full sea depth and the preset fixed installation angle of the single-beam forward looking sonar; , Wherein, the Representing a preset fixed mounting angle of the single-beam forward looking sonar, A terrain relief height representing the forward detection point; according to the navigational speed data, the sampling time interval and the navigational attitude angle data, calculating the accumulated forward navigational distance of each sampling point based on the following formula; , Wherein, the Represent the first The navigational speed data of the individual sampling points, Represent the first The sampling point is the first The sampling time interval of the individual sampling points, Represent the first The navigation attitude angle data of the sampling points, Represent the first The cumulative forward travel distance of the individual sample points, Represent the first Cumulative forward travel distance of each sampling point and ; Calculating the accumulated forward navigation distance of the front detection point based on the following formula according to the accumulated forward navigation distance of the current time sampling point, the forward sight distance data of the current time sampling point, the voyage angle data of the current time sampling point and the preset fixed installation angle of the single-beam forward sight sonar; , Wherein, the Indicating the cumulative forward travel distance of the forward probe point.
5. The stable voyage control method of an autonomous underwater vehicle according to claim 1, wherein estimating a terrain gradient result of a sampling point at a current time based on a terrain relief height and an accumulated forward voyage distance of each sampling point and a forward detection point comprises: Correspondingly selecting data samples from the topographic relief heights and the accumulated forward sailing distances of all the sampling points and the front detection points based on a dynamic sliding window, wherein the number of the data samples in the dynamic sliding window is determined according to the navigational speed data of the sampling point at the current moment of the autonomous underwater robot and the topographic gradient result of the sampling point at the previous moment, and the data samples comprise historical sampling points, the sampling point at the current moment and the front detection points; Removing wild values of the topographic relief heights in the data samples by adopting a quartile range jump point removing method, wherein the reserved topographic relief heights and reserved accumulated forward navigation distances correspond to form effective data samples; And taking the accumulated forward sailing distance of the effective data sample as an independent variable and the terrain fluctuation height of the effective data sample as a dependent variable, and fitting the change rate by using a random sample consistency algorithm to obtain the terrain gradient result of the sampling point at the current moment.
6. The method for controlling stable navigation of autonomous underwater robot according to claim 2, wherein predicting the predicted off-bottom height of the future preset duration based on the terrain gradient result of the current time sampling point, off-bottom height data of the current time sampling point, navigational speed data of the current time sampling point, and navigational attitude angle data of the current time sampling point comprises: calculating the theoretical horizontal distance of the autonomous underwater robot based on the following formula according to the navigational speed data of the sampling point at the current moment, the navigational attitude angle data of the sampling point at the current moment and the future preset duration; , Wherein, the Indicating a preset time period in the future, A theoretical horizontal range representing a future preset duration; Calculating the theoretical vertical range of the autonomous underwater robot based on the following formula according to the navigational speed data of the sampling point at the current moment, the navigational attitude angle data of the sampling point at the current moment and the future preset duration; , Wherein, the A theoretical vertical range representing a future preset duration; according to the theoretical horizontal distance and the terrain gradient result of the sampling point at the current moment, calculating the terrain height variation of the future preset duration based on the following formula; , Wherein, the Representing the result of the terrain gradient at the sampling point at the current moment, The terrain height variation quantity representing the future preset duration; Calculating the predicted bottom-off height of the future preset duration based on the following formula according to the bottom-off height data of the sampling point at the current moment, the theoretical vertical distance and the terrain height variation; , Wherein, the A predicted off-bottom height representing a future preset time period.
7. The stable navigation control method of the autonomous underwater vehicle according to claim 6, wherein the determining the trim control angle according to the target off-bottom height of the optimal height range, off-bottom height data of the sampling point at the current time, a preset scaling factor, and a terrain gradient result of the sampling point at the current time includes: Determining a target off-bottom height based on the optimal height range; calculating a height difference based on the following formula according to the target bottom-off height and the bottom-off height data of the sampling point at the current moment; , Wherein, the Representing the height of the target from the bottom, Representing a height difference; Calculating a pitching control angle based on the following formula according to the height difference, a preset proportion coefficient and a terrain gradient result of a sampling point at the current moment; , Wherein, the Representing a preset scale factor and , Representing the pitch control angle.
8. A stable navigational control system of an autonomous underwater robot comprising: the information sensing module is used for acquiring depth data, off-bottom height data, navigational speed data, navigational attitude angle data and forward looking distance data of each sampling point; the parameter calculation module is used for determining the terrain relief height and the accumulated forward sailing distance of each sampling point and the terrain relief height and the accumulated forward sailing distance of the front detection point according to the depth data, the off-bottom height data, the navigational speed data, the navigational gesture angle data and the forward looking distance data; The gradient estimation module is used for estimating the terrain gradient result of the sampling point at the current moment according to the terrain fluctuation height and the accumulated forward navigation distance of each sampling point and the terrain fluctuation height and the accumulated forward navigation distance of the front detection point; The height prediction module is used for predicting the predicted off-bottom height of the future preset duration according to the terrain gradient result of the sampling point at the current moment, off-bottom height data of the sampling point at the current moment, navigational speed data of the sampling point at the current moment and navigational attitude angle data of the sampling point at the current moment; The navigation attitude control module is used for determining a trim control angle according to the target off-bottom height of the optimal height range, off-bottom height data of the sampling point at the current moment, a preset proportionality coefficient and a terrain gradient result of the sampling point at the current moment if the predicted off-bottom height exceeds the optimal height range of the autonomous underwater robot, and continuing to control the autonomous underwater robot to navigate according to the trim control angle; And the navigation attitude control module is also used for continuously controlling the autonomous underwater robot to navigate according to the navigation attitude angle data of the sampling point at the current moment if the predicted off-bottom height does not exceed the optimal height range of the autonomous underwater robot.
9. An electronic device comprising a memory, a processor and a computer program stored in the memory and executable on the processor, characterized in that the processor, when executing the computer program, realizes the steps of the method for stable navigation control of an autonomous underwater vehicle according to any of claims 1 to 7.
10. A computer-readable storage medium storing a computer program, characterized in that the computer program, when executed by a processor, implements the steps of the stable voyage control method of an autonomous underwater robot according to any of claims 1 to 7.

Description

Stable navigation control method, system, equipment and medium for autonomous underwater robot Technical Field The application relates to the technical field of autonomous underwater robot navigation control, in particular to a stable navigation control method, a system, equipment and a medium for an autonomous underwater robot. Background In the underwater near-bottom mapping operation process, an autonomous underwater robot is generally provided with various detection devices such as acoustic load, optical load and the like and is used for accurately mapping and collecting data of objects such as submarine topography, mineral resources, historical sunken ships and the like. According to the operation characteristic requirement of the carrying load, the autonomous underwater robot needs to keep a relatively fixed opposite-bottom height with the seabed in the operation process, so that the seabed target is always in an effective detection range of the operation load, and the accuracy and the integrity of detection data are ensured. However, the near-bottom operation sea area requiring the autonomous underwater robot often faces complicated submarine topography conditions, most sea areas have remarkable fluctuation changes instead of flat topography, and meanwhile, due to the characteristics of a platform design and a power system, the mobility of the underactuated autonomous underwater robot is relatively weak, and is difficult to quickly respond to topography changes. Therefore, how to control the autonomous underwater robot to accurately track the change of the terrain with complex fluctuation and always keep the stable bottom-to-bottom height becomes an important technical challenge facing the current near-bottom mapping operation field of the autonomous underwater robot. The current research trend of autonomous underwater robot terrain tracking control is to gradually turn to a self-adaptive and predictive control mode based on real-time environment perception from a static tracking mode depending on an accurate priori map. However, the prior art still has the defects that the partial control method has higher requirements on hardware equipment, depends on a high-precision and high-cost sensor, is unfavorable for popularization and application of the technology, the gradient estimation precision is insufficient to cause hysteresis in the high prediction and the navigation attitude control under the scene of severe change of the gradient of the terrain, and the suitability of the sampling frequency, the navigation speed and the gradient of the terrain is not fully considered by some control strategies, so that the problems of untimely data sampling or redundancy are easy to occur under the complex terrain condition, and the control effect is influenced. Therefore, there is a need for an autonomous underwater robot stable navigation control method with low hardware cost, accurate gradient estimation, timely height prediction, stable navigation attitude control, and capability of adapting to complex fluctuation topography changes, so as to meet the requirements of actual near-bottom mapping operation, and promote further application of the autonomous underwater robot in the field of ocean exploration. Disclosure of Invention In order to solve the problems, the embodiment of the application provides a stable navigation control method, a system, equipment and a medium for an autonomous underwater robot, which are used for realizing that the bottom height in the navigation process of the autonomous underwater robot is always kept in the optimal height range for carrying load work and ensuring stable navigation attitude through optimizing a control flow. The embodiment of the application adopts the following technical scheme: In a first aspect, the present application provides a stable navigation control method for an autonomous underwater robot, comprising: Acquiring depth data, off-bottom height data, navigational speed data, navigational attitude angle data and forward looking distance data of each sampling point; Determining the terrain relief height and the accumulated forward sailing distance of each sampling point and the terrain relief height and the accumulated forward sailing distance of the front detection point according to the depth data, the off-bottom height data, the navigational speed data, the navigational attitude angle data and the forward looking distance data; Estimating the terrain gradient result of the sampling point at the current moment according to the terrain fluctuation height and the accumulated forward navigation distance of each sampling point and the terrain fluctuation height and the accumulated forward navigation distance of the front detection point; Predicting the predicted off-bottom height of the future preset duration according to the terrain gradient result of the sampling point at the current moment, off-bottom height data of the sampling point at the current moment,