Sonar real-time high-precision synchronous positioning and mapping optimization method based on dynamic sliding window and inertial navigation

Abstract

The invention relates to the technical field of underwater detection and navigation, in particular to a sonar real-time high-precision synchronous positioning and map building optimization method based on a dynamic sliding window and inertial navigation, and the method comprises the steps: introducing a dynamic sliding window adjustment module and an inertial navigation module, the dynamic sliding window adjusting mechanism dynamically adjusts the length of a sliding window according to the operation state (such as the movement speed and the steering angular speed) of a sonar system, the real-time performance is enhanced by reducing the window in a high-dynamic environment, and the map precision is improved by enlarging the window in a low-dynamic environment. The sliding window adjustment rule is driven by output parameters of the inertial navigation module, including acceleration, angular velocity and the like, the size of the sliding window is adjusted in real time according to the motion state (such as speed and steering angular velocity) of the sonar system, and the environmental perception ability of the sonar system in a complex scene is improved.

Description

Technical Field

[0001] The present invention relates to the technical field of underwater detection and navigation, and specifically to an optimized method for real-time high-precision simultaneous localization and mapping of sonar based on a dynamic sliding window and inertial navigation. Background Art

[0002] A sonar system is a device that uses the propagation and reflection characteristics of sound waves in water to perform navigation and ranging through electro-acoustic conversion and information processing. Currently, traditional sonar systems in underwater detection, cleaning, and navigation applications mainly rely on fixed scanning and low-frequency sensor data, suffering from problems such as susceptibility to data sparsity and noise interference, resulting in image distortion and inaccurate target detection; especially in high-dynamic environments, the real-time response ability is poor and it is difficult to adapt to complex underwater environmental changes. In addition, during the environmental mapping process based on SLAM technology, existing methods have insufficient optimization in the recognition and processing of noise data and dynamic objects, restricting the mapping accuracy and system robustness. Therefore, there is an urgent need for an optimized method for sonar systems that can effectively improve real-time performance, accuracy, and robustness in dynamic environments. Summary of the Invention

[0003] Aiming at the problem that sonar systems are restricted in use in dynamic environments in the prior art, the present invention provides an optimized method for real-time high-precision simultaneous localization and mapping of sonar based on a dynamic sliding window and inertial navigation, solving the deficiencies of traditional sonar in terms of real-time performance, mapping accuracy, positioning error control, and adaptability to complex environments.

[0004] The present invention is realized through the following technical solutions: An optimized method for real-time high-precision simultaneous localization and mapping of sonar based on a dynamic sliding window and inertial navigation, comprising the following steps: Step 1, initializing the sonar system; Step 2, collecting sonar echo signals and motion state data of the inertial navigation module in real time in an underwater environment; Step 3, dynamically adjusting the sliding window length according to the environmental and motion state data, and outputting multi-frame sonar echo signals with an adaptively adjusted sliding window length in combination with real-time echo signals; Step 4, performing filtering processing and noise elimination on the multi-frame sonar echo signals to obtain optimized echo signals; Step 5, using the optimized echo signals as inputs to construct a grid map of the underwater environment under SLAM; Step 6, using the real-time pose data of the inertial navigation module to assist in correcting the grid map; Step 7, globally adjusting the pose solution result of the inertial navigation module using the key frame data of SLAM; real-time monitoring of the position and attitude data output by the INS, and triggering an automatic correction mechanism if the drift error exceeds the threshold.

[0005] Preferably, in step 1, the initialization step of the sonar system includes setting the parameters of the sonar transmitter and receiver and calibrating the inertial navigation module. When the state is normal, data collection is prepared.

[0006] Preferably, the calibration process of the inertial navigation module is as follows: First, synchronize the pose information of the inertial navigation module (INS) with the initial position of the sonar through the extended Kalman filter (EKF); then complete the zero-bias correction of the gyroscope and accelerometer.

[0007] Preferably, in step 2, the specific steps of echo collection are as follows: The transmitter sends an acoustic pulse signal to the target area, and the receiver collects the echo signal through the mixing module and Butterworth filter; then perform segmented frequency-domain processing on the echo signal, extract key feature data and remove high-frequency noise; The specific steps of collecting the motion state data of the inertial navigation module are as follows: Collect the acceleration, angular velocity, and attitude data output by the INS in real time at a frequency of 100 Hz; then the data synchronization module aligns the INS data with the sonar echo signal in time to generate a dataset with consistent time series.

