An Indoor and Outdoor Localization and Mapping Method Based on Multi-Sensor Fusion

Abstract

The present application discloses a method for indoor and outdoor positioning and mapping with multi-sensor fusion, which relates to the technical fields of map construction and positioning, and aims to solve the problem of low indoor and outdoor positioning accuracy. It includes: processing each frame of point cloud data and IMU data obtained outdoors according to a preset processing flow to obtain the laser odometer of each frame; aligning the laser odometer of each frame with GPS data according to the time stamp and calculating the position conversion relationship; calculating the point cloud data and IMU data of the current frame according to the preset processing flow; obtaining absolute positioning data for the laser odometer of the current frame by using the coordinate conversion relationship; if outdoors, adding the laser odometer and GPS data to a filter for correction to obtain absolute positioning data. Through the present application, the positioning accuracy is improved, and accurate indoor absolute positioning can be achieved without relying on GPS data.

Description

Technical Field

[0001] This application relates to the technical field of map construction and positioning, and in particular to a multi-sensor fusion indoor and outdoor positioning and mapping method. Background Art

[0002] The main technologies for indoor and outdoor positioning and mapping are SLAM technology combined with GPS global positioning to achieve precise indoor and outdoor positioning and map construction.

[0003] Generally, indoor positioning and mapping mainly rely on visual sensors and laser sensors. Common technical solutions include scanning the environmental map with a multi-line lidar. Due to motion, there are motion distortions in the laser point cloud in the same frame of laser data. By combining IMU data to give the motion change amount in each frame, the laser points in the current frame are aligned to the start time or end time of the laser scan to eliminate the distortion. In order to enable each frame of point cloud map to be stitched into a complete point cloud map, it is necessary to calculate the position relationship between the current frame of point cloud and the next frame of point cloud. Usually, a three-dimensional point cloud matching algorithm such as the ICP algorithm is used. This algorithm usually uses all the laser point cloud data in the current frame to calculate the position relationship between each point in the current frame and the corresponding point in the adjacent frame. When the sum of the distances of all points is the smallest, the corresponding pose relationship is the pose relationship between the two frames. However, since the point cloud matching is based on limited historical data as the matching benchmark, the matching error will directly affect the matching accuracy of the next frame, so-called cumulative error.

[0004] Therefore, the cumulative error is also corrected by loop detection of the point cloud. The purpose of laser point cloud loop detection is to determine whether the robot or device has returned to an area that has been explored before, so as to correct the cumulative error. Judging whether it has returned to the previous area requires combining point cloud feature matching, spatial geometric relationships, and pose estimation. Match the current frame of point cloud features with the previous point cloud library, and use a KD tree or other efficient indexing structure to find the historical frame most similar to the current frame. Since the pose amount between adjacent frames of point cloud represents the position relationship of adjacent frames of point cloud and can also represent the position relationship of the sensor, the pose amounts of consecutive frames are used as the odometer output. However, the method based on feature points is used for inter-frame association, but it is limited to traditional matching and optimization methods. When the feature information amount decreases, the accuracy brought by the algorithm will also decrease synchronously. Moreover, in this way, due to the limitations of the characteristics of GPS, it cannot give the absolute positioning indoors and cannot meet the requirements of indoor and outdoor synchronous positioning. Otherwise, a large amount of preliminary work needs to be done, such as building a high-precision map, storing absolute positioning information and associating it with the point cloud indoors, but the cost is too high. Summary of the Invention

[0005] Based on this, in view of the above technical problems, a multi-sensor fusion indoor and outdoor positioning and mapping method is provided to solve the problem of low indoor and outdoor positioning accuracy in the prior art.

[0006] In the first aspect, a multi-sensor fusion indoor and outdoor positioning and mapping method, the method includes:

[0007] Obtain multiple frames of historical point cloud data, IMU data, and GPS data collected outdoors in the mapping and positioning device; for each frame of historical data, process it according to a preset processing flow to obtain the laser odometer for each frame; align the laser odometer and GPS data for each frame according to the timestamp, and calculate the pose transformation relationship between the aligned laser odometer and GPS data to complete the initialization; the preset processing flow includes calculating the state quantity compared to the initial moment within the corresponding frame time according to the IMU data, compensating the motion deviation of the point cloud data of the corresponding frame according to the state quantity to obtain the corrected point cloud data, matching the corrected point cloud data with the previously constructed map to calculate the residual, and iteratively optimizing the state quantity according to the residual to obtain the corrected state quantity, denoted as the laser odometer;

