BnB-based global optimal pose estimation method

Abstract

The invention discloses a global optimal pose estimation method based on BnB, and relates to a global optimal pose estimation method. The invention aims to solve the problem that accurate pose information is difficult to provide when the surface of a target lacks obvious textures or features are sparse due to the fact that a traditional visual sensor is limited by dependence on the features. The method comprises the following steps: step 1, constructing a consensus set maximization-based target function about an attitude matrix and a position vector; step 2, constructing upper and lower bound functions of an objective function of the attitude matrix in a BnB algorithm, iteratively executing branching, solving upper and lower bounds and pruning until the BnB algorithm converges, thereby obtaining a global optimal solution of the attitude matrix; and step 3, establishing a Huber loss function for the position vector, and performing iterative solution to obtain a global optimal solution of the position and attitude matrix. The invention belongs to the technical field of laser radar autonomous relative navigation.

Description

Technical Field

[0001] The present invention relates to a global optimal pose estimation method, belonging to the technical field of lidar autonomous relative navigation. Background Art

[0002] In modern space missions, pose estimation of non-cooperative spacecraft faces many challenges. Especially when dealing with weak-texture targets, traditional vision sensors are limited by their dependence on features. When the target surface lacks obvious texture or has sparse features, it is difficult to provide accurate pose information.

[0003] Lidar technology can provide high-precision distance measurement and depth information by emitting laser beams and measuring the time of the reflected light. This ability enables lidar to overcome the difficulties brought by low-texture or sparse-feature targets, thus achieving more accurate three-dimensional pose estimation. Summary of the Invention

[0004] To solve the problem that traditional vision sensors are limited by their dependence on features and it is difficult to provide accurate pose information when the target surface lacks obvious texture or has sparse features, the present invention further provides a global optimal pose estimation method based on BnB.

[0005] The technical solution adopted by the present invention to solve the above problems is as follows: The steps of the present invention include:

[0006] Step 1: Construct an objective function based on maximizing the consensus set for the attitude matrix and the position vector;

[0007] Step 2: Construct the upper and lower bound functions of the objective function of the attitude matrix in the BnB algorithm, and iteratively perform branching, solving the upper and lower bounds, and pruning until the BnB algorithm converges, so as to obtain the global optimal solution of the attitude matrix;

[0008] Step 3: Establish a Huber loss function for the position vector and perform iterative solution to obtain the global optimal solutions of the position and the attitude matrix.

[0009] Further, in Step 1, an objective function based on maximizing the consensus set for the position vector of the attitude matrix of the lidar is respectively;

[0010] Given a point set in the target coordinate system and a point set in the lidar coordinate system, each point set contains M points, and for each point X i and its corresponding point Y i in these point sets, there is a rigid transformation as follows:

[0011] Y i = RX i + t (1),

[0012] In formula (1), R and t represent the attitude matrix and the position vector respectively;

[0013] For the corresponding point pairs X i , Y i and X j , Y j , the following relationship is satisfied:

[0014] Y i - Y j = R(X i - X j ) (2),

[0015] Let δY ij = Y i - Y j , δX ij = (X i - X j ) Then the relationship is satisfied:

[0016] δY ij = RδX ij (3),

[0017] In order to determine the transformation relationship between two sets of point sets and at the same time reduce the influence of outliers, a cost function based on consensus set maximization is established as follows:

[0018]

[0019] In formula (4), ε is defined as the threshold for classifying inliers, I is an indicator function, whose value is 1 when a certain criterion is met and 0 when not met;

[0020] The goal of the cost function J is to solve for the optimal attitude matrix R * , to maximize the number of corresponding point pairs between the transformed δX ij and δY ij :

[0021]

[0022] After obtaining the optimal attitude matrix R * , solve for the optimal position vector t * , to maximize the number of corresponding point pairs between the transformed X i and Y i :

[0023]

[0024] Furthermore, the specific process of step 2 is:

[0025] Define Then it can be deduced that:

[0026]

[0027] Thus, the upper and lower bounds of formula (\ref{eq601}) can be deduced as follows:

[0028]

[0029] The upper and lower bounds are obtained at the boundary and the center of this branch respectively;

[0030] During the iteration process, when the difference between the upper and lower bounds is 0, the algorithm converges, and an estimated value of the global optimal rotation matrix R can be obtained.

[0031] Furthermore, after obtaining the estimated value of the rotation matrix R in step 3, the robust clustering algorithm is used to solve the position vector t in formula (6). The specific steps are as follows:

[0032] Step 301: Define T i = Y i - RX i , taking the first component as an example, establish the model:

[0033] T i (1) = t(1) + ∈ i (10),

[0034] In formula (10), ∈ i is the error term;

[0035] Step 302: Establish the Huber loss function;

[0036]

[0037] where r is the residual r = T i (1) - t(1)), and δ is a threshold;

[0038] Step 303: In each iteration, use weighted average to estimate the parameter t(1);

[0039]

[0040] In formula (11), ω i is the weight of each observation value, which is calculated by the current residual r i = T i (1) - t(1), and the initial value is 1;

[0041] Step 304: Update the weight;

[0042] Define the weight as:

[0043]

[0044] Step 305, Iteration process;

[0045] Repeat Step 303 and Step 304 until the parameter t(1) converges;

[0046] Step 306, Output;

[0047] The finally returned parameter is the estimation result of robust clustering;

[0048] Similarly, for the 2nd and 3rd components of and repeat Steps 301 to 306, and the estimations of and

[0049] can be obtained, and finally the robust estimation of the position vector Brief Description of the Drawings

[0050] Figure 1 is the flowchart of the present invention;

[0051] Figure 2 is a schematic diagram of the transformation relationship of points in the lidar coordinate system;

[0052] Figure 3 is a schematic diagram of the algorithm pose estimation result;

[0053] Figure 3 a is a schematic diagram before pose transformation;

[0054] Figure 3 b is a schematic diagram after pose transformation;

[0055] Figure 4 is a schematic diagram of the algorithm convergence process;

[0056] Figure 4 a is a schematic diagram of the upper and lower limit change curve;

[0057] Figure 4 b is a schematic diagram of the remaining search area change curve;

[0058] Figure 4 c is a schematic diagram of the remaining total number of branches change curve. Detailed Embodiments

[0059] Detailed Embodiment 1: As Figures 1 to 4As shown in the figure, a global optimal pose estimation method based on BnB, the specific steps include:

[0060] Step 1: Construct an objective function based on maximizing the consensus set for the pose matrix and position vector; respectively, an objective function based on maximizing the consensus set for the position vector of the lidar with respect to the pose matrix;

[0061] Given a point set in the target coordinate system and a point set in the lidar coordinate system Each point set contains M points, and for each point X i in these point sets and its corresponding point Y i there is a rigid transformation as follows:

[0062] Y i = RX i + t (1),

[0063] In formula (1), R and t represent the pose matrix and position vector respectively;

[0064] For corresponding point pairs X i , Y i and X j , Y j , the following relationship is satisfied:

[0065] Y i - Y j = R (X i - X j ) (2),

[0066] Let δY ij = Y i - Y j , δX ij = (X i - X j ) Then the relationship is satisfied:

[0067] δY ij = RδX ij (3),

[0068] In order to determine the transformation relationship between the two point sets and at the same time reduce the influence of outliers, a cost function based on maximizing the consensus set is established as follows:

[0069]

[0070] In formula (4), ε is defined as the threshold for classifying inliers, and I is an indicator function whose value is 1 when a certain criterion is met and 0 when not met;

[0071] The goal of the cost function J is to solve for the optimal pose matrix R* to maximize the number of corresponding point pairs between the transformed δX ij and δY ij :

[0072]

[0073] After obtaining the optimal attitude matrix R * solve for the optimal position vector t * to maximize the number of corresponding point pairs between the transformed X i and Y i :

[0074]

[0075] Step 2: Construct the upper and lower bound functions of the objective function of the attitude matrix in the BnB algorithm, and iteratively perform branching, solving the upper and lower bounds, and pruning until the BnB algorithm converges, so as to obtain the global optimal solution of the attitude matrix; the specific process is as follows:

[0076] Define Then it can be deduced that:

[0077]

[0078] Thus, the upper and lower bounds of formula (\ref{eq601}) can be deduced as:

[0079]

[0080] The upper and lower bounds are obtained at the boundary and the center of this branch respectively;

[0081] In the iterative process, when the difference between the upper and lower bounds is 0, the algorithm converges, and the estimated value of the global optimal rotation matrix R can be obtained;

[0082] Step 3: Establish a Huber loss function for the position vector and perform iterative solution to obtain the global optimal solutions of the position and attitude matrix; after obtaining the estimated value of the rotation matrix R, use the robust clustering algorithm to solve the position vector t in formula (6), and the specific steps are as follows:

[0083] Step 301: Define T i = Y i - RX i , taking the first component as an example, establish the model:

[0084] T i (1) = t(1) + ∈ i (10),

[0085] In formula (10), ∈ i is the error term;

[0086] Step 302: Establish a Huber loss function;

[0087]

