Tight coupling positioning method of multi-epoch robust navigation system based on msckf

Abstract

The application particularly relates to a tight combination positioning method of a multi-epoch robust navigation system based on MSCKF, which comprises the following steps: receiving base station and rover station observations of a first navigation device, point cloud of a second navigation device and specific force and angular velocity of an inertial measurement unit; performing mechanical arrangement on a third navigation device by using the specific force and the angular velocity, then predicting a state vector and a covariance of the MSCKF, based on a prediction result, performing data preprocessing on the point cloud data by using prior pose of the third navigation device, performing multi-frame data association, and constructing a multi-state constraint second navigation device measurement model of the MSCKF; performing gross error detection and elimination on the first navigation device observation by using multi-epoch robustness, then constructing a pseudo-range and Doppler measurement model of the first navigation device, and performing measurement update on the state vector and the covariance of the MSCKF, and controlling the first navigation device, the second navigation device and the third navigation device to perform tight combination positioning according to an update result.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of automatic driving, and particularly relates to a tight combination positioning method of a multi-epoch robust navigation system based on MSCKF. BACKGROUND

[0002] Positioning is the core basis of the "perception-decision-control" closed loop of an automatic driving system. For an automatic driving vehicle, high-precision positioning is a key prerequisite for realizing high-order functions such as high-precision map matching, real-time path planning and dynamic obstacle avoidance. LiDAR (Light Detection And Ranging) has become an indispensable part of an automatic driving multi-sensor fusion positioning system due to its high-precision ranging capability, strong robustness to light changes and cost optimization brought by large-scale production. In order to cope with point cloud distortion caused by motion, LiDAR is usually fused with IMU (Inertial Measurement Unit) into LIO (LiDAR-Inertial Odometry), thereby improving the accuracy and robustness of the positioning system. LIO essentially belongs to a DR (Dead Reckoning) system and can only provide relative pose information, and GNSS (Global Navigation Satellite System) can provide all-weather and high-precision absolute position information for users, and tight combination of LIO and GNSS can realize continuous and high-precision absolute positioning.

[0003] The fusion framework of GNSS / LiDAR / IMU is divided into graph optimization and filtering. FGO-GIL, GIVL-SLAM and GLIO tightly combine GNSS observations, LiDAR measurements and IMU measurements under the factor graph framework, and P3-LINS and GIL fuse GNSS, IMU and LiDAR at the measurement level under the extended EKF (Extended Kalman Filter) framework. Graph optimization shows higher accuracy through multiple iterations, but significantly increases the computational burden; EKF does not perform iteration and has higher efficiency, but the accuracy of its state estimation is affected by nonlinear errors. How to balance the accuracy and efficiency of GNSS / LiDAR / IMU fusion positioning is a difficult problem that needs to be solved in the field of automatic driving technology.

[0004] MSCKF (Multi-State Constraint Kalman Filter) has been proven to be comparable to graph optimization methods in terms of positioning accuracy, and has higher computational efficiency. However, the MSCKF framework has not been introduced into existing GNSS / LiDAR / INS (Inertial Navigation System) fusion positioning methods, which to some extent limits their application potential in real-time and resource-constrained scenarios.

[0005] GNSS observations are susceptible to multipath effects and NLOS (Non-Line-of-Sight), which can cause the measured values to deviate significantly from the true values (i.e., gross errors), thereby severely reducing positioning accuracy. Therefore, detecting and suppressing GNSS gross errors is crucial for achieving high-precision and robust positioning. GNSS outlier rejection methods can be divided into single-epoch outlier rejection methods and multi-epoch outlier rejection methods, depending on whether time correlation of GNSS observations is utilized. Single-epoch outlier rejection methods require accurate absolute pose estimation to achieve precise GNSS gross error detection and rejection.

[0006] However, it is difficult to ensure continuous and accurate absolute pose estimation during navigation and positioning. Considering the time correlation of GNSS observations, multi-epoch outlier rejection methods use multi-epoch GNSS observations for joint optimization to achieve more robust GNSS gross error detection and rejection. For example, some skilled persons use multi-epoch pseudorange observations while simultaneously solving positions and clock biases at multiple times, where the constraints between adjacent states are established using CTRV (Constant Turn Rate and Velocity) models. Some skilled persons use multi-epoch GNSS pseudorange and Doppler observations to simultaneously optimize states (positions, velocities, and clock biases) at multiple times and the weights of GNSS pseudorange observations, thereby mitigating the impact of pseudorange gross errors. In this system, no other sensors are used, so the Doppler velocity is used to relate adjacent states. Some skilled persons use LIO to provide more powerful relative pose constraints while estimating states at multiple times, thereby suppressing GNSS gross errors. The above multi-epoch outlier rejection methods use CTRV models, Doppler velocities, and LIO to construct correlations between states at different times. However, the constraint ability of CTRV models and Doppler velocities is limited, and although LIO provides stronger relative pose constraints, the LiDAR measurement model only constructs relative constraints between two frames, limiting the relative accuracy of LIO and thereby limiting the GNSS outlier rejection performance. More importantly, existing multi-epoch outlier rejection methods all need to estimate states at multiple times, resulting in high dimensionality of the parameter space to be estimated, an increase in local minima, and a higher probability of the parameter optimization process falling into local optima, thereby severely reducing the performance of multi-epoch outlier rejection methods, which needs to be addressed urgently. SUMMARY

[0007] The application provides a tight combination positioning method of a multi-epoch robust navigation system based on MSCKF to solve the problems of limited constraint capability of a multi-epoch robust method in the related art, which limits the GNSS robust performance, and causes high dimension of to-be-estimated parameter space, increase of local minimum, high probability of parameter optimization process falling into local optimum and the like.

[0008] The first aspect embodiment of the application provides a tight combination positioning method of a multi-epoch robust navigation system based on MSCKF, comprising the following steps:

[0009] receiving base station data and flow station observation data of a first navigation device, point cloud data of a second navigation device and specific force data and angular velocity data of an inertial measurement unit of a third navigation device;

[0010] mechanically arranging the third navigation device by using the specific force data and the angular velocity data of the inertial measurement unit, obtaining a running state of the third navigation device based on an arrangement result, and predicting a state vector and a covariance of MSCKF based on the running state of the third navigation device to obtain a prediction result of the MSCKF;

[0011] based on the prediction result of the MSCKF, performing data preprocessing on the point cloud data of the second navigation device by using a priori pose of the third navigation device, performing multi-frame data association on the point cloud data of the second navigation device, and constructing a multi-state constraint second navigation device measurement model of the MSCKF;

[0012] performing gross error detection and elimination processing on first navigation device observation by using the multi-epoch robust method, constructing a pseudo-range and Doppler measurement model of the first navigation device based on the first navigation device observation after the gross error detection and the elimination processing, performing measurement update on the state vector and the covariance of the MSCKF based on the prediction result of the MSCKF, the multi-state constraint second navigation device measurement model and the pseudo-range and Doppler measurement model, and controlling the first navigation device, the second navigation device and the third navigation device to perform tight combination positioning according to an update result.

[0013] According to one embodiment of the application, the state vector of the MSCKF comprises a first navigation device error state, a second navigation device error state and a third navigation device error state.

[0014] According to one embodiment of the present application, based on the prediction result of the MSCKF, the point cloud data of the second navigation device is preprocessed using the prior pose of the third navigation device, and the point cloud data of the second navigation device is correlated in multiple frames to construct a multi-state constrained second navigation device measurement model of the MSCKF, comprising:

[0015] Correcting the motion distortion of the point cloud data based on the prior pose of the third navigation device;

[0016] Obtaining displacement data and rotation angle of the vehicle, and selecting key frame point cloud data of the second navigation device based on the displacement data, the rotation angle and a preset interval length;

