Unmanned remote control submersible multi-center calibration system and method for underwater positioning

Abstract

The invention discloses an unmanned remote control submersible multi-center calibration system and method for underwater positioning. The system comprises an environment model setting module which comprises a water body model, a water bottom terrain model and a navigation obstacle model and is used for setting an environment model of an underwater positioning task; the motion model setting module is used for describing and calculating the motion characteristics of the remote control unmanned submersible based on a model predictive control algorithm; the multi-center calibration module is used for calibrating sensor data in a task initialization stage based on a Kalman filter; the sensor fusion positioning module is used for carrying out fusion processing on sensor information based on the underwater sensor database; and the simulation verification module is used for simulating data acquisition of different types of water bodies and sensors based on the environment model and the motion model, testing the rapid calibration capability of the model prediction control algorithm and visually displaying the motion characteristics and tracks of the unmanned remote control submersible. According to the invention, the unmanned remote control submersible can be positioned more accurately.

Description

Technical Field

[0001] The present invention relates to the field of computer simulation technology, and in particular to a multi-center calibration system and method for an unmanned remotely operated submersible for underwater positioning. Background Art

[0002] The ocean, Earth's most extensive ecosystem, covers over 70% of the planet's surface. It is not only a precious resource bestowed by nature but also an indispensable foundation for human survival and development. This vast expanse of water not only underpins global economic activity and meets society's diverse resource needs, but also profoundly influences the balance and stability of the global environment. Given the immense potential of marine resources, their rational and efficient development and utilization have become crucial for the sustainable progress of human society.

[0003] With the deepening implementation of ocean strategies, the construction and maintenance of underwater infrastructure are becoming increasingly extensive. From submarine tunnels to deep-sea exploration platforms, these facilities are of immeasurable value in promoting economic development, deepening scientific exploration, and ensuring local security. However, faced with the complex and ever-changing underwater environment, how to efficiently and safely detect and repair these critical facilities has become a major issue that needs to be addressed. Traditionally, such tasks rely mainly on divers to perform manual operations and data collection, followed by tedious data processing and analysis. Although divers' professional skills are irreplaceable, working in underwater environments full of unknowns and challenges is not only difficult but also carries extremely high safety risks. Especially when faced with adverse conditions such as extreme currents and turbid water, both operational efficiency and personnel safety are severely tested.

[0004] In recent years, thanks to an overall leap in scientific and technological capabilities and the rapid development of unmanned sensing technology, the field of autonomous intelligent robots has made significant progress, providing new solutions for underwater operations. However, compared to the mature applications of unmanned systems on land or in the air, my country's underwater unmanned sensing systems are still in the early stages of development and face numerous technical bottlenecks. In complex underwater environments, factors such as strong current disturbances and unpredictable water conditions can severely interfere with the performance of individual sensors, significantly compromising their reliability. Traditional positioning and navigation algorithms often struggle in such environments, making it difficult to accurately measure and estimate the real-time position of remotely operated vehicles (ROVs), limiting the accuracy and efficiency of underwater operations.

[0005] Therefore, the existing technology needs to be improved and enhanced. Summary of the Invention

[0006] The main purpose of the present invention is to provide a multi-center calibration system and method for unmanned remote-controlled underwater vehicles for underwater positioning, aiming to solve the problem in the prior art that it is difficult to accurately measure and estimate the robot's position information in a complex and variable underwater environment.

[0007] To achieve the above-mentioned purpose, the first aspect of the present invention provides a multi-center calibration system for an unmanned remotely operated vehicle for underwater positioning, the multi-center calibration system for an unmanned remotely operated vehicle for underwater positioning comprising:

[0008] The environment model setting module includes a water model, underwater terrain model, and navigation obstacle model, which is used to set the environment model for underwater positioning tasks;

[0009] The motion model setting module is used to describe and calculate the motion characteristics of the unmanned remotely operated underwater vehicle based on the model predictive control algorithm;

[0010] Multi-center calibration module, used to calibrate sensor data during mission initialization based on Kalman filter;

[0011] Sensor fusion positioning module, used for fusion processing of sensor information based on underwater sensor database;

[0012] The simulation verification module is used to simulate different types of water bodies and sensor data collection based on the environmental model and the motion model, test the rapid calibration capability of the model predictive control algorithm and visualize the motion characteristics and trajectory of the unmanned remotely operated underwater vehicle.

[0013] In one implementation, the multi-center calibration system for underwater positioning of unmanned remotely operated submersibles further includes an underwater sensor database, wherein the underwater sensor database includes sensor type information, sensor measurement information, and sensor positioning information, and is used to provide data support for algorithm verification;

[0014] The sensor type information includes the sensor model information, internal parameters, and installation parameters carried by the unmanned remotely operated submersible;

[0015] The sensor measurement information includes sensor data type and measurement data information for performing underwater positioning tasks;

[0016] The sensor positioning information is the state position and attitude estimation information of the unmanned remote-controlled underwater vehicle independently performed by all sensors.

[0017] In one implementation, the water model is based on a first-order Gauss-Markov process, defining the ocean current amplitude, horizontal angle, vertical angle, and duration;

[0018] The underwater terrain model includes a swimming pool model, a sandbar model, a deep sea plain model and a submarine volcano model, which are used to establish the landform under the water cover;

[0019] The navigation obstacle model includes a three-dimensional shipwreck model in an upright, sideways or inverted state and a seabed coral model, which is used to simulate an area that has a significant impact on the safe movement of the unmanned remotely operated submersible.

