Multi-sensor fusion positioning method and related equipment

Abstract

The invention provides a multi-sensor fusion positioning method and related equipment. The method comprises the following steps: acquiring original data of a plurality of sensors; performing preprocessing and synchronous processing on the original data to obtain aligned sensor data; the sensor data comprises point cloud data, image data, accelerometer data, gyroscope data and satellite positioning data; similarity matching is carried out based on the point cloud data and loopback sampling points of a target area, and an initial pose is determined; performing branch and bound search based on the initial pose to determine a global pose; fusing the sensor factors corresponding to the sensor data to obtain a joint factor; and obtaining a positioning result based on the joint factor and the global pose.

Description

Technical Field

[0001] The present disclosure relates to the field of data processing, and in particular to a multi-sensor fusion positioning method and related equipment. Background Art

[0002] The time cost of global pose initialization in existing technologies is closely related to the size of the prior map. As the complexity of the prior map increases, the time required for initialization will also increase accordingly, resulting in a high overall computational cost. This is particularly evident when dealing with large-scale or detailed maps, which may limit the robot's real-time response capabilities. Existing global pose initialization algorithms may easily fall into local optimal solutions in some cases, which will lead to large errors in the final positioning. In large-scale scenes, the prior map may contain a large number of repeated information features, thereby increasing the computational burden of the algorithm. In smaller closed spaces, due to the lack of available constraint information, the positioning results may be unstable, that is, the algorithm lacks sufficient information to determine the exact position of the robot. Developing efficient positioning algorithms that can adapt to scenes of different scales has become an urgent need to achieve robot navigation in ubiquitous scenes. Therefore, existing autonomous positioning technologies have problems such as inaccurate positioning. Summary of the invention

[0003] The present disclosure proposes a multi-sensor fusion positioning method and related equipment to solve the technical problems of inaccurate positioning of existing autonomous positioning technologies to a certain extent.

[0004] In a first aspect, the present disclosure provides a multi-sensor fusion positioning method, comprising:

[0005] Get raw data from multiple sensors;

[0006] Preprocessing and synchronizing the raw data to obtain aligned sensor data; the sensor data includes point cloud data, image data, accelerometer data, gyroscope data and satellite positioning data;

[0007] Performing similarity matching based on the point cloud data and the loop sampling points of the target area to determine the initial pose;

[0008] Performing a branch and bound search based on the initial pose to determine a global pose;

[0009] fusing the sensor factors corresponding to the sensor data to obtain a joint factor;

[0010] A positioning result is obtained based on the joint factor and the global pose.

[0011] In a second aspect of the present disclosure, a multi-sensor fusion positioning device is provided, comprising:

[0012] An acquisition module is used to acquire raw data from multiple sensors;

[0013] A preprocessing module, used for preprocessing and synchronizing the raw data to obtain aligned sensor data; the sensor data includes point cloud data, image data, accelerometer data, gyroscope data and satellite positioning data;

[0014] A matching module, used to perform similarity matching based on the point cloud data and the loop sampling points of the target area to determine the initial pose;

[0015] A search module, configured to perform a branch and bound search based on the initial pose to determine a global pose;

[0016] A fusion module, used for fusing the sensor factors corresponding to the sensor data to obtain a joint factor;

[0017] A result module is used to obtain a positioning result based on the joint factor and the global pose.

[0018] In a third aspect of the present disclosure, an electronic device is provided, comprising one or more processors, a memory; and one or more programs, wherein the one or more programs are stored in the memory and executed by the one or more processors, and the programs include instructions for executing the method described in the first aspect.

[0019] According to a fourth aspect of the present disclosure, a non-volatile computer-readable storage medium containing a computer program is provided. When the computer program is executed by one or more processors, the processors execute the method described in the first aspect.

[0020] In a fifth aspect of the present disclosure, a computer program product is provided, comprising computer program instructions, which, when executed on a computer, enable the computer to execute the method described in the first aspect.

[0021] From the above, it can be seen that the multi-sensor fusion positioning method and related equipment provided by the present invention can overcome the challenges faced by the prior art in different scale scenarios by introducing a pose initialization method combining coarse-grained and fine-grained positioning and tracking algorithm of multi-sensor fusion, thereby achieving more efficient and stable robot positioning and navigation and improving positioning accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0022] In order to more clearly illustrate the technical solutions in the present disclosure or related technologies, the drawings required for use in the embodiments or related technical descriptions are briefly introduced below. Obviously, the drawings described below are only embodiments of the present disclosure. For ordinary technicians in this field, other drawings can be obtained based on these drawings without creative work.

[0023] Figure 1 Schematic diagram of a multi-sensor fusion positioning architecture according to an embodiment of the present disclosure.

[0024] Figure 2 The figure is a schematic diagram of the hardware structure of an exemplary electronic device according to an embodiment of the present disclosure.

[0025] Figure 3 The figure is a flowchart of the multi-sensor fusion positioning method according to an embodiment of the present disclosure.

[0026] Figure 4 It is a schematic diagram of a multi-sensor fusion positioning method according to an embodiment of the present disclosure.

[0027] Figure 5 It is a schematic diagram of a multi-sensor fusion positioning method according to an embodiment of the present disclosure.

[0028] Figure 6-Figure 8 This is a schematic diagram for positioning comparison.

[0029] Fig. 9 Schematic diagram of a multi-sensor fusion positioning device according to an embodiment of the present disclosure. DETAILED DESCRIPTION

[0030] In order to make the objectives, technical solutions and advantages of the present disclosure more clearly understood, the present disclosure is further described in detail below in combination with specific embodiments and with reference to the accompanying drawings.

