A Beidou multi-source fusion positioning method in disaster environment

Abstract

The present invention proposes a Beidou multi-source fusion positioning method under a disaster environment, comprising the following steps: step 1, using a camera on a carrier to be positioned under the disaster environment to collect images, and constructing a VIO factor based on point and line feature extraction; step 2, constructing a map and performing fine correction on it under GNSS / IMU constraints to obtain a global map; step 3, expanding the global map with visual factors, and performing GNSS / INS / Vision combined navigation global pose optimization solution to complete Beidou multi-source fusion positioning under the disaster environment; the method realizes the real-time and refinement of disaster scene map construction, and through the real-time generated global map prediction, selects appropriate visual points to expand and add and delete the factor graph to adapt to the dynamic environmental changes under natural disasters, thereby improving the robustness and accuracy of the system providing positioning for rescue vehicles.

Description

Technical Field

[0001] The present invention relates to a multi-source fusion positioning method, in particular to a Beidou multi-source fusion positioning method under disaster environment. Background Art

[0002] Among various positioning methods, GNSS (Global Navigation Satellite System) can provide all-weather global reference positioning, but the applicability and operability of navigation and positioning receiving equipment in disaster emergency environments will be reduced, and it is easily affected by physical occlusion and electromagnetic interference in complex disaster environments, resulting in reduced accuracy; IMU (Inertial Measurement Unit) can provide high-precision posture derivation in a short period of time, but its error will gradually accumulate and increase over time. GNSS / IMU combined navigation can be complementary to a certain extent, but it still cannot meet user needs when GNSS is interrupted for a long time. Visual sensors can autonomously locate and build maps, and are suitable for unknown and complex environments in disasters. They greatly make up for the shortcomings in the GNSS / IMU combined navigation process. Therefore, multi-source fusion positioning of cameras with GNSS and IMU to improve the positioning accuracy of the system has become a research hotspot in recent years.

[0003] Zhu et al. used a priori feature maps to assist the GNSS / INS / Vision integrated navigation system, added visual matching landmarks and lane line constraints to the algorithm, and achieved continuous high-precision positioning in complex urban environments. However, the system separated the construction of maps and positioning and ran them one after another, ignoring the impact of map updates and sudden changes in road conditions on real-time positioning, resulting in the system's final positioning being unable to guarantee its reliability and real-time performance. Zhai and Yua proposed a semantic-based dynamic adaptive Kalman filter fusion algorithm for visual-assisted GNSS positioning to form a loose combination system of RTK / Vision. Although the use of deep learning in the SLAM process to add semantic information to the filter algorithm can improve positioning accuracy, it still does not solve the positioning needs under long-term GNSS interruptions. Narula et al. proposed an algorithm that adds GNSS position estimation to the VSLAM framework for joint optimization positioning, matches and updates the pose results of the improved GNSS positioning model with Vision, and then builds a high-precision map. The introduction of high-precision GNSS positioning can indeed improve the positioning accuracy of the system, but in the integrated navigation, the lack of consideration of the factors affecting GNSS positioning and the connection between vision and map points leads to the inability to achieve a full range of pose deviation reduction.

[0004] Gong et al. proposed a tight combination method of GNSS and VSLAM on manifolds, using asynchronous tracking based on motion decomposition to fuse GNSS pseudorange measurements and visual feature points to adapt to heterogeneous sensor observations. This fusion method can significantly improve the positioning accuracy of the system, and its performance is better than that of the loosely coupled method. Chu et al. proposed a GNSS / Vision / IMU integrated navigation algorithm based on EKF, which can perform multi-state update solutions, but EKF only uses the latest state estimate, and the nonlinear factors representing the measurement values ​​cannot be well relinearized. Zhang and Wei proposed a GNSS / INS / OD integrated navigation algorithm based on factor graphs, which improves the robustness of the system by judging whether the sensor effectively adds or deletes the corresponding factor nodes. Chen and Xu et al. proposed a GNSS / Vision / IMU integrated navigation algorithm based on factor graphs, which uses the GNSS solution position as prior information, enters the factor graph node and provides constraints for the factors of the VIO (Visual-Inertial Odometry) system. The algorithm can further constrain the global trajectory accuracy, but does not consider the situation where GNSS is interrupted or interfered in complex environments and cannot be positioned, and its robustness cannot be guaranteed.

[0005] In summary, the prior art has the following defects:

[0006] (1) In the existing GNSS / IMU / Vision combined navigation, open source maps are often used. However, such maps are updated late and have insufficient accuracy, especially unable to respond to changes in surrounding scenes in disaster environments in a timely manner. Although the visual algorithm is constantly being improved, it has not closely combined the positioning characteristics of GNSS with the effective information of visual composition, and ultimately cannot further provide reasonable and effective visual factors for global positioning.

[0007] (2) The existing GNSS / IMU / Vision integrated navigation uses factor graph optimization to improve positioning accuracy. However, in disaster environments, there is strong uncertainty and the environment changes frequently. The existing algorithms do not take into account the situation where positioning failure or positioning error may occur due to interference from dynamic objects in the changing environment. In recent years, factor graphs have been gradually used in fusion algorithms, but the existing factor graph-based fusion does not fully utilize the feedback effect of the global map on factor graph optimization, which ultimately leads to low algorithm accuracy and robustness. Summary of the invention

[0008] Purpose of the invention: The technical problem to be solved by the present invention is to provide a Beidou multi-source fusion positioning method in a disaster environment in view of the shortcomings of the prior art.

[0009] In order to solve the above technical problems, the present invention discloses a Beidou multi-source fusion positioning method in a disaster environment, comprising the following steps:

[0010] Step 1, using a camera on the carrier to be located in the disaster environment to collect images, and constructing a VIO factor based on point and line feature extraction;

[0011] Step 2: Map construction and fine-tuning under GNSS / IMU constraints to obtain a global map.

[0012] Step 3: Expand the visual factors of the global map and perform global pose optimization solution of GNSS / INS / Vision combined navigation to complete Beidou multi-source fusion positioning in disaster environments.

[0013] Furthermore, the VIO factor is constructed based on point and line feature extraction described in step 1, specifically including:

[0014] Step 1-1, point and line feature extraction and key frame judgment;

[0015] Step 1-2, construct visual feature reprojection residual factor;

[0016] Step 1-3, construct IMU pre-integration residual factor;

[0017] Steps 1-4, construct VIO factors.

[0018] Furthermore, the point and line feature extraction and key frame determination described in step 1-1 specifically include:

[0019] Step 1-1-1, point and line feature extraction: Based on the acquired image, use FAST point feature extraction and LSD line feature extraction to obtain point and line features; match the point and line features with the help of BRIEF descriptor, and use the RANSAC method to eliminate the results of incorrect matching of point and line features;

