Mapping method and positioning method based on multi-sensor data fusion

Abstract

The invention relates to a mapping method and a positioning method based on multi-sensor data fusion, and the mapping method comprises the steps: carrying out the space-time alignment of an image sensor, a laser radar and an IMU (Inertial Measurement Unit) through employing the IMU as a space-time reference; respectively acquiring image data, point cloud data and pose data through an image sensor, a laser radar and an IMU (Inertial Measurement Unit); when the pose data meets a key frame acquisition condition, determining that the current frame is a key frame; preprocessing the image data of the key frame, and extracting text data from the image data of the key frame; and binding the image data, the text data and the point cloud data corresponding to each key frame to the pose data corresponding to the current key frame, and constructing a multi-sensor data fusion map comprising an image map, a text map and a point cloud map. According to the method, the multi-sensor data fusion map including the image map, the text map and the point cloud map is constructed, so that the environment can be perceived and described more comprehensively.

Description

Technical Field

[0001] The present invention belongs to the technical field of robot positioning, and specifically relates to a mapping method and a positioning method based on multi-sensor data fusion. Background Art

[0002] In the context of the continuous expansion of current robot indoor application scenarios, due to poor indoor GPS signals, in order to better enable robots to perform tasks and make autonomous decisions, the accuracy and stability of robot indoor positioning are particularly important.

[0003] However, although existing robot positioning methods have made certain progress, traditional single-sensor positioning schemes are often limited by the inherent characteristics of sensors. The scanning range of lidar has blind spots, and the recognition rate for irregular objects is relatively low, resulting in missed detections and misjudgments; in addition, the inertial measurement unit (IMU) is affected by cumulative errors, and the positioning accuracy will gradually decrease during long-term operation. When a robot is in an environment such as a straight corridor lacking feature matching points, existing feature-matching-based algorithms may face problems such as failure or a serious decline in accuracy, making it difficult to meet the high-precision real-time positioning requirements. During the mapping stage, the robot's perception ability of environmental information is not comprehensive, and its data mining ability is not deep enough, resulting in the robot not extracting key information that can truly distinguish different positions, and the information features extracted are not accurately expressed, thus affecting the positioning accuracy.

[0004] Therefore, with the continuous expansion of robot indoor application scenarios, existing single-sensor solutions are no longer able to meet the application requirements. Summary of the Invention

[0005] In order to solve the above problems existing in the prior art, the present invention provides a mapping method and a positioning method based on multi-sensor data fusion. The technical problems to be solved by the present invention are realized through the following technical solutions:

[0006] The first aspect of the present invention provides a mapping method based on multi-sensor data fusion, which is applied to a device configured with an image sensor, an IMU, and a lidar, and includes the following steps:

[0007] S11: Taking the IMU as the spatio-temporal reference, perform spatio-temporal alignment on the image sensor, the lidar, and the IMU;

[0008] S12: Respectively collect image data, point cloud data, and pose data through the image sensor, the lidar, and the IMU;

[0009] S13: When the pose data meets the key frame acquisition condition, determine the current frame as a key frame;

[0010] S14: Preprocess the image data of the key frame and extract text data from the image data of the key frame;

[0011] S15: Bind the image data, text data, and point cloud data corresponding to each key frame to the pose data corresponding to the current key frame, and construct a multi-sensor data fusion map including an image map, a text map, and a point cloud map.

[0012] In an implementable manner, the key frame acquisition conditions in step S13 include:

[0013] The change value of the pose data is equal to a preset value of the travel distance or a preset value of the rotation angle.

[0014] In an implementable manner, step S11 includes:

[0015] S111: Based on the data of the IMU, control the image sensor and the lidar to sample at equal distance intervals;

[0016] S112: Based on the position of the IMU, establish a reference coordinate system, and perform translation and rotation operations on the position coordinates of the image sensor and the lidar according to the relative position relationship between the image sensor, the IMU, and the lidar, and convert the data of the image sensor and the lidar to the reference coordinate system.

[0017] In an implementable manner, after step S15, it further includes:

[0018] S16: Perform loop closure detection on the multi-sensor data fusion map;

[0019] S17: Store the multi-sensor data fusion map in the ROS map format.

[0020] In an implementable manner, the preprocessing in step S14 includes: denoising, binarization, and image enhancement processing.

[0021] The second aspect of the present invention provides a positioning method based on multi-sensor data fusion, including the following steps:

[0022] S21: Respectively collect real-time image data, real-time point cloud data, and real-time pose data through an image sensor, a lidar, and an IMU;

[0023] S22: Select feature image data and feature point cloud data from the real-time image data and the real-time point cloud data through evaluation indicators;

[0024] S23: Detect whether there is text data in the feature image data; if so, perform step S24; if not, perform step S25;

[0025] S24: Extract text data from the feature image data, map the text data to the multi-sensor data fusion map established by the mapping method provided in the first aspect of the present invention to obtain the pose data bound to the current text data; obtain the image data and point cloud data of several frames before and after the current pose data according to the multi-sensor data fusion map, and compare them with the real-time image data and the real-time point cloud data to determine the most matching image data and point cloud data; obtain real-time positioning information based on the most matching image data and point cloud data;

[0026] S25: Perform state prediction using an improved extended Kalman filter algorithm based on the real-time image data, the real-time point cloud data, and the real-time pose data to obtain real-time positioning information.

[0027] In an implementable manner, step S22 includes:

[0028] S221: Perform weighted summation on multiple parameters for image quality assessment to obtain an image comprehensive quality index;

[0029] S222: Select feature image data from the real-time image data with the image comprehensive quality index as the evaluation criterion;

[0030] S223: Select feature point cloud data from the real-time point cloud data with curvature and point cloud density as the evaluation criteria.

[0031] In an implementable manner, the multiple parameters for image quality assessment include: gradient magnitude similarity index and gray variance;

[0032] The weight values for the weighted summation of the gradient magnitude similarity index and the gray variance are adjusted according to the light intensity.

[0033] In an implementable manner, at low light intensity, increase the weight of the gradient magnitude similarity index; at high light intensity, increase the weight of the gray variance.

[0034] In an implementable manner, the improved extended Kalman filter algorithm includes:

[0035] Set a light distribution weight according to the light intensity, and use the light distribution weight as the weight of the final state update formula.

[0036] Compared with the prior art, the beneficial effects of the present invention:

[0037] The mapping method and positioning method based on multi-source heterogeneous sensors provided by the present invention collect data using images, lidar, and IMU, and bind the image data, text data, and point cloud data corresponding to each key frame to the pose data corresponding to the current key frame, constructing a multi-sensor data fusion map including an image map, a text map, and a point cloud map, realizing a more comprehensive perception and description of the environment. During positioning, the text data extracted from the image is used to provide preliminary position information for subsequent positioning, thereby avoiding full database search, accelerating the matching process, and improving the positioning speed. In addition, by constructing an image comprehensive quality index (IQI) and based on the weights of multiple parameters for dynamic image quality assessment under different lighting conditions, the quality assessment of image data is improved, thereby optimizing the data fusion process and ensuring effective image and point cloud matching in different environments. During the sensor data fusion process, a dynamic weight adjustment mechanism based on lighting conditions is introduced into the traditional extended Kalman filter algorithm to enhance the robustness and accuracy under different lighting environments and optimize pose estimation. The mapping method and positioning method provided by the present invention can effectively improve the indoor positioning accuracy, system robustness, and real-time performance of the robot. BRIEF DESCRIPTION OF THE DRAWINGS

[0038] Figure 1 FIG. is a step diagram of a mapping method based on multi-sensor data fusion provided by an embodiment of the present invention;

[0039] Figure 2 FIG. is a flowchart of a mapping method based on multi-sensor data fusion provided by an embodiment of the present invention;

[0040] Figure 3 FIG. is a schematic diagram of time synchronization provided by an embodiment of the present invention;