[0031] It should be noted that, unless otherwise defined, the technical terms or scientific terms used in the embodiments of the present disclosure should be understood by people with ordinary skills in the field to which the present disclosure belongs. The "first", "second" and similar words used in the embodiments of the present disclosure do not indicate any order, quantity or importance, but are only used to distinguish different components. "Including" or "comprising" and similar words mean that the elements or objects appearing before the word cover the elements or objects listed after the word and their equivalents, without excluding other elements or objects. "Connect" or "connected" and similar words are not limited to physical or mechanical connections, but can include electrical connections, whether direct or indirect. "Up", "down", "left", "right" and the like are only used to indicate relative positional relationships. When the absolute position of the described object changes, the relative positional relationship may also change accordingly.

[0032] It is understandable that before using the technical solutions disclosed in the embodiments of the present disclosure, the types, scope of use, usage scenarios, etc. of the personal information involved in the present disclosure should be informed to the user and the user's authorization should be obtained in an appropriate manner in accordance with relevant laws and regulations.

[0033] For example, in response to receiving an active request from a user, a prompt message is sent to the user to clearly prompt the user that the operation requested to be performed will require obtaining and using the user's personal information. Thus, the user can autonomously choose whether to provide personal information to software or hardware such as an electronic device, application, server, or storage medium that performs the operation of the technical solution of the present disclosure according to the prompt message.

[0034] It is understandable that the above notification and the process of obtaining user authorization are merely illustrative and do not constitute a limitation on the implementation of the present disclosure. Other methods that meet the relevant laws and regulations may also be applied to the implementation of the present disclosure.

[0035] Figure 1 FIG. 1 is a schematic diagram showing a multi-sensor fusion positioning architecture according to an embodiment of the present disclosure. Figure 1 , the multi-sensor fusion positioning architecture 100 may include a server 110, a terminal 120, and a network 130 providing a communication link. The server 110 and the terminal 120 may be connected via a wired or wireless network 130. The server 110 may be an independent physical server, or a server cluster or distributed system consisting of multiple physical servers, or a cloud server providing basic cloud computing services such as cloud services, cloud databases, cloud computing, cloud functions, cloud storage, network services, cloud communications, middleware services, security services, and CDN.

[0036] The terminal 120 may be implemented in hardware or software. For example, when the terminal 120 is implemented in hardware, it may be various electronic devices having a display screen and supporting page display, including but not limited to smart phones, tablet computers, e-book readers, laptop portable computers, and desktop computers, etc. When the terminal 120 device is implemented in software, it may be installed in the electronic devices listed above; it may be implemented as multiple software or software modules (such as software or software modules used to provide distributed services), or it may be implemented as a single software or software module, which is not specifically limited here.

[0037] It should be noted that the multi-sensor fusion positioning method provided in the embodiment of the present application can be executed by the terminal 120 or by the server 110. It should be understood that Figure 1 The number of terminals, networks and servers in the embodiment is only for illustration and is not intended to limit the number of terminals, networks and servers. Any number of terminals, networks and servers may be provided as required.

[0038] Figure 2 FIG. 2 shows a schematic diagram of the hardware structure of an exemplary electronic device 200 provided in an embodiment of the present disclosure. Figure 2 As shown, the electronic device 200 may include: a processor 202, a memory 204, a network module 206, a peripheral interface 208 and a bus 210. The processor 202, the memory 204, the network module 206 and the peripheral interface 208 are connected to each other in communication within the electronic device 200 through the bus 210.

[0039] Processor 202 may be a central processing unit (CPU), a neural network processor (NPU), a microcontroller (MCU), a programmable logic device, a digital signal processor (DSP), an application specific integrated circuit (ASIC), or one or more integrated circuits. Processor 202 may be used to perform functions related to the technology described in this disclosure. In some embodiments, processor 202 may also include multiple processors integrated into a single logical component. For example, Figure 2 As shown, processor 202 may include a plurality of processors 202a, 202b, and 202c.

[0040] The memory 204 may be configured to store data (eg, instructions, computer code, etc.). Figure 2 As shown, the data stored in the memory 204 may include program instructions (for example, program instructions for implementing the multi-sensor fusion positioning method of the embodiment of the present disclosure) and data to be processed (for example, the memory may store configuration files of other modules, etc.). The processor 202 may also access the program instructions and data stored in the memory 204, and execute the program instructions to operate on the data to be processed. The memory 204 may include a volatile storage device or a non-volatile storage device. In some embodiments, the memory 204 may include a random access memory (RAM), a read-only memory (ROM), an optical disk, a magnetic disk, a hard disk, a solid-state drive (SSD), a flash memory, a memory stick, etc.

[0041] The network module 206 can be configured to provide communication with other external devices to the electronic device 200 via a network. The network can be any wired or wireless network capable of transmitting and receiving data. For example, the network can be a wired network, a local wireless network (e.g., Bluetooth, WiFi, near field communication (NFC) etc.), a cellular network, the Internet or a combination thereof. It is understood that the type of network is not limited to the above specific examples. In some embodiments, the network module 206 can include any number of network interface controllers (NICs), radio frequency modules, transceivers, modems, routers, gateways, adapters, cellular network chips, etc., in any combination.

[0042] The peripheral interface 208 can be configured to connect the electronic device 200 to one or more peripheral devices to achieve information input and output. For example, the peripheral devices can include input devices such as a keyboard, a mouse, a touch pad, a touch screen, a microphone, and various sensors, and output devices such as a display, a speaker, a vibrator, and an indicator light.