[0008] Preferably, in step 3, the dynamic adjustment of the window is driven by the motion state data provided by the inertial navigation module and completed in combination with the data stream of the real-time echo signal; The adjustment rule of the window is: When the speed of the underwater vehicle is greater than 1 m / s or the angular velocity is greater than 30° / s, the length of the sliding window is set to 3 - 5 frames; when the speed of the underwater vehicle is less than 1 m / s and the angular velocity is less than 30° / s, the length of the sliding window is set to 7 - 10 frames.

[0009] Preferably, in step 4, segmented frequency-domain matched filtering is adopted, specifically: Divide each frame of echo signal into small segments of 10 ms, perform fast Fourier transform to remove high-frequency noise components; then perform inverse Fourier transform (IFFT) on each processed segment of the signal to reconstruct the complete echo signal.

[0010] Preferably, in step 4, the specific steps of noise rejection are: Use the dynamic object recognition module to remove the signal artifacts generated by floating objects and dynamic interference objects.

[0011] Preferably, in step 5, the specific steps of constructing the underwater grid map are as follows: S51, Use adjacent frame point cloud to calculate the scene flow, and obtain the motion direction and speed of each point cloud cluster; S52, According to the principal component analysis and intersection over union, judge whether the point cloud cluster belongs to a dynamic object; S53, Perform a cleaning operation on the identified dynamic point cloud, and only retain the static point cloud data; S54. Construct a grid map of the underwater environment in the SLAM environment based on static point cloud data.

[0012] An electronic device includes a memory and a processor. The memory stores a computer program. When the processor executes the computer program, the steps of the sonar real-time high-precision synchronous positioning and map construction optimization method based on a dynamic sliding window and inertial navigation are implemented.

[0013] A storage medium stores a computer program. When the computer program is executed by a processor, the steps of the sonar real-time high-precision synchronous positioning and map construction optimization method based on a dynamic sliding window and inertial navigation are implemented.

[0014] Compared with the prior art, the present invention has the following beneficial effects: The sonar real-time high-precision synchronous positioning and map construction optimization method based on a dynamic sliding window and inertial navigation of the present invention adjusts the dynamic sliding window and inertial navigation module. Among them, the dynamic sliding window adjustment mechanism dynamically adjusts the sliding window length according to the operating state of the sonar system (such as movement speed, turning angular velocity). In a high-dynamic environment, the real-time performance is enhanced by narrowing the window, and in a low-dynamic environment, the map accuracy is improved by expanding the window. The sliding window adjustment rule is driven by the output parameters of the inertial navigation module, including acceleration, angular velocity, etc., and the sliding window size is adjusted in real time according to the movement state of the sonar system (such as speed, turning angular velocity), improving the environmental perception ability of the sonar system in complex scenarios.

[0015] Furthermore, the inertial navigation module assistance mechanism uses the IMU to provide the sonar system with acceleration and angular velocity information in real time as an important input for dynamic sliding window adjustment and data optimization. Combining inertial navigation data, the INS output is combined with the low-frequency pose estimation result of SLAM, and the drift error is corrected through the extended Kalman filter (EKF) to improve the accuracy of pose estimation in SLAM and reduce the drift error in long-term tasks.

[0016] Furthermore, when identifying dynamic objects, the point cloud scene flow estimation algorithm is used in combination with the sliding window technology to segment and label dynamic objects. A point cloud semantic segmentation plugin is introduced to enhance the classification accuracy of dynamic objects, and precise identification and removal of dynamic point clouds are achieved through methods such as intersection over union and principal component analysis. Description of the Drawings

[0017] Figure 1 is a flowchart of the sonar real-time high-precision synchronous positioning and map construction optimization method based on a dynamic sliding window and inertial navigation of the present invention; Figure 2 is a relationship diagram of the dynamic sliding window, SLAM, and inertial navigation unit in the present invention. Detailed implementation mode

[0018] The following further elaborates on the present invention in conjunction with specific embodiments, which is an explanation rather than a limitation of the present invention.

