Satellite underwater acoustic inertial optical tight combination underwater robot positioning method

Abstract

The invention discloses a satellite underwater acoustic inertial optical tight combination underwater robot positioning method, and belongs to the technical field of underwater navigation. According to the method, optical positioning data are innovatively and effectively fused into an underwater tight combination positioning system, the positioning precision is further optimized through joint calculation of overwater and underwater tight combination, and data provided by an optical sensor are added, so that the method not only expands multi-source data which can be used for calculation, but also improves the positioning precision of the underwater tight combination positioning system. The positioning capability of the underwater robot in a complex environment is also remarkably enhanced, especially for the error accumulation problem existing in an inertial navigation system, real-time compensation of optical data can effectively improve the positioning accuracy and reliability, and further, through combination of overwater and underwater tight combination calculation, the method has the advantages that the calculation resources are optimized, and meanwhile the positioning accuracy and reliability of the underwater robot are improved. The consistency and the stability of a positioning result in different working environments are guaranteed, and particularly, high-precision navigation positioning can still be guaranteed in deepwater or other complex water area environments with weak signals.

Description

Technical Field

[0001] The present invention belongs to the technical field of underwater navigation, and particularly relates to a positioning method for an underwater robot with a tight combination of satellite, underwater acoustic, and inertial optics. Background Technique

[0002] In an underwater environment, due to the large attenuation of electromagnetic waves by water bodies, GNSS cannot be used for navigation and positioning in the underwater environment. Inertial navigation devices rely on inertial measurement units and other supporting facilities to provide complete navigation information such as the speed and attitude of the carrier in all directions. Without any connection to the outside world, they have good autonomy and are commonly used as the main underwater navigation devices. However, inertial navigation also has the disadvantage that errors accumulate over time. As the mainstream device for underwater acoustic navigation, the ultra-short baseline has also been widely used in underwater navigation. The method of tightly combining an inertial navigation device and an ultra-short baseline to solve the position of an underwater robot can not only overcome the problem of pure inertial navigation error accumulation but also be applicable to the special environment of underwater navigation, providing high-precision positioning information for underwater targets. However, this method also has the problem that the positioning information of the underwater acoustic beacon of the ultra-short baseline is delayed, resulting in the inability to repair inertial navigation errors in a timely manner.

[0003] Accordingly, this patent proposes a positioning method for an underwater robot with a tight combination of satellite, underwater acoustic, inertial, and optics. By adding optical positioning data, this method increases the types of data sources participating in the positioning of the underwater robot, improving the accuracy and reliability of the underwater robot positioning. Summary of the Invention

[0004] The purpose of the present invention is to propose a positioning method for an underwater robot with a tight combination of satellite, underwater acoustic, inertial, and optics to solve the problems in the above background technique.

[0005] To achieve the above purpose, the present invention adopts the following technical solutions:

[0006] A positioning method for an underwater robot with a tight combination of satellite, underwater acoustic, inertial, and optics, comprising the following steps:

[0007] Step 1: Measure that the mother ship is equipped with a satellite positioning receiver, an ultra-short baseline underwater acoustic transducer, and an inertial navigation system, and the underwater robot is equipped with an acoustic beacon, an inertial navigation system, and a binocular optical camera. The multi-source sensors perform real-time observations in a time-synchronized state.

[0008] Step 2: Use the satellite positioning data and inertial navigation data of the mother ship to establish an overwater tight combination state equation and an observation equation, and use the extended Kalman filter for solution. Through coordinate transfer, the position, speed, and attitude of the underwater acoustic transducer carried by the mother ship in the geodetic coordinate system are output in real time.

[0009] Step 3: Using underwater acoustic positioning data, underwater inertial navigation data, and binocular optical positioning data, establish the underwater tightly coupled state equation and observation equation, and output the position, velocity, and attitude of the center of the underwater robot carrier in the geodetic coordinate system in real time;

[0010] Step 4: Comprehensively utilize the satellite positioning data, inertial navigation data, acoustic transducer data of the mother ship, as well as the acoustic beacon data, inertial navigation data, and binocular optical positioning data of the underwater robot to construct an overall navigation filtering model of the underwater robot with satellite / underwater acoustic / inertial / optical tight coupling, and feedback the overall navigation filtering information to the above-water tight coupling and underwater tight coupling to achieve real-time quality control.

[0011] As a further description of the above technical solution:

[0012] The said Step 1 includes:

[0013] Step 1.1: Measure that the mother ship is equipped with a satellite positioning receiver, an underwater acoustic transducer, and an inertial navigation device, and obtain the GNSS observations of the mother ship under the time synchronization state, including the original pseudorange observation value P r , phase observation value L r , the acceleration values a m in each direction output by the inertial navigation device and the attitude value and indirectly obtain the position r s , velocity v s and attitude

[0014] Step 1.2: The underwater robot is equipped with an acoustic beacon, an inertial navigation system, and a binocular optical camera, and obtain the real-time position value B U of the acoustic beacon, the position value B I output by the underwater inertial navigation, the acceleration a u in each direction, the attitude value and the position value B L of the underwater robot output by the binocular optical camera.