[0043] The bus 210 can be configured to transmit information between various components of the electronic device 200 (e.g., the processor 202, the memory 204, the network module 206, and the peripheral interface 208), such as an internal bus (e.g., a processor-memory bus), an external bus (USB port, PCI-E bus), etc.

[0044] It should be noted that, although the architecture of the electronic device 200 only shows the processor 202, the memory 204, the network module 206, the peripheral interface 208 and the bus 210, in the specific implementation process, the architecture of the electronic device 200 may also include other components necessary for normal execution. In addition, it can be understood by those skilled in the art that the architecture of the electronic device 200 may also only include the components necessary for implementing the embodiments of the present disclosure, and does not necessarily include all the components shown in the figure.

[0045] Autonomous positioning technology occupies an extremely important position in robotics, especially for application scenarios such as inspection robots, food delivery robots and service robots. In order to ensure that these robots can perform tasks efficiently, they need to have the ability to accurately locate themselves in complex environments. Simultaneous Localization and Mapping (SLAM) technology allows robots to simultaneously build environmental maps and determine their own positions in unknown environments, which is critical for exploratory tasks. However, in many actual deployment scenarios, even if the robot's operating area may be known, it is still important to build high-precision prior maps. These high-precision maps not only provide detailed environmental information, but also help robots navigate and locate more accurately, thereby improving the efficiency and reliability of the overall work.

[0046] When it comes to global positioning and navigation, the robot needs to use images or laser point cloud data to estimate its exact position at the beginning of startup. This process is critical to ensure the accuracy of subsequent navigation. Global positioning and navigation not only helps solve the problem of initial position estimation, but also provides other important advantages. First, it enhances the consistency of robot positioning in different environments, and maintains stable positioning performance even when switching between scenes of different scales from indoor to outdoor. Second, global positioning supports navigation in a larger range of scenes, which is particularly important for applications that need to cover a wide area. Through global positioning technology, the robot can achieve continuous navigation across multiple connected spaces without frequent recalibration, thereby improving the flexibility and practicality of the overall system. However, global positioning algorithms based on prior maps often differ in lighting conditions, structural characteristics and scales between indoor and outdoor environments, which increases the complexity and difficulty of implementation of the positioning system.

[0047] Therefore, how to improve the positioning accuracy in autonomous positioning has become a technical problem that needs to be solved urgently.

[0048] In view of this, the embodiments of the present disclosure provide a multi-sensor fusion positioning method and related equipment. By introducing a pose initialization method that combines coarse-grained and fine-grained positioning and a multi-sensor fusion positioning and tracking algorithm, it can overcome the challenges faced by the existing technology in different scale scenarios, thereby achieving more efficient and stable robot positioning and navigation.

[0049] See also Figure 3 , Figure 3 A schematic flow chart of a multi-sensor fusion positioning method according to an embodiment of the present disclosure is shown. The multi-sensor fusion positioning method according to an embodiment of the present disclosure can be deployed in a terminal. Figure 3 In the embodiment, the multi-sensor fusion positioning method 300 may further include the following steps.

[0050] In step S310, raw data of multiple sensors are acquired.

[0051] Among them, multiple sensors can refer to sensors of different modalities, such as lidar, camera, IMU, GPS, etc. Specifically, the raw data of the sensor can be collected in real time through appropriate interfaces (such as I2C, SPI, UART, Ethernet, etc.) and protocols. By fusing multiple sensor data, the accuracy and robustness of positioning can be significantly improved.

[0052] In step S320, the raw data is preprocessed and synchronized to obtain aligned sensor data; the sensor data includes point cloud data, image data, accelerometer data, gyroscope data and satellite positioning data.

[0053] The raw data may be subjected to denoising, filtering, calibration and other processing to improve data quality. A hardware timestamp or software synchronization algorithm may also be used to ensure that data from different sensors are aligned in time. For example, timestamp matching and interpolation processing may be performed. Different sensor data may also be converted into a unified coordinate system for subsequent processing. In some embodiments, the raw data is preprocessed and synchronized to obtain aligned sensor data, including:

[0054] Perform feature extraction and feature matching on the original image data of the image sensor to obtain image feature point data;

[0055] Downsampling the original point cloud data of the radar sensor to obtain initial point cloud data;

[0056] Perform pre-integration processing on the raw data of the accelerometer and the gyroscope to obtain pre-integrated data;

[0057] Converting the original satellite positioning data, the image feature point data and the initial point cloud data into the coordinate system of the attitude sensor to obtain conversion data;

[0058] The conversion data and the pre-integration data of the same time period are stored in the same frame to obtain the sensor data.

[0059] Specifically, the raw data from different sensors are preprocessed to improve the accuracy of subsequent data fusion and pose estimation. Specifically, the system will extract feature points in the visual image, filter and downsample the radar point cloud data, and pre-integrate the IMU (inertial measurement unit) data. These steps are designed to optimize the quality and availability of the data and lay a solid foundation for subsequent analysis.

[0060] After completing the data preprocessing, the system will synchronize the data of different sensors in time and space. Specifically, GNSS (Global Navigation Satellite System) data and radar point cloud data will be converted to the IMU coordinate system to ensure the accuracy of the data in the same coordinate frame. In addition, the IMU data will be segmented according to the timestamp of the radar point cloud data to achieve accurate time alignment, providing a reliable data foundation for subsequent multi-sensor fusion and positioning tracking.

[0061] The data processing thread mainly includes the following steps:

[0062] Step 1: Extract feature points from the visual image and use the optical flow method to perform feature matching to obtain the matching relationship between the current feature points and the historical feature points. At the same time, convert the radar point cloud to the camera coordinate system to provide depth information for the visual feature points;

[0063] Step 2: Count the average distances from all radar points to the radar, and evaluate the scale of the current nearby environment based on this average distance. When the scale of the environment is large, the sampled radar feature points are sparser, and vice versa, when the scale of the environment is small, the sampled radar feature points are denser;

[0064] Step 3: Convert GNSS data, camera feature point data, and radar point cloud data to the IMU coordinate system, and put the data from different sensors in the same period into the same frame;

[0065] Step 4: Pre-integrate the IMU in the frame and store it in each frame for use in state derivation and optimization. Pre-integrate each IMU data between two frames. The pre-integration formula is shown in formula (1):

[0066]

[0067] Where a and w are the acceleration and angular velocity data within a frame, ΔP, ΔQ, and ΔV are the increments of translation, velocity, and rotation, respectively. is the direction at the previous moment, t last ,t next The timestamps of the previous frame and the current frame respectively.

[0068] In step S330, similarity matching is performed based on the point cloud data and the loop sampling points of the target area to determine the initial pose.

[0069] The point cloud data is downsampled, filtered, and processed to reduce the amount of calculation. A set of representative points is selected in the target area as loop sampling points. The current point cloud data is matched with the loop sampling points using the ICP (Iterative Closest Point) algorithm or other similarity measurement methods to determine the initial pose.

[0070] In some embodiments, performing similarity matching based on the point cloud data and the loop sampling points of the target area to determine the initial pose includes:

[0071] Acquire target point cloud data corresponding to the loop sampling point in the point cloud data;

[0072] Extracting edge features and plane features from the target point cloud data to obtain target point cloud features;

[0073] Calculating the similarity between the point cloud features and the loop closure sampling features of the loop closure sampling points;

[0074] The initial pose is determined by using the target point cloud features corresponding to the first n points with the largest similarity.

[0075] Among them, the collected point cloud data is preprocessed, including denoising, filtering, downsampling, etc., to reduce the amount of calculation and improve the matching efficiency. From the preprocessed point cloud data, according to the position or feature of the loop sampling point, the target point cloud data corresponding to the loop sampling point is extracted. The edge features in the target point cloud can be extracted using a point cloud processing algorithm (such as the edge detection algorithm in the PCL library). The edge feature can represent a sudden change or contour part in the point cloud. Similarly, the plane features in the target point cloud can be extracted using a point cloud processing algorithm (such as the RANSAC algorithm). The plane feature can represent a flat area in the point cloud, such as the ground, the wall, etc. The extracted edge features and plane features are described to generate feature descriptors for the subsequent feature matching process. For each loop sampling point, its corresponding features (such as position, direction, size, etc.) can be extracted and a feature descriptor can be generated. The similarity between the target point cloud features and the loop sampling features is calculated using a similarity measurement method between feature descriptors (such as Euclidean distance, cosine similarity, etc.). Sort the calculated similarities and find the top n target point cloud features with the greatest similarity, where n is a positive integer. For each target point cloud feature with the greatest similarity, use a feature matching algorithm (such as the ICP algorithm or its improved version) to estimate its relative pose with the loop sampling point. Combine the pose estimation results of multiple target point cloud features with the greatest similarity, and determine the final initial pose by weighted averaging, voting, or other methods.

[0076] Specifically, loop points can be sampled in the traversable area to extract prior loop information, preparing for coarse-grained loop detection and initial pose estimation. At the same time, the system processes the prior map information, including analyzing the terrain in the map. In this process, the system removes ground points and puts the remaining non-ground point cloud data into the grid map to provide necessary data support for subsequent branch and bound search.

[0077] Next, the system performs similarity matching between the point cloud data acquired at startup and the sampled loop points. By analyzing the similarity between different points, the system can determine the N loop points with the highest matching degree. Subsequently, these N loop points are used for pose estimation to obtain N possible initial poses.

[0078] On this basis, the system performs fine-grained branch and bound searches on each of the N initial poses. After this series of precise search processes, the search result with the highest score is finally selected as the global initial pose of the robot in the map. This ensures the accuracy and robustness of pose estimation, laying a solid foundation for subsequent positioning and tracking.

[0079] (1) Loop detection algorithm

[0080] When the loop detection algorithm starts, it first reads the radar point cloud data corresponding to the sampled loop points and extracts key features, including Link3D edge features and plane features. These features are then stored in the bag-of-words model for efficient matching in subsequent processing.

[0081] When the positioning algorithm is started, the system also processes the point cloud of the initial radar frame, extracts Link3D edge features and plane features, and matches them with the features in the bag-of-words model. By calculating the matching score, the system can evaluate the similarity between the current feature and the stored loop point features.

[0082] Next, the system will select the N matches with the highest matching scores and calculate the relative position of the current radar relative to the loop point based on these matches. Based on the position information of the loop point, the system further calculates multiple candidate global positions and passes these candidate positions to the branch and bound search algorithm. This process not only improves the accuracy of loop detection, but also provides effective support for subsequent pose estimation.

[0083] In step S340, a branch and bound search is performed based on the initial pose to determine the global pose.

[0084] Among them, based on the initial pose, the branch and bound algorithm can be used to find the global optimal pose in the search space. For example, the search space can be divided, the cost of each branch can be evaluated, and pruning can be performed according to the set threshold. The global pose can be fine-tuned and optimized through iterative optimization algorithms (such as nonlinear least squares method, gradient descent method, etc.) to improve positioning accuracy.

