Multi-sensor fusion positioning method based on factor graph

Abstract

The invention provides a multi-sensor fusion positioning method based on a factor graph, and the method comprises the steps: 1, converting vehicle sensor data into a same space coordinate system, and carrying out the time alignment; 2, the original data queue and the variable set of the vehicle are emptied, and sensor data aligned through time are imported again; step 3, according to a global satellite navigation system, acquiring an initial position, an attitude and a speed state of the vehicle, and through a function # imgabs0 #, constructing a prior information factor; 4, verifying the availability of the data after receiving the vehicle sensor data, constructing an observation information factor and a motion model factor, and adding the observation information factor and the motion model factor into the factor graph; and step 5, optimizing the constructed factor graph to form an optimized objective function, resolving the optimized objective function through a nonlinear optimization method to obtain a vehicle state, and completing vehicle positioning.

Description

Technical Field

[0001] The present application relates to the field of autonomous driving technology, and in particular to a multi-sensor fusion positioning method based on factor graph. Background Art

[0002] Accurate positioning is one of the key technologies to achieve the safety and reliability of autonomous driving systems. The application scope of a single sensor is limited by the influence of the environment. The mainstream research direction in the positioning of autonomous driving vehicles is the multi-sensor fusion algorithm that comprehensively considers multiple positioning sources and sensors. The multi-sensor fusion algorithm refers to the data of different sensors such as GPS (Global Positioning System), inertial measurement unit, wheel speed meter, vision, laser radar, etc., aiming to provide positioning information when a sensor fails or cannot be used in the current operating environment, and to ensure reliable and continuous and stable positioning information in all operating scenarios. In addition, different sensors obtain vehicle positioning information in different ways. For example, integrated navigation obtains its own positioning information by receiving satellite signals, laser SLAM (simultaneous localization and mapping) obtains depth information of objects in the environment through laser radar to build odometers, and lane positioning obtains lane lines and maps through camera perception to obtain location information. The multi-sensor fusion algorithm can use these independent information to calibrate each other and eliminate the interference of erroneous information. By reasonably configuring the availability of each positioning source information, it can comprehensively utilize the advantages of various sensors to achieve a more comprehensive and accurate perception of complex and changing environments and enhance the robustness of the positioning system.

[0003] However, the current mainstream multi-sensor fusion algorithm is based on Kalman filtering and its derived series of algorithms, such as patents CN 108733055 A, CN 118067112 A, and CN 116247565 A. However, the premise of the Kalman filter series multi-sensor fusion theory is that the state quantity in the time series space must satisfy the Markov process, that is, given the current state, the probability distribution of the future state depends only on the current state, and has nothing to do with the past state. When all observations are unavailable, the vehicle calculates its own posture based on sensors such as IMU (inertial measurement unit) and wheel speedometer to achieve positioning. However, with the increase of vehicle mileage and the deviation of wheel speedometer data, the errors of inertial measurement unit and wheel odometer will gradually accumulate, so that high-precision positioning results cannot be continuously provided. The deviation between the trajectory calculated by the global fusion algorithm and the actual trajectory is getting larger and larger. After the availability of the observation source is restored, the calculated trajectory quickly jumps to follow the observation data, resulting in braking or vehicle shaking, causing safety risks. This is because the state calculation of the Kalman filter series algorithm is only related to the predicted value of the previous moment and the observed value of the current moment. Once the data at a certain moment is abnormal, the subsequent calculation will diverge rapidly and cannot be restored in the absence of observation information. In addition, the Kalman filter series algorithm assumes that the noise satisfies the Gaussian distribution and the entire system is a linear system. However, in actual scenarios, the sensors of each positioning source will be subject to various interferences, such as the satellite clock error, satellite ephemeris error, and errors in the satellite signal propagation process such as ionosphere error and troposphere error contained in the combined navigation information, the time noise introduced by the laser radar due to scattering and reflection in the environment and the internal clock accuracy and stability, electromagnetic interference, and the wheel speed calibration factor, calibration error, and offset caused by the undesirable rigid connection relationship contained in the IMU information. Taking the above considerations into account, the overall noise distribution no longer meets the requirements of the linear system. The factors that can affect the positioning accuracy contained in each positioning module will eventually be indirectly reflected in the nonlinearity of the system, and the Kalman filter algorithm is not completely suitable for such nonlinear scenarios. Summary of the invention

