Multi-sensor fusion positioning method and electronic equipment

Through multi-sensor fusion technology, combined with IMU, vision and lidar data, SLAM and RTK algorithms are used to solve the problem of inaccurate positioning when GNSS signals are weak, and high-precision and robust positioning are achieved in different environments.

CN119164389APending Publication Date: 2024-12-20SHANGHAI DAHUA SURVEYING & MAPPING TECHNOLOGY CO LTD
View PDF 0 Cites 2 Cited by

Patent Information

Application Number
CN202411402721.5
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2024-10-09
Publication Date
2024-12-20

AI Technical Summary

Technical Problem

In an environment where GNSS signals are weak or without signals, it is difficult for the prior art to achieve accurate continuous positioning, especially when lasers and visual feature points are sparse, resulting in inaccurate positioning.

Method used

Using a multi-sensor fusion positioning method, combining inertial measurement unit (IMU), visual and lidar data, the instant positioning and map construction (SLAM) algorithm and real-time dynamic carrier phase difference (RTK) positioning technology are used to adaptively evaluate each sensor's Odometer accuracy, select the appropriate fusion scheme for positioning.

Benefits of technology

Achieve high-precision and robust continuous positioning in different scenarios, with a wider range of application, fully combining the advantages of multiple sensors, improving positioning accuracy and adaptability.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN119164389A_ABST
    Figure CN119164389A_ABST
Patent Text Reader

Abstract

The invention provides a multi-sensor fusion positioning method. The method comprises the following steps: judging whether GNSS signal intensity is greater than a signal threshold value or not; and if the signal threshold value is greater than the signal threshold value, acquiring GNSS positioning information and I MU information, fusing RTK positioning information and I MU acceleration information by using an EKF algorithm, and determining continuous accurate positioning information. If the GNSS signal strength is smaller than or equal to the signal threshold value, whether the visual feature richness is larger than a visual threshold value or not is judged; if the visual features are rich and are greater than the visual threshold value, using an SLAM algorithm to fuse the I MU information, the radar information and the visual information, and determining perception positioning information; and performing auxiliary correction on the sensing positioning information by using the RTK positioning information, and determining continuous accurate positioning information. And if the visual features are rich and are less than or equal to the visual threshold value, fusing the I MU information and the radar information by using an SLAM algorithm, and determining continuous accurate positioning information. According to the scheme provided by the invention, positioning requirements of different scenes can be adapted, and accurate and continuous positioning is realized.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention relates to the field of automatic positioning technology, and in particular to a multi-sensor fusion positioning method and electronic equipment. Background Art

[0002] In the application of autonomous driving of unmanned ships in modern engineering, accurate positioning information is crucial. Most related technologies rely on the Global Navigation Satellite System (GNSS) for positioning, but in environments with weak or no GNSS signals (such as canyons, bridge tunnels, etc.), it is difficult to meet the needs by relying solely on GNSS. Simultaneous Localization and Mapping (SLAM) technology can use lidar and cameras for real-time positioning and map construction, but there is also the problem of sparse laser and visual feature points in the environment, resulting in sensor degradation and inaccurate positioning.

[0003] Therefore, how to adapt to the positioning requirements of different scenarios and achieve accurate and continuous positioning is a technical problem that needs to be solved urgently. Summary of the invention

[0004] The embodiments of the present application provide a multi-sensor fusion positioning method and electronic device, which can adapt to the positioning requirements of different scenarios and achieve accurate and continuous positioning.

[0005] An embodiment of the present application provides a multi-sensor fusion positioning method, comprising the following steps: determining whether the global navigation satellite system GNSS signal strength is greater than a signal threshold; if the GNSS signal strength is greater than the signal threshold, obtaining GNSS positioning information and inertial measurement unit I MU information, and using an extended Kalman filter EKF algorithm to fuse real-time dynamic carrier phase differential RTK positioning information and I MU acceleration information to determine continuous precise positioning information; wherein the RTK positioning information is determined by the GNSS positioning information, and the I MU acceleration information is determined by the I MU information.

[0006] In one embodiment, if the GNSS signal strength is less than or equal to the signal threshold, it is determined whether the visual feature richness is greater than the visual threshold; if the visual feature richness is greater than the visual threshold, GNSS positioning information, I MU information, radar information and visual information are obtained, and the I MU information, radar information and visual information are integrated using a real-time positioning and map construction SLAM algorithm to determine the perceived positioning information; the RTK positioning information is used to assist in correcting the perceived positioning information to determine continuous and precise positioning information.

[0007] In one embodiment, if the visual feature richness is less than or equal to the visual threshold, IMU information and radar information are obtained, and the IMU information and radar information are fused using a SLAM algorithm to determine continuous and precise positioning information.

