Multi-source data information fusion positioning method, system and device and storage medium
By using a multi-source data fusion positioning method, and by fusing IMU and map matching positioning information using an error state Kalman filter model, the positioning accuracy problem when satellite navigation is lost is solved, and high-precision and smooth positioning of vehicles is achieved in all scenarios.
Patent Information
- Application Number
- CN202411159233.6
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-08-22
- Publication Date
- 2024-11-22
AI Technical Summary
Existing technologies cannot maintain high accuracy in vehicle positioning in areas with obstructions or sparse satellites, especially in scenarios where satellite navigation and positioning information is lost, leading to pose jumps and error divergence.
A multi-source data fusion positioning method is adopted, which uses an error state Kalman filter model to fuse IMU, satellite navigation, and map matching positioning information. Through timestamp alignment and covariance processing, high-precision fusion positioning of multi-source data is achieved.
Even in the event of a loss of satellite navigation and positioning information, the system maintains the smoothness and high precision of vehicle positioning, avoids pose jumps, and achieves high-precision positioning in all scenarios.
Smart Images

Figure CN119001799A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the field of autonomous driving, and in particular to a fusion positioning method, system, device and storage medium for multi-source data information. Background Art
[0002] In actual vehicle or mobile robot systems, there are generally five sources of sensor data: 1. Satellite navigation positioning information (GNSS pose), mainly GPS, Beidou, etc.; 2. Inertial navigation information (IMU), including gyroscope and accelerometer information; 3. Vehicle body speed (vehicle velocity, referred to as vv, generally only the forward speed is taken for the vehicle body coordinate system, and the lateral and vertical directions are both 0); 4. Perception information based on vehicle-mounted cameras or lidars; 5. Maps, generally from high-precision map suppliers or self-built.
[0003] The existing vehicle positioning technology relies on GNSS positioning information (satellite navigation positioning information) in open areas, and relies on inertial navigation and the recursion of the vehicle speed information in areas with occlusion or sparse satellite numbers (such as tunnels, long overpasses, dense high-rise buildings, etc.), so as to obtain the system's absolute position in the earth coordinate system, and based on this position, the camera or lidar's perception information is matched with the map (Map Matching, map matching positioning information) to obtain the geographic position (Map Matching Pose, referred to as mm pose) in the map coordinate system. However, the geographic position obtained by this technology is not smooth, and it is easy to have position jumps. Moreover, due to the error divergence limitations of the inertial navigation information and the vehicle speed information itself, this technology cannot maintain high accuracy in scenarios such as tunnels or long overpasses where satellite navigation positioning information is lost for a long time. Summary of the invention
[0004] In response to the above technical problems, the present invention provides a multi-source data information fusion positioning method, system, device and storage medium, which can maintain high-precision positioning in the scenario where satellite navigation positioning information is lost.
[0005] A first aspect of the present invention provides a fusion positioning method for multi-source data information, comprising: Acquire multi-source data information, perform multi-source data information fusion positioning based on the error state Kalman filter model, obtain the state vector of the vehicle motion model and the covariance matrix corresponding to the state vector, and use the current vehicle body speed to update the observation information of the current speed state vector; If there is satellite navigation positioning information, the first posture observation is updated according to the satellite navigation positioning information; if there is map matching positioning information, the second posture observation is updated.
[0006] In an optional embodiment, the multi-source data information fusion positioning method further includes cyclically updating the first pose observation and updating the second pose observation, and storing the results of the first pose observation and updating the second pose observation at the current moment in the history state queue Buffer state The historical state queue includes a state vector, a covariance matrix corresponding to the state vector, and IMU information.
[0007] In an optional implementation, if there is satellite navigation positioning information, updating the first pose observation according to the satellite navigation positioning information includes: According to the time information of the satellite navigation positioning information, the timestamp alignment is performed in the historical state queue, which is recorded as T n-k state The state vector corresponding to the satellite navigation positioning information is recorded as state n-k ; The satellite navigation positioning information includes an input covariance matrix. Based on the input covariance matrix of the satellite navigation positioning information and the state vector, the positioning information is observed and updated to obtain the first posture observation update.
[0008] In an optional implementation, the updating of the observation of positioning information based on the state vector and the covariance matrix corresponding to the state vector to obtain the first posture observation update includes: The input covariance matrix of the satellite navigation positioning information is processed according to the following formula: cov veh =scale*cov gnss / mm ; where cov gnss / mm is the covariance matrix of satellite navigation positioning information or map matching positioning information, cov veh To perform covariance processing on the plane position; scale is obtained from the input covariance matrix, scale = var x / var constant var constant To meet the parameters of the scene with normal accuracy requirements; The processed covariance matrix is linearly transformed according to the following formula to obtain the observed covariance matrix: cov global =(Trans*cov veh )*(Trans*cov veh ) T ;cov global is the observation covariance matrix, and Trans is the coordinate parameter that converts the current heading angle from the covariance matrix of the vehicle coordinate system to the enu coordinate system.
[0009] In an optional implementation, the updating of the observation of the positioning information based on the input covariance matrix of the satellite navigation positioning information and the state vector to obtain the first posture observation update includes: The state vector state is calculated and updated according to the following observation Jacobian matrix formula n-k , and update the corresponding covariance matrix; H gnss =[I 0 0 0 0]I is the identity matrix; The current satellite navigation positioning information is stored in a history state queue, recorded as Buffergnss; Based on the state vector state n-k将 The IMU information in the historical state queue from time nk to the current time is recursively predicted and updated.
[0010] In an optional implementation, if there is map matching positioning information, the second posture observation update includes: updating the history state queue Buffer according to the time information of the map matching positioning information state The timestamp alignment is recorded as T n-1 state , the state vector corresponding to the map matching positioning information is recorded as state n-1 ; The map matching positioning information includes an input covariance matrix, based on the input covariance matrix of the map matching positioning information and the state vector state n-1 Perform observation update of the positioning information to obtain the second posture observation update.
[0011] In an optional implementation, the historical state queue Buffer gnss Find all the times T where it is determined that the map matching positioning information exists mm The satellite navigation positioning information received afterwards performs cyclic positioning observation on all the satellite navigation positioning information found.
[0012] In an optional implementation, the acquiring of multi-source data information includes acquiring IMU information, vehicle speed information, satellite navigation positioning information, map matching positioning information, visual perception information, and lidar information.
[0013] A second aspect of the present invention provides a fusion positioning system for multi-source data information, comprising: A processing module is used to obtain multi-source data information, perform multi-source data information fusion positioning based on an error state Kalman filter model, obtain a state vector of a vehicle motion model and a covariance matrix corresponding to the state vector, and update observation information of a current speed state vector using a current vehicle body speed; An updating module is used to update the first posture observation according to the satellite navigation positioning information if there is any; and to update the second posture observation according to the map matching positioning information if there is any.
[0014] According to a third aspect of the present invention, there is provided an electronic device, comprising: At least one processor; and at least one memory in communication with the processor, wherein: the memory stores program instructions that can be executed by the processor, and the processor calls the program instructions to execute the multi-source data information fusion positioning method as described in the first aspect of the embodiment of the present invention.
[0015] A fourth aspect of the present invention provides a computer-readable storage medium having a computer program stored thereon. When the computer program is executed by a computer, the method for fusion positioning of multi-source data information as described in the first aspect of the embodiment of the present invention is executed.
[0016] The present invention can obtain a smooth global geographic position of a vehicle or mobile robot system during driving and maintain high-precision positioning in all scenarios. It also supports maintaining high-precision positioning in scenarios where satellite navigation positioning information is lost for a long time. BRIEF DESCRIPTION OF THE DRAWINGS
[0017] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings required for use in the embodiments are briefly introduced below. It should be understood that the following drawings only show certain embodiments of the present invention and therefore should not be regarded as limiting the scope. For ordinary technicians in this field, other related drawings can be obtained based on these drawings without creative work.
[0018] Figure 1 The present invention is a data flow diagram of a method for fusion positioning of multi-source data information in an embodiment of the present invention.
[0019] Figure 2 The figure is a flow chart of a method for fusion positioning of multi-source data information in an embodiment of the present invention.
[0020] Figure 3 It is a schematic diagram of a fusion positioning method of multi-source data information in another embodiment of the present invention.
[0021] Figure 4 The present invention is a schematic diagram of a module of a multi-source data information fusion positioning system according to an embodiment of the present invention.
[0022] Figure 5 FIG. 1 is a schematic structural diagram of an electronic device according to an embodiment of the present invention. DETAILED DESCRIPTION
[0023] 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 those skilled in the art without creative work are within the scope of protection of the present invention.
[0024] It should be understood that the terms "include" and "comprises" used in the specification and claims of the present invention indicate the presence of the described features, wholes, steps, operations, elements and / or components, but do not exclude the presence or addition of one or more other features, wholes, steps, operations, elements, components and / or their collections. It should also be understood that the terms used in this specification of the present invention are only for the purpose of describing specific embodiments and are not intended to limit the present invention. As used in the specification and claims of the present invention, the singular forms of "one", "an" and "the" are intended to include plural forms unless the context clearly indicates otherwise. It should also be further understood that the term "and / or" used in the specification and claims of the present invention refers to any combination of one or more of the associated listed items and all possible combinations, and includes these combinations.
[0025] The present invention fuses multi-source sensor data based on the framework of the Error State Kalman Filter model. Kalman filtering is a common method for realizing multi-source data fusion in the prior art. The multi-source data information comes from: IMU sensor (inertial measurement sensor), lidar sensor, visual sensor (such as camera), satellite navigation positioning, map information, and vehicle speed sensor. The specific derivation process is as follows: The framework of the error state Kalman filter model is: Among them, P n-1 is the state covariance matrix of the previous moment, P n is the state covariance matrix after observation update, P n - is the state covariance matrix after the prediction update, F x is the state Jacobian matrix, F i is the input Jacobian matrix, Q i is the input noise matrix, K is the Kalman gain matrix, V is the observation noise matrix, I is the identity matrix, H T is the transpose of the observation Jacobian matrix, H is the observation Jacobian matrix, and n is the time.
[0026] The above model framework is used for fusion positioning, where the state vector of the model is: x=[pv θ bias a bias ω ]; (2) Where p and v are the three-axis coordinates and rates of the system in the enu coordinate system (northeast celestial coordinate system), θ is the Euler angle, and bias a is the drift error of the IMU input acceleration, bias ω It is the drift error of the IMU input angular velocity.
[0027] The position information obtained by recursively deducing the angular velocity and acceleration information obtained from the IMU through the vehicle kinematic model is used as the prediction information of the error state Kalman filter, which is specifically implemented using the following formula: v n =v n-1 +a*Δt 2 ; (3) Where ω and a are the processed three-axis angular velocity and acceleration, Δt is the processing interval, and n is the time. The state covariance is updated according to the prediction equation, specifically: Where R is the system rotation matrix, a x To take the skew-symmetric matrix of the acceleration vector a, V i is the acceleration noise, θ i is the angular velocity noise, A i Bias a The noise of Ωi is bias ω The noise, F x is the state Jacobian matrix, F i is the input Jacobian matrix, Q i is the input noise matrix, I is the unit matrix, Δt is the processing interval time, R T is the transpose of the rotation matrix.
[0028] Taking the vehicle velocity vector vv as the observation information of the velocity state quantity of the error state Kalman filter, the observation Jacobian matrix of the vehicle velocity is: H v =[0 R T R T *v × *R 0 0]; (7) where v x is the skew-symmetric matrix of the velocity vector, R is the system rotation matrix, R Tis the transpose of the rotation matrix.
[0029] Satellite navigation positioning information (GNSS pose) and geographic pose (MM pose) are used as observation information of the position, and their observation Jacobian matrices are: H gnss =[I 0 0 0 0], I is the unit matrix; (8) H mm =[I 0 I 0 0], I is the unit matrix; (9) The covariance matrix of satellite navigation positioning information and geographic posture in the vehicle coordinate system is: Where a and b are constants, representing the variance of the satellite navigation positioning information in the longitudinal direction of the vehicle coordinate system and the geographic posture in the lateral direction of the vehicle coordinate system, respectively; s1 and s2 are values greater than 1.
[0030] The above covariance matrix is then processed according to the input covariance matrix in the satellite navigation positioning and map matching information: cov veh =scale*cov gnss / mm ; (12) where cov gnss / mm is the covariance matrix of satellite navigation positioning information and map matching posture in the vehicle coordinate system, cov veh is the input covariance matrix of satellite navigation positioning and map matching information. scale is obtained from the input covariance matrix, scale = var x / var constant ;(13). Among them, var constant Parameters that meet normal precision requirements, var x is the variance in the x direction.
[0031] Based on the processed covariance matrix and the heading angle of the current system, the covariance matrix of the vehicle coordinate system is converted to the enu coordinate system: cov global =(Trans*cov veh )*(Trans*Cov veh ) T ; (15) Among them, cov global That is, the observation covariance matrix of the satellite navigation positioning information or map matching pose that ultimately acts on the model.
[0032] In conjunction with the above derivation process, see Figure 1 , Figure 1 The figure is a schematic diagram of the data flows in the present invention. EM is the error state Kalman filter (eksf) processing module, Map Mathcing (MM) is the map matching module, EHP / EHR is the map element module sent to the downstream of the system, and the vehicle speed, IMU, satellite navigation positioning information, camera images or radar data are the input data of each sensor. Fusion pose is the pose after the error state Kalman filter processing. The pose is sent to MM (map matching module, the pose matches the perception information of the camera or lidar with the map) for map matching combined with the map data of MAP. The matching result is the geographic pose (MapMatching Pose), which will be sent back to EM for further filtering processing in EM.
[0033] The speed information, IMU information, and satellite navigation positioning information in the figure are input into the error state Kalman filter. The error state Kalman filter sends the processed geographic pose to the map matching module (Map Mathcing), and the laser radar information or the image information collected by the camera is input into the map matching module. The map matching module combines the geographic pose with the map data for map matching, and the matching result is the map matching positioning information (mm pose). The map matching positioning information is then sent back to the error state Kalman filter, and the error state Kalman filter performs further filtering processing.
[0034] See also Figure 2 , Figure 2 The flowchart of a method for fusion positioning of multi-source data information in an embodiment of the present invention is shown in FIG. The method for fusion positioning of multi-source data information includes the following steps: Step 100: Acquire multi-source data information, perform multi-source data information fusion positioning based on the error state Kalman filter model, obtain the state vector of the vehicle motion model and the covariance matrix corresponding to the state vector, and use the current vehicle body speed to update the observation information of the current speed state vector.
[0035] The obtaining of multi-source data information includes obtaining IMU information, vehicle speed information, satellite navigation positioning information, map matching positioning information, visual perception information, and lidar information.
[0036] Combination Figure 1As shown, the vehicle body speed information, IMU information, and satellite navigation positioning information are obtained. Based on the error state Kalman filter model, the original values of angular velocity and acceleration obtained from the IMU are processed, and the processed angular velocity ω and acceleration a are substituted into formula (2) to recursively deduce the model state vector to obtain the model prediction, and the state vector of the current model and the corresponding state covariance matrix {obtained by formula (3) and formula (4)} and the data in the IMU information are stored in the historical state queue (recording the data of nk moments before the current moment n), recorded as Buffer state The current vehicle body speed can be used to update the observation information of the current speed state vector according to formula (7).
[0037] Step 200: If there is satellite navigation positioning information, the first pose observation is updated according to the satellite navigation positioning information; if there is map matching positioning information, the second pose observation is updated.
[0038] In this step, it is detected whether there is satellite navigation positioning information. If there is satellite navigation positioning information, the timestamp T generated by the satellite navigation positioning information is gnss In the above stored historical state queue Buffer state The timestamp alignment can be performed in order to obtain the corresponding IMU information and state vector. The aligned timestamp is denoted as T n-k state , the corresponding state vector is state n-k , based on this state vector, the satellite navigation positioning information is observed and updated to obtain the first position observation update.
[0039] After the satellite navigation positioning information is lost, if the satellite navigation positioning information is received again, the timestamp alignment can be performed according to the time when the satellite navigation positioning information was generated and the information in the historical state queue. After the timestamp alignment, the corresponding IMU information when the satellite navigation positioning information was generated, and the state vector and the corresponding state covariance matrix of the model at the current moment can be obtained. From this, the observation information of the satellite navigation positioning information before the current moment can be derived, so that the observation information of the time period when the satellite navigation positioning information was lost can be supplemented, so that it is relatively easy to obtain the pose (i.e., geographic pose) in a smooth map coordinate system without pose jumps.
[0040] When performing the first pose observation update, first substitute the input covariance matrix of the received satellite navigation positioning information (satellite navigation positioning information carries the input covariance matrix) into the above formula (12) and formula (13), process the covariance matrix of the satellite navigation positioning information under the vehicle coordinates, and then perform a linear transformation on the processed covariance matrix according to formula (15) to obtain the observation covariance matrix of the satellite navigation positioning information, so that Kalman filtering can be performed according to formula (1) to obtain the geographic pose. Based on this, the observation covariance matrix of the satellite navigation positioning information can be recursively derived from the current time n to the previous time nk to obtain the geographic pose at time nk.
[0041] Furthermore, after the timestamps are aligned, the state vector state corresponding to the satellite navigation positioning information at the current moment can be determined n-k And the related covariance matrix; The observation Jacobian matrix of the satellite navigation positioning information can be calculated according to formula (8), and based on this, the state vector state n-k And the related covariance matrix is updated, such as using formula (10) to obtain the covariance matrix, and the state vector is updated and recorded as state n-k ', then store the current satellite navigation positioning information, its corresponding state vector, and covariance matrix into the historical state queue, recorded as Buffer gnss .
[0042] Based on state n-k 'Re-perform recursive prediction update on the IMU information stored from time nk to the current time. Therefore, the IMU information corresponding to the satellite navigation positioning information at the current time can be calculated according to the above derivation process, and the IMU information at time nk before can be recursively predicted and updated, that is, the result is derived forward to complete the information update.
[0043] In this step, since the original historical state queue stores the IMU information from time nk to the current time n, the current time is the time when the satellite navigation positioning information is received. From the above, it can be seen that the IMU information corresponding to the current time is obtained and updated, and the state vector from time nk to the current time n can be obtained. Combined with the historical state queue Buffer gnss The Kalman filter process is performed using formula (1).
[0044] Then determine whether there is map matching positioning information. If there is map matching positioning information, generate a timestamp T according to the map matching positioning information. mm In the storage history state queue Buffer state The timestamp alignment is performed in , and the aligned timestamp is recorded as T n-l state , the corresponding state vector is staten-l , based on this state vector, the observation update of map matching positioning information is performed.
[0045] The calculation method is consistent with the above satellite navigation positioning information. The input covariance matrix in the map matching positioning information is substituted into formula (12) to obtain the covariance matrix of the map matching positioning information in vehicle coordinates. According to formula (15), the covariance matrix is linearly transformed to obtain the observed covariance matrix of the map matching positioning information.
[0046] Then, according to formula (9), the state obtained based on the above timestamp alignment n-l Calculate and update the state vector and related covariance matrix to get state n-l '. In the above-mentioned historical state queue Buffer of satellite navigation positioning information gnss In the , find all the times T at which the map matching positioning information is obtained mm The satellite navigation positioning information received later, for all the satellite navigation positioning information found, at time T mm The state vector state corresponding to the satellite navigation positioning information is then n-k And the related covariance matrix are repeatedly calculated and updated to obtain the state vector staten-k at the current moment, completing the Kalman filter processing.
[0047] The present invention can obtain a smooth global geographic position of a vehicle or mobile robot system during driving, and maintain high-precision positioning in all scenarios (such as long-term loss of satellite navigation positioning information).
[0048] The difference from the prior art is that the present invention uses multi-source observation input, and the specific input is satellite navigation positioning information and map matching positioning information. At the same time, after a specific analysis of the characteristics of these two types of information, the input noise matrix and the corresponding observation Jacobian matrix of the two are processed in a targeted manner, so that the posture output by the model can have the advantages of both, making the posture accuracy higher. The present invention performs targeted timestamp alignment for the input of multi-source sensors, so that the output posture is smoother without losing the existing sensor information.
[0049] Please continue reading Figure 3 , Figure 3 1 is a flow chart of a method for fusion positioning of multi-source data information in another embodiment of the present invention. The method for fusion positioning of multi-source data information comprises the following steps: Step 301: Model initialization.
[0050] The state vector and state covariance matrix of the error state Kalman filter model (hereinafter referred to as the model) are initialized.
[0051] Step 302: Sensor data input.
[0052] The receiving interface of all vehicle input data receives IMU information, vehicle speed, satellite navigation positioning information, and map matching positioning information.
[0053] Step 303: Determine whether the IMU has input.
[0054] If the IMU has no input, the process goes to step 303. If the IMU has input, the process goes to step 304.
[0055] Step 304: Determine whether the number of IMU information lost frames reaches a threshold. If the number of IMU information lost frames reaches the threshold, return to step 302.
[0056] Step 305: Prediction update.
[0057] The original values of angular velocity and acceleration obtained from the IMU are processed, and the processed angular velocity ω and acceleration a are substituted into formula (2) to recursively deduce the state vector to obtain the predicted value of the model. The state vector at the current moment and the corresponding state covariance matrix and the data in the IMU information are stored in the historical state queue, recorded as Buffer state .
[0058] Step 306: Determine whether the vehicle speed is input.
[0059] If the vehicle speed is not input, the process proceeds to step 307 . If the vehicle speed is input, the vehicle speed is updated.
[0060] Step 307: Vehicle speed update.
[0061] According to formula (7), the vehicle body speed information is calculated as the observation of the speed state vector in the model, and the scale update of the vehicle body speed is also updated here.
[0062] Step 308: Determine whether GNSS (satellite navigation positioning information) is input.
[0063] If input, execute step 309; if not input, execute step 310.
[0064] Step 309: Perform timestamp alignment, update GNSS observations, and re-enter step 308.
[0065] According to the time information of the satellite navigation positioning information, the timestamp alignment is performed in the initial historical state queue (the initial historical state queue includes the state vector, the covariance matrix corresponding to the state vector and the IMU information), which is recorded as T n-k state At this moment, the satellite navigation positioning information corresponds to the historical status queue Buffer stateThe state vector is denoted as state n-k . The satellite navigation positioning information includes an input covariance matrix. The time of the satellite navigation positioning information is thus determined, and the IMU information stored at the same time as the satellite navigation positioning information can also be determined. The state vector and the corresponding state covariance matrix calculated based on the IMU information can also be determined and aligned in the form of a timestamp. Therefore, the IMU information in the history queue can be updated based on the observation results of the satellite navigation positioning information.
[0066] Furthermore, the observation update of the positioning information can be performed based on the input covariance matrix of the satellite navigation positioning information and the state vector to obtain the first posture observation update.
[0067] Specifically, the input covariance matrix of the satellite navigation positioning information is processed according to the following formula: cov veh =scale*cov gnss / mm ; where cov gnss / mm is the covariance matrix of satellite navigation positioning information or map matching positioning information, cov veh To perform covariance processing of plane position; scale is obtained from the input covariance matrix. Where scale = var x / var constant var constant Parameters for scenarios that meet normal accuracy requirements.
[0068] Then, the processed covariance matrix is linearly transformed according to the following formula to obtain the observed covariance matrix: cov global =(Trans*cov veh )*(Trans*cov veh ) T ;cov global is the observation covariance matrix, and Trans is the coordinate parameter for converting the current heading angle from the covariance matrix of the vehicle coordinate system to the enu coordinate system (northeast celestial coordinate system).
[0069] Then the state vector state is calculated and updated according to the following observation Jacobian matrix formula n-k , and update the corresponding covariance matrix; H gnss =[I 0 0 0 0]I is the identity matrix; The current satellite navigation positioning information is stored in the historical state queue, recorded as Buffer gnss ; Based on the state vector state n-kThe IMU information in the historical state queue from time nk to the current time is recursively predicted and updated.
[0070] In this step, the first pose observation update is performed cyclically, and the results of the first pose observation update at the current moment and the second pose observation update are stored in the history state queue Buffer state to facilitate information updating.
[0071] Step 310: Determine whether mm (map matching positioning information) is input.
[0072] If yes, execute step 311 , if no, execute step 312 .
[0073] Step 311: perform timestamp alignment, perform mm observation update, and re-enter step 308.
[0074] The time information of the map matching positioning information is stored in the history state queue Buffer state The timestamp alignment is recorded as T n-1 state , the state vector corresponding to the map matching positioning information is recorded as state n-1 .
[0075] The map matching positioning information includes an input covariance matrix, based on the input covariance matrix of the map matching positioning information and the state vector state n-1 Perform observation update of the positioning information to obtain the second posture observation update.
[0076] Specifically, the input covariance matrix of the map matching positioning information is processed according to the following formula to obtain the covariance matrix of the map matching positioning information in the vehicle body coordinates: cov veh =scale*cov gnss / mm where cov gnss / mm is the covariance matrix of satellite navigation positioning information or map matching positioning information, cov veh To perform covariance processing of plane position; scale is obtained from the input covariance matrix. scale=var x / var constant var constant Parameters for scenarios that meet normal accuracy requirements.
[0077] Then, the processed covariance matrix is linearly transformed according to the following formula to obtain the observed covariance matrix of the map matching positioning information: cov global =(Trans*covveh )*(Trans*cov veh ) T ;cov globa1 is the observation covariance matrix, Trans It is the coordinate parameter for converting the current heading angle from the covariance matrix of the vehicle coordinate system to the enu coordinate system (northeast celestial coordinate system).
[0078] Then the state vector state is calculated and updated according to the following observation Jacobian matrix formula n-1 , and update the corresponding covariance matrix to get state n-1 '; H mm =[I 0 I 0 0], where I is the unit matrix.
[0079] Store the results of the first pose observation update and the second pose observation update at the current moment into the historical state queue Buffer gnss .
[0080] For example, in the historical state queue Buffer gnss Find all the times T where it is determined that the map matching positioning information exists mm The satellite navigation positioning information received thereafter performs cyclic positioning observation on all the satellite navigation positioning information found, and then obtains the state vector at the current moment; specifically, steps 307 to 308 are executed as described above with respect to the satellite navigation positioning information.
[0081] Step 312: Output the pose.
[0082] like Figure 4 As shown, the present invention also provides a fusion positioning system of multi-source data information, including: The processing module 41 is used to obtain multi-source data information, perform multi-source data information fusion positioning based on the error state Kalman filter model, obtain the state vector of the vehicle motion model and the covariance matrix corresponding to the state vector, and use the current vehicle body speed to update the observation information of the current speed state vector. The multi-source data information acquisition includes obtaining IMU information, vehicle body speed information, satellite navigation positioning information, map matching positioning information, visual perception information, and laser radar information.
[0083] The updating module 42 is used to determine whether there is satellite navigation positioning information. If so, the first posture observation update is performed according to the satellite navigation positioning information; if there is no satellite navigation positioning information, it is determined whether there is map matching positioning information. If so, the second posture observation update is performed according to the map matching positioning information.
[0084] In some embodiments, the multi-source data information fusion positioning system further includes an output module 43 for performing map matching on the data information sensed by the visual sensor or the lidar sensor to output the geographic position if the map matching positioning information does not exist.
[0085] The multi-source data information fusion positioning system also includes a loop module, which is used to loop the first pose observation update and the second pose observation update, and store the results of the first pose observation update and the second pose observation update at the current moment into the history state queue Buffer gnss ; The historical state queue includes a state vector, a covariance matrix corresponding to the state vector, and IMU information.
[0086] The updating module 42 is further used to: perform time stamp alignment in the historical state queue according to the time information of the satellite navigation positioning information, denoted as T n-k state The state vector corresponding to the satellite navigation positioning information is recorded as state n-k ; The satellite navigation positioning information includes an input covariance matrix, and the positioning information is observed and updated based on the input covariance matrix of the satellite navigation positioning information and the state vector to obtain the first posture observation update.
[0087] The observation update of positioning information based on the state vector and the covariance matrix corresponding to the state vector to obtain the first posture observation update is specifically: processing the input covariance matrix of the satellite navigation positioning information according to the following formula: cov veh =scale*cov gnss / mm ; where cov gnss / mm is the covariance matrix of satellite navigation positioning information or map matching positioning information, cov veh To perform covariance processing of plane position; scale is obtained from the input covariance matrix, scale=var x / var constant var constant To meet the parameters of the scene with normal accuracy requirements; The processed covariance matrix is linearly transformed according to the following formula to obtain the observed covariance matrix: cov global =(Trans*cov veh )*(Trans*cov veh ) T ;cov globa1 is the observation covariance matrix, Trans To convert the current heading angle from the covariance matrix of the vehicle coordinate system to the coordinate parameters of the enu coordinate system.
[0088] The satellite navigation positioning information includes an input covariance matrix, and the state vector state is calculated and updated according to the following observation Jacobian matrix formula n-k , and update the corresponding covariance matrix; H gnss =[I 0 0 0 0]I is the identity matrix; The current satellite navigation positioning information is stored in the historical state queue, recorded as Buffer gnss ; Based on the state vector state n-k The IMU information in the historical state queue from time nk to the current time is recursively predicted and updated.
[0089] The updating module 42 is further used to: update the historical state queue Buffer according to the time information of the map matching positioning information state The timestamp alignment is recorded as T n-l state , the state vector corresponding to the map matching positioning information is recorded as state n-1 The map matching positioning information includes an input covariance matrix, based on the input covariance matrix of the map matching positioning information and the state vector state n-1 Perform observation update of positioning information to obtain the second posture observation update; and in the history state queue Buffer gnss Find all the times T where it is determined that the map matching positioning information exists mm The satellite navigation positioning information received afterwards performs cyclic positioning observation on all the satellite navigation positioning information found.
[0090] The specific execution process can refer to the description of the fusion positioning method of the above-mentioned multi-source data information, which will not be repeated here.
[0091] like Figure 5 As shown, the present invention also provides an electronic device, including: At least one processor; and at least one memory in communication with the processor, wherein: the memory stores program instructions that can be executed by the processor, and the processor calls the program instructions to execute the above-mentioned multi-source data information fusion positioning method.
[0092] The present invention also provides a computer-readable storage medium, on which a computer program is stored. When the computer program is executed by a processor, the above-mentioned multi-source data information fusion positioning method is implemented.
[0093] It can be understood that computer-readable storage media may include: any entity or device capable of carrying a computer program, recording media, USB flash drives, mobile hard disks, magnetic disks, optical disks, computer memory, read-only memory (ROM), random access memory (RAM), and software distribution media, etc. A computer program includes computer program code. The computer program code may be in source code form, object code form, executable file, or some intermediate form, etc. A computer-readable storage medium may include: any entity or device capable of carrying a computer program code, recording media, USB flash drives, mobile hard disks, magnetic disks, optical disks, computer memory, read-only memory (ROM), random access memory (RAM), and software distribution media, etc.
[0094] In certain embodiments of the present invention, the electronic device may include a controller or a processor, and the controller is a single-chip microcomputer chip that integrates a processor, a memory, a communication module, etc. The processor may refer to the processor contained in the controller. The processor may be a central processing unit (CPU), or other general-purpose processors, digital signal processors (DSP), application specific integrated circuits (ASIC), field-programmable gate arrays (FPGA) or other programmable logic devices, discrete gates or transistor logic devices, discrete hardware components, etc.
[0095] Any process or method description in a flowchart or otherwise described herein may be understood to represent a module, segment or portion of code that includes one or more executable instructions for implementing the steps of a specific logical function or process, and the scope of the preferred embodiments of the present invention includes alternative implementations in which functions may not be performed in the order shown or discussed, including performing functions in a substantially simultaneous manner or in the reverse order depending on the functions involved, which should be understood by technicians in the technical field to which the embodiments of the present invention belong.
[0096] Those of ordinary skill in the art will appreciate that the units and algorithm steps of each example described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, computer software, or a combination of the two. In order to clearly illustrate the interchangeability of hardware and software, the composition and steps of each example have been generally described in terms of function in the above description. Whether these functions are performed in hardware or software depends on the specific application and design constraints of the technical solution. Professional and technical personnel can use different methods to implement the described functions for each specific application, but such implementation should not be considered to be beyond the scope of the present invention.
[0097] The above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit the same. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that the technical solutions described in the aforementioned embodiments may still be modified, or some of the technical features may be replaced by equivalents. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present invention.
Claims
1. A fusion positioning method for multi-source data information, characterized in that: include: Acquire multi-source data information, perform multi-source data information fusion positioning based on the error state Kalman filter model, and obtain the state vector of the vehicle motion model and the covariance matrix corresponding to the state vector; If there is satellite navigation positioning information, the first posture observation is updated according to the satellite navigation positioning information; if there is map matching positioning information, the second posture observation is updated.
2. The fusion positioning method of multi-source data information according to claim 1 is characterized in that: It also includes looping the first pose observation update and the second pose observation update, and storing the results of the first pose observation update and the second pose observation update at the current moment into the history state queue Buffer state The historical state queue includes a state vector, a covariance matrix corresponding to the state vector, and IMU information.
3. The fusion positioning method of multi-source data information according to claim 2 is characterized in that: If there is satellite navigation positioning information, updating the first pose observation according to the satellite navigation positioning information includes: According to the time information of the satellite navigation positioning information, the timestamp alignment is performed in the historical state queue, which is recorded as T n-k state The state vector corresponding to the satellite navigation positioning information is recorded as state n-k ; The satellite navigation positioning information includes an input covariance matrix. Based on the input covariance matrix of the satellite navigation positioning information and the state vector, the positioning information is observed and updated to obtain the first posture observation update.
4. The fusion positioning method of multi-source data information according to claim 3 is characterized in that: The step of performing observation update of positioning information based on the input covariance matrix of the satellite navigation positioning information and the state vector to obtain the first posture observation update includes: The input covariance matrix of the satellite navigation positioning information is processed according to the following formula: ; where cov gnss / mm is the covariance matrix of satellite navigation positioning information or map matching positioning information, cov veh To perform covariance processing of plane position; scale is obtained from the input covariance matrix, var constant To meet the parameters of the scene with normal accuracy requirements; The processed covariance matrix is linearly transformed according to the following formula to obtain the observed covariance matrix: ;cov global is the observation covariance matrix, and Trans is the coordinate parameter that converts the current heading angle from the covariance matrix of the vehicle coordinate system to the enu coordinate system.
5. The fusion positioning method of multi-source data information according to claim 4 is characterized in that: The step of performing observation update of positioning information based on the input covariance matrix of the satellite navigation positioning information and the state vector to obtain the first posture observation update includes: The state vector state is calculated and updated according to the following observation Jacobian matrix formula n-k , and update the corresponding covariance matrix; , I is the identity matrix; The current satellite navigation positioning information is stored in the historical state queue, recorded as Buffer gnss ; Based on the state vector state n-k The IMU information in the historical state queue from time nk to the current time is recursively predicted and updated.
6. The fusion positioning method of multi-source data information according to claim 2 or 3, characterized in that: If there is map matching positioning information, performing a second posture observation update includes: According to the time information of the map matching positioning information, the historical state queue Buffer state The timestamp alignment is recorded as T n-l state , the state vector corresponding to the map matching positioning information is recorded as state n-1 ; The map matching positioning information includes an input covariance matrix, based on the input covariance matrix of the map matching positioning information and the state vector state n-1 Perform observation update of the positioning information to obtain the second posture observation update.
7. The fusion positioning method of multi-source data information according to claim 6 is characterized in that: In the history state queue Buffer gnss Find all the times T where it is determined that the map matching positioning information exists mm The satellite navigation positioning information received afterwards performs cyclic positioning observation on all the satellite navigation positioning information found.
8. The fusion positioning method of multi-source data information according to claim 1 is characterized in that: The obtaining of multi-source data information includes obtaining IMU information, vehicle speed information, satellite navigation positioning information, map matching positioning information, visual perception information, and lidar information.
9. A fusion positioning system of multi-source data information, characterized in that: include: A processing module is used to obtain multi-source data information, perform multi-source data information fusion positioning based on an error state Kalman filter model, and obtain a state vector of a vehicle motion model and a covariance matrix corresponding to the state vector; An updating module is used to update the first posture observation according to the satellite navigation positioning information if there is any; and to update the second posture observation according to the map matching positioning information if there is any.
10. An electronic device, characterized in that: include: at least one processor; And at least one memory communicatively connected to the processor, wherein: the memory stores program instructions executable by the processor, and the processor calls the program instructions to execute the multi-source data information fusion positioning method as described in any one of claims 1 to 8.
11. A computer-readable storage medium, characterized in that: A computer program is stored thereon, and when the computer program is run by a computer, the method for fusion positioning of multi-source data information as claimed in any one of claims 1 to 8 is executed.