[0020] Step 1-1-2, determine the key frame: set the collected image to a set of continuous frames F = {F 1 ,F 2 ,F 3 ,...,F i ,},F i Represents the i-th frame, for each frame image F i , including the following parameters: the corresponding timestamp T i And the number of point and line features contained in the frame image b i , using K n Represents the nth key frame, and the i-th frame image F is judged according to the following criteria i Is it a key frame?

[0021] First criterion: the i-th frame image F i The time interval between adjacent key frames is greater than the specified threshold, that is, Indicates the timestamp of the most recent key frame, T 0 is the set time difference threshold;

[0022] The second criterion: the number of point and line features contained in the key frame is greater than the specified threshold, that is, b i >b 0 , b 0 is the set point and line feature quantity threshold;

[0023] For image frames that are judged as non-key frames, they are directly discarded;

[0024] Step 1-1-3, feature matching and screening: After finding the key frame, the KNN algorithm is used for feature matching, that is: for a feature in one frame of the image, the most similar feature is found in the other frame of the image, and then they are regarded as a matching pair; in the above matching process, the RANSAC algorithm is used to eliminate false matches.

[0025] Furthermore, the construction of the visual feature reprojection residual factor described in step 1-2, that is, establishing a visual feature reprojection residual model, taking the distance between the projection point of the map point in the image and the corresponding observation point as the visual feature reprojection residual, specifically includes:

[0026] Step 1-2-1, point feature reprojection residual, as follows:

[0027] For the lth feature point L, let it be in the i-th frame F i The normalized coordinates observed in the corresponding i-th camera coordinate system are By using the image feature tracking method, we can get the jth frame F j The normalized coordinates of the same feature point observed in the corresponding j-th camera coordinate system are The feature point L is transformed from the i-th camera coordinate system where it is first observed to the normalized coordinates of the camera coordinate system j where it is observed again, that is, the observed prediction value is The visual observation residual term is the difference between the predicted value and the actual observed value Right now:

[0028]

[0029] in, represents the global state vector of the local pose of VIO in the world coordinate system, Represents the point feature observation of the camera;

[0030]

[0031] Among them, x n Includes the carrier position provided by the nth IMU direction speed Acceleration bias b an and gyro bias b gn ; f, p and l represent the total number of image frames, point features and line features in the sliding window respectively; λ i+m Represents the inverse depth between the mth feature point and the initial feature point; o i+w represents the orthogonal representation of the w-th feature line in the world coordinate system;

[0032] Step 1-2-2, line feature reprojection residual, that is, the distance from the end point of the line observed by the camera plane to the reprojected line is taken as the residual, as follows:

[0033] Assume that the i-th camera captures the i-th frame F i The camera at time , observes the straight line L k , the straight line L k Projected onto the normalized plane of the i-th key frame to obtain the projection line equation The two endpoints of the straight line observed by the camera plane are and Let d(x,L) represent the distance from point x to line L, then the line feature reprojection residual is for:

[0034]

[0035] Furthermore, the construction of the IMU pre-integration residual factor described in steps 1-3 specifically includes:

[0036] Step 1-3-1, establish the IMU pre-integration residual model, as follows:

[0037] The IMU integral between two adjacent key frames is calculated in the previous key frame F i The IMU carrier coordinate system is used to obtain the value independent of the key frame F i The relative motion increment of the state, where the zero bias change of the IMU is processed using a first-order linear approximation; in the IMU coordinate system, the IMU measurement equation is:

[0038]

[0039] in, is the measured acceleration value at time t, a t is the noise-free acceleration at time t, is the random walk of acceleration at time t, is the coordinate transformation matrix, g w is the acceleration due to gravity, n a is the accelerometer white noise; is the gyroscope measurement value at time t, wt is the noise-free angular velocity at time t, is the angular velocity random walk at time t, n w is the gyroscope white noise;

[0040] According to the above IMU measurement equation integration and coordinate system transformation, the IMU pre-integration is as follows:

[0041]

[0042] in, For integration i to j The relative position, speed and rotation at the moment, the intermediate term Ω(w) is calculated as follows:

[0043]

[0044] Where w represents the gyroscope measurement value, × represents the matrix antisymmetric operation, and T represents the matrix transpose operation;

[0045] Step 1-3-2, establish the IMU pre-integration residual factor, as follows:

[0046] After IMU pre-integration, the residual between the system state and the IMU measurement as follows:

[0047]

[0048] Among them, δ represents the difference between the measured value and the predicted value, Represents the IMU observation.

[0049] Furthermore, the specific method of constructing the VIO factor described in steps 1-4 includes:

[0050] According to the point feature reprojection residual, line feature reprojection residual and IMU pre-integration residual, the least squares method is used for joint optimization, as follows:

[0051]

[0052] in, Indicates the corresponding state value r of the optimal solution p and H p is the prior information, r B is the IMU pre-integration residual, r l is the point feature reprojection residual, is the line feature reprojection residual; B is the IMU measurement value set, C 1 and C 2 is a collection of point and line feature measurement values, and is the corresponding covariance matrix;

[0053] According to the above optimization results, using t i-1 Time to t i The VIO position increment and posture increment between moments are modeled, and the measurement model is obtained as follows:

[0054]

[0055] Among them, y δR,i and Δp,i is the measured value of the visual odometer attitude increment and position increment, n δR,i Attitude increment measurement noise, R i-1 and R i is the attitude measurement value at adjacent moments, P i-1 and P i is the position measurement value at adjacent moments, n Δp,i is the position increment measurement noise; then the VIO factor for:

[0056]

[0057] in, represents the VIO system measurement value, r δR,i and r Δp,i Represent the measurement values ​​of the visual inertial odometry attitude increment and position increment respectively.

[0058] Furthermore, the map construction described in step 2 is refined and corrected under GNSS / IMU constraints, specifically including:

[0059] Step 2-1, preliminary map construction, as follows:

[0060] After key frame judgment, the key frame image after camera image screening is transformed into three-dimensional space through LSS method, and the pixel point (u, v) in the image is mapped to the corresponding global coordinate system point (X w ,Y w ,Z w ), as follows:

[0061] [uv 1] T =KRT[X w Y w Z w 1] T

[0062] Among them, R is the camera rotation matrix, T is the camera translation vector, and K is the camera intrinsic parameter matrix;

[0063]

[0064] Where h is the camera height and θ is the camera pitch angle;

[0065] Step 2-2, open area recognition, uses the convolutional neural network semantic segmentation method to assign labels to the mapping elements, i.e., the map points in the three-dimensional space, i.e., the global coordinate system points described in step 2-1, and judge the density of the objects with the same label, as follows:

[0066] The open area is defined as a building in which the width-to-height ratio of high-rise buildings is greater than 1:1, and the density of buildings meeting the height requirements in the same area is less than a preset threshold;