[0008] In one embodiment, the SLAM algorithm includes a radar inertial odometry module and a visual inertial odometry module.

[0009] In one embodiment, the using of a SLAM algorithm to fuse the I MU information, radar information and visual information to determine the perception positioning information includes: determining point cloud information according to the radar information;

[0010] According to the visual information, the posture information is determined; according to the point cloud information and the posture information, a SLAM algorithm is used to establish a three-dimensional topographic map of the environment and determine the initial positioning information; according to the IMU information, the initial positioning information is corrected to obtain the perceived positioning information.

[0011] In one embodiment, a three-dimensional topographic map of the environment is established using a SLAM algorithm based on the point cloud information and posture information, including: processing each frame of point cloud information to obtain feature points, feature lines and feature surfaces; comparing the coordinate changes of the same feature points in the point cloud information of two adjacent frames, and obtaining the three-dimensional spatial relative displacement and relative three-dimensional posture angle change between the point cloud information of two adjacent frames based on the laser scanning visual measurement principle; through coordinate system conversion, the point cloud information of the latter frame is aligned with the point cloud information of the previous frame, and through continuous alignment processing, the point cloud information of each frame can be aligned to the reference coordinate system of the starting point of the laser scanning measurement to obtain a three-dimensional topographic map of the environment.

[0012] In one embodiment, the initial positioning information is corrected according to the IMU information to obtain the perceived positioning information, including: according to the IMU information, factor graph maintenance, deep feature association, radar failure detection, visual failure detection and loop detection are used to correct the initial positioning information to obtain the perceived positioning information.

[0013] In one embodiment, the GNSS positioning information is obtained through a GNSS antenna; the IMU information is obtained through an IMU; the radar information is obtained through a multi-line mechanical laser radar and a full-field hybrid solid-state laser radar; and the visual information is obtained through an industrial camera.

[0014] In one embodiment, before acquiring IMU information, radar information and visual information, it also includes: calibrating the IMU, multi-line mechanical laser radar, full-field hybrid solid-state laser radar and industrial camera.

[0015] An embodiment of the present application further provides an electronic device, comprising: a processor; a memory for storing instructions executable by the processor; wherein the processor is configured to execute the above-mentioned multi-sensor fusion positioning method.

[0016] The solution provided in the above-mentioned embodiments of the present application realizes high-precision positioning by integrating sensor data such as inertial measurement unit (IMU), vision, laser radar, real-time kinematic (RTK), etc. In the case of strong GNSS signal on the open water surface, GNSS and IMU are integrated to locate the unmanned ship, and laser radar and camera are used to detect obstacles to achieve positioning and navigation; when evaluating the odometer accuracy and the GNSS signal is weak, laser radar is used to build a grid map, and the SLAM algorithm of multi-sensors is integrated to locate the unmanned ship and detect obstacles to achieve positioning and navigation. Compared with the prior art, the present invention adaptively evaluates the estimated accuracy of each sensor odometer in different scenarios when the GNSS signal is missing, and adopts different sensor fusion schemes for positioning and navigation of the unmanned ship. It has a wider scope of application, fully combines the advantages of multiple sensors, has high positioning accuracy and good robustness. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] In order to more clearly illustrate the technical solution of the embodiments of the present application, the drawings required for use in the embodiments of the present application are briefly introduced below.

[0018] Figure 1 is a schematic diagram of the structure of an electronic device provided in an embodiment of the present application;

[0019] Figure 2 It is a flowchart of a multi-sensor fusion positioning method provided in an embodiment of the present application;

[0020] Figure 3 It is a flowchart of another multi-sensor fusion positioning method provided in an embodiment of the present application; Figure 4 It is a flow chart of an adaptive evaluation continuous positioning working method provided in an embodiment of the present application. DETAILED DESCRIPTION

[0021] The technical solutions in the embodiments of the present application will be described below in conjunction with the drawings in the embodiments of the present application.

[0022] Similar reference numerals and letters represent similar items in the following drawings, so once an item is defined in one drawing, it does not need to be further defined and explained in subsequent drawings. At the same time, in the description of this application, the terms "first", "second", etc. are only used to distinguish the description and cannot be understood as indicating or implying relative importance.

[0023] Figure 1 1 is a schematic diagram of the structure of an electronic device provided in an embodiment of the present application. The electronic device 100 can be used to execute the multi-sensor fusion positioning method provided in an embodiment of the present application. Figure 1 As shown, the electronic device 100 includes: one or more processors 102, and one or more memories 104 storing processor executable instructions. The processor 102 is configured to execute the multi-sensor fusion positioning method provided in the following embodiments of the present application.

