Multi-laser tracker pose measurement method based on composite constraint

Abstract

The invention provides a multi-laser tracker pose measurement method based on composite constraint, and belongs to the technical field of laser measurement, and the method comprises the steps: employing a preset laser tracker, building a pose measurement system, obtaining an instrument coordinate system, building a world coordinate system, setting a target plane, and setting a preset number of target balls to obtain a pose measurement result; constructing to obtain a target coordinate system; according to the pose measurement system, the laser trackers are used for tracking the target ball, the target ball coordinates of the target ball under the instrument coordinate systems of the laser trackers are measured, the coordinate systems of the target ball coordinates are unified to a world coordinate system based on coordinate system transformation, and to-be-solved system parameters are obtained; and constructing a composite constraint, constructing an optimal equation set based on an optimization algorithm, solving system parameters to be solved, obtaining a measurement pose, and completing pose measurement of the multiple laser trackers. According to the invention, the problems of small measurement range, low precision and large demand on the number of observation stations in a multilateral method in a traditional pose measurement method are solved.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of laser measurement, and particularly relates to a multi-laser tracker pose measurement method based on composite constraints. BACKGROUND

[0002] Currently, the measurement of the six-degree-of-freedom pose of an object mainly relies on technical means such as machine vision, a three-coordinate measuring machine (CMM), indoor GPS (iGPS), and a multi-edge measurement method. However, the traditional visual measurement method and iGPS are limited by the installation and positioning accuracy of the instrument, and are often limited by the complexity of the system and the lack of measurement accuracy. The measurement range of the three-coordinate measuring machine is limited, and it is difficult to realize pose measurement. The traditional laser multi-edge pose measurement system is difficult to eliminate the cumulative error in the cooperative measurement of multiple instruments due to the single constraint condition. The parameter calibration measures the distance between the station and the target ball through a seven-station system, and the angle information is not utilized. In addition, the system has high complexity and requires a large number of stations. SUMMARY

[0003] In view of the above problems in the prior art, the multi-laser tracker pose measurement method based on composite constraints provided by the application solves the problems of small measurement range, low precision, and large number of stations required by the multi-edge method in the traditional pose measurement method.

[0004] In order to achieve the above purpose, the technical scheme adopted by the application is as follows: a multi-laser tracker pose measurement method based on composite constraints, comprising the following steps: S1, a pose measurement system is established by using a preset laser tracker, an instrument coordinate system is obtained, a world coordinate system is established, a target plane is set, and a target coordinate system is constructed by setting a preset number of target balls; S2, according to the pose measurement system, the target balls are tracked by using the laser tracker, the coordinates of the target balls in the instrument coordinate system of each laser tracker are measured, the coordinate system of the target ball coordinates is unified to the world coordinate system based on the coordinate system transformation, and the system parameters to be solved are obtained; S3, a composite constraint is constructed, an optimization equation set is constructed based on an optimization algorithm, the pose parameters of the instrument coordinate system in the world coordinate system are calibrated, the system parameters to be solved are solved, the measurement pose is obtained, and the multi-laser tracker pose measurement is completed.

[0005] The application has the beneficial effects that: the application constructs a redundant observation network by four laser trackers, and for the first time, a space point constraint, a length constraint, a plane constraint, and a circular trajectory constraint are jointly constructed as a composite optimization problem. The system parameters are calibrated based on the LM algorithm, the large-range high-precision pose measurement is realized, and the demand for the number of stations in the multi-edge method is reduced.

[0006] Further, the S1 comprises the following steps: S101, a pose measurement system is established by using four laser trackers to obtain an instrument coordinate system; S102, based on the positional relationship of the laser trackers in the pose measurement system, the coordinate of the first laser tracker is set as the origin, and the line connecting the first laser tracker and the second laser tracker is set as the X axis of the world coordinate system; S103, the plane formed by the coordinates of the first laser tracker, the second laser tracker and the third laser tracker is set as the XOY plane of the world coordinate system, the Y axis of the world coordinate system is obtained, and the fourth laser tracker is located on the XOY plane of the world coordinate system; S104, the perpendicular line perpendicular to the XOY plane is set as the Z axis of the world coordinate system, and the world coordinate system is established according to the set X axis, Y axis and Z axis of the world coordinate system; S105, a target plane is set, three target balls are set on the target plane, the coordinate of the first target ball is set as the origin of the target coordinate system, and the line connecting the first target ball and the second target ball is set as the X axis of the target coordinate system; S106, the plane formed by the coordinates of the first laser tracker, the second laser tracker and the third laser tracker is set as the XOY plane of the target coordinate system, and the Y axis of the target coordinate system is obtained; S107, the direction of the perpendicular line is obtained according to the XOY plane of the target coordinate system, the perpendicular line is set as the Z axis of the target coordinate system, and the target coordinate system is constructed according to the set X axis, Y axis and Z axis of the target coordinate system.

