Indoor and outdoor positioning and mapping method based on multi-sensor fusion

Abstract

The invention discloses a multi-sensor fusion indoor and outdoor positioning and mapping method, relates to the technical field of map construction and positioning, and aims to solve the problem of low indoor and outdoor positioning precision. Comprising the following steps: processing each frame of point cloud data and IMU data obtained outdoors according to a preset processing flow to obtain a laser odometer of each frame; carrying out data alignment on each frame of laser speedometer and GPS data according to a timestamp and calculating a position conversion relation; calculating the point cloud data and the IMU data of the current frame according to a preset processing flow; acquiring absolute positioning data of the current-frame laser speedometer by using the coordinate conversion relation; if outdoor, the laser speedometer and GPS data are added into a filter for correction to obtain absolute positioning data. According to the invention, the positioning precision is improved, and accurate indoor absolute positioning can be realized without depending on GPS data.

Description

Technical Field

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

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

[0003] Normally, indoor positioning and mapping mainly rely on visual sensors and laser sensors. Common technical solutions include scanning the environment map through multi-line laser radar. Due to motion, the laser point cloud in the same frame of laser data has motion distortion. Combined with IMU data, the motion change in each frame is given so that the laser point in the current frame is aligned to the start time or end time of the laser scan to eliminate distortion. In order to enable each frame of the point cloud map to be spliced ​​into a complete point cloud map, it is necessary to calculate the positional relationship between the current frame point cloud and the next frame point cloud. Usually, a three-dimensional point cloud matching algorithm such as the ICP algorithm is used. This algorithm usually uses all laser point cloud data in the current frame to calculate the positional relationship between each point in the current frame and the corresponding point in the adjacent frame. When the distance of all points is minimized, the corresponding posture relationship is the posture relationship between the two frames. However, since the matching of point clouds is based on limited historical data as the matching benchmark, the matching error will directly affect the matching accuracy of the next frame, the so-called cumulative error.

[0004] Therefore, the accumulated error is corrected by loop detection of point cloud. The purpose of laser point cloud loop detection is to determine whether the robot or device has returned to the area that has been explored before, so as to correct the accumulated error. To determine whether to return to the previous area, it is necessary to combine point cloud feature matching, spatial geometric relationship, and pose estimation. Match the point cloud features of the current frame with the previous point cloud library, and use KD tree or other efficient index structures to find the historical frame that is most similar to the current frame. Since the pose quantity between adjacent frame point clouds represents the positional relationship between adjacent frame point clouds, it can also represent the positional relationship of sensors, so the pose quantity of continuous frames is output as odometer. However, the method based on feature points is used to associate frames, but it is limited to traditional matching and optimization methods. When the amount of feature information decreases, the accuracy brought by the algorithm will also decrease synchronously. Moreover, due to the limitation of GPS characteristics, this method cannot provide absolute indoor positioning, and cannot meet the needs of indoor and outdoor synchronous positioning, or requires a lot of preliminary work, such as establishing high-precision maps, storing absolute positioning information and point clouds indoors for association, but the cost is too high. Summary of the invention

[0005] Based on this, in order to solve 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 achieved by the existing technology.

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

[0007] Acquire multiple frames of historical point cloud data, IMU data and GPS data collected outdoors in a mapping and positioning device; process each frame of historical data according to a preset processing flow to obtain a laser odometer for each frame; align the laser odometer and GPS data for each frame according to a timestamp, and calculate the position and posture transformation relationship between the laser odometer and GPS data after data alignment to complete initialization; the preset processing flow includes calculating the state quantity compared to the initial moment in 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, and matching the corrected point cloud data with the previously constructed map to calculate the residual, iteratively optimizing the state quantity according to the residual, and obtaining the corrected state quantity, which is recorded as the laser odometer;

[0008] Receive the collected point cloud data and IMU data of the current frame in real time; calculate the IMU data and point cloud data of the current frame according to the preset processing flow to obtain the laser odometer of the current frame and the corrected point cloud data;