[0041] Figure 4 FIG. is a schematic diagram of spatial calibration provided by an embodiment of the present invention;

[0042] Figure 5 FIG. is a schematic diagram of the mapping process provided by an embodiment of the present invention;

[0043] Figure 6 FIG. is a flowchart of a positioning method based on multi-sensor data fusion provided by an embodiment of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0044] The following further describes the present invention in detail with reference to specific embodiments, but the embodiments of the present invention are not limited thereto.

[0045] Embodiment 1

[0046] Please refer to Figure 1 and Figure 2 , Figure 1It is a step diagram of a mapping method based on multi-sensor data fusion provided by an embodiment of the present invention. Figure 2 It is a flowchart of a mapping method based on multi-sensor data fusion provided by an embodiment of the present invention.

[0047] A mapping method based on multi-sensor data fusion provided by this embodiment is applied to a device configured with an image sensor, an IMU, and a lidar, and includes the following steps:

[0048] S11: Using the IMU as the spatio-temporal reference, perform spatio-temporal alignment on the image sensor, lidar, and IMU.

[0049] Specifically, to ensure the effective fusion of data collected by different sensors, it is first necessary to perform spatial calibration on multiple sensors to determine the relative positions and orientation relationships between the sensors. Using the pose information of the IMU and the spatial relationships between the sensors, convert and align data from different sources, and perform time synchronization to align the timestamps of different sensor data, ensuring that different types of data can accurately describe the environmental characteristics at the same moment.

[0050] In this embodiment, step S11 includes:

[0051] S111: Using the data of the IMU as the reference, control the image sensor and lidar to sample at equal distance intervals.

[0052] As Figure 3 shown, using the data of the IMU as the reference, sample the environment at equal distance intervals. Exemplarily, whenever the IMU accumulates a travel distance of 20 cm, the lidar collects a frame of point cloud data, and the image sensor (camera) collects a frame of image data.

[0053] S112: Using the position of the IMU as the reference, establish a reference coordinate system. According to the relative position relationships of the image sensor, IMU, and lidar, perform translation and rotation operations on the position coordinates of the image sensor and lidar, and convert the data of the image sensor and lidar to the reference coordinate system.

[0054] As Figure 4 shown, the relative positions of the image sensor, lidar, and IMU installed on the robot are fixed. Through translation and rotation operations on the coordinates of the lidar and the image sensor, convert the data of the image sensor and lidar to the same reference coordinate system as the IMU.

[0055] S12: Respectively collect image data, point cloud data, and pose data through the image sensor, lidar, and IMU.

[0056] S13: When the pose data meets the key frame acquisition conditions, determine the current frame as the key frame.

[0057] In this embodiment, the key frame acquisition conditions in step S12 include:

[0058] The change value of the pose data is equal to the preset value of the travel distance or the preset value of the rotation angle.

[0059] Specifically, through the inertial measurement unit (IMU) and the positioning module on the robot, the pose data (including position and attitude) of the robot is obtained in real time, and a reasonable fixed value is set according to the dynamic range of the robot pose change to determine whether the current frame needs to be selected as a key frame for mapping. As Figure 5 shown, the pose change of the robot is determined according to the pose data. When the translation exceeds a certain distance or the rotation angle exceeds a certain value, the current frame is selected as the key frame to reduce the calculation amount, and at the same time ensure that the sampling of the environment is continuous and uniform when constructing the digital map. Exemplarily, when the cumulative travel distance of the robot reaches 20 cm after the completion of the previous key frame data acquisition, or when the cumulative rotation angle of the robot reaches 20 degrees after the completion of the previous key frame data acquisition, the current frame is selected as the key frame.

[0060] S14: Preprocess the image data of the key frame, and extract the text data from the image data of the key frame.

[0061] Specifically, the preprocessing in step S14 includes: denoising, binarization, and image enhancement processing.

[0062] Furthermore, perform denoising, binarization, and image enhancement processing on the image data of the key frame, extract its feature points and descriptors from the image data of the selected key frame as image feature points for image data registration and map construction. And detect the text area of the image data of the key frame, and extract the text data in the image.