[0017] Projecting and accumulating non-key frame point cloud data of the second navigation device to the key frame point cloud data using the prior pose of the third navigation device to obtain a key frame point cloud map of the second navigation device at the current time;

[0018] Correlating the key frame point cloud map at the current time and the historical key frame point cloud map in multiple frames to construct a multi-state constrained second navigation device measurement model of the MSCKF.

[0019] According to one embodiment of the present application, after constructing the multi-state constrained second navigation device measurement model of the MSCKF, further comprising:

[0020] Calculating the residual and Jacobian of the multi-state constrained second navigation device measurement model, and updating the state vector and covariance of the MSCKF based on the residual and Jacobian of the multi-state constrained second navigation device measurement model;

[0021] Adding the key frame point cloud map at the current time to a key frame point cloud map sliding window of the second navigation device, and obtaining a pose error state of an inertial measurement unit in the key frame point cloud map at the current time;

[0022] Judging whether the number of key frame point cloud data in the key frame point cloud map sliding window exceeds a preset maximum sliding window length;

[0023] If the number of key frame point cloud data exceeds the preset maximum sliding window length, deleting the earliest stored key frame point cloud map, and augmenting the pose error state to the state vector and covariance of the MSCKF, and performing a marginalization operation on the state vector and covariance of the MSCKF.

[0024] According to one embodiment of the present application, the rough error detection and elimination processing of the first navigation device observation using the multi-epoch robustness method comprises:

[0025] receive first navigation device base station observations and first navigation device rover station observations, and construct inter-station single-difference observations using the first navigation device base station observations and the first navigation device rover station observations;

[0026] based on the inter-station single-difference observations, use the multi-state constrained second navigation device measurement model and the relative pose of the third navigation device to perform gross error detection and elimination processing on the pseudo-range observations in the first navigation device observations, and use the innovation chi-square test method to perform gross error detection and elimination processing on the Doppler observations in the first navigation device observations.

[0027] According to the tight combination positioning method of the multi-epoch robust navigation system based on the MSCKF provided in the embodiments of the present application, base station and rover station observations of a first navigation device, point cloud and specific force and angular velocity of an inertial measurement unit of a second navigation device are received; the specific force and angular velocity are used to mechanically arrange a third navigation device, and then the state vector and covariance of the MSCKF are predicted; based on the prediction result, the point cloud data are preprocessed using the prior pose of the third navigation device, and multi-frame data association is performed to construct a multi-state constrained second navigation device measurement model of the MSCKF; the first navigation device observations are detected and eliminated for gross errors using multi-epoch robustness, and then pseudo-range and Doppler measurement models of the first navigation device are constructed, and the state vector and covariance of the MSCKF are updated for measurement, and the first navigation device, the second navigation device and the third navigation device are controlled for tight combination positioning according to the update result. Thus, the problems in the related art that the constraint ability of the multi-epoch robustness method is limited, the GNSS robustness performance is limited, the dimension of the to-be-estimated parameter space is high, the number of local minima is increased, and the probability that the parameter optimization process falls into a local optimum is large are solved.

[0028] The second aspect embodiment of the present application provides a tight combination positioning device of a multi-epoch robust navigation system based on the MSCKF, comprising:

[0029] The receiving module is configured to receive base station data and rover station observation data of a first navigation device, point cloud data of a second navigation device, and specific force data and angular velocity data of an inertial measurement unit;

[0030] The prediction module is configured to mechanically arrange a third navigation device using the specific force data and angular velocity data of the inertial measurement unit, and obtain the running state of the third navigation device based on the arrangement result, predict the state vector and covariance of the MSCKF using the running state of the third navigation device, and obtain the prediction result of the MSCKF;

[0031] a model construction module, configured to perform data preprocessing on point cloud data of the second navigation device based on a prediction result of the MSCKF and the prior pose of the third navigation device, perform multi-frame data association on the point cloud data of the second navigation device, and construct a multi-state constrained second navigation device measurement model of the MSCKF;

[0032] a control module, configured to perform rough error detection and elimination processing on first navigation device observations by using the multi-epoch robustness method, construct a pseudo-range and Doppler measurement model of the first navigation device based on the first navigation device observations after the rough error detection and the elimination processing, perform measurement update on a state vector and a covariance of the MSCKF based on a prediction result of the MSCKF, the multi-state constrained second navigation device measurement model, and the pseudo-range and Doppler measurement model, and control the first navigation device, the second navigation device, and the third navigation device to perform tight combination positioning according to an update result.

[0033] According to an embodiment of the present application, the state vector of the MSCKF includes a first navigation device error state, a second navigation device error state, and a third navigation device error state.

[0034] According to an embodiment of the present application, the model construction module includes:

[0035] a correction unit, configured to correct motion distortion of the point cloud data based on the prior pose of the third navigation device;

[0036] an acquisition unit, configured to acquire displacement data and a rotation angle of a vehicle, and select key frame point cloud data of the second navigation device based on the displacement data, the rotation angle, and a preset interval length;

[0037] a projection unit, configured to project and accumulate non-key frame point cloud data of the second navigation device to the key frame point cloud data by using the prior pose of the third navigation device, to obtain a key frame point cloud map of the second navigation device at a current time;

[0038] a first model construction unit, configured to perform multi-frame data association on the key frame point cloud map at the current time and a historical key frame point cloud map, and construct a multi-state constrained second navigation device measurement model of the MSCKF.

[0039] According to an embodiment of the present application, after the multi-state constrained second navigation device measurement model of the MSCKF is constructed, the model construction unit further includes:

[0040] an updating subunit configured to calculate a residual and a Jacobian of the multi-state constrained second navigation device measurement model, and update a state vector and a covariance of the MSCKF based on the residual and the Jacobian of the multi-state constrained second navigation device measurement model;

[0041] an obtaining subunit configured to add the keyframe point cloud map at the current time to a keyframe point cloud map sliding window of the second navigation device, and obtain a pose error state of an inertial measurement unit in the keyframe point cloud map at the current time;

[0042] a judging subunit configured to judge whether a number of keyframe point cloud data in the keyframe point cloud map sliding window exceeds a preset maximum sliding window length;

[0043] an edge subunit configured to, if the number of keyframe point cloud data exceeds the preset maximum sliding window length, delete a first stored keyframe point cloud map, and augment the pose error state to the state vector and the covariance of the MSCKF, and perform an edge operation on the state vector and the covariance of the MSCKF.

[0044] According to an embodiment of the present application, the control module comprises:

[0045] a second model constructing unit configured to receive first navigation device base station observations and first navigation device rover station observations, and construct inter-station single-difference observations by using the first navigation device base station observations and the first navigation device rover station observations;

[0046] a detecting and rejecting unit configured to perform coarse error detection and rejection processing on pseudo-range observations in the first navigation device observations by using the multi-state constrained second navigation device measurement model and a relative pose of a third navigation device, and perform coarse error detection and rejection processing on Doppler observations in the first navigation device observations by using a new information chi-square test method.

[0047] The tight combination positioning device of the multi-epoch robust navigation system based on MSCKF according to the embodiment of the application receives base station and rover station observations of a first navigation device, point cloud and specific force and angular velocity of an inertial measurement unit of a second navigation device; the specific force and angular velocity are used for mechanical arrangement of a third navigation device, and then the state vector and covariance of the MSCKF are predicted; based on the prediction result, the point cloud data are preprocessed by using the prior pose of the third navigation device, and multi-frame data association is performed, and a multi-state constraint second navigation device measurement model of the MSCKF is constructed; the multi-epoch robustness is used for rough error detection and elimination of the first navigation device observation, and then a pseudo-range and Doppler measurement model of the first navigation device is constructed, and the state vector and covariance of the MSCKF are measured and updated, and the first navigation device, the second navigation device and the third navigation device are controlled to perform the tight combination positioning according to the update result. Therefore, the problems that the constraint ability of the multi-epoch robustness method in the related art is limited, the GNSS robustness performance is limited, the dimension of the to-be-estimated parameter space is high, the local minimum value is increased, and the probability that the parameter optimization process falls into a local optimum is high are solved.