[0009] If the current frame data does not contain GPS data and cannot be received indoors in real time, the current frame laser odometer is converted using the posture transformation relationship to obtain absolute positioning data; if the current frame data can be received outdoors in real time and contains GPS data, the corrected state quantity of the current frame laser odometer and the GPS data are aligned according to the timestamp, the current frame laser odometer after data alignment is converted using the posture transformation relationship, and the converted laser odometer and the current frame GPS data are added as observation values ​​to the EKF filter to correct and obtain absolute positioning data;

[0010] Update the map based on the corrected point cloud data.

[0011] In the above scheme, optionally, the state quantity in the corresponding frame time compared with the initial time is calculated according to the IMU data by the following formula;

[0012]

[0013] in, represents the rotation matrix at time t, is the angular velocity collected by the IMU, is the gyroscope bias, is the gyroscope noise, is the time step, represents the speed 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, matching the corrected point cloud data with the previously constructed map to calculate the residual includes:

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

[0016] Find the five points in the previously constructed map that are closest to any point in the corrected point cloud, and calculate the distance between this point and the fitting plane constructed by the corresponding five 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] Among them, a, b, c are the coefficients for determining the plane, and d is the scaling factor. Represents the three-dimensional coordinates of point i, i=1, 2, 3, 4, 5; is the plane parameter.

