Hydrometeorological unmanned ship based on visual inertia laser radar and SLAM navigation method thereof

Abstract

The invention provides a hydrometeorological unmanned ship based on a visual inertia laser radar and an SLAM navigation method thereof.The unmanned ship comprises a ship protection cover, a ship shell, the laser radar, a depth camera, an IMU module, an intelligent development board and the like, and the laser radar, the depth camera and the IMU module are connected with the intelligent development board; the unmanned ship further comprises a Beidou positioning module, a sensor module, an STM32 driving board, a brushless motor driving module and an electronic speed regulator, the unmanned ship can achieve precise positioning and path planning of a ship body, environment monitoring and sampling and has the functions of automatic cruise, obstacle avoidance and the like, laser radar information and binocular vision IMU attitude estimation are fused through tight coupling, and the navigation precision is improved. According to the SLAM front-end attitude estimator, a VILO front-end attitude estimator is formed, and laser radar observation is added to further limit the attitude, so that the accuracy and robustness of the SLAM front-end attitude estimator in indoor and outdoor environments are improved.

Description

Technical Field

[0001] The invention relates to an intelligent mobile device, in particular to a hydrological and meteorological unmanned ship based on visual inertial laser radar and a SLAM navigation method thereof. Background Art

[0002] At present, SLAM solutions are widely used in indoor robots and unmanned vehicles, while there are fewer SLAM solutions based on surface platforms and applied to inland rivers, urban waterways, ports and other environments. Unmanned boats have more flexible behaviors, more efficient operation efficiency and more accurate data measurement than manual operations in closed waterways. Unmanned boats use SLAM to accurately draw three-dimensional maps of the waterway in order to survey the waterway and understand the siltation and blockage in the waterway. In the design and development process of unmanned boats, for different inspection environments and tasks, unmanned boats can be equipped with corresponding sensors, positioning systems, control systems and other core modules as needed to achieve autonomous navigation and mapping of various environments. Therefore, for the research on unmanned boats, it is necessary not only to deeply explore the hardware design and software algorithms of unmanned boats, but also to study the interaction between unmanned boats and cloud databases.

[0003] The development of domestic unmanned surface vessels has encountered a series of challenges. The first is the technical difficulty, which requires unmanned surface vessels to have complex functions such as autonomous navigation, environmental recognition and mission implementation, thus putting forward strict standards for core technologies such as sensor technology, communication systems and control systems. In addition, unmanned surface vessels also face security threats that cannot be ignored, including potential risks of network intrusion and data leakage, which emphasizes the urgency of strengthening network security protection measures.

[0004] The development time of unmanned ships in existing technologies is still short, and some unmanned ship technologies are still in the experimental stage. There are many problems in actual application scenarios. For example, the functions of unmanned ships are relatively simple and they can only perform fixed monitoring functions; or they are large in size, consume a lot of power, and have a short monitoring operation endurance; or the operator is required to actively control the navigation, and the obstacle avoidance and automatic cruising functions are insufficient, making it difficult to meet the purpose of automated cruising monitoring.

[0005] The comparison with the existing technology is as follows: Technical comparison with patent number CN118377032A "A method, device and system for monitoring unmanned boat environment perception based on multi-sensor fusion SLAM" Patent CN118377032A proposes a loosely coupled fusion based on LiDAR + IMU + GPS, and proposes IMU pre-integration distortion removal and GPS vertical direction constraints for the water surface environment to solve the problems of point cloud distortion and feature sparsity. This application proposes a tightly coupled fusion of vision (depth camera) + inertia + LiDAR, and proposes a visual-inertial-LiDAR odometry (VILO), which integrates LiDAR observations with visual and inertial posture estimation to enhance the robustness of the SLAM front end. There is an essential difference between the fusion strategies of the two.

[0006] Patent CN118377032A targets nearshore port waters, focusing on high-precision mapping and anti-interference navigation, coping with wind and wave vibrations, lack of water surface reflections, and dynamic interference (such as ships / floating objects) to optimize the point cloud distortion (motion compensation) and feature sparsity (full point cloud registration) of laser SLAM in the water surface environment. This application is suitable for hydrological and meteorological monitoring (such as lakes and oceans), and it is necessary to complete three-dimensional mapping, water quality testing, marine meteorology, and automatic cruise obstacle avoidance at the same time, solve the failure problem of visual SLAM in dark / strong light / low texture environments, and enhance constraints through laser radar. There are essential differences in the application scenarios and targeted solutions of the two.

[0007] The front-end technology of patent CN118377032A compensates for laser point cloud distortion in real time through IMU pre-integration, uses kdtree search and tangent plane normal vector for point cloud registration, and introduces GPS constraints to optimize vertical errors in the back-end technology, combining loop detection with the GTSAM library (iSAM2) for global optimization. The front-end technology of this application constructs a tightly coupled nonlinear least squares model of visual-inertial-lidar, uses the Huber loss function to suppress outliers, and the back-end technology relies on sensor tight coupling optimization, which is universal for indoor and outdoor use. There are essential differences between the two in data processing and optimization methods. Summary of the invention

[0008] In order to solve the above technical problems, the present invention proposes a hydrological and meteorological unmanned boat based on visual inertial laser radar and its SLAM navigation method. The unmanned boat can realize accurate positioning and path planning of the hull, monitoring and sampling of the water quality environment, and is easy to use and simple to operate. It also has functions such as automatic cruising and obstacle avoidance. The SLAM method integrates the laser radar information with the binocular vision IMU attitude estimation through tight coupling to form a binocular vision IMU-lidar odometer (VILO) front-end attitude estimator. It also increases laser radar observation to further restrict the attitude, thereby improving the accuracy and robustness of the SLAM front-end attitude estimator in indoor and outdoor environments.

