A method and system for seamless indoor and outdoor positioning and 3D mapping

Abstract

The present invention discloses a method and system for seamless indoor and outdoor positioning and 3D mapping, which relates to the field of positioning and mapping. The method comprises: preprocessing laser radar data and inertial measurement sensor data respectively; obtaining the first pose information of the carrier by adopting a graph optimization method based on the dedistorted laser radar data and the inertial measurement sensor data after pre-integration processing; calculating the second pose information of the carrier by adopting a Kalman filter method based on GPS data and the inertial measurement sensor data after pre-integration processing; performing coordinate transformation on the first pose information by the second pose information; taking the first pose information as prediction and the second pose information after coordinate transformation as observation, performing filtering optimization by adopting a Kalman filter; updating the filtered pose information by adopting a radar inertial odometer; registering the dedistorted laser radar data according to the updated pose information, and generating a three-dimensional point cloud map. The present invention can improve positioning accuracy.

Description

Technical Field

[0001] The present invention relates to the field of positioning and mapping, and in particular to a method and system for seamless indoor and outdoor positioning and 3D mapping. Background Art

[0002] With the maturity of Beidou positioning technology, China's outdoor positioning and navigation fields have reached new heights. The outdoor field can rely on the joint positioning of a multi-satellite positioning system composed of Beidou to achieve higher-precision positioning results. However, the signal reception and positioning method of GPS positioning determine that this positioning method cannot achieve good positioning effects in indoor and outdoor semi-obstructed areas.

[0003] Simultaneous Localization And Mapping (SLAM) technology, as a technology that relies only on its own sensors for autonomous positioning, has achieved rapid development in the field of robotics. This method mainly relies on vision and laser sensors, and there are visual SLAM based on vision and laser SLAM based on lidar. Compared with visual SLAM, laser SLAM is more robust, and laser SLAM based on 3D lidar is easier to achieve 3D mapping. Real-time positioning based on SLAM obtains relative position information, which belongs to the local coordinate system and cannot be combined with the outdoor GPS coordinate framework. In addition, the positioning error of SLAM-based positioning technology will increase as the positioning distance increases. Summary of the invention

[0004] Based on this, an embodiment of the present invention provides a method and system for seamless indoor and outdoor positioning and 3D mapping to improve positioning accuracy.

[0005] To achieve the above object, the present invention provides the following solutions:

[0006] A method for seamless indoor and outdoor positioning and 3D mapping, comprising:

[0007] Obtain lidar data, inertial measurement sensor data and GPS data of the carrier in the target space;

[0008] Preprocessing the laser radar data and the inertial measurement sensor data respectively to obtain dedistorted laser radar data and pre-integrated inertial measurement sensor data;

[0009] Using a radar inertial odometer, based on the dedistorted laser radar data and the pre-integrated inertial measurement sensor data, a graph-based optimization method is used to obtain the first position information of the carrier;

[0010] Using a Kalman filter method, the second position information of the carrier is calculated based on the GPS data and the pre-integrated inertial measurement sensor data;

[0011] Performing coordinate transformation on the first posture information based on the second posture information to obtain second posture information based on GPS coordinates;

[0012] Taking the first posture information as prediction and the second posture information based on GPS coordinates as observation, performing filtering optimization using a Kalman filter to obtain filtered posture information;

[0013] Using the radar inertial odometer to update the filtered posture information to obtain updated posture information;

[0014] The dedistorted LiDAR data is registered according to the updated posture information to generate a three-dimensional point cloud map.

[0015] Optionally, the performing data preprocessing on the laser radar data and the inertial measurement sensor data respectively to obtain dedistorted laser radar data and pre-integrated inertial measurement sensor data specifically includes:

[0016] The position and posture of the laser radar when collecting each frame of the laser radar data are determined by using the timestamp and the motion trajectory of the inertial sensor to obtain the coordinate value of each frame of the laser radar data; the laser radar data is collected by using the laser radar; the inertial measurement sensor data is collected by using the inertial sensor;

[0017] The coordinate value of each frame of the laser radar data is converted into the laser radar coordinate system at the starting time of the laser radar data of the frame by using an external reference to obtain dedistorted laser radar data;

[0018] The inertial sensor data between the key frame laser radar data collected by the laser radar is pre-integrated by using a pre-integration processing formula to obtain an inertial sensor data constraint factor; the inertial sensor data constraint factor includes: position, speed and rotation constraints between the laser radar key frame laser radar data; the key frame laser radar data is laser radar data whose movement distance and rotation angle of two adjacent frames of laser radar data are greater than corresponding set thresholds;

[0019] The inertial sensor data constraint factor is determined as the inertial measurement sensor data after pre-integration processing.

[0020] Optionally, the radar inertial odometer is used to obtain the first position information of the carrier in a graph optimization-based manner based on the dedistorted laser radar data and the pre-integrated inertial measurement sensor data, specifically including:

[0021] Determine whether the Z-axis coordinate of each frame of laser radar data in the dedistorted laser radar data is less than a preset height threshold, and if so, delete the laser radar data less than the preset height threshold to obtain the deleted dedistorted laser radar data;

[0022] The three-dimensional point cloud map generated by the deleted dedistorted laser radar data and the key frame laser radar data is matched based on the nearest point iteration algorithm to obtain the laser radar constraint factor; the key frame laser radar data is the laser radar data whose movement distance and rotation angle of two adjacent frames of laser radar data are greater than the corresponding set threshold value;

[0023] The first posture information is obtained by performing a joint optimization calculation based on the laser radar constraint factor and the inertial measurement sensor data after the pre-integration processing.

[0024] Optionally, the using of a Kalman filter method to calculate the second position information of the carrier based on the GPS data and the pre-integrated inertial measurement sensor data specifically includes:

[0025] According to the inertial measurement sensor data after the pre-integration processing, an inertial navigation system is used to perform prediction to obtain an inertial sensor position, an inertial sensor position attitude and an inertial sensor position speed;

[0026] The inertial sensor position, the inertial sensor position attitude, the inertial sensor position speed and the double difference observation values ​​in the GPS data are subjected to Kalman filtering to obtain the error-corrected position, the error-corrected posture and the error-corrected posture speed; the double difference observation values ​​include the double difference observation values ​​of the pseudorange and the double difference observation values ​​of the carrier phase; the double difference observation values ​​of the pseudorange and the double difference observation values ​​of the carrier phase are both obtained by station-satellite differential;

[0027] The error-corrected position, the error-corrected posture, and the error-corrected posture speed are determined as the second posture information.

[0028] Optionally, the performing coordinate transformation on the first posture information based on the second posture information to obtain the second posture information based on GPS coordinates specifically includes:

[0029] The first posture information is subjected to coordinate transformation based on a seven-parameter model to obtain second posture information based on GPS coordinates.

[0030] The present invention also provides an indoor and outdoor seamless positioning and 3D mapping system, comprising:

[0031] A data acquisition module, used to acquire the laser radar data, inertial measurement sensor data and GPS data of the carrier in the target space;

[0032] A preprocessing module, used to perform data preprocessing on the laser radar data and the inertial measurement sensor data respectively to obtain dedistorted laser radar data and pre-integrated inertial measurement sensor data;

[0033] A first pose calculation module, configured to obtain the first pose information of the carrier by using a radar inertial odometer based on the dedistorted laser radar data and the inertial measurement sensor data after the pre-integration processing, using a graph optimization-based method;

[0034] A second posture calculation module, used for calculating the second posture information of the carrier based on the GPS data and the inertial measurement sensor data after the pre-integration processing by adopting a Kalman filter method;

[0035] A coordinate conversion module, used for performing coordinate conversion on the first posture information based on the second posture information to obtain second posture information based on GPS coordinates;

[0036] A filtering module, using a card to take the first posture information as a prediction, taking the second posture information based on the GPS coordinates as an observation, and using a Kalman filter to perform filtering optimization to obtain filtered posture information;

[0037] A posture updating module, used to update the filtered posture information using the radar inertial odometer to obtain updated posture information;

[0038] A registration module is used to register the dedistorted lidar data according to the updated posture information to generate a three-dimensional point cloud map.

[0039] Optionally, the preprocessing module specifically includes:

[0040] A coordinate value determination unit, used to determine the position and posture of the laser radar when collecting each frame of the laser radar data by using the timestamp and the motion trajectory of the inertial sensor, and obtain the coordinate value of each frame of the laser radar data; the laser radar data is collected by using the laser radar; the inertial measurement sensor data is collected by using the inertial sensor;

[0041] A dedistortion unit, used to convert the coordinate value of each frame of the laser radar data into the laser radar coordinate system at the starting time of the laser radar data of the frame through an external reference, so as to obtain dedistorted laser radar data;

[0042] A pre-integration processing unit is used to perform pre-integration processing on the inertial sensor data between the key frame laser radar data collected by the laser radar using a pre-integration processing formula to obtain an inertial sensor data constraint factor; the inertial sensor data constraint factor includes: position, speed and rotation constraints between the laser radar key frame laser radar data; the key frame laser radar data is laser radar data in which the movement distance and rotation angle of two adjacent frames of laser radar data are greater than corresponding set thresholds;

[0043] The pre-integration processed data determining unit is used to determine the inertial sensor data constraint factor as the pre-integration processed inertial measurement sensor data.

[0044] Optionally, the first posture calculation module specifically includes:

[0045] A data screening unit, used to determine whether the Z-axis coordinate of each frame of laser radar data in the dedistorted laser radar data is less than a preset height threshold, and if so, delete the laser radar data less than the preset height threshold to obtain the deleted dedistorted laser radar data;

[0046] A matching unit is used to perform matching based on a nearest point iteration algorithm using the deleted dedistorted laser radar data and a three-dimensional point cloud map generated by key frame laser radar data to obtain a laser radar constraint factor; the key frame laser radar data is laser radar data in which the movement distance and rotation angle of two adjacent frames of laser radar data are both greater than corresponding set thresholds;

[0047] A joint optimization unit is used to perform a joint optimization calculation based on the laser radar constraint factor and the inertial measurement sensor data after the pre-integration processing to obtain the first posture information.

[0048] Optionally, the second posture calculation module specifically includes:

[0049] A data prediction unit, configured to use an inertial navigation system to predict the inertial measurement sensor data after the pre-integration process to obtain an inertial sensor position, an inertial sensor position attitude, and an inertial sensor position speed;

[0050] A Kalman filter unit is used to perform Kalman filtering on the inertial sensor position, the inertial sensor position attitude, the inertial sensor position speed and the double difference observation values ​​in the GPS data to obtain the error-corrected position, the error-corrected posture and the error-corrected posture speed; the double difference observation values ​​include the double difference observation values ​​of the pseudorange and the double difference observation values ​​of the carrier phase; the double difference observation values ​​of the pseudorange and the double difference observation values ​​of the carrier phase are both obtained by station-satellite differential;

[0051] The second posture determination unit is used to determine the error-corrected position, the error-corrected posture and the error-corrected posture speed as the second posture information.

[0052] Optionally, the coordinate conversion module specifically includes:

[0053] A conversion unit is used to perform coordinate conversion on the first posture information based on a seven-parameter model to obtain second posture information based on GPS coordinates.

[0054] Compared with the prior art, the present invention has the following beneficial effects:

[0055] The embodiment of the present invention proposes a method and system for seamless indoor and outdoor positioning and 3D mapping, which integrates lidar data, inertial measurement sensor data and GPS data to achieve performance and scene complementarity. The integration of the three can effectively perform seamless indoor and outdoor positioning, improve positioning accuracy, and meet the needs of high-precision positioning of complex road sections and the establishment of 3D point cloud maps in various scenarios; the graph optimization method and the Kalman filtering method are integrated to fully utilize the characteristics of different data, greatly exert the performance of different data, and reduce the amount of calculation as much as possible. BRIEF DESCRIPTION OF THE DRAWINGS

[0056] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative labor.

[0057] Figure 1 A flow chart of a method for seamless indoor and outdoor positioning and 3D mapping provided by an embodiment of the present invention;

[0058] Figure 2 A flow chart of a GNSS / INS integrated navigation algorithm provided by an embodiment of the present invention;

[0059] Figure 3 A specific implementation block diagram of the indoor and outdoor seamless positioning and 3D mapping method provided by an embodiment of the present invention;

[0060] Figure 4 A structural diagram of the indoor and outdoor seamless positioning and 3D mapping system provided in an embodiment of the present invention. DETAILED DESCRIPTION

[0061] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0062] Laser SLAM technology refers to the process in which an object relies on the laser it carries and the fusion inertial measurement unit (IMU) sensor to achieve its own positioning while building a two-dimensional or three-dimensional map of the surrounding environment. It is an important direction of mobile robot positioning and navigation technology. Laser SLAM has the characteristics of high precision and good robustness. However, there are degradation and other conditions in structured environments. Therefore, most laser SLAM technologies will fuse inertial measurement units, i.e., inertial sensors in this application, to provide initial values ​​of motion in a short time and improve matching effects. This method can effectively alleviate the positioning accuracy and robustness in a short time, but the IMU sensor has zero bias error, and the long-term positioning effect has serious error accumulation. Therefore, this embodiment designs a spatial positioning method that utilizes GPS / IMU and laser SLAM fusion, which can not only effectively solve the problem of long-distance error accumulation of laser SLAM, but also assist GPS in the positioning problem in shielded space, and achieve seamless positioning indoors and outdoors. In addition, the dense 3D point cloud provided by the 3D laser radar can also be used for subsequent 3D high-precision map production.

[0063] The purpose of the embodiment of the present invention is to provide a seamless indoor and outdoor positioning and mapping method and system, combining GPS / IMU with laser SLAM, taking advantage of the high-precision positioning of laser SLAM in a short time, and GPS providing global coordinates without error accumulation. Combining graph optimization with Kalman filtering, making full use of the advantages of GPS, IMU, and 3D LiDAR sensors, real-time seamless indoor and outdoor high-precision positioning and 3D point cloud modeling.

[0064] In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, the present invention is further described in detail below with reference to the accompanying drawings and specific embodiments.

[0065] Figure 1 Flow chart of the indoor and outdoor seamless positioning and 3D mapping method provided by the embodiment of the present invention. Figure 1 The indoor and outdoor seamless positioning and 3D mapping method of this embodiment specifically includes:

[0066] Step 101: Obtain the laser radar data, inertial measurement sensor data and GPS data of the carrier in the target space. The laser radar data is collected by the laser radar (3D Lidar) on the carrier; the inertial measurement sensor data is collected by the inertial sensor (IMU); and the GPS data is the GPS signal received by the GPS receiver.

[0067] Step 102: Preprocess the laser radar data and the inertial measurement sensor data respectively to obtain dedistorted laser radar data and pre-integrated inertial measurement sensor data.

[0068] Step 103: Using a radar inertial odometer (LIO), based on the dedistorted lidar data and the pre-integrated inertial measurement sensor data, a graph-based optimization method is used to obtain the first position pose information of the carrier.

[0069] Step 104: Using a Kalman filter method, calculate the second position information of the carrier based on the GPS data and the pre-integrated inertial measurement sensor data.

[0070] Step 105: Perform coordinate transformation on the first posture information based on the second posture information to obtain second posture information based on GPS coordinates.

[0071] Step 106: Take the first posture information as prediction and the second posture information based on GPS coordinates as observation, and use Kalman filter to perform filtering optimization to obtain filtered posture information. In the blocked area, GPS data will not be observed in real time. Only when the GPS data observation quality is high, Kalman filter is used for filtering optimization to achieve update.

[0072] Step 107: Use the radar inertial odometer to update the filtered posture information to obtain updated posture information.

[0073] Step 108: align the dedistorted LiDAR data according to the updated posture information to generate a three-dimensional point cloud map.

[0074] In this embodiment, the SLAM system composed of 3D lidar and IMU and the GPS positioning system have complementary performance and scenarios. The integration of the two can effectively perform seamless positioning indoors and outdoors, improve positioning accuracy, and meet the needs of high-precision positioning of complex roads in various scenarios and the establishment of 3D point cloud maps.

[0075] In one example, step 102 specifically includes:

[0076] 1) The timestamp and the motion trajectory of the inertial sensor are used to determine the position and posture of the laser radar when collecting each frame of the laser radar data, and the coordinate value of each frame of the laser radar data is obtained.

[0077] 2) According to the time interval of each frame of the laser radar data and the motion posture of the process, coordinate interpolation is performed to calculate the carrier posture of each laser point cloud at the corresponding time, so as to convert the coordinate value of each frame of the laser radar data to the laser radar coordinate system at the starting time of the laser radar data of the frame through external references to obtain the dedistorted laser radar data. Steps 1) and 2) implement dedistortion of the laser radar data using an inertial sensor.

[0078] 3) Pre-integration processing is performed on the inertial sensor data between the key frame lidar data collected by the lidar using a pre-integration processing formula to obtain an inertial sensor data constraint factor; the inertial sensor data constraint factor includes: position, speed and rotation constraints between the lidar key frame lidar data; the key frame lidar data is lidar data in which the movement distance and rotation angle of two adjacent frames of lidar data are greater than the corresponding set threshold value.