[0085] In some embodiments, performing a branch and bound search based on the initial pose to determine the global pose includes:

[0086] Removing ground points from a map of a target area and projecting the point cloud data onto the ground;

[0087] storing the point cloud data projected onto the ground into a grid map based on different resolutions to obtain a first map;

[0088] Searching from the grid map with the maximum resolution in the first map, and calculating the sum of the values ​​of the current point cloud data falling in each grid map, to obtain an initial score for the initial pose;

[0089] The initial pose with the highest initial score is determined as the global pose.

[0090] Among them, ground points are removed from the map of the target area. This is usually to reduce the amount of computation for subsequent processing and to avoid interference of ground points in the matching and search process. The preprocessed point cloud data is projected onto the ground. This can be achieved by calculating the vertical distance from each point in the point cloud data to the ground and projecting it to the corresponding position on the ground. Based on different resolutions, the point cloud data projected onto the ground is stored in a grid map. The choice of resolution should be determined according to the needs of the application scenario and the limitations of computing resources. A higher resolution can provide more detailed information, but it will also increase the amount of computation; a lower resolution can reduce the amount of computation, but some detail information may be lost. Through the above steps, a first map containing grid maps of different resolutions can be generated for subsequent search and matching processes. Start the search from the grid map with the highest resolution in the first map. This is because a high-resolution grid map can provide more accurate information and help to quickly locate possible matching areas. For each initial pose, the sum of the values ​​of the current point cloud data falling in each grid map is calculated. The sum of the values ​​here can represent the degree of matching or similarity between the point cloud data and the corresponding area in the grid map. Based on the sum of the values ​​calculated above, an initial score is assigned to each initial pose. The initial score reflects the closeness between the initial pose and the true pose. From all the initial poses, the one with the highest initial score can be selected as a candidate for the global pose. Specifically, determining the global pose may include: Step 1: The existing map may be segmented into ground points first, the map may be divided into blocks, the z-axis of each point cloud is counted and sorted, and the points with a z-axis less than the minimum value + 10 cm are regarded as ground points;

[0091] Step 2: After removing the ground points, the point cloud is projected onto the ground and stored in a grid map at different resolutions to obtain a grid pyramid;

[0092] Step 3: Use the branch and bound method to search in the local area at the candidate global position. First, search from the grid map with the highest resolution in the grid pyramid, prune the branches with scores below the threshold, and then continue to search the grid map of the next layer of resolution until the layer with the lowest resolution, and find the fine-grained coordinates with the highest score. The score calculation method is the sum of the values ​​of the current radar point falling in each grid.

[0093] Step 4: Count the search results of all candidate global locations and select the one with the highest score as the final global positioning result.

[0094] In step S350, the sensor factors corresponding to the sensor data are fused to obtain a joint factor.

[0095] Among them, the data from different sensors can be converted into factor form to represent the difference between the sensor measurement value and the true value. Multiple sensor factors can be fused using methods such as factor graphs or Bayesian networks to obtain joint factors. This helps to comprehensively consider the information of different sensors and improve the robustness and accuracy of positioning.

[0096] In some embodiments, fusing the sensor factors corresponding to the sensor data to obtain a combined factor includes:

[0097]

[0098] Among them, r piror 、r imu 、r camera 、r lidar、 r gps They are prior factor, attitude factor, image factor, radar factor, satellite positioning factor, and W imu , W camera , W lidar , W gps They are the credibility weights of accelerometer and gyroscope factor, image factor, radar factor, and satellite positioning factor respectively.

[0099] Among them, for each sensor factor, a credibility weight can be assigned to it according to factors such as its measurement accuracy, stability, and current environmental conditions, for example, it can be determined by experimental data, historical experience, or a special calibration process. After obtaining each sensor factor and its corresponding credibility weight, a joint factor can be constructed. Specific fusion methods may include advanced fusion algorithms such as weighted average, Kalman filtering, Bayesian network, or factor graph. In this process, the contribution of each sensor factor will be adjusted according to its credibility weight to ensure that the final joint factor can more accurately reflect the true state of the system. Specifically, according to the initial coordinates obtained by the global pose initialization thread, the system uses it as the initial value of the robot in the map. First, the radar and IMU are jointly initialized to ensure the synergy of the data of the two. Then, the system performs a preliminary pose estimation based on the pre-integration results of the IMU, laying the foundation for subsequent precise positioning. On this basis, the system will fuse the synchronous multi-sensor feature data from the data processing thread. In this stage, by constructing the optimization factor and optimizing the factor graph, the system can make a more accurate estimate of the pose. This series of steps effectively integrates the data advantages of each sensor, thereby significantly improving the accuracy and robustness of the robot's positioning and providing reliable support for subsequent navigation and operation. The positioning and tracking thread includes the following main steps:

[0100] Step 1: Initialize the radar-IMU system jointly to estimate the current speed, IMU zero bias, and gravity direction;

[0101] Step 2: Obtain the visual factor by calculating the reprojection error between the key frames in the sliding window. The error function formula is as follows, which means that for each feature point, the residual between its observed pixel coordinate z and the pixel coordinate h projected onto the camera plane is calculated:

[0102]

[0103] Among them, z ij is the pixel coordinate of the observed feature point, h ij is the pixel coordinate of the projected feature point, m is the number of visual frames in the sliding window, and n is the number of feature points in each visual frame;

[0104] Step 3: Based on the pre-integration information of the IMU data, calculate the relative motion between the two frames and obtain the IMU factor. The state of the next frame is inferred based on the IMU data between the two frames and the state of the previous frame. The state is mainly the position coordinate P, direction Q and speed V. The derivation formula is as follows:

[0105]

[0106] Among them, ΔP, ΔQ, and ΔV are the relative changes of posture and velocity obtained by accumulating IMU data between two frames.

[0107] The IMU factor is calculated and optimized based on the residual formula shown below based on the posture, velocity and pre-integrated data derived by the front end. The factors to be optimized are posture, velocity, position and zero bias.

[0108]

[0109] Among them, ΔP, ΔQ, and ΔV are the relative changes of position, orientation, and velocity obtained by accumulating the accelerometer data and gyroscope data between two frames, respectively. last is the position coordinate of the previous frame, Q last is the orientation of the previous frame, V last is the speed of the previous frame, P next is the position coordinate of the latest frame, Q next is the orientation of the latest frame, V next is the latest frame speed, ba next is the zero bias of the accelerometer in the latest frame, ba last is the zero bias of the accelerometer in the previous frame, g next is the zero bias of the gyroscope in the latest frame, bg last is the zero bias of the gyroscope in the previous frame, r p is the displacement residual, r q is the residual error, r v is the velocity residual, r bais the bias residual of the accelerometer, r bg is the bias residual of the gyroscope, Δt is the time difference between two adjacent frames and g is the vector of gravity in the world coordinate system;

[0110] Step 4: Find the nearest neighbor map point for the key point in the radar point cloud and fit it into a plane. Evaluate the fit of the fitted plane and use it as a weight. The radar factor is the sum of the distances from each key point to the fitted plane:

[0111]

[0112] Where Normal_Vector is the normal vector of the fitting plane, Normal_Offset is the translation of the fitting plane, and Pn is the nth radar point;

[0113] Step 5: GNSS data is converted from longitude and latitude to coordinates of the local coordinate system, and the GNSS factor is the difference between the current position and the GNSS coordinates;

[0114] Step 6: Put all sensor factors into the factor graph for fusion optimization. According to the credibility evaluation of various sensor data, different weights are adaptively assigned to each factor. The factor graph optimization residual function is:

[0115]

[0116] where r piror 、r imu 、r camera 、r lidar 、r gps They are the prior residual, IMU residual, camera residual, radar residual, and GNSS residual. imu , W camera , W lidar , W gps are the credibility weights of different sensors respectively. After optimization, the accurate pose is obtained and output.

[0117] In step S360, a positioning result is obtained based on the joint factor and the global pose.

[0118] Among them, the final positioning result is calculated based on the joint factor and the global pose to improve the positioning performance.

[0119] In some embodiments, obtaining a positioning result based on the joint factor and the global pose includes: superimposing the joint factor and the global pose to obtain the positioning result.

[0120] Specifically, the positioning information (such as position, direction, etc.) in the joint factor can be superimposed or weighted averaged with the global pose to obtain a more accurate positioning result. It should be noted that the superposition method may vary depending on the application scenario and sensor characteristics, and is not limited here.

[0121] The effectiveness of the proposed coarse-grained hierarchical global pose initialization algorithm can be evaluated by comparing the global branch-and-bound search method. The test results are shown in Table 1. In outdoor scenes, the traditional branch-and-bound search algorithm has the disadvantages of long search time and low accuracy when dealing with corridor environments with similar structures. Relatively speaking, the coarse-grained hierarchical global pose initialization algorithm can achieve 100% initialization success rate in a short time. In the experiment of indoor scenes, although the traditional branch-and-bound search algorithm can achieve a high initialization success rate in large-scale outdoor environments, its search time is significantly too long. In contrast, the fine-grained hierarchical global pose initialization algorithm still shows efficient initialization results and stable success rate in outdoor scenes. In summary, through the systematic verification of the algorithm in different scenes, the experiment proves the significant advantages of the coarse-grained hierarchical global pose initialization algorithm in improving the initialization success rate and reducing the search time. This provides an important basis and support for subsequent research and application.

[0122]

[0123]

[0124] Table 1

[0125] Ablation experiments were conducted in indoor scenes. In narrow road scenes, when the sampling hyperparameter is set to 1.5, the number of sampling points is too small, resulting in insufficient constraints, which leads to positioning failure. In contrast, when the sampling hyperparameter is 0.1 or adaptive sampling is used, a more ideal positioning result can be obtained. The specific situation is as follows: Figure 4 As shown in the ablation experiment of outdoor scenes, when the sampling hyperparameter is set to 0.1, the sampling density is too large, which causes a significant increase in optimization overhead, resulting in the system being unable to achieve real-time operation. However, when the sampling hyperparameter is set to 1.5 or adaptive sampling is used, the system can effectively sample appropriate feature points in large-scale scenes while maintaining a low optimization overhead, as shown in Figure 5 In summary, the experimental results show that the adaptive radar sampling method can flexibly adjust the sampling strategy in different scenarios, thereby effectively improving the positioning effect and reducing the system's computational overhead. This provides an important basis for subsequent radar data processing and positioning algorithm optimization.

[0126]

[0127] Table 2

[0128] In indoor scenes, the true value is obtained by measuring the relative distance at fixed points. By comparing the relative pose distances between the fixed-point positions of the positioning trajectory, the final average positioning accuracy is 0.0586 meters, as shown in Table 3. It can be seen that in indoor environments, the positioning algorithm can achieve high-precision pose estimation.

[0129]

[0130]

[0131] Table 3