[0048] The third aspect embodiment of the application provides an electronic device, comprising a memory, a processor and a computer program stored in the memory and executable on the processor, and the processor executes the program to implement the tight combination positioning method of the multi-epoch robust navigation system based on MSCKF as described in the above embodiments.

[0049] The fourth aspect embodiment of the application provides a computer readable storage medium, which stores computer instructions for causing the computer to execute the tight combination positioning method of the multi-epoch robust navigation system based on MSCKF as described in the above embodiments.

[0050] The fifth aspect embodiment of the application provides a computer program product, comprising computer programs / instructions, which, when executed by a processor, implement the tight combination positioning method of the multi-epoch robust navigation system based on MSCKF as described in the above embodiments.

[0051] Additional aspects and advantages of the application will be made apparent by the following description and the accompanying drawings. BRIEF DESCRIPTION OF DRAWINGS

[0052] The above and / or additional aspects and advantages of the application will become apparent and be readily understood by practice of the application from the following detailed description, the accompanying drawings, and the claims.

[0053] Figure 1 A flowchart of a tight combination positioning method of a multi-epoch robust navigation system based on MSCKF according to an embodiment of the application is provided.

[0054] Figure 2 System block diagram of a multi-epoch robust GNSS / LiDAR / INS tightly coupled positioning method based on MSCKF according to an embodiment of the present application;

[0055] Figure 3 Principle block diagram of LiDAR multi-frame data association according to an embodiment of the present application;

[0056] Figure 4 Principle block diagram of LiDAR coplanar cluster tracking method according to an embodiment of the present application;

[0057] Figure 5 Principle block diagram of GNSS pseudorange multi-epoch robust method based on high-precision relative pose according to an embodiment of the present application;

[0058] Figure 6 North direction positioning error diagram of open source tightly coupled method GLIO and the present application in an embodiment according to an embodiment of the present application;

[0059] Figure 7 East direction positioning error diagram of open source tightly coupled method GLIO and the present application in an embodiment according to an embodiment of the present application;

[0060] Figure 8 Vertical direction positioning error diagram of open source tightly coupled method GLIO and the present application according to an embodiment of the present application;

[0061] Figure 9 Block diagram of tightly coupled positioning device of multi-epoch robust navigation system based on MSCKF according to an embodiment of the present application;

[0062] Figure 10 Structural diagram of an electronic device according to an embodiment of the present application. DETAILED DESCRIPTION

[0063] Embodiments of the present application are described in detail below with reference to the accompanying drawings. Examples of the embodiments are shown in the drawings, in which the same or similar reference numerals represent the same or similar elements or elements having the same or similar functions throughout. The embodiments described below by reference to the drawings are exemplary and are intended to explain the present application, and cannot be understood as limiting the present application.

[0064] A method for tightly coupled positioning of a multi-epoch robust navigation system based on MSCKF according to an embodiment of the present application is described below with reference to the accompanying drawings. In view of the limited constraint capability of the multi-epoch robust method in the related art mentioned in the background, the GNSS robust performance is limited, and the dimension of the parameter space to be estimated is high, the local minimum is increased, and the probability of the parameter optimization process falling into local optimum is large. The present application provides a method for tightly coupled positioning of a multi-epoch robust navigation system based on MSCKF. In this method, the base station and rover observation data of a first navigation device, the point cloud data of a second navigation device, and the specific force and angular velocity data of an inertial measurement unit are received. The third navigation device is mechanically arranged using the specific force and angular velocity, and the state vector and covariance of the MSCKF are predicted. Based on the prediction result, the point cloud data is preprocessed using the prior pose of the third navigation device, and multi-frame data association is performed to construct a multi-state constrained second navigation device measurement model of the MSCKF. The multi-epoch robust method is used to detect and eliminate gross errors of the first navigation device observation, and then the pseudo-range and Doppler measurement models of the first navigation device are constructed. The state vector and covariance of the MSCKF are updated, and the first navigation device, the second navigation device, and the third navigation device are controlled for tightly coupled positioning according to the update result. Thus, the problem of limited constraint capability of the multi-epoch robust method in the related art, which limits the GNSS robust performance, and causes high dimension of the parameter space to be estimated, increased local minimum, and high probability of the parameter optimization process falling into local optimum, is solved.

[0065] Specifically, Figure 1 A flowchart of a method for tightly coupled positioning of a multi-epoch robust navigation system based on MSCKF according to an embodiment of the present application is shown.

[0066] As Figure 1 shown, the method for tightly coupled positioning of a multi-epoch robust navigation system based on MSCKF includes the following steps:

[0067] In step S101, the base station data and rover observation data of a first navigation device, the point cloud data of a second navigation device, and the specific force data and angular velocity data of an inertial measurement unit are received.

[0068] Specifically, to solve the problems of GNSS signal reliability, balance between sensor fusion accuracy and efficiency, LiDAR point cloud distortion, and local minimum of high-dimensional parameter optimization in the fusion positioning method of the navigation system (such as GNSS / LiDAR / INS) in the related art, the embodiments of the present application use the MSCKF framework, the high-precision ranging capability of LiDAR, and the continuous position estimation of IMU to ensure the positioning accuracy and improve the calculation efficiency, and combine the multi-epoch robustness method to realize low-computing, high-precision, and high-reliable navigation positioning, and meet the accurate and reliable real-time navigation positioning requirements of robots and autonomous vehicles in complex environments.

[0069] Specifically, in the embodiments of the present application, first, data collection is performed, including base station data and rover observation data of a first navigation device (such as GNSS) collected by a sensor, point cloud data of a second navigation device (such as LiDAR), and specific force data and angular velocity data of an inertial measurement unit, then, the base station data and the rover observation data of the first navigation device, the point cloud data of the second navigation device, and the specific force data and the angular velocity data of the inertial measurement unit are provided as original input data for subsequent processing.

[0070] Preferably, the embodiments of the present application use a hardware trigger to time synchronize the GNSS, LiDAR, and IMU sensors, so as to unify the time base of the three.

[0071] In step S102, the specific force data and the angular velocity data of the inertial measurement unit are used to mechanically arrange the third navigation device, and the running state of the third navigation device is obtained based on the arrangement result, and the state vector and the covariance of the MSCKF are predicted based on the running state of the third navigation device, and the prediction result of the MSCKF is obtained.

[0072] Specifically, in the embodiments of the present application, the IMU as the core sensor establishes the connection of the LiDAR and the GNSS, once the IMU data (such as the specific force data and the angular velocity data of the IMU) is received, the mechanical arrangement of the third navigation device (such as INS) is performed, the high-frequency INS position, velocity, and attitude are calculated, and the MSCKF error state and its covariance are predicted, wherein the LiDAR and the GNSS are used as observation data for MSCKF measurement update.

[0073] Specifically, as shown in FIG. 2, the embodiments of the present application include the following steps: Figure 2As shown, the third navigation device (for example, INS) is mechanically arranged using the specific force data and angular velocity data of the IMU, that is, the INS is mechanically arranged using the IMU specific force and angular velocity data to calculate the running state of the high-frequency INS, mainly including the INS position, speed and attitude, wherein the INS mechanical arrangement mainly forwards the INS state (position, speed and attitude) by using the IMU measurement value, and the INS mechanical arrangement is performed in the world coordinate system (for example, w system), and the w system is defined as the navigation coordinate system at the initial moment, and the INS mechanical arrangement is derived through the following INS dynamics model:

[0074]

[0075]

[0076]

[0077] wherein, and respectively represent the rotation matrix of the quaternion between the IMU coordinate system (b system) and the w system; is the velocity of the b system relative to the w system; is the position of the b system in the w system; is the angular velocity measured by the IMU, f b is the specific force data measured by the IMU; g w is the local gravity; is the projection of the angular velocity of the geocentric coordinate system (e system) relative to the inertial coordinate system (i system) in the b system; is the quaternion multiplication, (·) x is the skew-symmetric matrix of a three-dimensional vector.

[0078] Further, the embodiment of the present application needs to further predict the MSCKF state vector and its covariance while performing the INS mechanical arrangement, wherein the MSCKF state vector δx is composed of the first navigation device error state (GNSS state δx GNSS ), the second navigation device error state (LiDAR state δx LiDAR ) and the third navigation device error state (INS state δx INS ), wherein δx, δx GNSS , δx LiDAR and δx INS can be respectively represented as:

[0079] δx=[δx INS ,δx LiDAR ,δx GNSS ] T

[0080]

[0081]

[0082]

[0083] wherein, is the current time IMU in the world coordinate system under the attitude, is the current time IMU in the world coordinate system under the position, is the current time IMU in the world coordinate system under the velocity error, δb g is the zero offset error of the gyroscope, δb a is the zero offset error of the accelerometer, δs g is the scale factor error of the gyroscope, δs a is the scale factor error of the accelerometer, wherein the zero offset and the scale factor are modeled as a first-order Gauss Markov process; and is the LiDAR-IMU extrinsic error, M is the length of the LiDAR sliding window, and are the IMU attitude and position errors at the kth LiDAR key frame time, respectively; is the error of the inter-station difference of the GPS, is the error of the inter-station difference of the Beidou, is the error of the inter-station difference of the Galileo satellite system, is the error of the inter-station difference minute drift, wherein the change rate of the clock error is clock drift, and the clock drift is modeled as a random walk.

[0084] Further, the error state transition equation can be derived by performing error disturbance on the INS dynamic model and combining the sensor error modeling, and the state prediction equation of the ESKF (Error-State Kalman Filter) is used to predict the MSCKF state vector and its covariance.

[0085] It should be noted that the extrinsic parameters of the LiDAR-IMU can be automatically calibrated by online estimation without manual calibration.

[0086] In step S103, based on the prediction result of the MSCKF, the point cloud data of the second navigation device is preprocessed using the prior pose of the third navigation device, and the point cloud data of the second navigation device is associated with multiple frames of data to construct a multi-state constrained second navigation device measurement model of the MSCKF.

[0087] According to one embodiment of the present application, based on the prediction result of the MSCKF, the point cloud data of the second navigation device is preprocessed using the prior pose of the third navigation device, and the point cloud data of the second navigation device is correlated with multiple frames of data, and a multi-state constrained second navigation device measurement model of the MSCKF is constructed, including: correcting the motion distortion of the point cloud data based on the prior pose of the third navigation device; obtaining displacement data and rotation angle of the vehicle, and selecting key frame point cloud data of the second navigation device based on the displacement data, the rotation angle and a preset interval length; projecting and accumulating non-key frame point cloud data of the second navigation device to the key frame point cloud data using the prior pose of the third navigation device to obtain the key frame point cloud map of the second navigation device at the current time; and correlating the key frame point cloud map at the current time with the historical key frame point cloud map to construct the multi-state constrained second navigation device measurement model of the MSCKF.

[0088] The preset interval length can be set by a person skilled in the art according to actual test requirements, or can be obtained through limited computer simulation, and is not limited here.

[0089] Specifically, when receiving a LiDAR frame, the INS prior pose needs to be used to preprocess the LiDAR point cloud data. First, the motion distortion of the LiDAR point cloud data is corrected using the high-frequency prior pose output by the third navigation device INS; second, the LiDAR key frame is selected, the displacement data and rotation angle of the vehicle are obtained, and the LiDAR key frame point cloud data of the second navigation device is selected based on the displacement data, the rotation angle and a preset interval length. That is, when the carrier (such as a vehicle) travels more than the preset displacement data or the rotation angle exceeds the preset angle, or the time interval with the last key frame exceeds the preset interval length, the current LiDAR frame is identified as a key frame, that is, the current LiDAR frame is key frame point cloud data, and the non-key frame point cloud data between the last key frame point cloud data and the key frame point cloud data at the current time is projected and accumulated to the current key frame point cloud data using the INS high-frequency prior pose, thereby forming a dense key frame point cloud map at the current time, that is, the current LiDAR key frame point cloud map.

[0090] Specifically, as shown in Figure 3 , a same plane cluster tracking method is used for multi-frame data correlation, that is, the key frame point cloud map at the current time and the historical key frame point cloud map are correlated with multiple frames of data. As shown in Figure 4 , when initializing the same plane cluster, all point clouds of the first key frame point cloud map are regarded as candidate plane points, and each candidate plane point constructs a same plane cluster. For the jth same plane cluster S j in the same plane cluster set, the following tracking process is performed:

[0091] Firstly, the latest coplanar point p in S is projected to the current LiDAR keyframe point cloud map using the INS prior pose; then, five nearest neighbors of p in the current keyframe point cloud map are searched, and a plane is fitted, if the distances from p and its five nearest neighbors to the fitted plane are all less than a preset threshold, the coplanar cluster tracking is successful, at this time, the nearest neighbor p of p in the current LiDAR keyframe point cloud map is added to S n is added to S j When a new LiDAR keyframe point cloud map is selected, p n After tracking the coplanar point in the latest LiDAR keyframe point cloud map using the above method, S j can be expressed as represents the point from the kth keyframe, is the index set of the keyframes associated with S j .

[0092] When S j satisfies the following two conditions, i.e., (1) the number of keyframes associated with S j is not less than 5 and contains the current keyframe, (2) a plane is fitted using the points in S j and the distances from all points to the plane are less than a preset threshold, S j is regarded as a valid coplanar cluster, at this time, S j is used to construct the multi-state constraint second navigation device measurement model of MSCKF, i.e., the multi-state constraint LiDAR measurement model.

[0093] According to one embodiment of the present application, after constructing the multi-state constraint second navigation device measurement model of MSCKF, it further includes: calculating the residual and Jacobian of the multi-state constraint second navigation device measurement model, and updating the state vector and covariance of MSCKF based on the residual and Jacobian of the multi-state constraint second navigation device measurement model; adding the keyframe point cloud map at the current time to the keyframe point cloud map sliding window of the second navigation device, and obtaining the pose error state of the inertial measurement unit in the keyframe point cloud map at the current time; judging whether the number of keyframe point cloud data in the keyframe point cloud map sliding window exceeds the preset maximum sliding window length; if the number of keyframe point cloud data exceeds the preset maximum sliding window length, deleting the first stored keyframe point cloud map, and augmenting the pose error state to the state vector and covariance of MSCKF, and performing the marginalization operation on the state vector and covariance of MSCKF.

[0094] The preset maximum sliding window length can be set by a person skilled in the art according to actual use requirements, or can be obtained through limited computer simulation, which is not limited here.

[0095] Specifically, after constructing the multi-state constrained LiDAR measurement model of MSCKF, the MSCKF measurement update based on the multi-state constrained LiDAR measurement model is needed, first, the points in the same plane cluster S j are projected to w system by using INS pose and LiDAR-IMU extrinsic parameter, and the expression is as follows:

[0096]

[0097] wherein, and respectively represent the INS position and pose at the kth LiDAR key frame time; is the rotation part of LiDAR-IMU extrinsic parameter S j , is the translation part of LiDAR-IMU extrinsic parameter S j .