[0020] In one implementation, the multi-center calibration module generates a preliminary position estimate based on measurement information from an external sensor, obtains a sensor data calibration rule based on a central sensor, and updates an underwater sensor database based on the sensor data calibration rule, wherein the external sensor is an external sensor of the unmanned remotely operated vehicle and the central sensor is a central sensor of the unmanned remotely operated vehicle;

[0021] The external sensors include Doppler logs, sonars and pressure sensors;

[0022] The central sensor includes an inertial measurement unit and an image sensor.

[0023] In a second aspect, the present invention further provides a multi-center calibration method for an unmanned remotely operated vehicle for underwater positioning, wherein the multi-center calibration method for an unmanned remotely operated vehicle for underwater positioning comprises:

[0024] Acquire a target underwater positioning task, construct a target environment model based on the target underwater positioning task, and determine a target starting point, a target end point, and a target path type of a target unmanned remotely operated submersible in the target environment model;

[0025] Acquiring initial positioning information of a target center sensor and a target external sensor corresponding to the target unmanned remotely operated underwater vehicle based on a model predictive control algorithm, calibrating the initial positioning information of the target external sensor based on the positioning information of the target center sensor, and obtaining target center initial positioning information and target external initial positioning information;

[0026] Acquire real-time target center data information and target external data information based on the target center initial positioning information and the target external initial positioning information;

[0027] The target center data information and the target external data information are fused and processed to obtain a real-time positioning result of the target unmanned remote-controlled underwater vehicle.

[0028] In one implementation, the model predictive control algorithm is used to obtain data information of a target central sensor and a target external sensor corresponding to the target unmanned remotely operated vehicle, including:

[0029] Based on the state model, dynamic model and motion path planning rules, the motion characteristics of the target center sensor and the target external sensor are described and calculated to obtain data information of the target center sensor and the target external sensor. The motion characteristics include optimizing the control of the forward, backward, ascent, descent and turning of the target unmanned remote-controlled underwater vehicle.

[0030] In one implementation, the target center sensor includes a target inertial measurement unit and a target image sensor; the target external sensor includes a target Doppler odometer, a target sonar, and a target pressure sensor;

[0031] The calibrating the initial positioning information of the target external sensor based on the positioning information of the target center sensor to obtain the target center initial positioning information and the target external initial positioning information includes:

[0032] Calibrate the positioning data of the target inertial measurement unit and the target external sensor based on the initial positioning information of the target image sensor, the prediction model, the update model and the calibration cost function to obtain the calibrated initial positioning information of the target inertial measurement unit;

[0033] Based on the calibrated initial positioning information of the target inertial measurement unit, secondary calibration is performed on the speed information of the target Doppler odometer, the target sonar, and the initial positioning information of the target pressure sensor to obtain the target external initial positioning information.

[0034] In one implementation, the fusing of the target central data information and the target external data information includes:

[0035] Obtaining a positioning residual of the target inertial measurement unit, a positioning residual of the target image sensor, a positioning residual of the target external sensor, and a target calibration residual, and calculating a target positioning cost function based on the positioning residual of the target inertial measurement unit, the positioning residual of the target image sensor, the positioning residual of the target external sensor, and the target calibration residual;

[0036] Based on the target positioning cost function, the target center data information and the target external data information are fused in combination with a factor graph optimization method.

[0037] In the third aspect of the present invention, an embodiment of the present invention further provides a terminal device, wherein the terminal device includes a memory, a processor, and a multi-center calibration program for an unmanned remote-controlled submersible for underwater positioning, which is stored in the memory and can be run on the processor. When the processor executes the multi-center calibration program for an unmanned remote-controlled submersible for underwater positioning, the steps of the multi-center calibration method for an unmanned remote-controlled submersible for underwater positioning described in the above scheme are implemented.

[0038] A fourth aspect of the present invention provides a storage medium, wherein the storage medium stores one or more programs, and the one or more programs can be executed by one or more processors to implement the steps of the multi-center calibration method of an unmanned remotely operated submersible for underwater positioning as described in any of the above items.

[0039] Beneficial effects: Compared with the existing technology, the present invention provides a multi-center calibration system and method for underwater positioning of unmanned remote-controlled submersibles. The multi-center calibration system for underwater positioning of unmanned remote-controlled submersibles provided by the present invention includes: an environmental model setting module, including a water body model, a bottom terrain model, and a navigation obstacle model, for setting the environmental model for underwater positioning tasks; a motion model setting module, for describing and calculating the motion characteristics of the unmanned remote-controlled submersible based on a model predictive control algorithm; a multi-center calibration module, for calibrating sensor data during the task initialization phase based on a Kalman filter; a sensor fusion positioning module, for performing fusion processing of sensor information based on an underwater sensor database; and a simulation verification module, for simulating different types of water bodies and sensor data acquisition based on the environmental model and motion model, testing the rapid calibration capability of the model predictive control algorithm, and visually displaying the motion characteristics and trajectory of the unmanned remote-controlled submersible. The present invention provides users with a multi-center calibration system for unmanned remote-controlled submersibles for underwater positioning, which solves the problem in the prior art that it is difficult to accurately measure and estimate the robot's position information in a complex and variable underwater environment. It can realize multi-center calibration of unmanned remote-controlled submersibles for underwater positioning, more accurately position unmanned remote-controlled submersibles, and significantly reduce positioning errors. BRIEF DESCRIPTION OF THE DRAWINGS

