Vehicle positioning method, device, equipment and storage medium

Abstract

The present invention relates to the field of automatic driving control, and discloses a vehicle positioning method, device, equipment and storage medium. Real-time sensor data and factor graph of the vehicle are obtained, wherein the factor graph contains multiple first state nodes arranged in observation time sequence during driving; according to the first observation time of the sensor data, an association operation is performed on the sensor data and the first state node of the factor graph to obtain a new factor graph; using the preset state change conditions, the new factor graph is linearized and the state increment is updated to obtain each observation state information corresponding to the vehicle; according to each observation state information, the vehicle's posture is propagated and updated in turn to obtain the vehicle posture information, and the vehicle is positioned based on the vehicle posture information. The present invention realizes the time alignment of different types of sensor data, and improves the accuracy of vehicle positioning.

Description

Technical Field

[0001] The present invention relates to the field of automatic driving control, and in particular to a vehicle positioning method, device, equipment and storage medium. Background Art

[0002] Real-time vehicle positioning is an indispensable key module in the autonomous driving system. It provides real-time vehicle position information for downstream planning and perception modules, facilitating better decision-making and control of the vehicle. Because traditional filtering schemes have the advantages of good real-time performance, low computing resources and high reliability, traditional vehicle real-time positioning systems mostly use filtering schemes. However, due to some limitations of the filtering scheme itself, the positioning accuracy of the real-time positioning of the vehicle is easily affected by the sensor data with uncertain delays and observation state data with strong nonlinearity that are prone to appear during vehicle driving, making it difficult to achieve ideal results. At the same time, in the process of optimizing the observation state data, the traditional nonlinear optimization scheme cannot take into account both efficiency and accuracy, and it is difficult to run in real time. In short, the existing real-time positioning method of vehicles has the problem of low positioning accuracy. Summary of the invention

[0003] The main purpose of the present invention is to solve the technical problem that the existing real-time positioning method of vehicles has low positioning accuracy.

[0004] A first aspect of the present invention provides a vehicle positioning method, comprising: obtaining real-time sensor data and a factor graph of a vehicle, wherein the factor graph comprises a plurality of first state nodes arranged in observation time sequence during driving; performing an association operation on the sensor data and the first state node of the factor graph according to the first observation time of the sensor data to obtain a new factor graph; using a preset state change condition, linearizing and incrementally updating the new factor graph to obtain each observation state information corresponding to the vehicle; and propagating and updating the posture of the vehicle in turn according to each observation state information to obtain vehicle posture information, and positioning the vehicle based on the vehicle posture information.

[0005] Optionally, in a first implementation method of the first aspect of the present invention, the sensor data and the first state node of the factor graph are associated with each other according to the first observation time of the sensor data to obtain a new factor graph, including: determining the second observation time of each first state node in the factor graph, and respectively calculating the time difference between each second observation time and the first observation time; selecting at least one first state node from each first state node in order of time difference from small to large; and associating the sensor data with the selected first state node using a preset IMU integration algorithm according to the time difference and second observation time corresponding to the selected first state node to obtain a new factor graph.

[0006] Optionally, in a second implementation of the first aspect of the present invention, the new factor graph includes a first factor graph and a second factor graph, and the sensor data is associated with the selected first state node according to the time difference corresponding to the selected first state node and the second observation time using a preset IMU integration algorithm to obtain a new factor graph including: determining whether the time difference corresponding to the selected first state node is less than a preset time difference threshold; if it is less than the preset time difference threshold, synchronizing the sensor data to the first state node with the smallest time difference using a preset IMU integration algorithm, and associating the observation factor corresponding to the sensor data to the first state node with the smallest time difference to obtain a first factor graph; if it is not less than the preset time difference threshold, generating a second state node using the sensor data using a preset IMU integration algorithm; associating the second state node with the selected first state node according to the second observation time corresponding to the selected first state node, and associating the observation factor corresponding to the sensor data to the second state node to obtain a second factor graph.

[0007] Optionally, in a third implementation of the first aspect of the present invention, associating the second state node with the selected first state node according to the second observation time corresponding to the selected first state node includes: determining whether the second observation time corresponding to the selected first state node is less than a preset time threshold; if it is less than the preset time threshold, associating the second state node with the subsequent first state node with the smallest time difference; if it is not less than the preset time threshold, associating the second state node with the previous first state node and the subsequent first state node with the smallest time difference, respectively.

[0008] Optionally, in a fourth implementation of the first aspect of the present invention, the new factor graph is linearized and incrementally updated using a preset state change condition to obtain each observed state information corresponding to the vehicle, including: calculating the state difference between every two adjacent state nodes in the new factor graph, and selecting a state node whose state difference is greater than a preset state change threshold from the new factor graph; linearizing the state nodes selected from the new factor graph; arranging them in observation time sequence, and using a reverse chain propagation method to update the state of the subsequent state nodes in the linearized factor graph using the previous state nodes; stopping when the update amount of the state update is less than the preset state update threshold, to obtain each observed state information corresponding to the vehicle.

[0009] Optionally, in a fifth implementation method of the first aspect of the present invention, the posture of the vehicle is propagated and updated in sequence according to each of the observation status information to obtain the vehicle posture information, including: determining the posture information corresponding to the first observation status information at the observation time, and using the subsequent observation status information and the previous posture information to calculate the posture information corresponding to the subsequent observation status information at the observation time, until the posture information corresponding to the latest observation time is calculated and stopped to obtain the vehicle posture information.

[0010] Optionally, in a sixth implementation method of the first aspect of the present invention, after performing an association operation on the sensor data and the first state node of the factor graph according to the first observation time of the sensor data to obtain a new factor graph, it also includes: using a preset time window to filter edge state nodes from the new factor graph and placing them in a preset elimination sequence; performing elimination operations on the elimination sequence to remove edge state nodes in the new factor graph.