[0004] In view of this, the present invention provides a multi-sensor fusion positioning method based on factor graph to solve the technical problems of the prior art that after an abnormality occurs in the data at a certain moment, the inference will diverge, and it cannot be recovered in the absence of observation information, and it is not fully applicable to nonlinear scenarios.

[0005] The present invention provides a multi-sensor fusion positioning method based on factor graph, which is applied to autonomous driving vehicles. The method comprises: step 1, converting vehicle sensor data to the same spatial coordinate system and performing time alignment; step 2, after the vehicle is powered on, clearing the original data queue and variable set of the vehicle, and re-importing the time-aligned sensor data for initialization; step 3, obtaining the vehicle initialization position, attitude and speed state according to the global satellite navigation system, and performing the function Construct a priori information factor, where and ∑0 represent the given prior information mean and covariance matrix respectively; Step 4, after receiving the vehicle sensor data, verify the availability of the data, construct the observation information factor and the motion model factor, and then add them to the factor graph; Step 5, optimize the constructed factor graph to form an optimization objective function, solve the optimization objective function through a nonlinear optimization method, obtain the vehicle state, and complete the vehicle positioning.

[0006] Furthermore, the step 1 also includes: eliminating abnormal values ​​through a preset threshold test to facilitate interference prevention.

[0007] Furthermore, the vehicle sensors include inertial sensors, wheel speed sensors, distance sensors, and visual sensors; the sensor data include sensor configuration files, noise parameters, and covariance states.

[0008] Furthermore, the method for constructing the observation information factor and the motion model factor is as follows: Step 41, forming the observation information factor according to the following function: Among them, X i and Z j denote the state quantity and the sensor observation value respectively, ∑ denotes the covariance matrix of the sensor observation noise, and h(·) is the sensor observation model; Step 42, forming a motion model factor according to the following function: Among them, u i represents the information input to the motion model by the system, ∑ represents the covariance matrix of the observation noise, x i and x j is the state quantity at two moments.

[0009] Furthermore, the step 3 further includes: the formed prior information factor is constrained by the following error function: error p =NavState i LocalCoordinate(NavState prior ), where NavState prior The car status is the historical observation information.

[0010] Furthermore, the observation information factor includes global satellite navigation system positioning, laser map construction positioning, and lane line positioning; the step 41 also includes: the formed observation information factor is constrained by the following error function: error G =t_Pose3-t_GNSS,error G =t_Pose3-t_Lidar,error G =t_Pose3-t_Lane, where t_GNSS is integrated navigation, t_Lidar is laser SLAM positioning, and t_Lane is lane line positioning.

[0011] Furthermore, the motion model factors include: inertial sensor factors and wheel speed sensor factors; the step 42 also includes: the formed motion model factors are constrained by the following error function: error I =NavState j LocalCoordinate(NavState j ), error V =R T ·vv b , where NavState is the data structure of the inertial sensor factor, including position, attitude, speed, v b The data structure of the wheel speed sensor factor, including the linear velocity in three-dimensional space and the angular velocity around three axes.

[0012] Furthermore, the nonlinear optimization method is used to solve the optimization objective function, including: step 51, linearization at a given initial value x0; step 52, performing the kth iteration to solve the objective: The increment Δx that reaches the minimum k ; Step 53, when the increment Δx k If x is less than a preset threshold, or the number of iterations k reaches a preset value, the iterative update is stopped; Step 54, otherwise, let x k+1 =x k +Δx k , go to step 52.