[0040] Figure 1 A schematic diagram of the system structure of an embodiment of the multi-center calibration system for underwater positioning of an unmanned remotely operated submersible provided by the present invention;

[0041] Figure 2 A schematic diagram of a simulation system interface in an embodiment of the multi-center calibration system for underwater positioning of an unmanned remotely operated vehicle provided by the present invention;

[0042] Figure 3 This is a flowchart of an embodiment of the multi-center calibration method for underwater positioning of an unmanned remotely operated submersible provided by the present invention;

[0043] Figure 4 A flow chart of a calibration method in an embodiment of the multi-center calibration system for underwater positioning of an unmanned remotely operated vehicle provided by the present invention;

[0044] Figure 5A flow chart of sensor data calibration rules for an embodiment of the multi-center calibration method for an unmanned remotely operated vehicle for underwater positioning provided by the present invention;

[0045] Figure 6 A schematic diagram of positioning results in an embodiment of the multi-center calibration method for underwater positioning of an unmanned remotely operated submersible provided by the present invention;

[0046] Figure 7 This is a functional block diagram of a terminal device provided in an embodiment of the present invention. DETAILED DESCRIPTION

[0047] In order to make the purpose, technical solution and effect of the present invention clearer and more specific, the present invention is further described in detail below with reference to the accompanying drawings and examples. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention.

[0048] Example 1

[0049] like Figure 1 As shown, the multi-center calibration system for underwater positioning of unmanned remotely operated submersibles provided in this embodiment includes:

[0050] The environment model setting module includes a water model, underwater terrain model, and navigation obstacle model, which is used to set the environment model for underwater positioning tasks;

[0051] The motion model setting module is used to describe and calculate the motion characteristics of the unmanned remotely operated underwater vehicle based on the model predictive control algorithm;

[0052] Multi-center calibration module, used to calibrate sensor data during mission initialization based on Kalman filter;

[0053] Sensor fusion positioning module, used for fusion processing of sensor information based on underwater sensor database;

[0054] The simulation verification module is used to simulate different types of water bodies and sensor data collection based on the environmental model and the motion model, test the rapid calibration capability of the model predictive control algorithm and visualize the motion characteristics and trajectory of the unmanned remotely operated underwater vehicle.

[0055] Wherein, in the environmental model setting module, the water body model is based on a first-order Gauss-Markov process to define the ocean current amplitude, horizontal angle, vertical angle and duration;

[0056] The underwater terrain model includes a swimming pool model, a sandbar model, an abyssal plain model, and an underwater volcano model, and is used to establish the landform under the water body to implement terrain collision detection and prevent the unmanned remotely operated submersible from colliding with the underwater terrain during movement;

[0057] The navigation obstacle model includes a three-dimensional shipwreck model in an upright, sideways or inverted state and a seabed coral model, which is used to simulate an area where the safe movement of the unmanned remote-controlled submersible is greatly affected by human or natural factors.

[0058] Specifically, in this embodiment, the swimming pool model is a three-dimensional swimming pool model with a length of 50 meters, a width of 25 meters, and a depth of 2 meters and a tile texture;

[0059] The sandbar model is a three-dimensional sandbar model with a certain height and sandbar texture in the middle part using Gaussian distribution;

[0060] The abyssal plain model supports custom height values ​​and is a uniform cube with a plain texture;

[0061] The submarine volcano model is a three-dimensional volcano model with a certain height and volcanic texture in the middle portion using Gaussian distribution.

[0062] Furthermore, in this embodiment, the environment model setting module includes a state model, a dynamic model and motion path planning rules, which are used to describe and calculate the motion characteristics of the unmanned remotely operated underwater vehicle.

[0063] The environmental model setting module describes and calculates the motion characteristics of the unmanned remote-controlled underwater vehicle based on the model predictive control algorithm. That is, the model predictive control algorithm is based on the state model, the dynamic model and the motion path planning rules. The motion characteristics include optimizing the control of the unmanned remote-controlled underwater vehicle's forward, backward, ascent, descent and turning.

[0064] Further, the multi-center calibration module generates a preliminary position estimate based on measurement information of an external sensor, obtains a sensor data calibration rule based on a central sensor, and updates an underwater sensor database based on the sensor data calibration rule, wherein the external sensor is an external sensor of the unmanned remotely operated vehicle and the central sensor is a central sensor of the unmanned remotely operated vehicle;

[0065] The external sensors include Doppler logs, sonars and pressure sensors;

[0066] The central sensor includes an inertial measurement unit and an image sensor.

[0067] The sensor data calibration rule is as follows: Using a prediction model, an update model, and a calibration cost function, the image sensor is selected as the central sensor. The positioning information of the inertial measurement unit and external sensors is updated and saved to the underwater sensor database. Subsequently, the inertial measurement unit is reselected as the central sensor, and the velocity information of the Doppler log, as well as the positioning information of the sonar and pressure sensors, is updated again and saved to the underwater sensor database.

[0068] The underwater sensor database includes sensor type information, sensor measurement information and sensor positioning information, which is used to provide data support for algorithm verification;

[0069] The sensor type information includes the sensor model information, internal parameters, and installation parameters carried by the unmanned remotely operated submersible;

[0070] The sensor measurement information includes sensor data type and measurement data information for performing underwater positioning tasks;