[0011] The second aspect of the present invention provides a vehicle positioning device, comprising: an acquisition module, used to acquire real-time sensor data and a factor graph of the vehicle, wherein the factor graph comprises a plurality of first state nodes arranged in observation time sequence during driving; an association module, used to perform an association operation on the sensor data and the first state node of the factor graph according to the first observation time of the sensor data, so as to obtain a new factor graph; an optimization module, used to perform linearization processing and state incremental update on the new factor graph using preset state change conditions, so as to obtain each observation state information corresponding to the vehicle; and a positioning module, used to propagate and update the posture of the vehicle in turn according to each observation state information, so as to obtain vehicle posture information, and to position the vehicle based on the vehicle posture information.

[0012] Optionally, in a first implementation method of the second aspect of the present invention, the association module includes: a difference calculation unit, used to determine the second observation time of each first state node in the factor graph, and respectively calculate the time difference between each second observation time and the first observation time; a time difference selection unit, used to select at least one first state node from each first state node according to the time difference from small to large; an association unit, used to associate the sensor data with the selected first state node according to the time difference and the second observation time corresponding to the selected first state node, using a preset IMU integration algorithm, to obtain a new factor graph.

[0013] Optionally, in a second implementation method of the second aspect of the present invention, the new factor graph includes a first factor graph and a second factor graph, and the association unit is also used to: determine whether the time difference corresponding to the selected first state node is less than a preset time difference threshold; if it is less than the preset time difference threshold, use a preset IMU integration algorithm to synchronize the sensor data to the first state node with the smallest time difference, and associate the observation factor corresponding to the sensor data to the first state node with the smallest time difference to obtain a first factor graph; if it is not less than the preset time difference threshold, use a preset IMU integration algorithm to generate a second state node using the sensor data; according to the second observation time corresponding to the selected first state node, associate the second state node with the selected first state node, and associate the observation factor corresponding to the sensor data to the second state node to obtain a second factor graph.

[0014] Optionally, in a third implementation method of the second aspect of the present invention, the association unit is also used to: determine whether the second observation time corresponding to the selected first state node is less than a preset time threshold; if it is less than the preset time threshold, then associate the second state node with the subsequent first state node with the smallest time difference; if it is not less than the preset time threshold, then associate the second state node with the previous first state node and the subsequent first state node with the smallest time difference, respectively.

[0015] Optionally, in a fourth implementation of the second aspect of the present invention, the optimization module includes: a state difference selection unit, used to calculate the state difference between every two adjacent state nodes in the new factor graph, and select state nodes whose state difference is greater than a preset state change threshold from the new factor graph; an optimization unit, used to perform linearization processing using the state nodes selected from the new factor graph; arranged in observation time sequence, and using a reverse chain propagation method to update the state of the subsequent state nodes in the linearized factor graph using the previous state nodes; a loop unit, used to stop when the update amount of the state update is less than the preset state update threshold, and obtain each observation state information corresponding to the vehicle.

[0016] Optionally, in a fifth implementation method of the second aspect of the present invention, the positioning module is also used to: determine the posture information corresponding to the first observation state information at the observation time, and use the subsequent observation state information and the previous posture information to calculate the posture information corresponding to the subsequent observation state information at the observation time, until the posture information corresponding to the latest observation time is calculated, and stop to obtain the vehicle posture information.

[0017] Optionally, in a sixth implementation of the second aspect of the present invention, the vehicle positioning device also includes an elimination module, which is used to: use a preset time window to filter edge state nodes from the new factor graph and place them in a preset elimination sequence; execute the elimination operation of the elimination sequence to remove edge state nodes in the new factor graph.

[0018] A third aspect of the present invention provides a vehicle positioning device, comprising: a memory and at least one processor, wherein the memory stores instructions; the at least one processor calls the instructions in the memory to enable the vehicle positioning device to execute the above-mentioned vehicle positioning method.

[0019] A fourth aspect of the present invention provides a computer-readable storage medium, wherein the computer-readable storage medium stores instructions, which, when executed on a computer, enable the computer to execute the above-mentioned vehicle positioning method.

[0020] In the technical solution provided by the present invention, each time there is new sensor data, a new state node will be added to the appropriate position in the factor graph or directly synchronized to the original state node, wherein each state node can rely on the pre-integration of the inertial sensor to establish a connection edge and perform an association operation; since the factor graph can save any observation state of the vehicle, for sensor data with different delays, as long as the state node is established at its observation time or the closest state node is incorporated, the time synchronization and observation state alignment can be guaranteed. The state change condition is used to linearize the new factor graph and update the state increment to take into account the accuracy and computational efficiency of the observation state. At the same time, the improved iterative optimization solution method is used. Compared with the one-time solution of the filtering solution, the vehicle's current vehicle position information can be solved more accurately to improve the positioning accuracy of the vehicle. BRIEF DESCRIPTION OF THE DRAWINGS

[0021] Figure 1 A schematic diagram of a first embodiment of a vehicle positioning method in an embodiment of the present invention;

[0022] Figure 2 This is a schematic diagram of a first embodiment of a factor graph in an embodiment of the present invention;

[0023] Figure 3 Schematic diagram of a second embodiment of the vehicle positioning method in an embodiment of the present invention;

[0024] Figure 4 is a schematic diagram of a second embodiment of a factor graph in an embodiment of the present invention;

[0025] Figure 5 Schematic diagram of a third embodiment of a factor graph in an embodiment of the present invention;

[0026] Figure 6Schematic diagram of a fourth embodiment of a factor graph according to an embodiment of the present invention;

[0027] Figure 7 A schematic diagram of a first embodiment of a vehicle positioning device in an embodiment of the present invention;

[0028] Figure 8 A schematic diagram of a second embodiment of a vehicle positioning device in an embodiment of the present invention;

[0029] Fig. 9 FIG. 1 is a schematic diagram of an embodiment of a vehicle positioning device in an embodiment of the present invention. DETAILED DESCRIPTION

