Method and device for determining pose parameters of an underwater vehicle

Abstract

The present application provides a method and device for determining pose parameters of an underwater vehicle, relating to the technical field of underwater vehicle control, including: obtaining the position coordinates of each target receiving plane array in the carrier coordinate system, and the depth difference between the underwater vehicle and the carrier of the underwater vehicle; for any target receiving plane array, obtaining the first direction angle of the underwater vehicle relative to the target receiving plane array; performing coordinate transformation on the first direction angle to obtain the second direction angle of the underwater vehicle in various carrier coordinate systems; based on the second direction angle of the underwater vehicle in various carrier coordinate systems, the depth difference, and the position coordinates of each target receiving plane array in the carrier coordinate system, determining the target pose parameters of the underwater vehicle in the carrier coordinate system. The present application can achieve stable, low-cost, and multi-scenario measurement of the position and azimuth angle of the underwater vehicle relative to the carrier.

Description

Technical Field

[0001] The present application relates to the technical field of underwater vehicle control, and more particularly, to a method and device for determining pose parameters of an underwater vehicle. Background Art

[0002] An autonomous underwater vehicle is abbreviated as AUV (Autonomous Underwater Vehicle). Navigation and positioning is one of the key technologies of an AUV. Only on the premise of reliable and accurate positioning of the vehicle body, the measured data obtained is meaningful.

[0003] Currently, the distance and azimuth angle between an autonomous underwater vehicle and a carrier (mother ship) are mainly measured in a cooperative manner. However, this method requires the installation of an acoustic cooperative beacon on the autonomous underwater vehicle, which not only affects the dynamic characteristics and endurance of the vehicle, but also is greatly affected by noise interference such as ocean background noise, has a high cost, and the measurement is unstable and vulnerable to the influence of the external environment, with strong limitations in application scenarios. Summary of the Invention

[0004] The purpose of the embodiments of the present application is to provide a method and device for determining pose parameters of an underwater vehicle to solve the above problems existing in the prior art, and to realize stable, low-cost, and multi-scenario measurement of the position and azimuth angle of an underwater vehicle relative to a carrier.

[0005] In a first aspect, a method for determining pose parameters of an underwater vehicle is provided, which is applied to a display and control computer of a system for determining pose parameters of the underwater vehicle. The system further includes: a plurality of receiving planar arrays disposed on a carrier of the underwater vehicle. The method may include:

[0006] Obtain the position coordinates of each target receiving planar array in the carrier coordinate system, and the depth difference between the underwater vehicle and the carrier of the underwater vehicle.

[0007] For any target receiving planar array, obtain the first direction angle of the underwater vehicle relative to the target receiving planar array.

[0008] Perform coordinate transformation on the first direction angle to obtain the second direction angle of the underwater vehicle in various carrier coordinate systems; where any type of carrier coordinate system is constructed with any target receiving planar array as the origin.

[0009] Based on the second direction angle of the underwater vehicle in various carrier coordinate systems, the depth difference, and the position coordinates of each target receiving planar array in the carrier coordinate system, determine the target pose parameters of the underwater vehicle in the carrier coordinate system.

[0010] In an alternative implementation, the system further includes: a multi-channel signal conditioner, a signal processor, and a display and control unit;

[0011] Multiple receiving planar arrays are all connected to the multi-channel signal conditioner through watertight cables; the multi-channel signal conditioner and the signal processor are connected through a through-hull cable; the signal processor and the display and control unit are communicatively connected;

[0012] Each target receiving planar array is configured to receive the sound signal radiated by the underwater vehicle, perform acoustic-electric conversion on the sound signal to obtain a differential electrical signal; and send the differential electrical signal to the multi-channel signal conditioner;

[0013] The multi-channel signal conditioner is configured to perform filtering, amplification processing, and analog-to-digital conversion on the differential electrical signals of each target receiving planar array received, to obtain digital signals corresponding to each target receiving planar array; and send the digital signals corresponding to each target receiving planar array to the signal processor;

[0014] The signal processor is configured to perform beamforming processing on the digital signals corresponding to each target receiving planar array received, to obtain a first direction angle of the underwater vehicle relative to each target receiving planar array; and send the first direction angle to the display and control unit;

[0015] The display and control unit is configured to perform coordinate conversion on the first direction angle to obtain a second direction angle of the underwater vehicle in various carrier coordinate systems; and determine target pose parameters of the underwater vehicle in the carrier coordinate system based on each second direction angle, the depth difference between the underwater vehicle and the carrier of the underwater vehicle, and the position coordinates of each target receiving planar array in the carrier coordinate system.

[0016] In an alternative implementation, at least two receiving planar arrays are disposed on both sides of the carrier.

[0017] In an alternative implementation, before obtaining the position coordinates of each target receiving planar array in the carrier coordinate system, the method further includes:

[0018] Obtaining the relative position relationship between the underwater vehicle and the carrier;

[0019] According to the relative position relationship, taking the side of the carrier where the underwater vehicle is located as the target side;

[0020] Taking the receiving planar array located on the target side as the target receiving planar array.

[0021] In an alternative implementation, the target pose parameters include: target position coordinates, target distance, and target azimuth angle;

[0022] The position coordinates of each target receiving plane array in the carrier coordinate system include: the ordinate of each target receiving plane array in the carrier coordinate system;

[0023] Before determining the target pose parameters of the underwater vehicle in the carrier coordinate system, the method further includes:

[0024] According to the target side where the underwater vehicle is located, determine the positive and negative directions of the ordinates of each target receiving plane array in the carrier coordinate system.