[0008] Real-time receive the point cloud data and IMU data collected in the current frame; calculate the current frame laser odometer and the corrected point cloud data for the IMU data and point cloud data in the current frame according to the preset processing flow;

[0009] If GPS data is not included in the current frame data and cannot be received in real time indoors, use the pose transformation relationship to convert according to the current frame laser odometer to obtain the absolute positioning data; if GPS data is included in the current frame data and can be received in real time outdoors, align the corrected state quantity of the current frame laser odometer and the GPS data according to the timestamp, convert the aligned current frame laser odometer using the pose transformation relationship, and use the converted laser odometer and the current frame GPS data as observation values to join the EKF filter to correct and obtain the absolute positioning data;

[0010] Update the map according to the corrected point cloud data.

[0011] In the above solution, optionally, the calculation of the state quantity compared to the initial moment within the corresponding frame time according to the IMU data is performed through the following formula;

[0012]

[0013] where, represents the rotation matrix at time t, is the angular velocity collected by the IMU, is the bias of the gyroscope, is the noise of the gyroscope, is the time step, represents the velocity at time t, is the measured acceleration, is the accelerometer bias, is the noise of the accelerometer, g is the acceleration due to gravity, represents the position vector at time t, represents the acceleration vector at time t, is the random walk noise of the gyroscope bias, represents the random walk noise of the accelerometer bias.

[0014] In the above solution, optionally, the matching of the corrected point cloud data with the previously constructed map to calculate the residual includes:

[0015] Converting the position of the corrected point cloud from the radar coordinate system to the IMU coordinate system, and then from the IMU coordinate system to the world coordinate system;

[0016] Finding the 5 points closest to any point in the corrected point cloud in the previously constructed map, and calculating the distance between this point and the fitting plane constructed by the corresponding 5 points to obtain the residual.

[0017] In the above solution, optionally, the equation of the fitting plane is calculated by the following formula:

[0018]

[0019] where a, b, c are the coefficients determining the plane, and d is the scaling coefficient, represents the three-dimensional coordinates of point i, i = 1, 2, 3, 4, 5; are the plane parameters.

[0020] In the above solution, optionally, the iterative optimization of the state quantity according to the residual to obtain the corrected state quantity of the laser odometer includes:

[0021] Step a: Judging whether the optimal state is reached by iteratively optimizing the state quantity according to the residual calculated from the position of the corrected point cloud, using the following formula:

[0022]

[0023]

[0024] where, is the state quantity, is the state value corresponding to the k-th observation data, is the sliding window length parameter, represents the estimated value of the state quantity, The Jacobian matrix of the measurement model at time k, The estimation error of the system state at time k, The noise, The j-th measurement value at time k, The error between the estimated value and the true state, The error between the measurement value and the predicted measurement value, The state covariance matrix, The matrix representing the observation noise covariance, The observation model function, which describes the mapping from the state to the observation, The estimated state value; The error between the true value of the current system state and the current system predicted estimate value;

[0025] Step b: If it is not optimal, re-correct the point cloud position, calculate the residuals, and return to step a until it reaches the optimal.

[0026] In the above solution, optionally, after receiving the GPS data of the mapping and positioning device, it further includes:

[0027] Accumulate the GPS data for a specific time interval, perform smoothing filtering on the data, remove the abnormal points with large fluctuations, and obtain the filtered GPS data.

[0028] In the above solution, optionally, the data alignment of the corrected state quantity of each frame of laser odometer and the GPS data according to the time stamp includes:

[0029] Obtain the time stamp corresponding to the corrected state quantity of the laser odometer, find the two points before and after the time closest to this time stamp in the GPS data, perform linear interpolation on the positioning points corresponding to the two points before and after the time closest to this time stamp in the GPS data, and obtain the GPS data at the same time stamp as the corrected state quantity of the laser odometer after obtaining the difference.

[0030] In a second aspect, a multi-sensor fusion indoor and outdoor positioning and mapping system, the system includes:

[0031] Initialization module: It is used to obtain multiple frames of historical point cloud data, IMU data, and GPS data collected outdoors in the mapping and positioning device; for each frame of historical data, it is processed according to a preset processing flow to obtain the laser odometer for each frame; the laser odometer and GPS data for each frame are aligned according to the time stamp, and the pose transformation relationship between the laser odometer and GPS data after data alignment is calculated to complete the initialization; the preset processing flow includes calculating the state quantity compared to the initial moment within the corresponding frame time according to the IMU data, compensating for the motion deviation of the point cloud data of the corresponding frame according to the state quantity to obtain the corrected point cloud data, matching the corrected point cloud data with the previously constructed map to calculate the residual, and iteratively optimizing the state quantity according to the residual to obtain the corrected state quantity, denoted as the laser odometer;

[0032] Current frame state quantity and point cloud data processing module: It is used to receive the point cloud data and IMU data collected in the current frame in real time; calculate the current frame laser odometer and the corrected point cloud data for the IMU data and point cloud data of the current frame according to the preset processing flow;

[0033] Positioning module: If GPS data is not included in the current frame data and cannot be received in real time indoors, it is used to obtain absolute positioning data by converting according to the pose transformation relationship using the current frame laser odometer; if GPS data is included in the current frame data and can be received in real time outdoors, the corrected state quantity of the current frame laser odometer and the GPS data are aligned according to the time stamp, the current frame laser odometer after data alignment is converted using the pose transformation relationship, and the converted laser odometer and the current frame GPS data are used as observation values to be added to the EKF filter to correct and obtain absolute positioning data;

[0034] Map update module: It is used to update the map according to the corrected point cloud data.

[0035] In the third aspect, a computer device includes a memory and a processor. The memory stores a computer program, and when the processor executes the computer program, it implements the steps of the method for indoor and outdoor positioning and mapping with multi-sensor fusion described in the first aspect above.

[0036] In the fourth aspect, a computer-readable storage medium stores a computer program thereon, characterized in that when the computer program is executed by a processor, it implements the steps of the method for indoor and outdoor positioning and mapping with multi-sensor fusion described in the first aspect above.

[0037] This application has at least the following beneficial effects:

[0038] In this application, the power data is compensated for motion deviation based on the state quantity calculated from the IMU data of historical frames, and the state quantity is corrected based on the corrected point cloud data. The corrected state quantity and GPS data are aligned according to the time stamp, and the coordinate transformation formula for converting the corrected state quantity of each frame of the laser odometer to the world coordinate system is obtained based on the aligned corrected state quantity of the laser odometer and GPS data. Thus, for indoor positioning, only the corrected state quantity obtained by calculation needs to be converted using the coordinate transformation relationship to obtain the final positioning data. For outdoor positioning, the corrected state quantity of the current frame of the laser odometer and GPS data are aligned according to the time stamp, and the aligned corrected state quantity of the current frame of the laser odometer and GPS data are used as observation values to be added to the EKF filter to correct and obtain the absolute positioning data. Therefore, the method of this application does not rely on the data of historical feature points, thus improving the positioning accuracy. Moreover, accurate indoor absolute positioning can be achieved without relying on GPS data. BRIEF DESCRIPTION OF THE DRAWINGS

[0039] Figure 1 FIG. is a schematic flow chart of a multi-sensor fusion indoor and outdoor positioning and mapping method provided by an embodiment of this application;

[0040] Figure 2 FIG. is a specific flow chart of a multi-sensor fusion indoor and outdoor positioning and mapping method provided by an embodiment of this application;

[0041] Figure 3 FIG. is a specific flow chart of adding the laser odometer and GPS positioning data as observation values to the EKF filter to correct the cumulative deviation of the IMU recursion in an embodiment of this application;

[0042] Figure 4 FIG. is a structural diagram of a mapping and positioning device provided by an embodiment of this application. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0043] In order to make the objectives, technical solutions and advantages of this application clearer, the following further describes this application in detail with reference to the drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain this application and are not used to limit this application.

[0044] In one embodiment, as Figure 1 and Figure 2 shown, a multi-sensor fusion indoor and outdoor positioning and mapping method is provided, and the method includes:

[0045] Step S1: Obtain multiple frames of historical point cloud data, IMU data, and GPS data collected outdoors by the mapping and positioning device; for each frame of historical data, process it according to a preset processing flow to obtain the laser odometer for each frame; align the laser odometer and GPS data for each frame according to the time stamp, and calculate the pose transformation relationship between the laser odometer and GPS data after data alignment, so as to complete the initialization; the preset processing flow includes calculating the state quantity compared with the initial moment within the corresponding frame time according to the IMU data, compensating the motion deviation of the point cloud data of the corresponding frame according to the state quantity to obtain the corrected point cloud data, matching the corrected point cloud data with the previously constructed map to calculate the residual, and iteratively optimizing the state quantity according to the residual to obtain the corrected state quantity, denoted as the laser odometer.

