A 6D Pose Measurement Error Compensation Method Based on Binocular Vision

Abstract

The present invention relates to the technical field of multi-robot cooperative control, and solves the technical problem that the pose data obtained cannot accurately and stably compensate the motion accuracy of the robot due to the non-unique position of the robot in the binocular vision field. In particular, the present invention relates to a 6D pose measurement error compensation method based on binocular vision, including: constructing a single-point measurement error prediction model based on a multi-output least squares support vector regression model; using a genetic algorithm to globally optimize the hyperparameters of the single-point measurement error prediction model to obtain an optimal hyperparameter set; using the position coordinates of multiple visual landmark points under binocular vision as the input of the single-point measurement error prediction model to obtain the predicted single-point measurement error. The present invention can greatly reduce the maximum position measurement error and the maximum attitude measurement error of the tracking coordinate system, significantly improve the measurement accuracy of the binocular vision device, and at the same time ensure the measurement consistency under different observation angles.

Description

Technical Field

[0001] The present invention relates to the technical field of multi-robot cooperative control, and in particular, to a method for compensating 6D pose measurement errors based on binocular vision. Background Art

[0002] Currently, in the field of robot vision guidance, in order to achieve high-precision positioning of robots, most commercial binocular vision measurement devices are used to directly obtain the 6D pose information of the robots. Due to the measurement errors of the binocular vision devices and the inconsistent distribution of the error magnitudes within the camera's field of view, and since the robot is a moving observation object and its position in the binocular field of view is not unique, the obtained pose data cannot accurately and stably compensate for the motion accuracy of the robot.

[0003] The invention patent with the publication number CN115713554A discloses a binocular vision-based missile loading pose detection system and method, which can provide real-time status feedback information for missile automatic loading control. This method is only applied to missile loading pose measurement and does not compensate for the pose detection accuracy.

[0004] The invention patent with the publication number CN112497216B discloses a method for compensating the pose accuracy of industrial robots based on deep learning. This method is applicable to compensating the pose accuracy of industrial robots and is based on a deep learning model, and cannot directly perform accuracy compensation through algorithms. Summary of the Invention

[0005] Aiming at the deficiencies of the prior art, the present invention provides a method for compensating 6D pose measurement errors based on binocular vision, which solves the technical problem that the obtained pose data cannot accurately and stably compensate for the motion accuracy of the robot due to the non-unique position of the robot in the binocular field of view.

[0006] To solve the above technical problems, the present invention provides the following technical solution: A method for compensating 6D pose measurement errors based on binocular vision, the method comprising the following steps:

[0007] S1. Construct a single-point measurement error prediction model based on a multi-output least squares support vector regression model;

[0008] S2. Use a genetic algorithm to globally optimize the hyperparameters of the single-point measurement error prediction model to obtain an optimal set of hyperparameters ;

[0009] S3. Take the position coordinates of multiple visual landmark points under binocular vision as the input of the single-point measurement error prediction model to obtain the predicted single-point measurement error;

[0010] S4. By minimizing the coordinate transformation error function, convert multiple single-point measurement errors into multi-point coupling errors to achieve pose measurement error compensation for the tracking coordinate system.

[0011] Further, in step S1, the specific process includes the following steps:

[0012] S11. Define the optimization problem, that is, convert the solution of the linear problem of the multi-output least squares support vector regression model into the solution of the optimal estimation problem;

[0013] S12. Minimize the regression weight vector norm, the fitting error of a single output variable, and the overall fitting error of the samples in the optimization problem simultaneously to obtain the regression function of the multi-output least squares support vector regression model;

[0014] S13. According to step S12, use the position coordinates of the sampling points under binocular vision and the position errors in the current coordinate system as input and output data respectively, and train the multi-output least squares support vector regression model to obtain all regression functions, that is, obtain the final single-point measurement error prediction model.

[0015] Further, in step S12, the specific process includes the following steps:

[0016] S121. Let the multi-output sample set be , then the objective function and constraint equation of the multi-output least squares support vector regression model are respectively:

[0017] ; (1)

[0018] In the above formula, represents the weight coefficient of the j-th dimension output; represents the penalty coefficient for the sum of squares of the overall sample error; represents the penalty coefficient for the absolute value of the overall sample error; represents the error of the j-th dimension output of the i-th sample; , respectively represent the total number of samples i and dimension j; represents the constraint equation; represents the kernel function; represents the model parameter of the j-th dimension output;

[0019] S122. Introduce the Lagrange multiplier in formula (1), then its Lagrange function is:

[0020] ; (2)

[0021] Among them, represents the Lagrange multiplier; Denote the constraint equation;

[0022] S123. Respectively take the partial derivatives of Equation (2) with respect to the weight coefficient , , the output error and to obtain:

[0023] ; (3)

[0024] where, L represents the Lagrange function;

[0025] S124. Eliminate the weight coefficient , the output error in Equation (3), denote , and introduce the Gaussian kernel function to obtain the linear equation system as:

[0026] ; (4)

[0027] , , , ;

[0028] where, represents the calculated process value of the kernel function; , respectively represent the input sample and the output sample; represents the kernel function of; represents the standard deviation; is a column vector of dimension, and its value is:

[0029] ; (5)

[0030] S125. Solve and from the linear equation system to obtain the -dimensional regression function, that is:

[0031] ; (6)

[0032] In the above formula, , are the input sample and the output sample respectively.

[0033] Furthermore, in step S2, the specific process includes the following steps:

[0034] S21. Initialization: For the hyperparameter set Set the value ranges of three parameters respectively, and let , , , and select the root mean square error RMSE as the fitness function, which is:

[0035] ; (7)

[0036] In the formula, is the predicted value of the validation set; is the actual value of the validation set; M represents the number of samples in the validation set;

[0037] S22. Encoding: Encode the hyperparameters to be optimized;

[0038] S23. Selection, crossover and mutation: Calculate the fitness of all individuals in the current population and select the individuals with high fitness for retention. Randomly select two individuals for single-point crossover and randomly select the exchange point in the encoding for exchange to form two new encoding sequences. Then, randomly change a certain bit in the encoding sequence from 1 to 0 or from 0 to 1 according to the mutation probability to obtain a new encoding sequence;

[0039] S24. Decoding: Split the encoding sequence after selection, crossover and mutation operations according to the encoding lengths of the hyperparameters , and to obtain their respective binary encodings, and then perform decoding operations on the binary encodings to obtain the optimized specific parameter values;

[0040] S25. Repeat the above steps S21 - S25 until the iteration ends to obtain the optimal hyperparameter set .

[0041] Furthermore, in step S4, the specific process includes the following steps:

[0042] S41. According to the linear transformation relationship of the visual landmark points in the binocular measurement system coordinate system and the tracking coordinate system , transform the linear problem of the single-point measurement error prediction model into an optimal estimation problem, that is, minimize the coordinate transformation error function of formula (12). Minimize the coordinate transformation error function of formula (12), that is:

[0043] ; (12)

[0044] Among them, n is the number of visual landmark points;

[0045] S42. Solve the optimal rotation matrix RAnd the optimal translation vector t ;

[0046] S43, according to the rotation matrix R And the optimal translation vector t Get tracking coordinate system Relative to the binocular measurement system coordinate system The transformation relationship ,Right now:

[0047] ;(twenty four)

[0048] S44, according to the transformation relationship From the rotation matrix R And the optimal translation vector t The tracking coordinate system is obtained in Relative to the binocular measurement system coordinate system Compensated posture and position.

[0049] Further, in step S41, the visual marker point is located in the binocular measurement system coordinate system and tracking coordinate system The linear transformation relationship is:

[0050] ; (11)

[0051] in, R and t Represents the binocular measurement system coordinate system Transform to tracking coordinate system The rotation matrix and translation vector of The visual landmark point is in the tracking coordinate system The coordinates below.

[0052] Furthermore, in step S42, the specific process includes the following steps:

[0053] S421, the translation vector in the minimized coordinate transformation error function t Take the partial derivative to get the translation vector t The solution is:

[0054] ; (15)

[0055] In the formula, and Respectively represent the geometric center of the visual landmark point in the tracking coordinate system and binocular measurement system coordinate system The coordinates below; R Represents the binocular measurement system coordinate system Transform to tracking coordinate system The rotation matrix;

[0056] S422. Substitute the solution of the translation vector t into Equation (12) to obtain the optimization objective and , that is:

[0057] ; (16)

[0058] In the formula, and respectively represent the vectors formed from the geometric center of all visual landmark points to each landmark point in the tracking coordinate system and the binocular measurement system coordinate system ;

[0059] S423. Further expand the optimization objectives and , and transform the minimum value optimization problem into a maximum value problem;

[0060] S424. Perform singular value decomposition on the maximum value problem to obtain the optimal rotation matrix R ;

[0061] S425. Solve the optimal translation vector R according to the optimal rotation matrix t . By substituting the optimal rotation matrix R into Equation (15), the optimal translation vector t can be solved.

[0062] Furthermore, in step S423, specifically, expand the optimization objective in Equation (16):

[0063] ; (17)

[0064] Ignoring the constant term in Equation (17), the minimum value optimization problem can be transformed into the maximum value problem shown in Equation (18), that is:

[0065] ; (18)

[0066] where R is the rotation matrix; n is the number of visual landmark points.

