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

Abstract

The invention discloses an indoor and outdoor positioning and mapping method and system based on multi-sensor fusion, and relates to the field of multi-sensor fusion positioning, and the method comprises the steps: obtaining inertial sensor data, laser radar data, global navigation satellite data and ultra-wideband data of a carrier in a target space; judging indoor and outdoor states of the carrier according to the global navigation satellite data and the ultra-wideband data; if the carrier is located outdoors, positioning and mapping are carried out based on global navigation satellite data, inertial sensor data and laser radar data; and if the carrier is located indoors, positioning and mapping are carried out based on inertial sensor data, laser radar data and ultra-wideband data. According to the invention, indoor and outdoor seamless high-precision positioning and mapping can be carried out in real time, and the precision and robustness of positioning and mapping are greatly improved.

Description

Technical Field

[0001] The present application relates to the field of multi-sensor fusion positioning, and in particular to an indoor and outdoor positioning and mapping method and system based on multi-sensor fusion. Background Art

[0002] The successful construction and application of Beidou satellite navigation has brought outdoor positioning and navigation technology to a new level. Outdoors, high positioning results can be achieved through Beidou multi-satellite positioning technology, but the accuracy of this positioning method is easily affected by the interference of surrounding buildings. In addition, the positioning accuracy of related positioning methods will drop significantly when they are operated for a long time in a single indoor structure environment. Summary of the invention

[0003] The purpose of this application is to provide an indoor and outdoor positioning and mapping method and system based on multi-sensor fusion, which can perform seamless high-precision positioning and mapping indoors and outdoors in real time, and improve the accuracy and robustness of positioning and mapping.

[0004] To achieve the above objectives, this application provides the following solutions:

[0005] In a first aspect, the present application provides an indoor and outdoor positioning and mapping method based on multi-sensor fusion, comprising:

[0006] Acquire inertial sensor data, lidar data, global navigation satellite data, and ultra-wideband data of the carrier in the target space;

[0007] Constructing an inertial sensor constraint factor and a lidar constraint factor based on the inertial sensor data and the lidar data;

[0008] Determining the indoor and outdoor status of the carrier according to the global navigation satellite data and the ultra-wideband data;

[0009] If the carrier is located outdoors, constructing a Doppler data constraint factor, a carrier phase constraint factor, a code measurement pseudorange constraint factor and a clock error constraint factor based on the global navigation satellite data;

[0010] Based on the sliding window optimization method, the inertial sensor constraint factor, the laser radar constraint factor, the Doppler data constraint factor, the carrier phase constraint factor, the code measurement pseudorange constraint factor and the clock error constraint factor are jointly optimized to obtain the positioning information of the carrier;

[0011] If the carrier is located indoors, the inertial sensor constraint factor and the lidar constraint factor are jointly optimized based on the sliding window optimization method, and loop detection is performed based on the ultra-wideband data to eliminate the accumulated error during the indoor operation of the carrier and obtain the positioning information of the carrier;

[0012] A three-dimensional point cloud map of the target space is generated according to the positioning information of the carrier and the laser radar data.

[0013] In the second aspect, the present application provides an indoor and outdoor positioning and mapping system based on multi-sensor fusion, including:

[0014] A data acquisition module, used to acquire inertial sensor data, lidar data, global navigation satellite data and ultra-wideband data of the carrier in the target space;

[0015] A first constraint factor construction module, used to construct an inertial sensor constraint factor and a laser radar constraint factor based on the inertial sensor data and the laser radar data;

[0016] A carrier state determination module, used to determine the indoor and outdoor state of a carrier according to the global navigation satellite data and the ultra-wideband data;

[0017] A second constraint factor construction module is used to construct a Doppler data constraint factor, a carrier phase constraint factor, a code measurement pseudorange constraint factor and a clock error constraint factor based on the global navigation satellite data when the carrier is located outdoors;

[0018] An outdoor optimization module is used to jointly optimize the inertial sensor constraint factor, the laser radar constraint factor, the Doppler data constraint factor, the carrier phase constraint factor, the code measurement pseudorange constraint factor and the clock error constraint factor based on a sliding window optimization method to obtain positioning information of the carrier;

[0019] An indoor optimization module, used for jointly optimizing the inertial sensor constraint factor and the lidar constraint factor based on a sliding window optimization method when the carrier is located indoors, and performing loop detection based on the ultra-wideband data to eliminate the accumulated error during the indoor operation of the carrier, and obtain the positioning information of the carrier;

[0020] A mapping module is used to generate a three-dimensional point cloud map of the target space based on the positioning information of the carrier and the laser radar data.

[0021] According to the specific embodiments provided in this application, this application has the following technical effects:

[0022] The present application provides an indoor and outdoor positioning and mapping method and system based on multi-sensor fusion, which combines inertial sensor data, lidar data, global navigation satellite data and ultra-wideband data, and judges the indoor and outdoor status of the carrier according to the global navigation satellite data and ultra-wideband data. When outdoors, the global navigation satellite data, inertial sensor data and lidar data are integrated for positioning and mapping, and when indoors, the inertial sensor data, lidar data and ultra-wideband data are integrated for positioning and mapping. High-precision positioning and mapping can be performed seamlessly indoors and outdoors in real time, which greatly improves the accuracy and robustness of positioning and mapping. BRIEF DESCRIPTION OF THE DRAWINGS

[0023] In order to more clearly illustrate the embodiments of the present application 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 application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0024] Figure 1 This is an application environment diagram of an indoor and outdoor positioning and mapping method based on multi-sensor fusion in an embodiment of the present application;

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

[0026] Figure 3 A schematic diagram of the functional modules of an indoor and outdoor positioning and mapping system based on multi-sensor fusion provided in one embodiment of the present application. DETAILED DESCRIPTION

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

[0028] First, some technical terms involved in the embodiments of the present application are introduced.

[0029] Simultaneous Localization and Mapping (SLAM) technology refers to the carrier relying on its own laser Lidar sensor to determine its own position and perceive the environment in real time. However, laser SLAM technology has certain limitations in application. For example, long-term operation in a single-structure environment such as a long corridor will continuously accumulate positioning errors and cause loop detection failures, resulting in a significant decrease in positioning accuracy during long-term operation.