[0025] In an alternative implementation, based on the second direction angle, the depth difference, and the position coordinates of each target receiving plane array in the carrier coordinate system, determining the target pose parameters of the underwater vehicle in the carrier coordinate system includes:

[0026] Based on the second direction angle of the underwater vehicle in various carrier coordinate systems, the position coordinates of each target receiving plane array in the carrier coordinate system, and the depth difference, determine the slant range from the underwater vehicle to each target receiving plane array;

[0027] According to the second direction angle of the underwater vehicle in various carrier coordinate systems, the slant range, the depth difference, the position coordinates of each target receiving plane array in the carrier coordinate system, and the positive and negative directions of the corresponding ordinates, calculate the target position coordinates of the underwater vehicle in the carrier coordinate system;

[0028] According to the target position coordinates, calculate the target distance between the underwater vehicle and the carrier and the target azimuth angle between the underwater vehicle and the carrier.

[0029] In an alternative implementation, after determining the target pose parameters of the underwater vehicle in the carrier coordinate system, the method further includes:

[0030] Obtain the real-time pose parameters of the carrier in the geodetic coordinate system;

[0031] Based on the target position coordinates and the real-time pose parameters of the carrier, determine the position coordinates of the underwater vehicle in the geodetic coordinate system.

[0032] In an alternative implementation, the real-time pose parameters of the carrier include: the speed and the position coordinates of the carrier;

[0033] After determining the position coordinates of the underwater vehicle in the geodetic coordinate system, the method further includes:

[0034] Obtain the historical position coordinates of the underwater vehicle in the geodetic coordinate system and the historical carrier position coordinates of the carrier in the geodetic coordinate system at the previous moment of the current moment;

[0035] Determine the vehicle trajectory of the vehicle in the geodetic coordinate system at the current moment according to the real-time pose parameters of the vehicle, the vehicle speed, and the historical vehicle position coordinates;

[0036] Determine the target trajectory of the underwater vehicle in the geodetic coordinate system at the current moment according to the historical position coordinates of the underwater vehicle in the geodetic coordinate system and the position coordinates at the current moment;

[0037] Fit the target trajectory and the vehicle trajectory to obtain the angle between the target trajectory and the vehicle trajectory.

[0038] In a second aspect, an electronic device is provided. The electronic device includes a processor, a communication interface, a memory, and a communication bus. Among them, the processor, the communication interface, and the memory complete communication with each other through the communication bus;

[0039] The memory is used to store a computer program;

[0040] The processor is configured to implement the method steps described in any one of the above first aspects when executing the program stored on the memory.

[0041] In a third aspect, a computer-readable storage medium is provided. The computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the method steps described in any one of the above first aspects are implemented.

[0042] This application uses a passive non-cooperative method to measure the distance and azimuth angle of an underwater autonomous vehicle relative to a carrier vehicle. It uses the navigation noise data of the underwater vehicle and does not require the installation of an acoustic cooperative beacon. It has a wide application range, strong applicability, and strong stability. It can achieve accurate measurement of position and azimuth angle in the scenario of short-range measurement, and is suitable for application occasions such as the carrier vehicle recovering the underwater autonomous vehicle.

[0043] This application combines a receiving planar array, a multi-channel signal conditioner, and a signal processor with signal processing technology to reduce the influence of ocean noise; this application is not easily affected by the external environment. Description of the Drawings

[0044] In order to more clearly illustrate the technical solutions of the embodiments of the present application, the following will briefly introduce the drawings required for the embodiments of the present application. It should be understood that the following drawings only show some embodiments of the present application, and therefore should not be regarded as limiting the scope. For those of ordinary skill in the art, other related drawings can be obtained based on these drawings without creative efforts.

[0045] Figure 1 It is an architecture diagram of a system for determining the pose parameters of an underwater vehicle provided by an embodiment of the present application;

[0046] Figure 2 Schematic diagram of a receiving planar array structure provided by an embodiment of the present application;

[0047] Figure 3 Schematic diagram of the size of a receiving planar array provided by an embodiment of the present application;

[0048] Figure 4 Schematic diagram of the structure of an electronic cabin of a multi-channel signal conditioner provided by an embodiment of the present application;

[0049] Figure 5 Block diagram of a multi-channel signal conditioner provided by an embodiment of the present application;

[0050] Figure 6 Flowchart of a method for determining the pose parameters of an underwater vehicle provided by an embodiment of the present application;

[0051] Figure 7 Schematic diagram of a method for determining the pose parameters of an underwater vehicle provided by an embodiment of the present application;

[0052] Figure 8 Schematic diagram of the principle of target distance measurement provided by an embodiment of the present application;

[0053] Figure 9 Schematic diagram of the principle of distance measurement under the condition of ideal no installation deviation provided by an embodiment of the present application;

[0054] Figure 10 Schematic diagram of the structure of an electronic device provided by an embodiment of the present application;

[0055] In the figure: 1. Receiving planar array; 2. Multi-channel signal conditioner; 3. Signal processor; 4. Display and control machine; 5. Penetrating cabin cable. Specific embodiments

[0056] Next, the technical solutions in the embodiments of the present application will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present application. Obviously, the described embodiments are only a part of the embodiments of the present application, rather than all the embodiments. Based on the embodiments of the present application, all other embodiments obtained by those of ordinary skill in the art without creative efforts belong to the scope of protection of the present application.