[0132] In the outdoor scene experiment, the true value uses high-precision RTK (real-time kinematic positioning) data. By calculating the RTE (relative trajectory error) of the positioning trajectory and comparing it with the RTK data, the final relative positioning accuracy is 0.140 meters. Figure 6-Figure 8 As shown in Figure 2, it can be seen that in complex outdoor environments, the positioning algorithm can still maintain a certain degree of accuracy.

[0133] According to the method of the embodiment of the present disclosure, the proposed highly robust multi-sensor fusion positioning method can adaptively adjust the sampling strategy of radar features according to the scale changes of the environment when facing an indoor and outdoor switching environment with large scale changes, thereby effectively maintaining the stability of the algorithm. In addition, in the system startup phase, the global initial pose can be quickly and accurately determined in a large-scale priori map, providing a stable foundation for subsequent positioning and tracking, and ensuring the reliable operation of the algorithm. The complementary characteristics of multi-source heterogeneous sensors are fully utilized, and pose estimation is performed through a fusion optimization method, thereby providing stable positioning and tracking services in indoor and outdoor scenes. This highly robust method shows broad application prospects in the fields of robotic automation and autonomous driving, and can provide strong support for the improvement and development of related technologies.

[0134] It should be noted that the method of the embodiment of the present disclosure can be performed by a single device, such as a computer or a server. The method of the present embodiment can also be applied in a distributed scenario and completed by multiple devices cooperating with each other. In the case of such a distributed scenario, one of the multiple devices can only perform one or more steps in the method of the embodiment of the present disclosure, and the multiple devices will interact with each other to complete the described method.

[0135] It should be noted that the above describes some embodiments of the present disclosure. Other embodiments are within the scope of the appended claims. In some cases, the actions or steps recorded in the claims can be performed in an order different from that in the above embodiments and still achieve the desired results. In addition, the processes depicted in the accompanying drawings do not necessarily require the specific order or continuous order shown to achieve the desired results. In some embodiments, multitasking and parallel processing are also possible or may be advantageous.

[0136] Based on the same technical concept, corresponding to any of the above embodiments and methods, the present disclosure also provides a multi-sensor fusion positioning device, see Fig. 9 , the multi-sensor fusion positioning device, the device comprising:

[0137] An acquisition module is used to acquire raw data from multiple sensors;

[0138] A preprocessing module, used for preprocessing and synchronizing the raw data to obtain aligned sensor data; the sensor data includes point cloud data, image data, accelerometer data, gyroscope data and satellite positioning data;

[0139] A matching module, used to perform similarity matching based on the point cloud data and the loop sampling points of the target area to determine the initial pose;

[0140] A search module, configured to perform a branch and bound search based on the initial pose to determine a global pose;

[0141] A fusion module, used for fusing the sensor factors corresponding to the sensor data to obtain a joint factor;

[0142] A result module is used to obtain a positioning result based on the joint factor and the global pose.

[0143] For the convenience of description, the above device is described by dividing it into various modules according to its functions. Of course, when implementing the present disclosure, the functions of each module can be implemented in the same or multiple software and / or hardware.

[0144] The device of the above embodiment is used to implement the corresponding multi-sensor fusion positioning method in any of the above embodiments, and has the beneficial effects of the corresponding method embodiment, which will not be repeated here.

[0145] Based on the same technical concept, corresponding to any of the above-mentioned embodiment methods, the present disclosure also provides a non-transitory computer-readable storage medium, wherein the non-transitory computer-readable storage medium stores computer instructions, and the computer instructions are used to enable the computer to execute the multi-sensor fusion positioning method as described in any of the above embodiments.

[0146] The computer-readable medium of this embodiment includes permanent and non-permanent, removable and non-removable media, and information storage can be implemented by any method or technology. Information can be computer-readable instructions, data structures, modules of programs, or other data. Examples of computer storage media include, but are not limited to, phase change memory (PRAM), static random access memory (SRAM), dynamic random access memory (DRAM), other types of random access memory (RAM), read-only memory (ROM), electrically erasable programmable read-only memory (EEPROM), flash memory or other memory technology, read-only compact disk read-only memory (CD-ROM), digital versatile disk (DVD) or other optical storage, magnetic cassettes, magnetic tape magnetic disk storage or other magnetic storage devices or any other non-transmission media that can be used to store information that can be accessed by a computing device.

[0147] The computer instructions stored in the storage medium of the above embodiment are used to enable the computer to execute the multi-sensor fusion positioning method described in any of the above embodiments, and have the beneficial effects of the corresponding method embodiments, which will not be repeated here.

[0148] Those skilled in the art should understand that the discussion of any of the above embodiments is merely illustrative and is not intended to imply that the scope of the present disclosure (including the claims) is limited to these examples. Based on the concept of the present disclosure, the technical features in the above embodiments or different embodiments may be combined, the steps may be implemented in any order, and there are many other variations of the different aspects of the embodiments of the present disclosure as described above, which are not provided in detail for the sake of simplicity.

[0149] In addition, to simplify the description and discussion, and in order not to make the embodiments of the present disclosure difficult to understand, the known power / ground connections to the integrated circuit (IC) chips and other components may or may not be shown in the provided figures. In addition, the device can be shown in the form of a block diagram to avoid making the embodiments of the present disclosure difficult to understand, and this also takes into account the fact that the details of the implementation of these block diagram devices are highly dependent on the platform on which the embodiments of the present disclosure will be implemented (that is, these details should be fully within the scope of understanding of those skilled in the art). Where specific details (e.g., circuits) are set forth to describe exemplary embodiments of the present disclosure, it is apparent to those skilled in the art that the embodiments of the present disclosure can be implemented without these specific details or with changes in these specific details. Therefore, these descriptions should be considered illustrative rather than restrictive.