[0007] The above further scheme has the beneficial effects that the world coordinate system is constructed by using four laser trackers, three target balls are measured by the laser trackers respectively, redundant data of distance measurement and angle measurement are provided to realize the nonlinear optimization algorithm to solve the pose parameters of the target object, the robustness of the pose measurement system is improved, and a more stable pose measurement method is realized.

[0008] Further, the S2 comprises the following steps: S201, according to the pose measurement system, the first target ball is measured by using the first laser tracker and the fourth laser tracker based on the transformation formula from the spherical coordinate system to the Cartesian coordinate system to obtain the first target ball coordinate in the instrument coordinate system; S202, the second target ball is measured by using the second laser tracker to obtain the second target ball coordinate in the instrument coordinate system, and the third target ball is measured by using the third laser tracker to obtain the third target ball coordinate in the instrument coordinate system; S203, according to the pose measurement system, the attitude transformation matrix from the instrument coordinate system to the world coordinate system is obtained based on the coordinate system transformation. S204, using the attitude transformation matrix, the first target ball coordinates, the second target ball coordinates and the third target ball coordinates are unified, and the system parameters to be solved are obtained.

[0009] Further, the S203 is specifically: According to the azimuth and elevation values in the coordinate system measured by the laser tracker in the pose measurement system, the rotation matrix and the translation matrix are obtained based on the coordinate system transformation, and the attitude transformation matrix from the instrument coordinate system to the world coordinate system is obtained according to the rotation matrix and the translation matrix.

[0010] Further, the expression of the rotation matrix is as follows: ; Among them, the rotation matrix, the world coordinate, each laser tracker, respectively represent the fixed angle of the X, Y and Z axis rotation of the world coordinate system; The expression of the translation matrix is as follows: , , , ; Among them, the translation matrix of the first laser tracker, the translation matrix of the second laser tracker, the translation matrix of the third laser tracker, the translation matrix of the fourth laser tracker, the to-be-solved coordinate of the second laser tracker on the X axis, the to-be-solved coordinate of the third laser tracker on the X axis, the to-be-solved coordinate of the third laser tracker on the Y axis, the to-be-solved coordinate of the fourth laser tracker on the X axis, the to-be-solved coordinate of the fourth laser tracker on the Y axis, the to-be-solved coordinate of the fourth laser tracker on the Z axis.

[0011] The beneficial effects of the further scheme are that the application measures the target ball coordinates in the instrument coordinate system by the laser tracker, and obtains the system parameters to be solved by using the attitude transformation matrix from the instrument coordinate system to the world coordinate system, realizes automatic conversion and unification of the instrument coordinate system to the world coordinate system based on mathematical modeling of the rotation matrix and the translation matrix through joint measurement of multiple laser trackers, effectively suppresses single-point measurement error, and improves the measurement accuracy of the target pose.

[0012] Further, the S3 comprises the following steps: S301, obtaining composite constraints by constructing spatial point constraints, length constraints, plane constraints and circular trajectory constraints; S302, constructing an optimal equation set based on the LM optimization algorithm according to the composite constraints; S303, calibrating the pose parameters of the instrument coordinate system in the world coordinate system to obtain calibrated pose parameters; S304, minimizing each item error sum of squares in the optimal equation set by using a nonlinear least squares optimization method to obtain optimal system parameter estimates, and determining the initial value of the system parameters through spatial point cloud registration and coordinate system conversion; S305, in response to the calibrated pose parameters reaching a preset number, setting a residual function, and according to the optimal system parameter estimates, the initial value of the system parameters and the optimal equation set, solving the system parameters to be solved by approaching the residual function to zero to obtain the measurement pose, and completing the multi-laser tracker pose measurement.

[0013] The beneficial effects of the further scheme are that the application realizes high-precision self-calibration and pose measurement of system parameters in a large space range based on multiple sets of pose data of the target ball target coordinate system, and reduces the influence of the angle measurement error of a single laser tracker by constructing an optimal equation set to realize calculation of the system parameters.

[0014] Further, the S301 comprises the following steps: S3011, in response to the change of the pose of the target plane rigid body, measuring the first target ball by using the first laser tracker and the fourth laser tracker, obtaining a position constraint equation, and obtaining spatial point constraints; S3012, based on the first target ball, the second target ball and the third target ball being fixed on the target plane rigid body, obtaining the fixed distance between the target balls in any pose, setting a distance constraint equation, and obtaining length constraints; S3013. Based on the fact that the plane formed by the target sphere under the first laser tracker, the second laser tracker, and the third laser tracker is the same as the plane formed by the target sphere under the second laser tracker, the third laser tracker, and the fourth laser tracker, the plane constraint equation is obtained, and the plane constraint is obtained. S3014. In response to the rotation of the target ball on the turntable, the radius and roundness of the circle around the center of the turntable of each target ball are measured, and the circle constraint equation is set to obtain the circle trajectory constraint. S3015. Based on spatial point constraints, length constraints, planar constraints, and circular trajectory constraints, composite constraints are obtained by integrating them.