[0009] To achieve the above object, the technical solution adopted by the present invention is: A hydrological and meteorological unmanned boat based on visual inertial laser radar, characterized in that it includes an unmanned boat protection cover and a hull, the unmanned boat protection cover is installed above the hull, and also includes a laser radar, a depth camera, an IMU module and an intelligent development board, the laser radar, the depth camera, and the IMU module are respectively connected to the intelligent development board, the laser radar is installed above the unmanned boat protection cover, the depth camera, the IMU module and the intelligent development board are installed in the hull, and also includes a Beidou positioning module, a sensor module and an STM32 driver board, the intelligent development board is connected to the STM32 driver board to realize data exchange, the Beidou positioning module is connected to the STM32 driver board, the sensor module includes a light intensity sensor, a water quality sensor, and a turbidity and PH sensor, the light intensity sensor, the water quality sensor The sensor and the turbidity and pH sensors are connected to the STM32 driver board respectively, the light intensity sensor is used to detect the ambient light intensity, the water quality sensor is used to detect the water quality of the water area, the turbidity and pH sensors are used to detect the turbidity and pH of the water area, the Beidou positioning module is installed on the unmanned ship protection cover, the STM32 driver board is installed in the hull, and also includes a brushless motor drive module and an electronic speed regulator, the brushless motor drive module is installed on both sides of the hull, a relay is installed on the STM32 driver board, the brushless motor drive module is connected to the relay on the STM32 driver board through the electronic speed regulator, and also includes a power supply, the power supply is respectively connected to the intelligent development board and the STM32 driver board to provide power for them, and the STM32 driver board is connected to the remote cloud platform signal.

[0010] In the above structure: the hydrological and meteorological unmanned boat based on visual inertial laser radar proposed by the present invention includes an unmanned boat protection cover and a hull. The unmanned boat protection cover is installed above the hull to protect the parts inside the hull and install and fix the Beidou positioning module and the laser radar. The laser radar is used to realize real-time three-dimensional perception of the water environment, and the depth camera is used to shoot the water environment. The IMU module is a high-performance three-dimensional motion attitude measurement system based on MEMS technology, which includes three-axis gyroscope data, three-axis accelerometer data, three-axis magnetometer data and barometer data, a total of ten-axis data. By integrating high-performance sensors and attitude dynamics core algorithm engines, combined with high-dynamic Kalman filtering algorithms, Obtain high-precision, high-dynamic, real-time compensated three-axis attitude angles. The IMU module can collect inertial information such as angular velocity and acceleration of objects. This information can be used to calculate the real-time position and motion trajectory of objects. In robotics, the IMU module is integrated with other sensors such as cameras and force sensors to achieve functions such as maintaining object balance, predicting speed and trajectory, and positioning and navigation. In this application, by combining with laser radar and depth camera, the three-dimensional construction of the water environment can be realized in the intelligent development board, and autonomous path planning can be performed. The planned path is transmitted to the STM32 driver board, and the STM32 driver board controls the brushless motor driver module to control the movement of the unmanned boat; The STM32 driver board integrates Beidou positioning module, light intensity sensor, water quality sensor, turbidity and pH sensor. The Beidou positioning module is used to realize the real-time positioning of the unmanned boat. The light intensity sensor is used to detect the ambient light intensity, the water quality sensor is used to detect the water quality of the water area, and the turbidity and pH sensors are used to detect the turbidity and pH of the water area. The core data is collected through the sensor module and transmitted to the remote cloud platform. After precise processing and comprehensive analysis, the control of the unmanned boat and the detection of the water environment are realized.

[0011] The brushless motor drive module is installed on both sides of the hull and controlled by the relay on the STM32 driver board to realize the operation of the unmanned boat.

[0012] The power supply is connected to the smart development board and the STM32 driver board respectively to provide power for them to ensure the normal operation of both.

[0013] The remote cloud platform ensures efficient management and stable operation of equipment. Operators can quickly obtain key data such as the location, speed and surrounding environment of the ship, so as to effectively monitor it. This function is achieved by extracting information related to the environment and the ship itself from the database. The remote cloud platform is designed for personnel management and equipment maintenance, providing an intuitive user interface, integrating fault detection and preventive maintenance functions, and monitoring key indicators in real time.

[0014] Further: it also includes a lighting lamp, which is connected to the STM32 driver board, and a lighting mounting board is arranged in front of the hull, and the lighting lamp is mounted on the lighting mounting board.

[0015] In the above structure: the lighting lamp is used in an environment with insufficient light, and the light intensity of the water area is detected by the light intensity sensor. When the light intensity is dark, the STM32 driver board controls the lighting lamp to turn on, which is convenient for the depth camera to shoot.

[0016] Further: a through hole is arranged in the front of the hull, and the depth camera is installed in the through hole.

[0017] In the above structure: the depth camera is installed in front of the hull for easy shooting.

[0018] Furthermore: a heat dissipation hole is arranged at the rear of the hull, a heat dissipation fan is installed in the heat dissipation hole, and the heat dissipation fan is directly facing the STM32 driver board in the hull.

[0019] In the above structure: the cooling fan in the cooling hole can dissipate heat for the parts in the hull to prevent the parts from aging and damage.

[0020] Furthermore: drive mounting bases are respectively installed on both sides of the hull, and the brushless motor drive module is installed on the drive mounting bases.

[0021] In the above structure: the drive mounting base is used to install and fix the brushless motor drive module.

[0022] Furthermore: an installation groove and a collecting hole are also provided at the rear of the hull, the light intensity sensor is installed in the installation groove, a detection groove is provided in the hull, the collecting hole is connected to the detection groove, and the water quality sensor and the turbidity and PH sensors are installed in the detection groove.

[0023] In the above structure: the light intensity sensor is used to collect the light intensity of the water area, which is installed behind the hull for easy monitoring; the water quality sensor and the turbidity and pH sensors are installed in the detection tank, which collect water from the water area through the collection hole for detection. When the unmanned boat is launched, the collection hole is below the horizontal plane, which is convenient for the water in the water area to enter the detection tank.