[0079] 4) Determine the inertial sensor data constraint factor as the inertial measurement sensor data after pre-integration processing.

[0080] In this example, only the laser key frame data is stored to reduce the amount of calculation and computer storage space. Sometimes the 3D Lidar does not move, or moves a very small distance, which will cause data redundancy and unnecessary calculations. Based on this, the following posture optimization and local mapping process only processes the laser Lidar key frame data.

[0081] The key frame of Lidar is determined based on the distance information and the rotation angle. Movement exceeding 5m or rotation exceeding 10° is considered a key frame.

[0082] The scanning frequency of the laser Lidar is between 10-30HZ, and the frequency of the IMU receiving data is between 100-500. In order to prevent repeated integration and save calculation, it is necessary to integrate the IMU acceleration data and angular velocity data between the radar key frame data, as shown in formula (1), to obtain the position, velocity, and rotation constraints between the laser Lidar key frames, that is, the IMU constraint factor. The formula for the pre-integration process is:

[0083]

[0084] Represents the acceleration and angular velocity measurements measured by the IMU, i and i+1 represent the two moments of the IMU measurement data between the kth key frame and the k+1th key frame, and δt represents the time interval between the two IMU data. They represent acceleration bias and angular velocity bias respectively. Represents the key frame b k The position, velocity and attitude expressed by quaternion in the coordinate system at time i, Represents the key frame b k The position, velocity and quaternion-expressed attitude in the coordinate system at time i+1. The start time of pre-integration between two key frames. is the three-dimensional zero vector, is the unit quaternion, Indicates the conversion of quaternion form into rotation matrix. The measurement model constructed by the pre-integration term, that is, the calculation formula of the inertial sensor data constraint factor is shown in formula (2), where g w Represents the gravity vector g in the world coordinate system w =[0,0,g] T .

[0085]

[0086] in, Represent key frames b k With keyframe b k+1 The position, velocity and angle pre-integrated increments between; is the unit quaternion; g w Represents the gravity vector g in the world coordinate system w =[0,0,g] T , g represents the acceleration due to gravity; Represents the world coordinate system to b k The rotation matrix of the keyframe; Indicates key frame b k Position in the world coordinate system; Indicates key frame b k+1 Position in the world coordinate system; t k Indicates key frame b k With keyframe b k+1 time difference; Indicates key frame b k Velocity in world coordinates; express The inverse matrix of Represents the world coordinate system to key frame b k The quaternion of the rotation; Indicates key frame b k+1The quaternion of the rotation to the world coordinate system; express The inverse matrix of Indicates the acceleration zero bias of the kth frame; Indicates the angular velocity zero bias of the kth frame; represents the acceleration zero bias of the k+1th frame; represents the angular velocity zero bias of the k+1th frame; the inertial sensor data constraint factor is the data after the pre-integration processing.

[0087] In one example, step 103 specifically includes:

[0088] 1) Determine whether the Z-axis coordinate of each frame of laser radar data in the dedistorted laser radar data is less than a preset height threshold. If so, delete the laser radar data less than the preset height threshold to obtain the deleted dedistorted laser radar data.

[0089] Specifically, ground points are removed based on the dedistorted lidar data, that is, a height threshold is set for the Z coordinate. For example, the preset height threshold is set to 0.2, that is, the lidar data (point cloud) smaller than 0.2 meters is deleted, reducing the point cloud data and the amount of calculation, in preparation for increasing the matching speed in the next step.

[0090] 2) Using the deleted dedistorted LiDAR data and the three-dimensional point cloud map generated by the key frame LiDAR data to perform matching based on the Iterative Closest Points (ICP) algorithm to obtain the LiDAR constraint factor; the key frame LiDAR data is the LiDAR data whose movement distance and rotation angle of two adjacent frames of LiDAR data are greater than the corresponding set threshold.

[0091] Specifically, the dedistorted lidar data after deletion is used to extract line features and surface features, and the pose transformation (R, T) (Lidar constraint factor) is obtained based on the distance minimization of the line features and surface features between adjacent key frames. The pose matching of adjacent key frames is used as the initial value, and then matched with the submap to obtain a more accurate pose result, that is, the lidar constraint factor. The collection of line features and surface features of several consecutive key frames constitutes a local submap, which is used for scan-submap matching with the features extracted from the point cloud of the current frame.

[0092] 3) Performing joint optimization calculation based on the laser radar constraint factor and the inertial measurement sensor data after the pre-integration processing to obtain the first posture information.

[0093] Specifically, the first pose is obtained based on the joint optimization calculation of the laser Lidar constraint factor and the IMU constraint factor. First, an abstract error model is established:

[0094]

[0095] r L , r B The Lidar constraint factor and the IMU constraint factor are respectively, represents the pose change between keyframes k and k+1 calculated based on lidar matching, It represents the pose change between key frames k and k+1 calculated based on IMU pre-integration. By adjusting the pose X(R, T) (where R and T represent the pose and position respectively), the overall error is minimized. At this time, X(R, T) is the optimal solution for the first pose.

[0096] In one example, see Figure 2 First, the original data of IMU is obtained, and then the INS navigation prediction is performed. Then, the position, attitude, speed input by INS and the double difference observation values ​​in GPS data are subjected to Kalman filtering to obtain the errors of position, attitude and speed. Finally, the position, attitude and speed output by INS are corrected by obtaining the errors. The double difference observation values ​​include the double difference observation values ​​of pseudorange and the double difference observation values ​​of carrier phase; the double difference observation values ​​of pseudorange and the double difference observation values ​​of carrier phase are both obtained by station-satellite differential.