[0067] Furthermore, in step S424, specifically, construct matrices and , then Equation (18) is transformed into:

[0068] ; (19)

[0069] According to the properties of the matrix trace, it can be obtained that: ; (20)

[0070] Among them, represents the matrix trace;

[0071] Denote , combining equations (19) and (20), and performing singular value decomposition on it to obtain the matrix , that is:

[0072] ; (21)

[0073] Denote , since the matrices obtained by singular value decomposition U are all orthogonal matrices, R is an orthogonal rotation matrix, so H is also an orthogonal matrix. Given that the maximum value of each element in the orthogonal matrix H does not exceed 1, thus:

[0074] ; (22)

[0075] If we want to maximize equation (22), the diagonal of the orthogonal matrix H must all be 1, thereby calculating the optimal rotation matrix R , which is:

[0076] ; (23)

[0077] Among them, H is an orthogonal matrix; V represents the matrix transpose; represents the transpose of the orthogonal matrix U .

[0078] By means of the above technical solution, the present invention provides a 6D pose measurement error compensation method based on binocular vision, which has at least the following beneficial effects:

[0079] 1. The present invention can greatly reduce the maximum position measurement error and maximum attitude measurement error of the tracking coordinate system, significantly improve the measurement accuracy of the binocular vision device, and at the same time ensure the measurement consistency under different observation angles.

[0080] 2. In this embodiment of the present invention, the genetic algorithm is used to optimize the hyperparameters to obtain the optimal hyperparameter set, which can eliminate the influence of each hyperparameter in the multi-output least squares support vector regression model on the prediction accuracy of the single-point measurement error prediction model, thereby improving the prediction accuracy of the single-point measurement error prediction model.

[0081] 3. The present invention combines the pose measurement principle, transforms the linear problem into an optimal estimation problem, and converts multiple single-point prediction errors into multi-point coupling errors by minimizing the coordinate transformation error function, thereby realizing the pose measurement error compensation of the tracking coordinate system. BRIEF DESCRIPTION OF THE DRAWINGS

[0082] The drawings described herein are used to provide a further understanding of the present application, and constitute a part of the present application. The schematic embodiments of the present application and their descriptions are used to explain the present application, and do not constitute an improper limitation of the present application. In the drawings:

[0083] Figure 1 is a flowchart of the 6D pose measurement error compensation method of the present invention;

[0084] Figure 2 is a schematic diagram of the geometric relationship of single-point measurement of the present invention;

[0085] Figure 3 is a schematic diagram of the coordinate systems of the binocular measurement system and the tracking coordinate system of the present invention;

[0086] Figure 4 is a schematic diagram of multi-angle measurement of the present invention;

[0087] Figure 5 is a schematic diagram of the sampling plan of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0088] In order to make the above objects, features, and advantages of the present invention more obvious and understandable, the present invention will be further described in detail below with reference to the drawings and specific embodiments. Thereby, the implementation process of how the present application uses technical means to solve technical problems and achieve technical effects can be fully understood and implemented accordingly.

[0089] As a non-contact measurement method, visual measurement has a larger measurement range and faster measurement speed compared to traditional measurement means, and is widely used in fields such as industrial part size measurement, industrial robots, and mobile robot navigation. Currently, in the field of visual guidance of robots, in order to achieve high-precision positioning of robots, most commercial binocular vision measurement devices are directly used to obtain the 6D pose information of the robots.

[0090] Due to the measurement errors of binocular vision devices, and the inconsistent distribution of the error sizes within the camera's field of view, and the fact that the robot, as a moving observation object, has a non-unique position in the binocular field of view, the obtained pose data cannot accurately and stably compensate for the motion accuracy of the robot. Therefore, it is necessary to apply a more optimized measurement error compensation method to the binocular vision device.

[0091] The invention patent with the publication number CN115713554A discloses a binocular vision-based missile loading pose detection system and method, including a binocular camera, a sling target group, a missile target group, and a camera controller. It can effectively solve the problem of high-precision positioning in the loading operation faced by the vehicle-mounted missile automatic loading system, and has flexible layout, convenient operation, and high automation. It can provide real-time status feedback information for missile automatic loading control. This method is only applied to missile loading pose measurement and does not compensate for the pose detection accuracy.

[0092] The invention patent with the publication number CN112497216B discloses a deep learning-based industrial robot pose accuracy compensation method. A deep learning model for industrial robot pose error compensation is proposed. Combining the advantages of strong feature expression ability and strong statistical interpretability of deep learning, taking the robot error similarity as an additional feature for supervised learning, introducing contrast information other than the robot state features itself, and improving the prediction accuracy of the deep learning model. This method is applicable to industrial robot pose accuracy compensation and is based on a deep learning model, and cannot directly perform accuracy compensation through algorithms.