[0098] Plane fitting is performed using all the projected points, and the fitted plane can be represented as:

[0099] n T p w +d=0

[0100] wherein, n is the normalized normal vector of the plane, p w is a point on the plane, and d is the intercept term of the plane.

[0101] The multi-state constrained LiDAR measurement model is modeled as:

[0102]

[0103] wherein, n j is the number of plane points in S j .

[0104] Then, error perturbation is performed on z j , the Jacobian is derived, and then the MSCKF state vector and its covariance are updated by using the measurement update equation of the standard ESKF.

[0105] Further, the current LiDAR key frame point cloud map is added to the LiDAR key frame point cloud map sliding window, if the number of key frame point cloud data in the LiDAR key frame point cloud map sliding window exceeds the preset maximum sliding window length, the first stored key frame point cloud map is deleted, and the IMU pose error state at the current time is augmented to the state vector and covariance matrix of MSCKF, and the edge operation is performed on the state vector and covariance of MSCKF, which can be represented as:

[0106]

[0107] where P n×n is the pre-augmented covariance matrix, P (n+6)×(n+6) is the post-augmented covariance matrix, J 6×n is the augmented pose state Jacobian with respect to the original state vector, I n×n is the n-dimensional identity matrix.

[0108] In step S104, the first navigation device observation is detected and removed by using a multi-epoch robustness method, and a pseudo-range and Doppler measurement model of the first navigation device is constructed based on the first navigation device observation after the detection and removal of the gross error, so as to perform measurement updating on the state vector and covariance of the MSCKF based on the prediction result of the MSCKF, the multi-state constraint second navigation device measurement model, and the pseudo-range and Doppler measurement model, and control the first navigation device, the second navigation device, and the third navigation device to perform tight combination positioning according to the updating result.

[0109] According to an embodiment of the present application, the first navigation device observation is detected and removed by using a multi-epoch robustness method, including: receiving first navigation device base station observation and first navigation device flow station observation, and constructing inter-station single difference observation by using the first navigation device base station observation and the first navigation device flow station observation; based on the inter-station single difference observation, the pseudo-range observation in the first navigation device observation is detected and removed by using the multi-state constraint second navigation device measurement model and the relative pose of the third navigation device, and the Doppler observation in the first navigation device observation is detected and removed by using the innovation chi-square test method.

[0110] Specifically, when receiving the observation of the GNSS base station and the flow station, the inter-station single difference observation is constructed by using the GNSS base station and the flow station observation, so as to eliminate satellite end errors (including clock errors and orbit errors) and space propagation errors (including troposphere delay and ionosphere delay), and the single difference pseudo-range observation of satellite s can be calculated by inter-station difference and the single difference Doppler observation

[0111]

[0112]

[0113] wherein subscript b is a base station, subscript r is a flow station, is a flow station pseudo-range observation of satellite s, is a base station pseudo-range observation of satellite s, is a flow station Doppler observation of satellite s, is a base station Doppler observation of satellite s.

[0114] Further, for GNSS pseudorange observations susceptible to multipath effects and NLOS, a multi-state constrained LiDAR measurement model and high-precision relative poses provided by INS are used to perform pseudorange multi-epoch robustness, wherein a principle diagram of a multi-epoch robustness method based on high-precision relative poses is as shown in FIG. 3. Figure 5 To implement multi-epoch robustness based on high-precision relative poses, a GNSS sliding window with a length of N is designed, which contains single-difference pseudorange observations from N-1 historical epochs, single-difference pseudorange observations at the current time, and poses corresponding to these time points, wherein, can be expressed as:

[0115]

[0116]

[0117]

[0118] wherein, is a set of IMU poses at the kth epoch in the GNSS sliding window, is a set of single-difference pseudorange observations at the kth epoch in the GNSS sliding window, is a set of satellites commonly observed by the base station and the rover station at the kth epoch in the GNSS sliding window, is a set of IMU poses at the current time, is a set of single-difference pseudorange observations at the current time, is an INS position at the kth epoch, is an INS attitude at the kth epoch, is a single-difference pseudorange observation of satellite s at the kth epoch. j

[0119] Since the multi-state constrained LiDAR measurement model used constructs relative constraints between multiple frames, the relative accuracy of the trajectory provided by the fusion positioning method is high, and the error of the relative trajectory is much smaller than the GNSS pseudorange error. Therefore, in the multi-epoch robustness process, the relative pose estimation value between epochs is considered to be accurate, and the relative pose between the kth epoch and the initial epoch can be calculated by the following formula:

[0120]

[0121]

[0122] Assuming is accurate, when the initial epoch position is determined, the pose Moreover, considering the receiver clock characteristics, the receiver clock drift can be considered as a constant in a short time, thus the receiver clock bias at the kth epoch can be written as:

[0123]

[0124] where, and are the receiver clock biases of satellite system sys estimated by the fusion positioning method at the kth epoch and the initial epoch, respectively, and the subscript denotes the epoch; is the receiver clock drift estimated by the fusion positioning method, t k is the time of the kth epoch, and t1 is the time of the initial epoch.

[0125] Thus, the single-difference pseudorange prediction at the kth epoch can be written as:

[0126]

[0127]

[0128] where, denotes the distance between the base station and satellite s j , is the rover slant range, is the INS position at the initial epoch, is the relative position between the initial epoch and the kth epoch, is the INS attitude at the initial epoch, is the relative attitude between the initial epoch and the kth epoch, is the lever arm between the rover antenna and the IMU, is the position of satellite s j in the world coordinate system.

[0129] From the above formula, the unknown variables are the position and attitude at the initial epoch, the receiver clock bias, and the clock drift. Thus, by making full use of the high-precision relative attitude, a data-intensive low-dimensional optimization problem can be constructed:

[0130]

[0131]

[0132] where, σ j,k is the standard deviation of the pseudorange observation, is the single-difference pseudorange observation of satellite s j at the kth epoch, is the prediction value of the single-difference pseudorange observation of satellite s j at the kth epoch, is the INS position at the initial epoch, INS attitude of initial epoch, receiver clock bias of initial epoch, receiver clock drift.

[0133] It should be noted that when solving F(x), there is a strong correlation between the elevation and the receiver clock bias, and if the elevation and the receiver clock bias are optimized at the same time, both of them are difficult to converge to the global optimum. Therefore, the idea of block coordinate descent is used to block and alternately optimize the estimated parameters.

[0134] Specifically, x is divided into and First, fix Optimize Then fix Optimize Until the change of the objective function is less than the preset threshold μ or the number of iterations reaches the preset threshold c max By using the optimization strategy based on block coordinate descent, the elevation and the receiver clock bias can be effectively decoupled.

[0135] Solving F(x) to obtain the optimal solution After that, the posterior residual of the single-difference pseudo-range observation of the current epoch is calculated

[0136]

[0137] Among them, only the single-difference pseudo-range observation with an absolute residual value less than the preset threshold is used for measurement update. GNSS Doppler observations are less affected by multipath effects and NLOS, so the innovation chi-square test is used for gross error detection and elimination.

[0138] Further, based on the constructed GNSS single-difference pseudo-range and single-difference Doppler measurement model, the residual and Jacobian are calculated, and the MSCKF state vector and its covariance are updated.

[0139] Specifically, the predicted value of the single-difference pseudo-range observation of satellite s can be written as:

[0140]

[0141]

[0142] wherein, is the rover estimated by the fusion positioning method; is the IMU position estimated by the fusion positioning method; is the estimated IMU attitude; is the rod arm of the GNSS antenna relative to the IMU; is the position of satellite s in the w system; is the position of the GNSS base station in the w system; receiver clock bias of the satellite system sys estimated by the fusion positioning method.

