A method for optimizing location reliability based on Bayesian multi-sensor error constraints

By using Bayesian decision equations and error constraints in the multi-source fusion state update model, the multi-source fusion positioning framework is optimized, and the problems of source weight adjustment and source confidence evaluation in complex shading environments are solved, achieving high precision and Adaptive navigation positioning.

CN114019552BActive Publication Date: 2024-11-26THE 54TH RESEARCH INSTITUTE OF CHINA ELECTRONICS TECHNOLOGY GROUP CORPORATION
View PDF 0 Cites 0 Cited by

Patent Information

Application Number
CN202111226747.5
Authority / Receiving Office
CN · China
Patent Type
Patents(China)
Current Assignee / Owner
Filing Date
2021-10-21
Publication Date
2024-11-26
Estimated Expiration
2041-10-21

AI Technical Summary

Technical Problem

The existing multi-source elastic coupling positioning technology is difficult to achieve high-precision navigation positioning in unknown environments, especially in complex shading environments, lacks effective theoretical basis for fusion and unified positioning availability evaluation methods, and cannot adaptively adjust the source weight and source. Confidence probability assessment.

Method used

Using Bayesian-based multi-sensor error constraint method, each sensor error constraint is extracted in the multi-source fusion state update model, and Bayesian decision equation is introduced in the timing sliding window, and each source is balanced to build a joint Global error constraint equations are used to optimize the adaptive fusion positioning framework to obtain the best confidence positioning solution for multi-source fusion.

Benefits of technology

It realizes adaptive and trusted positioning in complex shading environments, improves the accuracy of multi-source coupled positioning, and provides adaptive positioning technology in unknown environments to meet the safe travel and intelligent operation needs of unmanned platforms.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN114019552B_ABST
    Figure CN114019552B_ABST
Patent Text Reader

Abstract

The present invention discloses a positioning confidence optimization method based on Bayesian multi-sensor error constraints, and belongs to the field of navigation positioning and information fusion application technology. The present invention utilizes a multi-source fusion positioning system composed of GNSS\IMU\lidar\visual camera, and extracts the error constraints of each sensor on the basis of a multi-source fusion state update model; by introducing the Bayesian decision equation in the sliding window of the time series, the weight of each information source is balanced; a joint global error constraint equation is constructed, and an adaptive fusion positioning framework is optimized to obtain the multi-source fusion optimal confidence positioning solution. The method of the present invention is simple and easy to implement, and can provide adaptive positioning technology means for unmanned intelligent platforms in unknown shielding environments such as urban canyons, geological natural disasters, etc.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The invention relates to a positioning confidence optimization method based on Bayesian multi-sensor error constraints, and belongs to the technical field of navigation positioning and information fusion application. Background Art

[0002] With the rapid development of unmanned platforms such as unmanned logistics, smart agriculture, and assisted driving, high-precision positioning services are gradually moving towards more ubiquitous unknown and complex scenarios. Real-time, continuous, ubiquitous and seamless high-precision navigation and positioning is an important basis for ensuring the safe movement and intelligent operation of various unmanned platforms. However, restricted scenes such as urban canyons, tunnels, and mountain valleys limit the ubiquitous and seamless application of unmanned platforms. Given that visual feature tracking is limited to open natural environments such as large scale and lighting, the scene recognition ability of lidar is poor, and it is difficult to make closed-loop global corrections, the low-cost inertial navigation accumulates error time drift, and the GNSS signal has weak anti-interference ability and is easily blocked. At present, the development of a multi-sensor loosely coupled positioning method that combines GNSS\IMU\vision\lidar is the mainstream means to solve the problem of high-precision navigation and positioning in restricted environments.

[0003] At present, although the known multi-source loose / tight coupling technology has achieved certain results, its actual application is limited to structured environments such as urban roads, industrial parks, and airports. The optimization of positioning results depends on the strong constraints of prior maps, and the technology is difficult to apply and promote in unknown environments. Navigation and positioning technology based on multi-source fusion is a research hotspot in the field of unmanned intelligence at this stage, but there are few results published on the availability of navigation and positioning in multiple types of shielded environments. There is no perfect fusion theoretical foundation and unified positioning availability evaluation method. It is urgent to develop multi-source fusion weight adaptive adjustment and source confidence probability evaluation technology for complex shielded environments. However, there is no relevant application in the existing technology. Summary of the invention