[0030] UltraWide Band (UWB) technology can provide centimeter-level high-precision positioning capabilities by deploying base stations indoors. At the same time, it also has strong anti-multipath capabilities. Signals arriving from different directions are unlikely to have superimposed interference and there is no cumulative error. It is a commonly used high-precision positioning method indoors.

[0031] The Global Navigation Satellite System (GNSS) technology can only provide position coordinates normally outdoors. Indoors, the carrier cannot obtain enough satellite signals and the GNSS positioning method will fail.

[0032] When the structure of indoor buildings is simple, simply using laser SLAM for positioning and mapping will also cause large error accumulation. Therefore, this application provides a new positioning method, which can improve the accuracy of indoor and outdoor positioning and mapping by using different sensor combinations by judging the indoor and outdoor status of the carrier.

[0033] In order to make the above-mentioned objects, features and advantages of the present application more obvious and easy to understand, the present application is further described in detail below in conjunction with the accompanying drawings and specific implementation methods.

[0034] The indoor and outdoor positioning and mapping method based on multi-sensor fusion provided in the embodiment of the present application can be applied to Figure 1In the application environment shown. Among them, the terminal 102 communicates with the server 104 through the network. The data storage system can store the data that the server 104 needs to process. The data storage system can be set up separately, integrated on the server 104, or placed on the cloud or other servers. The terminal 102 can send inertial sensor data, lidar data, global navigation satellite data and ultra-wideband data to be processed to the server 104. After the server 104 receives the inertial sensor data, lidar data, global navigation satellite data and ultra-wideband data to be processed, it performs indoor and outdoor positioning of the carrier and generates a three-dimensional point cloud map of the target space. The server 104 can feedback the obtained positioning information and three-dimensional point cloud map to the terminal 102. In addition, in some embodiments, the indoor and outdoor positioning and mapping method based on multi-sensor fusion can also be implemented by the server 104 or the terminal 102 alone.

[0035] The terminal 102 may be, but is not limited to, various desktop computers, laptop computers, smart phones, tablet computers, IoT devices, and portable wearable devices. The IoT devices may be smart speakers, smart TVs, smart air conditioners, smart vehicle-mounted devices, etc. The portable wearable devices may be smart watches, smart bracelets, head-mounted devices, etc. The server 104 may be implemented as an independent server or a server cluster consisting of multiple servers, or may be a cloud server.

[0036] In an exemplary embodiment, Figure 2 As shown, a method for indoor and outdoor positioning and mapping based on multi-sensor fusion is provided. The method is executed by a computer device, and can be executed by a computer device such as a terminal or a server alone, or by a terminal and a server together. In the embodiment of the present application, the method is applied to Figure 1 The server 104 in the example is used for explanation, and the steps include the following steps 201 to 207.

[0037] Step 201, acquiring inertial sensor data, lidar data, global navigation satellite data and ultra-wideband data of a carrier in a target space.

[0038] In an exemplary embodiment, inertial sensor data is collected by an inertial measurement unit (IMU) on the carrier. Lidar data is collected by a 3D lidar on the carrier. Pseudorange, carrier phase and Doppler signals are received by a GNSS receiver to obtain global navigation satellite data. Ultra-wideband data is collected by a UWB signal receiver.

[0039] Between step 201 and step 202, it also includes the steps of obtaining conversion parameters of the geodetic coordinate system, the world coordinate system defined by the lidar and the UWB system coordinate system based on the coordinates of the initial point using a seven-parameter model, and converting the point coordinates in the UWB system coordinate system to the world coordinate system defined by the lidar based on the conversion parameters, and converting the point coordinates in the world coordinate system defined by the lidar to the geodetic coordinate system.

[0040] When outdoors, select the moment when the carrier starts to move as the origin of the world coordinate system defined by the lidar and the GNSS coordinate system. Select the coordinates of the carrier in the geodetic coordinate system represented by the next five consecutive frames of GNSS data, as well as the coordinates in the world coordinate system defined by the lidar that match these five frames of data. According to the coordinates of the carrier in the geodetic coordinate system represented by the GNSS data and the coordinates in the world coordinate system defined by the lidar, based on the above five points, calculate the rotation matrix r1 and translation matrix t1 between the geodetic coordinate system and the world coordinate system defined by the lidar according to the seven-parameter model. Then, according to the rotation matrix and the translation matrix, convert the coordinates of all points in the lidar world coordinate system to the geodetic coordinate system.

[0041] In an exemplary embodiment, by selecting the common point of the geodetic coordinate system and the world coordinate system defined by the laser radar, a seven-parameter model is used to solve the conversion parameters between the geodetic coordinate system and the world coordinate system defined by the laser radar. Among them, the geodetic coordinate system refers to the origin O (0,0,0) as the center of mass of the earth, the Z axis is parallel to the earth's axis and points to the North Pole, the X axis points to the intersection of the prime meridian and the equator, and the Y axis is perpendicular to the XOZ plane to form a right-handed coordinate system. The world coordinate system defined by the laser radar refers to a right-handed coordinate system with the origin at the point when the map is initialized, the X axis points forward, the Z axis is parallel to the direction of gravity, and the Y axis is perpendicular to the XOZ plane.

[0042] The seven-parameter model used in outdoor conditions is:

[0043]

[0044] Among them, [X W Y W Z W ] T and [X L Y L Z L ] T Respectively represent the coordinates of the same point in the geodetic coordinate system and the world coordinate system defined by the lidar, m represents the scale factor, R1 represents the conversion relationship from the world coordinate system defined by the lidar to the geodetic coordinate system, including r1 and t1, [ΔX0 ΔY0 ΔZ0] T Represents the coordinates of the origin of the world coordinate system defined by the lidar in the geodetic coordinate system.

[0045] In the seven-parameter model [X W Y W Z W ] T and [X L Y L Z L ] T The remainder is known and unknown. The remainder can be calculated by selecting five common points in the two coordinate systems, and the redundant data can be used to construct a least squares solution.

[0046] When indoors, the coordinates of the first point when the carrier starts moving are selected as the origin of the world coordinate system defined by the lidar and the UWB system coordinate system. Then, a point is selected every second to determine the coordinates of the point in the world coordinate system defined by the lidar and the UWB system coordinate system. A total of five points are collected. Based on the coordinates of the above five points, the seven-parameter model is used to calculate the rotation matrix r2 and translation matrix t2 between the world coordinate system coordinates defined by the lidar and the UWB system coordinates. According to r2 and t2, the point coordinates in the UWB system coordinates are transferred to the world coordinate system defined by the lidar, and then converted to the geodetic coordinate system according to r1 and t1, so as to realize the unity of the coordinate system of each point indoors and outdoors.