[0093] The literature "Huo J, Zhang G, Cui J, et al. Corrected calibration algorithm with a fixed constraint relationship and an error compensation technique for a binocular vision measurement system[J]. Applied optics, 2018, 57(19): 5492-5504", "Wei Song, Robot Vision Servo Control System for Composite Material Milling Edge Machining[D]. Nanjing: Nanjing University of Aeronautics and Astronautics, 2022." proposed a binocular camera measurement error compensation method based on spatial error similarity. A mapping model between the binocular camera measurement error and the position of the tracking model was established through spatial sampling, and the position measurement error prediction was realized using the mapping model. Since the direct sampling object of this method is the coordinate system rather than the visual landmark points, and the influence of different camera observation angles on the coordinate system measurement error is not considered, the prediction model obtained by this method has poor universality.

[0094] Due to the defect that under binocular vision, the robot, as a moving observation object, has measurement errors and cannot be better optimized and compensated, please refer to Figure 1, shows a specific implementation of this embodiment. This embodiment realizes single-point measurement error prediction by constructing a single-point measurement error prediction model, and uses a genetic algorithm to globally optimize the model hyperparameters; combined with the pose measurement principle, the linear problem is transformed into an optimal estimation problem, and by minimizing the coordinate transformation error function, multiple single-point measurement prediction errors are transformed into multi-point coupling errors, thereby realizing the pose measurement error compensation of the tracking coordinate system. This embodiment proposes a 6D pose measurement error compensation method based on binocular vision, and the method includes the following steps:

[0095] S1. Construct a single-point measurement error prediction model based on a multi-output least squares support vector regression model (MLSSVR); Figure 2 As shown, it is a schematic diagram of the geometric relationship of single-point measurement, where o 1 X 1 Y 1 and o 2 X 2 Y 2 are the image coordinate systems in imaging planes 1 and 2, respectively, and the lines connecting their origins and the centers of their respective camera lenses o 1 O 1 and o 2 O 2 forms the optical axis of the two cameras, f 1 and f 2 are the distances from the origin of the two image coordinate systems to the optical center of the lens, O 1 O The line connecting 2 forms a baseline with a length of B , with the line connecting the centers of the two lenses O 1 O Midpoint of 2 O is the origin, O 1 O 2 is the straight line X Axis to establish binocular measurement system coordinate system OXYZ In step S1, the specific process includes the following steps:

[0096] S11. Define the optimization problem, that is, transform the linear problem of solving the multi-output least squares support vector regression model into the optimal estimation problem; transform the predicted single-point error into a multi-point coupling error to achieve error compensation of the tracking coordinate system, significantly improve the measurement accuracy of the binocular vision equipment, and ensure the measurement consistency under different observation angles.

[0097] S12. Minimize the regression weight vector norm, the fitting error of a single output variable, and the overall fitting error of the samples in the optimization problem simultaneously to obtain the regression function of the multi-output least squares support vector regression model. In step S12, the specific process includes the following steps:

[0098] S121. Let the multi-output sample set be , then the objective function and constraint equation of the multi-output least squares support vector regression model are respectively:

[0099] ; (1)

[0100] In the above formula, represents the weight coefficient of the j-th dimension output; represents the penalty coefficient for the sum of squared errors of the overall samples; represents the penalty coefficient for the absolute value of the overall error of the samples; represents the output error of the j-th dimension of the i-th sample; , respectively represent the total number of sample i and dimension j; represents the constraint equation; represents the kernel function; represents the model parameter of the j-th dimension output;

[0101] S122. Introduce the Lagrange multiplier into formula (1), then its Lagrange function is:

[0102] ; (2)

[0103] Among them, represents the Lagrange multiplier; represents the constraint equation;

[0104] S123. According to the KKT conditions, take the partial derivatives of formula (2) with respect to the weight coefficient , , the output error and respectively to get:

[0105] ; (3)

[0106] Among them, L represents the Lagrange function;

[0107] S124. Eliminate the weight coefficient and the output error in formula (3), denote , and introduce the Gaussian kernel function , to obtain the linear equation system as:

[0108] ; (4)

[0109] , , , ;

[0110] Among them, represents the calculated process value of the kernel function; , respectively represent the input sample and the output sample of represents the kernel function of represents the standard deviation; is a column vector of dimension, and its value is:

[0111] ; (5)

[0112] S125. Solve according to the linear equations and to obtain the regression function of the th dimension, that is:

[0113] ; (6)

[0114] In the above formula, , are the input sample and the output sample respectively.