[0015] Furthermore, the expression for the position constraint equation is as follows: ; in, This represents the coordinates of the measurement point obtained by the first laser tracker in the world coordinate system. This represents the coordinates of the measurement point obtained by the fourth laser tracker in the world coordinate system. Indicates the first laser tracker A The selection matrix, The first target ball is represented in the first instrument coordinate system as the first target ball is... i Points of pose, This represents the transpose of the translation matrix of the first laser tracker. This represents the rotation matrix of the fourth laser tracker. This indicates the first target sphere in the fourth instrument coordinate system. i Points of pose, This represents the transpose of the translation matrix of the fourth laser tracker. Indicates the first i group pose; The expression for the distance constraint equation is as follows: ; in, Indicates the first i The distance between the first target ball and the second target ball in the group pose. Indicates all spacing The mean, Denotes the Euclidean norm. This represents the coordinates in the world coordinate system obtained from the measurement point of the second laser tracker. Indicates the first i The distance between the first and third target spheres in the group pose. Indicates all spacing The mean, This represents the coordinates in the world coordinate system obtained from the measurement point of the third laser tracker. the distance between the first target ball and the second target ball in the first group of poses, i the distance between the second target ball and the third target ball in the second group of poses, the distance between the first target ball and the third target ball in the third group of poses, the mean value of all the distances.

[0016] The above further scheme has the beneficial effect that the present application constructs a compound constraint by combining the multi-dimensional constraints of spatial points, lengths, planes and circular trajectories, different constraints adopt different construction methods, and multiple combinations are suitable for constraint construction in different fields, thereby improving the adaptability of pose measurement.

[0017] Further, the expression of the optimization equation set is as follows: ; wherein, the distance between the first target ball and the second target ball in the first group of poses, i the distance between the first target ball and the second target ball in the first group of poses, the distance between the second target ball and the third target ball in the second group of poses, i the distance between the second target ball and the third target ball in the second group of poses, the distance between the first target ball and the third target ball in the third group of poses, i the distance between the first target ball and the third target ball in the third group of poses, the optimized point coordinate equation of the first target ball measured by the first laser tracker and the fourth laser tracker in the first group of poses, i the optimized point coordinate equation of the first target ball measured by the first laser tracker and the fourth laser tracker in the first group of poses, the optimized plane equation constructed by the first laser tracker, the second laser tracker and the third laser tracker and the second laser tracker, the third laser tracker and the fourth laser tracker, respectively, in the second group of poses, i the small error of the first plane equation coefficient, the small error of the second plane equation coefficient, the small error of the second plane equation coefficient, the plane equation coefficient.

[0018] The above further scheme has the beneficial effect that the present application constructs an optimization equation set by combining the compound constraint and the LM algorithm, and makes the residual error close to zero through multi-pose data iteration, thereby significantly improving the measurement accuracy and the robustness of the pose measurement system. BRIEF DESCRIPTION OF DRAWINGS

[0019] Figure 1 The method flowchart of the present application.

[0020] Figure 2 The schematic diagram of the pose measurement system in the present embodiment.

[0021] Figure 3This is a diagram showing the coordinate system definition of the pose measurement system in this embodiment.

[0022] Figure 4 This is a schematic diagram of the target attitude measurement effect in this embodiment. Detailed Implementation

[0023] The specific embodiments of the present invention are described below to enable those skilled in the art to understand the present invention. However, it should be understood that the present invention is not limited to the scope of the specific embodiments. For those skilled in the art, various changes are obvious as long as they are within the spirit and scope of the present invention as defined and determined by the appended claims. All inventions utilizing the concept of the present invention are protected.

[0024] Before describing this embodiment, the following terms will be explained: LM optimization algorithm: Levenberg-Marquardt optimization algorithm.

[0025] Example like Figure 1 As shown, this invention provides a multi-laser tracker pose measurement method based on composite constraints, the implementation method of which is as follows: S1. Using a preset laser tracker, establish a pose measurement system, obtain the instrument coordinate system, establish a world coordinate system, set the target plane, and construct the target coordinate system by setting a preset number of target spheres. The specific steps are as follows: S101. Using four laser trackers, establish a pose measurement system and obtain the instrument coordinate system; S102. Based on the positional relationship of each laser tracker in the pose measurement system, set the coordinates of the first laser tracker as the origin, and set the line connecting the coordinates of the second laser tracker and the first laser tracker as the X-axis of the world coordinate system. S103. Set the plane formed by the coordinates of the first laser tracker, the second laser tracker and the third laser tracker as the XOY plane of the world coordinate system to obtain the Y-axis of the world coordinate system, and place the fourth laser tracker on the XOY plane of the world coordinate system. S104. Set the perpendicular line to the XOY plane as the Z-axis of the world coordinate system. Based on the set X-axis, Y-axis and Z-axis of the world coordinate system, establish the world coordinate system.