[0024] The SLAM method of the hydrological and meteorological unmanned ship based on visual inertial laser radar of the present invention has the following specific steps: S1, tightly couple the lidar observation with stereo vision and inertial observation, and fuse the lidar information with the binocular vision IMU attitude estimation through tight coupling to form the binocular vision IMU-lidar odometer VILO front-end attitude estimator; S2. Construct a nonlinear least squares model based on binocular vision IMU lidar: S3. Calculate and constrain binocular vision residuals r c : The pose of the keyframe is defined as From keyframe i Transition to Keyframe j , Two orthogonal basis vectors representing the tangent plane of the unit sphere, used to map the 3D camera coordinate difference to the unit sphere; is the pose from IMU to depth camera obtained by pre-calibration; S4. Calculate and constrain IMU residuals r b : When the initial attitude of the unmanned ship is known, the IMU is used for continuous integration time to obtain the speed and attitude of the unmanned ship relative to the reference coordinates in real time. The IMU integration information is then used to complement the information of the lidar and depth camera. In the visual inertial odometer, the IMU integration information between key frames is added as the residual constraint of the entire bundle adjustment optimization. According to the IMU pre-integration model, the IMU residual is as follows: ; The above formula subtracts the IMU pre-integrated measurement value from the predicted value to obtain the IMU prediction score residual term. In the variable, is the pre-integrated three-dimensional coordinate position residual of the IMU, is the pre-integrated velocity residual of the IMU, is the rotation error of the three axes of the IMU, They are the zero-point error residuals of the accelerometer and gyroscope in the IMU; S5. Calculate and constrain the residual of the lidar odometer r L : Based on the acquisition of lidar data, the attitude estimation problem of the lidar in the map is transformed into a nonlinear least squares problem. The formula is as follows: ; In the formula, is the attitude variable of the lidar relative to the grid map coordinate system, Indicates that when the laser radar is in attitude When the laser i The endpoint coordinates of the scanning ray in the grid map coordinate system, Is the grid map at a given coordinate position The probability of becoming an obstacle, the purpose of the above formula is to find the minimum objective function of Posture variables, It is expressed as follows: ; In the above formula, ,in and Represents the coordinates of the origin of the laser sensor coordinate system relative to the reference coordinate system, i is the heading angle of the unmanned ship, and the observation points of the laser radar are converted into the grid map coordinate system. Therefore, the core of the scan matching algorithm is to give the laser radar posture with a small deviation from the actual posture at the beginning. , and then according to the current time t Acquire laser data to find attitude increments , so that the end point of the laser scan and the probability grid map are at the current time t The best match is to establish the following formula: ; In order to solve the current time t The optimal posture increment that minimizes the above equation is , using the Gauss-Newton iteration method for the nonlinear function Performing a first-order Taylor expansion, we get the following formula: ; In the above formula, i is the laser sensor at time t No. i A laser beam will n The laser beams are added together to obtain the attitude increment from the objective function of the above formula. The partial derivative of makes the partial derivative equal to 0. Finally, the attitude increment as follows: ; In the above formula, Represents the probability value of an obstacle at the coordinate ,because Not an integer pair, in order to express , assuming , then write The approximation is as shown below: ; The Hessian matrix H As shown in the following formula; ; in About Posture The partial derivative of is shown below: ; In the LiDAR attitude Nearby approximate objective function , and perform multiple optimization iterations until the posture increment Small enough, the iteration stops and the current time is obtained t The optimal LiDAR pose , and the residual sum of squares of the original model is the smallest; S6. Derivation of the residual of the lidar odometer r L Relative to the state variable to be estimated The Jacobian matrix analytical formula of ; S7. After the binocular vision residual, IMU residual, and lidar odometry residual are jointly constrained and continuously iteratively optimized, the front-end attitude estimator is solved in indoor and outdoor environments.

[0025] As a further improvement of the present invention, the state variables to be solved in the VILO front-end attitude estimator in step S1 are x As shown below: ; in, Represents the world coordinate system, in the state variable middle, x k It is captured k The IMU state at the time of the image, including the position of the IMU in the world coordinate system ,speed and direction , and the accelerometer zero bias in the IMU carrier coordinate system and gyro bias ,also, n is the total number of keyframes, m is the total number of feature points in the sliding window, Respectively represent The inverse depth of the feature point in the first observed frame is l i Indicates i The inverse depth of the feature points in the first observed frame; In the local pose estimation process of VILO, the observation set Z used to constrain the variables to be estimated is defined by the following formula: ; In the formula, represents the visual measurement value, Is the collection of i Frame and i +1 frame image carrier coordinate system, Indicates the measured value from i Frame keyframe to i +1 keyframe lidar odometer value, k is the keyframe set in the sliding window.

[0026] As a further improvement of the present invention, the nonlinear least squares model of the binocular vision IMU laser radar in step S2 is as follows: According to the residual factor of the binocular vision IMU lidar, and using the Mahalanobis distance to represent the degree of deviation between the residual and covariance matrix of each sensor, the VILO nonlinear least squares problem is constructed as follows: ; In the above formula is the covariance matrix of the residual r The Mahalanobis distance is S , the Mahalanobis distance is defined as , the first item is the prior information of the marginalization of the key frames in the sliding window, r b is the residual error of the IMU, r c is the binocular vision residual, r L is the lidar odometry residual, k is the key frame set in the sliding window; In binocular vision residual r c and the lidar odometry residual r L The Huber loss function is used in r for: ; The Huber loss identifies outliers by considering the residual, denoted by s.

[0027] As a further improvement of the present invention, the residual factor of the laser radar attitude in step S6 is r L The laser radar odometer is used to observe the i Frame and i +1 relative displacement between frames To represent the relative displacement of the last two key frames before and after the state variable Optimization, and They are called observed values ​​and predicted values ​​respectively, and their specific forms are shown in the following formula: ; In the above formula, L is the laser radar coordinate system, Indicates that IMU is i The pose of the frame relative to the world coordinate system, Indicatesi Frame and i +1 relative pose estimation between frames, The specific expansion form is as follows: ; Finally, the relative displacement The residual term of the position and the posture of the variable to be optimized and lidar odometry frames i and i +1 obtained as follows: ; In the above formula, the relative displacement Obtained through the laser radar odometer, It is the attitude of the LiDAR coordinate system relative to the IMU coordinate system, which is obtained by pre-calibration. Since the LiDAR odometer can only observe the attitude yaw angle i , the residual model can only constrain the attitude yaw angle to the state variable , and then the state variables , specifically , residual r L The Jacobian matrix of the state variable The estimates are as follows: ; In the above formula, ∧ is the transformation from vector to antisymmetric matrix, J[0] 3×10 and J[1] 3×10 is the lidar residual r L The first-order partial derivative is relative to i and i +1 keyframe pose and , also known as the Jacobian matrix.