[0019] The present invention discloses an optimized method for real-time high-precision simultaneous localization and mapping of sonar based on a dynamic sliding window and inertial navigation. Referring to Figure 1 、 2 , it includes the following steps: Step 1, initialize the sonar system to ensure that the initial states and parameter settings of the sonar system and the inertial navigation module meet the working requirements, providing a stable hardware and data foundation for subsequent tasks.

[0020] The initialization steps of the sonar system include setting the parameters of the sonar transmitter and receiver and calibrating the inertial navigation module. Under normal conditions, it is ready to collect data. Specifically, the frequency of the sonar transmitter is set to 20kHz - 50kHz, and the pulse width is 1ms - 10ms; the gain control parameters of the receiver include fixed gain (10 - 40dB) and dynamic gain (20 - 60dB), and the echo filtering mode is enabled. The calibration process of the inertial navigation module is as follows: first, synchronize the pose information of the inertial navigation module (INS) with the initial position of the sonar through the extended Kalman filter (EKF); then complete the zero-bias correction of the gyroscope and accelerometer. The sonar system that has completed the hardware initialization and inertial navigation module calibration is in a normal state and ready to collect data.

[0021] Step 2, in the underwater environment, collect the sonar echo signal and the motion state data of the inertial navigation module in real time to obtain a multi-modal data set containing the sonar echo signal and the inertial navigation module data, providing input for subsequent sliding window processing and dynamic object recognition.

[0022] Specifically, the specific steps of echo collection are as follows: the transmitter sends an acoustic pulse signal to the target area, and the receiver collects the echo signal through a mixing module and a Butterworth filter; then perform segmented frequency-domain processing on the echo signal, extract key feature data, and remove high-frequency noise.

[0023] The specific steps of collecting the motion state data of the inertial navigation module are as follows: collect the acceleration, angular velocity, and attitude data output by the INS in real time at a frequency of 100Hz; then the data synchronization module aligns the INS data with the sonar echo signal in time to generate a data set with consistent time series.

[0024] Step 3, dynamically adjust the sliding window length according to the environmental and motion state data to balance real-time performance and accuracy. Then, in combination with the real-time echo signal, output multiple frames of sonar echo signals with the sliding window length adaptively adjusted.

[0025] Specifically, the dynamic adjustment of the window is driven by the motion state data provided by the inertial navigation module and completed in combination with the data stream of the real-time echo signal, and the multi-frame sonar echo signals after the adaptive adjustment of the sliding window length are output, meeting the real-time processing requirements of the dynamic scene.

[0026] Among them, the adjustment rule of the window is as follows: when the speed of the underwater vehicle is greater than 1 m / s or the angular velocity is greater than 30° / s, the sliding window length is set to 3 - 5 frames; when the speed of the underwater vehicle is less than 1 m / s and the angular velocity is less than 30° / s, the sliding window length is set to 7 - 10 frames.

[0027] Step 4, in order to reduce the interference of underwater noise on data processing, the multi-frame sonar echo signals are filtered and noise is removed to obtain optimized echo signals, providing a basis for subsequent mapping and recognition.

[0028] Specifically, segmented frequency-domain matched filtering is adopted, specifically: each frame of echo signal is divided into small segments of 10 ms, and fast Fourier transform is performed to remove high-frequency noise components; then the inverse Fourier transform (IFFT) is performed on each processed signal segment to reconstruct the complete echo signal.

[0029] The specific steps of noise removal are: using the dynamic object recognition module to remove the signal artifacts generated by floating objects and dynamic interference objects to ensure the purity of the data.

[0030] Step 5, taking the optimized echo signal as the input, a grid map of the underwater environment is constructed under SLAM; the specific steps are as follows: S51, using adjacent frame point cloud to calculate the scene flow, and obtaining the motion direction and speed of each point cloud cluster; S52, judging whether the point cloud cluster belongs to a dynamic object according to the principal component analysis (PCA) and the intersection over union (IoU); S53, performing a cleaning operation on the identified dynamic point cloud, only retaining the static point cloud data. The generated static point cloud data, excluding the interference of dynamic objects, improves the accuracy for mapping.

[0031] S54, constructing a grid map of the underwater environment based on the static point cloud data in the SLAM environment.

[0032] Step 6, using the real-time pose data of the inertial navigation module to assist in correcting the grid map, obtaining an accurate underwater environment grid map and the real-time AUV pose.