[0030] The embodiment of the present invention provides a vehicle positioning method, device, equipment and storage medium, which obtains real-time sensor data and factor graph of the vehicle, wherein the factor graph includes multiple first state nodes arranged in observation time sequence during driving; according to the first observation time of the sensor data, the sensor data and the first state node of the factor graph are associated to obtain a new factor graph; using the preset state change condition, the new factor graph is linearized and the state increment is updated to obtain each observation state information corresponding to the vehicle; according to each observation state information, the vehicle's posture is propagated and updated in turn to obtain the vehicle posture information, and the vehicle is positioned based on the vehicle posture information. The present invention realizes the time alignment of different types of sensor data, and improves the accuracy of vehicle positioning.

[0031] The terms "first", "second", "third", "fourth", etc. (if any) in the specification and claims of the present invention and the above-mentioned drawings are used to distinguish similar objects, and are not necessarily used to describe a specific order or sequence. It should be understood that the data used in this way can be interchanged where appropriate, so that the embodiments described herein can be implemented in an order other than that illustrated or described herein. In addition, the terms "including" or "having" and any variations thereof are intended to cover non-exclusive inclusions, for example, a process, method, system, product or device that includes a series of steps or units is not necessarily limited to those steps or units that are clearly listed, but may include other steps or units that are not clearly listed or inherent to these processes, methods, products or devices.

[0032] For ease of understanding, the specific process of the embodiment of the present invention is described below. Figure 1 , a first embodiment of the vehicle positioning method in the embodiment of the present invention includes:

[0033] 101. Acquire real-time sensor data and a factor graph of the vehicle, wherein the factor graph includes a plurality of first state nodes arranged in an observation time sequence during driving;

[0034] It is understandable that the execution subject of the present invention may be a vehicle positioning device, or a terminal or a server, which is not limited here. The embodiment of the present invention is described by taking a server as the execution subject as an example.

[0035] In this embodiment, during the vehicle driving process, sensor data of the driving scene collected by various sensors installed on the vehicle are acquired in real time, wherein the sensors installed on the vehicle include IMU (Inertial Measurement Unit) sensors, GPS (Global Positioning System) sensors, wheel speed meters and radar sensors, etc., and corresponding IMU sensor data, GPS sensor data, wheel speed sensor data and radar point cloud data, etc. are acquired.

[0036] In this embodiment, in order to time align the sensor data collected by different sensors due to delays, a factor graph is configured to record the collected sensor data and associate it with the state node with the observation time sequence. Here, the first state node refers to the factor graph maintained after the latest sensor data is recorded, and the state nodes contained in the latest factor graph.

[0037] In this embodiment, in addition to retaining the state node corresponding to the current latest observation, the factor graph also retains the state nodes corresponding to a series of observations at past moments. The state nodes at adjacent moments are connected through the pre-integration of the inertial sensor (IMU). Such an overall factor graph is used as the final optimization object, rather than only optimizing the current state like the traditional filtering scheme, which can effectively solve the time synchronization problem of different sensor data.

[0038] In one embodiment, Figure 2 The above is a real-time schematic diagram of the factor graph, where the observation factors {..., i, i+1, i+2, i+3, ...} are sorted according to the observation time, and each observation time is associated with the observation factors of adjacent observation times through the IMU pre-integration of factor E (IMU factor). Each state node is also associated with the sensor data collected at the corresponding observation time, and the state node is associated with the corresponding observation factor in the form of an observation factor. The observation factors corresponding to each different type of sensor data, such as factor A (GPS pos factor), factor B (GPS vel factor), factor C (Lidar factor), and factor D (wheel speed factor), the observation timing arrangement between the associated state nodes is independent of the observation factor type, and is arranged before and after the observation time. Among them, since factor A and factor B are obtained from two types of sensor data generated by the GPS sensor at the same time, factor A and factor B will be associated with the state node corresponding to the same observation time at the same time.

[0039] 102. Perform an association operation on the sensor data and a first state node of the factor graph according to a first observation time of the sensor data to obtain a new factor graph;

[0040] In this embodiment, whenever a sensor data is collected, it is necessary to time-align it with the sensor data collected in the past in the factor graph before the subsequent vehicle pose estimation is performed. The factor graph maintains each time node of the sensor data collected before that in the observation time sequence through the state node. The time alignment of the sensor data here specifically involves associating it with the first state node of the corresponding time node.

[0041] Specifically, the association method of the sensor data and the first state node includes: matching and synchronizing the sensor data to the first state node that already exists in the factor graph; creating a new state node for the sensor data, and then matching and inserting the new state node with the first state node. The latest collected sensor data is added to the previously collected sensor data in order of observation time, so that the sensor data is aligned according to the collection time.

[0042] Among them, if the sensor data is matched and synchronized to the existing first state node in the factor graph, the first state node in the new factor graph remains unchanged, and only the sensor data is added (the observation factor is increased); if a new state node is created for the sensor data, the number and order of the first state nodes in the new factor graph change.

[0043] In one embodiment, for example, if the factor graph includes first state nodes {S1, S2, ..., S c}, the observation time corresponding to the observation time sequence is {t1, t2, ..., t c}, at this time, the sensor data M of a certain sensor is received, and the measurement time corresponding to the sensor data is t m , due to the processing delay of this sensor, t m <t c , at this time the time difference between the two is a negative number, then if the state node S is directly c The state asynchrony caused by the time difference will merge the observation error into the subsequent positioning, affecting the positioning accuracy. m Establish a state node S at all times M , S M The inertial sensor (imu) is associated with the matching state node Si (establishing a connection edge). The association here includes both the state and the corresponding covariance information. After the association, the sensor data M is associated with the state node S M Fusion, get a new factor graph.

[0044] 103. Using the preset state change condition, linearize the new factor graph and update the state increment to obtain each observation state information corresponding to the vehicle;