[0028] Compared with the prior art, the present invention has the following beneficial effects: The hydrological and meteorological unmanned boat of the present invention can establish a three-dimensional map by setting up an IMU module, a depth camera, and a laser radar, and performs autonomous path planning and controls the movement of the hull through an intelligent development board. The STM32 driver board integrates a Beidou positioning module, a light intensity sensor, a water quality sensor, a turbidity and a pH sensor. The core data of the water area collected is processed accurately and comprehensively analyzed through data fusion technology to ensure the safe operation of the hull. The power of this application is supplied by a dedicated power supply. The measurement data is sent to the remote cloud platform server database through the MQTT communication protocol.

[0029] Data uploaded to the remote cloud platform via the STM32 driver board ensures efficient management and stable operation of the equipment. Operators can quickly obtain key data such as the ship's location, speed, and surrounding environment, so as to effectively monitor it. This function is achieved by extracting information related to the environment and the ship itself from the database. The management platform is designed for personnel management and equipment maintenance, providing an intuitive user interface, integrating fault detection and preventive maintenance functions, and monitoring key indicators in real time.

[0030] The SLAM method of the present invention tightly couples low-cost lidar observations with stereo vision and inertial observations, which can effectively improve the accuracy and robustness of pose estimation of traditional visual-inertial SLAM algorithms when visual features are lost due to darkness, strong light or lack of texture.

[0031] The lidar residual factor is constructed by utilizing a two-dimensional lidar odometry model, and the Jacobian matrix of the lidar residual relative to the state variable to be estimated is derived.

[0032] By constructing the residual constraint equation of the visual IMU LiDAR and using a nonlinear optimization method to obtain the optimal unmanned ship posture estimation, the problem of fusing LiDAR observations with binocular visual inertial information in a tightly coupled manner is solved.

[0033] The SLAM method of the present invention integrates the lidar information with the binocular vision IMU attitude estimation through tight coupling to form a binocular vision IMU-lidar odometer (VILO) front-end attitude estimator. It also adds lidar observations to further constrain the attitude, thereby improving the accuracy and robustness of the SLAM front-end attitude estimation in indoor and outdoor environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0034] Figure 1 It is a schematic diagram of the overall structure of the present invention; Figure 2 It is a schematic diagram of the internal structure of the present invention; Figure 3 It is a schematic diagram of the side structure of the present invention; Figure 4 It is a principle framework diagram of the present invention; Figure 5 It is a system flow chart of the present invention.

[0035] List of reference numerals: 1. Beidou positioning module; 2. Unmanned boat protection cover; 3. LiDAR; 4. Hull; 5. Lighting; 6. Depth camera; 7. Brushless motor driver module; 7-1. Driver mounting base; 8. Smart development board; 9. Electronic speed regulator; 10. Power supply; 11. Relay; 12. STM32 driver board; 13. Light intensity sensor; 14. Cooling fan; 15. Water quality sensor; 16. Turbidity and PH sensor. DETAILED DESCRIPTION

[0036] The present invention is further described in detail below in conjunction with the accompanying drawings and specific embodiments: like Figure 1-3 As shown, the present invention proposes a hydrological and meteorological unmanned boat based on visual inertial laser radar, including an unmanned boat protective cover 2 and a hull 4, wherein the unmanned boat protective cover 2 is installed above the hull 4, and also includes a laser radar 3, a depth camera 6, an IMU module and an intelligent development board 8, wherein the laser radar 3, the depth camera 6 and the IMU module are respectively connected to the intelligent development board 8, wherein the laser radar 3 is installed above the unmanned boat protective cover 2, wherein the depth camera 6, the IMU module and the intelligent development board 8 are installed in the hull 4, and also includes a Beidou positioning module 1, a sensor module and an STM32 driver board 12, wherein the intelligent development board 8 is connected to the STM32 driver board 12 to realize data exchange, wherein the Beidou positioning module 1 is connected to the STM32 driver board 12, wherein the sensor module includes a light intensity sensor 13, a water quality sensor 15 and a turbidity and PH sensor 16, wherein the light intensity sensor 13, the water quality sensor 15 and the turbidity and PH sensor 16 15 and turbidity and PH sensors 16 are respectively connected to the STM32 driver board 12, the light intensity sensor 13 is used to detect the ambient light intensity, the water quality sensor 15 is used to detect the water quality of the water area, the turbidity and PH sensors 16 are used to detect the turbidity and pH of the water area, the Beidou positioning module 1 is installed on the unmanned ship protection cover 2, the STM32 driver board 12 is installed in the hull 4, and also includes a brushless motor drive module 7 and an electronic speed regulator 9, the brushless motor drive module 7 is installed on both sides of the hull 4, the STM32 driver board 12 is installed with a relay 11, the brushless motor drive module 7 is connected to the relay 11 on the STM32 driver board 12 through the electronic speed regulator 9, and also includes a power supply 10, the power supply 10 is respectively connected to the intelligent development board 8 and the STM32 driver board 12 to provide power for them, and the STM32 driver board 12 is connected to the remote cloud platform signal.