[0097] Step 104, specifically, is: constructing an RTK / INS tightly integrated navigation system based on the GPS data and the inertial measurement sensor data after the pre-integration processing, so as to obtain the second posture information of the carrier. This step, further, specifically includes: 1) according to the inertial measurement sensor data after the pre-integration processing, using the inertial navigation system to predict and obtain the inertial sensor position, inertial sensor position attitude and inertial sensor position speed. 2) Kalman filtering the inertial sensor position, the inertial sensor position attitude, the inertial sensor position speed and the double difference observation value in the GPS data to obtain the error-corrected position, the error-corrected posture and the error-corrected posture speed. 3) The error-corrected position, the error-corrected posture and the error-corrected posture speed are determined as the second posture information.

[0098] More specifically, the state vector of the RTK / INS tightly integrated navigation system is composed as follows:

[0099]

[0100] In the formula, X INS represents the INS state vector, δp e ,δve and ψ e They are the INS position, velocity and attitude error terms based on the PSI angle error model, and ε are the zero bias generated by the accelerometer and gyroscope respectively. is the ambiguity of the carrier phase double difference observation, and the superscript T represents the transposition.

[0101] The state equation in the RTK / INS integrated navigation system is obtained from the INS error equation and the propagation equation of the GNSS related state vector. According to the INS mechanical arrangement model, the INS error equation of the RTK / INS tight combination is as follows:

[0102]

[0103] In the formula, the superscript represents the corresponding variable differential operation, δ() represents the error disturbance of the corresponding variable, represents the direction cosine matrix from the carrier coordinate system (b system) to the Earth-centered Earth-fixed coordinate system (e system), f b is the specific force in the b system measured by the accelerometer, g is the gravitational acceleration, represents the rotation angular velocity of the e-system relative to the inertial coordinate system (i-system) in the e-system, represents the rotational angular velocity of the b system relative to the i system measured by the gyroscope, w ▽ and w ε They represent the Gaussian white noise of the accelerometer and gyroscope zero bias respectively, and τ is the time interval.

[0104] According to the above error equation, the state prediction equation of the RTK / INS tightly integrated navigation system can be obtained:

[0105]

[0106] Where w is the process noise vector of state update, F INS is the state transfer matrix, which can be expanded as:

[0107]

[0108] The observation equation of RTK / INS tight combination can be expressed as:

[0109]

[0110] in, and represents the double difference observation value of pseudorange and the double difference observation value of carrier phase, and represents the double difference observation value of the pseudorange and the double difference observation value of the carrier phase for INS position prediction, A represents the direction cosine vector between the satellite and the receiver, l b is the arm value from the INS center to the GNSS antenna center in the b system, are the positions of the satellite and GNSS receiver respectively, ρ is the geometric distance between the satellite and the GNSS receiver, and λ is the carrier wavelength of the GNSS signal. Therefore, the observation equation under the RTK / INS tightly integrated navigation system can be expressed as:

[0111]

[0112] Among them, H is the measurement transfer matrix, V is the new information vector, X represents the estimated value of the state vector, and the second posture information to be solved is obtained through Kalman filtering and closed-loop feedback.

[0113] In one example, step 105 specifically includes:

[0114] The first posture information is subjected to coordinate transformation based on the seven-parameter model to obtain a second posture information coordinate frame based on a GPS coordinate frame, that is, to obtain second posture information based on GPS coordinates.

[0115] Specifically, construct the conversion formula as follows:

[0116]

[0117] [X G Y G Z G ] T ,[X L Y L Z L ] T They represent the coordinates of the same location in the GPS coordinate system and the world coordinate system defined by the laser Lidar. G , Y G , Z G They represent the coordinate values ​​of the X-axis, Y-axis, and Z-axis in the GPS coordinate system. L , Y L , Z L They represent the coordinate values ​​of the X-axis, Y-axis, and Z-axis in the world coordinate system defined by the laser Lidar. m represents the scale factor, R represents the conversion relationship from the world coordinate system defined by the laser Lidar to the GPS coordinate system, [ΔX0 ΔY0 ΔZ0] T The coordinates of the origin of the world coordinate system defined by the laser Lidar under GPS. G Y G Z G ]T ,[X L Y L Z L ] T The remaining quantity is unknown and can be calculated through two sets of coordinates of three points. The redundant data can be solved by least squares. The seven parameters are calculated and the first position calculated by Lidar and IMU is converted into the coordinates of GPS coordinate system according to the seven parameters.

[0118] In one example, step 106 and step 107 specifically include:

[0119] The second posture information is used to correct the first posture information to eliminate its cumulative error, and the first posture information and the second posture are fused using the Kalman filter method. The first posture information is used for state prediction, and the second posture information is used to update the first posture, and the filtered result is fed back to the radar inertial odometer to update the first posture information and obtain the updated posture information. Since the first posture information has a high positioning accuracy in a short period of time, it does not need to be updated at a high frequency. It is only updated when the GPS data quality is very good and the radar inertial odometer runs alone for a certain time interval.

[0120] In one example, step 108 specifically includes: performing coordinate transformation on the laser point cloud data of the key frame according to the result of the radar inertial odometer (i.e., updated posture information), and obtaining a three-dimensional point cloud map in the GPS coordinate system (i.e., a three-dimensional point cloud map in the world coordinate system).

[0121] The specific implementation block diagram of the indoor and outdoor seamless positioning and 3D mapping method of this embodiment is as follows Figure 3 As shown, 3DLiDAR / IMU is fused with GPS positioning signals, and the 3D LiDAR and IMU data are fused using a positioning technology based on graph optimization. Real-time sliding window-based optimization is performed and the optimization result is used as a prediction. In addition, the double difference observation values ​​of the pseudorange and the double difference observation values ​​of the carrier phase obtained by station-satellite differential in the GPS data are used as observation data. The prediction and observation data are input into the framework of the Kalman filter to generate a more accurate real-time positioning result. Based on the positioning results, the point cloud is unified into a world coordinate system based on GPS to generate a 3D point cloud map. This method has the following advantages:

[0122] 1) Combine GPS / IMU / 3D LiDAR, take advantage of the high-precision positioning of SLAM in a short time by optimizing the fusion of 3D LiDAR and IMU map, and the advantage of GPS / IMU fusion that the positioning accuracy does not change over time. Use Kalman filtering to fuse these two combinations to achieve seamless high-precision real-time positioning indoors and outdoors.

[0123] 2) GPS laser SLAM technology performs loop detection and eliminates accumulated errors.

[0124] 3) GPS can assist laser SLAM technology in coordinate system conversion, converting the local coordinate system of laser SLAM into a GPS-based coordinate system framework.

[0125] 4) 3D LiDAR / IMU / GPS fusion can achieve real-time seamless high-precision positioning indoors and outdoors while generating 3D point cloud maps.

[0126] 5) The optimization method is combined with the filter to make full use of the characteristics of different sensors, maximize the performance of the sensor, and reduce the amount of calculation as much as possible.

[0127] Based on the above-mentioned indoor and outdoor seamless positioning and 3D mapping method, a device for indoor and outdoor seamless positioning and 3D mapping is implemented, and the device includes:

[0128] The IMU processing unit is located on the outer surface or inside of the carrier, and is used to obtain the IMU data in real time, use the data to dedistort the lidar point cloud data, and perform pre-integration processing on the IMU data.

[0129] The 3D Lidar processing unit is located on the outer surface of the carrier and is used to obtain the laser radar point cloud data in real time, so as to facilitate the matching of the data based on the point cloud features and jointly construct the error equation with the pre-integrated error equation of the IMU for joint optimization to obtain the first position information of the carrier.

[0130] The GPS processing unit is located on the outer surface of the carrier and is used to obtain the satellite navigation data in real time, fuse the IMU data, perform RTK-based GNSS / INS combined navigation positioning, and obtain the second posture information of the carrier; use the second posture information to transform the coordinates of the first posture information and unify them into the GPS coordinate framework.

[0131] After the positioning and mapping units are integrated, the radar inertial odometer is unified into the GPS framework, and the subsequent optimization results are used as the pose prediction value. The second pose information of GPS / INS is used as the observation and put into the Kalman filter framework to obtain the filtered pose. According to the pose optimized by filtering, it is fed back to the lidar inertial odometer, and then predicted and updated based on the Kalman filter with the second pose information. This process is repeated as the system runs, and based on the obtained pose information, the laser point cloud is converted into coordinates to obtain a 3D point cloud map.

[0132] The present invention also provides a seamless indoor and outdoor positioning and 3D mapping system, see Figure 4 , the system comprising:

[0133] The data acquisition module 201 is used to acquire the laser radar data, inertial measurement sensor data and GPS data of the carrier in the target space.

[0134] The preprocessing module 202 is used to perform data preprocessing on the laser radar data and the inertial measurement sensor data respectively to obtain dedistorted laser radar data and pre-integrated inertial measurement sensor data.

[0135] The first pose calculation module 203 is used to use a radar inertial odometer to obtain the first pose information of the carrier based on the dedistorted lidar data and the inertial measurement sensor data after the pre-integration processing by adopting a graph optimization-based method.

[0136] The second posture calculation module 204 is used to calculate the second posture information of the carrier based on the GPS data and the pre-integrated inertial measurement sensor data by using a Kalman filter method.

[0137] The coordinate conversion module 205 is used to perform coordinate conversion on the first posture information based on the second posture information to obtain second posture information based on GPS coordinates.

[0138] The filtering module 206 uses the Kalman filter to perform filtering optimization by taking the first posture information as a prediction and the second posture information based on the GPS coordinates as an observation, so as to obtain the filtered posture information.

[0139] The posture updating module 207 is used to update the filtered posture information using the radar inertial odometer to obtain updated posture information.

[0140] The registration module 208 is used to register the dedistorted LiDAR data according to the updated posture information to generate a three-dimensional point cloud map.

[0141] In one example, the preprocessing module 202 specifically includes:

[0142] A coordinate value determination unit is used to determine the position and posture of the laser radar when collecting each frame of the laser radar data by using a timestamp and a motion trajectory of an inertial sensor, and obtain the coordinate value of each frame of the laser radar data; the laser radar data is collected by using the laser radar; the inertial measurement sensor data is collected by using the inertial sensor.

[0143] The dedistortion unit is used to convert the coordinate value of each frame of the laser radar data into the laser radar coordinate system at the starting time of the laser radar data in the frame through external references to obtain dedistorted laser radar data.

[0144] A pre-integration processing unit is used to perform pre-integration processing on the inertial sensor data between the key frame lidar data collected by the lidar using a pre-integration processing formula to obtain an inertial sensor data constraint factor; the inertial sensor data constraint factor includes: position, speed and rotation constraints between the lidar key frame lidar data; the key frame lidar data is the lidar data in which the movement distance and rotation angle of two adjacent frames of lidar data are greater than the corresponding set threshold value.

[0145] The pre-integration processed data determining unit is used to determine the inertial sensor data constraint factor as the pre-integration processed inertial measurement sensor data.

[0146] In one example, the first posture calculation module 203 specifically includes:

[0147] The data screening unit is used to determine whether the Z-axis coordinate of each frame of laser radar data in the dedistorted laser radar data is less than a preset height threshold. If so, the laser radar data less than the preset height threshold is deleted to obtain the deleted dedistorted laser radar data.