[0057] The method for determining the pose parameters of the underwater vehicle provided by the embodiment of the present application can be applied in, for example Figure 1In the shown system architecture, the system for determining the pose parameters of the underwater vehicle is set on the carrier of the underwater vehicle. This system includes: a receiving planar array 1, a multi-channel signal conditioner 2, a signal processor 3, and a display and control unit 4. Multiple receiving planar arrays are all connected to the multi-channel signal conditioner through watertight cables. The multi-channel signal conditioner and the signal processor are connected through a feedthrough cable 5. The signal processor and the display and control unit are communicatively connected. At least two receiving planar arrays are provided on both the left and right sides of the carrier.

[0058] The receiving planar array 1 is used to receive the sound signals (including active sound or passive navigation noise) radiated by the underwater vehicle, and perform acoustic-electric conversion on the sound signals to obtain differential electrical signals, and send the differential electrical signals to the multi-channel signal conditioner.

[0059] The multi-channel signal conditioner 2 consists of a conditioning and acquisition circuit, a stainless steel airtight enclosure, and a watertight socket. It is used to filter, amplify, and perform AD conversion on the differential electrical signals of each target receiving planar array to obtain digital signals of each target receiving planar array, and then transmit them to the signal processor through optical fibers.

[0060] The signal processor 3 consists of an optical fiber board, an FPGA processing board, a gigabit network sub-board, a chassis power supply, etc. It is used to perform beamforming processing on the signals corresponding to each target receiving planar array to obtain a first direction angle, and send the first direction angle to the display and control unit.

[0061] The display and control unit 4 consists of a display and control computer and display and control software. It is used to determine the target pose parameters of the underwater vehicle in the carrier coordinate system based on the received first direction angle in combination with the depth difference between the underwater vehicle and the carrier, and the position coordinates of each target receiving planar array in the carrier coordinate system. Specifically, the display and control unit completes operations such as target trajectory display, distance and azimuth calculation, comprehensive display and control of the system working mode, measurement result storage, and base array position calibration calculation.

[0062] In the embodiment of the present application, any receiving planar array is composed of 24 cylindrical hydrophone elements and 1 single-element broadband hydrophone. The 24 elements are arranged in parallel in the horizontal direction. According to the basic principles of array layout, in order to form good spatial directivity, the elements are arranged with equal spacing, and the element spacing is set to 70 mm (corresponding to the half-wavelength of a 10.7 kHz signal). The schematic diagram of the specific layout of the planar array is as Figure 2 shown.

[0063] In the embodiment of the present application, 25 elements of any receiving planar array will be installed in a polycarbonate plate with a length of 180 cm and a width of 10 cm. The polycarbonate plate is integrally installed on the titanium alloy frame through an anti-sound baffle; after any receiving planar array is completed by pouring polyurethane, the front surface as the radiation surface will form a complete plane, and there are 12 M10 mounting holes and two connector ports with a diameter of 80 mm on the back. The output signal of the planar array is led out through two multi-core (32-core) watertight cables with shielding layers. The outer diameter of the cable is about 20 mm and the length is 1 m, as Figure 3 shown.

[0064] In the embodiment of the present application, as Figure 4 shown, the electronic cabin adopts a watertight and pressure-resistant interface and can be installed outside the carrier and is connected to each receiving planar array through watertight cables; the conditioning and acquisition circuit in the electronic cabin mainly consists of a signal conditioning circuit, a multi-channel acquisition circuit, a main control and transmission circuit, and a power management circuit, as Figure 5 shown.

[0065] In the embodiment of the present application, the fiber optic board communicates with the multi-channel signal conditioner to complete the conversion of optical signals into electrical signals; the FPGA processing board completes real-time data processing and uploads it to the display control machine in real time through the gigabit network board; the chassis power supply completes the power conversion and output required by each component inside the signal processor; the signal processor is installed inside the carrier, and the chassis of the signal processor does not need to be watertight.

[0066] The display control machine can be in a server or in a terminal with strong computing power. The server can be a physical server, or a server cluster or distributed system composed of multiple physical servers, or a cloud server providing basic cloud computing services such as cloud services, cloud databases, cloud computing, cloud functions, cloud storage, network services, cloud communications, middleware services, domain name services, security services, Content Delivery Network (CDN), and big data and artificial intelligence platforms. The terminal can be a user equipment (UE) such as a mobile phone, a smart phone, a laptop computer, a digital broadcast receiver, a personal digital assistant (PDA), a tablet computer (PAD), a handheld device, a vehicle-mounted device, a wearable device, a computing device, or other processing devices connected to a wireless modem, a mobile station (MS), a mobile terminal, etc. The terminal and the server can be directly or indirectly connected through wired or wireless communication methods, which are not limited in the present application.

[0067] The preferred embodiments of the present application will be described below in conjunction with the accompanying drawings of the specification. It should be understood that the preferred embodiments described herein are only used to illustrate and explain the present application, and are not used to limit the present application. And without conflict, the embodiments in the present application and the features in the embodiments can be combined with each other.

[0068] Figure 6 It is a schematic flowchart of a method for determining the pose parameters of an underwater vehicle provided by an embodiment of the present application. As Figure 6 shown, the method may include:

[0069] Step S610: Obtain the position coordinates of each target receiving planar array in the carrier coordinate system, and the depth difference between the underwater vehicle and the carrier of the underwater vehicle; for any target receiving planar array, obtain the first direction angle of the underwater vehicle relative to the target receiving planar array.

[0070] In the embodiment of the present application, before obtaining the position coordinates of each target receiving planar array in the carrier coordinate system, the method further includes: obtaining the relative position relationship between the underwater vehicle and the carrier; according to the relative position relationship, select the side where the underwater vehicle exists from both sides of the carrier as the target side; and use the receiving planar array located on the target side as the target receiving planar array.