[0015] As a further description of the above technical solution:

[0016] The said Step 2 includes:

[0017] Step 2.1: Construct the ambiguity-fixed solution PPP observation equation and the inertial navigation state equation;

[0018] Respectively establish the satellite positioning pseudorange observation equation P and the phase observation equation L, and perform corrections such as antenna phase center and tide, and obtain the final PPP observation equation of the ambiguity-fixed solution after linearization;

[0019]

[0020] where represents the pseudorange observation quantity, represents the phase observation quantity, (X i Y i Z i ) represents the position of a certain satellite, (XYZ) represents the position of the ground receiver, C represents the speed of light, V tR represents the receiver clock error, V tS represents the satellite clock error, V trop represents the tropospheric delay correction, V ion represents the ionospheric delay correction, δρ represents the satellite ephemeris error, ε represents the measurement noise, N represents the integer ambiguity, and λ represents the wavelength;

[0021] After linearization, the inter-satellite single-difference observation equation is:

[0022]

[0023] where represents the inter-satellite single-difference quantity between the reference satellite v and other satellite n for the pseudorange observation quantity, represents the inter-satellite single-difference quantity between the reference satellite v and other satellite n for the phase observation quantity, represents the inter-satellite single-difference direction cosine vector between the reference satellite in each direction and the nth satellite, including ω vn represents the inter-satellite single-difference wet delay quantity, δr g , δW, δN vn respectively represent the position error, wet delay error, and ambiguity error, and the quantities related to ε all represent the observation noise quantities;

[0024] The state equation of the inertial navigation system is:

[0025]

[0026] where δr g , δV g , respectively represent the position error, velocity error, and attitude error; δa and δg respectively represent the accelerometer error and the constant zero bias error of the gyroscope, ε a , ε g respectively represent the observation noise vectors of the accelerometer and the gyroscope, is the output of the accelerometer after constant zero bias compensation, representing the projection of the acceleration value of the (body frame) b frame relative to the (inertial frame) i frame in the b frame, represents the attitude matrix of the b frame relative to the (earth coordinate frame) e frame, represents the projection of the attitude matrix of the e frame relative to the i frame in the e frame, E represents the unit matrix, and those related to × are all anti-symmetric matrices;

[0027] Step 2.2: Expand the established observation equations and state equations to obtain the expanded observation and state equations required for the tight integration on water. The expanded observation equation is as follows:

[0028]

[0029] where S n (n represents the superscript of a certain type of GNSS system, and 1 - 4 are used to represent 4 different satellite navigation systems) represents the inter-satellite single-difference pseudorange observation P n of the satellites obtained in a certain type of navigation system, the phase observation L n , and the Doppler observation D n and the difference between the inertial navigation output form the observation matrix, represents the matrix composed of the inter-satellite single-difference direction cosine vectors of a certain type of navigation system, represents the wet delay of a certain type of navigation system, and the quantities related to δ are all error quantities, represents the ambiguity error of a certain type of navigation system, and the quantities related to ε are the observation noises, represents the ambiguity coefficient matrix of a certain type of navigation system;

[0030] The expanded state equation is as follows:

[0031]

[0032] where δN 1 , δN 2 , δN 3 , δN 4 respectively represent the inter-satellite single-difference ambiguity error quantities of 4 types of navigation systems;

[0033] After constructing the state equation and the observation equation, the extended Kalman filter is used for solution to obtain the correction values of each item in the tight integration state equation, and the position, velocity, and attitude information of the underwater acoustic transducer are obtained through coordinate transformation.

[0034] As a further description of the above technical solution:

[0035] The said step 3 includes:

[0036] Step 3.1: Construct the state equation of the inertial / underwater acoustic / optical underwater integrated navigation system:

[0037] For the inertial / underwater acoustic / optical underwater integrated navigation system, the error quantities of the navigation parameters of the inertial navigation device are used as the state variables. The position error δr u includes δL, δλ, δh, and the velocity errors δV u in the northeast-up directionsincluding δV E 、δV N 、δV U ,the attitude angle error δφ in the northeast celestial direction u including δφ E 、δφ N 、δφ U ,the drift δε of the gyroscope u including δε x 、δε y 、δε z ,and the zero bias of the accelerometer including several parameters form the system state variable, ν wn and ν an represent white noise;

[0038] Thus, the state variable of the inertial / underwater acoustic / optical integrated navigation system is given:

[0039]