[0143] The predicted value of the single-difference Doppler observation of the satellite s can be written as:

[0144]

[0145]

[0146] where λ is the carrier wavelength; is the unit line-of-sight vector from the rover to the satellite s, is the unit line-of-sight vector from the base station to the satellite s; and are the velocities of the satellite s and the GNSS base station respectively, and the base station velocity is zero when a static base station is used; and are the velocities of the rover and the IMU estimated by the fusion positioning method respectively; is the angular velocity of the IMU.

[0147] The single-difference pseudorange observation residual of the satellite s and the single-difference Doppler observation residual can be written as:

[0148]

[0149]

[0150] where, is the predicted value of the single-difference pseudorange observation, is the predicted value of the single-difference Doppler observation.

[0151] where the residuals are error perturbed, the Jacobians are solved, and the MSCKF state vector and its covariance are updated using the standard ESKF measurement update equation.

[0152] To ensure the implementability of the embodiments of the present application, the following is described based on specific experimental test verification:

[0153] The embodiments of the present application use a wheeled robot to test in complex environments including boulevards, high-rise obstructions and the like, to verify the feasibility and advancement of the present application. The sensors used include a solid-state laser radar Livox Mid-70, a MEMS IMU ADIS16465, a navigation receiver Ublox F9P as a flow station, and a measurement receiver Panda PD318 as a base station, and a navigation-level IMU POS-A15 is also installed on the robot, which is used in combination with GNSS RTK to provide reference true values, and these sensors are well synchronized through hardware triggers. In order to verify the positioning performance of the multi-epoch robust GNSS / LiDAR / INS tight integration positioning method based on MSCKF of the present application, the positioning error is evaluated.

[0154] Figure 6 to Figure 8 The north, east and vertical positioning errors of GLIO and the present application are compared respectively, wherein GLIO is a representative open-source tight integration GNSS / LiDAR / INS fusion positioning algorithm based on factor graph optimization, which uses GNSS pseudorange and Doppler observations and performs inter-station difference, and it can be seen that the north, east and vertical positioning errors of the present application are smaller than those of GLIO, mainly due to the following three reasons: (1) GLIO needs to explicitly extract planar features, which performs poorly in unstructured scenes such as boulevards, while the direct method used in the present application does not need to explicitly extract planar features, but all points are regarded as candidate planar points, so that effective feature association can be achieved even in unstructured scenes; (2) the LiDAR measurement model used by GLIO only correlates two LiDAR key frames and does not construct multi-state constraints, while the multi-state constraint LiDAR measurement model of the present application constructs relative constraints between multiple frames, which has higher relative accuracy; (3) in detecting and rejecting GNSS pseudorange gross errors, GLIO does not fully utilize the relative accuracy of LIO, the dimension of the estimated parameters of the optimization problem is high, and it is easy to fall into local optimum, so it is difficult to accurately detect and reject gross errors, while the multi-epoch robust method proposed in the present application fully utilizes the high-precision relative pose provided by LIO, reduces the estimated parameters, and improves the accuracy of gross error detection and rejection.

[0155] The test results show that the present application uses MSCKF, multi-state constraint LiDAR measurement model and multi-epoch robustness to improve the availability of GNSS and the positioning accuracy of GNSS / LiDAR / INS fusion positioning system in complex environments, and meets the accurate and reliable real-time navigation and positioning requirements of robots and autonomous vehicles in complex environments.

[0156] In summary, based on the above specific embodiment, the present application can achieve the following beneficial effects:

[0157] (1) The method of the present application uses MSCKF to fuse GNSS, LiDAR and INS at the level of measurement, taking into account positioning accuracy and computational efficiency.

[0158] (2) The multi-state constraint LiDAR measurement model used in the application makes full use of the high-precision ranging capability of the LiDAR sensor, significantly improves the relative positioning accuracy, and effectively solves the problem of rapid divergence of positioning when the quality of GNSS observation is poor or even unavailable.

[0159] (3) The method of the present application makes full use of the high-precision relative positioning capability of LiDAR and IMU to assist GNSS multi-epoch robustness, which can effectively improve the availability of GNSS in complex urban environments.

[0160] According to the tight combination positioning method of the multi-epoch robustness navigation system based on MSCKF, the base station and the flow station observation of the first navigation device, the point cloud of the second navigation device and the specific force and angular velocity of the inertial measurement unit are received; the specific force and angular velocity are used to mechanically arrange the third navigation device, and then the state vector and covariance of the MSCKF are predicted; based on the prediction result, the prior pose of the third navigation device is used to preprocess the point cloud data, and multi-frame data association is performed to construct the multi-state constraint second navigation device measurement model of the MSCKF; the multi-epoch robustness is used to detect and eliminate the gross error of the first navigation device observation, and then the pseudo-range and Doppler measurement model of the first navigation device is constructed, and the measurement update is performed on the state vector and covariance of the MSCKF, and the first navigation device, the second navigation device and the third navigation device are controlled according to the update result to perform tight combination positioning. Thus, the problems of limited constraint capability of the multi-epoch robustness method in the related art, which limits the GNSS robustness performance, and simultaneously leads to high dimension of the to-be-estimated parameter space, increase of local minimum, and high probability of the parameter optimization process falling into local optimum are solved.

[0161] Secondly, the tight combination positioning device of the multi-epoch robustness navigation system based on MSCKF is described with reference to the accompanying drawings according to the embodiment of the present application.

[0162] Figure 9 is a block schematic diagram of the tight combination positioning device of the multi-epoch robustness navigation system based on MSCKF according to the embodiment of the present application.

[0163] As shown in Figure 9 , the tight combination positioning device 10 of the multi-epoch robustness navigation system based on MSCKF includes a receiving module 100, a prediction module 200, a model construction module 300 and a control module 400.

[0164] The receiving module 100 is configured to receive base station data and flow station observation data of the first navigation device, point cloud data of the second navigation device, and specific force data and angular velocity data of the inertial measurement unit of the third navigation device.

[0165] The prediction module 200 is configured to mechanically arrange the third navigation device by using the specific force data and the angular velocity data of the inertial measurement unit, and obtain a running state of the third navigation device based on an arrangement result; and predict a state vector and a covariance of the MSCKF by using the running state of the third navigation device, to obtain a prediction result of the MSCKF.

[0166] The model construction module 300 is configured to perform data preprocessing on the point cloud data of the second navigation device by using a prior pose of the third navigation device based on the prediction result of the MSCKF, and perform multi-frame data association on the point cloud data of the second navigation device, to construct a multi-state constraint second navigation device measurement model of the MSCKF.

[0167] The control module 400 is configured to perform rough error detection and elimination processing on the first navigation device observation by using a multi-epoch robustness method, and construct a pseudo-range and Doppler measurement model of the first navigation device based on the first navigation device observation after the rough error detection and elimination processing, to perform measurement update on the state vector and the covariance of the MSCKF based on the prediction result of the MSCKF, the multi-state constraint second navigation device measurement model, and the pseudo-range and Doppler measurement model, and control the first navigation device, the second navigation device, and the third navigation device to perform tight combination positioning according to an update result.

[0168] According to an embodiment of the present application, the state vector of the MSCKF includes a first navigation device error state, a second navigation device error state, and a third navigation device error state.

[0169] According to an embodiment of the present application, the model construction module 300 includes:

[0170] The correction unit is configured to correct motion distortion of the point cloud data based on the prior pose of the third navigation device.

[0171] The acquisition unit is configured to acquire displacement data and a rotation angle of the vehicle, and select key frame point cloud data of the second navigation device based on the displacement data, the rotation angle, and a preset interval length.

[0172] The projection unit is configured to project and accumulate non-key frame point cloud data of the second navigation device to the key frame point cloud data by using the prior pose of the third navigation device, to obtain a key frame point cloud map of the second navigation device at a current time.