[0037] The present invention proposes a hydrological and meteorological unmanned boat based on visual inertial laser radar, comprising an unmanned boat protective cover 2 and a hull 4. The unmanned boat protective cover 2 is installed above the hull 4 to protect the parts inside the hull 4 and to install and fix the Beidou positioning module 1 and the laser radar 3. The laser radar 3 is used to realize real-time three-dimensional perception of the water environment. The depth camera 6 is used to photograph the water environment. The IMU module is a high-performance three-dimensional motion attitude measurement system based on MEMS technology, which includes three-axis gyroscope data, three-axis accelerometer data, three-axis magnetometer data and barometer data, a total of ten-axis data. By integrating high-performance sensors and attitude dynamics core algorithm engines, combined with high-dynamic Kalman filtering algorithms, the system can obtain High-precision, high-dynamic, real-time compensation of three-axis attitude angle, IMU module can collect inertial information such as angular velocity and acceleration of objects, which can be used to calculate the real-time position and motion trajectory of objects. In robotics, IMU module is integrated with other sensors such as cameras and force sensors to achieve functions such as maintaining object balance, predicting speed and trajectory, and positioning and navigation. In this application, by combining with laser radar 3 and depth camera 6, three-dimensional construction of water environment can be realized in intelligent development board 8, autonomous path planning can be performed, and the planned path is transmitted to STM32 driver board 12, which controls brushless motor driver module 7 to control the movement of unmanned boat. The STM32 driver board 12 integrates the Beidou positioning module 1, the light intensity sensor 13, the water quality sensor 15, and the turbidity and pH sensor 16. The Beidou positioning module 1 is used to realize the real-time positioning of the unmanned boat. The light intensity sensor 13 is used to detect the ambient light intensity. The water quality sensor 15 is used to detect the water quality of the water area. The turbidity and pH sensor 16 is used to detect the turbidity and pH of the water area. The core data is collected through the sensor module and transmitted to the remote cloud platform. After precise processing and comprehensive analysis, the control of the unmanned boat and the detection of the water environment are realized.

[0038] The brushless motor drive module 7 is installed on both sides of the hull 4 and is controlled by the relay 11 on the STM32 drive board 12 to realize the operation of the unmanned boat.

[0039] The power supply 10 is respectively connected to the intelligent development board 8 and the STM32 driver board 12 to provide power thereto, thereby ensuring the normal operation of both.

[0040] The remote cloud platform ensures efficient management and stable operation of equipment. Operators can quickly obtain key data such as the location, speed and surrounding environment of the ship, so as to effectively monitor it. This function is achieved by extracting information related to the environment and the ship itself from the database. The remote cloud platform is designed for personnel management and equipment maintenance, providing an intuitive user interface, integrating fault detection and preventive maintenance functions, and monitoring key indicators in real time.

[0041] In this embodiment, a lighting lamp 5 is also included, and the lighting lamp 5 is connected to the STM32 driver board 12. A lighting mounting plate is provided in front of the hull 4, and the lighting lamp 5 is mounted on the lighting mounting plate. The lighting lamp 5 is used in an environment with insufficient light, and the light intensity of the water area is detected by the light intensity sensor 13. When the light intensity is dark, the STM32 driver board 12 controls the lighting lamp 5 to turn on, so as to facilitate the depth camera 6 to shoot.

[0042] In this embodiment, a through hole is provided in front of the hull 4, and the depth camera 6 is installed in the through hole. The depth camera 6 is installed in front of the hull 4 for easy shooting.

[0043] In this embodiment, a heat dissipation hole is provided at the rear of the hull 4, and a heat dissipation fan 14 is installed in the heat dissipation hole, and the heat dissipation fan 14 is directly opposite to the STM32 driver board 12 in the hull 4. The heat dissipation fan 14 in the heat dissipation hole can dissipate heat for the parts in the hull 4 to prevent the parts from aging and damage.

[0044] In this embodiment, the two sides of the hull 4 are respectively installed with drive mounting bases 7 - 1 , and the brushless motor drive module 7 is installed on the drive mounting bases 7 - 1 . The drive mounting bases 7 - 1 are used to realize the installation and fixation of the brushless motor drive module 7 .

[0045] In this embodiment: the rear of the hull 4 is also provided with a mounting slot and a collecting hole, the light intensity sensor 13 is installed in the mounting slot, a detection slot is provided in the hull 4, the collecting hole is connected to the detection slot, and the water quality sensor 15 and the turbidity and pH sensor 16 are installed in the detection slot. The light intensity sensor 13 is used to collect the light intensity of the water area, which is installed at the rear of the hull 4 for convenient monitoring, and the water quality sensor 15 and the turbidity and pH sensor 16 are installed in the detection slot, which collects water in the water area through the collecting hole for detection. When the unmanned boat is launched, the collecting hole is located below the horizontal plane, which facilitates the water in the water area to enter the detection slot.

[0046] The hydrological and meteorological unmanned boat based on visual inertial laser radar proposed in the present invention can establish a three-dimensional map by setting up an IMU module, a depth camera 6, and a laser radar 3, and execute autonomous path planning and control the movement of the hull through an intelligent development board 8. The STM32 driver board 12 integrates a Beidou positioning module 1, a light intensity sensor 13, a water quality sensor 15, and a turbidity and pH sensor 16. The core data of the water area collected is accurately processed and comprehensively analyzed through data fusion technology to ensure the safe operation of the hull. The power of this application is supplied by a dedicated power supply 10. The measurement data is sent to the remote cloud platform server database through the MQTT communication protocol.

[0047] The data uploaded to the remote cloud platform through the STM32 driver board 12 can ensure efficient management and stable operation of the equipment. The operator can quickly obtain key data such as the position, speed and surrounding environment of the ship, so as to effectively monitor it. This function is achieved by extracting information related to the environment and the ship itself from the database. The management platform is designed for personnel management and equipment maintenance, providing an intuitive user interface, integrating fault detection and preventive maintenance functions, and monitoring key indicators in real time.

[0048] The present invention proposes a SLAM method for hydrological and meteorological unmanned boats based on visual inertial lidar, wherein the principle framework diagram is as follows Figure 4 As shown, the following steps are included: S1, tightly couple the lidar observation with stereo vision and inertial observation, and fuse the lidar information with the binocular vision IMU attitude estimation through tight coupling to form the binocular vision IMU-lidar odometer VILO front-end attitude estimator; State variables to be solved in the VILO front-end attitude estimator x As shown below: ; in, Represents the world coordinate system, in the state variable middle, x k It is captured k The IMU state at the time of the image, including the position of the IMU in the world coordinate system ,speed and direction , and the accelerometer zero bias in the IMU carrier coordinate system and gyro bias ,also, n is the total number of keyframes, m is the total number of feature points in the sliding window, Respectively represent The inverse depth of the feature point in the first observed frame is l i Indicates i The inverse depth of the feature points in the first observed frame; In the local pose estimation process of VILO, the observation set Z used to constrain the variables to be estimated is defined by the following formula: ; In the formula, represents the visual measurement value, Is the collection of i Frame and i +1 frame image carrier coordinate system, Indicates the measured value from i Frame keyframe to i +1 keyframe lidar odometer value, k is the keyframe set in the sliding window.