[0071] In the embodiment of the present application, the relative position relationship between the underwater vehicle and the carrier can be determined according to the amplitude of the sound signals received by each receiving planar array, or other methods can also be used to obtain it.

[0072] In the embodiment of the present application, the position coordinates of each target receiving planar array in the carrier coordinate system include: the transverse wave, the vertical coordinate, and the depth coordinate of each target receiving planar array in the carrier coordinate system.

[0073] In practical applications, a plurality of receiving planar arrays are provided on both sides of the carrier, but not all receiving planar arrays work simultaneously, or rather, not all receiving planar arrays can receive the signals returned from the underwater vehicle, that is, the pose parameters of the underwater vehicle relative to the receiving planar array cannot be obtained. Therefore, the target receiving planar array is first determined according to the side of the carrier where the underwater vehicle is located; for example, if the underwater vehicle is on the left side of the carrier, then the left side is the target side, and the receiving planar array located on the left side is the target receiving planar array.

[0074] In the embodiment of the present application, the first direction angle is obtained through adaptive beamforming processing of the signals received by each target receiving planar array.

[0075] In practical applications, since each receiving planar array is installed separately, there are inevitably installation angle deviations and installation position deviations. The installation angle deviation is controllable, so its impact on the final result is relatively small. Here, it is temporarily assumed that the installation angle deviation is not considered, and only the installation position deviation is considered.

[0076] Step S620: Perform coordinate transformation on the first direction angle to obtain the second direction angle of the underwater vehicle in various carrier coordinate systems.

[0077] In the embodiments of the present application, a plurality of carrier-like coordinate systems are constructed with each target receiving planar array as the origin.

[0078] In the embodiments of the present application, the coordinate rotation of the first direction angle of the underwater vehicle relative to each target receiving planar array is performed to obtain the second direction angle of the underwater vehicle in the carrier-like coordinate system centered on each target receiving planar array.

[0079] Step S630: Based on the second direction angle of the underwater vehicle in various carrier coordinate systems, the depth difference, and the position coordinates of each target receiving planar array in the carrier coordinate system, determine the target pose parameters of the underwater vehicle in the carrier coordinate system.

[0080] In the embodiments of the present application, before determining the target pose parameters of the underwater vehicle in the carrier coordinate system, the method further includes: determining the positive and negative directions of the ordinate of each target receiving planar array in the carrier coordinate system according to the target side where the underwater vehicle is located.

[0081] In the embodiments of the present application, if the underwater vehicle is on the left side of the carrier, the ordinate symbol is negative; if the underwater vehicle is on the right side of the carrier, the ordinate symbol is positive; when the underwater vehicle passes through the carrier, the symbol change of the ordinate is determined according to the real-time position of the underwater vehicle.

[0082] In the embodiments of the present application, the target pose parameters include: target position coordinates, target distance, and target azimuth angle.

[0083] In the embodiments of the present application, the determination of the target pose parameters includes:

[0084] Based on the second direction angle of the underwater vehicle in various carrier coordinate systems, the position coordinates of each target receiving planar array in the carrier coordinate system, and the depth difference, determine the slant range from the underwater vehicle to each target receiving planar array; according to the second direction angle of the underwater vehicle in various carrier coordinate systems, the slant range, the depth difference, the position coordinates of each target receiving planar array in the carrier coordinate system, and the positive and negative directions of the corresponding ordinate, calculate the target position coordinates of the underwater vehicle in the carrier coordinate system; according to the target position coordinates and the positive and negative directions of the ordinate, calculate the target distance between the underwater vehicle and the carrier and the target azimuth angle between the underwater vehicle and the carrier.

[0085] In the embodiment of the present application, after obtaining the target azimuth angle, it is also necessary to smooth and average the target azimuth angle to obtain the final target azimuth angle.

[0086] In the embodiment of the present application, after determining the target pose parameters of the underwater vehicle in the vehicle coordinate system, the method further includes:

[0087] Obtaining the real-time pose parameters of the vehicle in the geodetic coordinate system; determining the position coordinates of the underwater vehicle in the geodetic coordinate system based on the target position coordinates and the real-time pose parameters of the vehicle.

[0088] In the embodiment of the present application, the real-time pose parameters of the vehicle include: the real-time position coordinates of the origin of the vehicle coordinate system in the geodetic coordinate system, as well as the pitch angle, roll angle, heading, and speed of the vehicle; the pitch angle and roll angle of the vehicle are measured by using the inertial navigation equipment carried on the vehicle; the pitch angle is defined as positive when the bow is downward from the horizontal plane, and the roll angle is defined as positive when the starboard side is upward from the horizontal plane; the heading direction is defined as: the bow is eastward as positive.

[0089] In the embodiment of the present application, after determining the position coordinates of the underwater vehicle in the geodetic coordinate system, the method further includes:

[0090] Obtaining the historical position coordinates of the underwater vehicle in the geodetic coordinate system and the historical vehicle position coordinates of the vehicle in the geodetic coordinate system at the previous moment of the current moment; determining the vehicle trajectory of the vehicle in the geodetic coordinate system at the current moment according to the real-time pose parameters of the vehicle, the speed, and the historical vehicle position coordinates; determining the target trajectory of the underwater vehicle in the geodetic coordinate system at the current moment according to the historical position coordinates of the underwater vehicle in the geodetic coordinate system and the position coordinates at the current moment; fitting the target trajectory and the vehicle trajectory to obtain the included angle between the target trajectory and the vehicle trajectory.