[0071] The sensor positioning information is the state position and attitude estimation information of the unmanned remote-controlled underwater vehicle independently performed by all sensors.

[0072] The internal parameters include the focal length of the image sensor, the coordinates of the optical center, the distortion coefficient, the zero bias of the inertial measurement unit, the scale factor error and the installation error.

[0073] The installation parameters are the relative positions of the external sensors and the central sensor relative to the center of gravity of the unmanned remotely operated submersible.

[0074] The sensor data types include timestamp, linear velocity, angular velocity, linear acceleration, pressure, and attitude.

[0075] Furthermore, the sensor fusion positioning module is based on a factor graph optimization method and fuses the preliminary positioning information generated by each sensor with the positioning information updated by the multi-center calibration module through a positioning cost function.

[0076] Furthermore, the simulation verification module is used to simulate different types of water bodies and sensor data collection based on the environmental model and motion model, test the rapid calibration capability of the model predictive control algorithm and visualize the motion characteristics and trajectory of the unmanned remotely operated submersible, specifically Figure 2 As shown, Figure 2 This is the simulation system interface diagram.

[0077] In summary, this embodiment provides a multi-center calibration system for underwater positioning of an unmanned remotely operated vehicle, comprising an environment model setting module, including a water body model, a bottom terrain model, and a navigation obstacle model, for setting the environment model for underwater positioning tasks; a motion model setting module, for describing and calculating the motion characteristics of the unmanned remotely operated vehicle based on a model predictive control algorithm; a multi-center calibration module, for calibrating sensor data during the task initialization phase based on a Kalman filter; a sensor fusion positioning module, for performing fusion processing of sensor information based on an underwater sensor database; and a simulation verification module, for simulating different types of water bodies and sensor data collection based on the environment model and motion model, testing the rapid calibration capability of the model predictive control algorithm, and visually displaying the motion characteristics and trajectory of the unmanned remotely operated vehicle. The multi-center calibration system for underwater positioning of an unmanned remotely operated vehicle provided by this embodiment solves the problem in the prior art of the difficulty in accurately measuring and estimating robot position information in complex and variable underwater environments. It can realize multi-center calibration of unmanned remotely operated vehicles for underwater positioning, more accurately locate the unmanned remotely operated vehicle, and significantly reduce positioning errors.

[0078] Example 2

[0079] Based on the above embodiments, the present invention also provides a multi-center calibration method for an unmanned remotely operated submersible for underwater positioning, such as Figure 3 As shown, the multi-center calibration method for underwater positioning of an unmanned remotely operated submersible provided in this embodiment includes the following steps:

[0080] S100: Acquire a target underwater positioning task, construct a target environment model based on the target underwater positioning task, and determine a target starting point, a target end point, and a target path type of a target unmanned remotely operated vehicle in the target environment model.

[0081] Specifically, when the target underwater task is received, the target environment model will first be constructed based on the target underwater positioning task. The target environment model includes a target water body model, a target underwater terrain model and a target navigation obstacle model, wherein the water body model, the underwater terrain model and the navigation obstacle model can be obtained through the simulation software DAVE, network download or manual drawing. The water body model, the underwater terrain model and the navigation obstacle model together constitute the environment model of the underwater positioning task. In this embodiment, the water body model and the underwater terrain model are world format models supported by gazebo GUI, and the navigation obstacle model is an sdf format model. In the simulation software provided in this embodiment, the underwater positioning task environment model is a visual 3D model with actual physical properties. For specific reference Figure 2 shown.

[0082] After the target water body model, the target underwater terrain model and the target navigation obstacle model are acquired, they are written into the environment model format supported by the simulation software to obtain the target environment model.

[0083] Next, instructions for setting the start point, end point, and path type are received. In other words, these are manually set and can be modified based on specific mission details. Common underwater sensor data is embedded in the simulation system, allowing for manual modification and parameter addition and subtraction. Furthermore, if non-embedded sensors are used, their databases can be manually configured within the simulation software. Upon receiving instructions for the start point, end point, and path type corresponding to the target underwater positioning mission, the target start point, end point, and path type for the target unmanned underwater vehicle are determined within the target environment model.

[0084] Specifically, the path type is divided into three trajectory types: the first type is a circular trajectory; the second type is a straight line trajectory; and the third type is a broken line trajectory.

[0085] S200. Obtain the initial positioning information of the target center sensor and the target external sensor corresponding to the target unmanned remote-controlled underwater vehicle based on a model predictive control algorithm, calibrate the initial positioning information of the target external sensor based on the positioning information of the target center sensor, and obtain the target center initial positioning information and the target external initial positioning information.

[0086] The method of obtaining data information of a target central sensor and a target external sensor corresponding to the target unmanned remotely operated underwater vehicle based on a model predictive control algorithm includes:

[0087] Based on the state model, dynamic model and motion path planning rules, the motion characteristics of the target center sensor and the target external sensor are described and calculated to obtain data information of the target center sensor and the target external sensor. The motion characteristics include optimizing the control of the forward, backward, ascent, descent and turning of the target unmanned remote-controlled underwater vehicle.

[0088] Specifically, the state model is represented based on a state model formula, which is:

[0089]

[0090] in, and are the position, direction, and velocity in the sensor coordinate system S relative to the world coordinate system W, respectively, and b is the accelerometer bias and gyroscope bias.

[0091] The kinetic model can be expressed based on a kinetic model formula, which is:

[0092]