[0067] If a road is identified in the open area, a point on the road is selected as the center of the open area and a new label is assigned to it: open area reference point; if the open area is an impassable area, the building closest to the drivable road in the open area is assigned the above new label: open area reference point;

[0068] After using the SSD convolutional neural network semantic segmentation method, the detection targets in the image are assigned classification labels and two-dimensional position information; according to the LSS method in step 2-1, the above classification labels are mapped to three-dimensional positions;

[0069] Step 2-3, GNSS / IMU constraint refinement, that is: according to the previous training results of the convolutional neural network semantic segmentation algorithm, during the real-time positioning process, the open area is identified while the map is constructed. According to the open area reference point labels, when it is impossible to pass through or approach the area, the GNSS / IMU loose combination positioning results are used to correct the constraints;

[0070] Steps 2-4, global high-precision map optimization and construction, are as follows:

[0071] The continuously generated local maps are determined to be repeated through loop detection. If repeated, the local maps are updated according to nonlinear optimization. This is achieved by solving the following nonlinear least squares problem using the Gauss-Newton method:

[0072]

[0073] Among them, e ij (x i ,x j ,z ij ) is from the pose x i In place x j The observed data z ij The error between the pose estimate and the ij is the information matrix of the observed data, ρ(·) is the robust kernel function;

[0074] If the areas passed through are not repeated, the common view relationship is obtained based on the perceptual similarity in the BEV perception process. The common view relationship is represented by the matrix V, V ij Represents the pose x i In place x j The degree of co-viewing between them; Find similar frames F based on the co-viewing relationship i With F j The feature point P under i and P j , and calculate the Euclidean distance d(P i ,P j ) Find the same feature points as follows:

[0075]

[0076] When the Euclidean distance is less than the specified threshold, the feature point P is judged i and P j Same, generate a global map based on the same feature points found by the common view relationship;

[0077] Step 2-5, global factor expansion based on feature matching, that is: using the generated or updated map points to expand the visual feature state vector, and generating new visual reprojection residual factors as auxiliary positioning information to enter the global factor graph optimization process; when using the map for visual factor expansion, only the feature point reprojection residual is expanded.

[0078] Furthermore, the GNSS / IMU constraints described in step 2-3 are refined as follows:

[0079] When passing through the open area reference point, the GNSS and IMU combined navigation obtains high-precision pose, corrects the pose of the point predicted earlier, and further provides new constraints for map construction after this moment: the position and pose information provided by GNSS / IMU is converted from the world coordinate system to the camera coordinate system, and further converted to the pixel coordinate system through the camera's built-in transformation matrix, and jointly optimized with the previously predicted visual target point at the same position, that is, the value η of the following formula is minimized:

[0080]

[0081] Among them, P is the target point coordinate for joint optimization of GNSS / IMU observation points and map points, K is the camera internal parameter, z is the depth of the feature point, P c are the prior transformation matrix information and map feature point location information in the process of visual image processing and map point generation, respectively. i The coordinates of the GNSS / IMU positioning and the observation values ​​composed of the left and right image disparity are converted to the pixel coordinate system through the coordinate system.

[0082] Furthermore, the global map expansion visual factor described in step 3 specifically includes:

[0083] Step 3-1, construct pseudorange factor and Doppler velocity factor, as follows:

[0084] The pseudorange measurement model is obtained during the single-point positioning solution process. The residual of the pseudorange factor is the error between the pseudorange prediction value and the observed value. According to the pseudorange measurement model, the satellite s at t is calculated. i The pseudorange residual at time r Pse ;

[0085] The GNSS velocity measurement results are introduced to constrain the position variables of adjacent epochs to obtain the Doppler velocity measurement model. The residual of the Doppler velocity factor is the error between the velocity prediction value and the observed value. The residual error of the Doppler velocity factor is calculated to obtain the error at t i The residual error r of the Doppler velocity measurement at time D ;

[0086] Step 3-2, global factor fusion and initialization, is as follows:

[0087] Determine whether the current frame is a key frame, that is, determine the key frame according to the method described in step 1; when GNSS satellite observation data is input, construct it as a key frame and add it to the sliding window;

[0088] The coordinate system of the visual inertial odometer is initialized and aligned with the GNSS system;

[0089] Step 3-3, sliding window optimization, after the initialization described in step 3-2 is successful, the subsequent observations are added to the factor graph, and the overall optimization based on the factor graph is performed; the sliding window is introduced to limit the scale of the graph, as follows:

[0090] When a new frame observation arrives, the corresponding system state quantity node is first constructed for it and added to the factor graph. The corresponding error factor is established according to its observation value type and connected to the relevant state quantity node. If the window is full at this time, the node corresponding to the oldest frame in the window is marginalized, the node is removed, and the prior constraint term factor is constructed for the node related to it in the window to participate in the subsequent system global state optimization.

[0091] Step 3-4, global optimal posture, after the above steps, the high-precision posture of the carrier to be positioned is obtained. According to the high-precision posture, the global map is further corrected to achieve real-time update of the map and used for calculation of the next epoch.

[0092] Furthermore, the sliding window optimization described in step 3-3, that is, the optimization problem constructed by each factor in the sliding window is:

[0093]

[0094] χ=[x′ i ,x′ i+1 ,…,x′ i+f ,λ i ,λ i+1 ,…,λ i+p ,o i ,o i+1 ,…,o i+l ,t r,s ,T]

[0095]

[0096] Among them, χ * represents the corresponding state of the optimal solution, χ represents the global state vector in the sliding window, relative to the state quantity in the VIO system, x′ n Indicates the sliding window t i The system status at the moment, Respectively represent t i The position, attitude and velocity in the e coordinate system at the moment; t r,s Indicates t i The receiver clock difference of different satellite systems at different times, r and s are the receiver and satellite system respectively, and T represents t i The tropospheric delay at time r i is the map expansion visual point feature reprojection residual; Pse is the set of pseudorange measurements in the sliding window, D is the set of Doppler velocity measurements in the sliding window, V is the set of VIO measurements in the sliding window, and M is the set of map point feature selection in the sliding window; Σ i,DV ,Σ i,VIO , Represents the pseudorange measurement values Doppler velocity measurements Visual Inertial Odometry Measurements Map point feature measurements The corresponding covariance matrix.

[0097] Beneficial effects:

[0098] (1) In order to solve the problems of lag and low accuracy in building high-precision maps using the GNSS / IMU / Vision combined navigation system in disaster environments, the LSS perception algorithm is used to construct preliminary maps based on the strong perception capability and environmental adaptability of visual sensors. The surrounding information is recognized through a convolutional neural network. Based on the results of semantic recognition and detection, GNSS / IMU constraints are introduced when building maps in open areas to achieve real-time and refined disaster scene map construction.