[0024] The processor 102 can be a gateway, a smart terminal, or a device including a central processing unit (CPU), a graphics processing unit (GPU), or other forms of processing units with data processing capabilities and / or instruction execution capabilities. It can process data of other components in the electronic device 100 and control other components in the electronic device 100 to perform desired functions.

[0025] The memory 104 may include one or more computer program products, and the computer program product may include various forms of computer-readable storage media, such as volatile memory and / or non-volatile memory. The volatile memory may include, for example, random access memory (RAM) and / or cache memory (cache), etc. The non-volatile memory may include, for example, read-only memory (ROM), hard disk, flash memory, etc. One or more computer program instructions may be stored on the computer-readable storage medium, and the processor 102 may run the program instructions to implement the multi-sensor fusion positioning method described below. Various applications and various data, such as various data used and / or generated by the application, may also be stored in the computer-readable storage medium.

[0026] In one embodiment, Figure 1 The electronic device 100 shown may also include an input device 106, an output device 108, and a data acquisition device 110, which are interconnected via a bus system 112 and / or other forms of connection mechanisms (not shown). Figure 1 The components and structures of the electronic device 100 shown are merely exemplary and non-limiting. The electronic device 100 may also have other components and structures as required.

[0027] The input device 106 may be a device for a user to input instructions, and may include one or more of a keyboard, a mouse, a microphone, a touch screen, etc. The output device 108 may output various information (e.g., images or sounds) to the outside (e.g., a user), and may include one or more of a display, a speaker, etc. The data acquisition device 110 may collect relevant information of the sensor, such as GNSS positioning information, IMU information, radar information, and visual information, etc., and store the collected data in the memory 104 for use by other components. Exemplarily, the data acquisition device 110 may be a camera.

[0028] In one embodiment, the various components in the example electronic device 100 for implementing the multi-sensor fusion positioning method of the embodiment of the present application can be integrated or dispersed, such as integrating the processor 102, memory 104, input device 106 and output device 108 into one, and separately arranging the data acquisition device 110.

[0029] In one embodiment, the example electronic device 100 for implementing the multi-sensor fusion positioning method of the embodiment of the present application can be implemented as an intelligent terminal such as a smart phone, a tablet computer, a desktop computer, a server, a vehicle-mounted device, etc.

[0030] Figure 2 FIG. 1 is a flow chart of a multi-sensor fusion positioning method according to an embodiment of the present application. Figure 2 As shown, the method includes the following steps 210-220.

[0031] Step 210: Determine whether the global navigation satellite system GNSS signal strength is greater than a signal threshold.

[0032] Step 220: If the GNSS signal strength is greater than the signal threshold, the GNSS positioning information and the inertial measurement unit IMU information are obtained, and the extended Kalman filter (EKF) algorithm is used to fuse the real-time dynamic carrier phase differential RTK positioning information and the IMU acceleration information to determine continuous precise positioning information.

[0033] The RTK positioning information is determined by the GNSS positioning information, and the IMU acceleration information is determined by the IMU information.

[0034] In one embodiment, Figure 3 As shown, the multi-sensor fusion positioning method also includes steps 230 to 250.

[0035] Step 230: If the GNSS signal strength is less than or equal to the signal threshold, determine whether the visual feature richness is greater than the visual threshold.

[0036] Step 240: If the visual feature richness is greater than the visual threshold, obtain GNSS positioning information, IMU information, radar information and visual information, use the real-time positioning and map construction SLAM algorithm to fuse the IMU information, radar information and visual information to determine the perceived positioning information; use RTK positioning information to assist in correcting the perceived positioning information to determine continuous and precise positioning information.

[0037] Step 250: If the visual feature richness is less than or equal to the visual threshold, IMU information and radar information are obtained, and the IMU information and radar information are fused using a SLAM algorithm to determine continuous and precise positioning information.

[0038] In steps 240 and 250, the SLAM algorithm includes a radar inertial odometry module and a visual inertial odometry module.