[0046] In step S1, it specifically includes:

[0047] (1) The IMU data is obtained by the IMU module. First, initialize the gravity direction and gyroscope zero bias, and then update the state quantities of subsequent different moments relative to the initial moment according to the input observed linear acceleration and angular velocity, state quantities including position, rotation, velocity, bias, etc. The update of the state quantity is based on Newton's kinematic laws, as shown in formula (1):

[0048] (1)

[0049] Among them, the left side of the equation is the state to be updated at the next moment, and the right side of the equation is the state at the previous moment. represents the rotation matrix at time t, is the angular velocity collected by the IMU, is the bias of the gyroscope, is the noise of the gyroscope, is the time step, represents the velocity at time t, is the measured acceleration, is the accelerometer bias, is the noise of the accelerometer, g is the gravitational acceleration, represents the position vector at time t, represents the acceleration vector at time t, is the random walk noise of the gyroscope bias, represents the random walk noise of the accelerometer bias.

[0050] (2) Compensate the motion deviation of the point cloud data of each point in the current frame according to the position and attitude state quantities calculated by the IMU forward propagation.

[0051] (3) Convert the current frame position from the radar coordinate system to the IMU coordinate system, and then find the 5 points closest to the current point to fit a plane after converting to the world coordinate system according to the pose estimated by forward propagation. The plane equation is shown in formula (2):

[0052] (2)

[0053] where a, b, and c are the coefficients determining the plane, and d is the scaling coefficient. represents the three-dimensional coordinates of point i, where i = 1, 2, 3, 4, 5; are the plane parameters.

[0054] Calculate the residual according to the distance between the current point and the fitted plane.

[0055] (4) State estimation: In order to achieve higher-precision pose estimation in a nonlinear system, the state update is optimized by iterating multiple times until the updated state converges to a more accurate state. Calculate the system residual and construct the optimal equations of the system state. Among them, the optimal equations are shown in formulas (3) and (4):

[0056] (3)

[0057] (4)

[0058] where is the state quantity, is the state value corresponding to the kth observation data, is the sliding window length parameter, represents the estimated value of the state quantity, is the Jacobian matrix of the measurement model at time k, is the estimation error of the system state at time k, is the noise, is the jth measurement value at time k, is the error between the estimated value and the true state, is the error between the measurement value and the predicted measurement value, is the state covariance matrix, is the covariance matrix representing the observation noise, is the observation model function, which describes the mapping from the state to the observation, is the estimated state value; is the error between the true value of the current system state and the current system predicted estimate value;

[0059] (5) Since the frequencies of the GPS positioning data and the lidar odometry are inconsistent, a large amount of data is lost during the data fusion process, reducing the positioning accuracy. Therefore, based on the timestamp corresponding to each frame of data published by the lidar odometry, the point in the GPS with the closest time is found, and linear interpolation is performed using the positioning points corresponding to the adjacent timestamps before and after this nearest neighbor. The interpolated points are used as the corresponding matching point pairs in the GPS positioning points, maximizing the association of the GPS positioning points with the positioning points of the lidar odometry at the closest time, and reducing the fusion error. At the same time, according to the corrected state quantity of each frame of lidar odometry after data alignment and the GPS data, obtain the coordinate transformation formula for converting the corrected state quantity of the lidar odometry to the world coordinate system.

[0060] At the same time, for the received RTK (GPS) data, parse the original data, accumulate the longitude and latitude data at specific time intervals, perform smoothing filtering on the data, and remove the abnormal points with large fluctuations to obtain the filtered RTK data.

[0061] Step S2: Receive the point cloud data and IMU data collected in the current frame in real time; calculate the current frame lidar odometry and the corrected point cloud data according to the preset processing flow for the IMU data and point cloud data of the current frame.