[0115] S13. Use the position coordinates of the sampling points under binocular vision and the position error under the current coordinates as the input and output data respectively, and train a multi-output least squares support vector regression model to obtain all regression functions, that is, obtain the final single-point measurement error prediction model. As Figure 3 and Figure 4 shown, as Figure 3 is a schematic diagram of the coordinate system of the binocular measurement system and the tracking coordinate system, Figure 4 is a schematic diagram of multi-angle measurement, that is, the corresponding single-point measurement error is sampled at different stations. As Figure 5 shown, it is a schematic diagram of the sampling plan. To ensure uniform sampling, a spatial cube with a length of 500 mm, a width of 500 mm, and a height of 500 mm is planned as the sampling area within the binocular camera's field of view. The center position of one end of the cube facing the binocular camera is located in the best measurement area within the camera's field of view.

[0116] Among them, let the coordinate system of the binocular measurement system be , and the tracking coordinate system be , if there are n visual landmark points bound to the tracking coordinate system The coordinates of the visual landmark points in the tracking coordinate system can form a matrix , which is used as the position coordinates input to the multi-output least squares support vector regression model. In the binocular measurement system coordinate system The coordinates can form a matrix . The single-point measurement error obtained by sampling can form a measurement error matrix , which is used as the position error output by the multi-output least squares support vector regression model. Then, the sampling data is used to train the multi-output least squares support vector regression model to obtain a single-point measurement error prediction model.

[0117] S2. Use the genetic algorithm to globally optimize the hyperparameters of the single-point measurement error prediction model to obtain the optimal hyperparameter set ; Since the hyperparameter set has a great influence on the prediction accuracy of the single-point measurement error prediction model, it needs to be optimized. Considering that the training of the single-point measurement error prediction model is carried out offline and the time cost of training does not need to be considered, in this embodiment, the genetic algorithm with strong global optimization ability is used to optimize the hyperparameters of the single-point measurement error prediction model. In step S2, the specific process includes the following steps:

[0118] S21. Initialization: For the hyperparameter set , the value ranges of the three parameters are set respectively, and let , , . The fitness function is selected as the root mean square error RMSE, which is:

[0119] ; (7)

[0120] In the formula, is the predicted value of the validation set; is the actual value of the validation set; M represents the number of samples in the validation set;

[0121] S22. Encoding: Encode the hyperparameters that need to be optimized; common encoding methods include binary encoding, Gray code, floating-point encoding, etc. In this embodiment, binary encoding is selected. Taking the encoding of the hyperparameter C as an example, its encoding calculation is as follows:

[0122] ; (8)

[0123] In the formula, is the encoding length of C; S is the search accuracy. Similarly, the hyperparameters and Coding length and 。

[0124] S23. Selection, crossover, and mutation: Calculate the fitness of all individuals in the current population and select individuals with high fitness for retention. Randomly select two individuals for single-point crossover and randomly select the exchange point in the coding for exchange to form two new coding sequences. Then, randomly change a certain bit in the coding sequence from 1 to 0 or from 0 to 1 according to the mutation probability to obtain a new coding sequence. Specifically, the selection operation needs to calculate the fitness of all individuals in the current population and select individuals with high fitness for retention. Common ways of the crossover operation include single-point crossover, multi-point crossover, order crossover, etc. In this paper, single-point crossover is selected, that is, randomly select two individuals and randomly select the exchange point in the coding for exchange to form two new coding sequences. The mutation operation needs to set the mutation probability to a small value and randomly change a certain bit in the coding sequence from 1 to 0 or from 0 to 1 to obtain a new coding sequence.

[0125] S24. Decoding: Split the coding sequence after selection, crossover, and mutation operations according to the hyperparameter Coding length 、 and to obtain their respective binary codings, and then perform decoding operations on the binary codings to obtain the optimized specific parameter values. Here, taking the decoding process of the hyperparameter C as an example, the decoding calculation is as follows:

[0126] ; (9)

[0127] In the formula: is the binary number at the i-th bit of the binary coding;

[0128] S25. Repeat the above steps S21 - S25 until the iteration ends to obtain the optimal hyperparameter set 。

[0129] In this embodiment, individuals are screened through selection, crossover, and mutation operations in the genetic process, and finally the optimal solution of the optimization problem is obtained. This embodiment uses the Genetic Algorithm (GA) to optimize the hyperparameters to obtain the optimal hyperparameter set , which can eliminate the influence of each hyperparameter in the multi-output least squares support vector regression model on the prediction accuracy of the single-point measurement error prediction model, thereby improving the prediction accuracy of the single-point measurement error prediction model.

[0130] S3. Use the position coordinates of multiple visual landmark points under binocular vision as the input of the single-point measurement error prediction model to obtain the predicted single-point measurement error; the predicted error matrix obtained through the trained single-point measurement error prediction model is , so the compensated coordinate values of all visual landmark points in the coordinate system are:

[0131] (10);