[0148] A matching unit is used to perform matching based on a nearest point iteration algorithm using the deleted dedistorted lidar data and a three-dimensional point cloud map generated by key frame lidar data to obtain a lidar constraint factor; the key frame lidar data is lidar data in which the movement distance and rotation angle of two adjacent frames of lidar data are both greater than a corresponding set threshold.

[0149] A joint optimization unit is used to perform a joint optimization calculation based on the laser radar constraint factor and the inertial measurement sensor data after the pre-integration processing to obtain the first posture information.

[0150] In one example, the second posture calculation module 204 specifically includes:

[0151] The data prediction unit is used to predict the inertial measurement sensor data after the pre-integration process using an inertial navigation system to obtain the inertial sensor position, inertial sensor position attitude and inertial sensor position speed.

[0152] The Kalman filter unit is used to perform Kalman filtering on the inertial sensor position, the inertial sensor position attitude, the inertial sensor position speed and the double difference observation values ​​in the GPS data to obtain the error-corrected position, the error-corrected posture and the error-corrected posture speed. The double difference observation values ​​include the double difference observation values ​​of the pseudorange and the double difference observation values ​​of the carrier phase; the double difference observation values ​​of the pseudorange and the double difference observation values ​​of the carrier phase are both obtained by station-satellite differential.

[0153] The second posture determination unit is used to determine the error-corrected position, the error-corrected posture and the error-corrected posture speed as the second posture information.

[0154] In one example, the coordinate conversion module 205 specifically includes:

[0155] A conversion unit is used to perform coordinate conversion on the first posture information based on a seven-parameter model to obtain second posture information based on GPS coordinates.

[0156] The indoor and outdoor seamless positioning and 3D mapping system of this embodiment combines 3D Lidar, IMU, and GPS, takes advantage of the high-precision positioning of the laser inertial odometer in a short time, and the advantage of the positioning accuracy of the GNSS / INS combination that does not change with time, and uses graph optimization to fuse the lidar constraint factor and the IMU pre-integration constraint factor to achieve high real-time positioning accuracy in a short time. The combined navigation technology based on the fusion of RTK and IMU is used to achieve high-precision positioning that does not change with time, such as outdoors. The Kalman filter method is used to carefully integrate the radar inertial odometer and the combined navigation and positioning technology based on RTK to achieve long-distance seamless indoor and outdoor positioning, based on the positioning results. In addition, the laser radar point cloud data is converted to the GPS coordinate system to generate a 3D point cloud map.

[0157] In this specification, each embodiment is described in a progressive manner, and each embodiment focuses on the differences from other embodiments. The same or similar parts between the embodiments can be referred to each other. For the system disclosed in the embodiment, since it corresponds to the method disclosed in the embodiment, the description is relatively simple, and the relevant parts can be referred to the method part.

[0158] The principles and implementation methods of the present invention are described in this article using specific examples. The description of the above embodiments is only used to help understand the method and core idea of ​​the present invention. At the same time, for those skilled in the art, according to the idea of ​​the present invention, there will be changes in the specific implementation methods and application scope. In summary, the content of this specification should not be understood as limiting the present invention.

Claims

1. A method for seamless indoor and outdoor positioning and 3D mapping, characterized in that: include: Obtain lidar data, inertial measurement sensor data and GPS data of the carrier in the target space; Preprocessing the laser radar data and the inertial measurement sensor data respectively to obtain dedistorted laser radar data and pre-integrated inertial measurement sensor data; Using a radar inertial odometer, based on the dedistorted laser radar data and the pre-integrated inertial measurement sensor data, a graph-based optimization method is used to obtain the first position information of the carrier; Using a Kalman filter method, the second position information of the carrier is calculated based on the GPS data and the pre-integrated inertial measurement sensor data; Performing coordinate transformation on the first posture information based on the second posture information to obtain second posture information based on GPS coordinates; Taking the first posture information as prediction and the second posture information based on GPS coordinates as observation, performing filtering optimization using a Kalman filter to obtain filtered posture information; Using the radar inertial odometer to update the filtered posture information to obtain updated posture information; Registering the dedistorted LiDAR data according to the updated posture information to generate a three-dimensional point cloud map; The Kalman filtering method is used to calculate the second posture information of the carrier based on the GPS data and the inertial measurement sensor data after the pre-integration processing, specifically including: According to the inertial measurement sensor data after the pre-integration processing, an inertial navigation system is used to perform prediction to obtain an inertial sensor position, an inertial sensor position attitude and an inertial sensor position speed; Performing Kalman filtering on the inertial sensor position, the inertial sensor position attitude, the inertial sensor position speed, and the GPS position and GPS speed obtained by carrier phase difference technology in the GPS data to obtain an error-corrected position, an error-corrected posture, and an error-corrected posture speed; The error-corrected position, the error-corrected posture, and the error-corrected posture speed are determined as the second posture information.

2. The method for seamless indoor and outdoor positioning and 3D mapping according to claim 1, characterized in that: The preprocessing of the laser radar data and the inertial measurement sensor data respectively to obtain the dedistorted laser radar data and the inertial measurement sensor data after pre-integration processing specifically includes: The position and posture of the laser radar when collecting each frame of the laser radar data are determined by using the timestamp and the motion trajectory of the inertial sensor to obtain the coordinate value of each frame of the laser radar data; the laser radar data is collected by using the laser radar; the inertial measurement sensor data is collected by using the inertial sensor; The coordinate value of each frame of the laser radar data is converted into the laser radar coordinate system at the starting time of the laser radar data of the frame by using an external reference to obtain dedistorted laser radar data; The inertial sensor data between the key frame laser radar data collected by the laser radar is pre-integrated by using a pre-integration processing formula to obtain an inertial sensor data constraint factor; the inertial sensor data constraint factor includes: position, speed and rotation constraints between the laser radar key frame laser radar data; the key frame laser radar data is laser radar data whose movement distance and rotation angle of two adjacent frames of laser radar data are greater than corresponding set thresholds; The inertial sensor data constraint factor is determined as the inertial measurement sensor data after pre-integration processing.