[0039] In step 240, the SLAM algorithm is used to fuse the IMU information, radar information and visual information to determine the perception positioning information, including: determining the point cloud information according to the radar information; determining the posture information according to the visual information; using the SLAM algorithm to establish a three-dimensional topographic map of the environment according to the point cloud information and the posture information, and determining the initial positioning information; correcting the initial positioning information according to the IMU information to obtain the perception positioning information. The SLAM algorithm is used to establish a three-dimensional topographic map of the environment according to the point cloud information and the posture information, including: processing each frame of point cloud information to obtain feature points, feature lines and feature surfaces; comparing the coordinate changes of the same feature points in the point cloud information of two adjacent frames, and obtaining the three-dimensional space relative displacement and relative three-dimensional attitude angle change between the point cloud information of two adjacent frames according to the laser scanning visual measurement principle; through the coordinate system conversion, the point cloud information of the latter frame is registered with the point cloud information of the previous frame, and through continuous registration processing, the point cloud information of each frame can be registered in the reference coordinate system of the starting point of the laser scanning measurement to obtain the three-dimensional topographic map of the environment. The method of correcting the initial positioning information according to the IMU information to obtain the perceived positioning information includes: correcting the initial positioning information according to the IMU information using factor graph maintenance, deep feature association, radar failure detection, visual failure detection and loop detection to obtain the perceived positioning information.

[0040] Before step 210, the method also includes: the GNSS positioning information is obtained through a GNSS antenna; the IMU information is obtained through an IMU; the radar information is obtained through a multi-line mechanical laser radar and a full-field hybrid solid-state laser radar; and the visual information is obtained through an industrial camera.

[0041] Before acquiring IMU information, radar information and visual information, the method also includes: calibrating the IMU, multi-line mechanical laser radar, full-field hybrid solid-state laser radar and industrial camera.

[0042] In one embodiment, the above-mentioned GNSS positioning information, IMU information, radar information and visual information are obtained through a multi-sensor fusion positioning device.

[0043] The multi-sensor positioning device includes a sensor scanning module and a data transmission and power supply module, and the sensor scanning module can be disassembled and installed separately. The sensor scanning device includes a sensor integrated control box and various sensors. Each sensor includes: a multi-line mechanical laser radar, a full-field hybrid solid-state laser radar, an industrial camera, an IMU104 and a GNSS antenna. The industrial camera and the IMU are located inside the sensor integrated control box, and the multi-line mechanical laser radar, the full-field hybrid solid-state laser radar and the GNSS antenna are located outside the sensor integrated control box. The sensor integrated control box contains some sensors and can play a certain role in waterproofing. The sensor integrated control box also includes a data transmission and power supply device. The data transmission and power supply device includes a network switch, a wireless routing module, a laser radar junction box and a voltage conversion module.

[0044] The multi-line mechanical laser radar is installed and fixed on the top of the sensor integrated control box to ensure that the radar scanning field of view is not blocked. It is used to send lasers to the environment and measure the distance and reflection intensity information of the laser emission direction during movement to obtain spatial point cloud data. The full-field hybrid solid-state laser radar is installed and fixed in front of the sensor integrated control box to ensure that the radar scanning field of view is not blocked and perceive the information of the large field of view in the space. The network switch is fixed on the back of the sensor integrated control box, connected to the multi-line mechanical laser radar and the full-field hybrid solid-state laser radar through multiple network cables, and connected to the control unit through a network cable to meet the needs of multi-radar data collection. The laser radar junction box is fixed on the bottom of the sensor integrated control box and is used for power supply and data return of the multi-line mechanical laser radar. In this way, the radar information is finally obtained.

[0045] The industrial camera is installed inside the sensor integrated control box, and the industrial camera lens is exposed to ensure that the field of view is not blocked. Symmetrically along the longitudinal center axis of the sensor integrated control box, an industrial camera is installed on each side to obtain a wider range of visual information. The industrial camera transmits data to the control unit through cables and realizes power supply. In this way, visual information is finally obtained.

[0046] The IMU is fixed inside the sensor integrated control box and integrates a gyroscope and an accelerometer. It can measure its own three-dimensional linear acceleration and three-dimensional attitude angular velocity, thereby obtaining instantaneous linear displacement and attitude angle. The IMU transmits data to the control unit through a cable and realizes power supply. In this way, the IMU information is finally obtained.

[0047] The GNSS antenna is fixed at the back of the sensor integrated control box to obtain GNSS positioning information. The wireless routing module is fixed at the back of the sensor integrated control box to provide 4G, 5G and other Internet services, ensure communication with the ground station, and realize real-time kinematic (RTK) measurement and positioning.

[0048] The voltage conversion module is fixed at the bottom of the sensor integrated control box and is used to power the full-field hybrid solid-state lidar, network switch, wireless routing module and lidar junction box.

[0049] The relative positions of the sensors remain fixed to facilitate subsequent calibration.

[0050] In one embodiment of the present application, the multi-sensor fusion positioning method can be divided into a calibration phase, a SLAM working phase and an adaptive positioning phase.