[0099] (2) In order to address the problem of low robustness and accuracy caused by interference from dynamic objects in disaster environments, factor graph optimization usually assumes that the environment is static. However, there are various forms of dynamic changes in disaster environments that reduce the performance of the optimization algorithm. At the same time, data mismatching in dynamic environments will also affect the accuracy of the optimization results. The present invention uses real-time global map prediction to select appropriate visual points to expand and add or delete factor graphs to adapt to dynamic environmental changes under natural disasters, thereby improving the robustness and accuracy of the system that provides positioning for rescue vehicles. BRIEF DESCRIPTION OF THE DRAWINGS

[0100] The present invention will be further described in detail below in conjunction with the accompanying drawings and specific embodiments, and the above and / or other advantages of the present invention will become more clear.

[0101] Figure 1 It is a schematic diagram of the overall process of the present invention. DETAILED DESCRIPTION

[0102] Existing open-source electronic maps are updated laggingly, and the large environmental changes in natural disaster scenarios lead to untimely updates. Although visual sensors can be used to perceive the surrounding environment and generate maps, the connection between GNSS and IMU positioning and map points in the global map is not fully explored. Therefore, further research is needed to improve the positioning accuracy of the system. The present invention is intended to solve the problems of map update lag in disaster environments and low positioning accuracy of vehicles in complex disaster environments. The core content of the present invention is three stages: (1) constructing VIO factors based on point and line feature extraction; (2) high-precision map construction and refined correction under GNSS / IMU constraints; (3) expanding the global map with visual factors to perform global pose optimization solution for GNSS / INS / Vision combined navigation.

[0103] The present invention proposes a Beidou multi-source fusion positioning method in a disaster environment. The overall process is as follows: Figure 1 As shown, the specific technical solution is as follows:

[0104] (1) Constructing VIO factors based on point and line feature extraction

[0105] 1) Point and line feature extraction and key frame judgment

[0106] Step 1: point and line feature extraction. After the camera captures the image, the present invention uses FAST (Features From Accelerated Segment Test) point feature extraction and LSD (Line Segment Detector) line feature extraction. After extracting the point and line features, they are matched with the help of BRIEF (Binary Robust Independent Elementary Features) descriptor, and RANSAC (Random Sample Consensus) is used to eliminate point and line mismatches.

[0107] Step 2: Determine key frames. In the face of a large number of image recognition, key frame screening is performed to reduce computing pressure and achieve real-time performance. The present invention has a set of continuous frames F = {F 1 ,F 2 ,F 3 ,...,F n ,}, each frame image has a corresponding timestamp T i and the number of point and line features contained therein b i , i represents the i-th frame, K n Indicates the nth key frame.

[0108] The key frame judgment in the present invention is based on three criteria: first, the time between two adjacent key frames is greater than a specified threshold, that is, is the timestamp of the most recent key frame, T 0 is the set time difference threshold; second, the sliding window map optimization performed by the global map construction and optimization module has ended. By setting a Boolean value to mark whether the optimization is completed, the system can build as many key frames as possible to improve the accuracy of the global map; third, the point and line features are greater than the specified threshold, that is, b i >b 0 , b 0 The threshold is set. For image frames that are not key frames, they are directly discarded to reduce the amount of calculation.

[0109] Step 3: Feature matching and screening. After finding the key frame, in order to find the matching relationship between the corresponding image features between frames, the present invention introduces the KNN (K-nearest neighbors) algorithm for matching. For a feature in one image, the present invention searches for the most similar feature (nearest neighbor) in another image, and then regards them as a matching pair. Taking into account the possible mismatches, the RANSAC (Random Sample Consensus) algorithm is introduced to eliminate these mismatches. The RANSAC algorithm randomly selects a data subset (sample), then uses these samples to build a model, and performs a consistency check on the entire data set, that is, the original data set is D, the sample data subset is S, and the test model is M. Sample S is randomly selected from the data set D, and then the model parameters are calculated using sample S and expressed as M(S). When applied to the entire data set, the present invention calculates the consistency of the non-sample data point d with the model M(S), expressed as C(d, M(S)), and then determines which data points are consistent with the model based on the threshold, thereby identifying and eliminating incorrect matches.

[0110] 2) Visual feature reprojection residual factor.

[0111] A visual feature reprojection residual model is established. The distance between the projection point of the map point in the image and the corresponding observation point is used as the visual feature reprojection residual.

[0112] Step 1: Point feature reprojection residual. For the lth feature point L, its normalized coordinates observed in the i-th camera coordinate system are According to feature matching, the normalized coordinates of the same feature point observed in the camera coordinate system j are: The feature point L is transformed from the i-th camera coordinate system where it is first observed according to the projection and back-projection principle to the normalized coordinates of the camera coordinate system j where it is observed again: (i.e., observed predicted value), then the visual observation residual term is the difference between the predicted value and the actual observed value, that is:

[0113]

[0114] Step 2: Line feature reprojection residual. The line feature reprojection error of the present invention is to take the distance from the end point of the line observed by the camera plane to the reprojected line as the residual. The i-th camera observes the line L k , the straight line L k Projected onto the normalized plane of the i-th key frame to obtain the projection line equation The two endpoints of the straight line observed by the camera plane are and Use d(x,L) to represent the distance from point x to line L, then the line feature reprojection residual is:

[0115]

[0116] 3) IMU pre-integration residual factor

[0117] Step 1: Establish the IMU pre-integration residual model. Perform the IMU integration between two adjacent key frames in the IMU carrier coordinate system of the previous key frame i to obtain the relative motion increment independent of the key frame i state, where the zero bias change of the IMU is processed using a first-order linear approximation. In the IMU coordinate system, the IMU measurement equation is:

[0118]

[0119] In the formula, is the measured acceleration value at time t, a t is the noise-free acceleration at time t, is the random walk of acceleration at time t, is the coordinate transformation matrix, g w is the acceleration due to gravity, n a is the accelerometer white noise, is the gyroscope measurement value at time t, w t is the noise-free angular velocity at time t, is the angular velocity random walk at time t, n w is the gyroscope white noise.

[0120] According to the above integration and coordinate system transformation, the IMU pre-integration formula can be written:

[0121]

[0122] Where α, β, and γ are the relative position, velocity, and rotation from time i to time j, and Ω is:

[0123]

[0124] Step 2: Establish IMU pre-integration residual factor

[0125] After IMU pre-integration, the residual formula of system state and IMU measurement can be written as:

[0126]

[0127] In the formula, δ represents the difference between the measured value and the predicted value.

[0128] 4) Constructing VIO factors

[0129] The present invention needs to utilize the feature point observation residuals, line feature observation residuals and IMU residuals obtained above, and adopt the least squares method for joint optimization:

[0130]

[0131] r p , H p is the prior information, r B is the IMU pre-integration residual, r l is the reprojection residual of the visual point feature, is the residual of visual line feature reprojection; B is the IMU measurement value set, C 1 and C 2 A collection of point and line feature measurements; are their respective covariance matrices.

[0132] Since the visual inertial odometer is accurate within a small range, the VIO position increment and attitude increment between time i-1 and time i are used to model the measurement model according to the above optimization results.

[0133]

[0134] In the formula, y δR,i and Δp,i is the measured value of the visual odometer attitude increment and position increment, n δR,i Attitude increment measurement noise, R i-1 and R i is the attitude measurement value at adjacent moments, P i-1 and P i is the position measurement value at adjacent moments, n Δp,i The noise is measured for the position increments.

[0135] Then the residual factor of VIO is:

[0136]

[0137] (2) High-precision map construction and refined correction under GNSS constraints

[0138] Step 1: Preliminary map construction. After selecting the camera keyframe, the camera image is transformed into three-dimensional space through the LSS (a 2D to 3D algorithm, ift-Splat-Shoot) method, that is, the two-dimensional image is inversely transformed by introducing depth estimation, and the 2D pixel point (u, v) is mapped to the corresponding global coordinate system point (X w ,Y w ,Z w ):

[0139] [uv 1]T =KRT[X w Y w Z w 1] T

[0140] Among them, R is the camera rotation matrix, T is the camera translation vector, and K is the camera intrinsic parameter matrix.

[0141]

[0142] Where h is the camera height and θ is the camera pitch angle.

[0143] The LSS method is used to associate camera features with depth, ultimately achieving three-dimensional perception from a spatial perspective.

[0144] Step 2: Open area recognition: Through the convolutional neural network semantic segmentation algorithm, the predicted mapping elements (3D map points) are assigned labels and the density of objects with the same label is determined.

[0145] The present invention adopts an SSD convolutional neural network, and the SSD algorithm can simultaneously detect and classify objects in an image. First, the SSD convolutional neural network model is trained using a data set (images in different scenes are acquired in advance through vehicle-mounted camera collection to establish a data set of high-rise buildings and roads), that is, convolution is performed on feature maps of different scales to generate a series of candidate regions of fixed size, and they are directly classified and positionally regressed to obtain relevant information of the target to be detected (i.e., high-rise buildings, roads, and open areas). The size of each feature map is different, which also leads to different sizes of generated candidate regions. Specifically, SSD will generate multiple default boxes at each position. These default boxes correspond to different aspect ratios and scales, such as aspect ratios of 1:1, 1:2, 2:1, etc., which are used to describe the diversity of object sizes and shapes. For high-rise buildings, a box with a larger height is generally required.

[0146] The open area defined in the present invention must meet the characteristics of low floors and small number, so for the aspect ratio of high-rise buildings in the open area, the present invention selects buildings with a ratio of 1:1 or above, and the building density with height requirements in the same area is less than a certain threshold. If a road is identified in the open area, a point on the road is selected as the center of the open area, and a new label "open area reference point" is assigned to it; if the open area belongs to an inaccessible area, the building closest to the drivable road in the open area is assigned this new label. After training with the SSD convolutional neural network, the final result of the visual image processing is a set of prediction boxes, each of which has a clear classification label (high-rise building, road or open area reference point) and location information (center coordinates of the box, width and height). According to the LSS algorithm in the previous step, the label obtained in this step can be assigned to a three-dimensional position.

[0147] Step 3: GNSS / IMU constraint refinement. According to the previous training results, during the real-time positioning process, the open areas are identified while the map is being constructed. According to the label of "open area reference point", when passing through the area or approaching the area (when it is impossible to pass through), the GNSS / IMU loosely combined positioning results are used to correct the constraints. That is, when passing through the open area reference point, the GNSS and IMU combined navigation can obtain high-precision pose, correct the pose of the point predicted in the early stage, and further provide new constraints for map construction after this moment: Use the position and attitude information provided by GNSS / IMU to transform from the world coordinate system to the camera coordinate system, and further transform it to the pixel coordinate system through the camera's built-in transformation matrix, and jointly optimize it with the previously predicted visual target point at the same position, so that the following formula is minimized:

[0148]

[0149] Where P is the coordinate of the target point for joint optimization of GNSS / IMU observation points and map points, K is the camera internal parameter, z is the depth of the feature point, P c are the prior transformation matrix information and map feature point location information in the process of visual image processing and map point generation, respectively. i The coordinates of the GNSS / IMU positioning and the observation values ​​composed of the left and right image disparity are converted to the pixel coordinate system through the coordinate system.

[0150] By minimizing the loss function through the gradient descent method, a semi-sparse high-precision map composed of feature points with high overlap accuracy can be obtained, which provides good observation constraints for the positioning algorithm. Through the above processing, a high-precision map containing geometric information and feature information is finally obtained after correction by GNSS / IMU constraints.

[0151] Step 4: Optimize and build a global high-precision map. The continuously generated local maps can be judged by loop detection to see if they are repeated. If they are repeated, the local maps can be updated based on nonlinear optimization. This can be achieved by solving the following nonlinear least squares problem using the Gauss-Newton method:

[0152]

[0153] Where e ij (x i ,x j ,z ij ) is from the pose x i In place x j The observed data z ij The error between the pose estimate and the ijis the information matrix of the observed data, and ρ(·) is the robust kernel function used to reduce the influence of outliers.