[0045] In this embodiment, the observation state information of the vehicle at each observation time sequence is calculated through iterative "linearization-state incremental update", which can overcome the errors caused by nonlinear sensor data. At the same time, state change conditions are added to perform linearization and state incremental update of the new factor graph for the state change amplitude of every two adjacent state nodes, effectively improving the generation efficiency of the observation state information while ensuring the generation accuracy.

[0046] Specifically, the state change in the new factor graph is nonlinear, so it needs to be converted into a linear representation for state incremental update. The preset state change conditions include the following two parts: the state change threshold set during linearization processing, and the state update threshold set during state incremental update. When processing the linearization of the new factor graph, the observation factors used for linearization processing are screened by the state change threshold, and then the observation factors expressed linearly are propagated and updated by the state update threshold to obtain the various observation state information of the vehicle at different observation times.

[0047] Specifically, in the new factor graph, except for the state node where the newly added sensor data is located, the observation state information of other state nodes has been calculated in the previous pose estimation process (obtained based on IMU pre-integration of the sensor data contained in each state node). Therefore, the new factor graph is linearized and the state incrementally updated here. In fact, the existing observation state information of each state node is linearized and updated in combination with the newly added sensor data to obtain the new observation state information of the state nodes retained after processing.

[0048] 104. According to each of the observation status information, the position and posture of the vehicle are propagated and updated in sequence to obtain vehicle position and posture information, and the vehicle is positioned based on the vehicle position and posture information.

[0049] In this embodiment, after obtaining the real-time observation state information, the vehicle's posture estimation can be performed to locate the vehicle. The propagation update of the vehicle's posture uses the observation state information obtained by optimizing the new factor graph as the integral starting point, and the real-time posture information of the vehicle is calculated through IMU integration. The details are as follows:

[0050] Get the pose Pose_s0 after factor graph optimization; get the imu at time s1, and calculate the pose Pose_s1 at time s1 based on Pose_s0; get the imu at time s2, and calculate the pose Pose_s2 at time s2 based on Pose_s1; ...get s N imu at the moment, and in Pose_s n-1 Based on this, s is calculated n Pose_s at the moment n ; Finally, the latest Pose_s n As the real-time vehicle position information of the vehicle, including the vehicle's posture, speed, position, etc., to locate the vehicle.

[0051] In the embodiment of the present invention, each time there is new sensor data, a new state node will be added at an appropriate position in the factor graph or directly synchronized to the original state node, wherein each state node can rely on the pre-integration of the inertial sensor to establish a connection edge and perform an association operation; since the factor graph can save any observation state of the vehicle, for sensor data with different delays, as long as the state node is established at its observation time or the closest state node is incorporated, the time synchronization and observation state alignment can be guaranteed. The state change condition is used to linearize the new factor graph and update the state increment to take into account the accuracy and computational efficiency of the observation state. At the same time, the improved iterative optimization solution method is used. Compared with the one-time solution of the filtering scheme, the vehicle's current vehicle position information can be solved more accurately to improve the positioning accuracy of the vehicle.

[0052] See also Figure 3 , a second embodiment of the vehicle positioning method in the embodiment of the present invention includes:

[0053] 301. Acquire real-time sensor data and a factor graph of the vehicle, wherein the factor graph includes a plurality of first state nodes arranged in an observation time sequence during driving;

[0054] In this embodiment, during the vehicle positioning process, it can be divided into a front-end module and a back-end module. The front-end module pre-processes all sensor data and inputs them into the back-end module. After the back-end module receives the sensor data, it adds corresponding factors to the factor graph and associates corresponding state nodes according to different sensor data types and in combination with its first observation time, calculates the latest observation state information, and sends it to the front-end module synchronously. The front-end module uses IMU integral calculation to obtain the latest vehicle posture information. The starting point of IMU integration is given by the back-end module. Such a design not only ensures real-time performance, but also ensures that there is no loss of accuracy.

[0055] In this embodiment, after obtaining the real-time sensor data of the vehicle, it is necessary to perform data status check and coordinate conversion of different types of sensor data in the front-end module. The former is to ensure the accuracy of the data, so that the calculation efficiency and accuracy are higher when the vehicle posture estimation and positioning are performed later; the latter is to uniformly convert different types of sensor data into the same coordinate system, so as to facilitate the subsequent fusion and unified processing of different types of sensor data.

[0056] Specifically, when performing a data status check on sensor data, exclude data with the following abnormal conditions: consecutive identical data in IMU type sensor data, data with timestamp bounce in IMU type sensor data, data with too low a number of collected signal stars and poor accuracy in GPS type sensor data, inconsistent data collected from the left and right ends of the vehicle in wheel speed type sensor data, and data with failed matching in radar type sensor data.

[0057] Specifically, the coordinate conversion of different types of sensor data mainly includes the conversion of GPS type sensor data and radar type sensor data to the IMU coordinate system. The execution process is as follows:

[0058] When converting GPS type sensor data to the IMU coordinate system, if the offset from the antenna position to the IMU sensor position in the IMU coordinate system is t ant , the position represented by GPS type sensor data is P ant , then P ant Convert to the IMU coordinate system to get P 1 imu =P ant -R*t ant , where R1 is the rotation value from the IMU coordinate system to the ENU coordinate system.

[0059] When converting radar type sensor data to the IMU coordinate system, if the offset from the radar installation position to the IMU position in the IMU coordinate system is t lidar , the position represented by the radar type sensor data is P lidar , then P lidar Convert to the IMU coordinate system to get P 2 imu =P lidar -R2*t lidar , where R2 is the rotation value from the IMU coordinate system to the ENU coordinate system. If the rotation value from the IMU coordinate system to the radar coordinate system is R i-l , the rotation value from the radar coordinate system to the ENU coordinate system is R l-e , then the rotation value from the IMU coordinate system to the ENU coordinate system is R2 = R l-e *R i-l .

[0060] 302. Determine a second observation time of each first state node in the factor graph, and respectively calculate a time difference between each second observation time and the first observation time;