[0051] The calibration phase needs to be completed before the work starts. It mainly calibrates the internal parameters of the IMU and industrial camera, the external parameters between the IMU and the industrial camera, and the external parameters between the IMU and the multi-line mechanical laser radar and the full-field hybrid solid-state laser radar. The data is collected when the IMU remains stationary for two hours, and then the Allan variance of the IMU data is analyzed. The four noise values ​​of the IMU's gyroscope, accelerometer white noise, and bias instability are obtained.

[0052] The specific steps of the calibration phase are as follows:

[0053] Step 301: Based on the pinhole camera model, the internal parameters of the two industrial cameras are calibrated using a camera calibration plate to obtain the internal and external parameters and distortion parameters of the industrial cameras, including focal length, optical center, radial distortion, tangential distortion, etc.

[0054] Step 302: Calibrate the external parameters between sensors. Each sensor needs to be installed and fixed in the sensor integrated control box. Calibrate the external parameters of the two industrial cameras and the IMU. Combine the posture changes measured by the IMU with the external parameters between the industrial camera and the IMU to obtain the posture information of the industrial camera. Therefore, calibrating the external parameters between the industrial camera and the IMU can be achieved by performing nonlinear optimization between the known postures of the industrial camera and the IMU. By collecting a series of IMU and industrial camera data, solving the above nonlinear optimization problem, and obtaining the external parameter matrices between the two industrial cameras and the IMU, respectively.

[0055] Step 303: The laser radar does not need to be calibrated with internal parameters, and the external parameters of the multi-line mechanical laser radar, full-field hybrid solid-state laser radar and IMU can be calibrated directly. Similar to step 302, the external parameters between the multi-line mechanical laser radar, full-field hybrid solid-state laser radar and IMU can be calibrated by performing nonlinear optimization between the known IMU and multi-line mechanical laser radar and full-field hybrid solid-state laser radar data. Collect a series of multi-line mechanical laser radar, full-field hybrid solid-state laser radar and IMU data, and minimize the error between the IMU and the multi-line mechanical laser radar and full-field hybrid solid-state laser radar through nonlinear optimization to obtain the external parameter matrices of the multi-line mechanical laser radar, full-field hybrid solid-state laser radar and IMU respectively.

[0056] Step 304: Record the collected calibration data into the parameter table of the SLAM algorithm to eliminate the influence of noise, distortion, etc. and obtain the relative position relationship between the IMU, industrial camera, multi-line mechanical laser radar, and full-field hybrid solid-state laser radar.

[0057] Step 305: According to the actual positioning requirements of the unmanned boat operation, set the scanning range of the algorithm and the adjustment of the viewing field, limit the scanning range to a certain extent to avoid irrelevant interference. According to the field of view conditions on site, adjust the exposure parameters of the camera, etc. According to the actual size of the wind and waves, adjust the stability of the algorithm.

[0058] During the SLAM working phase, multi-line mechanical laser radar, full-field hybrid solid-state laser radar, industrial camera and IMU are turned on. Multi-line mechanical laser radar and full-field hybrid solid-state laser radar obtain the geometric information of the environment, that is, radar information. The industrial camera captures the visual features of the environment, that is, visual information. The posture information provided by IMU is used for motion estimation, that is, IMU information. The SLAM algorithm that enables multi-sensor (IMU, vision, multi-line laser and RTK) fusion integrates the data of these different sensors to achieve autonomous positioning and map construction. The overall algorithm includes two parts: radar inertial odometer module and visual inertial odometer module.

[0059] The specific steps of the SLAM working phase are as follows:

[0060] Step 401: Initial value estimation.

[0061] Initial values ​​are very important for continuous scan matching, especially in intense motion. At the same time, frame-to-frame matching is also the most time-consuming step in the radar inertial odometer module. Before initializing the radar inertial odometer system, assuming that the bias and noise of the IMU are both 0, the position and attitude information of the two radar key frames are integrated by the IMU raw values ​​as the initial values ​​for frame-to-frame matching. This method can initialize the system for scenes with small initial velocity and angular velocity.

[0062] After the radar inertial odometry module is initialized, the bias, pose, and speed of the IMU can be estimated in the factor graph, and these quantities can be passed to the visual inertial odometry system to assist in its initialization. At this point, the source of the initial value prediction for inter-frame matching becomes the pre-integration of the IMU and the information of the visual inertial odometry system. When the odometer of the visual inertial odometry system can output pose information, its value is used as the initial value; if the visual inertial odometry system reports a failure, the pre-integration value of the IMU is used for initial value prediction. This process can improve the accuracy and robustness of the initial value prediction in both texture-rich and texture-poor environments, while increasing the speed of inter-frame matching.

[0063] Step 402: Initialization.