[0091] In the embodiment of the present application, the duration of the current moment and the previous moment of the current moment are both the same as the duration of the configured measurement period. The current moment is the current measurement period, and the previous moment of the current moment is the previous measurement period of the current measurement period; for example, if the measurement period is every hour, then the current moment is the current hour, and the previous moment of the current moment is the previous hour, that is, the just-passed hour.

[0092] During the underwater measurement process, since both the underwater vehicle and the carrier are in motion, and the target position obtained in each measurement cycle is based on the target position in the current carrier coordinate system. In order to obtain the angle between the trajectory of the underwater vehicle and the motion trajectory of the carrier to clarify their relative motion states, the target positioning result and the carrier position can be subjected to coordinate rotation and translation according to the speed, heading, and attitude data of the carrier to obtain the target trajectory and the carrier trajectory based on the fixed geodetic coordinate system, and then the angle between the target trajectory and the carrier trajectory can be calculated according to the trajectory fitting data.

[0093] As Figure 7 shown, taking 2 target receiving plane arrays as an example, the method for determining the pose parameters of the underwater vehicle provided by the embodiment of the present application includes the following steps:

[0094] First, the first target receiving plane array measures the first direction angle of the underwater vehicle in the first target receiving plane array coordinate system, and the second target receiving plane array measures the first direction angle of the underwater vehicle in the second target receiving plane array coordinate system;

[0095] Secondly, through coordinate rotation, the second direction angles in the pseudo-carrier coordinate system centered on the first target receiving plane array and the second direction angles in the pseudo-carrier coordinate system centered on the second target receiving plane array are respectively obtained;

[0096] Then, using each second direction angle, the position coordinates of the first target receiving plane array and the second target receiving plane array in the carrier coordinate system, and the depth difference data between the carrier and the underwater vehicle target, equations are listed according to the geometric relationship to calculate the slant ranges from the target to the first target receiving plane array and the second target receiving plane array, and at the same time, the target position coordinates in the carrier coordinate system are calculated;

[0097] Finally, obtaining the position of the origin of the carrier coordinate system in the geodetic coordinate system and the azimuth attitude of the carrier, the target position in the geodetic coordinate system can also be obtained. Finally, fitting calculations are performed on the target trajectory and the carrier trajectory in the geodetic coordinate system to obtain the angle between the underwater vehicle and the carrier.

[0098] In the embodiment of the present application, it is set that the position coordinates of the center of the first target receiving plane array in the underwater carrier coordinate system are , and the position coordinates of the center of the second target receiving plane array in the underwater carrier coordinate system are . Assuming that the center of the first target receiving plane array is on the X-axis of the underwater carrier coordinate system, that is, , this assumption does not affect the generality of the measurement result. In the case where and are not equal to 0, the calculation difference can be eliminated by simple translation. Assuming , the schematic diagram of the geometric relationship between the underwater vehicle and the target receiving plane array in the underwater vehicle coordinate system is as Figure 8 shown. At this time, it is assumed that the center of the second target receiving plane array is not on the X-axis of the underwater vehicle coordinate system.

[0099] Figure 8 In , the center of the first target receiving plane array is represented by "A", the center of the second target receiving plane array is represented by "B", the underwater vehicle is represented by "S", and the projection of the underwater vehicle on the horizontal plane is represented by " ". The projection on the X-axis of the underwater vehicle coordinate system is represented by "C". are the distances from the underwater vehicle to the first target receiving plane array, the second target receiving plane array, and the origin of the underwater vehicle coordinate system respectively. is the depth difference between the underwater vehicle and the underwater vehicle (or the Z coordinate of the underwater vehicle in the quasi-underwater vehicle coordinate system with the first target receiving plane array as the origin), which is assumed to be a known quantity.

[0100] According to Figure 8 in the geometric relationship, we can obtain: , , the position coordinates of the underwater vehicle in the quasi-underwater vehicle coordinate system with the first target receiving plane array as the origin are:

[0101] ;

[0102] The position coordinates of the underwater vehicle in the quasi-underwater vehicle coordinate system with the second target receiving plane array as the origin are:

[0103] ;

[0104] Among them, and respectively represent the second direction angles of the underwater vehicle in each type of vehicle coordinate system; .

[0105] In addition, according to the fact that the position of the underwater vehicle in the underwater vehicle coordinate system is unique after coordinate translation, we can obtain:

[0106] ;

[0107] Thus, a binary quadratic equation system can be obtained as:

[0108] ;

[0109] The unknowns of the above binary quadratic equation system are and , and the rest are all known quantities. Therefore, and , after obtaining and , the coordinates of the underwater vehicle in the underwater vehicle coordinate system can be calculated, and finally the target distance can be obtained .

[0110] In the embodiment of the present application, after obtaining and , the position coordinates of the underwater vehicle in the underwater vehicle coordinate system can be obtained as:

[0111] ;

[0112] Therefore, the target distance is:

[0113] ;

[0114] It should be noted that if the sign of the Y coordinate (i.e., the ordinate) is unknown, it may lead to errors in the measurement results. Therefore, before the solution, it is necessary to determine the sign of the Y coordinate through prior knowledge. Normally, if the underwater vehicle is on the port side of the underwater vehicle, the sign of the Y coordinate is negative; if the underwater vehicle is on the starboard side of the underwater vehicle, the sign of the Y coordinate is positive. It should be particularly noted that the sign change of the Y coordinate during the process of the underwater vehicle passing through the underwater vehicle.

