Underwater remote control submersible and space positioning method thereof

Abstract

The invention provides an underwater remote control submersible and a spatial positioning method thereof, and the spatial positioning method comprises the steps: collecting the real-time attitude data of an ROV and the distance data between the ROV and the surrounding space through employing a sonar matrix and an inertial measurement unit IMU which are disposed on the ROV, and calculating the sonar data of the relative position between the ROV and the surrounding space; and estimating the position and attitude state of the ROV based on the sonar data and the attitude data through an unscented Kalman filter (UKF) algorithm and a data fusion algorithm applied to unscented transformation of the sonar data and the attitude data. According to the method, required equipment is conventional and simple, the equipment cost is controllable, the method can flexibly adapt to the change of the underwater position of the ROV, meanwhile, a data fusion algorithm is carried out based on the application of the UKF and unscented transformation thereof, the calculation cost is controllable, the UKF does not need to linearize a nonlinear function through the unscented transformation, and the calculation cost is controllable. The problem of nonlinearity of data collected by the ROV in an underwater positioning scene can be effectively solved, and the position and attitude state of the ROV can be accurately estimated.

Description

Technical Field

[0001] The present invention relates to the technical field of underwater remote-controlled submersibles, and in particular to an underwater remote-controlled submersible and a spatial positioning method thereof. Background Art

[0002] The positioning technology of underwater remotely operated vehicles (ROVs) is one of the key technologies in the fields of underwater engineering, marine exploration and underwater archaeology. The existing ROV spatial positioning technologies mainly include sonar positioning, visual positioning, optical communication positioning, inertial navigation system (INS), global positioning system (GPS), underwater positioning base station, etc.

[0003] Due to the complex and changeable underwater environment, which has low visibility, high pressure, temperature changes and water flow, optical signals decay quickly in water and are easily disturbed by ambient light. The propagation speed and attenuation characteristics of sound waves in water are different from those in air, resulting in inaccurate positioning results using sonar positioning, visual positioning, and optical communication positioning. Inertial navigation systems (INS) have long-term cumulative errors and require regular calibration. GPS signals cannot penetrate water, making GPS positioning technology unsuitable for underwater use. Underwater positioning base stations use a network of underwater acoustic transmitters and receivers for positioning, which requires pre-deployment of base stations, which is not only costly but also inflexible in dynamic environments. None of the above methods are well suited for underwater positioning of ROVs. Summary of the invention

[0004] In view of this, the purpose of the present invention is to provide an underwater remotely operated submersible and a spatial positioning method thereof, to provide a spatial positioning method with high underwater positioning accuracy and controllable cost for ROV underwater positioning, and to solve the problem that the above-mentioned existing positioning methods cannot be well applied to ROV underwater positioning.

[0005] In order to solve the above technical problems, the technical solution used in the present invention is: A spatial positioning method for an underwater remote-controlled submersible according to the present invention comprises: Using the sonar matrix and inertial measurement unit (IMU) installed on the underwater remotely operated vehicle (ROV), the real-time attitude data of the ROV and the distance data from the surrounding space are collected, and the sonar data of the relative position of the ROV and the surrounding space are calculated; The sonar data and the attitude data are combined through a data fusion algorithm based on an unscented Kalman filter (UKF) algorithm and its unscented transformation to estimate the position and attitude state of the ROV.

[0006] Preferably, the method further includes preprocessing the collected real-time posture data and distance data respectively, and calculating the sonar data based on the preprocessed distance data.

[0007] Further preferably, the inclined length is calculated based on the preprocessed distance data, and the square root of the inclined length is obtained to obtain the sonar data.

[0008] Preferably, the sonar matrix includes at least a front sonar group, a rear sonar group and a side sonar group, the sonars in the front sonar group and the rear sonar group are arranged along the central axis of the ROV, and the sonar beam in the front sonar group forms an angle of 45°±5 with the forward direction of the ROV, the sonar beam in the rear sonar group forms an angle of 30°±5 with the forward direction of the ROV, and the sonars in the side sonar group are bilaterally symmetrical and arranged at intervals.

[0009] Further preferably, the center planes of the sonar beams in the front sonar group and the rear sonar group are parallel to the ROV body frame.

[0010] Preferably, the posture data includes acceleration, angular velocity, magnetic field data, roll angle, pitch angle and yaw angle.