[0173] The first model construction unit is configured to perform multi-frame data association on the key frame point cloud map at the current moment and the historical key frame point cloud map, and construct a multi-state constraint second navigation device measurement model of MSCKF.

[0174] According to an embodiment of the present application, after the multi-state constraint second navigation device measurement model of MSCKF is constructed, the model construction unit further comprises:

[0175] The update sub-unit is configured to calculate the residual and Jacobian of the multi-state constraint second navigation device measurement model, and update the state vector and covariance of MSCKF based on the residual and Jacobian of the multi-state constraint second navigation device measurement model.

[0176] The acquisition sub-unit is configured to add the key frame point cloud map at the current moment to the key frame point cloud map sliding window of the second navigation device, and acquire the pose error state of the inertial measurement unit in the key frame point cloud map at the current moment.

[0177] The judgment sub-unit is configured to judge whether the number of key frame point cloud data in the key frame point cloud map sliding window exceeds the preset maximum sliding window length.

[0178] The marginalization sub-unit is configured to, if the number of key frame point cloud data exceeds the preset maximum sliding window length, delete the earliest stored key frame point cloud map, and augment the pose error state to the state vector and covariance of MSCKF, and perform a marginalization operation on the state vector and covariance of MSCKF.

[0179] According to an embodiment of the present application, the control module 400 comprises:

[0180] The second model construction unit is configured to receive the first navigation device base station observation and the first navigation device flow station observation, and construct an inter-station single difference observation by using the first navigation device base station observation and the first navigation device flow station observation.

[0181] The detection and rejection unit is configured to perform rough error detection and rejection processing on the pseudo-range observation in the first navigation device observation by using the multi-state constraint second navigation device measurement model and the relative pose of the third navigation device, and perform rough error detection and rejection processing on the Doppler observation in the first navigation device observation by using the innovation chi-square test method.

[0182] The tight combination positioning device of the multi-epoch robust navigation system based on MSCKF according to the embodiment of the application receives base station and rover station observations of a first navigation device, point cloud and specific force and angular velocity of an inertial measurement unit of a second navigation device; the specific force and angular velocity are used for mechanical arrangement of a third navigation device, and then the state vector and covariance of the MSCKF are predicted; based on the prediction result, the point cloud data are preprocessed using the priori pose of the third navigation device, multi-frame data association is performed, and a multi-state constraint second navigation device measurement model of the MSCKF is constructed; multi-epoch robustness is used for gross error detection and elimination of the first navigation device observation, then a pseudo-range and Doppler measurement model of the first navigation device is constructed, and the state vector and covariance of the MSCKF are measured and updated, and the first navigation device, the second navigation device and the third navigation device are controlled to perform tight combination positioning according to the update result. Therefore, the problems that the constraint ability of the multi-epoch robustness method in the related art is limited, the GNSS robustness performance is limited, the dimension of the to-be-estimated parameter space is high, the local minimum value is increased, and the probability that the parameter optimization process falls into local optimization is large are solved.

[0183] Figure 10 The structural schematic diagram of the electronic device provided by the embodiment of the application is provided. The electronic device can include:

[0184] The memory 1001, the processor 1002 and the computer program stored in the memory 1001 and executable on the processor 1002.

[0185] The processor 1002 implements the tight combination positioning method of the multi-epoch robust navigation system based on MSCKF provided in the above embodiment when executing the program.

[0186] Further, the electronic device further includes:

[0187] The communication interface 1003 is used for communication between the memory 1001 and the processor 1002.

[0188] The memory 1001 is used for storing the computer program executable on the processor 1002.

[0189] The memory 1001 can include a high-speed RAM memory, and can also include a non-volatile memory, for example, at least one disk memory.

[0190] If the memory 1001, the processor 1002 and the communication interface 1003 are implemented independently, the communication interface 1003, the memory 1001 and the processor 1002 can be connected with each other through a bus and complete communication between each other. The bus can be an Industry Standard Architecture (ISA) bus, a Peripheral Component Interconnect (PCI) bus or an Extended Industry Standard Architecture (EISA) bus, etc. The bus can be divided into an address bus, a data bus, a control bus, etc. For convenience of representation, Figure 10 Only one thick line is used in the figure to represent the bus, but it does not mean that there is only one bus or only one type of bus.

[0191] Optionally, in a specific implementation, if the memory 1001, the processor 1002 and the communication interface 1003 are integrated on a chip, the memory 1001, the processor 1002 and the communication interface 1003 can complete communication between each other through an internal interface.

[0192] The processor 1002 can be a Central Processing Unit (CPU), or an Application Specific Integrated Circuit (ASIC), or one or more integrated circuits configured to implement one or more embodiments of the present application.

[0193] The embodiment further provides a computer readable storage medium, having stored thereon a computer program, which, when executed by a processor, implements the method of the above MSCKF-based multi-epoch robust navigation system tight combination positioning method.

[0194] The embodiment of the present application further provides a computer program product, comprising computer programs / instructions, which, when executed by a processor, implement the method of the above MSCKF-based multi-epoch robust navigation system tight combination positioning method.

[0195] In the description of the application, reference to "one embodiment", "some embodiments", "an example", "a specific example", or "some examples" means that a particular feature, structure, material, or characteristic being described is included in at least one embodiment or example of the application. The appearances of the phrase in various places in the specification are not necessarily all referring to the same embodiment or example. Furthermore, the described specific features, structures, materials, or characteristics can be combined in any suitable manner in one or more embodiments or examples. In addition, the usage of "N" means at least two, for example two, three or the like, unless explicitly stated otherwise.

[0196] Furthermore, the terms "first", "second", or the like, are used merely as a designation of certain elements or features of the application, and do not imply or connote relative importance or a specific order of precedence. Thus, features defined with "first", "second", etc. can include at least one of the features, either explicitly or implicitly.

[0197] Any process or method descriptions or blocks in flow charts or otherwise described herein represent embodiments of modules, segments, or portions of code which include one or more executable instructions for implementing specific logic functions or steps, and alternate implementations are possible. In some embodiments, the processes or methods described in flow charts or otherwise described herein are not necessarily performed in the order shown or discussed, including, for example, as performed by a computer processor. Alternate implementations can perform functions or steps described in different orders, including substantially concurrently, or in reverse order.

[0198] The logic and / or steps represented in the flowcharts and / or described herein, for example, can be considered as a sequence of instructions to implement logical functions, and can be embodied in any computer-readable medium for use by an instruction execution system, apparatus, or device, such as a computer-based system, processor- based system, or other system that can fetch the instructions from the instruction execution system, apparatus, or device and execute the instructions. For purposes of this specification, a "computer-readable medium" can be any apparatus that can contain, store, communicate, propagate, or transport the program for use by or in connection with the instruction execution system, apparatus, or device. The computer-readable medium can be a computer- readable storage medium or a computer-readable signal medium. The computer- readable storage medium can be, for example, but not limited to, an electronic, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or any suitable combination of the foregoing. More specific examples (a non-exhaustive list) of the computer-readable storage medium include the following: an electrical connection having one or more wires (electrical connections), a portable computer diskette (a magnetic device), a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or Flash memory), an optical fiber, and a portable compact disc read-only memory (CDROM). In addition, the computer-readable medium can even be paper or another suitable medium upon which the program is printed, as the program can be electronically captured, for example, via optical scanning of the paper or other medium, then compiled, interpreted, or otherwise processed in a suitable manner, if necessary, and stored in a computer memory.