[0013] Furthermore, the step 51 is specifically as follows: At the initial value x0, the nonlinear function is approximately converted into a linear function, where is h(x) at x 0 is the Jacobian matrix at , and σ is the disturbance variable.

[0014] Furthermore, the method of solving the optimization objective function by a nonlinear optimization method further includes: step 50, performing maximum a posteriori probability estimation on the factor graph optimization according to minimizing the sum of nonlinear least squares.

[0015] The present invention provides a multi-sensor fusion positioning method based on factor graph. The present application adopts a factor graph algorithm that can fully utilize the constraint information in historical posture data as a fusion algorithm to solve the problem that when short-term observation information is unavailable, the deviation between predicted data and real data is large, and the jump problem is caused after the observation information is restored. Historical data is used to detect abnormal data, and some erroneous information and minor errors can be eliminated by the constraints in the historical data. BRIEF DESCRIPTION OF THE DRAWINGS

[0016] Figure 1 It is a schematic flow chart of a multi-sensor fusion positioning method based on factor graph provided by the present invention;

[0017] Figure 2 It is a schematic diagram of the factor graph fusion process provided by the present invention;

[0018] Figure 3 It is a flow chart of a method for solving an optimization objective function using the nonlinear optimization method provided by the present invention. DETAILED DESCRIPTION

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

[0020] Embodiment 1

[0021] The present invention provides a multi-sensor fusion positioning method based on factor graph, which is applied to autonomous driving vehicles, such as Figure 1 As shown, the method steps are as follows.

[0022] Step 1: Convert the vehicle sensor data to the same spatial coordinate system and perform time alignment;

[0023] Step 2: After the vehicle is powered on, the original data queue and variable set of the vehicle are cleared, and the time-aligned sensor data is re-imported for initialization;

[0024] Step 3: Get the initial position, attitude and speed of the vehicle according to the global satellite navigation system, and use the function Construct a priori information factor, where and∑ 0 Respectively represent the given prior information mean and covariance matrix;

[0025] Step 4: After receiving the vehicle sensor data, verify the availability of the data, construct the observation information factor and the motion model factor, and then build them into the factor graph;

[0026] Step 5: Optimize the constructed factor graph to form an optimization objective function, solve the optimization objective function through a nonlinear optimization method, obtain the vehicle state, and complete the vehicle positioning.

[0027] The present invention provides a multi-sensor fusion positioning method based on factor graph, which adopts a factor graph algorithm that can fully utilize the constraint information in the historical posture data as a fusion algorithm, and utilizes the constraint relationship in the historical data to obtain high-precision positioning information when short-term observation is unavailable, thereby reducing the error between observation and prediction. It aims to improve the positioning capability of the vehicle in all scenarios, especially the positioning retention capability in special scenarios where the combined navigation fails. The results of laser SLAM and lane line matching positioning are integrated into the fusion positioning structure of GNSS / IMU / wheel speed sensor, and when GNSS (Global Satellite Navigation System) is available, it is used as a reference to assist the fusion positioning system for positioning, prevent false fixation, and enhance the reliability of the system; in GNSS denial scenarios, it can replace GNSS to provide global position observation, improve the positioning accuracy in short tunnels, boulevards, high-rise buildings, elevated roads and other scenarios, and there is no need to modify the environment to pre-set up equipment, and it has strong adaptability.

[0028] Embodiment 2

[0029] The present invention provides a multi-sensor fusion positioning method based on factor graph, which is applied to autonomous driving vehicles, such as Figure 2 As shown, the method steps are as follows.

[0030] Step 1: Convert the vehicle sensor data to the same spatial coordinate system and perform time alignment;

[0031] The sensor data, especially the observation information, is converted to a unified spatial coordinate system and time-aligned. In addition, the threshold is set through experience to check and exclude abnormal values ​​to prevent interference. The vehicle sensors include inertial sensors, wheel speed sensors, distance sensors, and line-of-sight sensors; the sensor data includes the sensor's configuration file, noise parameters, and covariance state.