[0004] In view of this, the present invention proposes a positioning confidence optimization method based on Bayesian multi-sensor error constraints. This method breaks through the limitation of traditional multi-source fusion capabilities and realizes adaptive reliable positioning in complex shielding environments.

[0005] In order to achieve the above object, the technical solution adopted by the present invention is:

[0006] A method for optimizing the positioning reliability based on Bayesian multi-sensor error constraints comprises the following steps:

[0007] (1) Based on the multi-source fusion state update model, the error constraints of each sensor are extracted;

[0008] (2) Introducing the Bayesian decision equation in the sliding window of the time series to balance the weights of each information source;

[0009] (3) Construct a joint global error constraint equation, optimize the adaptive fusion positioning framework, and obtain the multi-source fusion optimal confidence positioning solution.

[0010] Furthermore, the specific method of step (1) is as follows:

[0011] Establish a source fusion framework and define S, G, B, L, and C to represent the multi-source full system, GNSS sensor source, IMU sensor source, lidar, and visual camera respectively:

[0012] GNSS measurement solves the position of the unmanned platform through PPP-RTK G , and manually measure the antenna arm value to G To compensate, the GNSS antenna arm is the vector from the IMU center to the GNSS antenna phase center. When the satellite antenna is at the front, right, or top relative to the IMU, the arm value l is projected by the vector in the IMU coordinate system into three components l = (l x ,l y ,l z ) T To express, the compensated r G for:

[0013] r G =r a -l

[0014] Among them, r a Measure the value for the antenna mast;

[0015] Known r S is the system position measurement value, then the GNSS measurement error constraint f(G) is:

[0016] f(G) = arg min{r S -r G}

[0017] Among them, arg min{} represents the parameter factor solution that minimizes the relational expression in {};

[0018] IMU measures and calculates the velocity v of the unmanned platform at time t+Δt t+Δt 、Position t+Δt and rotation q t+Δt The estimation model is:

[0019]

[0020]

[0021]

[0022] Among them, at ,ω t They are the IMU raw acceleration and angular velocity measurements at time t respectively; For acceleration measurement deviation and random white noise, is the angular velocity measurement deviation and random white noise; g is the gravitational acceleration of the world coordinate system W, is the rotation from the inertial body coordinate system to the world coordinate system;

[0023] The IMU pre-integration error constraint f(B) is:

[0024] δv=R(v j -v i -gΔt ij )

[0025]

[0026]

[0027] f(B)=arg min{δv,δr,δq}

[0028] In the formula, Δt ij is the time interval between consecutive moments i and j, v i ,v j represents the speed at time i,j, r i ,r j represents the position at time i,j, q i ,q j represents the posture at time i, j, R is the rotation from the inertial body coordinate system to the world coordinate system, δv, δr, δq represent the pre-integrated velocity deviation, pre-integrated position deviation, and pre-integrated posture deviation respectively;

[0029] The state transition models of vision and lidar are both relative position measurements. The lidar odometer uses the LOAM algorithm to extract edge point and plane point features by calculating curvature, and performs continuous scanning matching from point to edge feature and point to plane feature to estimate sensor motion. e ,d h They represent the distance from point to edge feature and point to plane feature respectively, where the feature constraint from point to edge is the distance arg min{d e}, the feature constraint from point to plane is distance arg min{d h};

[0030] In view of the laser radar motion distortion caused by the unmanned platform during high-speed movement, the motion estimation is compensated, and the state transfer is performed through linear interpolation. Assume that the motion state matrix of i at time point t is The rotation matrix is ​​R and the translation is T, then:

[0031]

[0032] Among them, τ,τ i ,τ t They represent the complete scanning cycle, the time of scanning point i, and the time of starting scanning respectively. for An estimated value of

[0033] The above state transfer process is represented by 6 degrees of freedom [x, y, z, roll, pitch, yaw], and the Euler angle is transformed into a rotation matrix using the Rodriguez formula:

[0034]