[0033] Step 7, using the key frame data of SLAM to globally adjust the pose solution result of the inertial navigation module.

[0034] Real-time monitor the position and attitude data output by INS. If the drift error exceeds the threshold, trigger the automatic correction mechanism to solve the cumulative drift error of the inertial navigation module and ensure the accuracy of long-term tasks.

[0035] An optimized method for real-time high-precision synchronization positioning and mapping of sonar based on dynamic sliding window and inertial navigation in the present invention not only significantly improves the real-time performance and robustness of the sonar system by introducing dynamic sliding window adjustment and inertial navigation module, but also solves the interference problem of dynamic objects on the SLAM mapping accuracy. The whole system has a simple structure and low cost, and is suitable for wide applications in fields such as lake cleaning, ocean exploration, underwater resource management, etc. Among them, the dynamic sliding window adjustment mechanism adjusts the size of the sliding window in real time according to the motion state of the sonar system (such as speed, steering angular velocity), improving the environmental perception ability of the sonar system in complex scenarios. The inertial navigation module has independent high-precision pose solution, and combined with SLAM data optimization, can overcome the noise influence of the sonar system in the underwater environment, significantly improving the stability of the system in long-term tasks and the adaptability in complex underwater environments. The collaborative work of the dynamic sliding window and the inertial navigation module effectively enhances the real-time response ability and environmental mapping accuracy of the sonar system.

[0036] The present invention also discloses an electronic device, including a memory and a processor. The memory stores a computer program, and when the processor executes the computer program, the steps of the optimized method for real-time high-precision synchronization positioning and mapping of sonar based on dynamic sliding window and inertial navigation are implemented.

[0037] Specifically, the electronic device includes a memory, at least one processor, a computer program stored in the memory and executable on the at least one processor, and at least one communication bus.

[0038] The memory can be used to store the computer program. The processor realizes the steps of the multi-region electric energy and ancillary service joint clearing method described in the embodiment by running or executing the computer program stored in the memory and calling the data stored in the memory. The memory can mainly include a program storage area and a data storage area. Among them, the program storage area can store an operating system, application programs required for at least one function (such as a sound playing function, an image playing function, etc.); the data storage area can store data created according to the use of the electronic device (such as audio data, etc.). In addition, the memory can include non-volatile memory, such as a hard disk, a memory, a plug-in hard disk, a smart media card (SMC), a secure digital (SD) card, a flash card, at least one disk storage device, a flash device, or other non-volatile solid-state storage devices.

[0039] The at least one processor may be a Central Processing Unit (CPU), or may also be other general-purpose processors, digital signal processors, application specific integrated circuits, field programmable gate arrays or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. The processor may be a microprocessor or the processor may also be any conventional processor, etc. The processor is the control center of the electronic device and connects various parts of the entire electronic device through various interfaces and lines.

[0040] The memory in the electronic device stores a plurality of instructions to implement a multi-modal named entity recognition method for a power device, and the processor can execute the plurality of instructions to implement it.

[0041] The present invention also discloses a storage medium, on which a computer program is stored. When the computer program is executed by a processor, the steps of the sonar real-time high-precision synchronous positioning and map building optimization method based on dynamic sliding window and inertial navigation are implemented. Wherein, the computer program includes computer program code, and the computer program code may be in the form of source code, object code, executable file or some intermediate form, etc. The computer-readable medium may include: any entity or device capable of carrying the computer program code, recording medium, USB flash drive, mobile hard disk, magnetic disk, optical disc, computer memory and read-only memory (ROM, Read-Only Memory).

[0042] The above are only the preferred embodiments of the present invention and are not used to limit the technical solutions of the present invention. Those skilled in the art should understand that, without departing from the spirit and principle of the present invention, the technical solutions can be modified and replaced simply in several ways, and these modifications and replacements also fall within the protection scope covered by the claims.

Claims