[0093] in, is the mass matrix, C(v)v is the Coriolis and centripetal matrix, D(v)v is the hydrodynamic damping matrix, g(η) is the gravity and buoyancy vector, τ is the total propulsive force and torque, and w is the external disturbance.

[0094] Furthermore, in this embodiment, the motion path planning rules are based on the predetermined target path type, the path equation of the target unmanned remote-controlled submersible from the target starting point to the target end point, and the set time step, and the planning points are calculated to ensure that the position and attitude of the submersible are adjusted in accordance with the time step during the movement.

[0095] In this embodiment, when the target path type is a circular trajectory, the path equation is:

[0096] (xa) 2 +(yb) 2 =r 2 ;

[0097] When the target path type is a straight line trajectory, the path equation is:

[0098]

[0099] When the target path type is a broken line trajectory, the path equation is:

[0100]

[0101] Among them, a and b are the coordinates of the center of the set circular trajectory, r is the radius of the circular trajectory, (x i ,y i ) and (x j ,y j ) are the starting point coordinates and end point coordinates of the set straight line trajectory, m1, m2, ..., m n-1 is the slope of each segment of the line trajectory, (x1, y1), (x2, y2),…, (x n ,y n ) are the coordinates of the segment points of the broken line trajectory.

[0102] By describing and calculating the motion characteristics of the target center sensor and the target external sensor based on the state model, the dynamic model, and the motion path planning rules, initial positioning information of the target center sensor and the target external sensor can be obtained. Then, the initial positioning information of the target external sensor is calibrated based on the positioning information of the target center sensor to obtain target center initial positioning information and target external initial positioning information.

[0103] Specifically, the target center sensor includes a target inertial measurement unit and a target image sensor; the target external sensor includes a target Doppler odometer, a target sonar and a target pressure sensor;

[0104] The calibrating the initial positioning information of the target external sensor based on the positioning information of the target center sensor to obtain the target center initial positioning information and the target external initial positioning information includes:

[0105] Calibrate the positioning data of the target inertial measurement unit and the target external sensor based on the initial positioning information of the target image sensor, the prediction model, the update model and the calibration cost function to obtain the calibrated initial positioning information of the target inertial measurement unit;

[0106] Based on the calibrated initial positioning information of the target inertial measurement unit, secondary calibration is performed on the speed information of the target Doppler odometer, the target sonar, and the initial positioning information of the target pressure sensor to obtain the target external initial positioning information.

[0107] In this embodiment, the data of the central sensor and the external sensors are calibrated in the mission initialization phase based on the multi-center calibration module. The multi-center calibration module calibrates the sensor data in the underwater positioning mission initialization phase based on the Kalman filter.

[0108] Reference Figure 4 , Figure 4 A flowchart for implementing multi-center calibration based on the multi-center calibration module is provided. The multi-center calibration module generates a preliminary position estimate based on the measurement information of the external sensor, and updates the underwater sensor database based on the selected center sensor in combination with the sensor data calibration rules.

[0109] Reference Figure 5 , Figure 5The sensor data calibration rule flow chart provided in this embodiment, based on the sensor data calibration rule, uses the prediction model, the update model, and the calibration cost function to first select the image sensor in the target ROV as the target center sensor, update the positioning information of the target inertial measurement unit and the target external sensor, and save it to the underwater sensor database. Subsequently, the target inertial measurement unit is selected as the target center sensor, and the velocity information of the target Doppler log, the positioning information of the target sonar and the target pressure sensor are updated again, and then saved to the underwater sensor database again.

[0110] Specifically, in this embodiment, the prediction model is expressed as:

[0111]

[0112] in, is the posterior state estimate at time k-1, P k-1 is the posterior state estimation covariance matrix at time k-1, Q k-1 is the state transition covariance matrix at time k-1, F k∣k-1 is the state transition matrix.

[0113] The update model is:

[0114]

[0115] P k =(IK k H k )P k∣ k -1 ;

[0116] Among them, res k is the measurement residual at time k, Z k is the observed value at time k, H k is the observation matrix at time k, S k is the residual covariance matrix at time k, K k is the Kalman gain at time k, is the updated state value at time k, P k is the posterior state estimate covariance matrix at time k;

[0117] The calibration cost function is:

[0118]

[0119] Among them, Ω k is the covariance matrix, and represent the calibration residuals corresponding to the target center sensor being the target inertial measurement unit and the target image sensor, respectively, and represents the calibration state value at time k, x k is the state at time k.

[0120] Furthermore, when the target image sensor is selected as the target center sensor, the observation value is expressed as:

[0121]

[0122] in, is the calibration observation value of the target Doppler log, W p kD is the target Doppler log position estimate at time k, W p kV is the estimated position of the target image sensor at time k; and They respectively represent the position estimates of the target image sensor in the x direction (forward and backward direction) and the z direction (vertical direction) of the target unmanned remotely operated underwater vehicle. is the calibrated observation value of the target sonar, is the target sonar position estimate at time k, is the calibration observation value of the target pressure sensor, is the estimated position of the target pressure sensor at time k, is the calibration observation value of the target inertial measurement unit, W p kI is the estimated position of the target inertial measurement unit at time k.

[0123] When the target inertial measurement unit is selected as the target center sensor, the observation value is expressed as:

[0124]