[0061] 303. Select at least one first-state node from the first-state nodes according to the time difference from small to large;

[0062] 304. According to the time difference corresponding to the selected first state node and the second observation time, the sensor data is associated with the selected first state node using a preset IMU integration algorithm to obtain a new factor graph;

[0063] In this embodiment, based on the observation time of the sensor data acquired in real time and the observation time sequence arrangement in the factor graph, the state node position of the factor graph where the sensor data is located is determined, so that when the sensor data is associated with the factor graph, after the sensor data is added to the new factor graph, the order of each state node and the included sensor data are still sorted from earliest to latest according to the observation time.

[0064] In this embodiment, according to the difference between the second observation time and the second observation time of each first state node, and the early or late first observation time, different methods are used to perform the association between the sensor data and the first state node to refresh the factor graph. The time difference size and the early or late first observation time, and the corresponding association strategy are as follows:

[0065] 10) Determine whether the time difference value corresponding to the selected first state node is less than a preset time difference threshold;

[0066] 20) If it is less than a preset time difference threshold, the preset IMU integration algorithm is used to synchronize the sensor data to the first state node with the smallest time difference, and the observation factor corresponding to the sensor data is associated with the first state node with the smallest time difference to obtain a first factor graph;

[0067] 30) If it is not less than the preset time difference threshold, using the preset IMU integration algorithm, the sensor data is used to generate a second state node;

[0068] 40) According to the second observation time corresponding to the selected first state node, the second state node is associated with the selected first state node, and the observation factor corresponding to the sensor data is associated with the second state node to obtain a second factor graph.

[0069] In this embodiment, according to the time difference between the first observation time of the sensor data and the second observation time of each state node, there are two types of association strategies that can be executed, one is not to build a new state node, and the other is to build a new state node. The former directly synchronizes the sensor data to the first state node with the smallest time difference and builds an association with the first state node; the latter builds a new state node between, before or after one or two state nodes with the smallest time difference, and synchronizes the sensor data to the state node.

[0070] Specifically, Figure 4 As shown in the figure, if the first state node (for example, the state node i+1) whose difference with the observation time is less than the time difference threshold (for example, 5ms) is found in the factor graph, it can be basically equivalent to synchronously collecting the sensor data at the second observation time of the state node (that is, the first observation time is equal to the second observation time). In this case, the sensor data is directly synchronized to the first observation node through IMU integration, without adding additional state nodes and associated connection edges, and then the corresponding observation factor (factor D) is associated with the state node. At this time, the factor D in the dotted part is the newly added content of the new factor graph.

[0071] Specifically, when the first state node whose observation time difference is less than the time difference threshold cannot be found in the factor graph, a new state node needs to be added to represent its first observation time. It can be divided into the following two cases according to the early or late second observation time:

[0072] 41) Determine whether the second observation time corresponding to the selected first state node is less than a preset time threshold;

[0073] 42) If it is less than a preset time threshold, the second state node is associated with the subsequent first state node with the smallest time difference;

[0074] 43) If it is not less than the preset time threshold, the second state node is associated with the previous first state node and the next first state node with the smallest time difference respectively.

[0075] like Figure 5 As shown in the figure, when the second observation time corresponding to the selected first state node (i+1 state node) is less than the time threshold (for example, within 150ms of the current time), it means that the observation time of the sensor data is relatively new, and the second state node (i+2 state node) is associated with the first state node (i+1 state node) behind through the IMU integration of factor E, and then the corresponding observation factor (factor C) is associated with the second state node (i+2 state node). Among them, a factor C, an i+2 state node and a factor E in the dotted box are the newly added contents in the new factor graph.

[0076] like Figure 6 As shown in the figure, when the second observation time corresponding to the selected first state node (i and i+1 state nodes) is greater than the time threshold (for example, before 150ms of the current time), it means that the observation time of the sensor data is relatively old. Then, through the IMU integration of two factors E, the second state node (i+2 state node) is associated with the previous state node (i state node) with the closest observation time and the next state node (i+1 state node). At the same time, the associated edge connecting the upper and lower nodes needs to be deleted, and then the corresponding observation factor (factor C) is associated with the second state node (i+2 state node). Among them, a factor C, an i+2 state node and two factors E in the dotted box are the newly added content in the new factor graph.

[0077] It should be noted that one observation time (including the first observation time and the second observation time) node corresponds to one state node, which are interconnected through IMU integration. One state node is associated with one or more observation factors and bound to the corresponding sensor data.

[0078] 305. Using a preset time window, filter edge state nodes from the new factor graph and place them in a preset elimination sequence;

[0079] 306. Execute an elimination operation of the elimination sequence to remove edge state nodes in the new factor graph;

[0080] In this embodiment, since the state nodes with earlier observation time have basically no contribution when performing subsequent vehicle posture estimation, the length of the time window is pre-set to determine the observation time span for each vehicle posture estimation, such as 300ms. For nodes outside the time window, they can be removed from the graph by marginalization, so that the scale of the factor graph is always maintained at a certain size, rather than growing infinitely, and the purpose of reducing computing resources is achieved while ensuring positioning accuracy. In order to ensure the marginalization efficiency of edge state nodes, the nodes to be marginalized are placed at the beginning of the elimination sequence. When performing the marginalization operation, only the edge state nodes in the elimination sequence need to be deleted, and no other additional operations are required.

[0081] 307. Calculate the state difference between every two adjacent state nodes in the new factor graph, and select a state node whose state difference is greater than a preset state change threshold from the new factor graph;

[0082] 308. Perform linearization processing using the state nodes selected in the new factor graph;

[0083] 309. Arrange according to the observation time sequence, and use the reverse chain propagation method to use the previous state node in the linearized factor graph to update the state of the subsequent state node;

[0084] 310. When the update amount of the state update is less than a preset state update threshold, stop and obtain each observation state information corresponding to the vehicle;