[0063] S15: Bind the image data, text data, and point cloud data corresponding to each key frame to the pose data corresponding to the current key frame, and construct a multi-sensor data fusion map including an image map, a text map, and a point cloud map.

[0064] Specifically, using the feature matching algorithm, based on the feature points and descriptors of adjacent key frames, calculate the matching relationship between adjacent key frames, so as to determine the relative positions of these images in the map, and construct an image map. The image map can be used for visual matching during robot positioning and can also provide assistance for subsequent other maps. Bind the text data of the current key frame with the pose data to generate a text map. The text map can identify the environmental text features of the robot at a specific location, such as road signs and sign information, and can provide rich semantic information for the robot. Filter, denoise, register, and splice the point cloud data to form a local point cloud map, and use the robot pose data and multi-frame point cloud data for optimization to construct a globally consistent point cloud map. Since the point cloud data has accurate distance information, the point cloud map is an accurate three-dimensional model.

[0065] In this embodiment, after step S15, it further includes:

[0066] S16: Perform loop closure detection on the multi-sensor data fusion map.

[0067] Specifically, during the map construction process, the IMU may have cumulative errors. In this embodiment, through loop closure detection, that is, detecting whether the robot returns to a previously visited position, the entire map is globally optimized to reduce the cumulative error. Moreover, during the operation of the robot, the environment may change. Therefore, during the loop closure detection process, the map is dynamically updated to ensure the real-time and accuracy of the map information.

[0068] S17: Store the multi-sensor data fusion map in the ROS map format.

[0069] Specifically, store the multi-sensor data fusion map after loop closure detection in the ROS map format for subsequent calling, and the storage form is shown in Table 1. The multi-sensor data fusion map can test the performance of the map in the target application scenario, such as the navigation path planning and obstacle avoidance capabilities of the robot.

[0070] Table 1