[0150] Although the present disclosure has been described in conjunction with specific embodiments of the present disclosure, many replacements, modifications and variations of these embodiments will be apparent to those skilled in the art from the foregoing description. For example, other memory architectures (e.g., dynamic RAM (DRAM)) may use the embodiments discussed.

[0151] The embodiments of the present disclosure are intended to cover all such substitutions, modifications and variations that fall within the broad scope of the appended claims. Therefore, any omissions, modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the embodiments of the present disclosure should be included in the scope of protection of the present disclosure.

Claims

1. A multi-sensor fusion positioning method, comprising: Get raw data from multiple sensors; Preprocessing and synchronizing the raw data to obtain aligned sensor data; The sensor data includes point cloud data, image data, accelerometer data, gyroscope data and satellite positioning data; Performing similarity matching based on the point cloud data and the loop sampling points of the target area to determine the initial pose; Performing a branch and bound search based on the initial pose to determine a global pose; fusing the sensor factors corresponding to the sensor data to obtain a joint factor; A positioning result is obtained based on the joint factor and the global pose.

2. The method according to claim 1, wherein: Based on the point cloud data and the loop sampling points of the target area, similarity matching is performed to determine the initial pose, including: Acquire target point cloud data corresponding to the loop sampling point in the point cloud data; Extracting edge features and plane features from the target point cloud data to obtain target point cloud features; Calculating the similarity between the point cloud features and the loop closure sampling features of the loop closure sampling points; The initial pose is determined by using the target point cloud features corresponding to the first n points with the largest similarity.

3. The method according to claim 2, wherein: Performing a branch and bound search based on the initial pose to determine a global pose includes: removing ground points in a map of a target area and projecting the point cloud data onto the ground; storing the point cloud data projected onto the ground into a grid map based on different resolutions to obtain a first map; Searching from the grid map with the maximum resolution in the first map, and calculating the sum of the values ​​of the current point cloud data falling in each grid map, to obtain an initial score for the initial pose; The initial pose with the highest initial score is determined as the global pose.

4. The method according to claim 1, wherein: The step of fusing the sensor factors corresponding to the sensor data to obtain a combined factor includes: Among them, r piror 、r imu 、r camera 、r lidar 、r gps They are prior factor, attitude factor, image factor, radar factor, satellite positioning factor, and W imu , W camera , W lidar , W gps They are the credibility weights of accelerometer and gyroscope factor, image factor, radar factor, and satellite positioning factor respectively.

5. The method according to claim 4, wherein: The attitude factor is: in, ΔP, ΔQ, and ΔV are the relative changes of position, orientation, and velocity obtained by accumulating the accelerometer data and gyroscope data between two frames. last is the position coordinate of the previous frame, Q last is the orientation of the previous frame, V last is the speed of the previous frame, P next is the position coordinate of the latest frame, Q next is the orientation of the latest frame, V next is the latest frame speed, ba next is the zero bias of the accelerometer in the latest frame, ba last is the zero bias of the accelerometer in the previous frame, bg next is the zero bias of the gyroscope in the latest frame, bg last is the zero bias of the gyroscope in the previous frame, r p is the displacement residual, r q is the residual error, r v is the velocity residual, r ba is the bias residual of the accelerometer, r bg is the bias residual of the gyroscope, Δt is the time difference between two adjacent frames and g is the vector of gravity in the world coordinate system; The image factor is: Among them, z ij is the pixel coordinate of the observed feature point, h ij is the pixel coordinate of the projected feature point, m is the number of visual frames in the sliding window, and n is the number of feature points in each visual frame; The radar factor is: Wherein, Normal_Vector is the normal vector of the fitting plane of the point cloud data, Normal_Offset is the translation amount of the fitting plane of the point cloud data, and Pn is the nth radar point; The satellite positioning factor is the difference between the current position and the satellite positioning coordinates.

6. The method according to claim 1, wherein: Preprocessing and synchronizing the raw data to obtain aligned sensor data includes: Perform feature extraction and feature matching on the original image data of the image sensor to obtain image feature point data; Downsampling the original point cloud data of the radar sensor to obtain initial point cloud data; Perform pre-integration processing on the raw data of the accelerometer and the gyroscope to obtain pre-integrated data; Converting the original satellite positioning data, the image feature point data and the initial point cloud data into the coordinate system of the attitude sensor to obtain conversion data; The conversion data and the pre-integration data of the same time period are stored in the same frame to obtain the sensor data.

7. The method according to claim 1, wherein: A positioning result is obtained based on the joint factor and the global pose, including: The joint factor and the global pose are superimposed to obtain the positioning result.

8. A multi-sensor fusion positioning device, comprising: An acquisition module, used to acquire raw data from multiple sensors; A preprocessing module, used for preprocessing and synchronizing the raw data to obtain aligned sensor data; The sensor data includes point cloud data, image data, accelerometer data, gyroscope data and satellite positioning data; A matching module, used to perform similarity matching based on the point cloud data and the loop sampling points of the target area to determine the initial pose; A search module, configured to perform a branch and bound search based on the initial pose to determine a global pose; A fusion module, used for fusing the sensor factors corresponding to the sensor data to obtain a joint factor; A result module is used to obtain a positioning result based on the joint factor and the global pose.

9. An electronic device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the method according to any one of claims 1 to 7 when executing the program. 10 . A non-transitory computer-readable storage medium storing computer instructions, wherein the computer instructions are used to cause a computer to execute the method according to claim 1 .

Images