[0088] where r is the residual, r = T i (1) - t(1)), and δ is a threshold;

[0089] Step 303: In each iteration, use weighted average to estimate the parameter t(1);

[0090]

[0091] In formula (11), ω i is the weight of each observation value, calculated from the current residual r i = T i (1) - t(1), with an initial value of 1;

[0092] Step 304: Update the weights;

[0093] Define the weights as:

[0094]

[0095] Step 305: Iteration process;

[0096] Repeat Step 303 and Step 304 until the parameter t(1) converges;

[0097] Step 306: Output;

[0098] The finally returned parameter is the estimation result of robust clustering;

[0099] Similarly, for the second and third components of, repeat Steps 301 to 306 to obtain and the estimations of, and finally obtain the robust estimation of the position vector .

[0100] The above are only the preferred embodiments of the present invention, and do not impose any form of limitation on the present invention. Although the present invention has been disclosed above with preferred embodiments, it is not intended to limit the present invention. Any person skilled in the art can make some changes or modifications to the above-disclosed technical content to obtain equivalent embodiments with equivalent changes. However, as long as it does not depart from the technical content of the present invention, according to the technical essence of the present invention, any simple modifications, equivalent replacements, and improvements made to the above embodiments within the spirit and principles of the present invention still fall within the protection scope of the technical solution of the present invention.

Claims

1. A global optimal pose estimation method based on BnB, characterized in that: The specific steps include: Step 1: Construct an objective function based on consensus set maximization about the posture matrix and position vector; Step 2: Construct the upper and lower bound functions of the objective function of the posture matrix in the BnB algorithm, iteratively execute branches, solve the upper and lower bounds, and prune until the BnB algorithm converges, thereby obtaining the global optimal solution of the posture matrix; Step 3: Establish the Huber loss function for the position vector and solve it iteratively to obtain the global optimal solution of the position and attitude matrix.

2. The BnB-based global optimal pose estimation method according to claim 1, characterized in that: In step 1, the objective function based on consensus set maximization of the position vector of the laser mine with respect to the attitude matrix is ​​respectively performed; Given a point set in the target coordinate system and the point set in the laser radar coordinate system Each point set contains M points, and each point X in these point sets i and its corresponding point Y i There are rigid transformations as follows: Y i =RX i +t(1), In formula (1), R and t represent the attitude matrix and position vector respectively; For the corresponding point pair X i ,Y i and X j ,Y j , satisfying the following relationship: Y i -Y j =R(X i -X j )(2), Let δY ij =Y i -Y j ,δX ij =(X i -X j ) then the relationship is satisfied: δY ij =RδX ij (3), In order to determine the transformation relationship between the two sets of points and reduce the impact of outliers, a cost function based on maximizing the consensus set is established as follows: In formula (4), ε is defined as the threshold used to classify the inliers, and I is an indicator function whose value is 1 when a certain criterion is met and 0 when it is not met; The goal of the cost function J is to solve the optimal posture matrix R * , to maximize the transformed δX ij with δY ij The number of corresponding point pairs between : After obtaining the optimal posture matrix R * After that, find the optimal position vector t * , to maximize the transformed X i With Y i The number of corresponding point pairs between :

3. The BnB-based global optimal pose estimation method according to claim 1, characterized in that: The specific process of step 2 is: definition Then it can be deduced that: From this, we can deduce that the upper and lower bounds of formula (\ref{eq601}) are: The upper and lower bounds are obtained at the boundary and center of the branch respectively; During the iteration process, when the difference between the upper and lower bounds is 0, the algorithm converges and the estimated value of the global optimal rotation matrix R can be obtained.

4. A BnB-based global optimal pose estimation method according to claim 1 or 2, characterized in that: In step 3, after obtaining the estimated value of the rotation matrix R, the position vector t in formula (6) is solved using the robust clustering algorithm. The specific steps are: Step 301: Define T i =Y i -RX i , taking the first component as an example, establish the model: T i (1)=t(1)+∈ i (10), In formula (10), ∈ i is the error term; Step 302: Establishing Huber loss function; Where r is the residual r = T i (1)-t(1)),δ is a threshold; Step 303: In each iteration, the parameter t(1) is estimated by using weighted average. In formula (11), ω i is the weight of each observation, which is determined by the current residual r i =T i (1)-t(1) calculation, the initial value is 1; Step 304: weight update; Define the weights as: Step 305, iterative process; Repeat steps 303 and 304 until parameter t(1) converges; Step 306: output; The final returned parameter is the estimation result of robust clustering; Similarly, yes Repeat steps 301 to 306 for the second and third components of and The position vector is finally obtained Robust estimation of .

Images