[0064] The visual inertial odometry system uses the first frame of information received as the world coordinate system and aligns the inertial system with the world coordinate system for initialization.

[0065] The depth information observed by the radar inertial odometry system is used to estimate the system state and the IMU bias, thereby improving the robustness of the visual inertial odometry initialization. First, the radar inertial odometry system is initialized, and then the IMU bias is assumed to be constant between two image key frames, and the laser frame and the visual frame are aligned by timestamp through interpolation. Finally, the result of the radar inertial odometry system initialization is used as the initial value of the visual inertial odometry, which can effectively improve the initialization speed and robustness.

[0066] Step 403: Factor graph maintenance.

[0067] When the pose change exceeds the threshold, a new lidar keyframe will be selected, and the ordinary lidar frames between the keyframes will be discarded. After selecting a new keyframe, the new state will be added to the factor graph as a node. Adding keyframes in this way can not only achieve a balance between memory consumption and map density, but also maintain high efficiency, which helps maintain a relatively sparse factor graph for real-time optimization.

[0068] Step 404: Deep feature association.

[0069] According to the results of visual inertial odometry initialization, the visual odometry can be used to process the image and align the visual frame and radar frame. Since the points scanned by the lidar are relatively sparse, it is necessary to splice and combine multiple frames of laser point clouds to obtain a dense depth image. On this basis, the visual features and radar point clouds are projected onto a unit sphere with the camera as the center to achieve the association between features and depth values. The depth points are then downsampled to reduce the amount of calculation, and polar coordinates are used to represent the point cloud position to ensure the uniformity of the spatial distribution of the points. Finally, the center of the sphere, i.e. the center of the camera, is connected to the visual feature point into a line, and a two-dimensional KD tree is used to search for the three nearest points around the feature point to form a plane. The intersection of the line and the plane is the depth feature value.

[0070] The overlapping of LiDAR point cloud frames from different timestamps will cause depth errors. Due to the stacking of point clouds, some points observed at time ti can still be observed at time tj. If point cloud projections from different distances are used to associate feature depths, it will lead to inaccurate estimation. Therefore, when the maximum distance between the three closest depth points to a feature point is observed to be greater than 2m, data association is no longer performed for that feature point.

[0071] Step 405: LiDAR failure detection.

[0072] Although LiDAR can obtain detailed information of scenes at a longer range, it may fail to scan and match in some flat and open environments lacking characteristic structures, so it needs to be detected. The nonlinear optimization problem in inter-frame matching can be expressed in the form of linear least squares. When the minimum eigenvalue is lower than the threshold of the first optimization iteration, the radar inertial odometer module will report failure, and the LiDAR odometer constraint will no longer be added to the factor graph.

[0073] Step 406: Visual failure detection.

[0074] Drastic changes in motion, changes in lighting, or a lack of texture in the environment can cause the visual inertial odometry system to fail. Drastic motion can cause feature point tracking to fail, while lighting changes and low texture can make it difficult to extract valid feature points, and too few features can lead to optimization failure. In a tightly coupled visual inertial odometry system, the bias optimization of the IMU is highly dependent on feature points, and a failure in the visual inertial odometry system can cause a large bias in the IMU. Therefore, once it is detected that the visual inertial odometry system tracks too few feature points or the bias estimated by the IMU is too large, the visual inertial odometry system can be proactively considered to have failed, reinitialized, and notified to the radar inertial odometry module so that its failure does not affect the radar inertial odometry system.

[0075] Step 407: Loop detection.

[0076] When the sensor module scans the same location at different times, constraints are established to eliminate the error accumulation caused by the odometer. The algorithm uses the bag-of-words model (DBoW2) for closed-loop detection, detects feature descriptors for each new image keyframe, and matches them with the originally extracted descriptors. The timestamps of the closed-loop candidate frames detected by DBoW2 are provided to the radar inertial odometer system for further verification.

[0077] Step 408: After the above processing, the IMU pre-integration constraint, visual odometer constraint, lidar odometer constraint and loop closure constraint are finally jointly optimized to achieve state estimation. IMU, vision, and multi-line laser compensate each other to adapt to various complex environments and achieve compensation of ship motion posture to obtain high-precision three-dimensional point cloud scanning information.

[0078] Step 409: RTK uses the base station as a reference to calibrate the current GNSS positioning error in real time, thereby obtaining high-precision real-time dynamic positioning information based on the satellite navigation system, achieving centimeter-level positioning accuracy. It is also complementary to the SLAM positioning effect to adapt to various complex working environments such as open or blocked, and ensure the accuracy of posture state estimation.