[0049] S2. Construct a nonlinear least squares model based on binocular vision IMU lidar: The nonlinear least squares model of binocular vision IMU lidar is as follows: According to the residual factor of the binocular vision IMU lidar, and using the Mahalanobis distance to represent the degree of deviation between the residual and covariance matrix of each sensor, the VILO nonlinear least squares problem is constructed as follows: ; In the above formula is the covariance matrix of the residual r The Mahalanobis distance is S , the Mahalanobis distance is defined as , the first item is the prior information of the marginalization of the key frames in the sliding window, r b is the residual error of the IMU, r c is the binocular vision residual, r L is the lidar odometry residual, k is the key frame set in the sliding window; In binocular vision residual r c and the lidar odometry residual r L The Huber loss function is used in r for: ; The Huber loss identifies outliers by considering the residual, denoted by s.

[0050] S3. Calculate and constrain binocular vision residuals r c : The pose of the keyframe is defined as From keyframe i Transition to Keyframe j , Two orthogonal basis vectors representing the tangent plane of the unit sphere, used to map the 3D camera coordinate difference to the unit sphere; is the pose from IMU to depth camera obtained by pre-calibration; S4. Calculate and constrain IMU residuals r b : When the initial attitude of the unmanned ship is known, the IMU is used for continuous integration time to obtain the speed and attitude of the unmanned ship relative to the reference coordinates in real time. The IMU integration information is then used to complement the information of the lidar and depth camera. In the visual inertial odometer, the IMU integration information between key frames is added as the residual constraint of the entire bundle adjustment optimization. According to the IMU pre-integration model, the IMU residual is as follows: ; The above formula subtracts the IMU pre-integrated measurement value from the predicted value to obtain the IMU prediction score residual term. In the variable, is the pre-integrated three-dimensional coordinate position residual of the IMU, is the pre-integrated velocity residual of the IMU, is the rotation error of the three axes of the IMU, They are the zero-point error residuals of the accelerometer and gyroscope in the IMU; S5. Calculate and constrain the residual of the lidar odometer r L : Based on the acquisition of lidar data, the attitude estimation problem of the lidar in the map is transformed into a nonlinear least squares problem. The formula is as follows: ; In the formula, is the attitude variable of the lidar relative to the grid map coordinate system, Indicates that when the laser radar is in attitude When the laser i The endpoint coordinates of the scanning ray in the grid map coordinate system, Is the grid map at a given coordinate position The probability of becoming an obstacle, the purpose of the above formula is to find the minimum objective function of Posture variables, It is expressed as follows: ; In the above formula, ,in and Represents the coordinates of the origin of the laser sensor coordinate system relative to the reference coordinate system, i is the heading angle of the unmanned ship, and the observation points of the laser radar are converted into the grid map coordinate system. Therefore, the core of the scan matching algorithm is to give the laser radar posture with a small deviation from the actual posture at the beginning. , and then according to the current time t Acquire laser data to find attitude increments , so that the end point of the laser scan and the probability grid map are at the current time t The best match is to establish the following formula: ; In order to solve the current time t The optimal posture increment that minimizes the above equation is , using the Gauss-Newton iteration method for the nonlinear function Performing a first-order Taylor expansion, we get the following formula: ; In the above formula, i is the laser sensor at time t No. i A laser beam will n The laser beams are added together to obtain the attitude increment from the objective function of the above formula. The partial derivative of makes the partial derivative equal to 0. Finally, the attitude increment as follows: ; In the above formula, Represents the probability value of an obstacle at the coordinate ,because Not an integer pair, in order to express , assuming , then write The approximation is as shown below: ; The Hessian matrix H As shown in the following formula; ; in About Posture The partial derivative of is shown below: ; In the LiDAR attitude Nearby approximate objective function , and perform multiple optimization iterations until the posture increment Small enough, the iteration stops and the current time is obtained t The optimal LiDAR pose , and the residual sum of squares of the original model is the smallest; S6. Derivation of the residual of the lidar odometer r L Relative to the state variable to be estimated The Jacobian matrix analytical formula of ; Residual factor of LiDAR attitude r LThe laser radar odometer is used to observe the i Frame and i +1 relative displacement between frames To represent the relative displacement of the last two key frames before and after the state variable Optimization, and They are called observed values ​​and predicted values ​​respectively, and their specific forms are shown in the following formula: ; In the above formula, L is the laser radar coordinate system, Indicates that IMU is i The pose of the frame relative to the world coordinate system, Indicates i Frame and i +1 relative pose estimation between frames, The specific expansion form is as follows: ; Finally, the relative displacement The residual term of the position and the posture of the variable to be optimized and lidar odometry frames i and i +1 obtained as follows: ; In the above formula, the relative displacement Obtained through the laser radar odometer, It is the attitude of the LiDAR coordinate system relative to the IMU coordinate system, which is obtained by pre-calibration. Since the LiDAR odometer can only observe the attitude yaw angle i , the residual model can only constrain the attitude yaw angle to the state variable , and then the state variables , specifically , residual r L The Jacobian matrix of the state variable The estimates are as follows: ; In the above formula, ∧ is the transformation from vector to antisymmetric matrix, J[0] 3×10 and J[1] 3×10 is the lidar residual r L The first-order partial derivative is relative to i and i +1 keyframe pose and , also known as the Jacobian matrix.

[0051] S7. After the binocular vision residual, IMU residual, and lidar odometry residual are jointly constrained and continuously iteratively optimized, the front-end attitude estimator is solved in indoor and outdoor environments.

[0052] Covariance matrix of lidar ranging residual terms S is the covariance matrix of attitude estimation in the LiDAR ranging model. The covariance matrix can be obtained by the laser scan matching algorithm.

[0053] Limited by the above steps, this method has constructed the least squares problem based on the binocular IMU lidar and calculated the visual residual r c , IMU residual r b and the lidar odometry residual r L In addition, the lidar odometry residual is derived r L Relative to the state variable to be estimated x The Jacobian matrix analytical formula of . Therefore, after the stereo vision residual, IMU residual, and lidar odometry residual are jointly constrained and continuously iteratively optimized, the front-end attitude estimator can be solved in indoor and outdoor environments.