[0154] If the areas passed through are not repeated, the common view relationship is obtained according to the perceptual similarity in the BEV (Bird's Eye View) perception process. The common view relationship is represented by the matrix V, V ij Represents the degree of co-viewing between pose i and pose j. Find similar frames F based on the co-viewing relationship i With F j The feature point P under i and P j , and find the same feature points by calculating the Euclidean distance between feature points:

[0155]

[0156] When the Euclidean distance is less than the specified threshold, the feature point P is judged i and P j Similarly, a global map is generated based on the same feature points found through the common view relationship.

[0157] Step 5: Global factor expansion based on feature matching. Global map feedback is to use the generated / updated map points to expand the visual feature state vector, and generate new visual reprojection residual factors as auxiliary positioning information to enter the global factor graph optimization process. Considering the real-time nature of the algorithm, when using the map for visual factor expansion, only the feature point reprojection residual is considered and the line features are ignored.

[0158] r i =f(X,P i )-O i

[0159] Where X is the visual feature state vector, P i is the feature of a point in the 3D map that is similar to the observed frame after matching by the RANSAC algorithm, f() is the visual reprojection function, which maps the state vector and map point to the image plane, O i are the corresponding feature points observed in the image.

[0160] When expanding new visual reprojection residual factors, the present invention considers two standards to ensure the quality of factor expansion: one is that the difference between the projection point of the map point on the image and the actual observation point, that is, the reprojection error is less than a threshold; the other is whether the geometric relationship of the map points observed from different perspectives is consistent, that is, whether the geometric relationship of the feature points observed from different perspectives or at different time points is consistent.

[0161] (3) Global pose optimization of GNSS / INS / Vision integrated navigation under global factor expansion

[0162] The high-precision global map generated in the previous part stores high-precision geometric information of the perceived environment. With the help of this map, the present invention can construct collinear equations for feature points and matching landmarks through the optical center of the camera to provide additional information factors to constrain the whole system factor optimization process.

[0163] Step 1: Pseudorange factor and Doppler velocity factor construction. In the process of single point positioning solution, the pseudorange measurement model can be obtained as follows:

[0164]

[0165] In the formula, is the pseudorange, is the coordinate of satellite s in the geocentric inertial coordinate system, is the coordinate of the receiver r in the Earth-centered inertial coordinate system, is a 4×1 vector, the corresponding satellite constellation entity is 1, and the other three entities are 0, Δt s is the satellite clock error, and represent the tropospheric delay and the tropospheric delay, respectively. is the delay caused by multipath effect, is the measurement noise of the pseudorange.

[0166] The residual of the pseudorange factor is the error between the pseudorange prediction value and the observed value. Therefore, according to the pseudorange measurement model, the pseudorange residual of satellite s at time i can be written as:

[0167]

[0168] In order to establish the temporal correlation of GNSS observations in the factor graph, the GNSS velocity measurement results are introduced to constrain the position variables of adjacent epochs, and the Doppler velocity measurement model is obtained:

[0169]

[0170] In the formula, The Doppler velocity at time i, p i,x is the position at time i, is the noise of the velocity measurement, and Δt is the time difference between adjacent epochs.

[0171] The residual of the Doppler velocity factor is the error between the velocity prediction and the observed value, because the residual of the Doppler velocity measurement at time i can be written as:

[0172]

[0173] Step 2: Global factor fusion and initialization. During the initialization process, it is first necessary to determine whether the current frame is a key frame. The GNSS / INS / Vision algorithm proposed in this paper mainly judges the key frame of vision based on the points in the process of constructing the VIO factor. For GNSS observations, since the convergence of parameters in the pseudo-range single-point positioning process takes a certain amount of time, it is necessary to ensure the continuity of satellite observations. Therefore, when GNSS satellite observations are passed in, the system will construct them as key frames and add them to the sliding window.

[0174] Because the coordinate system of the visual inertial odometer is a local coordinate system, it needs to be initialized and aligned with the GNSS system before information fusion. The initialization alignment problem between the GNSS system and the visual inertial odometer can be regarded as a position trajectory alignment problem. The reference coordinate system of the GNSS system is the ENU system, and the coordinate system of the VIO and extended visual point information is the local world coordinate system. Before data fusion, two coordinate system conversion matrices need to be calculated. If the ENU coordinate system is converted to the world coordinate system used in this invention, the ECEF coordinate system, the coordinate conversion matrix is

[0175]

[0176] Where n is the ENU coordinate system; e is the ECEF coordinate system; α and β are the latitude and longitude of the origin of the ENU coordinate system, respectively.

[0177] Step 3: Sliding window optimization. After successful initialization, subsequent observations can be added to the factor graph to perform overall optimization based on the factor graph. In the navigation and positioning problem, as the number of observations increases, the scale of the factor graph also increases. Therefore, in order to improve the real-time performance of the graph optimization algorithm, the present invention usually introduces a sliding window to limit the scale of the graph.

[0178] When a new frame observation arrives, first construct the corresponding system state quantity node for it and add it to the factor graph, establish the corresponding error factor according to its observation value type, and connect it with the relevant state quantity node. If the window is full at this time, it is necessary to marginalize the node corresponding to the oldest frame in the window, remove the node and construct a priori constraint term factors for the nodes related to it in the window to participate in the subsequent system global state optimization;

[0179] The optimization problem constructed by each factor in the sliding window is:

[0180]

[0181] χ=[x′ i ,x′ i+1 ,…,x′ i+f ,λ i ,λ i+1,…,λ i+p ,o i ,o i+1 ,…,o i+l ,t r,s ,T]

[0182]

[0183] χ * represents the corresponding state of the optimal solution, χ represents the global state vector in the sliding window, relative to the state quantity in the VIO system, x′ n Indicates the sliding window t i The system status at the moment, Respectively represent t i The position, attitude and speed in the e system at the moment; t r,s Indicates t i The receiver clock difference of different satellite systems at different times, r and s are the receiver and satellite system respectively, and T represents t i The tropospheric delay at time.

[0184] r p , H p is the residual and coefficient matrix of the prior constraints generated by marginalization; r Pse is the pseudorange residual, r D is the Doppler velocity residual, r VIO is the visual inertial odometry residual, r i is the map expansion visual point feature reprojection residual; Pse is the set of pseudorange measurements in the sliding window, D is the set of Doppler velocity measurements in the sliding window, V is the set of VIO measurements in the sliding window, and M is the set of map point feature selection in the sliding window; ∑ i,DV ,∑ i,VIO , are their respective covariance matrices.

[0185] Step 4: Global optimal posture. After GNSS / IMU / Vision global factor expansion and semi-tight combined navigation solution, the high-precision posture of the rescue vehicle can be obtained. According to the solved high-precision posture of the carrier, the global map in the disaster environment is further corrected (the steps are the same as high-precision map construction), so as to achieve real-time update of the map and use it for the calculation of the next epoch.

[0186] In a specific implementation, the present application provides a computer storage medium and a corresponding data processing unit, wherein the computer storage medium can store a computer program, and when the computer program is executed by the data processing unit, the invention content of the Beidou multi-source fusion positioning method in a disaster environment provided by the present invention and some or all of the steps in each embodiment can be run. The storage medium can be a disk, an optical disk, a read-only memory (ROM) or a random access memory (RAM), etc.

[0187] Those skilled in the art can clearly understand that the technical solutions in the embodiments of the present invention can be implemented by means of computer programs and their corresponding general hardware platforms. Based on such an understanding, the technical solutions in the embodiments of the present invention can be essentially or partly contributed to the prior art in the form of computer programs, i.e., software products, which can be stored in a storage medium and include several instructions for enabling a device including a data processing unit (which can be a personal computer, a server, a single-chip microcomputer, an MCU or a network device, etc.) to execute the methods described in various embodiments of the present invention or certain parts of the embodiments.

[0188] The present invention provides a method and idea of ​​Beidou multi-source fusion positioning in a disaster environment. There are many methods and ways to implement the technical solution. The above is only a preferred implementation of the present invention. It should be pointed out that for ordinary technicians in this technical field, without departing from the principle of the present invention, several improvements and modifications can be made, and these improvements and modifications should also be regarded as the protection scope of the present invention. All components not specified in this embodiment can be implemented by existing technologies.

Claims

1. A Beidou multi-source fusion positioning method in a disaster environment, characterized in that: The following steps are involved: Step 1, using a camera on the carrier to be positioned in the disaster environment to collect images, and constructing a VIO factor based on point and line feature extraction; Step 2: Map construction and fine-tuning under GNSS / IMU constraints to obtain a global map; Step 3: Expand the visual factors of the global map and perform global pose optimization and solution of GNSS / INS / Vision combined navigation to complete Beidou multi-source fusion positioning in disaster environments; The construction of VIO factors based on point and line feature extraction described in step 1 specifically includes: Step 1-1, point and line feature extraction and key frame judgment; Step 1-2, construct visual feature reprojection residual factor; Step 1-3, construct IMU pre-integration residual factor; Steps 1-4, construct VIO factors; The point and line feature extraction and key frame judgment described in step 1-1 specifically include: Step 1-1-1, point and line feature extraction: Based on the acquired image, use FAST point feature extraction and LSD line feature extraction to obtain point and line features; match the point and line features with the help of BRIEF descriptor, and use the RANSAC method to eliminate the results of incorrect matching of point and line features; Step 1-1-2, determine the key frame: set the collected image to a set of continuous frames F = {F1, F2, F3, ..., F i ,},F i Represents the i-th frame, for each frame image F i , including the following parameters: the corresponding timestamp T i And the number of point and line features contained in the frame image b i , use K n Represents the nth key frame, and the i-th frame image F is determined according to the following criteria i Is it a key frame? First criterion: the i-th frame image F i The time interval between adjacent key frames is greater than the specified threshold, that is, Indicates the timestamp of the most recent key frame, T0 is the set time difference threshold; The second criterion: the number of point and line features contained in the key frame is greater than the specified threshold, that is, b i >b0, b0 is the set point and line feature quantity threshold; For image frames that are judged as non-key frames, they are directly discarded; Step 1-1-3, feature matching and screening: After finding the key frame, the KNN algorithm is used for feature matching, that is, for a feature in one frame, the most similar feature is found in another frame, and then they are regarded as matching pairs; in the above matching process, the RANSAC algorithm is used to eliminate false matches; The construction of the visual feature reprojection residual factor described in step 1-2, that is, establishing a visual feature reprojection residual model, taking the distance between the projection point of the map point in the image and the corresponding observation point as the visual feature reprojection residual, specifically includes: Step 1-2-1, point feature reprojection residual, as follows: For the lth feature point L, let it be in the i-th frame F i The normalized coordinates observed in the corresponding i-th camera coordinate system are By using the image feature tracking method, we can get the jth frame F j The normalized coordinates of the same feature point observed in the corresponding j-th camera coordinate system are The feature point L is transformed from the i-th camera coordinate system where it is first observed to the normalized coordinates of the camera coordinate system j where it is observed again, that is, the observed prediction value is The visual observation residual term is the difference between the predicted value and the actual observed value Right now: in, represents the global state vector of the local pose of VIO in the world coordinate system, Represents the point feature observation of the camera; Among them, x n Includes the carrier position provided by the nth IMU direction speed Acceleration bias b an and gyro bias b gn ; d, p and l represent the total number of image frames, point features and line features in the sliding window respectively; λ i+m Represents the inverse depth between the mth feature point and the initial feature point; o i+w represents the orthogonal representation of the w-th feature line in the world coordinate system; Step 1-2-2, line feature reprojection residual, that is, the distance from the end point of the line observed by the camera plane to the reprojected line is taken as the residual, as follows: Assume that the i-th camera captures the i-th frame F i The camera at time , observes the straight line L k , the straight line L k Projected onto the normalized plane of the i-th key frame to obtain the projection line equation The two endpoints of the straight line observed by the camera plane are and Let d(x,L) represent the distance from point x to line L, then the line feature reprojection residual is for: ； The construction of the IMU pre-integration residual factor described in steps 1-3 specifically includes: Step 1-3-1, establish the IMU pre-integration residual model, as follows: The IMU integral between two adjacent key frames is calculated in the previous key frame F i The IMU carrier coordinate system is used to obtain the value independent of the key frame F i The relative motion increment of the state, where the zero bias change of the IMU is processed using a first-order linear approximation; in the IMU coordinate system, the IMU measurement equation is: in, is the measured acceleration value at time t, a t is the noise-free acceleration at time t, is the random walk of acceleration at time t, is the coordinate transformation matrix, g w is the acceleration due to gravity, n a is the accelerometer white noise; is the gyroscope measurement value at time t, w t is the noise-free angular velocity at time t, is the angular velocity random walk at time t, n w is the gyroscope white noise; According to the above IMU measurement equation integration and coordinate system transformation, the IMU pre-integration is as follows: in, For integration i to j The relative position, speed and rotation at the moment, the intermediate term Ω(w) is calculated as follows: Where w represents the gyroscope measurement value, × represents the matrix antisymmetric operation, and T represents the matrix transpose operation; Step 1-3-2, establish the IMU pre-integration residual factor, as follows: After IMU pre-integration, the residual between the system state and the IMU measurement as follows: Among them, δ represents the difference between the measured value and the predicted value, Represents IMU observations; The specific method of constructing the VIO factor described in steps 1-4 includes: According to the point feature reprojection residual, line feature reprojection residual and IMU pre-integration residual, the least squares method is used for joint optimization, as follows: in, Indicates the corresponding state value r of the optimal solution p and H p is the prior information, r B is the IMU pre-integration residual, r l is the point feature reprojection residual, is the line feature reprojection residual; B is the IMU measurement value set, C1 and C2 are the point and line feature measurement value sets, and IMU measurement value Point feature measurements Line feature measurements The corresponding covariance matrix; According to the above optimization results, using t i-1 Time to t i The VIO position increment and posture increment between moments are modeled, and the measurement model is obtained as follows: Among them, y δR,i and Δp,i is the measured value of the visual odometer attitude increment and position increment, n δR,i Attitude increment measurement noise, R i-1 and R i is the attitude measurement value at adjacent moments, P i-1 and P i is the position measurement value at adjacent moments, n Δp,i is the position increment measurement noise; then the VIO factor for: in, represents the VIO system measurement value, r δR,i and r Δp,i Represent the measurement values ​​of the visual inertial odometry attitude increment and position increment respectively.

2. The Beidou multi-source fusion positioning method in a disaster environment according to claim 1 is characterized in that: The map construction described in step 2 is refined and corrected under GNSS / IMU constraints, including: Step 2-1, preliminary map construction, as follows: After key frame judgment, the key frame image after camera image screening is transformed into three-dimensional space through LSS method, and the pixel point (u, v) in the image is mapped to the corresponding global coordinate system point (X w ,Y w ,Z w ), as follows: [uv 1] T =KRT[X w Y w Z w 1] T Among them, R is the camera rotation matrix, T is the camera translation vector, and K is the camera intrinsic parameter matrix; Where h is the camera height and θ is the camera pitch angle; Step 2-2, open area recognition, uses the convolutional neural network semantic segmentation method to assign labels to the mapping elements, i.e., the map points in the three-dimensional space, i.e., the global coordinate system points described in step 2-1, and judge the density of the objects with the same label, as follows: The open area is defined as a building in which the width-to-height ratio of high-rise buildings is greater than 1:1, and the density of buildings meeting the height requirements in the same area is less than a preset threshold; If a road is identified in the open area, a point on the road is selected as the center of the open area and a new label is assigned to it: open area reference point; if the open area is an impassable area, the building closest to the drivable road in the open area is assigned the above new label: open area reference point; After using the SSD convolutional neural network semantic segmentation method, the detection targets in the image are assigned classification labels and two-dimensional position information; according to the LSS method in step 2-1, the above classification labels are mapped to three-dimensional positions; Step 2-3, GNSS / IMU constraint refinement, that is: according to the previous training results of the convolutional neural network semantic segmentation algorithm, during the real-time positioning process, the open area is identified while the map is constructed. According to the open area reference point labels, when it is impossible to pass through or approach the area, the GNSS / IMU loose combination positioning results are used to correct the constraints; Steps 2-4, global high-precision map optimization and construction, are as follows: The continuously generated local maps are determined to be repeated through loop detection. If repeated, the local maps are updated according to nonlinear optimization. This is achieved by solving the following nonlinear least squares problem using the Gauss-Newton method: Among them, e ij (x i ,x j ,z ij ) is from the pose x i In place x j The observed data z ij The error between the pose estimate and the ij is the information matrix of the observed data, ρ(·) is the robust kernel function; If the areas passed through are not repeated, the common view relationship is obtained based on the perceptual similarity in the BEV perception process. The common view relationship is represented by the matrix V, V ij Represents the pose x i In place x j The degree of co-viewing between them; Find similar frames F based on the co-viewing relationship i With F j The feature point P under i and P j , and calculate the Euclidean distance d(P i ,P j ) Find the same feature points, as follows: When the Euclidean distance is less than the specified threshold, the feature point P is judged i and P j Same, generate a global map based on the same feature points found by the common view relationship; Step 2-5, global factor expansion based on feature matching, that is: using the generated or updated map points to expand the visual feature state vector, and generating new visual reprojection residual factors as auxiliary positioning information to enter the global factor graph optimization process; when using the map for visual factor expansion, only the feature point reprojection residual is expanded.

3. The Beidou multi-source fusion positioning method in a disaster environment according to claim 2 is characterized in that: The GNSS / IMU constraints described in steps 2-3 are refined as follows: When passing through the open area reference point, the GNSS and IMU combined navigation obtains high-precision pose, corrects the pose of the point predicted earlier, and further provides new constraints for map construction after this moment: the position and pose information provided by GNSS / IMU is converted from the world coordinate system to the camera coordinate system, and further converted to the pixel coordinate system through the camera's built-in transformation matrix, and jointly optimized with the previously predicted visual target point at the same position, that is, the value η of the following formula is minimized: Among them, P is the target point coordinate for joint optimization of GNSS / IMU observation points and map points, K is the camera internal parameter, z is the depth of the feature point, P c are the prior transformation matrix information and map feature point location information in the process of visual image processing and map point generation, respectively. i The coordinates of the GNSS / IMU positioning and the observation values ​​composed of the left and right image disparity are converted to the pixel coordinate system through the coordinate system.

4. The Beidou multi-source fusion positioning method in a disaster environment according to claim 3 is characterized in that: The global map expansion visual factors described in step 3 specifically include: Step 3-1, pseudorange factor and Doppler velocity factor are constructed as follows: The pseudorange measurement model is obtained during the single-point positioning solution process. The residual of the pseudorange factor is the error between the pseudorange prediction value and the observed value. According to the pseudorange measurement model, the satellite s at t is calculated. i The pseudorange residual at time r Pse ; The GNSS velocity measurement results are introduced to constrain the position variables of adjacent epochs to obtain the Doppler velocity measurement model. The residual of the Doppler velocity factor is the error between the velocity prediction value and the observed value. The residual error of the Doppler velocity factor is calculated to obtain the error at t i The residual error r of the Doppler velocity measurement at time D ; Step 3-2, global factor fusion and initialization, is as follows: Determine whether the current frame is a key frame, that is, determine the key frame according to the method described in step 1; when GNSS satellite observation data is input, construct it as a key frame and add it to the sliding window; Initialize and align the coordinate system of the visual inertial odometer with the GNSS system; Step 3-3, sliding window optimization, after the initialization described in step 3-2 is successful, the subsequent observations are added to the factor graph, and the overall optimization based on the factor graph is performed; the sliding window is introduced to limit the scale of the graph, as follows: When a new frame observation arrives, the corresponding system state quantity node is first constructed for it and added to the factor graph. The corresponding error factor is established according to its observation value type and connected to the relevant state quantity node. If the window is full at this time, the node corresponding to the oldest frame in the window is marginalized, the node is removed, and the prior constraint term factor is constructed for the node related to it in the window to participate in the subsequent system global state optimization. Step 3-4, global optimal posture, after the above steps, the high-precision posture of the carrier to be positioned is obtained. According to the high-precision posture, the global map is further corrected to achieve real-time update of the map and used for calculation of the next epoch.

5. The Beidou multi-source fusion positioning method in a disaster environment according to claim 4 is characterized in that: The sliding window optimization described in step 3-3, that is, the optimization problem constructed by each factor in the sliding window is: Among them, χ * represents the corresponding state of the optimal solution, χ represents the global state vector in the sliding window, relative to the state quantity in the VIO system, x′ n Indicates the sliding window t i The system status at the moment, Respectively represent t i The position, attitude and velocity in the e coordinate system at the moment; t r,s Indicates t i The receiver clock difference of different satellite systems at different times, r and s are the receiver and satellite system respectively, and T represents t i The tropospheric delay at time r i is the map expansion visual point feature reprojection residual; Pse is the set of pseudorange measurements in the sliding window, D is the set of Doppler velocity measurements in the sliding window, V is the set of VIO measurements in the sliding window, and M is the set of map point feature selection in the sliding window; ∑ i,DV ,∑ i,VIO , Represents the pseudorange measurement values Doppler velocity measurements Visual Inertial Odometry Measurements Map point feature measurements The corresponding covariance matrix.

Images