[0062] Step S3: If GPS data is not included in the current frame data received in real time indoors, use the pose transformation relationship to convert the current frame lidar odometry to obtain absolute positioning data; if GPS data is included in the current frame data received in real time outdoors, align the corrected state quantity of the current frame lidar odometry and the GPS data according to the timestamp, convert the current frame lidar odometry after data alignment using the pose transformation relationship, and use the converted lidar odometry and the current frame GPS data as observation values to join the EKF filter to correct and obtain the absolute positioning data.

[0063] The IMU raw data performs position recursion based on the basic principle of Newtonian kinematics. As the IMU recursion progresses over time, the cumulative error will gradually increase. Introduce the lidar odometry and GPS positioning data as observation values to join the EKF filter to correct the cumulative deviation of the IMU recursion. The state estimation system is as Figure 3 shown. Among them, the state quantities to be estimated by the system are x, y, z, roll, pitch, and yaw respectively. The observation matrix H and the state transition matrix of the system are both identity matrices, and the measurement noise and the observation noise are given corresponding weights according to the sensor measurement accuracy. The input quantities of the system are GPS positioning and lidar odometry respectively, and then after data alignment, state prediction, and positioning data update, finally output the fused positioning result. The prediction and update equations are shown in the following formulas (5)(6)(7)(8):

[0064] (5)

[0065] (6)

[0066] (7)

[0067] (8)

[0068] (9)

[0069] Wherein, is the predicted value of the system state at time k, based on the information at time , is the estimated value of the system state at time k, is the measurement residual at time k, is the Kalman gain, is the process noise covariance matrix at time k, is the system state transition matrix at time k, is the observation matrix of the measurement model at time k, is the observation noise covariance matrix.

[0070] Step S4: Update the map according to the corrected point cloud data.

[0071] In step S4, the radar is used as the center of the world coordinate system, and the boundary range is calculated according to the diagonal coordinates of the map. When the distance between the radar center and each surface of the map is less than the set threshold, it means that the radar is close to the map boundary, and at this time, the map needs to be updated. Then, the map is updated according to the final positioning data of the current frame and the corrected point cloud data.

[0072] The above indoor and outdoor positioning and mapping method based on multi-sensor fusion compensates the motion deviation of the power data according to the state quantity calculated from the IMU data of historical frames, corrects the state quantity according to the corrected point cloud data, aligns the corrected state quantity and GPS data according to the time stamp, and obtains the coordinate transformation formula for converting the corrected state quantity of each frame of laser odometry to the world coordinate system based on the corrected state quantity and GPS data after data alignment; thus, when positioning indoors currently, only the corrected state quantity obtained by calculation needs to be converted using the coordinate transformation formula to obtain the final positioning data. For outdoor positioning, the corrected state quantity of the current frame of laser odometry and GPS data are aligned according to the time stamp, and the corrected state quantity of the current frame of laser odometry and GPS data after data alignment are used as observation values to be added to the EKF filter to correct and obtain the final positioning data. Thus, the method of this application does not rely on the data of historical feature points, so the positioning accuracy is improved. Moreover, accurate indoor positioning can be achieved without relying on GPS data.

[0073] In one embodiment, an indoor and outdoor positioning and mapping system based on multi-sensor fusion is provided. The system includes:

[0074] An initialization module: used to obtain multiple frames of historical point cloud data, IMU data, and GPS data collected outdoors in the mapping and positioning device; for each frame of historical data, it is processed according to a preset processing flow to obtain the laser odometry of each frame; the laser odometry of each frame and GPS data are aligned according to the time stamp, and the pose transformation relationship between the laser odometry and GPS data after data alignment is calculated to complete the initialization; the preset processing flow includes calculating the state quantity compared with the initial moment within the time of the corresponding frame according to the IMU data, compensating the motion deviation of the point cloud data of the corresponding frame according to the state quantity to obtain the corrected point cloud data, matching the corrected point cloud data with the previously constructed map to calculate the residual, and iteratively optimizing the state quantity according to the residual to obtain the corrected state quantity, denoted as the laser odometry.

[0075] A current frame state quantity and point cloud data processing module: used to receive the point cloud data and IMU data collected in the current frame in real time; calculate the current frame laser odometry and the corrected point cloud data according to the preset processing flow for the IMU data and point cloud data of the current frame.