[0032] Step 2: After the vehicle is powered on, the original data queue and variable set of the vehicle are cleared, and the time-aligned sensor data is re-imported for initialization;

[0033] A series of preparatory work is performed after the vehicle is powered on. The data queue and variable set are cleared, and the configuration files, noise parameters, and oblique variance states of each sensor are imported to initialize the global fusion system. The GNSS measurement information is used to obtain the vehicle's initial position, attitude, and speed state to construct a priori information factors, and wait for observation information from other positioning sources or sensor information.

[0034] Step 3: Get the initial position, attitude and speed of the vehicle according to the global satellite navigation system, and use the function Construct a priori information factor, where and∑ 0 Respectively represent the given prior information mean and covariance matrix;

[0035] The prior information factor is used to describe the historical vehicle posture information and estimate the state information. Since the solution of positioning information is related to time, historical information can effectively help the current state estimation, whether from the perspective of linearization, iteration or time correlation. Suppose the state of the car in the historical observation information is NavState prior , so its error function is: error p =NavState i LocalCoordinate(NavState prior ).

[0036] Step 4: After receiving the vehicle sensor data, verify the availability of the data, construct the observation information factor and the motion model factor, and then build them into the factor graph;

[0037] Step 5: Optimize the constructed factor graph to form an optimization objective function, solve the optimization objective function through a nonlinear optimization method, obtain the vehicle state, and complete the vehicle positioning.

[0038] The present invention provides a multi-sensor fusion positioning method based on factor graph, which adopts a factor graph algorithm that can fully utilize the constraint information in the historical posture data as a fusion algorithm, and utilizes the constraint relationship in the historical data to obtain high-precision positioning information when short-term observation is unavailable, thereby reducing the error between observation and prediction. It is intended to improve the positioning capability of the vehicle in all scenarios, especially the positioning retention capability in special scenarios where the combined navigation fails. The results of laser SLAM and lane line matching positioning are integrated into the fusion positioning structure of GNSS / IMU / wheel speed sensor, and when GNSS is available, it is used as a reference to assist the fusion positioning system for positioning, prevent false fixation, and enhance the reliability of the system; in GNSS denial scenarios, it can replace GNSS to provide global position observations, improve the positioning accuracy in short tunnels, boulevards, high-rise buildings, elevated roads and other scenarios, and there is no need to modify the environment to pre-set up equipment, and it has strong adaptability.

[0039] Embodiment 3

[0040] The present invention provides a multi-sensor fusion positioning method based on factor graph, which is applied to autonomous driving vehicles, such as Figure 1 As shown, the method steps are as follows.

[0041] Step 1: Convert the vehicle sensor data to the same spatial coordinate system and perform time alignment;

[0042] Step 2: After the vehicle is powered on, the original data queue and variable set of the vehicle are cleared, and the time-aligned sensor data is re-imported for initialization;

[0043] Step 3: Get the initial position, attitude and speed of the vehicle according to the global satellite navigation system, and use the function Construct a priori information factor, where and∑ 0 Respectively represent the given prior information mean and covariance matrix;

[0044] Step 4: After receiving the vehicle sensor data, verify the availability of the data, construct the observation information factor and the motion model factor, and then build them into the factor graph;

[0045] As mentioned above, when the global fusion algorithm receives the observation data from the sensor, it first verifies the availability of the data to construct the corresponding factors, and then adds them to the factor graph according to the initialized noise weights. The factor graph algorithm of the present invention mainly includes three factors, namely, the prior information factor, the observation information factor and the motion model factor.

[0046] As mentioned above, the factor function for constructing the prior information factor is: In the formula, and∑0 Respectively represent the given prior information mean and covariance matrix. The prior information factor is used to describe the historical vehicle posture information estimation state information. Let the car state of the historical observation information be NavState prior , so its error function is:

[0047] error p =NavState i LocalCoordinate(NavState prior ).

[0048] The factor function for constructing the observation information factor can be expressed as: Among them, X i and Z j Represent the state quantity and the sensor observation value respectively, ∑ represents the covariance matrix of the sensor observation noise, and h(·) is the sensor observation model. GNSS, laser SLAM positioning, and lane positioning are all observation factors. Their data structure contains a three-dimensional point or vector to describe the position or direction in space. The data format is defined as Point3, which is defined as t_GNSS (integrated navigation), t_Lidar (laser SLAM positioning), and t_Lane (lane positioning). Therefore, the error function model of this type of observation information can be defined as:

[0049] error G =t_Pose 3-t_GNSS

[0050] error G =t_Pose 3-t_Lidar

[0051] error G =t_Pose 3-t_Lane

[0052] This type of observation information is mainly used to constrain the optimization of the position component in the calculation.

[0053] The factor function for constructing the motion model factor is: Among them, u i represents the information input to the motion model by the system, ∑ represents the covariance matrix of the observation noise, x i and x j It is the state quantity of two moments before and after. The IMU factor of the gyroscope and the Velocity factor of the wheel speed meter belong to this category. The data structure used by the IMU factor is NavState, which contains information such as position, attitude, and speed. Its error function is defined as follows:

[0054] error I =NavStatej LocalCoordinate(NavState j ), the data structure of the velocity factor is Velocity3, which contains the linear velocity in three-dimensional space and the angular velocity around three axes, expressed as v b To represent, the error function is defined as follows: error V =R T ·vv b .

[0055] Compared with the Kalman filter series algorithms, the factor graph algorithm model adopted in the present invention is more flexible in construction, and the noise no longer has to be Gaussian noise. It can directly process nonlinear factors and non-Gaussian noise, so it can more accurately characterize the state of the system; and the factor graph algorithm is linearized multiple times during the iteration process, and the error is smaller than that of the Kalman filter series which only performs linearization once, and good results can be achieved when the state changes greatly.

[0056] Step 5: Optimize the constructed factor graph to form an optimization objective function, solve the optimization objective function through a nonlinear optimization method, obtain the vehicle state, and complete the vehicle positioning.

[0057] The present invention provides a multi-sensor fusion positioning method based on factor graph, which adopts a factor graph algorithm that can fully utilize the constraint information in the historical posture data as a fusion algorithm, and utilizes the constraint relationship in the historical data to obtain high-precision positioning information when short-term observation is unavailable, thereby reducing the error between observation and prediction. It is intended to improve the positioning capability of the vehicle in all scenarios, especially the positioning retention capability in special scenarios where the combined navigation fails. The results of laser SLAM and lane line matching positioning are integrated into the fusion positioning structure of GNSS / IMU / wheel speed sensor, and when GNSS is available, it is used as a reference to assist the fusion positioning system for positioning, prevent false fixation, and enhance the reliability of the system; in GNSS denial scenarios, it can replace GNSS to provide global position observations, improve the positioning accuracy in short tunnels, boulevards, high-rise buildings, elevated roads and other scenarios, and there is no need to modify the environment to pre-set up equipment, and it has strong adaptability.

[0058] Embodiment 4

[0059] The present invention provides a multi-sensor fusion positioning method based on factor graph, which is applied to autonomous driving vehicles, such as Figure 1 As shown, the method steps are as follows.

[0060] Step 1: Convert the vehicle sensor data to the same spatial coordinate system and perform time alignment;

[0061] Step 2: After the vehicle is powered on, the original data queue and variable set of the vehicle are cleared, and the time-aligned sensor data is re-imported for initialization;

[0062] Step 3: Get the initial position, attitude and speed of the vehicle according to the global satellite navigation system, and use the function Construct a priori information factor, where and∑ 0 Respectively represent the given prior information mean and covariance matrix;