[0047] Among them, the UWB system coordinate system refers to the origin located at the corner of the wall or a clearly marked node, the X-axis is consistent with the extension direction of the wall, the Y-axis is perpendicular to the X-axis, and the Z-axis is perpendicular to the XOY plane to form a right-handed coordinate system.

[0048] The seven-parameter model used in indoor conditions is:

[0049]

[0050] Among them, [X L Y L Z L ] T and [X U Y U Z U ] T Respectively represent the coordinates of the same point in the world coordinate system defined by the lidar and the UWB system coordinate system. R2 represents the conversion relationship from the UWB system coordinate system to the world coordinate system defined by the lidar, including r2 and t2, [ΔX0 ΔY0 ΔZ0] T Represents the coordinates of the UWB system coordinate system at the origin of the world coordinate system defined by the lidar.

[0051] In the seven-parameter model [X L Y L Z L ] Tand [X U Y U Z U ] T The remainder is known and unknown. The remainder can be calculated by selecting five common points in the two coordinate systems, and the redundant data can be used to construct a least squares solution.

[0052] Step 202: construct an inertial sensor constraint factor and a lidar constraint factor based on the inertial sensor data and the lidar data.

[0053] In an exemplary embodiment, step 202 includes the following steps 2021 to 2024 .

[0054] Step 2021, using a pre-integration processing formula, pre-integrate the inertial sensor data between the key frame lidar data to obtain the inertial sensor constraint factor.

[0055] 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 both greater than the corresponding set thresholds. Specifically, the laser radar movement exceeding 5m or the rotation exceeding 10° is considered a key frame.

[0056] The inertial sensor constraint factors include: position, velocity and rotation constraints between key frame lidar data.

[0057] The formula for the inertial sensor data constraint factor is:

[0058]

[0059] in, is the three-dimensional zero vector of the position, is the three-dimensional zero vector of velocity, is the unit quaternion, g w Represents the gravity vector in the world coordinate system, g w =[0,0,g] T , g is gravity, Represents the world coordinate system to key frame b k The rotation matrix of Indicates key frame b k The position in the world coordinate system, Δt k Indicates key frame b k With keyframe b k+1 The time difference, Indicates key frame b k The velocity in the world coordinate system, yes The inverse matrix of key frame b k The quaternion of the rotation to the world coordinate system, Indicates key frame b k+1 The quaternion of the rotation to the world coordinate system, Indicates key frame b k The acceleration zero bias; Indicates key frame b k The angular velocity is zero bias.

[0060] Step 2022: Dedistort the laser radar data using the inertial sensor data to obtain dedistorted laser radar data.

[0061] In an exemplary embodiment, step 2022 specifically includes:

[0062] Using the timestamp and the motion trajectory of the inertial sensor, determine the displacement and velocity of each frame of IMU data in the world coordinate system defined by the lidar. Perform linear interpolation based on the obtained displacement and velocity. Assume that for the laser point cloud at time t, the nearest IMU data point t before and after it can be k-1 and t k Do linear interpolation between them to get the displacement and velocity at time t:

[0063]

[0064]

[0065] Among them, V(t) is the velocity obtained by interpolation, P(t) is the displacement obtained by interpolation, V k is the velocity of the IMU data point tk, V k-1 is the IMU data point t k-1 The speed, P k is the MU data point t k The displacement, P k-1 is the IMU data point t k-1 displacement.

[0066] Using the angular velocity information of the IMU, the rotation matrix R(t) from the start scanning time to the acquisition time of the laser point cloud is integrated. Using the velocity and displacement obtained by interpolation, the displacement vector ΔP(t) of the laser point acquisition time relative to the start time is determined to construct the change compensation matrix

[0067] Finally, the compensation change matrix is ​​multiplied by the original lidar point coordinates to obtain the dedistorted lidar data.

[0068] Step 2023, generating a local three-dimensional point cloud map based on the dedistorted lidar data and the key frame lidar data.

[0069] Step 2024: Use the closest point iteration algorithm to match the local three-dimensional point cloud map to obtain a laser radar constraint factor. The laser radar constraint factor is a relative transformation relationship of the posture.

[0070] Specifically, the dedistorted LiDAR data is used to extract line features and surface features, and the line features and surface features between adjacent key frames are minimized based on the distance to obtain the relative transformation relationship (R, T) of the pose. R represents the rotation matrix and T represents the translation matrix. The collection of line features and surface features of several consecutive key frames constitutes a local three-dimensional point cloud map. The pose matching of adjacent key frames is used as the initial value, and the nearest point selection algorithm is used to match the local three-dimensional point cloud map to obtain a higher-precision relative transformation relationship of the pose, that is, the LiDAR constraint factor.

[0071] Step 203: judging the indoor and outdoor status of the carrier according to the global navigation satellite data and the ultra-wideband data.

[0072] In an exemplary embodiment, step 203 includes the following steps 2031 to 2034 .

[0073] Step 2031, converting the global navigation satellite data from the time domain to the frequency domain, and determining the number of visible satellites according to the number of pulse peaks in the frequency domain.

[0074] When capturing satellite signals, the satellite signals are converted from the time domain to the frequency domain, each satellite signal is detected to see if there is an obvious pulse peak, and the number of pulse peaks in each received GNSS data is recorded. The number of pulse peaks represents the number of visible satellites.

[0075] Set the outdoor determination threshold for determining the outdoor state and the indoor determination threshold for determining the indoor state. The outdoor determination threshold is preferably set to 8, and the indoor determination threshold is preferably set to 2. The specific situation is set according to the actual application.

[0076] Step 2032: If the number of visible satellites is greater than the set outdoor determination threshold, it is determined that the carrier is outdoors.

[0077] Step 2033: If the number of visible satellites is less than the indoor determination threshold, the indoor and outdoor status of the carrier is determined based on the ultra-wideband data and the data stored in the cloud server. The cloud server stores the coordinates of the indoor and outdoor boundary points and the coordinates of the outdoor obstructions.