[0011] Preferably, the data fusion algorithm includes: S1: Initialize the state vector of the unscented Kalman filter (UKF) and set the covariance matrix; S2: defining an observation model and an observation noise covariance, and constructing a mapping relationship between the state vector and the sonar data and the attitude data respectively; S3: Calculate the mean of the state vector, and select a set of σ points through unscented Kalman transformation according to the mean and covariance matrix of the state vector; S4: define a state transfer model, and propagate each σ point through the state transfer model to obtain a prediction point σ' corresponding to each σ point; S5: Calculate the mean and covariance matrix of all prediction points σ'; S6: acquiring real-time sonar data and attitude data, and applying the observation model according to the prediction point σ' and the acquired sonar data and attitude data to obtain a predicted observation state value; S7: Calculate the mean and covariance matrix of the predicted observed state values; S8: Calculating the Kalman gain according to the obtained covariance of the predicted observation state value and the observation noise covariance; S9: updating the state vector and the covariance matrix in step S1 according to the Kalman gain and the real-time sonar data and attitude data, and outputting the updated state vector and covariance matrix; judging whether the updated state vector and covariance matrix meet the iteration end condition, if so, ending the calculation, if not, entering step S10; S10: Execute steps S3 to S9.

[0012] Further preferably, in step S5, when calculating the covariance matrix of the prediction point σ', the posture data is used as input variable data; And / or, in step S7, when calculating the covariance matrix of the predicted observed state value, the sonar data is used as input variable data.

[0013] Another object of the present invention is to provide an underwater remote-controlled submersible, comprising a submersible body, on which a sonar matrix, an inertial measurement unit and a processor are arranged, the sonar in the sonar matrix and the sensor in the inertial measurement unit are respectively connected to the processor signal, and the processor is written with a program including the above-mentioned spatial positioning method of an underwater remote-controlled submersible.

[0014] Preferably, the submersible body is provided with an anti-collision device, and the sonar is arranged inside the anti-collision device or on a side facing the submersible body.

[0015] Compared with the prior art, the beneficial effects of the spatial positioning method of an underwater remote-controlled submersible described in the present invention are mainly reflected in: The method of the present invention collects real-time attitude data of the ROV and distance data from the surrounding space by arranging the sonar matrix and the inertial measurement unit on the ROV, and then estimates the position and attitude state of the ROV through the data fusion algorithm. The required equipment is conventional and simple, not only the equipment cost is controllable, but also the ROV can be flexibly adapted to the continuous change of the underwater position of the ROV. At the same time, the data fusion algorithm is based on the use of the unscented Kalman filter algorithm (UKF) and its unscented transformation. Not only is the computational cost controllable, but also through the unscented transformation of UKF, UKF does not need to linearize nonlinear functions, so that it can effectively deal with the nonlinear problems of the sonar matrix and the data collected by the inertial measurement unit in the underwater positioning scenario of the ROV, and accurately estimate the position and attitude state of the ROV. This method does not require GPS signals and can achieve accurate estimation of the underwater position and attitude of the ROV in an environment where GPS signals cannot be obtained.

[0016] Compared with the prior art, the beneficial effects of the underwater remote-controlled submersible described in the present invention are mainly reflected in that the underwater remote-controlled submersible of the present invention can use the above-mentioned spatial positioning method to accurately estimate the underwater position and posture, and the implementation cost is relatively low. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] The above and other purposes, features and advantages of the present invention will become more clear by more specific description of the preferred embodiments of the present invention shown in the accompanying drawings. The same reference numerals indicate the same parts in all the accompanying drawings, and the accompanying drawings are not deliberately scaled to the actual size, and the focus is on illustrating the main purpose of the present invention.

[0018] Figure 1 A flow chart of a data fusion algorithm provided by an embodiment of the present invention; Figure 2 A schematic diagram of the structure of an underwater remotely operated submersible provided by an embodiment of the present invention; Description of the drawings: submersible body 1, processor 2, anti-collision rod 3, sonar 4. DETAILED DESCRIPTION

[0019] The technical solution of the present invention is further described in detail below in conjunction with the accompanying drawings and specific embodiments, so that those skilled in the art can better understand the present invention and implement it. However, the embodiments cited are not intended to limit the present invention. In the present embodiment, it should be understood that the orientations or positional relationships indicated by terms such as "longitudinal", "lateral", "upper", "lower", "front", "rear", "left", "right", "vertical", "horizontal", "top", "bottom", "inside", and "outside" are based on the orientations or positional relationships shown in the accompanying drawings, and are only for the convenience of describing the present invention, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore cannot be understood as a limitation of the present invention.

[0020] It should be noted that when an element is considered to be "connected" to another element, it may be directly connected to the other element and integrated therewith, or there may be an intermediate element at the same time. The terms "installed", "one end", "the other end" and similar expressions used in the present invention are for illustrative purposes only.