[0132] where , is the coordinate of the visual landmark point in the binocular measurement system coordinate system forming a matrix.

[0133] S4. By minimizing the coordinate transformation error function, convert multiple single-point measurement errors into multi-point coupling errors to achieve pose measurement error compensation for the tracking coordinate system; in this embodiment, in combination with the pose measurement principle, convert the linear problem into an optimal estimation problem, and by minimizing the coordinate transformation error function, convert multiple single-point prediction errors into multi-point coupling errors to achieve pose measurement error compensation for the tracking coordinate system. In step S4, the specific process includes the following steps:

[0134] S41. According to the linear transformation relationship between the visual landmark point in the binocular measurement system coordinate system and the tracking coordinate system , convert the linear problem of the single-point measurement error prediction model into an optimal estimation problem, that is, minimize the coordinate transformation error function of formula (12); where, the linear transformation relationship between the visual landmark point in the binocular measurement system coordinate system and the tracking coordinate system is:

[0135] ; (11)

[0136] where R and t respectively represent the rotation matrix and translation vector for transforming the binocular measurement system coordinate system to the tracking coordinate system ; is the coordinate of the visual landmark point in the tracking coordinate system .

[0137] During actual measurement, due to the existence of measurement noise and errors, there usually does not exist a rotation matrix to and a translation vector R that satisfy the linear transformation relationship of all coordinates t . Therefore, the linear problem can be converted into an optimal estimation problem, that is, minimize the coordinate transformation error function of formula (12), that is:

[0138] ; (12)

[0139] Wherein, n is the number of visual landmark points.

[0140] S42. Solve the optimal rotation matrix according to the minimized coordinate transformation error function R and the optimal translation vector t ; In step S42, the specific process includes the following steps:

[0141] S421. Take the partial derivative of the translation vector t in the minimized coordinate transformation error function to obtain the solution of the translation vector t ; Specifically, taking the partial derivative of the translation vector t in equation (12) gives:

[0142] ; (13)

[0143] Continue to simplify equation (13) to get:

[0144] ; (14)

[0145] Let the term after taking the partial derivative in equation (14) be zero, we can get:

[0146] ; (15)

[0147] In the formula, and respectively represent the coordinates of the geometric center of the visual landmark points in the tracking coordinate system and the binocular measurement system coordinate system ; R represents the rotation matrix for transforming the binocular measurement system coordinate system to the tracking coordinate system ;

[0148] S422. Substitute the solution of the translation vector t into equation (12) to obtain the optimization objectives and , that is:

[0149] ; (16)

[0150] In the formula, and respectively represent the vectors formed from the geometric center of all visual landmark points to each landmark point in the tracking coordinate system and the binocular measurement system coordinate system .

[0151] S423. Expand the optimization objective and further, and transform the minimum value optimization problem into a maximum value problem; specifically, expand the optimization objective in Equation (16) further:

[0152] ; (17)

[0153] Ignoring the constant term in Equation (17), the minimum value optimization problem can be transformed into the maximum value problem shown in Equation (18), that is:

[0154] ; (18)

[0155] where R is the rotation matrix; n is the number of visual landmark points.

[0156] S424. Perform singular value decomposition on the maximum value problem to obtain the optimal rotation matrix R ; specifically, construct matrices and , then Equation (18) is transformed into:

[0157] ; (19)

[0158] According to the properties of the matrix trace, it can be obtained that: ; (20)

[0159] where represents the matrix trace;

[0160] Denote , combining Equation (19) and (20), and performing singular value decomposition on it to obtain matrix , that is:

[0161] ; (21)

[0162] Denote , since the matrices obtained by singular value decomposition, U are all orthogonal matrices, R is an orthogonal rotation matrix, so H is also an orthogonal matrix. Given that the maximum value of each element in the orthogonal matrix H does not exceed 1, therefore:

[0163] ; (22)

[0164] If we want to make Equation (22) obtain the maximum value, the diagonal of the orthogonal matrix H must all be 1, thus calculating the optimal rotation matrix R , which is:

[0165] ;(twenty three)

[0166] in, H is an orthogonal matrix; V Representation Matrix The transpose of Represents an orthogonal matrix U The transpose of .

[0167] S425, according to the optimal rotation matrix R Solving for the optimal translation vector t , by taking the optimal rotation matrix R Substituting into equation (15) we can solve the optimal translation vector t .

[0168] S43, according to the rotation matrix R And the optimal translation vector t Get tracking coordinate system Relative to the binocular measurement system coordinate system The transformation relationship ,Right now:

[0169] ;(twenty four)

[0170] S44, according to the transformation relationship From the rotation matrix R And the optimal translation vector t The tracking coordinate system is obtained in Relative to the binocular measurement system coordinate system Compensated posture and position.