[0199] It should be understood that aspects of the application can be implemented in hardware, software, firmware or combinations thereof. In the above embodiments, the N steps or methods can be implemented in software or firmware stored in a memory and executed by a suitable instruction execution system. As such, if implemented in hardware, and in another embodiment, any of the following technologies, known in the art, or their combinations can be used: discrete logic circuitry having logic gates for implementing logic functions on data signals, application specific integrated circuits having appropriate combinational logic gates, programmable gate arrays (PGA), field programmable gate arrays (FPGA), and the like.

[0200] Those skilled in the art can understand that all or part of the steps carried out by the above-mentioned embodiment methods can be completed by programs instructing related hardware, and the programs can be stored in a computer-readable storage medium. When the programs are executed, they include one or a combination of the steps of the method embodiments.

[0201] In addition, each of the functional units in the various embodiments of the present application can be integrated in one processing module, or each of the units can be physically present separately, or two or more units can be integrated in one module. The integrated module can be implemented in the form of hardware or in the form of a software functional module. When the integrated module is implemented in the form of a software functional module and sold or used as an independent product, it can also be stored in a computer readable storage medium.

[0202] The storage medium mentioned above can be a read-only memory, a magnetic disk or an optical disk, etc. Although the embodiments of the present application have been shown and described above, it should be understood that the above embodiments are exemplary and should not be construed as limiting the present application, and those skilled in the art can make changes, modifications, replacements and variations to the above embodiments within the scope of the present application.

Claims

1. A compact combination positioning method for a multi-epoch robust navigation system based on MSCKF, characterized in that, Includes the following steps: It receives base station data and rover observation data from the first navigation device, point cloud data from the second navigation device, and force and angular velocity data from the inertial measurement unit. The mechanical arrangement of the third navigation device is performed using the specific force data and angular velocity data of the inertial measurement unit, and the operating state of the third navigation device is obtained based on the arrangement result. Based on the operating state of the third navigation device, the state vector and covariance of the MSCKF are predicted to obtain the prediction result of the MSCKF. Based on the prediction results of the MSCKF, the point cloud data of the second navigation device is preprocessed using the prior pose of the third navigation device, and multi-frame data association is performed on the point cloud data of the second navigation device to construct the multi-state constraint measurement model of the second navigation device based on the MSCKF. The multi-epoch robustness method is used to detect and eliminate gross errors in the observations of the first navigation device. Based on the observations of the first navigation device after gross error detection and elimination, a pseudorange and Doppler measurement model of the first navigation device is constructed. The state vector and covariance of the MSCKF are measured and updated based on the prediction results of the MSCKF, the multi-state constrained measurement model of the second navigation device, and the pseudorange and Doppler measurement model. Based on the update results, the first navigation device, the second navigation device, and the third navigation device are controlled to perform tight-combination positioning.

2. The method according to claim 1, characterized in that, The state vector of the MSCKF includes the error states of the first navigation device, the second navigation device, and the third navigation device.

3. The method according to claim 1, characterized in that, Based on the prediction results of the MSCKF, the point cloud data of the second navigation device is preprocessed using the prior pose of the third navigation device, and multi-frame data association is performed on the point cloud data of the second navigation device to construct the multi-state constrained measurement model of the second navigation device based on the MSCKF, including: The motion distortion of the point cloud data is corrected based on the prior pose of the third navigation device. The vehicle's displacement data and rotation angle are acquired, and key frame point cloud data of the second navigation device are selected based on the displacement data, the rotation angle, and a preset interval. The non-keyframe point cloud data of the second navigation device is projected onto the keyframe point cloud data using the prior pose of the third navigation device and accumulated into the keyframe point cloud data to obtain the keyframe point cloud map of the second navigation device at the current moment. Multi-frame data association is performed on the current keyframe point cloud map and the historical keyframe point cloud map to construct the multi-state constraint second navigation device measurement model of the MSCKF.

4. The method according to claim 3, characterized in that, After constructing the multi-state constrained second navigation device measurement model of the MSCKF, the following is also included: Calculate the residuals and Jacobian of the measurement model of the multi-state constrained second navigation device, and update the state vector and covariance of the MSCKF based on the residuals and Jacobian of the measurement model of the multi-state constrained second navigation device; Add the current keyframe point cloud map to the keyframe point cloud map slider of the second navigation device, and obtain the pose error status of the inertial measurement unit in the current keyframe point cloud map. Determine whether the number of keyframe point cloud data in the keyframe point cloud map sliding window exceeds the preset maximum sliding window length; If the number of keyframe point cloud data exceeds the preset maximum sliding window length, the first stored keyframe point cloud map is deleted, and the pose error state is augmented to the state vector and covariance of the MSCKF, and the state vector and covariance of the MSCKF are marginalized.

5. The method according to claim 1, characterized in that, The process of detecting and eliminating gross errors in the observations of the first navigation device using the multi-epoch robustness method includes: Receive observations from the base station of the first navigation device and observations from the rover of the first navigation device, and construct inter-station single-difference observations using the observations from the base station of the first navigation device and observations from the rover of the first navigation device; Based on the inter-station single-difference observations, the pseudorange observations in the first navigation device observations are subjected to gross error detection and elimination using the multi-state constrained second navigation device measurement model and the relative pose of the third navigation device. At the same time, the chi-square test method is used to perform gross error detection and elimination using the Doppler observations in the first navigation device observations.

6. A tightly coupled positioning device for a multi-epoch robust navigation system based on MSCKF, characterized in that, include: The receiving module is used to receive base station data and rover observation data from the first navigation device, point cloud data from the second navigation device, and force and angular velocity data from the inertial measurement unit. The prediction module is used to mechanically arrange the third navigation device using the specific force data and angular velocity data of the inertial measurement unit, and obtain the operating state of the third navigation device based on the arrangement result. The operating state of the third navigation device is used to predict the state vector and covariance of the MSCKF to obtain the prediction result of the MSCKF. The model building module is used to preprocess the point cloud data of the second navigation device based on the prediction results of the MSCKF and the prior pose of the third navigation device, and to perform multi-frame data association on the point cloud data of the second navigation device to build the multi-state constraint measurement model of the second navigation device based on the MSCKF. The control module is used to perform gross error detection and elimination processing on the observations of the first navigation device using the multi-epoch robust method, and to construct a pseudorange and Doppler measurement model of the first navigation device based on the observations of the first navigation device after gross error detection and elimination processing. The module then updates the state vector and covariance of the MSCKF based on the prediction results of the MSCKF, the multi-state constrained measurement model of the second navigation device, and the pseudorange and Doppler measurement model. Based on the update results, the module controls the first navigation device, the second navigation device, and the third navigation device to perform tight-fitting positioning.

7. The apparatus according to claim 6, characterized in that, The state vector of the MSCKF includes the first navigation device error state, the second navigation device error state, and the third navigation device error state.

8. The apparatus according to claim 6, characterized in that, The model building module includes: The correction unit is used to correct the motion distortion of the point cloud data based on the prior pose of the third navigation device; The acquisition unit is used to acquire the vehicle's displacement data and rotation angle, and select key frame point cloud data of the second navigation device based on the displacement data, the rotation angle and a preset interval time. The projection unit is used to project the non-keyframe point cloud data of the second navigation device onto the keyframe point cloud data using the prior pose of the third navigation device and accumulate it to the keyframe point cloud data to obtain the keyframe point cloud map of the second navigation device at the current moment. The model building unit is used to perform multi-frame data association between the current keyframe point cloud map and the historical keyframe point cloud map to build the multi-state constraint second navigation device measurement model of the MSCKF.

9. An electronic device, characterized in that, include: The system includes a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the program to implement the compact combination positioning method of the multi-epoch robust navigation system based on MSCKF as described in any one of claims 1-5.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that, The program is executed by the processor to implement the compact combination positioning method of the multi-epoch robust navigation system based on MSCKF as described in any one of claims 1-5.

Images