[0054] The SLAM method for the hydrometeorological unmanned vessel in the present invention effectively improves the accuracy and robustness of posture estimation of the traditional visual-inertial SLAM algorithm when visual features are lost due to darkness, strong light or lack of texture by tightly coupling low-cost lidar observations with stereo vision and inertial observations.

[0055] The lidar residual factor is constructed by utilizing a two-dimensional lidar odometry model, and the Jacobian matrix of the lidar residual relative to the state variable to be estimated is derived.

[0056] By constructing the residual constraint equation of the visual IMU LiDAR and using a nonlinear optimization method to obtain the optimal unmanned ship posture estimation, the problem of fusing LiDAR observations with binocular visual inertial information in a tightly coupled manner is solved.

[0057] The LiDAR information is fused with the stereo vision IMU pose estimation through tight coupling to form a stereo vision IMU-LiDAR odometer (VILO) front-end pose estimator. It also aims to increase LiDAR observations to further constrain the pose, thereby improving the accuracy and robustness of SLAM front-end pose estimation in indoor and outdoor environments.

[0058] System flow chart as follows Figure 5As shown. The system receives sensor data from two-dimensional lidar, inertial measurement unit (IMU) and stereo camera. The state and motion trajectory of the unmanned ship are estimated through these observations. The state estimation problem can be modeled as a maximum a posteriori estimation (MAP) problem. Under the assumption of a Gaussian noise model, solving the MAP inference is equivalent to solving a nonlinear least squares problem. This system constructs residual constraints for lidar, IMU and camera, and uses a nonlinear optimization library for optimization calculation to output the position and posture of the unmanned ship in the world coordinate system.

[0059] The above description is only a preferred embodiment of the present invention and does not constitute any other form of limitation to the present invention. Any modification or equivalent change made based on the technical essence of the present invention still falls within the scope of protection required by the present invention.

Claims

1. Hydrological and meteorological unmanned boat based on visual inertial lidar, characterized by: The invention comprises an unmanned ship protection cover (2) and a hull (4), wherein the unmanned ship protection cover (2) is installed above the hull (4), and further comprises a laser radar (3), a depth camera (6), an IMU module and an intelligent development board (8), wherein the laser radar (3), the depth camera (6) and the IMU module are respectively connected to the intelligent development board (8), wherein the laser radar (3) is installed above the unmanned ship protection cover (2), wherein the depth camera (6), the IMU module and the intelligent development board (8) are installed inside the hull (4), and further comprises a Beidou positioning module (1), a sensor module and an STM32 driver board (12), wherein the intelligent development board (8) is connected to the STM32 driver board (12) to realize data exchange, wherein the Beidou positioning module (1) is connected to the STM32 driver board (12), and wherein the sensor module comprises a light intensity sensor (13), a water quality sensor (15) and a turbidity and pH sensor. The invention relates to a device (16), wherein the light intensity sensor (13), the water quality sensor (15) and the turbidity and pH sensor (16) are respectively connected to the STM32 driver board (12), the Beidou positioning module (1) is installed on the unmanned ship protection cover (2), the STM32 driver board (12) is installed in the hull (4), and further comprises a brushless motor driver module (7) and an electronic speed regulator (9), wherein the brushless motor driver module (7) is installed on both sides of the hull (4), a relay (11) is installed on the STM32 driver board (12), and the brushless motor driver module (7) is connected to the relay (11) on the STM32 driver board (12) through the electronic speed regulator (9), and further comprises a power supply (10), wherein the power supply (10) is respectively connected to the intelligent development board (8) and the STM32 driver board (12), and the STM32 driver board (12) is connected to a remote cloud platform signal.

2. The hydrological and meteorological unmanned boat based on visual inertial laser radar according to claim 1 is characterized in that: It also includes a lighting lamp (5), the lighting lamp (5) being connected to an STM32 driving board (12), a lighting installation board being arranged in front of the hull (4), and the lighting lamp (5) being installed on the lighting installation board.

3. The hydrological and meteorological unmanned boat based on visual inertial laser radar according to claim 1 is characterized in that: A through hole is provided in the front of the hull (4), and the depth camera (6) is installed in the through hole.

4. The hydrological and meteorological unmanned ship based on visual inertial laser radar according to claim 1 is characterized in that: A heat dissipation hole is arranged at the rear of the hull (4), a heat dissipation fan (14) is installed in the heat dissipation hole, and the heat dissipation fan (14) is directly opposite to the STM32 driving board (12) in the hull (4).

5. The hydrological and meteorological unmanned boat based on visual inertial laser radar according to claim 1 is characterized in that: Drive mounting bases (7-1) are respectively installed on both sides of the hull (4), and the brushless motor drive module (7) is installed on the drive mounting bases (7-1).

6. The hydrological and meteorological unmanned ship based on visual inertial laser radar according to claim 1 is characterized in that: The rear of the hull (4) is also provided with an installation groove and a collecting hole, the light intensity sensor (13) is installed in the installation groove, a detection groove is provided in the hull (4), the collecting hole is connected to the detection groove, and the water quality sensor (15) and the turbidity and pH sensor (16) are installed in the detection groove.