[0115] In addition, when solving the above equations, it is necessary to set the and initial values. When selecting the initial values, it can be assumed that both the first target receiving plane array and the second target receiving plane array are located on the X axis of the underwater vehicle coordinate system, as shown in Figure 9 .

[0116] At this time, the reference value of the distance is calculated according to the following formula as the initial value:

[0117] ;

[0118] ;

[0119] ;

[0120] where D represents the distance between the first target receiving plane array and the second target receiving plane array.

[0121] Although the above formulas in the embodiments of the present application take two target receiving plane arrays as an example, the above formulas are also applicable when the number of target receiving plane arrays is 3, 4, or more. For example, when the number of target receiving plane arrays is 4, two target receiving plane arrays can be taken as a group for the above calculation, and then the final result is corrected or averaged to obtain the target distance.

[0122] In the embodiments of the present application, the determination of the included angle between the underwater vehicle and the carrier includes:

[0123] During the underwater measurement process, since both the underwater vehicle and the underwater carrier are in motion, and the target position obtained in each measurement cycle is based on the target position in the current carrier coordinate system. In order to obtain the included angle between the target trajectory and the underwater carrier's motion trajectory to clarify their relative motion states, the target positioning result and the carrier position can be subjected to coordinate rotation and translation according to the carrier's speed, heading, and attitude data to obtain the target trajectory and the carrier trajectory based on the fixed geodetic coordinate system, and then the azimuth angle can be calculated according to the trajectory fitting data. Specifically, a fixed reference coordinate system is set as the north-east-up geodetic coordinate system (left-handed coordinate system) with the initial position of the carrier as the origin.

[0124] Assume that the navigation depth of the underwater carrier remains unchanged, and the heading is (basically remains unchanged during the measurement process), the speed of the carrier measured in the current cycle is , the pitch angle of the carrier measured by the inertial navigation device carried by the carrier is , and the roll angle is , then the rotation matrix between the carrier coordinate system and the north-east-up coordinate system in the current cycle can be obtained as:

[0125] ;

[0126] Among them, , the heading direction is defined as: the bow facing east is positive, the pitch angle is defined as positive when the bow is downward from the horizontal plane, and the roll angle is defined as positive when the starboard side is upward from the horizontal plane.

[0127] Set the coordinates of the underwater vehicle in the carrier coordinate system measured in the current cycle as , and the position coordinates of the origin of the carrier coordinate system in the north-east-up geodetic coordinate system at this time are: , then the position coordinates of the target in the north-east-up geodetic coordinate system are:

[0128] ;

[0129] Among them, can be measured by the inertial navigation device carried by the underwater carrier, or can be deduced according to the initial position of the carrier and the real-time speed and heading. Assume that the synchronization period is , the position coordinates of the origin of the carrier coordinate system in the north-east-up geodetic coordinate system in the previous cycle are: , then the position coordinates of the origin of the carrier coordinate system in the north-east-up geodetic coordinate system in the current cycle are:

[0130] ;

[0131] Wherein, represents the speed of the vehicle in the current period. After the above attitude correction is completed, the trajectories of the target and the vehicle in the same fixed coordinate system can be obtained, and then the azimuth angle can be calculated through the trajectory fitting result.

[0132] Assume that the target motion trajectory and the vehicle motion trajectory are both straight lines. In the two-dimensional plane, the straight line equation of the target motion trajectory obtained by fitting is: , then the direction vector of this straight line is . The straight line equation of the vehicle motion trajectory obtained by fitting is: , then its direction vector is , so the azimuth angle (here it is assumed to be the angle between the target trajectory and the vehicle trajectory) can be calculated as follows:

[0133] ;

[0134] Wherein, the included angle has a value range of .

[0135] In the embodiment of the present application, although the vehicle and the aircraft do not always move in a straight line, when the measurement period is limited to a short time, both of them move in a straight line. Therefore, the above scheme of the present application can be applied to the aircraft recovery scenario and accurate results can be obtained. For a measurement period of 3 hours, the vehicle and the aircraft do not move in a straight line. However, after dividing the 3-hour period into multiple 3-minute small time periods, for any time period, the vehicle and the aircraft both move in a straight line. The method of the present application can obtain data such as the included angle and position between the vehicle trajectory and the aircraft trajectory within a 3-minute time period; then combining the data such as the included angle and position between the vehicle trajectory and the aircraft trajectory obtained in each time period, a complete measurement result of the vehicle and the aircraft in a measurement period can be obtained, and this measurement result is very accurate.

[0136] Corresponding to the above method, the embodiment of the present application also provides a device for determining the pose parameters of an underwater vehicle, and the device for determining the pose parameters of the underwater vehicle includes:

[0137] An acquisition unit, configured to acquire the position coordinates of each target receiving plane array in the vehicle coordinate system, and the depth difference between the underwater vehicle and the vehicle of the underwater vehicle; for any target receiving plane array, acquire the first direction angle of the underwater vehicle relative to the target receiving plane array;

[0138] A conversion unit, configured to perform coordinate conversion on the first direction angle to obtain a second direction angle of the underwater vehicle in various vehicle coordinate systems; wherein, any type of vehicle coordinate system is constructed with any target receiving plane array as the origin;

[0139] A determination unit, configured to determine the target pose parameters of the underwater vehicle in the vehicle coordinate system based on the second direction angle of the underwater vehicle in various vehicle coordinate systems, the depth difference, and the position coordinates of each target receiving planar array in the vehicle coordinate system.