[0079] In the adaptive positioning stage, the GNSS signal is strong on the open water surface, but the lidar and visual feature points are sparse. GNSS is mainly used as a positioning reference, and the extended Kalman filter algorithm combined with the IMU acceleration information is used to fuse the acceleration information with the RTK positioning data to obtain a higher frequency attitude estimation result. There are many reference objects around, and the laser and visual features are rich, but the GNSS signal is average. The SLAM algorithm is mainly used as a positioning reference, and the RTK positioning information is used as an auxiliary correction. In a shielded environment, the laser features are relatively rich, the vision is affected by light changes, and the GNSS signal is severely attenuated. The lidar-IMU SLAM algorithm is mainly used as a positioning reference.

[0080] like Figure 4 The figure shows a flow chart of an adaptive evaluation continuous positioning working method, which reflects the steps of the SLAM working stage, as follows:

[0081] Step 501: In open waters, where the GNSS signal is strong, the base station is used as an RTK reference to calibrate the current GNSS positioning error in real time, thereby obtaining high-precision real-time dynamic positioning information based on the satellite navigation system, achieving centimeter-level positioning accuracy.

[0082] Step 502: The control unit acquires and stores the posture state information and the laser radar point cloud information, and uses the SLAM algorithm integrated with multiple sensors (IMU, vision, multi-line laser and RTK) to establish the scanned environment map, and can monitor and watch it in real time at the remote end. The specific method is: process each frame of laser scanning point cloud to obtain feature points, lines and surfaces, compare the coordinate changes of the same feature points of two adjacent frames of point clouds, and obtain the three-dimensional space relative displacement and relative three-dimensional attitude angle change between two adjacent frames of point clouds according to the laser scanning visual measurement principle; through the coordinate system conversion, the laser point cloud of the next frame can be registered with the laser point cloud of the previous frame, and through continuous registration processing, each frame of laser point cloud can be registered in the reference coordinate system of the starting point of the laser scanning measurement to obtain a three-dimensional topographic map of the environment.

[0083] Step 503: In this process, the pose estimate is corrected in combination with the IMU data. The visual inertial navigation system uses the tracking of visual features associated with the depth features of the radar frame to provide initial values ​​for the inter-frame matching of the lidar by optimizing the visual odometer of the visual reprojection error and the IMU measurement error, and adds the constraints to the factor graph. The robustness of the mapping process is ensured by factor graph maintenance, failure detection, loop detection, etc.

[0084] Step 504: By adaptively evaluating the odometer estimation accuracy of each sensor for various scenarios, the corresponding positioning function is used to maximize the real-time optimal pose estimation, and adaptive switching of GNSS between shielded areas and non-shielded areas is achieved, thereby achieving continuous positioning function in an environment where GNSS signals are missing.

[0085] The above-mentioned embodiment of the present application adopts a structure that integrates multiple sensors (IMU, vision, multi-line laser and RTK) in a multi-sensor positioning device. The sensor scanning device can be disassembled and installed separately. The device structure is simple and can provide a large-range, multi-angle scanning space, solving the problem of easy degradation when directly using a single sensor for positioning. The SLAM algorithm and GNSS fusion technology of multi-sensor fusion are adopted. The mutual compensation of sensors is suitable for various complex environments. The estimated accuracy of each sensor odometer is adaptively evaluated for various scenarios, and the real-time optimal pose estimation is effectively and accurately obtained, solving the problem of difficulty in continuous and precise positioning in an environment where GNSS signals are missing. The continuous positioning system in an environment where GNSS signals are missing based on the above-mentioned multi-sensor fusion provides a set of working methods, adaptively switches to continuous and precise positioning, and fully combines the advantages of multiple sensors to achieve efficient and stable positioning of the system under different environmental conditions.

[0086] In several embodiments provided in the present application, the disclosed devices and methods can also be implemented in other ways. The device and method embodiments described above are merely schematic. For example, the flowcharts and block diagrams in the accompanying drawings show the possible architecture, functions and operations of the devices, methods and computer program products according to multiple embodiments of the present application. In this regard, each box in the flowchart or block diagram can represent a module, a program segment or a part of a code, and a part of a module, a program segment or a code contains one or more executable instructions for implementing a specified logical function. In some alternative implementations, the functions marked in the box can also occur in an order different from that marked in the accompanying drawings. For example, two consecutive boxes can actually be executed substantially in parallel, and they can sometimes be executed in the opposite order, depending on the functions involved. It should also be noted that each box in the block diagram and / or the flowchart, and the combination of boxes in the block diagram and / or the flowchart can be implemented with a dedicated hardware-based system that performs a specified function or action, or can be implemented with a combination of dedicated hardware and computer instructions.