[0085] In this embodiment, for the linearization processing of the new factor graph, all states of the entire factor graph will not be re-linearized, but state nodes whose state changes (the state difference between two adjacent state nodes) exceed a certain threshold (the state change threshold is set in advance) will be selected, and then the selected state nodes will be re-linearized. This is because when processing the nonlinear state nodes of the factor graph, the two state nodes with little difference in state changes will obtain the local linearization results that are not much different from the previous ones, which has little effect on the accuracy of the final result. In order to reduce computing resources, they can be excluded from the linearization processing.

[0086] In one embodiment, when screening state nodes whose state difference is greater than a preset state change threshold, start from the state node with the latest observation time and use it as a reference node to calculate the state difference between the reference node and a previous adjacent state node. If the difference is not greater than the state change threshold, the previous adjacent state node is eliminated, and then the state difference with the next previous adjacent state node is calculated, and so on until the state difference is greater than the state change threshold, and the last previous adjacent state node is selected for linearization processing; then the selected state node is used as a new reference node, and the next state node for linearization processing is executed until all state nodes in the factor graph are screened.

[0087] Specifically, the state difference between every two adjacent state nodes may also be the difference between the linearized value of the previous state node and the original value of the subsequent state node, so as to implement the above-mentioned implementation method.

[0088] In this embodiment, in the process of performing incremental state updates on the linearized factor graph, it can be equivalent to a reverse chain propagation process, which gradually updates from the latest state node to the oldest state node in reverse chronological order, that is, first update the latest state node to obtain the observed state information of the latest state node at the corresponding observation time; then update the previous state node associated with the latest state node, and so on, until the entire factor graph is updated to obtain the observed state information corresponding to each state node. In order to improve efficiency, in the reverse chain propagation process, once it is found that the update amount of a certain state node is less than a certain threshold (pre-set state update threshold), the update is stopped immediately. Because when the update amount of the state node is less than the set threshold, it means that the state changes of subsequent state nodes are not large, and the update is stopped to reduce the update time.

[0089] 311. Determine the posture information corresponding to the first observation state information at the observation time, and use the subsequent observation state information and the previous posture information to calculate the posture information corresponding to the subsequent observation state information at the observation time, and stop when the posture information corresponding to the latest observation time is calculated, obtain the vehicle posture information, and locate the vehicle based on the vehicle posture information.

[0090] In this embodiment, after obtaining the pose information Pose_s0 corresponding to the first observation state information after factor graph optimization; the pose information Pose_s1 at the time s1 after is calculated according to the observation state information at the time s1 after and the pose information Pose_s0 before; then the pose Pose_s2 at the time s2 after is calculated according to the observation state information at the time s2 after and the pose information Pose_s1 before, and so on, until the latest observation time s is calculated. n Pose information Pose_s n , and Pose_s n The vehicle posture information of the vehicle at the first observation time may specifically include the posture, speed, position, etc. of the vehicle.

[0091] Specifically, when calculating the posture corresponding to the subsequent observation time based on the subsequent observation state information and the previous posture, if the previous posture is Then the posture corresponding to the post-observation time is:

[0092]

[0093] in, is a constant, is the rotation of the ECEF coordinate system relative to the ECI coordinate system, is the rotation of the ENU coordinate system relative to the ECEF coordinate system, (+) and (-) represent the positive and reverse rotation directions respectively. is the rotation from the previous moment to the next moment, which can be calculated from the angular velocity in the next observation state information:

[0094]

[0095] in, is the angular velocity in the subsequent observation state information.

[0096] Specifically, when calculating the speed corresponding to the subsequent observation time based on the subsequent observation state information and the previous speed, if the previous speed is Then the speed corresponding to the post-observation time is:

[0097]

[0098] in, is the acceleration due to gravity, is the acceleration in the subsequent observed state information.

[0099] Specifically, when calculating the position corresponding to the subsequent observation time based on the subsequent observation state information and the previous position, if the previous position includes the height h b (-), latitude L b (-) and longitude λ b (-), then the height corresponding to the later observation time is:

[0100]

[0101] The latitude corresponding to the post-observation time is:

[0102]

[0103] The corresponding longitude at the later observation time is:

[0104]

[0105] The above describes the vehicle positioning method in the embodiment of the present invention. The following describes the vehicle positioning device in the embodiment of the present invention. Figure 7 In one embodiment of the present invention, a vehicle positioning device includes:

[0106] An acquisition module 701 is used to acquire real-time sensor data and a factor graph of the vehicle, wherein the factor graph includes a plurality of first state nodes arranged in an observation time sequence during driving;

[0107] An association module 702 is used to perform an association operation on the sensor data and a first state node of the factor graph according to a first observation time of the sensor data to obtain a new factor graph;

[0108] The optimization module 703 is used to perform linearization processing and state increment update on the new factor graph by using the preset state change condition to obtain each observation state information corresponding to the vehicle;

[0109] The positioning module 704 is used to propagate and update the vehicle's posture in sequence according to each of the observation status information, obtain vehicle posture information, and locate the vehicle based on the vehicle posture information.

[0110] In the embodiment of the present invention, each time there is new sensor data, a new state node will be added at an appropriate position in the factor graph or directly synchronized to the original state node, wherein each state node can rely on the pre-integration of the inertial sensor to establish a connection edge and perform an association operation; since the factor graph can save any observation state of the vehicle, for sensor data with different delays, as long as the state node is established at its observation time or the closest state node is incorporated, the time synchronization and observation state alignment can be guaranteed. The state change condition is used to linearize the new factor graph and update the state increment to take into account the accuracy and computational efficiency of the observation state. At the same time, the improved iterative optimization solution method is used. Compared with the one-time solution of the filtering scheme, the vehicle's current vehicle position information can be solved more accurately to improve the positioning accuracy of the vehicle.

[0111] See also Figure 8 Another embodiment of the vehicle positioning device in the embodiment of the present invention includes:

[0112] An acquisition module 701 is used to acquire real-time sensor data and a factor graph of the vehicle, wherein the factor graph includes a plurality of first state nodes arranged in an observation time sequence during driving;

[0113] An association module 702 is used to perform an association operation on the sensor data and a first state node of the factor graph according to a first observation time of the sensor data to obtain a new factor graph;

[0114] The optimization module 703 is used to perform linearization processing and state increment update on the new factor graph by using the preset state change condition to obtain each observation state information corresponding to the vehicle;

[0115] The positioning module 704 is used to propagate and update the vehicle's posture in sequence according to each of the observation status information, obtain vehicle posture information, and locate the vehicle based on the vehicle posture information.

[0116] Specifically, the association module 702 includes:

[0117] A difference calculation unit 7021 is used to determine the second observation time of each first state node in the factor graph, and respectively calculate the time difference between each second observation time and the first observation time;

[0118] A time difference selection unit 7022, configured to select at least one first state node from each of the first state nodes according to the time difference value from small to large;

[0119] The association unit 7023 is used to associate the sensor data with the selected first state node according to the time difference corresponding to the selected first state node and the second observation time, using a preset IMU integration algorithm to obtain a new factor graph.

[0120] Specifically, the new factor graph includes a first factor graph and a second factor graph, and the association unit 7023 is further used for:

[0121] Determine whether the time difference value corresponding to the selected first state node is less than a preset time difference threshold;

[0122] If it is less than a preset time difference threshold, the preset IMU integration algorithm is used to synchronize the sensor data to the first state node with the smallest time difference, and the observation factor corresponding to the sensor data is associated with the first state node with the smallest time difference to obtain a first factor graph;

[0123] If it is not less than a preset time difference threshold, using a preset IMU integration algorithm, the sensor data is used to generate a second state node;

[0124] According to the second observation time corresponding to the selected first state node, the second state node is associated with the selected first state node, and the observation factor corresponding to the sensor data is associated with the second state node to obtain a second factor graph.

[0125] Specifically, the association unit 7023 is further used for:

[0126] Determine whether the second observation time corresponding to the selected first state node is less than a preset time threshold;

[0127] If it is less than a preset time threshold, the second state node is associated with the subsequent first state node with the smallest time difference;

[0128] If it is not less than the preset time threshold, the second state node is associated with the previous first state node and the subsequent first state node with the smallest time difference respectively.

[0129] Specifically, the optimization module 703 includes:

[0130] A state difference selection unit 7031 is used to calculate the state difference between every two adjacent state nodes in the new factor graph, and select a state node whose state difference is greater than a preset state change threshold from the new factor graph;

[0131] The optimization unit 7032 is used to perform linearization processing using the state nodes selected in the new factor graph; according to the observation time sequence, using the reverse chain propagation method, in the linearized factor graph, the state of the previous state node is used to update the state of the subsequent state node;

[0132] The loop unit 7033 is used to stop when the update amount of the state update is less than a preset state update threshold, and obtain each observation state information corresponding to the vehicle.

[0133] Specifically, the positioning module 704 is further used for:

[0134] Determine the posture information corresponding to the first observation state information at the observation time, and use the subsequent observation state information and the previous posture information to calculate the posture information corresponding to the subsequent observation state information at the observation time, until the posture information corresponding to the latest observation time is calculated and stopped to obtain the vehicle posture information.

[0135] Specifically, the vehicle positioning device further includes an elimination module 705, which is used to:

[0136] Using a preset time window, edge state nodes are selected from the new factor graph and placed in a preset elimination sequence;

[0137] An elimination operation of the elimination sequence is performed to remove edge state nodes in the new factor graph.

[0138] above Figure 7 and Figure 8 The vehicle positioning device in the embodiment of the present invention is described in detail from the perspective of modular functional entities. The vehicle positioning device in the embodiment of the present invention is described in detail from the perspective of hardware processing.

[0139] Fig. 91 is a schematic diagram of the structure of a vehicle positioning device provided by an embodiment of the present invention. The vehicle positioning device 900 may have relatively large differences due to different configurations or performances, and may include one or more processors (central processing units, CPU) 910 (for example, one or more processors) and a memory 920, and one or more storage media 930 (for example, one or more mass storage devices) storing application programs 933 or data 932. Among them, the memory 920 and the storage medium 930 can be short-term storage or permanent storage. The program stored in the storage medium 930 may include one or more modules (not shown in the figure), and each module may include a series of instruction operations in the vehicle positioning device 900. Furthermore, the processor 910 can be configured to communicate with the storage medium 930 to execute a series of instruction operations in the storage medium 930 on the vehicle positioning device 900.

[0140] The vehicle positioning device 900 may also include one or more power supplies 940, one or more wired or wireless network interfaces 950, one or more input and output interfaces 960, and / or one or more operating systems 931, such as Windows Serve, Mac OS X, Unix, Linux, FreeBSD, etc. It will be appreciated by those skilled in the art that Fig. 9 The structure of the vehicle positioning device shown does not constitute a limitation on the vehicle positioning device, and may include more or less components than shown in the figure, or combine certain components, or arrange the components differently.

[0141] The present invention also provides a vehicle positioning device, the computer device includes a memory and a processor, the memory stores computer-readable instructions, and when the computer-readable instructions are executed by the processor, the processor executes the steps of the vehicle positioning method in the above embodiments.

[0142] The present invention also provides a computer-readable storage medium, which may be a non-volatile computer-readable storage medium or a volatile computer-readable storage medium. Instructions are stored in the computer-readable storage medium. When the instructions are executed on a computer, the computer executes the steps of the vehicle positioning method.

[0143] Those skilled in the art can clearly understand that, for the convenience and brevity of description, the specific working processes of the systems, devices and units described above can refer to the corresponding processes in the aforementioned method embodiments and will not be repeated here.