[0021] This embodiment provides a spatial positioning method for an underwater remotely operated submersible, including: Using the sonar matrix and inertial measurement unit (IMU) installed on the underwater remotely operated vehicle (ROV), the real-time attitude data of the ROV and the distance data from the surrounding space are collected, and the sonar data of the relative position of the ROV and the surrounding space are calculated; The sonar matrix is ​​used to collect the position information of the ROV and the surrounding space units to serve the subsequent spatial position calculation. In order to collect this information to the maximum extent and ensure the comprehensiveness of the distance data, in a preferred embodiment, the sonar matrix includes at least a front sonar group, a rear sonar group and a side sonar group. The number of sonars in each group is not specifically limited, wherein the side sonar group includes a left sonar group and a right sonar group, and the sonars in the left sonar group and the right sonar group are spaced and arranged symmetrically. The installation angle of the sonar in the method of this embodiment will directly affect the quality of the data collected by the sonar. In a preferred embodiment, if Figure 2As shown, the sonars in the front sonar group and the rear sonar group are both arranged along the central axis of the ROV, and the sonar beam in the front sonar group is at an angle of 45°±5 to the forward direction of the ROV, and the sonar beam in the rear sonar group is at an angle of 30°±5 to the forward direction of the ROV; the central planes of the sonar beams in the front sonar group and the rear sonar group are parallel to the ROV body frame; it should be noted that the aforementioned sonar beam is at an angle of 45°±5 or 30°±5 to the forward direction of the ROV, which means that the angle is preferably 45° or 30°, and considering the measurement error of the sonar, the structural installation error, the influence of the ROV load change and other factors, it is necessary to ensure that the maximum deviation of the angle value does not exceed 5°.

[0022] The inertial measurement unit (IMU) includes an accelerometer and a gyroscope, which are used to measure the ROV's attitude data, including acceleration, angular velocity, magnetic field data, roll angle, pitch angle, and yaw angle.

[0023] The sonar data and attitude data are combined through a data fusion algorithm based on the unscented Kalman filter (UKF) and its unscented transformation to estimate the position and attitude state of the ROV.

[0024] The spatial positioning method of the ROV in this embodiment collects the real-time attitude data of the ROV and the distance data from the surrounding space by setting a sonar matrix and an inertial measurement unit on the ROV, and then estimates the position and attitude state of the ROV through a data fusion algorithm. The required equipment is conventional and simple, which can not only flexibly adapt to the continuous change of the underwater position of the ROV, but also has controllable equipment cost; At the same time, the data fusion algorithm is based on the use of the unscented Kalman filter algorithm (UKF) and its unscented transform. Not only is the computational cost controllable, but through the unscented transform of UKF, UKF does not need to linearize the nonlinear function, so it can effectively deal with the nonlinear problem of the data collected by the sonar matrix and inertial measurement unit in the underwater positioning scene of ROV, and accurately estimate the position and attitude state of ROV. This method does not require GPS signals or vision, and will not be affected by the complex underwater environment and low visibility. Therefore, it can realize accurate estimation of the underwater position and attitude of ROV in an environment where GPS signals cannot be obtained, which can solve the problem that the existing positioning methods are not well applicable to ROV underwater positioning.

[0025] In a preferred embodiment, the real-time attitude data and distance data collected are preprocessed to improve the accuracy of the data, and the sonar data is calculated based on the preprocessed distance data. The sonar data is obtained by calculating the inclined length based on the preprocessed distance data and taking the square root of the inclined length.

[0026] Specifically, preprocessing includes filtering the sonar data to remove noise and multipath effects, and calibrating the IMU's attitude data to eliminate sensor bias and errors.

[0027] In a preferred embodiment, Figure 1 As shown, the data fusion algorithm includes: S1: Initialize the state vector of the Unscented Kalman Filter (UKF), such as position, velocity, etc., and set the covariance matrix, which represents the degree of uncertainty of the initial state estimate; For example, if you want to estimate the position and velocity of an object in a two-dimensional plane, the position can be expressed as (x, y), the velocity as (vx, vy), and the covariance matrix reflects the degree of uncertainty in these initial estimates.

[0028] S2: Define the observation model and observation noise covariance, and construct the mapping relationship between the state vector and the sonar data and attitude data respectively; The observation noise covariance is obtained by collecting multiple sets of data in actual experiments and tests, and analyzing these data. Different values ​​of the observation noise covariance can be selected, and the performance of the algorithm under these different values ​​can be observed, and then a better value or value range can be selected. Specifically, in this embodiment, the observation noise covariance is preferably between 0.5 and 1.