[0140] The functions of the functional units of the device for determining the pose parameters of the underwater vehicle provided in the above embodiments of the present application can be implemented by the above method steps. Therefore, the specific working processes and beneficial effects of each unit in the device for determining the pose parameters of the underwater vehicle provided in the embodiments of the present application will not be repeated here.

[0141] The embodiments of the present application also provide an electronic device, as Figure 10 shown, including a processor 1010, a communication interface 1020, a memory 1030, and a communication bus 1040. Among them, the processor 1010, the communication interface 1020, and the memory 1030 communicate with each other through the communication bus 1040.

[0142] The memory 1030 is used to store a computer program;

[0143] The processor 1010, when executing the program stored on the memory 1030, implements the following steps:

[0144] Obtain the position coordinates of each target receiving planar array in the vehicle coordinate system, and the depth difference between the underwater vehicle and the vehicle of the underwater vehicle;

[0145] For any target receiving planar array, obtain the first direction angle of the underwater vehicle relative to the target receiving planar array;

[0146] Perform coordinate transformation on the first direction angle to obtain the second direction angle of the underwater vehicle in various vehicle coordinate systems; wherein, any type of vehicle coordinate system is constructed with any target receiving planar array as the origin;

[0147] Based on the second direction angle of the underwater vehicle in various vehicle coordinate systems, the depth difference, and the position coordinates of each target receiving planar array in the vehicle coordinate system, determine the target pose parameters of the underwater vehicle in the vehicle coordinate system.

[0148] The communication bus mentioned above may be a Peripheral Component Interconnect (PCI) bus or an Extended Industry Standard Architecture (EISA) bus, etc. This communication bus can be divided into an address bus, a data bus, a control bus, etc. For the sake of convenience of representation, only a thick line is used in the figure, but it does not mean that there is only one bus or one type of bus.

[0149] The communication interface is used for communication between the above-mentioned electronic device and other devices.

[0150] The memory may include a Random Access Memory (RAM), or may also include a Non-Volatile Memory (NVM), such as at least one disk memory. Optionally, the memory may also be at least one storage device located away from the aforementioned processor.

[0151] The above-mentioned processor may be a general-purpose processor, including a Central Processing Unit (CPU), a Network Processor (NP), etc.; it may also be a Digital Signal Processor (DSP), an Application Specific Integrated Circuit (ASIC), a Field-Programmable Gate Array (FPGA), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components.

[0152] Since the implementation manners and beneficial effects of each component of the electronic device in the above embodiments for solving problems can be seen Figure 6 from the steps in the shown embodiments, therefore, the specific working process and beneficial effects of the electronic device provided in the embodiments of the present application will not be elaborated herein.

[0153] In another embodiment provided by the present application, a computer-readable storage medium is further provided. Instructions are stored in the computer-readable storage medium. When it runs on a computer, it causes the computer to execute the method for determining the pose parameters of the underwater vehicle in any one of the above embodiments.

[0154] In another embodiment provided by the present application, a computer program product containing instructions is further provided. When it runs on a computer, it causes the computer to execute the method for determining the pose parameters of the underwater vehicle in any one of the above embodiments.

[0155] Those skilled in the art should understand that the embodiments in the embodiments of this application can be provided as methods, systems, or computer program products. Therefore, the embodiments in this application can take the form of all-hardware embodiments, all-software embodiments, or embodiments combining software and hardware aspects. Moreover, the embodiments in this application can take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0156] The embodiments in this application are described with reference to the flowcharts and / or block diagrams of methods, devices (systems), and computer program products according to the embodiments in this application. It should be understood that each flow and / or block in the flowchart and / or block diagram, as well as the combination of flows and / or blocks in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to the processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing devices to generate a machine, so that the instructions executed by the processor of the computer or other programmable data processing devices generate means for implementing the functions specified in Figure 1 one flow or multiple flows and / or blocks Figure 1 one block or multiple blocks.

[0157] These computer program instructions can also be stored in a computer-readable memory that can direct a computer or other programmable data processing device to work in a specific manner, so that the instructions stored in the computer-readable memory generate a manufactured article including instruction means, and the instruction means implements the functions specified in Figure 1 one flow or multiple flows and / or blocks Figure 1 one block or multiple blocks.

[0158] These computer program instructions can also be loaded onto a computer or other programmable data processing device, so that a series of operation steps are executed on the computer or other programmable device to generate a computer-implemented process, and thus the instructions executed on the computer or other programmable device provide steps for implementing the functions specified in Figure 1 one flow or multiple flows and / or blocks Figure 1 one block or multiple blocks.

[0159] Although the preferred embodiments in the embodiments of this application have been described, those skilled in the art can make additional changes and modifications once they know the basic creative concepts. Therefore, the appended claims are intended to be construed as including the preferred embodiments and all changes and modifications falling within the scope of the embodiments of this application.

[0160] Obviously, those skilled in the art can make various changes and modifications to the embodiments in the embodiments of this application without departing from the spirit and scope of the embodiments in the embodiments of this application. Thus, if these modifications and variations of the embodiments in the embodiments of this application fall within the scope of the claims in the embodiments of this application and their equivalent technologies, then the embodiments of this application also intend to include these changes and modifications within.

Claims