[0078] Specifically, according to the ultra-wideband data, the carrier coordinates are determined, and it is determined whether the ultra-wideband signal can be received. If the ultra-wideband signal can be received, it is determined that the carrier is indoors, and the carrier coordinates are uploaded to the cloud server for storage as the boundary between indoors and outdoors. If the ultra-wideband signal cannot be received, it is determined whether the carrier coordinates are stored in the cloud server. If so, it is determined that the carrier is indoors, otherwise it is determined that the carrier is outdoors.

[0079] In an exemplary embodiment, if the number of visible satellites is less than the indoor determination threshold, if the UWB signal can be received and the coordinates of the point marked on the cloud server are at the boundary between indoors and outdoors, the carrier is considered to be indoors. If the UWB signal cannot be received, but the coordinates of the point marked on the cloud server are at the boundary between indoors and outdoors, the carrier is also considered to be indoors. If the UWB signal can be received but the cloud server does not have the position coordinates of the point, the carrier is also considered to be indoors, and the coordinates of the point are uploaded to the cloud server. In other cases, the carrier is considered to be still outdoors.

[0080] Step 2034: If the number of visible satellites is less than or equal to the outdoor determination threshold and greater than or equal to the indoor determination threshold, the average number of visible satellites within the continuous set time period is determined based on the global navigation satellite data within the continuous set time period. If the average number of visible satellites within the continuous set time period is less than the indoor determination threshold, it is determined that the carrier is indoors; otherwise, it is determined that the carrier is outdoors.

[0081] In an exemplary embodiment, when the number of visible satellites is between 2 and 8, GNSS data is collected for the next 10 seconds, and the average value of the number of visible satellites within ten seconds is calculated. If the average value is less than the indoor judgment threshold, the carrier is considered to be in an indoor environment. If the average value is not less than the indoor judgment threshold, it is determined that the observation conditions of the carrier at the current moment are not good, but it is still in an outdoor environment.

[0082] Furthermore, step 203 also includes the following steps 2035 and 2036.

[0083] Step 2035, judging whether the carrier is in a state of transition between indoors and outdoors or passing through an outdoor obstruction according to the number of visible satellites and the ultra-wideband data.

[0084] Step 2036: If the carrier is in a state of transition between indoor and outdoor or passes through an outdoor obstruction, the coordinates of the indoor and outdoor boundary point or the outdoor obstruction in geodetic coordinates are uploaded to the cloud server for storage.

[0085] Specifically, when it is determined that the environmental state of the carrier has changed between indoors and outdoors, the coordinate point at the time of change is determined as the boundary point between indoors and outdoors, and the coordinate position of the point in the geodetic coordinate system is uploaded to the cloud server through the communication device. When uploading, the newly uploaded coordinates are first compared with the existing coordinates on the cloud server. If the cloud server already has the coordinates of the point, it can further assist in confirming the position status of the carrier. If the cloud server does not have the coordinates of the point, the newly uploaded coordinates are stored on the cloud server, so that when the next GNSS and UWB data are temporarily invalid, the coordinates stored on the cloud server can be used to assist in judging the indoor and outdoor status of the carrier. By uploading different coordinates to the cloud server through multiple carriers, the cloud server will store more and more coordinates, and its judgment of the carrier's position will become more and more accurate, realizing the process of automatic learning.

[0086] Step 204: If the carrier is located outdoors, construct a Doppler data constraint factor, a carrier phase constraint factor, a code measurement pseudorange constraint factor and a clock error constraint factor based on the global navigation satellite data.

[0087] (1) Doppler data constraint factor:

[0088] The Doppler shift reflects the relative motion of the receiver and the satellite along the signal propagation path. It can be measured by the difference between the received carrier signal and the designed signal. Its measurement accuracy is usually one order of magnitude higher than that of the code pseudorange. The Doppler shift is modeled as:

[0089]

[0090] in, represents the Doppler frequency shift, V s E Represents the coordinates of the receiver in the Earth-Centered Inertial coordinate system (ECI) coordinate frame. represents the coordinates of the satellite in the ECI coordinate frame, λ represents the wavelength of the carrier signal, is the unit vector from the receiver to the satellite in the ECI frame, represents the time deviation between the satellite and the receiver, represents the drift rate of the satellite clock error, c represents the speed of light, represents the Doppler measurement noise.

[0091] Based on the Doppler frequency shift modeling equation, the carrier state at time k and satellites j The residuals associated with the Doppler measurements in It is expressed as:

[0092]

[0093] in, Indicates satellite j The related Doppler measurement value, χ represents the set of system state variables, and ψ represents other state variables of the carrier.

[0094] Its variance Defined as:

[0095]

[0096] Among them, n s represents the broadcast satellite spatial accuracy index, n dp represents the receiver measurement noise figure, θ el Indicates the satellite elevation angle on the receiver side view.

[0097] The Doppler data constraint factor can be constructed through the above residuals and variances.

[0098] (2) Carrier phase constraint factor:

[0099] The carrier phase measurement modeling can be expressed as:

[0100]

[0101] in, Indicates the carrier phase, P s E It represents the position of the satellite at the time of signal arrival in the Earth-Centered, Earth-Fixed (ECEF) coordinate frame. represents the ECI coordinates of receiver r, is a 4×1 indicator vector, the corresponding satellite constellation entity is 1, and the other three entities are 0, Δt s is the satellite clock error, which can be obtained from the broadcast navigation message. represents the tropospheric delay, represents the ionospheric delay, represents the delay caused by multipath effect, represents the integer ambiguity wavelength, represents the measurement noise, which is assumed to follow a zero-mean Gaussian distribution represents the variance of the measurement noise, which is modeled as follows:

[0102]

[0103] Among them, n pr Represents the code pseudorange measurement noise figure reported by the receiver.

[0104] Since there is a time difference between the transmission and arrival of satellite carrier signals, P s E and There is the following transformation relationship:

[0105]

[0106] in, Indicates the position of the satellite in the ECEF coordinate frame at the time of signal transmission, R z (θ) represents the rotation angle θ around the z-axis of the ECI, ω E represents the angular velocity of the Earth's rotation, t f Indicates the propagation time of the GNSS signal.

[0107] Finally, at time k, the carrier state and satellites j The carrier phase residual measured on (i.e., carrier phase constraint factor) can be expressed as:

[0108]

[0109] Among them, r k represents the GNSS receiver at time k.

[0110] (3) Code measurement pseudorange constraint factor:

[0111] The code pseudorange measurement modeling can be expressed as:

[0112]

[0113] in, Represents code pseudorange.