[0076] Positioning module: If GPS data is not included in the current frame data and cannot be received in real time indoors, absolute positioning data is obtained by using the pose transformation relationship according to the current frame laser odometer; if GPS data is included in the current frame data and can be received in real time outdoors, the corrected state quantity of the current frame laser odometer and the GPS data are aligned according to the time stamp, the current frame laser odometer after data alignment is converted by using the pose transformation relationship, and the converted laser odometer and the current frame GPS data are used as observation values to be added to the EKF filter for correction to obtain absolute positioning data;

[0077] Map update module: Used to update the map according to the corrected point cloud data.

[0078] This application includes sensor data acquisition, association and time synchronization. The installation positions of the lidar, camera and GPS are relatively fixed and the external parameters need to be calibrated. The position and angle data obtained by IMU recursion are compensated for the motion distortion of the laser point cloud, and then the pose of the point cloud between different frames is estimated by iterative Kalman filtering to achieve positioning and map stitching. In the positioning fusion process, based on the GPS positioning data, linear interpolation is performed on the laser odometer data to achieve data alignment at the nanometer level. Finally, the interpolated laser odometer and GPS data are used as observation values to correct the IMU recursion result to achieve positioning fusion.

[0079] The present invention designs a multi-sensor fusion algorithm. This algorithm fuses lidar and GPS data through an iterative Kalman filtering algorithm, can be successfully initialized outdoors, and provides an indoor and outdoor absolute positioning accuracy within 3 cm. It can scan the three-dimensional environment to generate a dense point cloud map, and can color the point cloud by combining the RGB image data collected by the monocular camera to restore the environmental texture information in the three-dimensional point cloud map.

[0080] Since the positioning of the local point cloud map needs to be associated with GPS absolute positioning data to achieve global fusion positioning, but the GPS signal is only limited to outdoor scenarios, through a special initialization method design, after the GPS and lidar positioning alignment is completed outdoors, subsequent indoor and outdoor scene switching will not affect the continuous output of absolute positioning.

[0081] In the self-developed algorithm, the IMU forward propagation calculates the position information of each laser point relative to the start time of the lidar scan. During the point cloud backpropagation process, the motion distortion is corrected and the state estimation is performed through the iterative Kalman filtering algorithm. For the need of absolute positioning, the present invention fuses the gnss positioning technology, which can provide a positioning accuracy of 5 cm outdoors. At the same time, through the online calibration initialization function, after initialization outdoors, relatively high-precision absolute positioning can also be provided indoors.

[0082] In one embodiment, the present application further provides an indoor and outdoor positioning and mapping device with multi-sensor fusion. Figure 4 This is the external view of the device designed by the present invention. The equipment designed by the present invention can be used in handheld, vehicle-mounted, robotic, and industrial scenarios. The specific functions and structures are introduced as follows:

[0083] The three-dimensional mapping and positioning device mentioned in the present application has a detachable scanning module and a power-supplied handle module.

[0084] The scanning module includes a lidar, a GNSS antenna, a data transmission antenna, a camera, and a central processing unit and a power management unit installed inside. The relative positions of the lidar, GNSS antenna, data transmission antenna, and camera are all relatively fixed and unchanged. There is a heat dissipation module installed on the front inside the housing. The power-supplied handle module is internally equipped with a power supply lithium battery, and the base has a charging port and a power status indicator light.

[0085] The present invention provides a portable and efficient positioning, mapping, and environment perception device. This device integrates multiple sensors such as a GPS antenna, a monocular camera, and a lidar into a handheld device, which has the characteristics of light weight, portability, and detachability. It can be installed on different devices such as drones, robots, and vehicles according to scene requirements to achieve positioning and environment perception functions. The handheld device can also be independent of other devices and has a rechargeable handheld handle, which can meet the indoor and outdoor mapping and positioning operations for 2 hours at a time.

[0086] For the specific limitations of a multi-sensor fusion indoor and outdoor positioning and mapping system, reference can be made to the limitations of a multi-sensor fusion indoor and outdoor positioning and mapping method in the above text, which will not be elaborated here. Each module in the above-mentioned multi-sensor fusion indoor and outdoor positioning and mapping system can be implemented in whole or in part through software, hardware, and their combinations. The above-mentioned modules can be embedded in the processor of a computer device in hardware form or be independent of it, or be stored in the memory of the computer device in software form, so that the processor can call and execute the operations corresponding to the above-mentioned modules.