1. A method for determining the pose parameters of an underwater vehicle, characterized in that Applied to the attitude parameter determination system of the underwater vehicle, and no acoustic cooperation beacon is installed on the system; The system further includes: a plurality of receiving planar arrays disposed on the carrier of the underwater vehicle; the method includes: Obtaining the position coordinates of each target receiving planar array in the carrier coordinate system, and the depth difference between the underwater vehicle and the carrier of the underwater vehicle; For any target receiving planar array, obtaining the first direction angle of the underwater vehicle relative to the target receiving planar array; Performing coordinate transformation on the first direction angle to obtain the second direction angle of the underwater vehicle in various carrier coordinate systems; wherein, any type of carrier coordinate system is constructed with any target receiving planar array as the origin; Based on the second direction angle of the underwater vehicle in various carrier coordinate systems, the depth difference, and the position coordinates of each target receiving planar array in the carrier coordinate system, determining the target attitude parameters of the underwater vehicle in the carrier coordinate system; The system further includes: a multi-channel signal conditioner, a signal processor, and a display and control unit; the plurality of receiving planar arrays are all connected to the multi-channel signal conditioner through waterproof cables; the multi-channel signal conditioner and the signal processor are connected through a feedthrough cable; the signal processor and the display and control unit are communicatively connected; Each target receiving planar array is configured to receive the sound signal radiated by the underwater vehicle, and the sound signal is navigation noise data; performing acoustic-electric conversion on the sound signal to obtain a differential electric signal; and sending the differential electric signal to the multi-channel signal conditioner; The multi-channel signal conditioner is configured to perform filtering, amplification processing, and analog-to-digital conversion on the differential electric signals received from each target receiving planar array to obtain digital signals corresponding to each target receiving planar array; and sending the digital signals corresponding to each target receiving planar array to the signal processor; The signal processor is configured to perform beamforming processing on the digital signals corresponding to each target receiving planar array received to obtain the first direction angle of the underwater vehicle relative to each target receiving planar array; and sending the first direction angle to the display and control unit; the display and control unit is configured to perform coordinate transformation on the first direction angle to obtain the second direction angle of the underwater vehicle in various carrier coordinate systems; based on each second direction angle, the depth difference between the underwater vehicle and the carrier of the underwater vehicle, and the position coordinates of each target receiving planar array in the carrier coordinate system, determining the target attitude parameters of the underwater vehicle in the carrier coordinate system.

2. The method according to claim 1, wherein At least two receiving planar arrays are disposed on both sides of the carrier.

3. The method according to claim 2, wherein Before obtaining the position coordinates of each target receiving planar array in the carrier coordinate system, the method further includes: Obtaining the relative position relationship between the underwater vehicle and the carrier; According to the relative position relationship, taking the side of the carrier where the underwater vehicle is located as the target side; Taking the receiving planar array located on the target side as the target receiving planar array.

4. The method according to claim 3, wherein The target attitude parameters include: target position coordinates, target distance, and target azimuth angle; The position coordinates of each target receiving planar array in the carrier coordinate system include: the ordinate of each target receiving planar array in the carrier coordinate system; Before determining the target pose parameters of the underwater vehicle in the vehicle coordinate system, the method further includes: Determine the positive and negative directions of the vertical coordinates of each target receiving planar array in the vehicle coordinate system according to the target side where the underwater vehicle is located.

5. The method according to claim 4, wherein Based on the second direction angle, the depth difference, and the position coordinates of each target receiving planar array in the vehicle coordinate system, determine the target pose parameters of the underwater vehicle in the vehicle coordinate system, including: Based on the second direction angle of the underwater vehicle in various vehicle coordinate systems, the position coordinates of each target receiving planar array in the vehicle coordinate system, and the depth difference, determine the slant range from the underwater vehicle to each target receiving planar array; According to the second direction angle of the underwater vehicle in various vehicle coordinate systems, the slant range, the depth difference, the position coordinates of each target receiving planar array in the vehicle coordinate system, and the positive and negative directions of the corresponding vertical coordinates, calculate the target position coordinates of the underwater vehicle in the vehicle coordinate system; According to the target position coordinates, calculate the target distance between the underwater vehicle and the vehicle and the target azimuth angle between the underwater vehicle and the vehicle.

6. The method according to claim 5, wherein After determining the target pose parameters of the underwater vehicle in the vehicle coordinate system, the method further includes: Obtain the real-time pose parameters of the vehicle in the geodetic coordinate system; Based on the target position coordinates and the real-time pose parameters of the vehicle, determine the position coordinates of the underwater vehicle in the geodetic coordinate system.

7. The method according to claim 6, characterized in that, The real-time pose parameters of the vehicle include: the speed and the vehicle position coordinates; After determining the position coordinates of the underwater vehicle in the geodetic coordinate system, the method further includes: Obtain the historical position coordinates of the underwater vehicle in the geodetic coordinate system and the historical vehicle position coordinates of the vehicle in the geodetic coordinate system at the previous moment of the current moment; According to the real-time pose parameters of the vehicle, the speed, and the historical vehicle position coordinates, determine the vehicle trajectory of the vehicle in the geodetic coordinate system at the current moment; According to the historical position coordinates of the underwater vehicle in the geodetic coordinate system and the position coordinates at the current moment, determine the target trajectory of the underwater vehicle in the geodetic coordinate system at the current moment; Fit the target trajectory and the vehicle trajectory to obtain the included angle between the target trajectory and the vehicle trajectory.

8. An electronic device, characterized in that, The electronic device includes a processor, a communication interface, a memory, and a communication bus. Among them, the processor, the communication interface, and the memory complete mutual communication through the communication bus; The memory is used to store a computer program; The processor is used to implement the method according to any one of claims 1-7 when executing the program stored on the memory.

9. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores a computer program, and when the computer program is executed by the processor, it implements the method according to any one of claims 1-7.

Images