[0071] Serial number IMU position measurement Image data Point cloud data Text data 1 <![CDATA[(x1,y1,z1)]]> <![CDATA[Img1]]> <![CDATA[Point Cloud1]]> <![CDATA[Text1]]> 2 <![CDATA[(x2,y2,z2)]]> <![CDATA[Img2]]> <![CDATA[Point Cloud2]]> <![CDATA[Text2]]> …… …… …… …… …… n <![CDATA[(x n ,y n ,z n )]]> <![CDATA[Img n > <![CDATA[Point Cloud1]]> <![CDATA[Text n >

[0072] Through steps S11 to S17, a comprehensive map including an image map, a text map, and a point cloud map can be constructed, providing the robot with multi-modal environmental perception capabilities.

[0073] The map construction method based on multi-sensor data fusion provided in this embodiment

[0074] Please refer to Figure 6 , Figure 6 is the flowchart of a positioning method based on multi-sensor data fusion provided by an embodiment of the present invention.

[0075] The second aspect of this embodiment provides a positioning method based on multi-source heterogeneous sensors, including the following steps:

[0076] S21: Collect real-time image data, real-time point cloud data, and real-time pose data through an image sensor, a lidar, and an IMU respectively.

[0077] Specifically, during the positioning process, real-time data is collected from different sensors, including real-time image data, real-time point cloud data, and real-time pose data, and then matched with the multi-sensor data fusion map established in the first aspect of this embodiment to complete precise positioning.

[0078] S22: Select feature image data and feature point cloud data from the real-time image data and the real-time point cloud data through evaluation indicators.

[0079] Specifically, step S22 includes:

[0080] S221: Perform a weighted sum of multiple parameters for image quality assessment to obtain an image comprehensive quality index.

[0081] In this embodiment, the multiple parameters for image quality assessment include: gradient magnitude similarity index and gray variance. The weight values of the weighted sum of the gradient magnitude similarity index and the gray variance are adjusted according to the illumination intensity. At low illumination intensity, increase the weight of the gradient magnitude similarity index; at high illumination intensity, increase the weight of the gray variance.

[0082] Specifically, this embodiment defines a new comprehensive evaluation index through the gradient magnitude similarity index (GMSD) and the gray variance, named the image comprehensive quality index (IQI). The IQI can be expressed by the weighted sum of different quality features. The specific formula is as follows:

[0083] IQI = w1 * GMSD + w2 * gray variance (1)

[0084] Among them, w1 and w2 are the weights of GMSD and gray variance respectively, and w1 and w2 are adjusted according to the specific application scenario to reflect the relative importance of each feature in image quality assessment.

[0085] Since the value ranges and scales of each performance index are different, for fusion, it is necessary to perform normalization first to map the value of each index to between [0, 1]:

[0086]

[0087] Among them, is the value after the normalization process of the i-th index, X i is the original value of the i-th index, Xmax , X min are the maximum and minimum values of this index respectively. Through normalization processing, it can ensure that the dimensions of all indexes are consistent, so that they can be reasonably weighted and summed.

[0088] Furthermore, when comprehensively evaluating the image quality under different environments and weather conditions, the weights of each image quality index can be dynamically adjusted according to the readings of the light sensor, which can ensure that the image quality can be reasonably evaluated under different light conditions, thereby improving the reliability and pertinence of the evaluation results.

[0089] Specifically, measure the light intensity of the current environment through the light sensor, divide the readings of the light sensor into two categories, set a threshold L_th for the light sensor. When the light sensor reading L < L_th, it is considered that the current is a low-light environment. In this environment, the image usually has problems of low contrast and few details, and the image is prone to blurring. GMSD can better evaluate the distortion degree of the image. Therefore, GMSD should account for a larger proportion, and at this time, the weight w1 of GMSD should be increased; when the light sensor reading L > L_th, it is considered that the current is a high-light environment. In this case, the details and contrast of the image are usually better, the contrast and detail information of the image are rich, and the gray variance can better capture the details and high-frequency information. Therefore, the weight of the gray variance should be higher, and at this time, the weight w2 of the gray variance should be increased.

[0090] Exemplarily, the image comprehensive quality index (IQI) after dynamically adjusting the weight according to the light intensity can be expressed as:

[0091]

[0092] where the weights w1 and w2 are dynamically adjusted according to the light conditions, is the GMSD after normalization processing, is the gray variance after normalization processing.

[0093] S222: Select the characteristic image data from the real-time image data with the image comprehensive quality index as the evaluation criterion.

[0094] Specifically, through the dynamic adjustment mechanism of weights, it can be ensured that under different lighting conditions, the evaluation index can reasonably reflect the change of image quality. This has very important practical significance for camera image quality evaluation, data fusion, and subsequent image processing. Different image quality indicators can be fused together to obtain an intuitive and unified quality evaluation standard. When image data participates in data fusion, IQI can be used to compare and screen different image data, thereby improving the effect of data fusion and the performance of the final application. The definition of this evaluation index simplifies the complex image quality evaluation process and can meet the requirements of different application scenarios.

[0095] S223: Select characteristic point cloud data from the real-time point cloud data with curvature and point cloud density as the evaluation criteria.

[0096] Specifically, curvature is an important index to measure the local geometric characteristics of a point in the point cloud. When calculating the curvature of a point, it is usually necessary to find multiple points before and after this point (such as 5 points before and after) to calculate together. Points with larger curvature often have more obvious geometric characteristics. Point cloud density reflects the number of point clouds per unit volume. In point cloud data, regions with higher density usually contain more characteristic points because these regions provide more geometric and texture information. Therefore, point cloud density can be used as an indirect index to evaluate the number of characteristic points.

[0097] Furthermore, combining curvature and point cloud density can more accurately select characteristic points. First, an initial set of characteristic points is calculated according to curvature, denoted as set S_curvature. Then, a set of points with higher density is calculated according to point cloud density, denoted as set S_density. The final set of characteristic points can be obtained through set operation S_final = S_curvature ∩ S_density, that is, the points that meet both curvature and density requirements are the final characteristic point cloud data. The number of these characteristic points can be used to evaluate the quality of the point cloud and as a gain basis when fusing with other heterogeneous data sources. In multi-source heterogeneous sensor data fusion, selecting high-quality characteristic points helps to improve the accuracy and stability of the fusion result. Combining curvature and density information can better select characteristic point cloud data with significant geometric characteristics and rich details.

[0098] S23: Detect whether there is text data in the characteristic image data; if so, proceed to step S24; if not, proceed to step S25.

[0099] Specifically, when there is text data in the image data, the text map in the multi-sensor data fusion map established by the mapping method provided in the first aspect of this embodiment can quickly locate the current position, provide preliminary key frames for subsequent image and point cloud matching, avoid traversing in a massive image and point cloud database, and accelerate the speed of autonomous positioning.

[0100] S24: Extract text data from the feature image data, map the text data to the multi-sensor data fusion map established by the mapping method provided in the first aspect of this embodiment to obtain the pose data bound to the current text data; obtain the image data and point cloud data of several frames before and after the current pose data according to the multi-sensor data fusion map, and compare them with the real-time image data and real-time point cloud data to determine the most matching image data and point cloud data; obtain the real-time positioning information according to the most matching image data and point cloud data.

[0101] Specifically, based on the preliminary position information provided by the text map, find the corresponding image data and point cloud data in the image map and the point cloud map respectively, find the data of 50 frames before and after the current frame in the image map and the point cloud map respectively, and compare and match them with the real-time image data and real-time point cloud data respectively to select the best matching frame and obtain the corresponding position information. The IMU calculates the current position of the robot through pre-integration starting from the fused position, obtains the current position information of the robot. At the same time, the robot detects whether there is text information available for positioning on the forward path. If so, the position information provided by the text information is used as the standard. If not, step S25 is performed.

[0102] S25: According to the real-time image data, real-time point cloud data and real-time pose data, use the improved extended Kalman filter algorithm for state prediction to obtain the real-time positioning information.

[0103] In this embodiment, the improved extended Kalman filter algorithm includes:

[0104] Set the light distribution weight according to the light intensity, and use the light distribution weight as the weight of the final state update formula. The remaining steps are the same as those of the traditional extended Kalman filter algorithm.

[0105] Specifically, during the process of actual environmental movement, pose estimation of two sensors (image sensor and lidar) may encounter a situation where the pose estimation of one sensor fails. When the light is dim and unable to meet the acquisition of visual information, pose estimation is completed solely relying on the point cloud data of the radar; when the light is strong and visual feature points are abundant, it is assumed that the data of both sensors conform to the Gaussian distribution. An improved Kalman filter algorithm is used to predict the state of the system. The fusion weight of visual information is updated according to the change of visual feature matching point pairs during the process of light intensity change to complete the update of observation information, realizing the fusion of poses. The equations of the system are non-linear differentiable functions.

[0106] In the improved Kalman filter algorithm of this embodiment, the state prediction equation is:

[0107] x k = f(x k-1 , u k ) + w k , w k ~ N(0, Q k ) (4)

[0108] The observation equation is:

[0109] z k = h(x k ) + v k , v k ~ N(0, R k ) (5)

[0110] Among them, x k is the pose state vector at time k, x k-1 is the pose state vector at time k - 1, u k is the input control quantity at time k, z k is the pixel position on the corresponding image at time k, f and h are the state model and observation model of the system respectively, w k , v k are Gaussian white noises, Q k is the process noise covariance matrix, and R k is the observation noise covariance matrix.

[0111] In this embodiment, the observation model adopts the joint observation model of the radar and the camera. The pose information of the camera is converted to the polar coordinate system of the radar through the transformation matrix of radar-camera joint calibration. The update of observation information is realized through the sensor model, and the pose increment from time k - 1 to time k is used as the input. At this time, k - 1 and k represent adjacent timestamps respectively, and the pose increment from time k - 1 to time k is calculated as:

[0112]

[0113] Among them, is the optimal state estimation vector at time k-1, β is the increment of the pose estimation distance, δ is the increment of the yaw angle of the pose estimation, and θ is the deflection angle value of the radar polar coordinates. The corresponding Jacobian matrix F k is:

[0114]

[0115] The state prediction equation is:

[0116]

[0117] The covariance update equation is:

[0118]

[0119] is the prior covariance at the prediction stage at time k, is the posterior covariance at the update stage at time k-1, is the k transpose matrix of F, is the prior state prediction at time k, is the posterior state estimation at time k-1.

[0120] The traditional extended Kalman filter observation equation updates the current optimal estimate based on the prior value, while the improved EKF algorithm updates the optimal estimate at time k by allocating weights according to the prior estimates at times k and k-1.

[0121] The observation equation for the update process is:

[0122]

[0123] Among them, K k is the standard Kalman gain at time k, P k is the prior error covariance matrix at time k, H k is the mapping observation matrix from the state space to the observation space at time k, is the k transpose matrix of H.

[0124] The state update matrix is:

[0125]

[0126] Among them, E(k) is the state update matrix at times k and k-1, z k is the measured value at time k,

[0127] is the observation residual at time k-1.

[0128] The Kalman gain is updated as:

[0129] K(k) = (λ1K k W k + λ2K k-1 W k-1 )(W k + W k-1 ) -1 (12)

[0130]

[0131] where K(k) is the dynamic fusion Kalman gain at time k, represents the confidence of the observation information at the current time k (based on the noise covariance matrix), represents the observation confidence at time k-1, α is the attenuation coefficient, λ1 is the pose estimation weight of the camera, and λ2 is the pose estimation weight of the lidar. Adjust λ1 and λ2 through IQI to affect the Kalman gain K(k) and ensure that the fusion process is more biased towards reliable sensors.

[0132] The final state update formula is:

[0133]

[0134] The covariance update formula is:

[0135]

[0136] where, is the posterior state estimate at time k, E k is the observation residual at time k, E k-1 is the observation residual at time k-1, is the prior state estimate at time k, K k is the standard Kalman gain at time k, K k-1 is the standard Kalman gain at time k-1. K(k) is the dynamic fusion Kalman gain at time k.

[0137] When calculating the final state, when the IQI is high, the system relies more on the camera, and the update is mainly determined by the gain K k of the camera and the error E k . When the IQI is low, the system relies more on the lidar, and the update is determined by the gain K k-1 of the lidar and the error E k-1 . Therefore, adjust the final state update formula through IQI to ensure that the pose estimation is more biased towards the sensor with high confidence.

[0138] The mapping method and positioning method based on multi-source heterogeneous sensors provided in this embodiment collect data using images, lidar, and IMU, and bind the image data, text data, and point cloud data corresponding to each key frame to the pose data corresponding to the current key frame, constructing a multi-sensor data fusion map including an image map, a text map, and a point cloud map to achieve a more comprehensive perception and description of the environment. During positioning, the text data extracted from the image is used to provide preliminary position information for subsequent positioning, thereby avoiding a full database search, accelerating the matching process, and improving the positioning speed. In addition, by constructing an Image Quality Index (IQI) and based on the weights of multiple parameters for dynamic image quality assessment under different lighting conditions, the quality assessment of image data is improved, thereby optimizing the data fusion process to ensure effective image and point cloud matching in different environments. During the sensor data fusion process, a dynamic weight adjustment mechanism based on lighting conditions is introduced into the traditional Extended Kalman Filter algorithm to enhance the robustness and accuracy in different lighting environments and optimize pose estimation. The mapping method and positioning method provided in this embodiment can effectively improve the indoor positioning accuracy, system robustness, and real-time performance of the robot.

[0139] The above content is a further detailed description of the present invention in combination with specific preferred embodiments. It cannot be determined that the specific implementation of the present invention is only limited to these descriptions. For those of ordinary skill in the technical field to which the present invention belongs, without departing from the concept of the present invention, several simple deductions or substitutions can be made, and all should be regarded as belonging to the protection scope of the present invention.

Claims

1. A mapping method based on multi-sensor data fusion, characterized in that: Applied to a device equipped with an image sensor, an IMU and a laser radar, the method comprises the following steps: S11: Using the IMU as a spatiotemporal reference, performing spatiotemporal alignment on the image sensor, the laser radar, and the IMU; S12: Collect image data, point cloud data and posture data respectively through the image sensor, laser radar and IMU; S13: When the posture data meets the key frame acquisition condition, determine the current frame as a key frame; S14: preprocessing the image data of the key frame, and extracting text data from the image data of the key frame; S15: Bind the image data, text data and point cloud data corresponding to each key frame to the pose data corresponding to the current key frame, and construct a multi-sensor data fusion map including an image map, a text map and a point cloud map.

2. The mapping method based on multi-sensor data fusion according to claim 1, characterized in that: The key frame acquisition conditions in step S13 include: The change value of the posture data is equal to the preset value of the travel distance or the preset value of the rotation angle.

3. The mapping method based on multi-sensor data fusion according to claim 1, characterized in that: Step S11 includes: S111: Based on the data of the IMU, control the image sensor and the laser radar to sample at equal intervals; S112: Establish a reference coordinate system based on the position of the IMU, perform translation and rotation operations on the position coordinates of the image sensor and the laser radar according to the relative position relationship among the image sensor, the IMU and the laser radar, and convert the data of the image sensor and the laser radar to the reference coordinate system.

4. The mapping method based on multi-sensor data fusion according to claim 1, characterized in that: After step S15, the following steps are also included: S16: performing closed-loop detection on the multi-sensor data fusion map; S17: Storing the multi-sensor data fusion map in ROS map format.

5. The mapping method based on multi-sensor data fusion according to claim 1, characterized in that: The preprocessing in step S14 includes: denoising, binarization and image enhancement processing.

6. A positioning method based on multi-sensor data fusion, characterized in that: The following steps are involved: S21: collect real-time image data, real-time point cloud data and real-time pose data through image sensor, laser radar and IMU respectively; S22: selecting feature image data and feature point cloud data from the real-time image data and the real-time point cloud data through evaluation index selection; S23: Detect whether there is text data in the feature image data; if so, proceed to step S24; if not, proceed to step S25; S24: extracting text data from the feature image data, and mapping the text data to a multi-sensor data fusion map established according to the mapping method according to any one of claims 1 to 5, to obtain the posture data bound to the current text data; obtaining image data and point cloud data of several frames before and after the current posture data according to the multi-sensor data fusion map, and comparing them with the real-time image data and the real-time point cloud data to determine the most matching image data and point cloud data; and obtaining real-time positioning information according to the most matching image data and point cloud data; S25: According to the real-time image data, the real-time point cloud data and the real-time posture data, an improved extended Kalman filter algorithm is used to perform state prediction to obtain real-time positioning information.

7. The positioning method based on multi-sensor data fusion according to claim 6, characterized in that: Step S22 includes: S221: performing weighted summation on multiple parameters of image quality assessment to obtain an image comprehensive quality index; S222: Taking the image comprehensive quality index as an evaluation standard, selecting feature image data from the real-time image data; S223: Selecting feature point cloud data from the real-time point cloud data based on curvature and point cloud density as evaluation criteria.

8. The positioning method based on multi-sensor data fusion according to claim 7, characterized in that: The multiple parameters for image quality assessment include: gradient amplitude similarity index and grayscale variance; The weight value of the weighted sum of the gradient amplitude similarity index and the grayscale variance is adjusted according to the light intensity.

9. The positioning method based on multi-sensor data fusion according to claim 8, characterized in that: Under low light intensity, the weight of the gradient amplitude similarity index is increased; under high light intensity, the weight of the grayscale variance is increased.

10. The positioning method based on multi-sensor data fusion according to claim 6, characterized in that: The improved extended Kalman filter algorithm comprises: The illumination distribution weight is set according to the illumination intensity, and the illumination distribution weight is used as the weight of the final state update formula.

Images