[0087] In one embodiment, a computer device is provided. This computer device can be a server. The computer device includes a processor, a memory, and a network interface connected through a system bus. Among them, the processor of the computer device is used to provide computing and control capabilities. The memory of the computer device includes a non-volatile storage medium and an internal memory. The non-volatile storage medium stores an operating system, a computer program, and a database. The internal memory provides an environment for the operation of the operating system and the computer program in the non-volatile storage medium. The network interface of the computer device is used to communicate with an external terminal through a network connection. When the computer program is executed by the processor, it realizes the above-mentioned multi-sensor fusion indoor and outdoor positioning and mapping method.

[0088] In one embodiment, a computer-readable storage medium is further provided, on which a computer program is stored. When the computer program is executed by a processor, it involves all or part of the processes in the method of the above embodiment.

[0089] Those of ordinary skill in the art can understand that to implement all or part of the processes in the method of the above embodiment, it can be completed by instructing relevant hardware through a computer program. The computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the processes of the embodiments of the above methods. Among them, any reference to a memory, storage, database, or other medium used in the embodiments provided in the present application can include at least one of non-volatile and volatile memories. Non-volatile memory can include read-only memory (ROM), magnetic tape, floppy disk, flash memory, or optical memory, etc. Volatile memory can include random access memory (RAM) or external cache memory. By way of illustration and not limitation, RAM can be in various forms, such as static random access memory (SRAM) or dynamic random access memory (DRAM), etc.

[0090] The technical features of the above embodiments can be combined arbitrarily. For the sake of concise description, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, it should be considered as the scope described in this specification.

[0091] The above-described embodiments only represent several implementation manners of the present application. The description is relatively specific and detailed, but it should not be construed as a limitation on the scope of the invention patent. It should be noted that for those of ordinary skill in the art, without departing from the concept of the present application, several modifications and improvements can still be made, and these all belong to the protection scope of the present application. Therefore, the protection scope of the patent of the present application should be subject to the appended claims.

Claims

1. An indoor and outdoor positioning and mapping method based on multi-sensor fusion, characterized in that The method includes: Obtaining multiple frames of historical point cloud data, IMU data, and GPS data collected outdoors by the mapping and positioning device; for each frame of historical data, processing it according to a preset processing flow to obtain the laser odometer for each frame; aligning the laser odometer and GPS data for each frame according to the time stamp, and calculating the pose transformation relationship between the laser odometer and GPS data after data alignment, thereby completing the initialization; the preset processing flow includes calculating the state quantity compared to the initial moment within the corresponding frame time according to the IMU data, compensating for the motion deviation of the point cloud data of the corresponding frame according to the state quantity to obtain the corrected point cloud data, and matching the corrected point cloud data with the previously constructed map to calculate the residual, and iteratively optimizing the state quantity according to the residual to obtain the corrected state quantity, denoted as the laser odometer; Receiving the point cloud data and IMU data collected in the current frame in real time; calculating the laser odometer of the current frame and the corrected point cloud data according to the preset processing flow for the IMU data and point cloud data of the current frame; If GPS data is not included in the current frame data received in the indoor environment in real time, then converting and obtaining the absolute positioning data according to the laser odometer of the current frame using the pose transformation relationship; if GPS data is included in the current frame data that can be received in the outdoor environment in real time, then aligning the corrected state quantity of the laser odometer of the current frame and the GPS data according to the time stamp, converting the laser odometer of the current frame after data alignment using the pose transformation relationship, and adding the converted laser odometer and the GPS data of the current frame as observation values to the EKF filter to correct and obtain the absolute positioning data; Updating the map according to the corrected point cloud data.

2. The multi-sensor fusion indoor and outdoor positioning and mapping method according to claim 1, characterized in that The calculation of the state quantity compared to the initial moment within the corresponding frame time according to the IMU data is carried out through the following formula; ， Among them, represents the rotation matrix at time t, is the angular velocity collected by the IMU, is the bias of the gyroscope, is the noise of the gyroscope, is the time step, represents the velocity at time t, is the measured acceleration, is the accelerometer bias, is the noise of the accelerometer, and g is the acceleration due to gravity, represents the position vector at time t, represents the acceleration vector at time t, is the random walk noise of the gyroscope bias, represents the random walk noise of the accelerometer bias.

3. The indoor and outdoor positioning and mapping method based on multi-sensor fusion according to claim 1, characterized in that The matching of the corrected point cloud data with the previously constructed map to calculate the residual includes: Converting the position of the corrected point cloud from the radar coordinate system to the IMU coordinate system, and then from the IMU coordinate system to the world coordinate system; Finding the 5 points closest to any point in the corrected point cloud in the previously constructed map, and calculating the distance between this point and the fitting plane constructed by the corresponding 5 points to obtain the residual.