[0029] S3: Calculate the mean of the state vector, and select a set of σ points through the unscented Kalman transform according to the mean and covariance matrix of the state vector to capture the probability distribution of the state; The number of σ points in a group is determined by a symmetric sampling method, and is usually selected as 2n + 1, where n is the dimension of the state vector. The device dimension in this embodiment is 3, so the number of final calculation points is 7.

[0030] S4: Define the state transfer model, and propagate each σ point through the state transfer model to obtain the prediction point σ' corresponding to each σ point; It should be noted that in the underwater positioning scenario, the distance data collected by the sonar may be nonlinear, and the attitude data provided by the IMU may also have nonlinear changes. In order to adapt to the ROV underwater positioning problem that combines the nonlinear data of sonar and IMU, the observation model and the state transfer model are both nonlinear models.

[0031] S5: Calculate the mean and covariance matrix of all prediction points σ', which represent the predicted state estimate and its uncertainty level respectively; S6: acquiring real-time sonar data and attitude data, applying the observation model according to the prediction point σ' and the acquired sonar data and attitude data, and obtaining the predicted observation state value; S7: Calculate the mean and covariance matrix of the predicted observed state values ​​for subsequent update steps; S8: Calculate the Kalman gain according to the obtained covariance of the predicted observed state value and the observed noise covariance; S9: Update the state vector and covariance matrix in step S1 according to the Kalman gain and the real-time sonar data and attitude data, and output the updated state vector and covariance matrix; determine whether the updated state vector and covariance matrix meet the iteration end condition, if so, end the calculation, if not, proceed to step S10; The iteration end condition may be a preset accuracy threshold when initializing step S1. Exemplarily, the accuracy threshold may range from 0.3 to 0.7. When the accuracy threshold is within the range or greater than 0.7, the algorithm is considered to have converged and the iteration end condition is reached.

[0032] S10: Execute steps S3 to S9.

[0033] In this embodiment, based on the traditional unscented Kalman filter (UKF), the parameter matching process can be carried out through a large number of actual field tests. More importantly, through the use of unscented transformation, the UKF does not need to linearize the nonlinear function, but approximates the probability density distribution of the nonlinear function through the σ point. It is very suitable for dealing with systems with nonlinear state transfer models and observation models. For example, the ROV underwater positioning problem combining sonar and IMU nonlinear data can achieve accurate estimation of the ROV position and attitude, especially in a small space, such as positioning in a space of about 25mx25m or even smaller. Its accuracy is particularly outstanding, which can provide reliable support for ROV operations in small spaces.

[0034] In a further preferred embodiment, in step S5, when calculating the covariance matrix of the predicted point σ', the posture data is used as input variable data to further process the measurement deviation of the IMU and improve the accuracy of state estimation. Specifically, a deviation model can be established, and then the covariance matrix is ​​established together with the posture data of the IMU, and the covariance matrix operation is performed together in step S7.

[0035] Similarly, in step S7, when calculating the covariance matrix of the predicted observed state value, the sonar data is used as the input variable data to further process the measurement deviation of the sonar data, that is, in the process of updating and correcting the predicted ROV state, the sonar data is considered to improve the accuracy of the ROV state estimation. Specifically, the covariance matrix operation can be performed by establishing a deviation model and then establishing a covariance matrix together with the sonar data.

[0036] In addition, if Figure 2As shown, an embodiment of an underwater remote-controlled submersible is also provided, including a submersible body 1, on which a sonar matrix, an inertial measurement unit (not shown) and a processor 2 are arranged, the sonar in the sonar matrix and the sensor in the inertial measurement unit are respectively connected to the processor 2 signal, and the processor 2 is written with a program including the above-mentioned spatial positioning method of an underwater remote-controlled submersible.

[0037] The underwater remotely operated vehicle of the embodiment of the present invention can use the above-mentioned spatial positioning method to accurately estimate the underwater position and posture, and the implementation cost is relatively low.

[0038] Preferably, the submersible body 1 is provided with an anti-collision device, such as Figure 2 As shown in the figure, the anti-collision rod 3 and the sonars 4 are arranged inside the anti-collision device or on the side facing the submersible body 1, so as to protect the sonar device through the anti-collision device.

[0039] In this specification, unless otherwise clearly specified and limited, a first feature being "above" or "below" a second feature may mean that the first and second features are in direct contact, or the first and second features are in indirect contact through an intermediate medium. Moreover, a first feature being "above", "above" or "above" a second feature may mean that the first feature is directly above or obliquely above the second feature, or simply means that the first feature is higher in level than the second feature. A first feature being "below", "below" or "below" a second feature may mean that the first feature is directly below or obliquely below the second feature, or simply means that the first feature is lower in level than the second feature.