[0063] Step 4: After receiving the vehicle sensor data, verify the availability of the data, construct the observation information factor and the motion model factor, and then build them into the factor graph;

[0064] Step 5: Optimize the constructed factor graph to form an optimization objective function, solve the optimization objective function through a nonlinear optimization method, obtain the vehicle state, and complete the vehicle positioning.

[0065] Considering the real-time requirements, after constructing each factor graph structure, it is optimized at regular intervals, and the corresponding optimization objective function is obtained according to the preset time. The vehicle state at the corresponding moment is solved by using nonlinear optimization method. Figure 3 As shown, the steps of solving the optimization objective function by the nonlinear optimization method are as follows.

[0066] Step 50, performing maximum a posteriori probability estimation on the factor graph optimization according to minimizing the sum of nonlinear least squares.

[0067] The maximum a posteriori probability estimation problem of factor graph optimization can be transformed into minimizing the sum of nonlinear least squares, that is: Maximum a posteriori estimation (MAP) is a point estimate of an unobservable quantity obtained based on empirical data. It is similar to maximum likelihood estimation, but the biggest difference is that the maximum a posteriori estimation incorporates the prior distribution of the quantity to be estimated, so the maximum a posteriori estimation can be regarded as a regularized maximum likelihood estimation.

[0068] Step 51, linearization is performed at a given initial value x0;

[0069] The step 51 is specifically: At the initial value x0, the nonlinear function is approximately converted into a linear function, where is h(x) at x 0 is the Jacobian matrix at , and σ is the disturbance variable.

[0070] Step 52, perform the kth iteration and solve the target: The increment Δx that reaches the minimum k ;

[0071] Step 53, when the increment Δx k If the value is less than a preset threshold or the number of iterations k reaches a preset value, the iterative update is stopped;

[0072] Step 54, otherwise, let x k+1 =x k +Δx k , go to step 52.

[0073] In addition, when optimization is not performed or fails within a short period of time, the result obtained by integrating the IMU data is used as the vehicle status output. If the optimization fails for more than a period of time, an error is reported.

[0074] The present invention provides a multi-sensor fusion positioning method based on factor graph, which adopts a factor graph algorithm that can fully utilize the constraint information in the historical posture data as a fusion algorithm, and utilizes the constraint relationship in the historical data to obtain high-precision positioning information when short-term observation is unavailable, thereby reducing the error between observation and prediction. It is intended to improve the positioning capability of the vehicle in all scenarios, especially the positioning retention capability in special scenarios where the combined navigation fails. The results of laser SLAM and lane line matching positioning are integrated into the fusion positioning structure of GNSS / IMU / wheel speed sensor, and when GNSS is available, it is used as a reference to assist the fusion positioning system for positioning, prevent false fixation, and enhance the reliability of the system; in GNSS denial scenarios, it can replace GNSS to provide global position observations, improve the positioning accuracy in short tunnels, boulevards, high-rise buildings, elevated roads and other scenarios, and there is no need to modify the environment to pre-set up equipment, and it has strong adaptability.

[0075] In summary, the embodiments of the present invention provide a multi-sensor fusion positioning method based on factor graph. Aiming at the positioning problem in the operation of autonomous driving vehicles, a factor graph optimization fusion positioning algorithm based on GNSS / IMU / wheel speed sensor / laser SLAM / lane line positioning is proposed to solve the problem that when short-term observation information is unavailable, the deviation between the predicted data and the real data is large, and the jump problem is caused after the observation information is restored. Historical data is used to detect abnormal data, and some erroneous information and minor errors can be eliminated by the constraints in the historical data.

[0076] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the scope of protection of the present invention.

Claims