[0144] If the integrated unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention is essentially or the part that contributes to the prior art or all or part of the technical solution can be embodied in the form of a software product. The computer software product is stored in a storage medium, including a number of instructions to enable a computer device (which can be a personal computer, a server, or a network device, etc.) to perform all or part of the steps of the method described in each embodiment of the present invention. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (ROM), random access memory (RAM), disk or optical disk and other media that can store program codes.

[0145] As described above, 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 thereof 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 vehicle positioning method, characterized in that: The vehicle positioning method comprises: Acquire real-time sensor data and a factor graph of the vehicle, wherein the factor graph includes a plurality of first state nodes arranged in an observation time sequence during driving; According to a first observation time of the sensor data, an association operation is performed on the sensor data and a first state node of the factor graph to obtain a new factor graph; Using the preset state change conditions, the new factor graph is linearized and the state increment is updated to obtain each observation state information corresponding to the vehicle; According to each of the observation state information, the position and posture of the vehicle are propagated and updated in sequence to obtain vehicle position and posture information, and the vehicle is positioned based on the vehicle position and posture information; The method of using a preset state change condition to linearize and incrementally update the new factor graph to obtain each observed state information corresponding to the vehicle includes: calculating the state difference between every two adjacent state nodes in the new factor graph, and selecting a state node whose state difference is greater than a preset state change threshold from the new factor graph; using the state nodes selected from the new factor graph to perform linearization processing; arranging them in observation time sequence, using a reverse chain propagation method, and using the previous state node in the linearized factor graph to perform state update on the subsequent state node; stopping when the update amount of the state update is less than the preset state update threshold, and obtaining each observed state information corresponding to the vehicle.

2. The vehicle positioning method according to claim 1, characterized in that: The performing an association operation on the sensor data and the first state node of the factor graph according to the first observation time of the sensor data to obtain a new factor graph comprises: Determine a second observation time of each first state node in the factor graph, and respectively calculate a time difference between each second observation time and the first observation time; According to the time difference from small to large, at least one first state node is selected from each of the first state nodes, and according to the time difference corresponding to the selected first state node and the second observation time, the preset IMU integration algorithm is used to associate the sensor data with the selected first state node to obtain a new factor graph.

3. The vehicle positioning method according to claim 2, characterized in that: The new factor graph includes a first factor graph and a second factor graph. The sensor data is associated with the selected first state node using a preset IMU integration algorithm according to the time difference corresponding to the selected first state node and the second observation time, and the new factor graph includes: Determine whether the time difference value corresponding to the selected first state node is less than a preset time difference threshold; If it is less than a preset time difference threshold, the preset IMU integration algorithm is used to synchronize the sensor data to the first state node with the smallest time difference, and the observation factor corresponding to the sensor data is associated with the first state node with the smallest time difference to obtain a first factor graph; If it is not less than a preset time difference threshold, using a preset IMU integration algorithm, the sensor data is used to generate a second state node; According to the second observation time corresponding to the selected first state node, the second state node is associated with the selected first state node, and the observation factor corresponding to the sensor data is associated with the second state node to obtain a second factor graph.

4. The vehicle positioning method according to claim 3, characterized in that: The associating the second state node with the selected first state node according to the second observation time corresponding to the selected first state node comprises: Determine whether the second observation time corresponding to the selected first state node is less than a preset time threshold; If it is less than a preset time threshold, the second state node is associated with the subsequent first state node with the smallest time difference; If it is not less than the preset time threshold, the second state node is associated with the previous first state node and the next first state node with the smallest time difference respectively.

5. The vehicle positioning method according to claim 1, characterized in that: The updating of the vehicle's posture in sequence according to each of the observation state information to obtain the vehicle's posture information includes: Determine the posture information corresponding to the first observation state information at the observation time, and use the subsequent observation state information and the previous posture information to calculate the posture information corresponding to the subsequent observation state information at the observation time, until the posture information corresponding to the latest observation time is calculated and stopped to obtain the vehicle posture information.

6. The vehicle positioning method according to any one of claims 1 to 5, characterized in that: After performing an association operation on the sensor data and the first state node of the factor graph according to the first observation time of the sensor data to obtain a new factor graph, the method further includes: Using a preset time window, edge state nodes are selected from the new factor graph and placed in a preset elimination sequence; An elimination operation of the elimination sequence is performed to remove edge state nodes in the new factor graph.

7. A vehicle positioning device, characterized in that: The vehicle positioning device comprises: An acquisition module, used to acquire real-time sensor data and a factor graph of the vehicle, wherein the factor graph includes a plurality of first state nodes arranged in an observation time sequence during driving; an association module, configured to perform an association operation on the sensor data and a first state node of the factor graph according to a first observation time of the sensor data to obtain a new factor graph; An optimization module, used to use a preset state change condition to perform linearization processing and state increment update on the new factor graph to obtain each observation state information corresponding to the vehicle; A positioning module, used to propagate and update the position and posture of the vehicle in sequence according to each of the observation state information, obtain vehicle position and posture information, and locate the vehicle based on the vehicle position and posture information; The optimization module includes: a state difference selection unit, which is used to calculate the state difference between every two adjacent state nodes in the new factor graph, and select state nodes whose state difference is greater than a preset state change threshold from the new factor graph; an optimization unit, which is used to use the state nodes selected from the new factor graph for linearization processing; according to the observation time sequence, using the reverse chain propagation method, in the linearized factor graph, the state of the previous state node is updated to the state of the subsequent state node; a circulation unit, which is used to stop when the update amount of the state update is less than the preset state update threshold, and obtain each observation state information corresponding to the vehicle.

8. A vehicle positioning device, characterized in that: The vehicle positioning device comprises: a memory and at least one processor, wherein instructions are stored in the memory; The at least one processor calls the instructions in the memory to enable the vehicle positioning device to execute the steps of the vehicle positioning method according to any one of claims 1-6.

9. A computer-readable storage medium having instructions stored thereon, characterized in that: When the instructions are executed by the processor, the steps of the vehicle positioning method as described in any one of claims 1-6 are implemented.

Images