[0026] In this embodiment, as Figure 2 As shown, a pose measurement system is established using four laser trackers, and a world coordinate system is established based on their positional relationships. A For the first laser tracker, B For the second laser tracker, C For the third laser tracker, DThis is the fourth laser tracker; The first target ball, The second target ball. The primary target ball; like Figure 3 As shown, in the pose measurement system, a redundant observation network is constructed using four laser trackers, and the laser trackers are set up... A , B , C , D The instrument coordinate systems are respectively , , , ; The specific steps for establishing a world coordinate system are as follows: Based on the positional relationship of each laser tracker in the pose measurement system, the coordinates of the first laser tracker are set as the origin, the line connecting the coordinates of the second laser tracker and the first laser tracker is set as the X-axis of the world coordinate system, the plane formed by the coordinates of the first, second, and third laser trackers is set as the XOY plane of the world coordinate system, the Y-axis of the world coordinate system is obtained, and the fourth laser tracker is placed on the XOY plane of the world coordinate system. The perpendicular line to the XOY plane is set as the Z-axis of the world coordinate system. Based on the set X-axis, Y-axis, and Z-axis of the world coordinate system, the world coordinate system is established. The four laser trackers are configured A , B , C , D World coordinate system The coordinates are: Located at the origin , The X-axis of the world coordinate system is located in the world coordinate system. express, The XOY plane of the world coordinate system is used in the XOY plane of the world coordinate system. express, Located in the plane Above.

[0027] S105. Set the target plane and set three target spheres on the target plane. Set the coordinates of the first target sphere as the origin of the target coordinate system and set the line connecting the coordinates of the first target sphere and the second target sphere as the X-axis of the target coordinate system. S106. Set the plane formed by the coordinates of the first laser tracker, the second laser tracker and the third laser tracker as the XOY plane of the target coordinate system to obtain the Y-axis of the target coordinate system. S107, obtain a perpendicular line direction according to the XOY plane of the target coordinate system, set the perpendicular line as the Z axis of the target coordinate system, and construct the target coordinate system according to the set X axis, Y axis and Z axis of the target coordinate system.

[0028] In this embodiment, a target plane is provided and three target balls are arranged on the target plane 、 and for establishing a target coordinate system, taking the first target ball as the origin of the coordinate system, arranging the connecting line of the coordinates of the first target ball and the second target ball as the X axis of the target coordinate system, arranging the plane composed of the coordinates of the first laser tracker, the second laser tracker and the third laser tracker as the XOY plane of the target coordinate system, arranging the perpendicular line from the third laser tracker to the target plane as the Y axis direction of the target coordinate system, performing cross multiplication on the X axis of the target coordinate system and the Y axis direction vector of the target coordinate system to obtain the Z axis of the target coordinate system, and constructing the target coordinate system. Constructing the target coordinate system represents the pose of the measured target.

[0029] S2, according to the pose measurement system, track the target balls by using the laser trackers, measure the ball coordinates of the target balls in the instrument coordinate systems of the laser trackers, unify the coordinate systems of the ball coordinates to the world coordinate system based on the coordinate system transformation, and obtain the to-be-solved system parameters, and the specific steps are as follows: S201, according to the pose measurement system, measure the first target ball by using the first laser tracker and the fourth laser tracker based on the transformation formula from the spherical coordinate system to the Cartesian coordinate system, and obtain the first ball coordinates in the instrument coordinate system. S202, measure the second target ball by using the second laser tracker to obtain the second ball coordinates in the instrument coordinate system, and measure the third target ball by using the third laser tracker to obtain the third ball coordinates in the instrument coordinate system.

[0030] In this embodiment, according to the pose measurement system, the first target ball is tracked and measured by using the laser tracker A 、 D , the second target ball is tracked and measured by using the laser tracker , and the third target ball is tracked and measured by using the laser tracker B . C in the spherical coordinates of the respective instrument coordinate systems. Let the first target ball be in the first instrument coordinate system ​​​​​​​The next i The points of the pose group are Transformation formula from spherical coordinates to Cartesian coordinates ,get Similarly, the first target ball was obtained. In the fourth instrument coordinate system The next i Points of pose Second target ball Second instrument coordinate system The next i Points of pose Third target ball In the third instrument coordinate system The next i Points of pose .

[0031] S203. Based on the pose measurement system and coordinate system transformation, the pose transformation matrix from the instrument coordinate system to the world coordinate system is obtained, specifically: Based on the azimuth and elevation angles measured by the laser tracker in the pose measurement system, the rotation and translation matrices are obtained based on the coordinate system transformation. Then, based on the rotation and translation matrices, the attitude transformation matrix from the instrument coordinate system to the world coordinate system is obtained. S204. Using the attitude transformation matrix, the coordinates of the first target ball, the second target ball, and the third target ball are unified to obtain the system parameters to be determined.

