A processing method for state estimation based on pose graph optimization
By constructing a pose graph and using the Huber Loss function to optimize the relationship between state nodes and observation nodes, the problem of divergence of calculation results in the filtering method is solved, and the efficiency and robustness of state estimation are improved.
Patent Information
- Application Number
- CN202210515968.2
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2022-05-12
- Publication Date
- 2025-09-09
- Estimated Expiration
- 2042-05-12
AI Technical Summary
The state estimation method based on filtering in the prior art easily leads to divergence of calculation results, which reduces the smoothness and robustness of the state estimation.
A pose graph optimization method is adopted. By constructing a pose graph and using the Huber Loss function to build error constraints, the relationship between state nodes and observation nodes is optimized. Combined with the odometry motion constraints, nonlinear least squares optimization is performed to estimate the state quantity.
The computational efficiency and smoothness of state estimation are improved, the robustness is enhanced, and the divergence of the computational results is avoided.
Smart Images
Figure CN114882063B_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the field of data processing technology, and in particular to a processing method for state estimation based on pose graph optimization. Background Art
[0002] When tracking targets, autonomous driving perception systems typically use filtering methods to estimate the state of the target's motion transformation, given known target pose observations. Common filtering methods include Kalman filtering, extended Kalman filtering, and unscented Kalman filtering. Long-term practice has shown that this filtering-based state estimation approach can lead to divergent calculation results. Once the results diverge, the smoothness and robustness of the state estimate curve are reduced. Summary of the Invention
[0003] The purpose of the present invention is to address the shortcomings of the existing technology and provide a method, electronic device, and computer-readable storage medium for state estimation based on pose graph optimization. The method uses pose graph optimization to estimate state quantities based on known observation quantities, and initializes the transformed pose graph using historical state quantities and observation quantities using a pose graph transformation method similar to a sliding window. This method improves computational efficiency while also enhancing the smoothness and robustness of the state estimate.
[0004] To achieve the above objectives, a first aspect of an embodiment of the present invention provides a method for state estimation based on pose graph optimization, the method comprising:
[0005] receiving a motion model type as a first model type;
[0006] Receive the first number k of pose observations from time tk to t-1 as the first observation m i Form the first observation sequence; 1≤i≤k;
[0007] Constructing a first pose graph according to the first model type and the first observation sequence;
[0008] Obtaining a first estimated state quantity sequence corresponding to the first observation quantity sequence by optimizing the first pose graph;
[0009] Receive the pose observation at time t as the second observation m';
[0010] Performing a pose graph transformation on the first pose graph according to the second observation value m' and the first estimated state quantity sequence to obtain a second pose graph;
[0011] A second estimated state quantity at time t is obtained by optimizing the second pose graph.
[0012] Preferably, constructing the first pose graph according to the first model type and the first observation sequence specifically includes:
[0013] Construct the k first state nodes Ns of the first pose graph i ; and the first state node Ns i The corresponding state quantity is taken as the corresponding first estimated state quantity s i ; and each of the first state nodes Ns i Connect them sequentially to form a node chain of odometer motion relationship, and connect the node chain from the first state node Ns i To the first state node Ns i+1 The node edge is the corresponding first state node edge Ls i,i+1 ;
[0014] And construct the first state node edge Ls according to the first model type i,i+1 The odometer motion constraint relationship is s i+1 =A i|i+1 ·s i +w i Among them, A i|i+1 For the node Ns from the first state i To the first state node Ns i+1 The first state transition matrix, w i The first state node Ns i The first state noise matrix; the first state transition matrix A i|i+1 is the odometer motion state conversion matrix, whose structure corresponds to the first model type;
[0015] Construct the k first observation nodes Nm of the first pose graph i ; and each of the first observation nodes Nm i As the corresponding first state node Ns i The error constraint node; and the first observation node Nm i The corresponding observation value is initialized to the first observation value m corresponding to the first observation value sequence i ; and from the first observation node Nm i To the first state node Ns i The node edge is the corresponding first error node edge Lm-s i ;
[0016] And construct the first error node edge Lm-s i The error constraint relationship is Where f(·) is the loss function.
[0017] Furthermore, the loss function f(·) is specifically constructed based on the Huber Loss function.
[0018] Preferably, obtaining a first estimated state quantity sequence corresponding to the first observation quantity sequence by optimizing the first pose graph specifically includes:
[0019] With the first state node edge Ls i,i+1 The odometry motion constraint relationship and the first error node edge Lm-s i The error constraint relationship is used as the corresponding first state node Ns i Two constraints, constructing a first optimization objective function of the nonlinear least squares problem of the first pose graph;
[0020] Each of the first observation nodes Nm i The corresponding observation value of is substituted into the first optimization objective function as a known quantity, and the first state nodes Ns that make the first optimization objective function reach the optimal value are calculated. i The corresponding state quantity is estimated, and the estimated first state nodes Ns are obtained. i The corresponding state quantity is taken as the corresponding first estimated state quantity s i ; and all the first estimated state quantities s obtained i The corresponding first estimated state quantity sequence is formed and saved.
[0021] Preferably, performing a pose graph transformation on the first pose graph according to the second observation quantity m' and the first estimated state quantity sequence to obtain a second pose graph specifically includes:
[0022] From the first pose graph, delete the first first state node Ns i And the corresponding first observation node Nm i , the first state node edge Ls i,i+1 and the first error node edge Lm-s i ;
[0023] In the first pose graph after completing node deletion, the original 2nd to kth first state nodes Ns i As the new 1st to k-1th second state nodes Ns' i , and the original 2nd to kth first observation nodes Nm i As the new 1st to k-1th second observation nodes Nm' i , and the original 2nd to k-1 first state node edge Ls i,i+1 As the new 1st to k-2 second state node edge Ls' i,i+1, and the original 2nd to kth first error node edge Lm-s i As the new 1st to k-1th second error node edge Lm-s' i ;
[0024] In the first pose graph where the node transformation is completed, at the k-1 second state node Ns' i＝k-1 Then add the kth second state node Ns' i＝k ; and construct a second state node Ns' based on the odometer motion constraint relationship i＝k-1 to Ns' i＝k The state node edge is used as the k-1 second state node edge Ls' i,i+1 ;
[0025] In the first pose graph newly added in the completion state node, it is the kth second state node Ns' i＝k Add the corresponding observation node as the kth second observation node Nm' i＝k ; and construct the second observation node Nm' based on the error constraint relationship i＝k To the second state node Ns' i＝k The node edge is the kth second error node edge Lm-s' i ;
[0026] The first pose graph newly added by the completed observation node is used as the corresponding second pose graph;
[0027] And in the second pose graph, the second state node Ns' i＝1 The corresponding state quantity is initialized to the first estimated state quantity s corresponding to the first estimated state quantity sequence i＝2 , and so on, until the second state node Ns' i＝k-1 The corresponding state quantity is initialized to the corresponding first estimated state quantity s in the first estimated state quantity sequence i＝k and the second observation node Nm' i＝1 The corresponding observation quantity is initialized to the first observation node Nm i＝2 The corresponding observation quantity, and so on, until the second observation node Nm' i＝k-1 The corresponding observation quantity is initialized to the first observation node Nm i＝k The corresponding observation value is set as follows; and the second observation node Nm' i＝k The corresponding observation quantity is initialized as the second observation quantity m'.
[0028] Preferably, obtaining the second estimated state quantity at time t by optimizing the second pose graph specifically includes:
[0029] In the second state node edge Ls'i,i+1 The odometry motion constraint relationship and the second error node edge Lm-s' i The error constraint relationship is used as the corresponding second state node Ns' i Two constraints, constructing a second optimization objective function of the nonlinear least squares problem of the second pose graph;
[0030] Each of the second observation nodes Nm' i The corresponding observation value and the second state node Ns' i＝1 to Ns' i＝k-1 The corresponding state quantity is substituted into the second optimization objective function as a known quantity, and the second state node Ns' that makes the second optimization objective function reach the optimal value is obtained. i＝k The corresponding state quantity is estimated, and the second state node Ns' is obtained by estimation i＝k The corresponding state quantity is used as the second estimated state quantity at time t.
[0031] A second aspect of an embodiment of the present invention provides an electronic device, including: a memory, a processor, and a transceiver;
[0032] The processor is configured to be coupled to the memory, read and execute instructions in the memory, so as to implement the method steps described in the first aspect above;
[0033] The transceiver is coupled to the processor, and the processor controls the transceiver to send and receive messages.
[0034] A third aspect of an embodiment of the present invention provides a computer-readable storage medium, which stores computer instructions. When the computer instructions are executed by a computer, the computer executes the instructions of the method described in the first aspect.
[0035] Embodiments of the present invention provide a state estimation processing method, electronic device, and computer-readable storage medium based on pose graph optimization. Using pose graph optimization, state variables are estimated based on known observation variables. The transformed pose graph is initialized using historical state variables and observation variables using a pose graph transformation similar to a sliding window. This method improves computational efficiency while also enhancing the smoothness and robustness of the state estimate. BRIEF DESCRIPTION OF THE DRAWINGS
[0036] Figure 1 A schematic diagram of a processing method for state estimation based on pose graph optimization provided in Example 1 of the present invention;
[0037] Figure 2a A schematic diagram of the first pose diagram provided in the first embodiment of the present invention;
[0038] Figure 2b A schematic diagram of the conversion from the first pose graph to the second pose graph provided in the first embodiment of the present invention;
[0039] Figure 3 This is a structural diagram of an electronic device provided in Embodiment 2 of the present invention. DETAILED DESCRIPTION
[0040] To make the objectives, technical solutions, and advantages of the present invention more apparent, the present invention will be further described in detail below with reference to the accompanying drawings. It should be understood that the embodiments described herein are merely some, rather than all, of the present invention. All other embodiments derived by persons of ordinary skill in the art based on the embodiments of the present invention without inventive effort are intended to fall within the scope of protection of the present invention.
[0041] The first embodiment of the present invention provides a method for state estimation based on pose graph optimization, such as Figure 1 A schematic diagram of a method for state estimation based on pose graph optimization provided in the first embodiment of the present invention is shown. The method mainly includes the following steps:
[0042] Step 1, receiving a motion model type as a first model type;
[0043] Among them, the first model type includes at least a constant velocity (CV) motion model, a constant acceleration (CA) motion model, a constant turn rate and velocity (CTRV) motion model, a constant turn rate and acceleration (CTRA) motion model, a constant steering angle and velocity (CSAV) motion model, and a constant curvature and acceleration (CCA) motion model.
[0044] Here, the first model type is a preset motion model type for the tracking target, which determines the vector structure of the observation quantity and state quantity corresponding to the target, as well as the motion state conversion relationship between the state quantities.
[0045] Step 2: Receive the first number k of pose observations from time tk to t-1 as the first observation m i Form the first observation sequence;
[0046] Among them, 1≤i≤k.
[0047] Here, if the tracking target is the ego vehicle, the pose observation is the actual pose vector of the ego vehicle at the specified time. i The vector structure corresponds to the first model type:
[0048] If the first model type is a CV motion model, then the first observation m i Specifically (x,y,v x ,v y ) T , where x and y are the corresponding horizontal and vertical coordinates, v x 、v y are the corresponding lateral and longitudinal velocity components;
[0049] If the first model type is a CA motion model, then the first observation m i Specifically (x,y,v x ,v y ,a x ,a y ) T , where x and y are the corresponding horizontal and vertical coordinates, v x 、v y are the corresponding lateral and longitudinal velocity components, a x 、a y are the corresponding lateral and longitudinal acceleration components;
[0050] If the first model type is the CTRV motion model, then the first observation m i Specifically (x,y,v x ,v y ,ω) T , where x and y are the corresponding horizontal and vertical coordinates, v x 、v y are the corresponding lateral and longitudinal velocity components, ω is the turning rate corresponding to the yaw angle;
[0051] If the first model type is a CTRA motion model, then the first observation m i Specifically (x,y,v x ,v y ,a x ,a y ,ω) T , where x and y are the corresponding horizontal and vertical coordinates, v x 、v y are the corresponding lateral and longitudinal velocity components, a x 、a y are the corresponding lateral and longitudinal acceleration components, ω is the turning rate;
[0052] If the first model type is a CSAV motion model, then the first observation mi Specifically (x,y,v x ,v y ,δ) T , where x and y are the corresponding horizontal and vertical coordinates, v x 、v y are the corresponding lateral and longitudinal velocity components, a x 、a y are the corresponding lateral and longitudinal acceleration components, and δ is the steering angle;
[0053] If the first model type is a CCA motion model, then the first observation m i Specifically (x,y,v x ,v y ,a x ,a y ,c) T , where x and y are the corresponding horizontal and vertical coordinates, v x 、v y are the corresponding lateral and longitudinal velocity components, a x 、a y are the corresponding lateral and longitudinal acceleration components, r is the road curvature.
[0054] It should be noted that the first model type may also be a combined motion model of two groups of models among the above-mentioned multiple models. If the first model type is a CTRV+CV model, it means a combined motion model of the CTRV and CV models.
[0055] Step 3, constructing a first pose graph according to the first model type and the first observation sequence;
[0056] Specifically including: Step 31, constructing k first state nodes Ns of the first pose graph i ; and the first state node Ns i The corresponding state quantity is taken as the corresponding first estimated state quantity s i ; and each first state node Ns i Connect them sequentially to form the odometer motion relationship node chain, and connect the node chain from the first state node Ns i To the first state node Ns i+1 The node edge is the corresponding first state node edge Ls i,i+1 ;
[0057] Here, the first estimated state quantity s i The vector structure of is associated with the first model type;
[0058] Step 32, and construct the first state node edge Ls according to the first model type i,i+1 The odometer motion constraint relationship is s i+1=A i|i+1 ·s i +w i Among them, A i|i+1 From the first state node Ns i To the first state node Ns i+1 The first state transition matrix, w i The first state node Ns i The first state noise matrix; the first state transition matrix A i|i+1 is the odometer motion state conversion matrix, whose structure corresponds to the first model type;
[0059] Here, the first state transition matrix A in the odometry motion constraint relationship is i|i+1 The matrix structure of is related to the first model type. If the first model type is CV, CA, CTRV, CTRA, CSAV or CCA motion model, the first state transition matrix A i|i+1 is the corresponding CV, CA, CTRV, CTRA, CSAV or CCA motion state transition matrix. The specific state transition matrix can refer to the specific implementation related to the CV, CA, CTRV, CTRA, CSAV or CCA motion model, which will not be further described here. It should be noted that if the first model type is a combined motion model composed of two motion models, the first state transition matrix A i|i+1 It can be decomposed into two corresponding motion state conversion matrices;
[0060] For example, if the first model type is CTRV+CV model, then the first state transition matrix A i|i+1 =CTRV motion state transition matrix B i|i+1 +CV motion state conversion matrix B' i|i+1 ;
[0061] Step 33: Construct the k first observation nodes Nm of the first pose graph i ; and each first observation node Nm i As the corresponding first state node Ns i The error constraint node; and the first observation node Nm i The corresponding observation is initialized to the first observation m in the first observation sequence i ; and from the first observation node Nm i To the first state node Ns i The node edge is the corresponding first error node edge Lm-s i ;
[0062] Step 34, and construct the first error node edge Lm-s i The error constraint relationship is Among them, f(·) is the loss function, which is specifically constructed based on the Huber Loss function.
[0063] Here, the well-known Huber Loss function is as follows:
[0064] Wherein, δ is the Huber parameter. When the absolute value of a is less than the Huber parameter δ, the Huber Loss function is specifically a square error function. When the absolute value of a is not less than the Huber parameter δ, the Huber Loss function is specifically a linear error function.
[0065] Then the loss function f(·) constructed based on the Huber Loss function in the embodiment of the present invention is:
[0066]
[0067] Among them, δ * is the pre-set Huber parameter.
[0068] Here, the error constraint relationship is also related to the first model type. If the first model type is a separate CV, CA, CTRV, CTRA, CSAV or CCA motion model, the error constraint relationship err i It only contains the loss function f(·) under one motion model. If the first model type is a combined motion model composed of two motion models, the error constraint relationship err i There are two corresponding loss functions f(·) under the motion model. For example, if the first model type is CTRV+CV model, then err i =f CTRV (·)+f CV (·).
[0069] In order to have a clear understanding of the structure of the first pose graph, the following takes k = 3 and the first observation sequence includes 3 first observations m1, m2, and m3 as an example. Figure 2a The schematic diagram of the first pose graph provided in the first embodiment of the present invention illustrates the first pose graph obtained by the above steps 31-34;
[0070] like Figure 2a As shown, the first pose graph includes three first-state nodes Ns1, Ns2, and Ns3. The odometer motion relationship node chain is formed by sequentially connecting the first-state nodes Ns1, Ns2, and Ns3. The node edge connecting Ns1 and Ns2 is the first-state node edge Ls 1,2 , the node edge connecting Ns2 and Ns3 is the first state node edge Ls 2,3 ; Correspondingly, the first state node edge Ls 1,2 The odometer motion constraint relationship is s2=A1|2 s1+w1, first state node edge Ls 2,3 The odometer motion constraint relationship is s3=A 2|3 s2+w2;
[0071] like Figure 2a As shown, the first pose graph also includes three first observation nodes Nm1, Nm2 and Nm3, and the node edge connecting Nm1 and Ns1 is the first error node edge Lm-s1, the node edge connecting Nm2 and Ns2 is the first error node edge Lm-s2, and the node edge connecting Nm3 and Ns3 is the first error node edge Lm-s3; correspondingly, the corresponding observation quantities of the first observation nodes Nm1, Nm2 and Nm3 are the first observation quantities m1, m2 and m3 respectively; the error constraint relationship of the first error node edge Lm-s1 is The error constraint relationship of the first error node edge Lm-s2 is The error constraint relationship of the first error node edge Lm-s3 is
[0072] Step 4, obtaining a first estimated state quantity sequence corresponding to the first observation quantity sequence by optimizing the first pose graph;
[0073] Specifically including: Step 41, taking the first state node edge Ls i,i+1 Odometer motion constraint relationship and the first error node edge Lm-s i The error constraint relationship is used as the corresponding first state node Ns i The first optimization objective function of the first pose graph nonlinear least squares problem is constructed under the two constraints of
[0074] Step 42: each first observation node Nm i The corresponding observation quantity of is substituted into the first optimization objective function as a known quantity, and the first state nodes Ns that make the first optimization objective function reach the optimal value are calculated. i The corresponding state quantity is estimated, and the estimated first state nodes Ns are obtained. i The corresponding state quantity is taken as the corresponding first estimated state quantity s i ; and all the first estimated state quantities s obtained i The corresponding first estimated state quantity sequence is formed and saved.
[0075] Here, there are many ways to construct the first pose graph nonlinear least squares problem in the embodiment of the present invention. One of them is to perform modeling and objective function construction through maximum likelihood estimation. For details, please refer to the relevant technical implementation, which will not be described in detail here. Correspondingly, the solution to the least squares problem can also be referred to the relevant technical implementation, which will not be described in detail here. The first estimated state quantities s in the final first estimated state quantity sequence arei That is, each first observation m in the first observation sequence i Corresponding state estimation. Because the first observation sequence is the pose observation from time tk to t-1, the first estimated state sequence is also the k state estimations from time tk to t-1. The state estimation curve of the tracking target from time tk to t-1 can be obtained from the first estimated state sequence.
[0076] Step 5: Receive the pose observation at time t as the second observation m'.
[0077] Step 6, performing a pose graph transformation on the first pose graph according to the second observation m' and the first estimated state sequence to obtain a second pose graph;
[0078] Here, the current step actually uses a sliding window pose graph transformation method similar to a step size of 1 to process the conversion from the first pose graph to the second pose graph, that is, the first pose graph is slid backward by a set of state nodes + observation nodes with a step size of 1, and a new set of observation nodes + state nodes corresponding to the newly added observations are added to the last position of the first pose graph after sliding to obtain the second pose graph;
[0079] Specifically including: Step 61, deleting the first first state node Ns from the first pose graph i And the corresponding first observation node Nm i , the first first state node edge Ls i,i+1 and the first error node edge Lm-s i ;
[0080] Here, slide the first pose graph backward by a group of state nodes + observation nodes;
[0081] by Figure 2b Taking the conversion diagram from the first pose graph to the second pose graph provided in the first embodiment of the present invention as an example, the first pose graph is slid backward by a group of state nodes + observation nodes, that is, the first state node Ns1, the first observation node Nm1, the first state node edge Ls in the first pose graph 1,2 and the first error node edge Lm-s1 is deleted; a set of deleted state nodes + observation nodes and their corresponding deleted node edges are deleted, such as Figure 2b The nodes and node edges in the deletion section are shown as follows;
[0082] Step 62: In the first pose graph where node deletion is completed, the original 2nd to kth first state nodes Ns i As the new 1st to k-1th second state nodes Ns' i , and the original 2nd to kth first observation nodes Nm i As the new 1st to k-1th second observation nodes Nm'i , and the original 2nd to k-1 first state node edge Ls i,i+1 As the new 1st to k-2 second state node edge Ls' i,i+1 , and the original 2nd to kth first error node edge Lm-s i As the new 1st to k-1th second error node edge Lm-s' i ;
[0083] Here, after sliding the first pose graph backward by a group of state nodes + observation nodes, the index of the remaining nodes needs to be moved forward by 1;
[0084] by Figure 2b For example, after sliding a group of state nodes + observation nodes backward in the first pose graph, the indexes of the remaining nodes need to be moved forward by 1, that is, the footers of the first state nodes Ns2 and Ns3 in the original first pose graph are moved forward by 1 as the new second state nodes Ns'1 and Ns'2, and the footers of the first observation nodes Nm2 and Nm3 in the original first pose graph are moved forward by 1 as the new second observation nodes Nm'1 and Nm'2, and the edge Ls of the first state node in the original first pose graph is moved forward by 1. 2,3 The subscript of the edge Ls' is moved forward by 1 as the new second state node 1,2 , move the indexes of the first error node edges Lm-s2 and Lm-s3 in the original first pose graph forward by 1 position to form the new second error node edges Lm-s'1 and Lm-s'2;
[0085] Step 63: In the first pose graph after completing the node transformation, at the k-1 second state node Ns' i＝k-1 Then add the kth second state node Ns' i＝k ; and construct the second state node Ns' based on the odometer motion constraint relationship i＝k-1 to Ns' i＝k The state node edge is used as the k-1 second state node edge Ls' i,i+1 ;
[0086] Here, after moving the index of the remaining nodes in the first pose graph forward by 1, a new state node needs to be added at the last position of the first pose graph;
[0087] by Figure 2b For example, after moving the index of the remaining nodes in the first pose graph forward by 1, a new state node needs to be added to the last position of the first pose graph, that is, a new second state node Ns'3 is added after the first state node Ns3 (new second state node Ns'2) of the first pose graph, and a new second state node edge Ls' is constructed from the first state node Ns3 (new second state node Ns'2) to the second state node Ns'3. 2,3;
[0088] Step 64: In the first pose graph newly added to the completed state node, the kth second state node Ns' i＝k Add the corresponding observation node as the kth second observation node Nm' i＝k ; and construct the second observation node Nm' based on the error constraint relationship i＝k To the second state node Ns' i＝k The node edge is used as the kth second error node edge Lm-s' i ;
[0089] Here, after a new state node is added to the last position of the first pose graph, a new observation node needs to be added to correspond to the newly added state node;
[0090] by Figure 2b For example, after adding the second state node Ns'3 to the last position of the first pose graph, a new observation node needs to be added to correspond to the second state node Ns'3, that is, a new second observation node Nm'3 is added to the first pose graph to correspond to the second state node Ns'3, and a new second error node edge Lm-s'3 is constructed from the second observation node Nm'3 to the second state node Ns'3; a new set of state nodes + observation nodes and their corresponding new node edges are formed, such as Figure 2b As shown in the newly added part of nodes and node edges;
[0091] Step 65: The newly added first pose graph of the observation node is used as the corresponding second pose graph; and in the second pose graph, the second state node Ns' i＝1 The corresponding state quantity is initialized to the first estimated state quantity s in the first estimated state quantity sequence i＝2 , and so on, until the second state node Ns' i＝k-1 The corresponding state quantity is initialized to the corresponding first estimated state quantity s in the first estimated state quantity sequence i＝k and the second observation node Nm' i＝1 The corresponding observation quantity is initialized to the first observation node Nm i＝2 The corresponding observation quantity of the second observation node Nm' i＝k-1 The corresponding observation quantity is initialized to the first observation node Nm i＝k The corresponding observation value is set as follows; and the second observation node Nm' i＝k The corresponding observation is initialized as the second observation m'.
[0092] by Figure 2b For example, the second pose graph consists of three second state nodes Ns' i＝1 、Ns' i＝2 、Ns' i＝3, 3 second observation nodes Nm'1, Nm'2, Nm'3, 2 second state node edges Ls' 1,3 、Ls' 2,3 , the pose graph composed of three second error node edges Lm-s'1, Lm-s'2, and Lm-s'3;
[0093] Among them, the second state node Ns' i＝1 、Ns' i＝2 Corresponding to the first state nodes Ns2 and Ns3 in the first pose graph, the first estimated state quantities s2 and s3 corresponding to the first state nodes Ns2 and Ns3 are used to estimate the second state node Ns' i＝1 、Ns' i＝2 Initialize the corresponding state quantity;
[0094] The second observation nodes Nm'1 and Nm'2 correspond to the first observation nodes Nm2 and Nm3 in the first pose graph, so the corresponding observation quantities corresponding to the first observation nodes Nm2 and Nm3, that is, the first observation quantities m2 and m3, are used to initialize the corresponding observation quantities of the second observation nodes Nm'1 and Nm'2;
[0095] The corresponding observation value of the newly added second observation node Nm'3 is initialized to the second observation value m'.
[0096] Step 7, obtaining a second estimated state at time t by optimizing the second pose graph;
[0097] Specifically including: Step 71, with the second state node edge Ls' i,i+1 The odometry motion constraint relationship and the second error node edge Lm-s' i The error constraint relationship is used as the corresponding second state node Ns' i The two constraints are used to construct the second optimization objective function of the nonlinear least squares problem of the second pose graph;
[0098] Step 72: each second observation node Nm' i The corresponding observation quantity and the second state node Ns' i＝1 to Ns' i＝k-1 The corresponding state quantity is substituted into the second optimization objective function as a known quantity, and the second state node Ns' that makes the second optimization objective function reach the optimal value is calculated. i＝k The corresponding state quantity is estimated, and the second state node Ns' is obtained by estimation i＝k The corresponding state quantity is taken as the second estimated state quantity at time t.
[0099] Here, there are many ways to construct the nonlinear least squares problem of the second pose graph in the embodiment of the present invention. One of them is to perform modeling and construct the objective function through maximum likelihood estimation. For details, please refer to the relevant technical implementation, which will not be described here one by one; correspondingly, the solution to the least squares problem can also be referred to the relevant technical implementation, which will not be further described here. It should be noted that, because in the second pose graph, except for the last state node, the remaining state nodes are initialized using the estimated amount of the repeated node in the first pose graph, so in the actual solution, only the last state node needs to be estimated, thereby achieving the purpose of improving computational efficiency. And precisely because the optimization of the second pose graph is based on most of the optimization results of the first pose graph, the estimated second state node Ns' i＝k The corresponding state quantity can also have good smoothness and robustness.
[0100] Figure 3 This is a schematic diagram of the structure of an electronic device provided in the second embodiment of the present invention. The electronic device may be the aforementioned terminal device or server, or may be a terminal device or server connected to the aforementioned terminal device or server to implement the method of the embodiment of the present invention. Figure 3 As shown, the electronic device may include: a processor 301 (such as a CPU), a memory 302, and a transceiver 303; the transceiver 303 is coupled to the processor 301, and the processor 301 controls the transceiver 303's transceiver actions. Various instructions may be stored in the memory 302 for completing various processing functions and implementing the processing steps described in the aforementioned method embodiment. Preferably, the electronic device involved in the embodiment of the present invention further includes: a power supply 304, a system bus 305, and a communication port 306. The system bus 305 is used to realize communication connections between components. The above-mentioned communication port 306 is used for connection and communication between the electronic device and other peripherals.
[0101] exist Figure 3The system bus 305 mentioned in the figure can be a Peripheral Component Interconnect (PCI) bus or an Extended Industry Standard Architecture (EISA) bus, etc. The system bus can be divided into an address bus, a data bus, a control bus, etc. For ease of representation, only one thick line is used in the figure, but it does not mean that there is only one bus or one type of bus. The communication interface is used to realize communication between the database access device and other devices (such as clients, read-write libraries, and read-only libraries). The memory may include random access memory (RAM) and may also include non-volatile memory (Non-Volatile Memory), such as at least one disk storage.
[0102] The above-mentioned processor can be a general-purpose processor, including a central processing unit (CPU), a network processor (NP), a graphics processing unit (GPU), etc.; it can also be a digital signal processor (DSP), an application specific integrated circuit (ASIC), a field programmable gate array (FPGA) or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components.
[0103] It should be noted that an embodiment of the present invention further provides a computer-readable storage medium, which stores instructions. When the computer-readable storage medium is run on a computer, it enables the computer to execute the methods and processing procedures provided in the above embodiments.
[0104] An embodiment of the present invention further provides a chip for executing instructions, which is used to execute the processing steps described in the above method embodiment.
[0105] Embodiments of the present invention provide a state estimation processing method, electronic device, and computer-readable storage medium based on pose graph optimization. Using pose graph optimization, state variables are estimated based on known observation variables. The transformed pose graph is initialized using historical state variables and observation variables using a pose graph transformation similar to a sliding window. This method improves computational efficiency while also enhancing the smoothness and robustness of the state estimate.
[0106] Professionals should also be further aware 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 above description has generally described the components and steps of each example according to their functions. Whether these functions are performed in hardware or software depends on the specific application and design constraints of the technical solution. Professionals and technicians 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.
[0107] The steps of the methods or algorithms described in conjunction with the embodiments disclosed herein may be implemented using hardware, a software module executed by a processor, or a combination of the two. The software module may be placed in a random access memory (RAM), a memory, a read-only memory (ROM), an electrically programmable ROM, an electrically erasable programmable ROM, a register, a hard disk, a removable disk, a CD-ROM, or any other form of storage medium known in the art.
[0108] The specific implementation methods described above further illustrate the objectives, technical solutions and beneficial effects of the present invention in detail. It should be understood that the above description is only a specific implementation method of the present invention and is not intended to limit the scope of protection of the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the scope of protection of the present invention.
Claims
1. A method for state estimation based on pose graph optimization, characterized in that: The method comprises: receiving a motion model type as a first model type; Receive the first number k of pose observations from time tk to t-1 as the first observation m i Form the first observation sequence; 1≤i≤k; Constructing a first pose graph according to the first model type and the first observation sequence; Obtaining a first estimated state quantity sequence corresponding to the first observation quantity sequence by optimizing the first pose graph; Receive the pose observation at time t as the second observation m'; Performing a pose graph transformation on the first pose graph according to the second observation value m' and the first estimated state quantity sequence to obtain a second pose graph; Obtaining a second estimated state quantity at time t by optimizing the second pose graph; The step of constructing a first pose graph according to the first model type and the first observation sequence specifically includes: Construct the k first state nodes Ns of the first pose graph i ; and the first state node Ns i The corresponding state quantity is taken as the corresponding first estimated state quantity s i ; and each of the first state nodes Ns i Connect them sequentially to form a node chain of odometer motion relationship, and connect the node chain from the first state node Ns i To the first state node Ns i+1 The node edge is the corresponding first state node edge Ls i,i+1 ; And construct the first state node edge Ls according to the first model type i,i+1 The odometer motion constraint relationship is s i+1 =A i|i+1 ·s i +w i Among them, A i|i+1 For the node Ns from the first state i To the first state node Ns i+1 The first state transition matrix, w i The first state node Ns i The first state noise matrix; the first state transition matrix A i|i+1 is the odometer motion state transition matrix, whose structure corresponds to the first model type; the first model type is CV motion model, CA motion model, CTRV motion model, CTRA motion model, CSAV motion model or CCA motion model, or a combined motion model of two groups of models among CV, CA, CTRV, CTRA, CSAV, and CCA motion models; if the first model type is CV, CA, CTRV, CTRA, CSAV, or CCA motion model, then the first state transition matrix A i|i+1 is the corresponding CV, CA, CTRV, CTRA, CSAV or CCA motion state transition matrix; the first model type is the combined motion model composed of two motion models, then the first state transition matrix A i|i+1 It is formed by adding the corresponding two motion state conversion matrices; Construct the k first observation nodes Nm of the first pose graph i ; and each of the first observation nodes Nm i As the corresponding first state node Ns i The error constraint node; and the first observation node Nm i The corresponding observation value is initialized to the first observation value m corresponding to the first observation value sequence i ; and from the first observation node Nm i To the first state node Ns i The node edge is the corresponding first error node edge Lm-s i ; And construct the first error node edge Lm-s i The error constraint relationship is Where f(·) is the loss function; The loss function f(·) is constructed based on the Huber Loss function.
2. The method for state estimation based on pose graph optimization according to claim 1, characterized in that: The step of obtaining a first estimated state quantity sequence corresponding to the first observation quantity sequence by optimizing the first pose graph specifically includes: With the first state node edge Ls i,i+1 The odometry motion constraint relationship and the first error node edge Lm-s i The error constraint relationship is used as the corresponding first state node Ns i Two constraints, constructing a first optimization objective function of the nonlinear least squares problem of the first pose graph; Each of the first observation nodes Nm i The corresponding observation value of is substituted into the first optimization objective function as a known quantity, and the first state nodes Ns that make the first optimization objective function reach the optimal value are calculated. i The corresponding state quantity is estimated, and the estimated first state nodes Ns are obtained. i The corresponding state quantity is taken as the corresponding first estimated state quantity s i ; and all the first estimated state quantities s obtained i The corresponding first estimated state quantity sequence is formed and saved.
3. The method for state estimation based on pose graph optimization according to claim 1, characterized in that: The performing pose graph transformation on the first pose graph according to the second observation quantity m' and the first estimated state quantity sequence to obtain the second pose graph specifically includes: From the first pose graph, delete the first first state node Ns i And the corresponding first observation node Nm i , the first state node edge Ls i,i+1 and the first error node edge Lm-s i ; In the first pose graph after completing node deletion, the original 2nd to kth first state nodes Ns i As the new 1st to k-1th second state nodes Ns' i , and the original 2nd to kth first observation nodes Nm i As the new 1st to k-1th second observation nodes Nm' i , and the original 2nd to k-1 first state node edge Ls i,i+1 As the new 1st to k-2 second state node edge Ls' i,i+1 , and the original 2nd to kth first error node edge Lm-s i As the new 1st to k-1th second error node edge Lm-s' i ; In the first pose graph where the node transformation is completed, at the k-1 second state node Ns' i＝k-1 Then add the kth second state node Ns' i＝k ; and construct a second state node Ns' based on the odometer motion constraint relationship i＝k-1 to Ns' i＝k The state node edge is used as the k-1 second state node edge Ls' i,i+1 ; In the first pose graph newly added in the completion state node, it is the kth second state node Ns' i＝k Add the corresponding observation node as the kth second observation node Nm' i＝k ; and construct the second observation node Nm' based on the error constraint relationship i＝k To the second state node Ns' i＝k The node edge is the kth second error node edge Lm-s' i ; The first pose graph newly added by the completed observation node is used as the corresponding second pose graph; And in the second pose graph, the second state node Ns' i＝1 The corresponding state quantity is initialized to the first estimated state quantity s corresponding to the first estimated state quantity sequence i＝2 , and so on, until the second state node Ns' i＝k-1 The corresponding state quantity is initialized to the corresponding first estimated state quantity s in the first estimated state quantity sequence i＝k and the second observation node Nm' i＝1 The corresponding observation quantity is initialized to the first observation node Nm i＝2 The corresponding observation quantity, and so on, until the second observation node Nm' i＝k-1 The corresponding observation quantity is initialized to the first observation node Nm i＝k The corresponding observation value is set as follows; and the second observation node Nm' i＝k The corresponding observation quantity is initialized as the second observation quantity m'.
4. The method for state estimation based on pose graph optimization according to claim 3, characterized in that: Obtaining a second estimated state quantity at time t by optimizing the second pose graph specifically includes: In the second state node edge Ls' i,i+1 The odometry motion constraint relationship and the second error node edge Lm-s' i The error constraint relationship is used as the corresponding second state node Ns' i Two constraints, constructing a second optimization objective function of the nonlinear least squares problem of the second pose graph; Each of the second observation nodes Nm' i The corresponding observation value and the second state node Ns' i＝1 to Ns' i＝k-1 The corresponding state quantity is substituted into the second optimization objective function as a known quantity, and the second state node Ns' that makes the second optimization objective function reach the optimal value is obtained. i＝k The corresponding state quantity is estimated, and the second state node Ns' is obtained by estimation i＝k The corresponding state quantity is used as the second estimated state quantity at time t.
5. An electronic device, characterized in that: include: memory, processors, and transceivers; The processor is configured to be coupled to the memory, read and execute instructions in the memory, so as to implement the method according to any one of claims 1 to 4; The transceiver is coupled to the processor, and the processor controls the transceiver to send and receive messages.
6. A computer-readable storage medium, characterized in that The computer-readable storage medium stores computer instructions, and when the computer instructions are executed by a computer, the computer is caused to execute the method according to any one of claims 1 to 4.