[0171] The present invention can significantly reduce the maximum position measurement error and the maximum attitude measurement error of the tracking coordinate system, significantly improve the measurement accuracy of the binocular vision device, and ensure the measurement consistency under different observation angles.

[0172] Those skilled in the art can understand that all or part of the steps in the above-mentioned embodiment method can be completed by instructing the relevant hardware through a program, so the present application can take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware. Moreover, the present application can take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program codes.

[0173] Each embodiment in this specification is described in a progressive manner. Each embodiment focuses on the differences from other embodiments. For the same or similar parts among the embodiments, reference can be made to each other. For the above embodiments, since they are basically similar to the method embodiments, the description is relatively simple. For the relevant parts, reference can be made to the partial description of the method embodiments.

[0174] The above embodiments have introduced the present invention in detail. Specific examples are used in this article to elaborate on the principle and implementation of the present invention. The description of the above embodiments is only used to help understand the method and its core idea of the present invention. At the same time, for those of ordinary skill in the art, based on the idea of the present invention, there will be changes in the specific implementation and application scope. In summary, the content of this specification should not be construed as a limitation to the present invention.

Claims

1. A 6D posture measurement error compensation method based on binocular vision, characterized in that: The method comprises the following steps: S1. Construct a single-point measurement error prediction model based on a multi-output least squares support vector regression model. The specific process includes the following steps: S11, defining an optimization problem, that is, transforming the linear problem of solving the multi-output least squares support vector regression model into the problem of solving the optimal estimation problem; S12, the regression weight vector norm, the single output variable fitting error and the sample overall fitting error in the optimization problem are simultaneously minimized to obtain the regression function of the multi-output least squares support vector regression model, and the specific process includes the following steps: S121, let the multi-output sample set be Z={(X i ,Y i )|,X i ∈R m ,Y i ∈R n ,i=1,2,…,l}, then the objective function and constraint equations of the multi-output least squares support vector regression model are: In the above formula, W j represents the weight coefficient of the j-th dimension output; C represents the penalty coefficient for the sum of squared errors of the sample as a whole; C0 represents the penalty coefficient for the absolute value of the overall error of the sample; e i,j represents the j-th dimension output error of the i-th sample; l and n represent the total number of samples i and dimension j respectively; sty i,j represents the constraint equation; represents the kernel function; b j Represents the model parameters of the j-th dimension output; S122, introduce the Lagrange multiplier α into equation (1) i,j , then its Lagrange function is: Among them, α i,j represents the Lagrange multiplier; S123, according to the KKT condition, calculate the weight coefficient W for equation (2) j 、b j , output error e i,j and α i,j The partial derivative of is: Where L represents the Lagrange function; M j is an l-dimensional column vector; S124, eliminate the weight coefficient W in formula (3) j , output error e i,j ,remember And introduce the Gaussian kernel function K(X k ,X j ) = exp(-||X k -X j || 2 / 2σ 2 ), the linear equations are: α j ＝[α 1，j α 2，j ...α i，j ] T AND i =[and 1,j and 2,j ...a l,j ] T I＝[1 1...1] T Among them, Ω kj Represents the calculation process value of the kernel function; X k , X j Respectively represent input samples and output samples from 1 to l; Indicates (X k ) T The kernel function of M; σ represents the standard deviation; M j Its value is: S125. Solve α based on the linear equations i,j and b j Get the regression function of the jth dimension, that is: In the above formula, K(X i ,X j ) = exp(-||X i -X j || 2 / 2σ 2 ), X i , X j are input samples and output samples respectively; S13, according to step S12, the position coordinates of the sampling point under binocular vision and the position error under the current coordinates are used as input and output data respectively, and a multi-output least squares support vector regression model is trained to obtain all regression functions, that is, to obtain a final single-point measurement error prediction model; S2, using genetic algorithm to globally optimize the hyperparameters of the single-point measurement error prediction model to obtain the optimal hyperparameter set θ = [C, C0, σ]; S3, using the position coordinates of multiple visual landmarks under binocular vision as input of a single-point measurement error prediction model to obtain a predicted single-point measurement error; S4. By minimizing the coordinate transformation error function, multiple single-point measurement errors are converted into multi-point coupling errors to achieve position and posture measurement error compensation of the tracking coordinate system.