[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: Iteratively optimize the residual pair calculated from the corrected point cloud position according to the residual pair state quantity, and determine whether the optimal state is achieved by the following formula:

[0022]

[0023]

[0024] in, 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, is the Jacobian matrix of the measurement model at time k, is the estimated error of the system state at time k, For noise, is the jth measurement at time k, is the error between the estimated value and the true state, is the error between the measured value and the predicted measured value, is the state defense difference matrix, is the observation noise coordination difference matrix, is the observation model function, describing the mapping from state to observation, is the estimated state value; is the error between the actual value of the current system state and the estimated value of the current system prediction;

[0025] Step b: If it is not optimal, the point cloud position is corrected again, the residual is calculated, and the process returns to step a until the optimal result is reached.

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

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

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

[0029] Get the timestamp corresponding to the corrected state of the laser odometer, find the two points before and after the timestamp in the GPS data, perform linear interpolation on the positioning points corresponding to the two points before and after the timestamp in the GPS data, and get the GPS data with the same timestamp time as the corrected state of the laser odometer after the difference.

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

[0031] Initialization module: used to obtain multiple frames of historical point cloud data, IMU data and GPS data collected outdoors in the mapping and positioning equipment; for each frame of historical data, process it according to the preset processing flow to obtain the laser odometer of each frame; align the data of each frame of laser odometer and GPS data according to the timestamp, and calculate the posture 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 to the initial moment in 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, so as to obtain the corrected point cloud data, and matching the corrected point cloud data with the previously constructed map to calculate the residual, iteratively optimizing the state quantity according to the residual, and obtaining the corrected state quantity, which is recorded as the laser odometer;

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

[0033] Positioning module: if the current frame data does not contain GPS data and cannot be received indoors in real time, the laser odometer of the current frame is converted using the posture transformation relationship to obtain absolute positioning data; if the current frame data can be received outdoors in real time and contains GPS data, the corrected state quantity of the laser odometer of the current frame and the GPS data are aligned according to the timestamp, the laser odometer of the current frame after data alignment is converted using the posture transformation relationship, and the converted laser odometer and the current frame GPS data are added as observation values ​​to the EKF filter to correct and obtain absolute positioning data;

[0034] Map update module: used to update the map based on the corrected point cloud data.

[0035] In a third aspect, a computer device includes a memory and a processor, wherein the memory stores a computer program, and when the processor executes the computer program, the steps of the multi-sensor fusion indoor and outdoor positioning and mapping method described in the first aspect are implemented.

[0036] In a fourth aspect, a computer-readable storage medium stores a computer program thereon, wherein when the computer program is executed by a processor, the steps of the multi-sensor fusion indoor and outdoor positioning and mapping method described in the first aspect are implemented.

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

[0038] The present application compensates the power supply data for motion deviation by calculating the state quantity of the historical frame according to the IMU data, and corrects the state quantity according to the corrected point cloud data, aligns the corrected state quantity and GPS data according to the timestamp, and obtains the coordinate conversion formula for converting the corrected state quantity of the laser odometer to the world coordinate system according to the corrected state quantity and GPS data of each frame of the laser odometer after data alignment; thus, for the current indoor positioning, it is only necessary to convert the final positioning data according to the coordinate conversion relationship based on the calculated corrected state quantity. For outdoor positioning, the corrected state quantity of the laser odometer of the current frame and the GPS data are aligned according to the timestamp, and the corrected state quantity of the laser odometer of the current frame after data alignment and the GPS data are added as observation values ​​to the EKF filter to correct and obtain the absolute positioning data. Therefore, the method of the present application does not rely on the data of historical feature points, thereby improving the positioning accuracy. Moreover, accurate indoor absolute positioning can also be achieved without relying on GPS data. BRIEF DESCRIPTION OF THE DRAWINGS

[0039] Figure 1 A schematic diagram of a flow chart of a multi-sensor fusion indoor and outdoor positioning and mapping method provided in one embodiment of the present application;

[0040] Figure 2 A schematic diagram of a specific process of a multi-sensor fusion indoor and outdoor positioning and mapping method provided in one embodiment of the present application;

[0041] Figure 3 A schematic diagram of a specific process of adding laser odometer and GPS positioning data as observation values ​​to the EKF filter to correct the accumulated deviation of IMU recursion in one embodiment of the present application;

[0042] Figure 4 A structural diagram of a mapping and positioning device provided for one embodiment of the present application. DETAILED DESCRIPTION

[0043] In order to make the purpose, technical solution and advantages of the present application more clearly understood, the present application is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present application and are not used to limit the present application.

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

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

[0046] In step S1, it specifically includes:

[0047] (1) IMU data is obtained by the IMU module. First, the gravity direction and gyroscope zero bias are initialized. Then, according to the input observation line acceleration and angular velocity, the state quantity position, rotation, speed, bias and noise quantity are updated at different subsequent moments. The state quantity includes position, speed, attitude, bias, etc. The update of the state quantity is based on Newton's laws of kinematics, 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 gyroscope bias, is the gyroscope noise, is the time step, represents the speed 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.

[0050] (2) Compensate for the motion deviation of the point cloud data of each point in the current frame based on the position and attitude state 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 convert it to the world coordinate system based on the pose estimated by the forward propagation. Find the five points closest to the current point to fit the plane. The plane equation is shown in formula (2):

[0052] (2)

[0053] Among them, a, b, c are the coefficients for determining the plane, and d is the scaling factor. Represents the three-dimensional coordinates of point i, i=1, 2, 3, 4, 5; is the plane parameter.

[0054] Calculate the residual based on the distance between the current point and the fitted plane.

[0055] (4) State estimation: In order to obtain more accurate posture estimation in nonlinear systems, the state update is optimized through multiple iterations until the updated state converges to a more accurate state. The system residual is calculated and the optimal equation of the system state is constructed, where the optimal equation is shown in formulas (3) and (4):

[0056] (3)

[0057] (4)

[0058] in, 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, is the Jacobian matrix of the measurement model at time k, is the estimated error of the system state at time k, For noise, is the jth measurement at time k, is the error between the estimated value and the true state, is the error between the measured value and the predicted measured value, is the state defense difference matrix, is the observation noise coordination difference matrix, is the observation model function, describing the mapping from state to observation, is the estimated state value; is the error between the actual value of the current system state and the estimated value of the current system prediction;

[0059] (5) Due to the inconsistency between the frequency of GPS positioning data and the laser odometer, a large amount of data will be lost during the data fusion process, reducing the positioning accuracy. Therefore, based on the timestamp corresponding to each frame of data released by the laser odometer, the nearest neighbor point in GPS is found, and the positioning points corresponding to the adjacent timestamps before and after the nearest neighbor are linearly interpolated. The interpolation points are used as the corresponding matching point pairs in the GPS positioning points, which maximize the association of the GPS positioning points with the positioning points of the laser odometer at the closest moment, thereby reducing the fusion error. At the same time, based on the corrected state quantity of each frame of the laser odometer after data alignment and the GPS data, the coordinate conversion formula for converting the corrected state quantity of the laser odometer to the world coordinate system is obtained.

[0060] At the same time, the received RTK (GPS) data is parsed, the latitude and longitude data at specific time intervals are accumulated, the data is smoothed and filtered, and abnormal points with large fluctuations are removed to obtain the filtered RTK data.

[0061] Step S2: receiving the collected point cloud data and IMU data of the current frame in real time; calculating the IMU data and point cloud data of the current frame according to the preset processing flow to obtain the laser odometer of the current frame and the corrected point cloud data.

[0062] Step S3: If the current frame data does not contain GPS data and cannot be received indoors in real time, the current frame laser odometer is converted using the posture transformation relationship to obtain absolute positioning data; if the current frame data can be received outdoors in real time and contains GPS data, the corrected state quantity of the current frame laser odometer and the GPS data are aligned according to the timestamp, the current frame laser odometer after data alignment is converted using the posture transformation relationship, and the converted laser odometer and current frame GPS data are added as observation values ​​to the EKF filter to correct and obtain absolute positioning data.

[0063] The IMU raw data uses Newton kinematics as the basic principle to recurse the position. As the IMU recursion progresses, the cumulative error will gradually increase. The laser odometer and GPS positioning data are introduced as observation values ​​and added to the EKF filter to correct the cumulative deviation of the IMU recursion. Figure 3 As shown. Among them, the state quantities to be estimated in the system are x, y, z, roll, pitch, and yaw. The system's observation matrix H and state transfer matrix are both unit matrices, and the measurement noise and observation noise are given corresponding weights according to the sensor measurement accuracy. The input quantities of the system are GPS positioning and laser odometer, respectively, and then after data alignment, state prediction, and positioning data update, the fused positioning result is finally output. The prediction and update equations are shown in the following formulas (5), (6), (7), and (8):

[0064] (5)

[0065] (6)

[0066] (7)

[0067] (8)

[0068] (9)

[0069] in, is the predicted value of the system state at time k, based on time Information, 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 transfer 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 based on the diagonal coordinates of the map. When the distance between the center of the radar and each surface of the map is less than the set threshold, it means that the radar is close to the map boundary, and the map needs to be updated. Then the map is updated based on the final positioning data of the current frame and the corrected point cloud data.

[0072] The above-mentioned multi-sensor fusion indoor and outdoor positioning and mapping method compensates the power supply data for motion deviation by using the state quantity calculated according to the IMU data of the historical frame, and corrects the state quantity according to the corrected point cloud data, aligns the corrected state quantity and GPS data according to the timestamp, and obtains the coordinate conversion formula for converting the corrected state quantity of the laser odometer to the world coordinate system according to the corrected state quantity and GPS data of each frame of the laser odometer after data alignment; thus, for the current indoor positioning, it is only necessary to convert the calculated corrected state quantity using the coordinate conversion formula to obtain the final positioning data. For outdoor positioning, the corrected state quantity of the laser odometer of the current frame and the GPS data are aligned according to the timestamp, and the corrected state quantity of the laser odometer of the current frame after data alignment and the GPS data are aligned according to the timestamp as the observation value and added to the EKF filter to correct and obtain the final positioning data. Therefore, the method of this application does not rely on the data of historical feature points, thereby improving the positioning accuracy. Moreover, accurate indoor positioning can also be achieved without relying on GPS data.

[0073] In one embodiment, a multi-sensor fusion indoor and outdoor positioning and mapping system is provided, the system comprising:

[0074] Initialization module: used to obtain multiple frames of historical point cloud data, IMU data and GPS data collected outdoors in the mapping and positioning equipment; for each frame of historical data, process it according to the preset processing flow to obtain the laser odometer of each frame; align the data of each frame of laser odometer and GPS data according to the timestamp, and calculate the posture 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 to the initial moment in 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, so as to obtain the corrected point cloud data, and matching the corrected point cloud data with the previously constructed map to calculate the residual, iteratively optimizing the state quantity according to the residual, and obtaining the corrected state quantity, which is recorded as the laser odometer;

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

[0076] Positioning module: if the current frame data does not contain GPS data and cannot be received indoors in real time, the laser odometer of the current frame is converted using the posture transformation relationship to obtain absolute positioning data; if the current frame data can be received outdoors in real time and contains GPS data, the corrected state quantity of the laser odometer of the current frame and the GPS data are aligned according to the timestamp, the laser odometer of the current frame after data alignment is converted using the posture transformation relationship, and the converted laser odometer and the current frame GPS data are added as observation values ​​to the EKF filter to correct and obtain absolute positioning data;

[0077] Map update module: used to update the map based on the corrected point cloud data.

[0078] This application includes sensor data acquisition, association and time synchronization, in which the installation positions of lidar, camera and GPS are relatively fixed and external parameter calibration needs to be completed. Use the IMU recursive position and angle data to compensate for the motion distortion of the laser point cloud, and then use iterative Kalman filtering to estimate the point cloud pose between different frames to achieve positioning and map stitching. In the positioning fusion process, the laser odometer data is linearly interpolated based on the GPS positioning data to achieve nanometer-level data alignment. Finally, the interpolated laser mileage calculation and GPS data are used as observation values ​​to correct the IMU recursive results and achieve positioning fusion.

[0079] The present invention designs a multi-sensor fusion algorithm, which fuses lidar and GPS data through an iterative Kalman filter algorithm, and can provide an absolute positioning accuracy within 3 cm indoors and outdoors after successful outdoor initialization. It can scan the three-dimensional environment to generate a dense point cloud map, and can add color to the point cloud in combination with the RGB image data collected by the monocular camera, and 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 the GPS absolute positioning data to achieve global fusion positioning, but the GPS signal is only limited to outdoor scenes, the present invention is designed through a special initialization method. After the positioning alignment of GPS and lidar is completed outdoors, the subsequent arbitrary switching of indoor and outdoor scenes 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 laser radar scan, corrects the motion distortion in the point cloud back propagation process, and performs state estimation through the iterative Kalman filter algorithm. For the need for absolute positioning, the present invention integrates GNSS positioning technology and can provide a positioning accuracy of 5cm outdoors. At the same time, through the initialization function of online calibration, after completing the initialization outdoors, entering the indoor area can also provide relatively high-precision absolute positioning.

[0082] In one embodiment, the present application also provides a multi-sensor fusion indoor and outdoor positioning and mapping device, Figure 4 This is the appearance diagram 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 present application relates to a three-dimensional mapping and positioning device having a detachable scanning module and a powered handle module.

[0084] The scanning module includes a laser radar, 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 laser radar, GNSS antenna, data transmission antenna and camera are relatively fixed, and a heat dissipation module is installed on the front of the shell. The power-supplying handle module is equipped with a power supply lithium battery, and the base has a charging port and a power status indicator.

[0085] The present invention provides a portable and efficient positioning, mapping and environmental perception device. The device is equipped with a GPS antenna, a monocular camera and a laser radar multi-sensor integrated into a handheld device. It 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 realize positioning and environmental perception functions. The handheld device can also be independent of other devices and has a rechargeable handheld handle to meet a single 2-hour indoor and outdoor mapping and positioning operation.

[0086] For the specific definition of a multi-sensor fusion indoor and outdoor positioning and mapping system, please refer to the definition of a multi-sensor fusion indoor and outdoor positioning and mapping method above, which will not be repeated 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 by software, hardware and a combination thereof. The above-mentioned modules can be embedded in or independent of the processor in the computer device in the form of hardware, or can be stored in the memory of the computer device in the form of software, so that the processor can call and execute the operations corresponding to the above modules.

[0087] In one embodiment, a computer device is provided, which may be a server. The computer device includes a processor, a memory, and a network interface connected via a system bus. 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 via a network connection. When the computer program is executed by the processor, the above-mentioned multi-sensor fusion indoor and outdoor positioning and mapping method is implemented.

[0088] In one embodiment, a computer-readable storage medium is also 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 above-mentioned embodiment method.

[0089] Those of ordinary skill in the art can understand that all or part of the processes in the above-mentioned embodiment methods can be completed by instructing the relevant hardware through a computer program, and 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-mentioned methods. Among them, any reference to memory, storage, database or other media used in the embodiments provided in this application can include at least one of non-volatile and volatile memory. Non-volatile memory can include read-only memory (ROM), tape, floppy disk, flash memory or optical memory, etc. Volatile memory can include random access memory (RAM) or external cache memory. As an illustration and not limitation, RAM can be in various forms, such as static random access memory (SRAM) or dynamic random access memory (DRAM).

[0090] The technical features of the above embodiments may be combined arbitrarily. To make the description concise, 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, they should be considered to be within the scope of this specification.

[0091] The above-mentioned embodiments only express several implementation methods of the present application, and the descriptions thereof are relatively specific and detailed, but they cannot be understood as limiting the scope of the invention patent. It should be pointed out that, for a person of ordinary skill in the art, several variations and improvements can be made without departing from the concept of the present application, and these all belong to the protection scope of the present application. Therefore, the protection scope of the patent of the present application shall be subject to the attached claims.

Claims

1. A multi-sensor fusion indoor and outdoor positioning and mapping method, characterized in that: The method comprises: Acquire multiple frames of historical point cloud data, IMU data and GPS data collected outdoors in a mapping and positioning device; process each frame of historical data according to a preset processing flow to obtain a laser odometer for each frame; align the laser odometer and GPS data for each frame according to a timestamp, and calculate the position and posture transformation relationship between the laser odometer and GPS data after data alignment to complete initialization; the preset processing flow includes calculating the state quantity compared to the initial moment in 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, and matching the corrected point cloud data with the previously constructed map to calculate the residual, iteratively optimizing the state quantity according to the residual, and obtaining the corrected state quantity, which is recorded as the laser odometer; Receive the collected point cloud data and IMU data of the current frame in real time; calculate the IMU data and point cloud data of the current frame according to the preset processing flow to obtain the laser odometer of the current frame and the corrected point cloud data; If the current frame data does not contain GPS data and cannot be received indoors in real time, the current frame laser odometer is converted using the posture transformation relationship to obtain absolute positioning data; if the current frame data can be received outdoors in real time and contains GPS data, the corrected state quantity of the current frame laser odometer and the GPS data are aligned according to the timestamp, the current frame laser odometer after data alignment is converted using the posture transformation relationship, and the converted laser odometer and the current frame GPS data are added as observation values ​​to the EKF filter to correct and obtain absolute positioning data; Update the map based on 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 in the corresponding frame time compared to the initial time according to the IMU data is performed by the following formula: ； in, represents the rotation matrix at time t, is the angular velocity collected by the IMU, is the gyroscope bias, is the gyroscope noise, is the time step, represents the speed 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.

3. The multi-sensor fusion indoor and outdoor positioning and mapping method 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: Convert the corrected point cloud position from the radar coordinate system to the IMU coordinate system, and then from the IMU coordinate system to the world coordinate system; Find the five points in the previously constructed map that are closest to any point in the corrected point cloud, and calculate the distance between this point and the fitting plane constructed by the corresponding five points to obtain the residual.

4. The multi-sensor fusion indoor and outdoor positioning and mapping method according to claim 3, characterized in that: The equation of the fitted plane is calculated by the following formula: ； Among them, a, b, c are the coefficients for determining the plane, and d is the scaling factor. Represents the three-dimensional coordinates of point i, i=1, 2, 3, 4, 5; is the plane parameter.

5. The multi-sensor fusion indoor and outdoor positioning and mapping method 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 optimize the residual pair calculated from the corrected point cloud position according to the residual pair state quantity, and determine whether the optimal state is achieved by the following formula: ； ； in, 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, is the Jacobian matrix of the measurement model at time k, is the estimated error of the system state at time k, For noise, is the jth measurement at time k, is the error between the estimated value and the true state, is the error between the measured value and the predicted measured value, is the state defense difference matrix, is the observation noise coordination difference matrix, is the observation model function, describing the mapping from state to observation, is the estimated state value; is the error between the actual value of the current system state and the estimated value of the current system prediction; Step b: If it is not optimal, the point cloud position is corrected again, the residual is calculated, and the process returns to step a until the optimal result is reached.

6. The method for indoor and outdoor positioning and mapping based on multi-sensor fusion according to claim 1, characterized in that: After receiving the GPS data of the mapping and positioning device, the method further includes: Accumulate GPS data at specific time intervals, perform smoothing filtering on the data, remove abnormal points with large fluctuations, and obtain filtered GPS data.

7. The method according to claim 1, characterized in that The data alignment of the corrected state of each frame of the laser odometer and the GPS data according to the timestamp includes: Get the timestamp corresponding to the corrected state of the laser odometer, find the two points before and after the timestamp in the GPS data, perform linear interpolation on the positioning points corresponding to the two points before and after the timestamp in the GPS data, and get the GPS data with the same timestamp time as the corrected state of the laser odometer after the difference.

8. A multi-sensor fusion indoor and outdoor positioning and mapping system, characterized in that: The system comprises: Initialization module: used to obtain multiple frames of historical point cloud data, IMU data and GPS data collected outdoors in the mapping and positioning equipment; for each frame of historical data, process it according to the preset processing flow to obtain the laser odometer of each frame; align the data of each frame of laser odometer and GPS data according to the timestamp, and calculate the posture 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 to the initial moment in 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, so as to obtain the corrected point cloud data, and matching the corrected point cloud data with the previously constructed map to calculate the residual, iteratively optimizing the state quantity according to the residual, and obtaining the corrected state quantity, which is recorded as the laser odometer; Current frame state quantity and point cloud data processing module: used to receive the collected point cloud data and IMU data of the current frame in real time; calculate the IMU data and point cloud data of the current frame according to the preset processing flow to obtain the laser odometer of the current frame and the corrected point cloud data; Positioning module: if the current frame data does not contain GPS data and cannot be received indoors in real time, the laser odometer of the current frame is converted using the posture transformation relationship to obtain absolute positioning data; if the current frame data can be received outdoors in real time and contains GPS data, the corrected state quantity of the laser odometer of the current frame and the GPS data are aligned according to the timestamp, the laser odometer of the current frame after data alignment is converted using the posture transformation relationship, and the converted laser odometer and the current frame GPS data are added as observation values ​​to the EKF filter to correct and obtain absolute positioning data; Map update module: used to update the map based on the corrected point cloud data.

9. A computer device comprising a memory and a processor, wherein the memory stores a computer program, wherein: When the processor executes the computer program, the steps of the method according to any one of claims 1 to 7 are implemented.

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

Images