[0114] The carrier state at time k and satellites j The code pseudorange residual measured on (i.e., the code measurement pseudorange constraint factor) can be expressed as:

[0115]

[0116] (4) Clock error constraint factor:

[0117] In t k and t k-1 The receiver clock deviation at has the following relationship:

[0118]

[0119] Among them, δt k Indicates t k The receiver clock deviation at k-1Indicates t k-1 The receiver clock deviation at 4×1 Represents a 4×1 matrix with all elements set to 1.

[0120] The receiver clock drift rate is modeled as a random walk process, with the residual (i.e. clock error constraint factor) is expressed as:

[0121]

[0122] Step 205, based on the sliding window optimization method, the inertial sensor constraint factor, the lidar constraint factor, the Doppler data constraint factor, the carrier phase constraint factor, the code measurement pseudorange constraint factor and the clock error constraint factor are jointly optimized to obtain the positioning information of the carrier.

[0123] In an exemplary embodiment, step 205 includes the following steps 2051 and 2052 .

[0124] Step 2051, based on the inertial sensor constraint factor, the laser radar constraint factor, the Doppler data constraint factor, the carrier phase constraint factor, the code measurement pseudorange constraint factor and the clock error constraint factor, construct an outdoor sliding window optimization equation:

[0125]

[0126] Among them, r L represents the lidar constraint factor in abstract form, r B Represents the inertial sensor data constraint factor in abstract form, r D represents the Doppler data constraint factor in abstract form, r R represents the carrier phase constraint factor in abstract form, r P Represents the abstract form of the code measurement pseudorange constraint factor, r T represents the clock error constraint factor in abstract form, represents the laser radar data observation value, represents the inertial sensor data observation value, represents the Doppler data observation value, represents the carrier phase data observation value, Represents the observed value of the code measurement pseudorange data, Represents the observed value of clock error data.

[0127] Step 2052: Use the LM algorithm to solve the outdoor sliding window optimization equation to obtain the positioning information of the carrier. During the solution process, the posture X(R, T) is adjusted to minimize the error of the outdoor sliding window optimization equation. At this time, X(R, T) is the optimal solution for the positioning information of the outdoor carrier.

[0128] Step 206: If the carrier is located indoors, the inertial sensor constraint factor and the lidar constraint factor are jointly optimized based on the sliding window optimization method, and loop detection is performed based on the ultra-wideband data to eliminate the accumulated error during the indoor operation of the carrier and obtain the positioning information of the carrier.

[0129] In an exemplary embodiment, step 206 includes the following steps 2061 to 2063 .

[0130] Step 2061, based on the inertial sensor constraint factor and the laser radar constraint factor, construct an indoor sliding window optimization equation:

[0131]

[0132] Step 2062, solving the indoor sliding window optimization model to obtain preliminary position information of the carrier.

[0133] During the solution process, the posture X(R, T) is adjusted to minimize the error of the indoor sliding window optimization equation. At this time, X(R, T) is the optimal solution for the preliminary posture information of the indoor carrier.

[0134] Step 2063, performing loop detection on the preliminary posture information based on the ultra-wideband data to eliminate the accumulated errors during the indoor operation of the carrier and obtain the positioning information of the carrier.

[0135] In practical applications, a large number of UWB base stations are deployed indoors, and the distance between the carrier and each base station is obtained through the UWB receiver on the carrier. The carrier can be positioned based on these distances. By calculating the transformation relationship between the current key frame pose obtained by UWB positioning and the previous key frame pose, and performing loop detection on the preliminary pose information through the transformation relationship, the accumulated error of the SLAM front end can be eliminated, thereby improving the accuracy of positioning and mapping.

[0136] The basic principle of locating the carrier according to the measured distance is based on the least squares method. The specific calculation process is as follows:

[0137] Assume that there are four UWB base stations. The coordinates of the four UWB base stations are converted to the geodetic coordinate system. Their positions in the geodetic coordinate system are: A1 = [x1, y1, z1], A2 = [x2, y2, z2], A3 = [x3, y3, z3] and A4 = [x4, y4, z4].

[0138] Assume that the distance d between the four base stations and the carrier at time k is k =[d k,1 ,d k,2 ,d k,3,d k,4 ], and assume that the coordinates of the carrier position to be solved in the geodetic coordinate system are p k =[p x,k ,p y,k ,p z,k ],but:

[0139]

[0140] According to the least squares rule:

[0141] p k =(A T A) -1 A T b;

[0142]

[0143]

[0144] The above formula can be used to determine the positioning information after indoor carrier optimization.

[0145] Step 207, generating a three-dimensional point cloud map of the target space according to the positioning information of the carrier and the laser radar data.

[0146] Specifically, according to the positioning information of the carrier, the laser point cloud data between the key frame laser radar data are aligned to generate a three-dimensional point cloud map of the target space; the key frame laser radar data is the laser radar data in which the movement distance and rotation angle of two adjacent frames of laser radar data are greater than the corresponding set threshold.

[0147] It should be noted that step 207 is applicable both indoors and outdoors, that is, when the carrier is outdoors, step 207 processes the positioning information of the carrier obtained in step 205 to generate an outdoor three-dimensional point cloud map; when the carrier is indoors, step 207 processes the positioning information of the carrier obtained in step 206 to generate an indoor three-dimensional point cloud map.

[0148] The present invention combines GNSS, UWB, IMU and laser SLAM, and utilizes the advantages of laser SLAM's high-precision positioning in a short time, the advantages of GNSS providing global coordinates and no error accumulation, and the advantage of UWB's positioning accuracy not changing with time. By using the number of visible satellites to judge the indoor and outdoor states of the carrier, the present invention integrates GNSS data, IMU data and lidar data when outdoors, and integrates IMU data, lidar data and UWB data when indoors. It can perform real-time seamless high-precision positioning and mapping indoors and outdoors, greatly improving the accuracy and robustness of positioning and mapping.

[0149] Based on the same inventive concept, the embodiment of the present application also provides an indoor and outdoor positioning and mapping system based on multi-sensor fusion for implementing the indoor and outdoor positioning and mapping method based on multi-sensor fusion involved above. The implementation scheme for solving the problem provided by the system is similar to the implementation scheme recorded in the above method, so the specific limitations in one or more embodiments of the indoor and outdoor positioning and mapping system based on multi-sensor fusion provided below can refer to the limitations of the indoor and outdoor positioning and mapping method based on multi-sensor fusion above, and will not be repeated here.