[0087] In addition, the functional modules in the various embodiments of the present application may be integrated together to form an independent part, or each module may exist separately, or two or more modules may be integrated to form an independent part.

[0088] If the function is implemented in the form of a software function module and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present application, or the part that contributes to the prior art or the part of the technical solution, can be embodied in the form of a software product, which is stored in a storage medium and includes several instructions for a computer device (which can be a personal computer, a server, or a network device, etc.) to perform all or part of the steps of the various embodiments of the present application. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), disk or optical disk, and other media that can store program codes.

Claims

1. A multi-sensor fusion positioning method, It is characterized in that The following steps are involved: Determine whether the GNSS signal strength is greater than the signal threshold; If the GNSS signal strength is greater than the signal threshold, the GNSS positioning information and the inertial measurement unit IMU information are obtained, and the extended Kalman filter EKF algorithm is used to fuse the real-time dynamic carrier phase differential RTK positioning information and the IMU acceleration information to determine continuous precise positioning information; The RTK positioning information is determined by the GNSS positioning information, and the IMU acceleration information is determined by the IMU information.

2. The multi-sensor fusion positioning method according to claim 1, It is characterized in that If the GNSS signal strength is less than or equal to the signal threshold, determining whether the visual feature richness is greater than the visual threshold; If the visual feature is richer than the visual threshold, GNSS positioning information, IMU information, radar information and visual information are obtained, and the IMU information, radar information and visual information are integrated using a real-time positioning and map construction SLAM algorithm to determine the perception positioning information; The RTK positioning information is used to assist in correcting the perceived positioning information to determine continuous and precise positioning information.

3. The multi-sensor fusion positioning method according to claim 2, It is characterized in that If the visual feature richness is less than or equal to the visual threshold, the IMU information and the radar information are obtained, and the SLAM algorithm is used to fuse the IMU information and the radar information to determine continuous and precise positioning information.

4. The multi-sensor fusion positioning method according to claim 3, It is characterized in that The SLAM algorithm includes a radar inertial odometer module and a visual inertial odometer module.

5. The multi-sensor fusion positioning method according to claim 2, It is characterized in that The method of using a SLAM algorithm to fuse the IMU information, radar information, and visual information to determine the perception positioning information includes: Determining point cloud information according to the radar information; Determining position information according to the visual information; Based on the point cloud information and the position information, a SLAM algorithm is used to establish a three-dimensional topographic map of the environment and determine initial positioning information; The initial positioning information is corrected according to the IMU information to obtain the perceived positioning information.

6. The multi-sensor fusion positioning method according to claim 5, It is characterized in that The method of using a SLAM algorithm to establish a three-dimensional terrain map of an environment according to the point cloud information and the position information includes: Process each frame of point cloud information to obtain feature points, feature lines and feature surfaces; Compare the coordinate changes of the same feature points in the point cloud information of two adjacent frames, and obtain the three-dimensional spatial relative displacement and relative three-dimensional posture angle change between the point cloud information of two adjacent frames based on the principle of laser scanning vision measurement; Through coordinate system conversion, the point cloud information of the latter frame is registered with the point cloud information of the previous frame. Through continuous registration processing, the point cloud information of each frame can be registered in the reference coordinate system of the starting point of the laser scanning measurement to obtain a three-dimensional topographic map of the environment.

7. The multi-sensor fusion positioning method according to claim 5, It is characterized in that The correcting the initial positioning information according to the IMU information to obtain the perception positioning information includes: According to the IMU information, the initial positioning information is corrected by utilizing factor graph maintenance, deep feature association, radar failure detection, visual failure detection and loop detection to obtain the perceived positioning information.

8. The multi-sensor fusion positioning method according to claim 2, It is characterized in that The GNSS positioning information is obtained through a GNSS antenna; The IMU information is obtained through the IMU; The radar information is obtained by a multi-line mechanical laser radar and a full-field hybrid solid-state laser radar; The visual information is obtained through an industrial camera.

9. The multi-sensor fusion positioning method according to claim 8, It is characterized in that Before obtaining IMU information, radar information, and visual information, it also includes: The IMU, multi-line mechanical laser radar, full-field hybrid solid-state laser radar and industrial camera are calibrated.

10. An electronic device, It is characterized in that The electronic device comprises: processor; a memory for storing processor-executable instructions; Wherein, the processor is configured to execute the multi-sensor fusion positioning method described in any one of claims 1-9.

Citation Information

Cited By

  • Technology for providing spatial positioning for virtual world of meta universe based on RTK and VIO precise positioning

    CN120370361A

  • Single antenna anomaly detection and correction method based on Doppler velocity measurement

    CN122345874A