[0035] Among them, Euler angle θ = [roll, pitch, yaw], is the unit vector of the rotation direction, T = [x, y, z] is the rotation amplitude; the laser radar matching distance constraint f(L) is:

[0036] f(L)=arg min{∑d e +∑d h}

[0037] In the formula, ∑d e ,∑d h Respectively represent all d in the current laser frame e ,d h The cumulative sum of

[0038] Based on the positioning technology of visual camera, the camera pose parameters of continuous image frames are obtained by using motion recovery structure, assuming that there are camera poses of N key image frames that can be continuously observed.

[0039]

[0040] in, L, is the key image frame camera pose parameter, L, are the feature points contained in the continuous key image frames;

[0041] The visual reprojection error constraint f(C) is:

[0042]

[0043]

[0044]

[0045] Among them, M ijis the common view coordinate of key frame i, j in the world coordinate system, s is the feature depth scale, K C is the camera intrinsic parameter matrix.

[0046] Furthermore, the specific method of step (2) is as follows:

[0047] The multi-source fusion navigation and positioning system uses four types of sensors: satellite navigation GNSS, inertial navigation IMU, visual camera, and lidar;

[0048] Determine the prior probability of a single source in a multi-source system S, The prior probabilities of GNSS positioning measurement weight, IMU positioning measurement weight, LiDAR positioning measurement weight, and visual camera positioning measurement weight are represented respectively; the degree of shielding in the unmanned platform's traveling environment affects the prior probabilities of GNSS, LiDAR, and visual cameras, and the IMU prior probability is only related to time accumulation; the shielding environment is divided into 4 categories, which are fuzzily described as open, semi-shielded, severely shielded, and completely shielded;

[0049] GNSS positioning measurement weight prior probability P r G It is described by detecting and eliminating satellite cycle slips and current satellite configuration. The more frequent the cycle slips, the more serious the occlusion. When there are no available satellites in the sampling, it is a complete occlusion environment with a probability P r G Always 0; when the number of available satellites does not exceed 3, it is severely blocked, with a probability of P r G Always 0; when the number of available satellites reaches more than 4 satellites, it is a semi-obstructed or open environment, and the satellite positioning can be solved; when the satellite is in a solvable state, the satellite configuration is described by the PDOP position precision factor. When the PDOP position precision factor is less than or equal to 3, the ideal position can be generated, and the probability P r G The initial value is 0.95; when the PDOP position precision factor is between 3 and 7, the positioning result is acceptable, and the probability P r G The initial value is 0.5. When the PDOP position precision factor is greater than or equal to 7, the positioning result is unacceptable. The probability P r G Initially 0.05;

[0050] Known LiDAR positioning measurement weight prior probability P r L and the visual camera positioning measurement weight prior probability P r C and the GNSS positioning measurement weight prior probability P r GNegative correlation, then in open, semi-occluded, severely occluded, and fully occluded environments:

[0051] P r L =1-λ L P r G ,

[0052] P r C =1-λ C P r G ,

[0053] λ L ,λ C is the coefficient;

[0054] IMU positioning measurement weight prior probability P r B Regardless of the environmental shielding, within a sliding window period W, the IMU not only maintains navigation when the GNSS signal is blocked, but the IMU pre-integration is also used to optimize the GNSS convergence and the odometers of the lidar and visual camera; let P r B =1-λ B P r G ,λ B As coefficients, λ is simplified L =λ C =λ B , normalize the prior probability of each single source;

[0055] For 4 types of sensors and 4 types of occlusion environments, a chi-square distribution with 9 degrees of freedom is selected:

[0056] A=G,B,L,C

[0057] Among them, z S ,z A They represent the multi-source system positioning results and the single-source positioning results in the world coordinate system respectively. In the chi-square test, the theoretical value is the single-source world coordinate system positioning result, and the measured value is the multi-source system world coordinate system positioning result;

[0058] Query the chi-square test table to obtain the initialization multi-source fusion system positioning condition probabilities P(S|G), P(S|B), P(S|L), and P(S|C);

[0059] The confidence posterior probability of a single source is obtained by the Bayesian formula:

[0060] P(A|S)=P(S|A)P r A / P(S),A=G,B,L,C