[0150] In an exemplary embodiment, Figure 3 As shown, an indoor and outdoor positioning and mapping system based on multi-sensor fusion is provided, including:

[0151] The data acquisition module 301 is used to acquire inertial sensor data, lidar data, global navigation satellite data and ultra-wideband data of the carrier in the target space.

[0152] A first constraint factor construction module 303, configured to construct an inertial sensor constraint factor and a laser radar constraint factor based on the inertial sensor data and the laser radar data;

[0153] In an exemplary embodiment, the first constraint factor construction module 303 specifically includes:

[0154] The 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 the inertial sensor data constraint factor.

[0155] The dedistortion unit is used to dedistort the laser radar data using an inertial sensor to obtain dedistorted laser radar data.

[0156] The local three-dimensional point cloud map generation unit is used to generate a local three-dimensional point cloud map using the dedistorted laser radar data and the key frame laser radar data.

[0157] The matching unit is used to match the local three-dimensional point cloud map using the nearest point iteration algorithm to obtain the lidar constraint factor.

[0158] The carrier state determination module 304 is used to determine the indoor and outdoor state of the carrier according to the global navigation satellite data and the ultra-wideband data.

[0159] In an exemplary embodiment, the carrier status determination module 304 specifically includes:

[0160] The indoor and outdoor threshold setting unit is used to set an outdoor determination threshold and an indoor determination threshold for determining indoor and outdoor states.

[0161] The outdoor state determination unit is used to determine whether the number of visible satellites is greater than an outdoor determination threshold. If the number of visible satellites is greater than the outdoor determination threshold, it is determined that the carrier is currently in an outdoor state.

[0162] The indoor status determination unit is used to determine whether the number of visible satellites is less than the indoor determination threshold. If the number of visible satellites is less than the indoor determination threshold, if the UWB signal can be received and the coordinates of the point marked on the cloud server are at the boundary between indoor and outdoor, then the carrier is determined to be indoors. If the UWB signal cannot be received, but the coordinates of the point marked on the cloud server are at the boundary between indoor and outdoor, then the carrier is determined to be indoors. If the UWB signal can be received but the cloud server does not have the position coordinates of the point, then the carrier is determined to be indoors, and the coordinates of the point are uploaded to the cloud server. In other cases, the carrier is still determined to be outdoors.

[0163] The average determination unit is used to determine whether the number of visible satellites is between the indoor determination threshold and the outdoor determination threshold. If the number of visible satellites is between the indoor determination threshold and the outdoor determination threshold, GNSS data is continuously collected within the next 10 seconds, and the average value of the number of visible satellites within 10 seconds is taken. If the average value is not less than the indoor determination threshold, it is determined that the carrier is in an outdoor environment. If the average value is less than the indoor determination threshold, it is determined that the carrier is in an indoor environment.

[0164] The second constraint factor construction module 305 is used to construct a Doppler data constraint factor, a carrier phase constraint factor, a code measurement pseudorange constraint factor and a clock error constraint factor based on the global navigation satellite data when the carrier is located outdoors.

[0165] In an exemplary embodiment, the second constraint factor construction module 305 specifically includes:

[0166] The Doppler constraint factor construction unit is used to measure the Doppler frequency shift according to the difference between the received carrier signal and the design signal, determine the residual related to the Doppler measurement between the carrier and the satellite at a certain moment according to the Doppler modeling frequency shift modeling equation, and construct the Doppler constraint factor.

[0167] The carrier phase constraint factor construction unit is used to determine the residual related to the carrier phase between the carrier system and the satellite at a certain moment according to the carrier phase modeling equation, and construct the carrier phase constraint factor.

[0168] The coding pseudorange constraint factor construction unit is used to determine the residual related to the coding pseudorange between the carrier system and the satellite at a certain moment according to the coding pseudorange modeling equation, and construct the coding pseudorange constraint factor.

[0169] The clock error constraint factor construction unit is used to determine the clock error constraint factor by modeling according to the receiver clock drift rate.

[0170] The outdoor optimization module 306 is used to jointly optimize the inertial sensor constraint factor, the lidar constraint factor, the Doppler data constraint factor, the carrier phase constraint factor, the code measurement pseudorange constraint factor and the clock error constraint factor based on the sliding window optimization method to obtain the positioning information of the carrier.

[0171] In an exemplary embodiment, the outdoor optimization module 306 specifically includes:

[0172] The outdoor sliding window optimization equation construction unit is used to construct an outdoor sliding window optimization equation based on the inertial sensor constraint factor, the lidar constraint factor, the Doppler data constraint factor, the carrier phase constraint factor, the code measurement pseudorange constraint factor and the clock error constraint factor.

[0173] The outdoor solution subunit is used to solve the outdoor sliding window optimization equation using the LM algorithm to obtain the positioning information of the carrier.

[0174] The indoor optimization module 307 is used to jointly optimize the inertial sensor constraint factor and the lidar constraint factor based on the sliding window optimization method when the carrier is located indoors, and perform loop detection based on the ultra-wideband data to eliminate the accumulated error during the indoor operation of the carrier and obtain the positioning information of the carrier.

[0175] In an exemplary embodiment, the indoor optimization module 307 specifically includes:

[0176] The indoor sliding window optimization equation construction unit is used to construct the indoor sliding window optimization equation based on the inertial sensor constraint factor and the laser radar constraint factor.

[0177] The preliminary posture calculation unit is used to solve the indoor sliding window optimization model to obtain preliminary posture information of the carrier.

[0178] The loop detection unit is used to perform loop detection on the preliminary posture information based on the ultra-wideband data, eliminate the accumulated errors during the indoor operation of the carrier, and obtain the positioning information of the carrier.

[0179] The mapping module 308 is used to generate a three-dimensional point cloud map of the target space according to the positioning information of the carrier and the laser radar data.

[0180] Furthermore, the indoor and outdoor positioning and mapping system based on multi-sensor fusion also includes:

[0181] The indoor and outdoor coordinate conversion module 302 is used to convert the indoor and outdoor systems according to the carrier status.

[0182] In an exemplary embodiment, the indoor and outdoor coordinate conversion module 302 specifically includes:

[0183] The outdoor coordinate system conversion unit is used to solve the conversion parameters between the geodetic coordinate system and the world coordinate system defined by the lidar according to the coordinates of the common points of the geodetic coordinate system and the world coordinate system defined by the lidar, using a seven-parameter model, and unify the coordinates of each point in the world coordinate system defined by the lidar to the geodetic coordinate system.

[0184] The indoor coordinate system conversion unit is used to solve the conversion parameters between the world coordinate system defined by the lidar and the UWB system coordinate system based on the common point coordinates of the world coordinate system defined by the lidar and the UWB system coordinate system, and unify the coordinates of each point in the UWB system coordinate system to the world coordinate system defined by the lidar, and then use the conversion parameters between the geodetic coordinate system and the world coordinate system defined by the lidar to unify the coordinates of each point in the world coordinate system defined by the lidar to the geodetic coordinate system.

[0185] A data communication module is used for communication between the carrier and the cloud server, so as to upload the coordinate position of the carrier to the cloud server and compare the coordinate position of a certain point with the existing coordinate position in the cloud server;

[0186] In an exemplary embodiment, the data communication module specifically includes:

[0187] The position uploading unit is used to upload the coordinate values ​​of indoor and outdoor boundary points and obstructions in the geodetic coordinate system to the cloud server when it is determined that the carrier is in a transformation state between indoor and outdoor and passes through outdoor obstructions based on the number of visible satellites and UWB data.

[0188] The information comparison unit is used to compare the coordinates uploaded by the carrier with the existing coordinates on the cloud server. If the cloud server already has the coordinates of the point, it is determined whether the point is located at the intersection of indoor and outdoor areas or an outdoor obstruction. If the cloud server does not have the coordinates of the point, the coordinates of the point are stored on the cloud server so that the next time the GNSS and UWB data are temporarily invalid, the coordinates stored on the cloud server can be used to assist in determining the indoor and outdoor status of the carrier.

[0189] In an exemplary embodiment, a computer device is provided, including a memory and a processor, wherein a computer program is stored in the memory, and the processor implements the steps in the above-mentioned method embodiments when executing the computer program.

[0190] In an exemplary embodiment, a computer-readable storage medium is provided, storing a computer program, and when the computer program is executed by a processor, the steps in the above method embodiments are implemented.

[0191] In an exemplary embodiment, a computer program product is provided, including a computer program, and when the computer program is executed by a processor, the steps in the above method embodiments are implemented.

[0192] It should be noted that the user information (including but not limited to user device information, user personal information, etc.) and data (including but not limited to data used for analysis, stored data, displayed data, etc.) involved in this application are all information and data authorized by the user or fully authorized by all parties, and the collection, use and processing of relevant data must comply with relevant regulations.

[0193] In this application, all actions to obtain signals, information or data are carried out in compliance with the relevant data protection laws and policies of the country where they are located and with the authorization given by the owner of the corresponding device.

[0194] Those skilled 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 the memory, database or other medium used in the embodiments provided in the present application can include at least one of non-volatile and volatile memory. Non-volatile memory can include read-only memory (ROM), magnetic tape, floppy disk, flash memory, optical memory, high-density embedded non-volatile memory, resistive random access memory (ReRAM), magnetoresistive random access memory (MRAM), ferroelectric random access memory (FRAM), phase change memory (PCM), graphene memory, etc. Volatile memory can include random access memory (RAM) or external cache memory, etc. By way of illustration and not limitation, RAM may be in various forms, such as static random access memory (SRAM) or dynamic random access memory (DRAM).

[0195] The database involved in each embodiment provided in this application may include at least one of a relational database and a non-relational database. The non-relational database may include a distributed database based on blockchain, etc., but is not limited thereto. The processor involved in each embodiment provided in this application may be a general-purpose processor, a central processing unit, a graphics processor, a digital signal processor, a programmable logic device, a data processing logic device based on quantum computing, etc., but is not limited thereto.

[0196] 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.

[0197] This article uses specific examples to illustrate the principles and implementation methods of this application. The description of the above embodiments is only used to help understand the method and core ideas of this application. At the same time, for those skilled in the art, according to the ideas of this application, 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 this application.

Claims

1. A method for indoor and outdoor positioning and mapping based on multi-sensor fusion, characterized in that: The indoor and outdoor positioning and mapping method based on multi-sensor fusion includes: Acquire inertial sensor data, lidar data, global navigation satellite data, and ultra-wideband data of the carrier in the target space; Constructing an inertial sensor constraint factor and a lidar constraint factor based on the inertial sensor data and the lidar data; Determining the indoor and outdoor status of the carrier according to the global navigation satellite data and the ultra-wideband data; If the carrier is located outdoors, constructing a Doppler data constraint factor, a carrier phase constraint factor, a code measurement pseudorange constraint factor and a clock error constraint factor based on the global navigation satellite data; Based on the sliding window optimization method, the inertial sensor constraint factor, the laser radar constraint factor, the Doppler data constraint factor, the carrier phase constraint factor, the code measurement pseudorange constraint factor and the clock error constraint factor are jointly optimized to obtain the positioning information of the carrier; If the carrier is located indoors, the inertial sensor constraint factor and the lidar constraint factor are jointly optimized based on the sliding window optimization method, and loop detection is performed based on the ultra-wideband data to eliminate the accumulated error during the indoor operation of the carrier and obtain the positioning information of the carrier; A three-dimensional point cloud map of the target space is generated according to the positioning information of the carrier and the laser radar data.

2. The indoor and outdoor positioning and mapping method based on multi-sensor fusion according to claim 1, characterized in that: Acquire inertial sensor data, lidar data, global navigation satellite data and ultra-wideband data of the carrier in the target space, including: Collecting inertial sensor data through the inertial sensor on the carrier; Collecting lidar data through the lidar on the carrier; Receive pseudorange, carrier phase and Doppler signals through a global navigation satellite system receiver to obtain global navigation satellite data; The ultra-wideband data is collected through an ultra-wideband signal receiver.

3. The indoor and outdoor positioning and mapping method based on multi-sensor fusion according to claim 1, characterized in that: Constructing an inertial sensor constraint factor and a laser radar constraint factor based on the inertial sensor data and the laser radar data, specifically including: The inertial sensor data between key-frame laser radar data are pre-integrated by using a pre-integration processing formula to obtain an inertial sensor constraint factor; 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 both greater than corresponding set thresholds; the inertial sensor constraint factor includes: position, speed and rotation constraints between key-frame laser radar data; Dedistorting the laser radar data using the inertial sensor data to obtain dedistorted laser radar data; Generate a local three-dimensional point cloud map according to the dedistorted laser radar data and the key frame laser radar data; The local three-dimensional point cloud map is matched by using a nearest point iteration algorithm to obtain a laser radar constraint factor; the laser radar constraint factor is a relative transformation relationship of posture.