[0040] System noise: T(t) = [ν wx ν wy ν wz ν ax ν ay ν az T

[0041] The state equation is:

[0042]

[0043] where β(t) and γ(t) represent the state transition matrix and the system noise driving matrix respectively, represents the attitude matrix of the b system relative to the n system (navigation system), and γ nn represents the attitude transition matrix;

[0044] are respectively:

[0045]

[0046] Step 3.2: Observation equation of the inertial / underwater acoustic / optical underwater integrated navigation system:

[0047] Subtract the position values given by the underwater beacon and the position values given by the optical camera from the position values given by the underwater inertial navigation respectively, and multiply by the weight values ρ UI and ρ LI ​The observed quantity is obtained by addition. For the initial values of both weights, 1 / 2 is assigned. The subsequent weights are determined based on the difference between the underwater robot positioning result obtained from the third tight integration and the position value given by the underwater beacon, and the position value given by the optical camera. The weight formula is as follows:

[0048] e UI = |X3 - B U | e LI = |X3 - B L |

[0049] X3 is the result obtained from the third tight integration, e UI , e LI respectively represent the differences between the underwater robot positioning result obtained from the third tight integration and the position value given by the underwater beacon, and the position value given by the optical camera;

[0050] The derivation of the observation equation is as follows:

[0051] The position information output by the underwater beacon is y represents the position of the underwater beacon in the earth coordinate system, b u is the position of the carrier determined by the ultra-short baseline in the earth coordinate system, and T represents the observation noise; the position information output by the underwater inertial navigation is b I is the position of the carrier determined by the underwater inertial navigation in the earth coordinate system, and the position information output by the optical binocular camera is b L is the position of the carrier determined by the binocular optical camera in the earth coordinate system; represents the attitude matrix of the e system relative to the b system, represents the attitude matrix of the e system relative to the n system;

[0052] The position weighting gives:

[0053]

[0054] δr = [δx δy δz] T represents the error quantity, δb represents the inertial navigation error quantity, T represents the observation noise, (x, y, z) is the carrier position in the earth rectangular coordinate system, (L, λ, h) is the carrier position in the earth coordinate system, R is the curvature radius of the prime vertical circle of the reference ellipsoid, e is the ellipsoidal eccentricity, K is the two-coordinate error conversion matrix, φ is the attitude angle matrix, and those related to × are all skew-symmetric matrices;

[0055]

[0056] Therefore, the derived observation equation can be written as:

[0057] Z(t) = A(t)X(t) + T(t)

[0058] State variable The observation matrix A(t) is

[0059] The observation noise is It is the attitude matrix of the e-frame relative to the b-frame;

[0060] Tightly combine the state equation and the observation equation of the inertial / underwater acoustic / optical underwater integrated navigation system using an extended Kalman filter to obtain the correction values of each state variable, and then obtain the position, velocity and attitude of the center of the underwater robot carrier.

[0061] As a further description of the above technical solution:

[0062] The said step 4 includes:

[0063] Step 4.1: Tightly combine the mother ship inertial navigation, satellite positioning data with the inertial / underwater acoustic / optical underwater integrated navigation system for the third time, combine the surface and underwater observation equations and state equations to form a joint state equation and a joint observation equation, and use the extended Kalman filter for the third tight combination to obtain the position, velocity and attitude of the underwater robot;

[0064] Joint state equation:

[0065] X = AB + CD

[0066]

[0067] D = [ν wx ν wy ν wz ν ax ν ay ν az ε a ε g T

[0068] Joint observation equation:

[0069] W = KL + T

[0070]

[0071] T = [ε 1 + Tε 2 + Tε 3 + Tε 4 + T] T

[0072] ​The combined state equation and the combined observation equation are tightly combined using the extended Kalman filter to obtain each correction quantity;

[0073] Step 4.2 Quality control part:

[0074] The underwater robot position result obtained from the third tight combination is fed back to the underwater robot position result obtained from the underwater tight combination for quality control. Denote the result of the third tight combination as X3(x3, y3, z3), the position result obtained from the underwater tight combination as X2(x2, y2, z2), and the result obtained from the surface tight combination as X1(x1, y1, z1). First, determine whether there are outliers by the residual (RE) being less than three times the mean error (M). Second, use the root mean square error (RMSE) and the standard error (SE) to measure the result accuracy and the fluctuation of the positioning result of the positioning result. Construct an evaluation criterion Q, and assign weights of 20%, 40%, and 40% to the residual, the root mean square error, and the standard error respectively to obtain the Q value. The smaller the Q value, the better the positioning result;

[0075] Taking the quality control of the underwater robot positioning result as an example, the residual should be ensured less than 3 times the mean error Otherwise, it indicates that there are outliers in the positioning result, and the outliers should be removed;

[0076] According to the standard error and root mean square error formulas:

[0077]

[0078] Select the value of n as 10, indicating that quality control is performed every 10 times the positioning results of the underwater tight combination and the third tight combination are output. X is the mean of the 10 positioning results, is the predicted value of the 10 positioning results. In the underwater quality control process, this value is selected as the result output by the underwater inertial navigation. Calculate the Q values of the underwater tight combination and the third tight combination respectively according to the following formula and compare them;

[0079] Q i = 0.2·RE + 0.4·SE + 0.4·RMSE

[0080] If the Q value of the third tight combination is less than the Q value of the underwater tight combination, then output the underwater robot positioning result obtained by the third tight combination as the final result. The quality control part of the surface tight combination can be obtained in the same way.

[0081] In summary, due to the adoption of the above technical solution, the beneficial effects of the present invention are:

[0082] 1. In the present invention, the optical positioning data is innovatively and effectively integrated into the underwater tightly coupled positioning system, and the positioning accuracy is further optimized through the combined calculation of the tight combination of above-water and underwater. By adding the data provided by the optical sensor, this method not only expands the multi-source data available for calculation, but also significantly enhances the positioning ability of the underwater robot in complex environments. Especially for the problem of error accumulation in the inertial navigation system, the real-time compensation of optical data can effectively improve the accuracy and reliability of positioning. Further, through the combined tight calculation of above-water and underwater, the present invention optimizes the computing resources while ensuring the consistency and stability of the positioning results in different operating environments. Especially in deep water or other complex water areas with weak signals, high-precision navigation and positioning can still be ensured. BRIEF DESCRIPTION OF THE DRAWINGS

[0083] Figure 1 FIG. is a flowchart of a positioning method for an underwater robot with a tightly coupled satellite, underwater acoustic, inertial, and optical system proposed by the present invention;

[0084] Figure 2 FIG. is a schematic diagram of a positioning method for an underwater robot with a tightly coupled satellite, underwater acoustic, inertial, and optical system proposed by the present invention;

[0085] Figure 3 FIG. is a comparison of the combined above-water and underwater tight combination position error and the underwater tight combination position error for a positioning method for an underwater robot with a tightly coupled satellite, underwater acoustic, inertial, and optical system proposed by the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0086] The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all of the embodiments. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts shall fall within the protection scope of the present invention.

[0087] Please refer to the attached Figure 1 - attached Figure 3 , the present invention provides a technical solution: a positioning method for an underwater robot with a tightly coupled satellite, underwater acoustic, inertial, and optical system, including the following steps:

[0088] Step 1: The mother ship is equipped with a satellite positioning receiver, an ultra-short baseline underwater acoustic transducer, and an inertial navigation system, and the underwater robot is equipped with an acoustic beacon, an inertial navigation system, and a binocular optical camera. The multi-source sensors perform real-time observations in a time-synchronized state.

[0089] Step 2: Establish the state equation and observation equation of the tight combination on the water surface using the satellite positioning data and inertial navigation data of the mother ship, and solve them using the extended Kalman filter. Real-time output the position, velocity, and attitude of the underwater acoustic transducer carried by the mother ship in the geodetic coordinate system through coordinate transfer;

[0090] Step 3: Establish the state equation and observation equation of the tight combination underwater using the underwater acoustic positioning data, underwater inertial navigation data, and binocular optical positioning data, and real-time output the position, velocity, and attitude of the center of the underwater robot carrier in the geodetic coordinate system;

[0091] Step 4: Comprehensively utilize the satellite positioning data, inertial navigation data, acoustic transducer data of the mother ship, and the acoustic beacon data, inertial navigation data, and binocular optical positioning data of the underwater robot to construct an overall navigation filtering model of the underwater robot with a tight combination of satellite / underwater acoustic / inertial / optical. Feed back the overall navigation filtering information to the tight combination on the water surface and the tight combination underwater to achieve real-time quality control.

[0092] The said Step 1 includes:

[0093] Step 1.1: Measure the satellite positioning receiver, underwater acoustic transducer, and inertial navigation equipment carried by the mother ship, and obtain the GNSS observation values of the mother ship under the time synchronization state, including the original pseudorange observation value P r , phase observation value L r , the acceleration values a m in each direction output by the inertial navigation equipment and the attitude value and indirectly obtain the position r s of the underwater acoustic transducer, velocity v s and attitude

[0094] Step 1.2: The underwater robot is equipped with an acoustic beacon, an inertial navigation system, and a binocular optical camera, and obtains the real-time position value B U of the acoustic beacon, the position value B I output by the underwater inertial navigation, the acceleration a u in each direction, the attitude value and the position value B L of the underwater robot output by the binocular optical camera under the time synchronization state.

[0095] The said Step 2 includes:

[0096] Step 2.1: Construct the ambiguity-fixed solution PPP observation equation and the inertial navigation state equation;

[0097] Respectively establish the satellite positioning pseudorange observation equation P and the phase observation equation L, and perform corrections such as antenna phase center and tide. After linearization, obtain the final PPP observation equation of the ambiguity-fixed solution;

[0098]

[0099] wherein represents the pseudorange observation quantity, represents the phase observation quantity, (X i Y i Z i ) represents the position of a certain satellite, (XYZ) represents the position of the ground receiver, C represents the speed of light, V tR represents the receiver clock error, V tS represents the satellite clock error, V trop represents the tropospheric delay correction, V ion represents the ionospheric delay correction, δρ represents the satellite ephemeris error, ε represents the measurement noise, N represents the integer ambiguity, and λ represents the wavelength;

[0100] After linearization, the inter-satellite single-difference observation equation is:

[0101]

[0102] wherein represents the inter-satellite single-difference quantity between the reference satellite v and other satellites n for the pseudorange observation quantity, represents the inter-satellite single-difference quantity between the reference satellite v and other satellites n for the phase observation quantity, represents the inter-satellite single-difference direction cosine vector of the reference satellite in each direction and the nth satellite, including ω vn represents the inter-satellite single-difference wet delay quantity, δr g , δW, δN vn respectively represent the position error, the wet delay error, and the ambiguity error, and the quantities related to ε all represent the observation noise quantities;

[0103] The state equation of the inertial navigation system is:

[0104]

[0105] wherein δr g , δV g , respectively represent the position error, the velocity error, and the attitude error; δa and δg respectively represent the accelerometer error and the constant zero bias error of the gyroscope, ε a , ε g respectively represent the observation noise vectors of the accelerometer and the gyroscope, is the output of the accelerometer after constant zero bias compensation, representing the projection of the acceleration value of the (body frame) b frame relative to the (inertial frame) i frame in the b frame, represents the attitude matrix of the b frame relative to the (earth coordinate frame) e frame, Denote the projection of the attitude matrix of the e - system relative to the i - system in the e - system, E represents the identity matrix, and those related to × are all skew - symmetric matrices;

[0106] Step 2.2: Expand the established observation equation and state equation to obtain the expanded observation and state equations required for the underwater tight integration. The expanded observation equation is as follows:

[0107]

[0108] where S n (n represents the superscript of a certain type of GNSS system, and 1 - 4 are used to represent 4 different satellite navigation systems) represents the inter - satellite single - difference pseudorange observation P n of satellites obtained in a certain type of navigation system, the phase observation L n , and the Doppler observation D n and the difference from the inertial navigation output form the observation matrix, represents the matrix composed of the inter - satellite single - difference direction cosine vectors of a certain type of navigation system, represents the wet delay of a certain type of navigation system, and those related to δ are all error quantities, represents the ambiguity error of a certain type of navigation system, and those related to ε are the observation noises, represents the ambiguity coefficient matrix of a certain type of navigation system;

[0109] The expanded state equation is as follows:

[0110]

[0111] where δN 1 , δN 2 , δN 3 , δN 4 respectively represent the inter - satellite single - difference ambiguity error quantities of 4 types of navigation systems;

[0112] After constructing the state equation and the observation equation, use the extended Kalman filter for solution to obtain the correction values of each item in the tight - integration state equation, and obtain the position, velocity, and attitude information of the underwater acoustic transducer through coordinate transformation.

[0113] Step 3 includes:

[0114] Step 3.1: Construct the state equation of the inertial / underwater acoustic / optical underwater integrated navigation system:

[0115] For the inertial / underwater acoustic / optical underwater integrated navigation system, take the error quantities of the navigation parameters of the inertial navigation device as the state variables. The position error δr u includes δL, δλ, δh, and the velocity error δV uincluding δV E 、δV N 、δV U ,the attitude angle error δφ in the northeast celestial direction u including δφ E 、δφ N 、δφ U ,the drift δε of the gyroscope u including δε x 、δε y 、δε z ,and the zero bias of the accelerometer including several parameters form the system state variable, ν wn ν and ν an represent white noise quantities;

[0116] Thus, the state variable of the inertial / underwater acoustic / optical integrated navigation system is given:

[0117]

[0118] System noise: T(t) = [ν wx ν wy ν wz ν ax ν ay ν az T

[0119] The state equation is:

[0120]

[0121] where β(t) and γ(t) represent the state transition matrix and the system noise driving matrix respectively, represents the attitude matrix of the b system relative to the n system (navigation system), and γ nn represents the attitude transition matrix;

[0122] are respectively:

[0123]

[0124] Step 3.2: Observation equation of the inertial / underwater acoustic / optical underwater integrated navigation system:

[0125] Subtract the position values given by the underwater beacon and the optical camera from the position value given by the underwater inertial navigation respectively, and multiply by the weight values ρ UI and ρ LI ​The observed quantity is obtained by addition. The initial values of both weights are assigned 1 / 2. The subsequent weights are determined based on the difference between the positioning result of the underwater robot obtained from the third tight integration and the position value given by the underwater beacon, as well as the position value given by the optical camera. The weight formula is as follows:

[0126] e UI =|X3 - B U | e LI =|X3 - B L |

[0127] X3 is the result obtained from the third tight integration. e UI and e LI respectively represent the differences between the positioning result of the underwater robot obtained from the third tight integration and the position value given by the underwater beacon, as well as the position value given by the optical camera;

[0128] The derivation of the observation equation is as follows:

[0129] The position information output by the underwater beacon is y represents the position of the underwater beacon in the earth coordinate system. b u is the position of the carrier determined by the ultra-short baseline in the earth coordinate system. T represents the observation noise. The position information output by the underwater inertial navigation is b I is the position of the carrier determined by the underwater inertial navigation in the earth coordinate system. The position information output by the optical binocular camera is b L is the position of the carrier determined by the binocular optical camera in the earth coordinate system; represents the attitude matrix of the e - frame relative to the b - frame, represents the attitude matrix of the e - frame relative to the n - frame;

[0130] The position weighting gives:

[0131]

[0132] δr = [δx δy δz] T represents the error quantity. δb represents the inertial navigation error quantity. T represents the observation noise. (x, y, z) is the position of the carrier in the earth rectangular coordinate system. (L, λ, h) is the position of the carrier in the earth coordinate system. R is the radius of curvature of the prime vertical circle of the reference ellipsoid. e is the eccentricity of the ellipsoid. K is the conversion matrix of the two - coordinate errors. φ is the attitude angle matrix. Those related to × are all skew - symmetric matrices;

[0133]

[0134] Therefore, the derived observation equation can be written as:

[0135] Z(t) = A(t)X(t) + T(t)

[0136] State variable The observation matrix A(t) is

[0137] The observation noise is is the attitude matrix of the e-frame relative to the b-frame;

[0138] The state equation and the observation equation of the inertial / underwater acoustic / optical underwater integrated navigation system are tightly combined using an extended Kalman filter to obtain the correction values of each state variable, and then the position, velocity, and attitude of the center of the underwater robot carrier are obtained.

[0139] Step 4 includes:

[0140] Step 4.1: The mother ship inertial navigation, satellite positioning data, and the inertial / underwater acoustic / optical underwater integrated navigation system are tightly combined for the third time. The surface and underwater observation equations and the state equation are combined to form a joint state equation and a joint observation equation. The extended Kalman filter is used for the third tight combination to obtain the position, velocity, and attitude of the underwater robot;

[0141] Joint state equation:

[0142] X = AB + CD

[0143]

[0144]

[0145] D = [ν wx ν wy ν wz ν ax ν ay ν az ε a ε g T

[0146] Joint observation equation:

[0147] W = KL + T

[0148]

[0149] T = [ε 1 + Tε 2 + Tε 3 + Tε 4 + T] T

[0150] The joint state equation and the joint observation equation are tightly combined using an extended Kalman filter to obtain each correction quantity;​

[0151] Step 4.2 Quality Control Part:

[0152] Feed back the underwater robot position result obtained from the third tight combination to the underwater robot position result obtained from the underwater tight combination for quality control. Denote the result of the third tight combination as X3(x3, y3, z3), the position result obtained from the underwater tight combination as X2(x2, y2, z2), and the result obtained from the surface tight combination as X1(x1, y1, z1). First, determine whether there are outliers by the residual (RE) being less than three times the mean square error (M). Secondly, use the root mean square error (RMSE) and standard error (SE) to measure the result accuracy and the fluctuation of the positioning result of the positioning result, and construct an evaluation criterion Q. Assign weights of 20%, 40%, and 40% to the residual, root mean square error, and standard error respectively to obtain the Q value. The smaller the Q value, the better the positioning result;

[0153] Taking the quality control of the underwater robot positioning result as an example, the residual should be ensured to be less than 3 times the mean square error Otherwise, it indicates that there are outliers in the positioning result, and the outliers should be removed;

[0154] According to the standard error and root mean square error formulas:

[0155]

[0156] Select the value of n as 10, indicating that quality control is performed every 10 positioning results output by the underwater tight combination and the third tight combination, which is the mean value of 10 positioning results, is the predicted value of 10 positioning results. In the underwater quality control process, this value selects the result output by the underwater inertial navigation. Calculate the Q values of the underwater tight combination and the third tight combination respectively according to the following formula and compare them;

[0157] Q i = 0.2·RE + 0.4·SE + 0.4·RMSE

[0158] If the Q value of the third tight combination is less than the Q value of the underwater tight combination, output the underwater robot positioning result obtained by the third tight combination as the final result. The quality control part of the surface tight combination can be obtained in the same way.

[0159] As described above, it is only a preferred specific implementation manner of the present invention, but the protection scope of the present invention is not limited thereto. Any person skilled in the art within the technical scope disclosed by the present invention, according to the technical solution of the present invention and its inventive concept, makes equivalent substitutions or changes, and should be covered by the protection scope of the present invention.

Claims

1. A method for positioning an underwater robot using a satellite, hydroacoustic, inertial and optical tight combination, characterized in that: The following steps are involved: Step 1: The measurement mother ship is equipped with a satellite positioning receiver, an ultra-short baseline hydroacoustic transducer, and an inertial navigation system. The underwater robot is equipped with an acoustic beacon, an inertial navigation system, and a binocular optical camera. Multi-source sensors conduct real-time observations in a time-synchronized state. Step 2: Use the satellite positioning data and inertial navigation data of the mother ship to establish the water tight combined state equation and observation equation, solve them using the extended Kalman filter, and output the position, velocity and attitude of the hydroacoustic transducer carried by the mother ship in the geodetic coordinate system in real time through coordinate transfer; Step 3: Using the hydroacoustic positioning data, underwater inertial navigation data, and binocular optical positioning data, the underwater tightly combined state equation and observation equation are established to output the position, velocity, and attitude of the center of the underwater robot carrier in the geodetic coordinate system in real time; Step 4: Comprehensively utilize the satellite positioning data, inertial navigation data, acoustic transducer data of the mother ship, as well as the acoustic beacon data, inertial navigation data, and binocular optical positioning data of the underwater robot to build an overall navigation filter model of the underwater robot with a satellite\hydroacoustic\inertial\optical tight combination, and feed the overall navigation filter information back to the surface tight combination and the underwater tight combination to achieve real-time quality control.

2. The underwater robot positioning method of the satellite-hydroacoustic-inertial-optical tight combination according to claim 1 is characterized in that: The step 1 comprises: Step 1.1: The mother ship is equipped with a satellite positioning receiver, a hydroacoustic transducer, and an inertial navigation device, and the mother ship GNSS observation values ​​are obtained in a time-synchronized state, including the original pseudo-range observation value P r , phase observation value L r , the acceleration value a in each direction output by the inertial navigation device m With attitude value And the position r of the underwater acoustic transducer is indirectly obtained by tight combination and coordinate transfer of the above values s , speed v s With posture Step 1.2: The underwater robot is equipped with an acoustic beacon, an inertial navigation system, and a binocular optical camera, and the real-time position value B of the underwater acoustic beacon is obtained by observation in a time-synchronous state. U , the position value B output by the underwater inertial navigation I , acceleration in each direction a u , attitude value The underwater robot position value B output by the binocular optical camera L .

3. The underwater robot positioning method of the satellite-hydroacoustic-inertial-optical tight combination according to claim 1 is characterized in that: The step 2 comprises: Step 2.1: Construct the ambiguity fixed solution PPP observation equation and inertial navigation state equation; The satellite positioning pseudorange observation equation P and phase observation equation L are respectively established, and corrections such as antenna phase center and tide are performed. After linearization, the PPP observation equation with the final ambiguity fixed solution is obtained. in represents the pseudorange observation, represents the phase observation, (X i Y i Z i ) represents the position of a satellite, (XYZ) represents the position of the ground receiver, C represents the speed of light, V tR Represents the receiver clock error, V tS represents the satellite clock error, V trop represents the tropospheric delay correction, V ion represents the ionospheric delay correction, δρ represents the satellite ephemeris error, ε represents the measurement noise, N represents the integer ambiguity, and λ represents the wavelength; The linearized intersatellite single-difference observation equation is: in represents the inter-satellite single difference between the pseudo-range observation reference star v and other stars n, represents the inter-satellite single difference between the phase observation reference star v and other stars n, Represents the inter-satellite single difference direction cosine vector between the reference satellite and the nth satellite in each direction, including ω vn Denotes the inter-satellite single difference wet delay, δr g , δW, δN vn They represent the position error, wet delay error and ambiguity error respectively, and the quantities related to ε all represent the amount of observation noise; The state equation of the inertial navigation system is: where δr g , δV g , They represent position error, velocity error, and attitude error respectively; δa and δg represent the accelerometer error and the gyro's constant bias error respectively, and ε a , ε g denote the observed noise vectors of the accelerometer and gyroscope respectively, is the accelerometer output after constant zero bias compensation, which indicates the projection of the acceleration value of the (carrier system) b system relative to the (inertial system) i system in the b system. Represents the attitude matrix of system b relative to system e (Earth coordinate system), It represents the projection of the attitude matrix of the e system relative to the i system in the e system, E represents the unit matrix, and those related to × are all antisymmetric matrices; Step 2.2: Expand the observation equation and state equation listed to obtain the expanded observation and state equation required for the water tight combination. The expanded observation equation is as follows: Where S n (n represents a certain type of GNSS system, and the superscript 1-4 represents 4 different types of satellite navigation systems) represents the inter-satellite single-difference pseudo-range observation value P obtained in a certain type of navigation system. n , phase observation L n , and the Doppler observation value D n And the inertial output The observation matrix composed of the difference of represents the matrix composed of single-difference direction cosine vectors between satellites of a certain type of navigation system, It represents the wet delay of a certain type of navigation system, and those related to δ are all errors. represents the ambiguity error of a certain type of navigation system, and the one related to ε is the observation noise, represents the ambiguity coefficient matrix of a certain type of navigation system; The expanded state equation is as follows: Where δN 1 , δN 2 , δN 3 , δN 4 Respectively represent the inter-satellite single-difference ambiguity errors of the four types of navigation systems; After constructing the state equation and observation equation, the extended Kalman filter is used to solve them, and the correction values ​​in the tightly combined state equation are obtained. The position, velocity and attitude information of the underwater acoustic transducer are obtained through coordinate transformation.

4. The underwater robot positioning method of the satellite-hydroacoustic-inertial-optical tight combination according to claim 1 is characterized in that: The step 3 comprises: Step 3.1: Construct the state equation of the inertial\hydroacoustic\optical underwater integrated navigation system: For the inertial, hydroacoustic, and optical underwater integrated navigation system, the error of the navigation parameters of the inertial navigation device is taken as the state quantity, and the position error δr u Including δL, δλ, δh, and the velocity error δV in the northeast direction u Including Delta V E , δV N , δV U , attitude angle error δφ in the northeast direction u Including δφ E ,δφ N ,δφ U , the drift of the gyroscope δε u Including δε x ,δε y ,δε z , and the zero bias of the accelerometer include Several parameters make up the system state quantity, ν wn With ν an represents the amount of white noise; The state quantity of the inertial\hydroacoustic\optical integrated navigation system is given as follows: System noise:T(t)=[n wx n wy n wz n ax n ay n az ] T The state equation is: Where β(t) and γ(t) represent the state transfer matrix and system noise driving matrix respectively. represents the attitude matrix of b system relative to (navigation system) n system, γ nn represents the attitude transfer matrix; They are: Step 3.2: Observation equations of inertial, hydroacoustic and optical underwater integrated navigation system: The position values ​​given by the underwater beacon, the position values ​​given by the optical camera and the position values ​​given by the underwater inertial navigation are subtracted and multiplied by the weights ρ UI and ρ LI The observation value is obtained by adding them together. The initial values ​​of the two weights are both assigned 1 / 2. The subsequent weights are determined based on the position error according to the position result of the underwater robot obtained by the third tight combination and the position value given by the underwater beacon and the position value given by the optical camera. The weight formula is as follows: e UI =|X3-B U | e LI =|X3-B L | X3 is the result of the third tight combination, e UI , e LI They represent the differences between the underwater robot positioning result obtained by the third tight combination and the position value given by the underwater beacon and the position value given by the optical camera; The observation equation is derived as follows: The position information output by the underwater beacon is y represents the position of the underwater beacon in the earth coordinate system, b u The position of the carrier determined by the ultra-short baseline in the earth coordinate system, T represents the observation noise; the position information output by the underwater inertial navigation is b I The position of the carrier determined by the underwater inertial navigation in the earth coordinate system, and the position information output by the optical binocular camera is b L The position of the carrier determined for the binocular optical camera in the earth coordinate system; represents the attitude matrix of the e system relative to the b system, Represents the attitude matrix of the e system relative to the n system; Position weighting gives: δr=[δx δy δz] T represents the error, δb represents the inertial navigation error, T represents the observation noise, (x, y, z) represents the position of the object in the earth's rectangular coordinate system, (L, λ, h) represents the position of the object in the earth's coordinate system, R represents the radius of curvature of the unitary circle of the reference ellipsoid, e represents the eccentricity of the ellipsoid, K represents the two coordinate error conversion matrices, φ represents the attitude angle matrix, and all those related to × are antisymmetric matrices; Therefore, the observation equation can be written as: Z(t)=A(t)X(t)+T(t) State quantity The observation matrix A(t) is The observation noise is is the attitude matrix of system e relative to system b; The state equation of the inertial\hydroacoustic\optical underwater integrated navigation system and the observation equation are tightly combined using the extended Kalman filter to obtain the correction value of each state quantity, and then the position, velocity and attitude of the center of the underwater robot carrier are obtained.

5. The underwater robot positioning method of the satellite-hydroacoustic-inertial-optical tight combination according to claim 1 is characterized in that: The step 4 comprises: Step 4.1: The mother ship inertial navigation, satellite positioning data and inertial, acoustic and optical underwater integrated navigation system are combined for the third time, and the surface and underwater observation equations and state equations are combined to form joint state equations and joint observation equations. The extended Kalman filter is used for the third time to obtain the position, velocity and attitude of the underwater robot. Joint state equation: X=AB+CD D=[ν wx n wy n wz n ax n ay n az e a e g ] T Joint observation equation: W=KL+T T=[e 1 +The 2 +The 3 +The 4 +T] T The joint state equation and the joint observation equation are tightly combined by using the extended Kalman filter to obtain various correction quantities; Step 4.2 Quality Control Section: The underwater robot position result obtained by the third tight combination is fed back to the underwater robot position result obtained by the underwater tight combination for quality control. The third tight combination result is recorded as X3 (x3, y3, z3), the position result obtained by the underwater tight combination is recorded as X2 (x2, y2, z2), and the result obtained by the above-water tight combination is recorded as X1 (x1, y1, z1). First, the residual (RE) is less than three times the mean error (M) to determine whether there are abnormal values. Secondly, the root mean square error (RMSE) and standard error (SE) are used to measure the result accuracy and fluctuation of the positioning result. The evaluation standard Q is constructed, and the residual, root mean square error, and standard error are weighted with 20%, 40%, and 40% respectively to obtain the Q value. The smaller the Q value, the better the positioning result. Taking the quality control of underwater robot positioning results as an example, the residual Less than 3 times the mean error Otherwise, it means that there are outliers in the positioning results, and the outliers should be removed; According to the standard error and root mean square error formula: The value of n is selected as 10, which means that the underwater tight combination and the third tight combination will perform quality control every 10 positioning results output, and X is the average of the 10 positioning results. is the predicted value of 10 positioning results. In the underwater quality control process, this value selects the result of underwater inertial navigation output. The Q value of underwater tight combination and the third tight combination is calculated and compared according to the following formula; Q i =0.2·RE+0.4·SE+0.4·RMSE If the Q value of the third tight combination is less than the Q value of the underwater tight combination, the underwater robot positioning result obtained by the third tight combination is output as the final result. The quality control part of the water tight combination can be obtained in the same way.

Images