7. The SLAM method for a hydrological and meteorological unmanned ship based on visual inertial laser radar according to any one of claims 1 to 6, characterized in that: The steps include: S1, tightly couple the lidar observation with stereo vision and inertial observation, and fuse the lidar information with the binocular vision IMU attitude estimation through tight coupling to form the binocular vision IMU-lidar odometer VILO front-end attitude estimator; S2. Construct a nonlinear least squares model based on binocular vision IMU lidar: S3. Calculate and constrain binocular vision residuals r c : The pose of the keyframe is defined as From keyframe i Transition to Keyframe j , Two orthogonal basis vectors representing the tangent plane of the unit sphere, used to map the 3D camera coordinate difference to the unit sphere; is the pose from IMU to depth camera obtained through pre-calibration; S4. Calculate and constrain IMU residuals r b : When the initial attitude of the unmanned ship is known, the IMU is used for continuous integration time to obtain the speed and attitude of the unmanned ship relative to the reference coordinates in real time. The IMU integration information is then used to complement the information of the lidar and depth camera. In the visual inertial odometer, the IMU integration information between key frames is added as the residual constraint of the entire bundle adjustment optimization. According to the IMU pre-integration model, the IMU residual is as follows: ； The above formula subtracts the IMU pre-integrated measurement value from the predicted value to obtain the IMU prediction score residual term. In the variable, is the pre-integrated three-dimensional coordinate position residual of the IMU, is the pre-integrated velocity residual of the IMU, is the rotation error of the three axes of the IMU, They are the zero-point error residuals of the accelerometer and gyroscope in the IMU; S5. Calculate and constrain the residual of the lidar odometer r L : Based on the acquisition of lidar data, the attitude estimation problem of the lidar in the map is transformed into a nonlinear least squares problem. The formula is as follows: ； In the formula, is the attitude variable of the lidar relative to the grid map coordinate system, Indicates that when the laser radar is in attitude When the laser i The endpoint coordinates of the scanning ray in the grid map coordinate system, Is the grid map at a given coordinate position The probability of becoming an obstacle, the purpose of the above formula is to find the minimum objective function of Posture variables, It is expressed as follows: ； In the above formula, ,in and Represents the coordinates of the origin of the laser sensor coordinate system relative to the reference coordinate system, θ is the heading angle of the unmanned ship, and the observation points of the laser radar are converted into the grid map coordinate system. Therefore, the core of the scan matching algorithm is to give the laser radar posture with a small deviation from the actual posture at the beginning. , and then according to the current time t Acquire laser data to find attitude increments , so that the end point of the laser scan and the probability grid map are at the current time t The best match is to establish the following formula: ； In order to solve the current time t The optimal posture increment that minimizes the above equation is , using the Gauss-Newton iteration method for the nonlinear function Performing a first-order Taylor expansion, we get the following formula: ； In the above formula, i is the laser sensor at time t No. i A laser beam will n The laser beams are added together to obtain the attitude increment from the objective function of the above formula. The partial derivative of makes the partial derivative equal to 0. Finally, the attitude increment as follows: ； In the above formula, Represents the probability value of obstacles at the coordinates ,because Not an integer pair, in order to express , assuming , then write The approximation is as shown below: ； The Hessian matrix H As shown in the following formula; ; in About Posture The partial derivative of is shown below: ; In the LiDAR attitude Nearby approximate objective function , and perform multiple optimization iterations until the posture increment Small enough, the iteration stops and the current time is obtained t The optimal LiDAR pose , and the residual sum of squares of the original model is the smallest; S6. Derivation of the residual of the lidar odometer r L Relative to the state variable to be estimated The Jacobian matrix analytical formula of ; S7. After the binocular vision residual, IMU residual, and lidar odometry residual are jointly constrained and continuously iteratively optimized, the front-end attitude estimator is solved in indoor and outdoor environments.

8. The SLAM method for hydrological and meteorological unmanned boat based on visual inertial laser radar according to claim 7 is characterized in that: The state variables to be solved in the VILO front-end attitude estimator in step S1 χ As shown below: ； in, Represents the world coordinate system, in the state variable middle, x k It is captured k The IMU state at the time of the image, including the position of the IMU in the world coordinate system ,speed and direction , and the accelerometer zero bias in the IMU carrier coordinate system and gyro bias ,also, n is the total number of keyframes, m is the total number of feature points in the sliding window, Respectively represent The inverse depth of the feature point in the first observed frame is λ i Indicates i The inverse depth of the feature points in the first observed frame; In the local pose estimation process of VILO, the observation set Z used to constrain the variables to be estimated is defined by the following formula: ; In the formula, represents the visual measurement value, Is the collection of i Frame and i +1 frame image carrier coordinate system, Indicates the measured value from i Frame keyframe to i +1 keyframe lidar odometer value, κ is the keyframe set in the sliding window.

9. The SLAM method for hydrological and meteorological unmanned boat based on visual inertial laser radar according to claim 7 is characterized in that: The nonlinear least squares model of the binocular vision IMU laser radar in step S2 is as follows: According to the residual factor of the binocular vision IMU lidar, and using the Mahalanobis distance to represent the degree of deviation between the residual and covariance matrix of each sensor, the VILO nonlinear least squares problem is constructed as follows: ; In the above formula is the covariance matrix of the residual r The Mahalanobis distance is Σ , the Mahalanobis distance is defined as , the first item is the prior information of the marginalization of the key frames in the sliding window, r b is the residual error of the IMU, r c is the binocular vision residual, r L is the lidar odometry residual, κ is the key frame set in the sliding window; In binocular vision residual r c and the lidar odometry residual r L The Huber loss function is used in ρ for: ; The Huber loss identifies outliers by considering the residual, denoted by s.

10. The SLAM method for hydrological and meteorological unmanned boat based on visual inertial laser radar according to claim 7, characterized in that: The residual factor of the laser radar attitude in step S6 r L The laser radar odometer is used to observe the i Frame and i +1 relative displacement between frames To represent the relative displacement of the last two key frames before and after the state variable Optimization, and They are called observed values ​​and predicted values ​​respectively, and their specific forms are shown in the following formula: ; In the above formula, L is the laser radar coordinate system, Indicates that IMU is i The pose of the frame relative to the world coordinate system, Indicates i Frame and i +1 relative pose estimation between frames, The specific expansion form is as follows: ; Finally, the relative displacement The residual term of the position and the posture of the variable to be optimized and lidar odometry frames i and i +1 obtained as follows: ; In the above formula, the relative displacement Obtained through the laser radar odometer, It is the attitude of the LiDAR coordinate system relative to the IMU coordinate system, which is obtained by pre-calibration. Since the LiDAR odometer can only observe the attitude yaw angle θ , the residual model can only constrain the attitude yaw angle to the state variable , and then the state variables , specifically , residual r L The Jacobian matrix of the state variable The estimates are as follows: ; In the above formula, ∧ is the transformation from vector to antisymmetric matrix, J[0] 3×10 and J[1] 3×10 is the lidar residual r L The first-order partial derivative is relative to i and i +1 keyframe pose and , also known as the Jacobian matrix.

Images