[0125] in, is the calibration observation value of the target Doppler log, W v kD is the target Doppler log velocity estimate at time k, W v kI is the target inertial measurement unit velocity estimate at time k; is the calibration observation value of the target pressure sensor, is the target sonar position estimate at time k, is the estimated position of the target pressure sensor at time k, W p kIis the estimated position of the target inertial measurement unit at time k, where x and z represent the front-to-back direction and the vertical direction of the robot, respectively.

[0126] S300, acquiring real-time target center data information and target external data information based on the target center initial positioning information and the target external initial positioning information;

[0127] S400: Fusing the target center data information and the target external data information to obtain a real-time positioning result of the target unmanned remote-controlled underwater vehicle.

[0128] The fusing of the target central data information and the target external data information includes:

[0129] Obtaining a positioning residual of the target inertial measurement unit, a positioning residual of the target image sensor, a positioning residual of the target external sensor, and a target calibration residual, and calculating a target positioning cost function based on the positioning residual of the target inertial measurement unit, the positioning residual of the target image sensor, the positioning residual of the target external sensor, and the target calibration residual;

[0130] Based on the target positioning cost function, the target center data information and the target external data information are fused in combination with a factor graph optimization method.

[0131] Specifically, in this embodiment, the target sensor fusion positioning module of the target unmanned remotely operated underwater vehicle performs fusion processing on the target center data information and the target external data information.

[0132] The target sensor fusion positioning module is based on a factor graph optimization method and, through the target positioning cost function, fuses the preliminary positioning information of each sensor with the positioning information updated by the multi-center calibration module.

[0133] In this embodiment, the positioning cost function is:

[0134]

[0135] in, is the target inertial measurement unit positioning residual between key frames p,q, for the target image sensor positioning residual, is the target extrinsic sensor positioning residual, The residuals are calibrated for the target.

[0136] Among them, the positioning residual of the target inertial measurement unit is:

[0137]

[0138] in, They represent the rotation constraint, velocity constraint, and position constraint between keyframes p and q respectively. Pre-integrated rotation for the inertial measurement unit, is the pre-integrated velocity of the inertial measurement unit, is the pre-integrated position of the inertial measurement unit. b g , b a They represent angular velocity deviation and acceleration deviation respectively. R represents the rotation matrix.

[0139] The target image sensor positioning residual is:

[0140]

[0141] The target external sensor positioning residual is:

[0142]

[0143] Among them, Jacb k is the Jacobian matrix.

[0144] The target calibration residual is:

[0145]

[0146] in, is the position estimate updated by the multi-center calibration module.

[0147] Furthermore, in this embodiment, the state information of the target unmanned remotely operated underwater vehicle, including position and attitude information, is recursively estimated through two steps of prediction and update.

[0148] The prediction process uses the dynamic model and positioning information from the previous moment to predict the current positioning information and covariance. The update process obtains the current external sensor and central sensor data from the underwater sensor database. External sensors include Doppler logs, sonar, and pressure sensors. The positioning information corresponding to each sensor is obtained based on the current external sensor and central sensor data. Subsequently, the external sensors are calibrated for each of the two central sensors, obtaining the calibrated velocity and positioning information for the current external sensors.

[0149] In this embodiment, an underwater unmanned remote-controlled submersible is tested based on this. Figure 6 As shown, the positioning result obtained by the multi-center calibration method for underwater positioning of an unmanned remotely operated vehicle provided by this embodiment has a smaller error than that before the improvement, and the obtained trajectory is closer to the true trajectory.

[0150] Furthermore, in this embodiment, the underwater sensor database includes sensor type information, sensor measurement information, and sensor positioning information, which is used to provide data support for algorithm verification;

[0151] The sensor type information includes the sensor model information, internal parameters, and installation parameters carried by the unmanned remotely operated submersible;

[0152] The sensor measurement information includes sensor data type and measurement data information for performing underwater positioning tasks;

[0153] The sensor positioning information is the state position and attitude estimation information of the unmanned remote-controlled underwater vehicle independently performed by all sensors.

[0154] Internal parameters include image sensor focal length, optical center coordinates, distortion coefficients, inertial measurement unit zero bias, scale factor error, and installation error. Installation parameters are the relative positions of the external and central sensors relative to the center of gravity of the unmanned remotely operated vehicle. Sensor data types include timestamp, linear velocity, angular velocity, linear acceleration, pressure, and attitude.

[0155] In summary, this embodiment provides a multi-center calibration method for an unmanned remote-controlled submersible for underwater positioning. When performing trajectory positioning on a target unmanned remote-controlled submersible, a target underwater positioning task is obtained, a target environment model is constructed based on the target underwater positioning task, and the target starting point, target end point, and target path type of the target unmanned remote-controlled submersible are determined in the target environment model. Then, based on a model predictive control algorithm, the initial positioning information of a target center sensor and a target external sensor corresponding to the target unmanned remote-controlled submersible is obtained. Based on the positioning information of the target center sensor, the initial positioning information of the target external sensor is calibrated to obtain target center initial positioning information and target external initial positioning information. Then, based on the target center initial positioning information and the target external initial positioning information, real-time target center data information and target external data information are obtained. Finally, the target center data information and the target external data information are fused to obtain a real-time positioning result of the target unmanned remote-controlled submersible. This embodiment provides users with a multi-center calibration method for unmanned remote-controlled submersibles for underwater positioning, which solves the problem in the existing technology that it is difficult to accurately measure and estimate the robot's position information in a complex and variable underwater environment. It can realize multi-center calibration of unmanned remote-controlled submersibles for underwater positioning, more accurately position the unmanned remote-controlled submersible, and significantly reduce positioning errors.