[0032] In this embodiment, the system parameter features include the origin coordinates and angle transformation coefficients of the instrument coordinate system in the world coordinate system. Based on the pose measurement system, the ranging values ​​measured by the four laser trackers in their respective instrument coordinate systems, along with the azimuth and elevation angles measured by the optical encoder, are transformed into coordinate values ​​in the world coordinate system. The transformation process is based on coordinate system transformation, using fixed angles of rotation around the X, Y, and Z axes of the world coordinate system. , obtained from the first K Instrument coordinate system To the world coordinate system rotation matrix The expression, Indicates each laser tracker; The rotation matrix The expression is as follows: ; in, Represents world coordinates, Indicates each laser tracker, These represent the fixed angles of rotation along the X, Y, and Z axes of the world coordinate system, respectively. The position of each laser tracker in the world coordinate system is obtained, and the translation matrix from the instrument coordinate system to the world coordinate system is obtained, and the expression is as follows: K instrument coordinate system to the world coordinate system The expression is as follows: , , , ; wherein, represents the translation matrix of the first laser tracker, represents the translation matrix of the second laser tracker, represents the translation matrix of the third laser tracker, represents the translation matrix of the fourth laser tracker, represents the to-be-solved coordinate of the X axis of the second laser tracker, represents the to-be-solved coordinate of the X axis of the third laser tracker, represents the to-be-solved coordinate of the Y axis of the third laser tracker, represents the to-be-solved coordinate of the X axis of the fourth laser tracker, represents the to-be-solved coordinate of the Y axis of the fourth laser tracker, represents the to-be-solved coordinate of the Z axis of the fourth laser tracker. According to the rotation matrix and the translation matrix, the attitude transformation matrix of the instrument coordinate system to the world coordinate system is obtained. And the first target ball coordinate, the second target ball coordinate and the third target ball coordinate are unified in coordinates by using the attitude transformation matrix, and the to-be-solved system parameters are obtained. The to-be-solved system parameters include: 12 to-be-solved items of fixed angles representing each laser tracker, and , , , , , , six to-be-solved items of and a total of 18 to-be-solved system parameters.

[0033] S3, construct a composite constraint, and based on an optimization algorithm, construct an optimization equation set, calibrate the pose parameters of the instrument coordinate system in the world coordinate system, and solve the to-be-solved system parameters to obtain a measurement pose, complete the pose measurement of multiple laser trackers, and the specific steps are as follows: S301, by constructing a space point constraint, a length constraint, a plane constraint and a circular trajectory constraint, a composite constraint is obtained, and the specific steps are as follows: S3011, in response to the target plane being in the rigid body changing pose, measuring the first target ball by using the first laser tracker and the fourth laser tracker, measuring the position constraint equation, and obtaining the space point constraint; S3012, based on the first target ball, the second target ball, and the third target ball being fixed on the target plane being in the rigid body, obtaining the fixed distance between the target balls in any attitude, setting the distance constraint equation, and obtaining the length constraint; S3013, based on the target ball constituting a plane under the first laser tracker, the second laser tracker, and the third laser tracker being the same as the target ball constituting a plane under the second laser tracker, the third laser tracker, and the fourth laser tracker, obtaining the plane constraint equation, and obtaining the plane constraint; S3014, in response to the target ball rotating on the turntable, measuring the circle radius and the roundness of each target ball rotating around the center of the turntable, setting the circle constraint equation, and obtaining the circle trajectory constraint; S3015, integrating the composite constraint according to the space point constraint, the length constraint, the plane constraint, and the circle trajectory constraint.

[0034] In this embodiment, the composite constraint condition is constructed by various constraints such as circle constraint (roundness and radius), plane, length, and space point, and the pose parameter calibration of the instrument coordinate system in the world coordinate system is based on the optimization algorithm: different construction methods of different constraints are used: For the space point constraint, the target plane being in the rigid body changes a pose, and the first laser tracker A and the fourth laser tracker D jointly measure the first target ball , and construct the position constraint equation, which is expressed as follows: ; Wherein, represents the coordinates in the world coordinate system obtained by the first laser tracker A measurement point, represents the coordinates in the world coordinate system obtained by the fourth laser tracker D measurement point, represents the selection matrix of the first laser tracker A , represents the first target ball in the first instrument coordinate system, i represents the translation matrix of the first laser tracker A , represents the rotation matrix of the fourth laser tracker D , represents the first target ball in the fourth instrument coordinate system, i ​Points of pose, Indicates the fourth laser tracker D The transpose of the translation matrix, Indicates the first i group pose; Regarding the length constraint, based on the first target ball... Second target ball And the third target ball The distance between target spheres in any orientation, fixed on a rigid body containing the target plane. , , It does not change with pose, therefore a distance constraint can be constructed, the expression of which is as follows: ; in, Indicates the first i First target ball in group pose With the second target ball The spacing, Indicates all spacing The mean, Denotes the Euclidean norm. Indicates the second laser tracker B The coordinates of the measurement point in the world coordinate system Indicates the first i First target ball in group pose With the third target ball The spacing, Indicates all spacing The mean, Indicates the third laser tracker C The coordinates of the measurement point in the world coordinate system Indicates the first i Second target ball in group pose With the third target ball The spacing, Indicates all spacing The mean.