4. The indoor and outdoor positioning and mapping method with multi-sensor fusion according to claim 3, characterized in that The equation of the fitting plane is calculated through the following formula: ， where a, b, and c are coefficients for determining a plane, and d is a scaling coefficient. represents the three-dimensional coordinates of point i, where i = 1, 2, 3, 4, 5; is the plane parameter.

5. The indoor and outdoor positioning and mapping method based on multi-sensor fusion according to claim 1, characterized in that The iterative optimization of the state quantity according to the residual to obtain the corrected state quantity of the laser odometer includes: Step a: Iteratively optimizing the state quantity according to the residual calculated from the residual calculated from the position of the corrected point cloud, and judging whether the optimal state is reached through the following formula: ， ， Among them, is a state quantity, represents the state value corresponding to the k-th observation data, is the sliding window length parameter, represents the estimated value of the state quantity, is the Jacobian matrix of the measurement model at time k, is the estimation error of the system state at time k, is the noise, is the j-th measurement value at time k, is the error between the estimated value and the true state, is the error between the measurement value and the predicted measurement value, is the state covariance matrix, represents the observation noise covariance matrix, Observation model function, describing the mapping from state to observation, is the estimated state value; is the error between the true value of the current system state and the current system predicted estimate value; Step b: If it is not the optimal state, then correcting the position of the point cloud again, calculating the residual, and returning to step a until the optimal state is reached.

6. The indoor and outdoor positioning and mapping method based on multi-sensor fusion according to claim 1, characterized in that After receiving the GPS data of the mapping and positioning device, it further includes: Accumulating the GPS data within a specific time interval, performing smoothing filtering on the data, and removing the abnormal points with large fluctuations to obtain the filtered GPS data.

7. The method according to claim 1, characterized in that The data alignment of the corrected state quantity of the laser odometer and the GPS data for each frame according to the time stamp includes: Obtain the timestamp corresponding to the corrected state quantity of the laser odometer, find the two nearest points before and after in the GPS data to this timestamp, perform linear interpolation on the positioning points corresponding to the two nearest points before and after in the GPS data to this timestamp, and obtain the GPS data at the same timestamp as the corrected state quantity of the laser odometer after obtaining the difference.

8. An indoor and outdoor positioning and mapping system with multi-sensor fusion, characterized in that, The system includes: Initialization module: used to obtain multiple frames of historical point cloud data, IMU data, and GPS data collected outdoors in the mapping and positioning device; for each frame of historical data, process it according to a preset processing flow to obtain the laser odometer for each frame; align the laser odometer and GPS data for each frame according to the timestamp, and calculate the pose transformation relationship between the aligned laser odometer and GPS data to complete the initialization; the preset processing flow includes calculating the state quantity compared to the initial moment within the time of the corresponding frame according to the IMU data, compensating for the motion deviation of the point cloud data of the corresponding frame according to the state quantity to obtain the corrected point cloud data, matching the corrected point cloud data with the previously constructed map to calculate the residual, and iteratively optimizing the state quantity according to the residual to obtain the corrected state quantity, denoted as the laser odometer; Current frame state quantity and point cloud data processing module: used to receive the point cloud data and IMU data collected in the current frame in real time; calculate the current frame laser odometer and the corrected point cloud data according to the preset processing flow for the IMU data and point cloud data of the current frame; Positioning module: used to, if GPS data is not included in the current frame data received in real time indoors, use the pose transformation relationship to convert according to the current frame laser odometer to obtain absolute positioning data; if GPS data is included in the current frame data that can be received in real time outdoors, align the corrected state quantity of the current frame laser odometer and the GPS data according to the timestamp, convert the current frame laser odometer after alignment using the pose transformation relationship, and use the converted laser odometer and the current frame GPS data as observation values to be added to the EKF filter to correct and obtain absolute positioning data; Map update module: used to update the map according to the corrected point cloud data.

9. A computer device, comprising a memory and a processor, the memory storing a computer program, characterized in that, When the processor executes the computer program, it implements the steps of the method described in any one of claims 1 to 7.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the steps of the method described in any one of claims 1 to 7.

Images