[0061] Update the prior probability of each single source, normalize and eliminate P(S), and get:

[0062] P r A =P(A|S),A=G,B,L,C.

[0063] Furthermore, the specific method of step (3) is as follows:

[0064] The posterior probability weighted minimum error model for global constraint optimization using bundle adjustment is known to include four types of sensor error constraints, including IMU pre-integration error constraints, GNSS measurement positioning error constraints, lidar matching distance error constraints, and visual reprojection error constraints. Global constraint optimization is to assign the posterior probability of a single source confidence as the weight of the error constraint, accumulate and combine to form a global constraint, use the Levenberg-Marquarelt algorithm for nonlinear parameter solution, optimize the motion estimation model of each source, and obtain the multi-source fusion optimal confidence positioning solution:

[0065] min{∑P(A|S)f(A)},A=G,B,L,C.

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

[0067] 1. The present invention utilizes a multi-source fusion positioning system composed of GNSS\IMU\lidar\visual camera, and extracts the error constraints of each sensor based on the multi-source fusion state update model; by introducing the Bayesian decision equation in the sliding window of the time series, the weight of each information source is balanced; by constructing a joint global error constraint equation, the adaptive fusion positioning framework is optimized to obtain the multi-source fusion optimal confidence positioning solution.

[0068] 2. The present invention can realize autonomous adjustment of source weights under different occlusion environments, satisfying the adaptive positioning of unmanned platforms in complex environments.

[0069] 3. The present invention is based on Bayesian joint global error constraint and can optimize and improve the multi-source coupling positioning accuracy.

[0070] 4. The method of the present invention is simple and easy to implement, and can provide adaptive positioning technology for unmanned intelligent platforms in unknown shielding environments such as urban canyons, geological natural disasters, etc. BRIEF DESCRIPTION OF THE DRAWINGS

[0071] Figure 1 A schematic diagram of the principle of the method according to an embodiment of the present invention. DETAILED DESCRIPTION

[0072] In order to better illustrate the purpose and advantages of the present invention, the technical solution of the present invention is further described below.

[0073] A positioning confidence optimization method based on Bayesian multi-sensor error constraints, the equipment required for this method includes GNSS receiver, inertial navigation IMU, laser radar, and optical camera. Figure 1 As shown in the figure, satellite observations are obtained through GNSS receivers, accelerometers and angular velocity meters are obtained through IMUs, point clouds are obtained through lidars, and sequence images are obtained through optical cameras. The error constraints of the multi-sensor state update model are extracted, and the Bayesian decision equation of multi-source weight balance is introduced in the sliding window of the time series. The joint global error constraint equation is constructed, the adaptive fusion positioning framework is optimized, and the multi-source fusion optimal confidence positioning solution is obtained.

[0074] The method specifically comprises the following steps:

[0075] (1) Based on the multi-source fusion state update model, the error constraints of each sensor are extracted;

[0076] (2) By introducing the Bayesian decision equation in the sliding window of the time series, the weights of each information source are balanced;

[0077] (3) Construct a joint global error constraint equation and optimize the adaptive fusion positioning framework.

[0078] Furthermore, the specific method of step (1) is as follows:

[0079] Establish a source fusion framework and define S, G, B, L, and C to represent the multi-source full system, GNSS sensor source, IMU sensor source, lidar, and visual camera respectively:

[0080] GNSS measurement uses PPP-RTK (Precise Point Positioning-Real Time Kinematic) to calculate the position of the unmanned platform G , and manually measure the antenna arm value to G To compensate, the GNSS antenna arm is the vector from the IMU center to the GNSS antenna phase center. When the satellite antenna is at the front, right, and top relative to the IMU, the arm value l is projected by the vector in the IMU coordinate system. The three components l = (l x ,l y ,l z ) T To express, the compensated r G for:

[0081] r G =r a -l

[0082] Among them, r a is the measured value of the antenna rod; r S is the system position measurement value, GNSS measurement error constraint f(G):

[0083] f(G) = arg min{r S -r G}

[0084] Inertial navigation IMU measures and calculates the motion state velocity v of the unmanned platform at time t+Δt t+Δt 、Position t+Δt and rotation q t+Δt Estimated model:

[0085]

[0086]

[0087]

[0088] Among them, a t ,ω t They are the IMU raw acceleration and angular velocity measurements at time t respectively; For acceleration measurement deviation and random white noise, is the angular velocity measurement deviation and random white noise; g is the gravitational acceleration of the world coordinate system W, is the rotation from the inertial body coordinate system to the world coordinate system;

[0089] IMU pre-integration error constraint f(B):

[0090] δv=R(v j -v i -gΔt ij )

[0091]

[0092]

[0093] f(B)=arg min{δv,δr,δq}

[0094] In the formula, Δt ij is the time interval between consecutive moments i and j, v i ,v j represents the speed at time i,j, r i ,r j represents the position at time i,j, q i ,q jrepresents the posture at time i, j, R is the rotation from the inertial body coordinate system to the world coordinate system, δv, δr, δq represent the pre-integrated velocity deviation, pre-integrated position deviation, and pre-integrated posture deviation, respectively.

[0095] Different from the absolute positioning of satellite navigation, the state transition models of vision and lidar are all relative position measurements; the lidar odometer uses the LOAM algorithm to extract edge points and plane point features by calculating the curvature, and performs continuous scanning matching of point-to-edge features and point-to-plane features to estimate the sensor motion, where the feature constraint from point to edge is the distance arg min{d e}, where the feature constraint from point to plane is the distance arg min{d h}.

[0096] When the unmanned platform moves at high speed, it will cause laser radar motion distortion, so it is necessary to compensate for the motion estimation process. Due to the high scanning frequency of the laser radar, the unmanned platform can be approximated as uniform motion during one scanning process, so the state transfer can be achieved through linear interpolation. Assuming that the motion state matrix of i at time t is Among them, the rotation matrix is ​​R and the translation is T:

[0097]

[0098] Among them, τ,τ i ,τ t Respectively represent the complete scanning cycle, the scanning point i moment, and the starting scanning moment; the above state transfer process is represented by 6 degrees of freedom [x, y, z, roll, pitch, yaw], and the Euler angle is transformed into a rotation matrix, and the Rodriguez formula is used for transformation:

[0099]

[0100] where θ = [roll, pitch, yaw], is the unit vector of the rotation direction, T = [x, y, z] is the rotation amplitude;

[0101] LiDAR matching distance constraint f(L):

[0102] f(L)=arg min{∑d e +∑d h}

[0103] In the formula, ∑d e ,∑d h Respectively represent all d in the current laser frame e ,d h The cumulative sum of

[0104] The positioning technology based on visual cameras uses motion recovery structure to obtain the camera posture parameters of continuous image frames, assuming that there are camera postures of N key image frames that can be continuously observed.

[0105]

[0106] in, L, is the key image frame camera pose parameter, L, are the feature points contained in the continuous key image frames;

[0107] Visual reprojection error constraint f(C):

[0108]

[0109]

[0110]

[0111] Among them, M ij is the common view coordinate of key frame i, j in the world coordinate system, s is the feature depth scale, K C is the camera intrinsic parameter matrix.

[0112] The specific method of step (2) is as follows:

[0113] The multi-source fusion navigation and positioning system uses four types of sensors, satellite navigation GNSS, inertial navigation IMU, visual camera and lidar. Given that each type of sensor has data source characteristics and applicable perception environments, when the unmanned platform passes through open, semi-occluded, and occluded environments, the fusion weight probability of the positioning results of each type of sensor in the system must be different. This probability can be defined as the confidence probability to describe the reliability of the positioning results of the multi-source fusion system at the current state time t. In this example, the sliding period window W (1 second) is selected to construct a multi-source Bayesian sliding window function model to accumulate the confidence probability of each single source;

[0114] First, determine the prior probability of a single source in the multi-source system S, P r G ,P r B ,P r L ,P r CRespectively characterize the prior probability of GNSS positioning measurement weight, IMU positioning measurement weight, LiDAR positioning measurement weight, and visual camera positioning measurement weight; the degree of shielding in the unmanned platform's traveling environment will directly affect the prior probability of GNSS, LiDAR, and visual camera, while the IMU prior probability is only related to time accumulation, in which the degree of shielding is vaguely described as empty, semi-shielded, severely shielded, and completely shielded;