[0156] It should be understood that, although the steps in the flowcharts provided in the accompanying drawings of the present invention are shown in sequence as indicated by the arrows, these steps are not necessarily performed in the order indicated by the arrows. Unless otherwise specified herein, there is no strict order restriction on the execution of the steps in the present invention, and these steps may be performed in other orders. Moreover, at least a portion of the steps of the present invention may include multiple sub-steps or multiple stages, and these sub-steps or stages are not necessarily performed at the same time, but may be performed at different times. The execution order of these sub-steps or stages is not necessarily sequential, but may be performed in turn or alternately with other steps or at least a portion of the sub-steps or stages of other steps.

[0157] Those skilled in the art will appreciate that all or part of the processes in the above-mentioned embodiments can be implemented by signaling related hardware through a computer program. The computer program can be stored in a non-volatile computer-readable storage medium. When executed, the computer program can include the processes of the above-mentioned embodiments. Among them, any reference to memory, storage, database or other media used in the embodiments provided by the present invention can include non-volatile and / or volatile memory. Non-volatile memory can include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM) or flash memory. Volatile memory can include random access memory (RAM) or external cache memory. By way of illustration and not limitation, RAM is available in various forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), double data rate SDRAM (DDRSDRAM), enhanced SDRAM (ESDRAM), synchronous link DRAM (SLDRAM), memory bus (Rambus) direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and memory bus dynamic RAM (RDRAM).

[0158] Example 3

[0159] Based on the above embodiment, the present invention further provides a terminal device, the principle block diagram of the terminal device can be as follows: Figure 7 The terminal device may include one or more processors 100 ( Figure 7Only one is shown in the figure), memory 101, and computer program 102 stored in memory 101 and executable on one or more processors 100, for example, a program for a multi-center calibration method for an unmanned remotely operated vehicle for underwater positioning. When one or more processors 100 execute computer program 102, each step in an embodiment of a multi-center calibration method for an unmanned remotely operated vehicle for underwater positioning can be implemented. Alternatively, when one or more processors 100 execute computer program 102, the functions of each module / unit in an embodiment of a multi-center calibration method for an unmanned remotely operated vehicle for underwater positioning can be implemented, without limitation herein.

[0160] In one embodiment, the processor 100 may be a central processing unit (CPU), or other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASIC), field-programmable gate arrays (FPGA), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. The general-purpose processor may be a microprocessor or any conventional processor.

[0161] In one embodiment, the memory 101 may be an internal storage unit of an electronic device, such as a hard disk or memory of the electronic device. The memory 101 may also be an external storage device of the electronic device, such as a plug-in hard disk, a smart media card (SMC), a secure digital (SD) card, a flash card, etc. equipped on the electronic device. Furthermore, the memory 101 may also include both an internal storage unit of the electronic device and an external storage device. The memory 101 is used to store computer programs and other programs and data required by the terminal device. The memory 101 may also be used to temporarily store data that has been output or is about to be output.

[0162] Those skilled in the art will understand that Figure 7 The principle block diagram shown in the figure is only a block diagram of a partial structure related to the solution of the present invention, and does not constitute a limitation on the terminal device to which the solution of the present invention is applied. The specific terminal device may include more or fewer components than shown in the figure, or combine certain components, or have a different component arrangement.

[0163] Those skilled in the art will understand that all or part of the processes in the above-mentioned embodiment methods can be implemented by instructing the relevant hardware through a computer program, and the computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the processes of the embodiments of the above-mentioned methods. Among them, any reference to memory, storage, operating database or other media used in the embodiments provided by the present invention may include non-volatile and / or volatile memory. Non-volatile memory may include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM) or flash memory. Volatile memory may include random access memory (RAM) or external cache memory. By way of illustration and not limitation, RAM is available in many forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), double data rate SDRAM (DDR SDRAM), enhanced SDRAM (ESDRAM), synchronous link DRAM (SLDRAM), RAMbus direct RAM (RDRAM), direct RAMbus dynamic RAM (DRDRAM), and RAMbus dynamic RAM (RDRAM), etc.

[0164] Example 4

[0165] The present invention also provides a storage medium storing one or more programs, which can be executed by one or more processors to implement the steps of the multi-center calibration method for unmanned remote-controlled submersibles for underwater positioning described in the above embodiment.

[0166] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or make equivalent replacements for some of the technical features therein. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the various embodiments of the present invention.

Claims

1. A multi-center calibration system for unmanned remotely operated submersibles for underwater positioning, characterized in that: The multi-center calibration system for unmanned remotely operated underwater vehicles for underwater positioning includes: The environment model setting module includes a water model, underwater terrain model, and navigation obstacle model, which is used to set the environment model for underwater positioning tasks; The motion model setting module is used to describe and calculate the motion characteristics of the unmanned remotely operated underwater vehicle based on the model predictive control algorithm; Multi-center calibration module, used to calibrate sensor data during mission initialization based on Kalman filter; Sensor fusion positioning module, used for fusion processing of sensor information based on underwater sensor database; The simulation verification module is used to simulate different types of water bodies and sensor data collection based on the environmental model and the motion model, test the rapid calibration capability of the model predictive control algorithm and visualize the motion characteristics and trajectory of the unmanned remotely operated underwater vehicle.