[0035] In this embodiment, for planar constraints, the three target spheres are in the first laser tracker A Second laser tracker B and the third laser tracker C The plane formed by the second laser tracker, the target sphere B Third laser tracker C and the fourth laser tracker D Since the planes formed below are identical, planar constraints can be constructed, as shown in the following expression: ; wherein, , , , all represent plane equation coefficients of the first laser tracker A, the second laser tracker B and the third laser tracker C, represents an X-axis parameter in the plane constraint, represents a Y-axis parameter in the plane constraint, represents a Z-axis parameter in the plane constraint, , , , all represent plane equation coefficients of the second laser tracker B , the third laser tracker C and the fourth laser tracker D ; For the circular trajectory constraint, the radii of the circles in which the three target balls rotate around the center of the turntable and the circularities of the circles can be constructed as the circular constraint when the turntable rotates. According to the spatial point constraint, the length constraint, the plane constraint and the circular trajectory constraint, the composite constraint is integrated.

[0036] S302, according to the composite constraint, an optimization equation set is constructed based on the LM optimization algorithm; S303, the pose parameters of the instrument coordinate system in the world coordinate system are calibrated to obtain calibrated pose parameters; S304, using a nonlinear least squares optimization method, each partial error sum of squares in the optimization equation set is minimized to obtain an optimal system parameter estimate value, and through spatial point cloud registration and coordinate system conversion, an initial value of the system parameter is determined; S305, in response to the calibrated pose parameters reaching a preset number, a residual function is set, and according to the optimal system parameter estimate value, the initial value of the system parameter and the optimization equation set, the system parameter to be solved is solved by approaching the residual function to zero to obtain a measurement pose, and the multi-laser tracker pose measurement is completed.

[0037] In this embodiment, according to the composite constraint, when the number of measured poses is sufficient, an optimization equation set is constructed to reduce the influence of the angle measurement error of a single laser tracker and realize the calculation of the system parameter; The pose measurement system introduces four-station redundant information, and optimizes calculation based on the LM algorithm under multi-dimensional constraint, thereby improving the fault tolerance and robustness of the pose measurement system to abnormal data; According to the composite constraint, an optimization equation set is constructed based on the LM optimization algorithm, and its expression is as follows: ; wherein, represents the firsti In the group position, the target ball spacing The constructed optimal distance equation, Indicates the first i In the group position, the target ball spacing The constructed optimal distance equation, Indicates the first i In the group position, the target ball spacing The constructed optimal distance equation, Indicates the first i The first laser tracker is used to track the pose of the group. A and the fourth laser tracker D Measure the first target ball The constructed optimal point coordinate equation, Indicates the first i The first laser tracker is used to track the pose of the group. A Second laser tracker B Third laser tracker C With the second laser tracker B Third laser tracker C Fourth laser tracker D The optimal plane equations for each plane are constructed. This indicates a small error in the coefficients of the first plane equation. This indicates a small error in the coefficients of the second plane equation. Denotes the coefficients of the plane equation; The position and pose parameters of the instrument coordinate system in the world coordinate system are calibrated to obtain the calibrated position and pose parameters. The nonlinear least squares optimization method is used to minimize the sum of squares of each component error to obtain the optimal system parameter estimate, thereby effectively suppressing calibration and measurement errors caused by ranging deviation, angle deviation, and random errors in various constraints. The initial values ​​of the parameters are determined through spatial point cloud registration and coordinate system transformation; Based on the optimization equations, in response to the condition that the calibration pose parameters reach a sufficient number of measurement poses, i.e., when the number of optimization equations exceeds the number of unknown system parameters to be calibrated, the number of unknown system parameters to be calibrated is related to the constraint selection and the construction of the optimization equation set. The residual function is set, and its expression is as follows: ; in, Represents the residual. This represents the transpose of the optimization equation. Represent the optimization equation; And according to the optimal system parameter estimation value, the system parameter initial value and the optimization equation set, the limit case is solved by approaching the residual function to zero, the system parameters to be solved are obtained, the measurement values of all 18 system parameters are obtained, and the measurement pose is obtained according to the measurement value of the system parameter; Based on the LM optimization algorithm, the optimal solution of the regression parameters of the nonlinear equation set under the least square criterion is solved, the measurement system parameter autonomous calibration process of the fusion composite constraint condition is completed, and the effect schematic diagram as shown in Figure 4 is obtained.

[0038] In the embodiment, the measurement pose is completed, the target pose matrix in the world coordinate system can be further calculated, the rotation angle turned during the attitude transformation is obtained through the rotation transformation matrices of two different attitudes, and the rotation matrix of the first i group pose i and the rotation matrix of the second j group pose is and , then the rotation transformation matrix is , and the expression of the rotation angle is as follows: ; Among them, , represents the rotation transformation matrix, , represents the trace of the rotation transformation matrix; The rotation angle turned during the attitude transformation calculated is compared with the reference value provided by the turntable, the attitude measurement accuracy is evaluated, and the pose measurement system is adjusted.