3. The method for seamless indoor and outdoor positioning and 3D mapping according to claim 1, characterized in that: The radar inertial odometer is used to obtain the first position information of the carrier in a graph-based optimization manner based on the dedistorted laser radar data and the pre-integrated inertial measurement sensor data, specifically including: Determine whether the Z-axis coordinate of each frame of laser radar data in the dedistorted laser radar data is less than a preset height threshold, and if so, delete the laser radar data less than the preset height threshold to obtain the deleted dedistorted laser radar data; The three-dimensional point cloud map generated by the deleted dedistorted laser radar data and the key frame laser radar data is matched based on the nearest point iteration algorithm to obtain the laser radar constraint factor; the key frame laser radar data is the laser radar data whose movement distance and rotation angle of two adjacent frames of laser radar data are greater than the corresponding set threshold value; The first posture information is obtained by performing a joint optimization calculation based on the laser radar constraint factor and the inertial measurement sensor data after the pre-integration processing.

4. The method for seamless indoor and outdoor positioning and 3D mapping according to claim 1, characterized in that: The step of performing coordinate transformation on the first posture information based on the second posture information to obtain second posture information based on GPS coordinates specifically includes: The first posture information is subjected to coordinate transformation based on a seven-parameter model to obtain second posture information based on GPS coordinates.

5. A seamless indoor and outdoor positioning and 3D mapping system, characterized in that: include: A data acquisition module, used to acquire the laser radar data, inertial measurement sensor data and GPS data of the carrier in the target space; A preprocessing module, used to perform data preprocessing on the laser radar data and the inertial measurement sensor data respectively to obtain dedistorted laser radar data and pre-integrated inertial measurement sensor data; A first pose calculation module, configured to obtain the first pose information of the carrier by using a radar inertial odometer based on the dedistorted laser radar data and the inertial measurement sensor data after the pre-integration processing, using a graph optimization-based method; A second posture calculation module, used for calculating the second posture information of the carrier based on the GPS data and the inertial measurement sensor data after the pre-integration processing by adopting a Kalman filter method; A coordinate conversion module, used for performing coordinate transformation on the first posture information based on the second posture information to obtain second posture information based on GPS coordinates; A filtering module, using a card to take the first posture information as a prediction, taking the second posture information based on the GPS coordinates as an observation, and using a Kalman filter to perform filtering optimization to obtain filtered posture information; A posture updating module, used to update the filtered posture information using the radar inertial odometer to obtain updated posture information; A registration module, used to register the dedistorted LiDAR data according to the updated posture information to generate a three-dimensional point cloud map; The second posture calculation module specifically includes: A data prediction unit, configured to use an inertial navigation system to predict the inertial measurement sensor data after the pre-integration process to obtain an inertial sensor position, an inertial sensor position attitude, and an inertial sensor position speed; A Kalman filter unit is used to perform Kalman filtering on the inertial sensor position, the inertial sensor position attitude, the inertial sensor position speed, and the GPS position and GPS speed obtained by the carrier phase difference technology in the GPS data to obtain an error-corrected position, an error-corrected posture, and an error-corrected posture speed; The second posture determination unit is used to determine the error-corrected position, the error-corrected posture and the error-corrected posture speed as the second posture information.

6. The indoor and outdoor seamless positioning and 3D mapping system according to claim 5, characterized in that: The preprocessing module specifically includes: A coordinate value determination unit, used to determine the position and posture of the laser radar when collecting each frame of the laser radar data by using the timestamp and the motion trajectory of the inertial sensor, and obtain the coordinate value of each frame of the laser radar data; the laser radar data is collected by using the laser radar; the inertial measurement sensor data is collected by using the inertial sensor; A dedistortion unit, used to convert the coordinate value of each frame of the laser radar data into the laser radar coordinate system at the starting time of the laser radar data of the frame through an external reference, so as to obtain dedistorted laser radar data; A pre-integration processing unit is used to perform pre-integration processing on the inertial sensor data between the key frame laser radar data collected by the laser radar using a pre-integration processing formula to obtain an inertial sensor data constraint factor; the inertial sensor data constraint factor includes: position, speed and rotation constraints between the laser radar key frame laser radar data; the key frame laser radar data is laser radar data in which the movement distance and rotation angle of two adjacent frames of laser radar data are greater than corresponding set thresholds; The pre-integration processed data determining unit is used to determine the inertial sensor data constraint factor as the pre-integration processed inertial measurement sensor data.

7. The indoor and outdoor seamless positioning and 3D mapping system according to claim 5, characterized in that: The first posture calculation module specifically includes: A data screening unit, used to determine whether the Z-axis coordinate of each frame of laser radar data in the dedistorted laser radar data is less than a preset height threshold, and if so, delete the laser radar data less than the preset height threshold to obtain the deleted dedistorted laser radar data; A matching unit is used to perform matching based on a nearest point iteration algorithm using the deleted dedistorted laser radar data and a three-dimensional point cloud map generated by key frame laser radar data to obtain a laser radar constraint factor; the key frame laser radar data is laser radar data in which the movement distance and rotation angle of two adjacent frames of laser radar data are both greater than corresponding set thresholds; A joint optimization unit is used to perform a joint optimization calculation based on the laser radar constraint factor and the inertial measurement sensor data after the pre-integration processing to obtain the first posture information.

8. The indoor and outdoor seamless positioning and 3D mapping system according to claim 5, characterized in that: The coordinate conversion module specifically includes: A conversion unit is used to perform coordinate conversion on the first posture information based on a seven-parameter model to obtain second posture information based on GPS coordinates.

Images