1. A sonar real-time high-precision synchronous positioning and map construction optimization method based on dynamic sliding window and inertial navigation, characterized in that: The following steps are involved: Step 1, sonar system initialization; Step 2, collecting sonar echo signals and motion state data of the inertial navigation module in real time in an underwater environment; Step 3, dynamically adjusting the sliding window length according to the environment and motion state data, and outputting a multi-frame sonar echo signal after the sliding window length is adaptively adjusted in combination with the real-time echo signal; Step 4, filtering and removing noise from the multi-frame sonar echo signals to obtain optimized echo signals; Step 5, using the optimized echo signal as input, constructing a grid map of the underwater environment under SLAM; Step 6, using the real-time posture data of the inertial navigation module to assist in correcting the grid map; Step 7: Use the key frame data of SLAM to globally adjust the position and attitude solution results of the inertial navigation module; monitor the position and attitude data output by INS in real time, and if the drift error exceeds the threshold, trigger the automatic correction mechanism.

2. The sonar real-time high-precision synchronous positioning and map construction optimization method based on dynamic sliding window and inertial navigation according to claim 1 is characterized in that: In step 1, the initialization step of the sonar system includes the parameter setting of the sonar transmitter and receiver and the calibration of the inertial navigation module. When the status is normal, it is ready to collect data.

3. The sonar real-time high-precision synchronous positioning and map construction optimization method based on dynamic sliding window and inertial navigation according to claim 2 is characterized in that: The calibration process of the inertial navigation module is as follows: first, the position information of the inertial navigation module is synchronized with the initial position of the sonar through the extended Kalman filter; then the zero bias correction of the gyroscope and accelerometer is completed.

4. The sonar real-time high-precision synchronous positioning and map construction optimization method based on dynamic sliding window and inertial navigation according to claim 1 is characterized in that: In step 2, the specific steps of echo acquisition are: the transmitter sends an acoustic pulse signal to the target area, and the receiver collects the echo signal through a mixing module and a Butterworth filter; then the echo signal is processed in the segmented frequency domain to extract key feature data and remove high-frequency noise; The specific steps of the motion state data collection of the inertial navigation module are: collect the acceleration, angular velocity and attitude data output by the INS in real time at a frequency of 100Hz; then the data synchronization module time-aligns the INS data with the sonar echo signal to generate a data set with consistent time sequence.

5. The sonar real-time high-precision synchronous positioning and map construction optimization method based on dynamic sliding window and inertial navigation according to claim 1 is characterized in that: In step 3, the dynamic adjustment of the window is driven by the motion state data provided by the inertial navigation module and completed in combination with the data stream of the real-time echo signal; The window adjustment rule is: when the speed of the underwater vehicle is greater than 1m / s or the angular velocity is greater than 30° / s, the sliding window length is set to 3-5 frames; when the speed of the underwater vehicle is less than 1m / s and the angular velocity is less than 30° / s, the sliding window length is set to 7-10 frames.

6. The sonar real-time high-precision synchronous positioning and map construction optimization method based on dynamic sliding window and inertial navigation according to claim 1 is characterized in that: In step 4, segmented frequency domain matched filtering is used, specifically: each frame of echo signal is divided into small segments of 10ms, and fast Fourier transform is performed to remove high-frequency noise components; then inverse Fourier transform is performed on each processed signal to reconstruct the complete echo signal.

7. The sonar real-time high-precision synchronous positioning and map construction optimization method based on dynamic sliding window and inertial navigation according to claim 1 is characterized in that: In step 4, the specific steps of noise removal are: using the dynamic object recognition module to remove signal artifacts generated by floating objects and dynamic interference objects.

8. The sonar real-time high-precision synchronous positioning and map construction optimization method based on dynamic sliding window and inertial navigation according to claim 1 is characterized in that: In step 5, the specific steps of constructing the underwater grid map are: S51, using the adjacent frame point cloud computing scene flow to obtain the movement direction and speed of each point cloud cluster; S52, judging whether the point cloud cluster belongs to a dynamic object according to principal component analysis and intersection-over-union ratio; S53, performing a cleaning operation on the identified dynamic point cloud, and only retaining static point cloud data; S54, constructs a grid map of the underwater environment in a SLAM environment based on static point cloud data.

9. An electronic device comprising a memory and a processor, wherein the memory stores a computer program, wherein: When the processor executes the computer program, the steps of the sonar real-time high-precision synchronous positioning and map construction optimization method based on dynamic sliding window and inertial navigation are implemented as described in any one of claims 1 to 8.

10. A storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the steps of the sonar real-time high-precision synchronous positioning and map construction optimization method based on dynamic sliding window and inertial navigation are implemented as described in any one of claims 1 to 8.

Images