[0039] In the embodiment, the reasons why the prior art cannot achieve the same effect as the present application are as follows: first, the traditional multi-edge pose measurement relies on a single constraint (such as a space point constraint), and the cumulative error and linear error elimination effect in multi-instrument cooperative measurement are poor; second, the seven-station tracking interferometer system has high complexity and high cost; third, the conventional optimization algorithm (such as the least square method) is sensitive to the initial value, and the composite constraint combined with the L-M algorithm in the present application makes the residual error approach to zero through multi-attitude data iteration, which significantly improves the measurement accuracy and system robustness.

[0040] Therefore, the present application has the following advantages: through the composite constraint, the number of devices is reduced, the system parameter high-precision self-calibration is realized, the measurement range of the target pose is large, and the measurement accuracy is improved.

Claims

1. A method for multi-laser tracker pose measurement based on compound constraints, characterized in that, The method comprises the following steps: S1, establishing a pose measurement system by using a preset laser tracker, obtaining an instrument coordinate system, establishing a world coordinate system, setting a target plane, and constructing a target coordinate system by setting a preset number of target balls; S2, tracking the target balls by using the laser tracker according to the pose measurement system, measuring the ball coordinates of the target balls in the instrument coordinate system of each laser tracker, unifying the coordinate systems of the ball coordinates to the world coordinate system based on the coordinate system transformation, obtaining the system parameters to be solved, and measuring the pose; S3, constructing a compound constraint, constructing an optimal equation set based on an optimization algorithm, calibrating the pose parameters of the instrument coordinate system in the world coordinate system, solving the system parameters to be solved, obtaining the measurement pose, and completing the pose measurement of the multiple laser trackers.

2. The multi-laser tracker pose measurement method based on compound constraints according to claim 1, wherein, The S1 comprises the following steps: S101, establishing a pose measurement system by using four laser trackers, and obtaining an instrument coordinate system; S102, setting the coordinate of the first laser tracker as the origin and setting the line connecting the first laser tracker and the second laser tracker as the X-axis of the world coordinate system based on the positional relationship of the laser trackers in the pose measurement system; S103, setting the plane composed of the coordinates of the first laser tracker, the second laser tracker and the third laser tracker as the XOY plane of the world coordinate system, obtaining the Y-axis of the world coordinate system, and making the fourth laser tracker located on the XOY plane of the world coordinate system; S104, setting the perpendicular line perpendicular to the XOY plane as the Z-axis of the world coordinate system, and establishing the world coordinate system according to the set X-axis, Y-axis and Z-axis of the world coordinate system; S105, setting a target plane and three target balls on the target plane, setting the coordinate of the first target ball as the origin of the target coordinate system, and setting the line connecting the first target ball and the second target ball as the X-axis of the target coordinate system; S106, setting the plane composed of the coordinates of the first laser tracker, the second laser tracker and the third laser tracker as the XOY plane of the target coordinate system, and obtaining the Y-axis of the target coordinate system; S107, obtaining the direction of the perpendicular line according to the XOY plane of the target coordinate system, setting the perpendicular line as the Z-axis of the target coordinate system, and constructing the target coordinate system according to the set X-axis, Y-axis and Z-axis of the target coordinate system.

3. The multi-laser tracker pose measurement method based on compound constraints of claim 2, wherein, The S2 comprises the following steps: S201, measuring the first target ball by using the first laser tracker and the fourth laser tracker according to the pose measurement system based on the transformation formula from the spherical coordinate system to the Cartesian coordinate system, and obtaining the first ball coordinate in the instrument coordinate system; S202, measuring the second target ball by using the second laser tracker, obtaining the second ball coordinate in the instrument coordinate system, and measuring the third target ball by using the third laser tracker, obtaining the third ball coordinate in the instrument coordinate system; S203, obtaining the attitude transformation matrix from the instrument coordinate system to the world coordinate system based on the coordinate system transformation according to the pose measurement system; S204, unifying the coordinates of the first ball coordinate, the second ball coordinate and the third ball coordinate by using the attitude transformation matrix, and obtaining the system parameters to be solved.

4. The multi-laser tracker pose measurement method based on compound constraints of claim 3, wherein, The S203 is specifically: According to the azimuth and elevation values in the coordinate system measured by the laser tracker in the pose measurement system, a rotation matrix and a translation matrix are obtained based on coordinate system transformation, and a pose transformation matrix from the instrument coordinate system to the world coordinate system is obtained according to the rotation matrix and the translation matrix.