4. The indoor and outdoor positioning and mapping method based on multi-sensor fusion according to claim 1, characterized in that: Judging the indoor and outdoor status of the carrier according to the global navigation satellite data and the ultra-wideband data specifically includes: Converting the global navigation satellite data from the time domain to the frequency domain, and determining the number of visible satellites based on the number of pulse peaks in the frequency domain; If the number of visible satellites is greater than the set outdoor determination threshold, it is determined that the carrier is outdoors; If the number of visible satellites is less than the indoor determination threshold, determining the indoor and outdoor status of the carrier based on the ultra-wideband data and the data stored in the cloud server; the cloud server stores the coordinates of the indoor and outdoor boundary points and the coordinates of the outdoor obstructions; If the number of visible satellites is less than or equal to the outdoor determination threshold and greater than or equal to the indoor determination threshold, the average number of visible satellites within the continuous set time period is determined based on the global navigation satellite data within the continuous set time period; if the average number of visible satellites within the continuous set time period is less than the indoor determination threshold, it is determined that the carrier is indoors; otherwise, it is determined that the carrier is outdoors.

5. The indoor and outdoor positioning and mapping method based on multi-sensor fusion according to claim 4 is characterized in that: Determining the indoor and outdoor status of the carrier according to the ultra-wideband data and the data stored in the cloud server, specifically includes: Determine the carrier coordinates according to the ultra-wideband data, and judge whether the ultra-wideband signal can be received; If the ultra-wideband signal can be received, it is determined that the carrier is indoors, and the carrier coordinates are uploaded to the cloud server for storage as the indoor and outdoor boundary points; If the ultra-wideband signal cannot be received, it is determined whether the carrier coordinates are stored in the cloud server. If so, it is determined that the carrier is indoors; otherwise, it is determined that the carrier is outdoors.

6. The indoor and outdoor positioning and mapping method based on multi-sensor fusion according to claim 4, characterized in that: Judging the indoor and outdoor status of the carrier according to the global navigation satellite data and the ultra-wideband data also includes: According to the number of visible satellites and the ultra-wideband data, determining whether the carrier is in a state of transition between indoors and outdoors or passing through an outdoor obstruction; If the carrier is in a state of changing between indoor and outdoor or passes through an outdoor obstruction, the coordinates of the indoor and outdoor boundary point or the outdoor obstruction in geodetic coordinates are uploaded to the cloud server for storage.

7. The indoor and outdoor positioning and mapping method based on multi-sensor fusion according to claim 1, characterized in that: Based on the sliding window optimization method, the inertial sensor constraint factor, the laser radar constraint factor, the Doppler data constraint factor, the carrier phase constraint factor, the code measurement pseudorange constraint factor and the clock error constraint factor are jointly optimized to obtain the positioning information of the carrier, specifically including: Based on the inertial sensor constraint factor, the laser radar constraint factor, the Doppler data constraint factor, the carrier phase constraint factor, the code measurement pseudorange constraint factor and the clock error constraint factor, an outdoor sliding window optimization equation is constructed; The LM algorithm is used to solve the outdoor sliding window optimization equation to obtain the positioning information of the carrier.

8. The indoor and outdoor positioning and mapping method based on multi-sensor fusion according to claim 1, characterized in that: Based on the sliding window optimization method, the inertial sensor constraint factor and the laser radar constraint factor are jointly optimized, and loop detection is performed based on the ultra-wideband data to eliminate the accumulated error during the indoor operation of the carrier, and obtain the positioning information of the carrier, which specifically includes: Based on the inertial sensor constraint factor and the laser radar constraint factor, construct an indoor sliding window optimization equation; Solving the indoor sliding window optimization model to obtain preliminary position information of the carrier; A loopback detection is performed on the preliminary position information based on the ultra-wideband data to eliminate the accumulated errors during the indoor operation of the carrier and obtain the positioning information of the carrier.

9. The indoor and outdoor positioning and mapping method based on multi-sensor fusion according to claim 1, characterized in that: Generating a three-dimensional point cloud map of the target space according to the positioning information of the carrier and the laser radar data, specifically comprising: According to the positioning information of the carrier, the laser point cloud data between the key frame laser radar data are aligned to generate a three-dimensional point cloud map of the target space; the key frame laser radar data is the laser radar data in which the movement distance and rotation angle of two adjacent frames of laser radar data are greater than the corresponding set threshold.

10. An indoor and outdoor positioning and mapping system based on multi-sensor fusion, applied to the indoor and outdoor positioning and mapping method based on multi-sensor fusion according to any one of claims 1 to 9, characterized in that: The indoor and outdoor positioning and mapping system based on multi-sensor fusion includes: A data acquisition module, used to acquire inertial sensor data, lidar data, global navigation satellite data and ultra-wideband data of the carrier in the target space; A first constraint factor construction module, used to construct an inertial sensor constraint factor and a laser radar constraint factor based on the inertial sensor data and the laser radar data; A carrier state determination module, used to determine the indoor and outdoor state of a carrier according to the global navigation satellite data and the ultra-wideband data; A second constraint factor construction module is used to construct a Doppler data constraint factor, a carrier phase constraint factor, a code measurement pseudorange constraint factor and a clock error constraint factor based on the global navigation satellite data when the carrier is located outdoors; An outdoor optimization module is used to jointly optimize the inertial sensor constraint factor, the laser radar constraint factor, the Doppler data constraint factor, the carrier phase constraint factor, the code measurement pseudorange constraint factor and the clock error constraint factor based on a sliding window optimization method to obtain positioning information of the carrier; An indoor optimization module, used for jointly optimizing the inertial sensor constraint factor and the laser radar constraint factor based on a sliding window optimization method when the carrier is located indoors, and performing loop detection based on the ultra-wideband data to eliminate the accumulated error during the indoor operation of the carrier, and obtain the positioning information of the carrier; A mapping module is used to generate a three-dimensional point cloud map of the target space based on the positioning information of the carrier and the laser radar data.

Images