[0115] GNSS positioning measurement weight prior probability P r G The satellite cycle slips and current satellite configuration description can be detected and eliminated. The more frequent the cycle slips, the more serious the occlusion. When there are no available satellites in the sampling, it is a complete occlusion environment (probability P r G When the number of available satellites does not exceed 3, it is severely blocked (probability P r G When the number of available satellites reaches 4 or more, it is a semi-blocked or open environment, and the satellite positioning can be solved. When the satellite is in a solvable state, the satellite configuration is described by PDOP (Position Dilution of Precision). When PDOP is less than or equal to 3, the ideal position can be generated (probability P r G Initially 0.95), PDOP positioning between 3 and 7 is acceptable (probability P r G Initially 0.5), a PDOP greater than or equal to 7 is considered unacceptable (probability P r G Initially 0.05);

[0116] Known LiDAR positioning measurement weight prior probability P r L and the visual camera positioning measurement weight prior probability P r C and the GNSS positioning measurement weight prior probability P r G Negative correlation, then P is given in open, semi-occluded, severely occluded, and fully occluded environments. r L =1-λ L P r G , P r C =1-λ C P r G ,λ L ,λ C is the coefficient;

[0117] IMU positioning measurement weight prior probability P r B Regardless of the environmental shielding, within a sliding window period, the IMU can not only maintain navigation when the GNSS signal is blocked, but also optimize the GNSS convergence and the LiDAR\Visual Camera odometer. r B =1-λ B P r G ,λ B As coefficient, λ can be simplified L =λ C =λ B , normalize the prior probability of each single source;

[0118] This example involves 4 types of sensors and 4 types of occlusion environments. The chi-square distribution with 9 degrees of freedom is selected, and the chi-square test table is queried to obtain the initialization multi-source fusion system positioning condition probabilities, P(S|G), P(S|B), P(S|L), P(S|C);

[0119] A=G,B,L,C

[0120] Among them, z S ,z A They respectively represent the multi-source system positioning results and the single source positioning results in the world coordinate system. In the chi-square test, the theoretical value is the single source world coordinate system positioning result, and the measured value is the multi-source system world coordinate system positioning result.

[0121] The confidence posterior probability of a single source is obtained by the Bayesian formula:

[0122] P(A|S)=P(S|A)P r A / P(S),A=G,B,L,C

[0123] Update the prior probability of each single source and normalize to eliminate P(S):

[0124] P r A =P(A|S),A=G,B,L,C

[0125] The specific method of step (3) is as follows:

[0126] The posterior probability weighted minimum error model for global constraint optimization is implemented using bundle adjustment. It is known that four types of sensor error constraints include IMU pre-integration error constraint, GNSS measurement positioning error constraint, lidar matching distance error constraint, and visual reprojection error constraint. The global constraint optimization method is to assign the posterior probability of a single source confidence as the weight of the error constraint, accumulate and combine them to form a global constraint, use the LM (Levenberg-Marquarelt) algorithm for nonlinear parameter solution, optimize the motion estimation model of each source, and obtain the multi-source fusion optimal confidence positioning solution.

[0127] min{∑P(A|S)f(A)},A=G,B,L,C

[0128] In short, real-time and continuous high-precision navigation and positioning is an important basis for ensuring the safe movement and intelligent operation of unmanned platforms. However, restricted scenes such as urban canyons, tunnels, and mountainous areas limit the ubiquitous and seamless application of unmanned platforms. In view of the fact that visual feature tracking is limited by open natural environments such as large scale and lighting, the lidar scene recognition ability is poor and it is difficult to make closed-loop global corrections, the low-cost inertial navigation accumulates error time drift, the GNSS signal has weak anti-interference ability, and is susceptible to occlusion. At present, the development of a multi-sensor loosely coupled positioning method that combines GNSS\IMU\vision\lidar is the mainstream means to solve the problem of high-precision navigation and positioning in restricted environments. The present invention extracts the error constraints of the multi-sensor state update model, introduces the Bayesian decision equation of multi-source weight balance in the sliding window of the time series, constructs a joint global error constraint equation, optimizes the adaptive fusion positioning framework, and obtains the multi-source fusion optimal confidence positioning solution.

Claims

1. A method for optimizing the positioning reliability based on Bayesian multi-sensor error constraints, characterized in that: The steps include: (1) Based on the multi-source fusion state update model, the error constraints of each sensor are extracted; the specific method is as follows: Establish a source fusion framework and define S, G, B, L, and C to represent the multi-source full system, GNSS sensor source, IMU sensor source, lidar, and visual camera respectively: GNSS measurement solves the position of the unmanned platform through PPP-RTK G , and manually measure the antenna arm value to G To compensate, the GNSS antenna arm is the vector from the IMU center to the GNSS antenna phase center. When the satellite antenna is at the front, right, and top relative to the IMU, the arm value l is projected by the three components of the vector in the IMU coordinate system l = (l x ,l y ,l z ) T To express, the compensated r G for: r G =r a -l Among them, r a Measure the value for the antenna mast; Known r S is the system position measurement value, then the GNSS measurement error constraint f(G) is: f(G)=argmin{r S -r G } Among them, argmin{} represents the parameter factor solution that minimizes the relational expression in {}; IMU measures and calculates the motion state speed v of the unmanned platform at time t+Dt t+Dt 、Position t+Dt and rotation q t+Dt The estimation model is: q t+Dt =q t exp((w t -b t w-n t w)Dt) Among them, a t ,w t They are the IMU raw acceleration and angular velocity measurements at time t respectively; For acceleration measurement deviation and random white noise, is the angular velocity measurement deviation and random white noise; g is the gravitational acceleration of the world coordinate system W, is the rotation from the inertial body coordinate system to the world coordinate system; The IMU pre-integration error constraint f(B) is: δv=R(v j -v i -gΔt ij ) f(B)=argmin{dv,δr,δq} In the formula, Δt ij is the time interval between consecutive moments i and j, v i ,v j represents the speed at time i,j, r i ,r j represents the position at time i,j, q i ,q j represents the posture at time i, j, R is the rotation from the inertial body coordinate system to the world coordinate system, δv, δr, δq represent the pre-integrated velocity deviation, pre-integrated position deviation, and pre-integrated posture deviation respectively; The state transition models of vision and lidar are both relative position measurements. The lidar odometer uses the LOAM algorithm to extract edge point and plane point features by calculating curvature, and performs continuous scanning matching from point to edge feature and point to plane feature to estimate sensor motion. e ,d h They represent the distance from point to edge feature and point to plane feature respectively, where the feature constraint from point to edge is the distance argmin{d e }, the feature constraint from point to plane is the distance argmin{d h }; In view of the laser radar motion distortion caused by the unmanned platform during high-speed movement, the motion estimation is compensated, and the state transfer is performed through linear interpolation. Assume that the motion state matrix of i at time point t is The rotation matrix is ​​R and the translation is T, then: Among them, t,t i ,t t They represent the complete scanning cycle, the time of scanning point i, and the time of starting scanning respectively. for An estimated value of The above state transfer process is represented by 6 degrees of freedom [x, y, z, roll, pitch, yaw], and the Euler angle is transformed into a rotation matrix using the Rodriguez formula: Among them, Euler angle q = [roll, pitch, yaw], is the unit vector of the rotation direction, T = [x, y, z] is the rotation amplitude; The laser radar matching distance constraint f(L) is: <h2 style=";text-align:left;direction:ltr">f(L) = argmin{Σd<h2 style=";text-align:left;direction:ltr"> e <h2 style=";text-align:left;direction:ltr"> +Σd<h2 style=";text-align:left;direction:ltr"> h <h2 style=";text-align:left;direction:ltr">} In the formula, Σd e ,Σd h Respectively represent all d in the current laser frame e ,d h The cumulative sum of Based on the positioning technology of visual camera, the camera pose parameters of continuous image frames are obtained by using motion recovery structure, assuming that there are camera poses of N key image frames that can be continuously observed. in, is the key image frame camera pose parameter, are the feature points contained in the continuous key image frames; The visual reprojection error constraint f(C) is: Among them, M ij is the common view coordinate of key frame i, j in the world coordinate system, s is the feature depth scale, K C is the camera intrinsic parameter matrix; (2) Introducing the Bayesian decision equation in the sliding window of the time series to balance the weights of each information source; (3) Construct a joint global error constraint equation, optimize the adaptive fusion positioning framework, and obtain the multi-source fusion optimal confidence positioning solution.

2. The method for optimizing the positioning reliability based on Bayesian multi-sensor error constraints according to claim 1, characterized in that: The specific method of step (2) is as follows: The multi-source fusion navigation and positioning system uses four types of sensors: satellite navigation GNSS, inertial navigation IMU, visual camera, and lidar; Determine the prior probability of a single source in a multi-source system S, The prior probabilities of GNSS positioning measurement weight, IMU positioning measurement weight, LiDAR positioning measurement weight, and visual camera positioning measurement weight are represented respectively; the degree of shielding in the unmanned platform's traveling environment affects the prior probabilities of GNSS, LiDAR, and visual cameras, and the IMU prior probability is only related to time accumulation; among them, the shielding environment is divided into 4 categories, which are fuzzily described as open, semi-shielded, severely shielded, and completely shielded; GNSS positioning measurement weight prior probability It is described by detecting and eliminating satellite cycle slips and current satellite configuration. The more frequent the cycle slips, the more serious the occlusion. When there are no available satellites in the sampling, it is a complete occlusion environment with a probability of Always 0; when the number of available satellites does not exceed 3, it is severely blocked, the probability Always 0; when the number of available satellites reaches 4 or more, it is a semi-obstructed or open environment, and the satellite positioning can be solved; when the satellite is in a solvable state, the satellite configuration is described by the PDOP position precision factor. When the PDOP position precision factor is less than or equal to 3, an ideal position can be generated, and the probability The initial value is 0.95; when the PDOP position precision factor is between 3 and 7, the positioning result is acceptable. The initial value is 0.

5. When the PDOP position precision factor is greater than or equal to 7, the positioning result is unacceptable. Initially 0.05; Prior probability of known lidar positioning measurement weights and the visual camera positioning measurement weight prior probability Respectively with the GNSS positioning measurement weight prior probability Negative correlation, then in open, semi-occluded, severely occluded, and fully occluded environments: λ L ,λ C is the coefficient; IMU positioning measurement weight prior probability Regardless of the environmental shielding, within a sliding window period W, the IMU not only maintains navigation when the GNSS signal is blocked, but the IMU pre-integration is also used to optimize the GNSS convergence and the odometer of the lidar and visual camera; let λ B As coefficients, λ is simplified L = l C = l B , normalize the prior probability of each single source; For 4 types of sensors and 4 types of occlusion environments, a chi-square distribution with 9 degrees of freedom is selected: Among them, z S ,z A They represent the multi-source system positioning results and the single-source positioning results in the world coordinate system respectively. In the chi-square test, the theoretical value is the single-source world coordinate system positioning result, and the measured value is the multi-source system world coordinate system positioning result; Query the chi-square test table to obtain the initialization multi-source fusion system positioning condition probabilities P(S|G), P(S|B), P(S|L), and P(S|C); The confidence posterior probability of a single source is obtained by the Bayesian formula: Update the prior probability of each single source, normalize and eliminate P(S), and get:

3. The method for optimizing the positioning reliability based on Bayesian multi-sensor error constraints according to claim 1 is characterized in that: The specific method of step (3) is as follows: The posterior probability weighted minimum error model for global constraint optimization using bundle adjustment is known to include four types of sensor error constraints, including IMU pre-integration error constraints, GNSS measurement positioning error constraints, lidar matching distance error constraints, and visual reprojection error constraints. Global constraint optimization is to assign the posterior probability of a single source confidence as the weight of the error constraint, accumulate and combine to form a global constraint, use the Levenberg-Marquarelt algorithm for nonlinear parameter solution, optimize the motion estimation model of each source, and obtain the multi-source fusion optimal confidence positioning solution: min{ΣP(A|S)f(A)},A=G,B,L,C.