1. A multi-sensor fusion positioning method based on factor graph, applied to autonomous driving vehicles, characterized in that: The method comprises: Step 1: Convert the vehicle sensor data to the same spatial coordinate system and perform time alignment; Step 2: After the vehicle is powered on, the original data queue and variable set of the vehicle are cleared, and the time-aligned sensor data is re-imported for initialization; Step 3: Get the initial position, attitude and speed of the vehicle according to the global satellite navigation system, and use the function Construct a priori information factors and add them to the factor graph, where and ∑0 represent the given prior information mean and covariance matrix respectively; Step 4: After receiving the vehicle sensor data, verify the availability of the data, construct the observation information factor and the motion model factor, and then add them to the factor graph; Step 5: Optimize the constructed factor graph to form an optimization objective function, solve the optimization objective function through a nonlinear optimization method, obtain the vehicle state, and complete the vehicle positioning.

2. According to the multi-sensor fusion positioning method based on factor graph in claim 1, it is characterized in that: The step 1 also includes: eliminating abnormal values ​​through a preset threshold test to facilitate interference prevention.

3. According to the multi-sensor fusion positioning method based on factor graph in claim 1, it is characterized in that: The vehicle sensors include inertial sensors, wheel speed sensors, distance sensors, and visual sensors; the sensor data include sensor configuration files, noise parameters, and covariance states.

4. According to the multi-sensor fusion positioning method based on factor graph in claim 1, it is characterized in that: The method for constructing the observation information factor and the motion model factor is: Step 41, forming an observation information factor according to the following function: Among them, X i and Z j denote the state quantity and the sensor observation value respectively, ∑ denotes the covariance matrix of the sensor observation noise, and h(·) is the sensor observation model; Step 42, forming a motion model factor according to the following function: Among them, u i represents the information input to the motion model by the system, ∑ represents the covariance matrix of the observation noise, x i and x j is the state quantity at two moments.

5. According to claim 4, a multi-sensor fusion positioning method based on factor graph is characterized in that: The step 3 also includes: the formed prior information factor is constrained by the following error function: p =NavState i LocalCoordinate(NavState prior ), where NavState prior The car status is the historical observation information.

6. According to claim 4, a multi-sensor fusion positioning method based on factor graph is characterized in that: The observation information factors include global satellite navigation system positioning, laser map construction positioning, and lane line positioning; The step 41 also includes: the formed observation information factor is constrained by the following error function: error G =t_Pose3-t_GNSS, error G = t_Pose3 - t_Lidar, error G = t_Pose3 - t_Lane, Among them, t_GNSS is integrated navigation, t_Lidar is laser SLAM positioning, and t_Lane is lane line positioning.

7. The multi-sensor fusion positioning method based on factor graph according to claim 4 is characterized in that: The motion model factors include: an inertial sensor factor and a wheel speed sensor factor; The step 42 also includes: the formed motion model factor is constrained by the following error function: error I =NavState j LocalCoordinate(NavState j ), error V =R T ·v-v b , Among them, NavState is the data structure of inertial sensor factors, including position, attitude, speed, v b The data structure of the wheel speed sensor factor, including the linear velocity in three-dimensional space and the angular velocity around three axes.

8. The multi-sensor fusion positioning method based on factor graph according to claim 1, characterized in that: The method of solving the optimization objective function by a nonlinear optimization method includes: Step 51, linearization is performed at a given initial value x0; Step 52, perform the kth iteration and solve the target: The increment Δx that reaches the minimum k ; Step 53, when the increment Δx k If the value is less than a preset threshold or the number of iterations k reaches a preset value, the iterative update is stopped; Step 54, otherwise, let x k+1 =x k +Δx k , go to step 52.

9. The multi-sensor fusion positioning method based on factor graph according to claim 8, characterized in that: The step 51 is specifically: At the initial value x0, the nonlinear function is approximately converted into a linear function, where is the Jacobian matrix of h(x) at x0, and σ is the disturbance variable.

10. The multi-sensor fusion positioning method based on factor graph according to claim 8, characterized in that: The method of solving the optimization objective function by a nonlinear optimization method further includes: step 50, performing maximum a posteriori probability estimation on the factor graph optimization according to minimizing the sum of nonlinear least squares.

Images