5. The multi-laser tracker pose measurement method based on compound constraints according to claim 4, characterized in that, The expression of the rotation matrix is as follows: wherein, R represents a rotation matrix, XW represents a world coordinate, LTI represents each laser tracker, respectively represent fixed angles of rotation of the world coordinate system X, Y, and Z axes. The expression of the translation matrix is as follows: ， ， ， wherein, T1represents the translation matrix of the first laser tracker, T2represents the translation matrix of the second laser tracker, T3represents the translation matrix of the third laser tracker, T4represents the translation matrix of the fourth laser tracker, X2represents the unknown coordinate on the X-axis of the second laser tracker, X3represents the unknown coordinate on the X-axis of the third laser tracker, Y3represents the unknown coordinate on the Y-axis of the third laser tracker, X4represents the unknown coordinate on the X-axis of the fourth laser tracker, Y4represents the unknown coordinate on the Y-axis of the fourth laser tracker, Z4represents the unknown coordinate on the Z-axis of the fourth laser tracker.

6. The multi-laser tracker pose measurement method based on compound constraints of claim 2, wherein, The S3 includes the following steps: S301, obtaining a composite constraint by constructing a space point constraint, a length constraint, a plane constraint and a circular trajectory constraint; S302, constructing an optimization equation set based on the LM optimization algorithm according to the composite constraint; S303, calibrating the pose parameters of the instrument coordinate system in the world coordinate system to obtain calibrated pose parameters; S304, using a nonlinear least squares optimization method to minimize the sum of squares of each item error in the optimization equation set to obtain an optimal system parameter estimate value, and determining an initial value of the system parameter through space point cloud registration and coordinate system conversion; S305, in response to the calibrated pose parameters reaching a preset number, setting a residual function, and according to the optimal system parameter estimate value, the initial value of the system parameter and the optimization equation set, solving the to-be-solved system parameter by approaching the residual function to zero to obtain a measurement pose, and completing the multi-laser tracker pose measurement.

7. The multi-laser tracker pose measurement method based on compound constraints of claim 6, wherein, The S301 includes the following steps: S3011, in response to the changing pose of the rigid body on which the target plane is located, measuring a first target ball using the first laser tracker and the fourth laser tracker to obtain a position constraint equation and obtain a space point constraint; S3012, based on the first target ball, the second target ball and the third target ball being fixed on the rigid body on which the target plane is located, obtaining a fixed distance between the target balls in any attitude, setting a distance constraint equation, and obtaining a length constraint; S3013, based on the planes formed by the target balls under the first laser tracker, the second laser tracker and the third laser tracker being the same as the planes formed by the target balls under the second laser tracker, the third laser tracker and the fourth laser tracker, obtaining a plane constraint equation, and obtaining a plane constraint; S3014, in response to the target balls rotating around the turntable, measuring the radii and circularities of the rotations of the target balls around the center of the turntable to set a circular constraint equation and obtain a circular trajectory constraint; S3015, integrating the space point constraint, the length constraint, the plane constraint and the circular trajectory constraint to obtain a composite constraint.

8. The multi-laser tracker pose measurement method based on compound constraints of claim 7, wherein, The expression of the position constraint equation is as follows: in, This represents the coordinates of the measurement point obtained by the first laser tracker in the world coordinate system. This represents the coordinates of the measurement point obtained by the fourth laser tracker in the world coordinate system. This represents the selection matrix for the first laser tracker. The first target ball is represented in the first instrument coordinate system as the first target ball is... i Points of pose, This represents the transpose of the translation matrix of the first laser tracker. This represents the rotation matrix of the fourth laser tracker. This indicates the first target sphere in the fourth instrument coordinate system. i Points of pose, This represents the transpose of the translation matrix of the fourth laser tracker. Indicates the first i group pose; The expression of the distance constraint equation is as follows: wherein, represents the distance between the first target ball and the second target ball in the first set of poses, i represents the distance between the first target ball and the second target ball in the second set of poses, represents the mean of all distances represents the mean of all distances represents the Euclidean norm, represents the coordinates in the world coordinate system obtained from the second laser tracker measurement point, represents the distance between the first target ball and the third target ball in the first set of poses, i represents the distance between the first target ball and the third target ball in the second set of poses, represents the mean of all distances represents the mean of all distances represents the coordinates in the world coordinate system obtained from the third laser tracker measurement point, represents the distance between the second target ball and the third target ball in the first set of poses, i represents the distance between the second target ball and the third target ball in the second set of poses, represents the mean of all distances represents the mean of all distances 9. The multi-laser tracker pose measurement method based on compound constraints of claim 8, wherein, The expression of the optimization equation set is as follows: in, Indicates the first i The optimal distance equation is constructed based on the distance between the first target ball and the second target ball under the given pose. Indicates the first i The optimal distance equation is constructed based on the distance between the second and third target spheres under the given pose. Indicates the first i The optimal distance equation is constructed based on the distance between the first and third target spheres under the given pose. Indicates the first i The optimal point coordinate equation is constructed by measuring the first target sphere using the first and fourth laser trackers under the group pose. Indicates the first i Under the given pose, the optimal plane equations for the plane construction are formed by the first, second, and third laser trackers, and the second, third, and fourth laser trackers, respectively. This indicates a small error in the coefficients of the first plane equation. This indicates a small error in the coefficients of the second plane equation. Represents the coefficients of the plane equation.

Images