2. The 6D posture measurement error compensation method according to claim 1, characterized in that: In step S2, the specific process includes the following steps: S21, Initialization: For the hyperparameter set θ = [C, C0, σ], set the value ranges of the three parameters respectively, let C = [C min ,C max ],C0=[C 0min ,C 0max ],σ=[σ min ,σ max ], the fitness function selects the root mean square error RMSE, which is: In the formula, is the predicted value of the validation set; y i is the actual value of the validation set; M represents the number of samples in the validation set; S22, encoding: encoding the hyperparameters to be optimized; S23, selection, crossover and mutation: Calculate the fitness of all individuals in the current population and select individuals with high fitness to retain, use single-point crossover to randomly select two individuals, and randomly select exchange points in the code to exchange to form two new code sequences, and then randomly change a bit in the code sequence from 1 to 0 or from 0 to 1 according to the mutation probability, so as to obtain a new code sequence; S24, decoding: splitting the coded sequence after the selection, crossover and mutation operations according to the coding lengths l1, l2 and l3 of the hyperparameters C, C0 and σ to obtain respective binary codes, and then performing a decoding operation on the binary codes to obtain the optimized specific parameter values; S25. Repeat the above steps S21-S24 until the iteration ends, and obtain the optimal hyperparameter set θ=[C, C0, σ].

3. The 6D posture measurement error compensation method according to claim 1, characterized in that: In step S4, the specific process includes the following steps: S41, according to the visual mark point in the binocular measurement system coordinate system F S and tracking coordinate system F E The linear transformation relationship under , transforms the linear problem of the single-point measurement error prediction model into an optimal estimation problem, that is, minimizing the coordinate transformation error function of formula (12), that is: Where n is the number of visual landmarks; a i is the visual landmark point in the tracking coordinate system F E The coordinates under; R is the rotation matrix; t is the optimal translation vector; S42, solving the optimal rotation matrix R and the optimal translation vector t according to the minimized coordinate transformation error function; S43, obtain the tracking coordinate system F according to the rotation matrix R and the optimal translation vector t E Relative to the binocular measurement system coordinate system F S The transformation relationship Right now: S44, according to the transformation relationship Obtain the tracking coordinate system F from the rotation matrix R and the optimal translation vector t E Relative to the binocular measurement system coordinate system F S Compensated posture and position.

4. The 6D posture measurement error compensation method according to claim 3, characterized in that: In step S41, the visual marker point is located in the binocular measurement system coordinate system F S and tracking coordinate system F E The linear transformation relationship is: Ra i +t=b′ i (11) Where R and t represent the binocular measurement system coordinate system F S Transform to tracking coordinate system F E The rotation matrix and translation vector.

5. The 6D posture measurement error compensation method according to claim 3, characterized in that: In step S42, the specific process includes the following steps: S421, taking partial derivative of the translation vector t in the minimized coordinate transformation error function to obtain a solution for the translation vector t, namely: In the formula, and Respectively represent the geometric center of the visual landmark point in the tracking coordinate system F E and the binocular measurement system coordinate system F S The coordinates under; R represents the binocular measurement system coordinate system F S Transform to tracking coordinate system F E The rotation matrix of S422, Substitute the solution of the translation vector t into equation (12) to obtain the optimization target a ci and b' ci ,Right now: In the formula, a ci and b' ci Respectively represent the tracking coordinate system F E and the binocular measurement system coordinate system F S Next, the vectors from the geometric center of all visual landmarks to each landmark; S423, optimize the target a ci and b' ci Go a step further and transform the minimum optimization problem into a maximum problem; S424, performing singular value decomposition on the maximum value problem to obtain an optimal rotation matrix R; S425. Solve the optimal translation vector t according to the optimal rotation matrix R. The optimal translation vector t can be obtained by substituting the optimal rotation matrix R into equation (15).

6. The 6D posture measurement error compensation method according to claim 5, characterized in that: In step S423, the optimization objective in equation (16) is further expanded: Ignoring the constant term in equation (17), the minimum optimization problem can be transformed into the maximum problem shown in equation (18), that is: Among them, R is the rotation matrix; n is the number of visual landmark points.

7. The 6D posture measurement error compensation method according to claim 6, characterized in that: In step S424, specifically, the matrix A is constructed c =[a c1 a c2 ...a cn ] and B' c =[b' c1 b' c2 ...b' cn ], then transform equation (18) into equation (19), that is: According to the properties of matrix trace, we can get: where tr(·) represents the matrix trace; remember Combining equations (19) and (20), and performing singular value decomposition on them, we can get the matrix V T ,Right now: tr(RP)=tr(RUΣV T )=tr(ΣV T EN) (21) Let H = V T RU, due to the singular value decomposition of the matrix V T , U are all orthogonal matrices, R is an orthogonal rotation matrix, so H is also an orthogonal matrix. It is known that the maximum value of each element in the orthogonal matrix H does not exceed 1, so: To maximize the value of equation (22), the diagonals of the orthogonal matrix H must all be 1, so that the optimal rotation matrix R can be calculated as: R=VHU T =VU T (23) Among them, H is an orthogonal matrix; V represents the matrix V T The transpose of U T represents the transpose of the orthogonal matrix U.

Images