2. The multi-center calibration system for underwater positioning of unmanned remotely operated submersibles according to claim 1, characterized in that: The multi-center calibration system for underwater positioning of unmanned remotely operated submersibles further includes an underwater sensor database, which includes sensor type information, sensor measurement information, and sensor positioning information, and is used to provide data support for algorithm verification; The sensor type information includes the sensor model information, internal parameters, and installation parameters carried by the unmanned remotely operated submersible; The sensor measurement information includes sensor data type and measurement data information for performing underwater positioning tasks; The sensor positioning information is the state position and attitude estimation information of the unmanned remote-controlled underwater vehicle independently performed by all sensors.

3. The multi-center calibration system for underwater positioning of unmanned remotely operated submersibles according to claim 1, characterized in that: The water model is based on a first-order Gauss-Markov process, which defines the ocean current amplitude, horizontal angle, vertical angle and duration; The underwater terrain model includes a swimming pool model, a sandbar model, a deep sea plain model and a submarine volcano model, which are used to establish the landform under the water cover; The navigation obstacle model includes a three-dimensional shipwreck model in an upright, sideways or inverted state and a seabed coral model, which is used to simulate an area that has a significant impact on the safe movement of the unmanned remotely operated submersible.

4. The multi-center calibration system for underwater positioning of unmanned remotely operated submersibles according to claim 1, characterized in that: The multi-center calibration module generates a preliminary position estimate based on measurement information of an external sensor, obtains a sensor data calibration rule based on a central sensor, and updates an underwater sensor database based on the sensor data calibration rule, wherein the external sensor is an external sensor of the unmanned remotely operated vehicle and the central sensor is a central sensor of the unmanned remotely operated vehicle; The external sensors include Doppler logs, sonars and pressure sensors; The central sensor includes an inertial measurement unit and an image sensor.

5. A multi-center calibration method for an unmanned remotely operated vehicle for underwater positioning, characterized in that: The multi-center calibration system for underwater positioning of an unmanned remotely operated vehicle according to any one of claims 1 to 4 is applied, and the multi-center calibration method for underwater positioning of an unmanned remotely operated vehicle comprises: Acquire a target underwater positioning task, construct a target environment model based on the target underwater positioning task, and determine a target starting point, a target end point, and a target path type of a target unmanned remotely operated submersible in the target environment model; Acquiring initial positioning information of a target center sensor and a target external sensor corresponding to the target unmanned remotely operated underwater vehicle based on a model predictive control algorithm, calibrating the initial positioning information of the target external sensor based on the positioning information of the target center sensor, and obtaining target center initial positioning information and target external initial positioning information; Acquire real-time target center data information and target external data information based on the target center initial positioning information and the target external initial positioning information; The target center data information and the target external data information are fused and processed to obtain a real-time positioning result of the target unmanned remote-controlled underwater vehicle.

6. The multi-center calibration method for underwater positioning of an unmanned remotely operated submersible according to claim 5, characterized in that: The method of obtaining data information of a target central sensor and a target external sensor corresponding to the target unmanned remotely operated underwater vehicle based on a model predictive control algorithm includes: Based on the state model, dynamic model and motion path planning rules, the motion characteristics of the target center sensor and the target external sensor are described and calculated to obtain data information of the target center sensor and the target external sensor. The motion characteristics include optimizing the control of the forward, backward, ascent, descent and turning of the target unmanned remote-controlled underwater vehicle.

7. The multi-center calibration method for underwater positioning of an unmanned remotely operated submersible according to claim 5, characterized in that: The target center sensor includes a target inertial measurement unit and a target image sensor; the target external sensor includes a target Doppler log, a target sonar and a target pressure sensor; The calibrating the initial positioning information of the target external sensor based on the positioning information of the target center sensor to obtain the target center initial positioning information and the target external initial positioning information includes: Calibrate the positioning data of the target inertial measurement unit and the target external sensor based on the initial positioning information of the target image sensor, the prediction model, the update model and the calibration cost function to obtain the calibrated initial positioning information of the target inertial measurement unit; Based on the calibrated initial positioning information of the target inertial measurement unit, secondary calibration is performed on the speed information of the target Doppler odometer, the target sonar, and the initial positioning information of the target pressure sensor to obtain the target external initial positioning information.

8. The multi-center calibration method for underwater positioning of an unmanned remotely operated submersible according to claim 7, characterized in that: The fusing of the target central data information and the target external data information includes: Obtaining a positioning residual of the target inertial measurement unit, a positioning residual of the target image sensor, a positioning residual of the target external sensor, and a target calibration residual, and calculating a target positioning cost function based on the positioning residual of the target inertial measurement unit, the positioning residual of the target image sensor, the positioning residual of the target external sensor, and the target calibration residual; Based on the target positioning cost function, the target center data information and the target external data information are fused in combination with a factor graph optimization method.

9. A terminal device, characterized in that: The terminal device includes a memory, a processor, and a multi-center calibration program for unmanned remote-controlled submersibles for underwater positioning, which is stored in the memory and can be run on the processor. When the processor executes the multi-center calibration program for unmanned remote-controlled submersibles for underwater positioning, the steps of the multi-center calibration method for unmanned remote-controlled submersibles for underwater positioning as described in any one of claims 5 to 8 are implemented.

10. A storage medium, characterized in that: The storage medium stores one or more programs, and the one or more programs can be executed by one or more processors to implement the steps of the multi-center calibration method for unmanned remote-controlled submersibles for underwater positioning as described in any one of claims 5-8.

Images