[0040] In the description of this specification, the description with reference to the terms "preferred embodiment", "further embodiment", "other embodiments" or "specific example" etc. means that the specific features, structures, materials or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present application. In this specification, the schematic representations of the above terms do not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described may be combined in any one or more embodiments or examples in a suitable manner. In addition, those skilled in the art may combine and combine the different embodiments or examples described in this specification and the features of the different embodiments or examples, unless they are contradictory.

[0041] Although the embodiments of the present application have been shown and described above, it can be understood that the above embodiments are exemplary and cannot be understood as limitations on the present application. Ordinary technicians in this field can change, modify, replace and modify the above embodiments within the scope of the present application.

Claims

1. A method for spatial positioning of an underwater remotely operated vehicle, characterized in that: include: Using the sonar matrix and inertial measurement unit (IMU) installed on the underwater remotely operated vehicle (ROV), the real-time attitude data of the ROV and the distance data from the surrounding space are collected, and the sonar data of the relative position of the ROV and the surrounding space are calculated; The sonar data and the attitude data are combined through a data fusion algorithm based on an unscented Kalman filter algorithm UKF and its unscented transformation to estimate the position and attitude state of the ROV.

2. The spatial positioning method according to claim 1, characterized in that: The method also includes preprocessing the collected real-time attitude data and distance data respectively, and calculating the sonar data according to the preprocessed distance data.

3. The spatial positioning method according to claim 2, characterized in that: The inclined length is calculated according to the preprocessed distance data, and the square root of the inclined length is obtained to obtain the sonar data.

4. The spatial positioning method according to claim 1, characterized in that: The sonar matrix at least includes a front sonar group, a rear sonar group and a side sonar group. The sonars in the front sonar group and the rear sonar group are arranged along the central axis of the ROV, and the sonar beam in the front sonar group forms an angle of 45°±5 with the forward direction of the ROV, the sonar beam in the rear sonar group forms an angle of 30°±5 with the forward direction of the ROV, and the sonars in the side sonar group are bilaterally symmetrical and arranged at intervals.

5. The spatial positioning method according to claim 4, characterized in that: The central planes of the sonar beams in the front sonar group and the rear sonar group are parallel to the ROV body frame.

6. The spatial positioning method according to claim 1, characterized in that: The attitude data includes acceleration, angular velocity, magnetic field data, roll angle, pitch angle and yaw angle.

7. The spatial positioning method according to any one of claims 1 to 6, characterized in that: The data fusion algorithm includes: S1: Initialize the state vector of the unscented Kalman filter algorithm UKF and set the covariance matrix; S2: defining an observation model and an observation noise covariance, and constructing a mapping relationship between the state vector and the sonar data and the attitude data respectively; S3: Calculate the mean of the state vector, and select a set of σ points through unscented Kalman transformation according to the mean and covariance matrix of the state vector; S4: define a state transfer model, and propagate each σ point through the state transfer model to obtain a prediction point σ' corresponding to each σ point; S5: Calculate the mean and covariance matrix of all prediction points σ'; S6: acquiring real-time sonar data and attitude data, and applying the observation model according to the prediction point σ' and the acquired sonar data and attitude data to obtain a predicted observation state value; S7: Calculate the mean and covariance matrix of the predicted observed state values; S8: Calculating the Kalman gain according to the obtained covariance of the predicted observation state value and the observation noise covariance; S9: updating the state vector and the covariance matrix in step S1 according to the Kalman gain and the real-time sonar data and attitude data, and outputting the updated state vector and covariance matrix; judging whether the updated state vector and covariance matrix meet the iteration end condition, if so, ending the calculation, if not, entering step S10; S10: Execute steps S3 to S9.

8. The spatial positioning method according to claim 7, characterized in that: In step S5, when calculating the covariance matrix of the prediction point σ', the posture data is used as input variable data; And / or, in step S7, when calculating the covariance matrix of the predicted observed state value, the sonar data is used as input variable data.

9. An underwater remote-controlled submersible, comprising a submersible body, characterized in that: The submersible body is provided with a sonar matrix, an inertial measurement unit and a processor. The sonar in the sonar matrix and the sensor in the inertial measurement unit are respectively connected to the processor signal. The processor is written with a program including a spatial positioning method for an underwater remote-controlled submersible described in any one of claims 1 to 8.

10. An underwater remotely operated submersible according to claim 9, characterized in that: The submersible body is provided with an anti-collision device, and the sonar is arranged inside the anti-collision device or on a side facing the submersible body.

Images