Photogrammetry joint calibration method and system based on hierarchical graph constraints

Abstract

The application discloses a kind of based on layered graph constraint photogrammetry joint calibration method and system, method includes: using robot to carry face structure light scanner and form robot measurement system, multiple acquisition standard sphere point cloud;Based on face structure light point cloud registration, the relative pose relationship between nodes is constructed, the Euclidean distance constraint between nodes is constructed using standard ball stick system, and is combined into layered graph constraint;Optimize pose matrix and calibration parameter simultaneously in Lie group space using sequential quadratic programming algorithm, minimize algebraic error and geometric error;In photogrammetry system, the marker system including target ball cage and standard ball stick is built to verify, and optimization process is introduced into hierarchical graph constraint joint optimization model optimization framework, realize the global optimization of pose matrix and calibration parameter.The application realizes high-precision joint optimization of body, face structure light outside and calibration parameter in photogrammetry system, improves calibration efficiency and the position accuracy of system.

Description

Technical Field

[0001] This invention belongs to the field of photogrammetry technology, and more specifically, relates to a photogrammetric joint calibration method and system based on layered map constraints. Background Technology

[0002] In photogrammetry systems, sensor calibration aims to establish the transformation relationship between the poses of multiple sensors and a unified coordinate system, broadening the sensor's field of view and enabling accurate calculation of robot pose data. The calibration problem can be represented as AX = YB, and accurately solving AX = YB is crucial for ensuring the accuracy of data fusion and robot tracking in photogrammetry systems.

[0003] Traditional calibration methods mainly improve calibration accuracy and robustness in two aspects: (1) improving the solution framework (including analytical solutions, semidefinite programming, nonlinear optimization techniques and probabilistic models); (2) modeling the error propagation of robot and sensor noise. The calibration problem is defined on the homogeneous matrix SE(3) group or rotation matrix SO(3) space. The solution of the problem can be divided into: 1) parameterization of the rotation matrix or homogeneous matrix; 2) solving the problem based on the established parameterized model. One type of parameterization method simplifies the problem to a linear least squares model and obtains a closed solution of the calibration matrix. For example, rotation angle and rotation vector, dual quaternion, Clifford algebra, motion spinor, Euler group, dual tensor, 4D Prouk, vectorization, geometric algebra, etc. The above methods can be classified into separate and simultaneous solution methods. Although the simultaneous solution process can avoid the problem of the rotation matrix error being passed to the translation vector solution in the separate solution, and the closed solution is fast, it introduces the error caused by the process of mapping the closed solution from the vector space back to the original space of the rotation matrix or homogeneous matrix (SO(3) or SE(3)). Moreover, the closed solution is in least squares form, and its solution accuracy is sensitive to noise. In order to improve the solution accuracy, the closed solution is usually used as the initial value for subsequent optimization processes. There are two types of optimization objectives for optimization problems. The first type is to minimize algebraic error; the second type is to minimize geometric error. Minimizing algebraic error means minimizing the metric defined in the SO(3) or SE(3) space, such as minimizing chord length error, minimizing geodesic distance, quaternion distance, etc. Minimizing geometric error means minimizing the metric with real geometric meaning, such as projection error, plane projection error, geometric error defined by epipolar error and spatial distance constraints, and geometric error defined by point-to-point distance in the corresponding point set.

[0004] Although these methods can achieve high precision, they cannot guarantee global optimization and have the following limitations: (1) The noise error of the robot kinematics matrix and sensor extrinsic parameters will be directly transmitted to the calibration result, resulting in limited accuracy and uncontrollable error propagation; (2) Without joint optimization of pose matrix A and sensor extrinsic parameter matrix B, it is difficult to compensate for the error globally, resulting in low calibration accuracy. Summary of the Invention

[0005] To address the aforementioned deficiencies or improvement needs of existing technologies, this invention provides a photogrammetric joint calibration method based on layered graph constraints. This method involves repeatedly acquiring point clouds of a standard sphere using a robot equipped with a structured light scanner. Point cloud registration is used to construct the relative pose relationships between nodes, and Euclidean distance constraints are constructed using the known high-precision physical length of the standard ball. These two methods are then fused to form a layered graph constraint model. A sequential quadratic programming algorithm is employed to simultaneously optimize the pose matrices A and B and the calibration parameters X and Y within a Lie group space, minimizing algebraic and geometric errors. A marker system containing a target sphere cage and a standard ball is built within the photogrammetric system for verification, and the optimization process is incorporated into the HGC-JPO optimization framework. This method solves the problems of insufficient constraint information, low joint optimization accuracy, and poor convergence efficiency in existing methods. It achieves high-precision collaborative optimization of the photogrammetric system calibration parameters, effectively reducing the solution space, improving calibration stability, and accelerating convergence speed, thereby significantly improving the system's positional accuracy and measurement reliability.

[0006] To achieve the above objectives, this invention provides a photogrammetric joint calibration method based on layered map constraints, comprising the following steps:

[0007] S100: A robotic measurement system consisting of a robot equipped with a structured light scanner is used to collect point clouds of a standard sphere multiple times.

[0008] S200: Based on the registration of surface structured light point cloud, the relative pose relationship between nodes is constructed, and the Euclidean distance constraint between nodes is constructed using the standard ball-and-stick system and merged into a layered graph constraint.

[0009] S300: Employs a sequential quadratic programming algorithm to simultaneously optimize the pose matrix and calibration parameters in Lie group space, minimizing algebraic and geometric errors;

[0010] S400: A marker system including a target cage and a standard bat is built in the photogrammetry system for verification, and the optimization process is introduced into a joint optimization framework based on hierarchical graph constraints to achieve global optimization of the pose matrix and calibration parameters.

[0011] Further, in step S200, the hierarchical graph constraint includes the following steps:

[0012] S210: Input the pose graph constraints and Euclidean graph constraints, as well as the scaled initial pose hand-eye matrix X, robot-world matrix Y, robot pose and sensor extrinsic parameter matrix;

[0013] S220: Use Lie algebra to transform the pose variables into vectors in Euclidean space, and realize the first-order Taylor expansion of the error function;

[0014] S230: The Lagrange multiplier method is used to process the Euclidean graph constraints;

[0015] S240: Iteratively update the pose variables until the error converges or the number of iterations is satisfied, and extract the hierarchical graph constraint parameters from the solution.

[0016] Furthermore, the pose graph constraint G pg and Euclidean distance constraint G ed Represented as:

[0017]

[0018] In the formula, P i To calibrate the target ball, T ij P i =P j For point cloud matching, A is the robot pose matrix, B is the sensor extrinsic parameter matrix, X is the initial pose hand-eye matrix, Y is the robot-world matrix, and O is the G... ed Each node k in the diagram corresponds to a 3D point obtained at pose k, d kl For each edge (k,l), represent O k and O l The prior Euclidean distance between them.

[0019] Furthermore, the constrained pose graph optimization problem is transformed into:

[0020]

[0021] st|α k O k -α l O l |=d kl (k,l)∈G ed

[0022] In the formula, α is an intermediate variable.

[0023] Furthermore, in step S300, solving the joint optimization framework based on hierarchical graph constraints using the sequential quadratic programming algorithm includes the following steps:

[0024] S310: Input initial parameters, pose graph constraints, layer graph constraints, scale parameters;

[0025] S320: Call the scale function to normalize the scale of all homogeneous matrices and obtain the normalized initial pose.

[0026] S330: Set the iteration number k = 0 and start the first iteration;

[0027] S340: Construct the objective function and constraint functions;

[0028] S350: Use a sequential quadratic programming algorithm to solve for the pose update amount and achieve iterative update;

[0029] S360: Determine if ||Ξ|| < 10 is satisfied. -9 or k < k max If the condition is not met, continue iterating; otherwise, restore the optimal pose under the normalized scale to the original scale.

[0030] Furthermore, the optimization problem can be restated as:

[0031]

[0032] Among them, intermediate variables For the relative transformation between anchor point pose f and pose i;

[0033] It can be defined as

[0034]

[0035] f lasso The lasso function is defined as follows:

[0036]

[0037] The symbol ~ represents the initial input value.

[0038] Furthermore, the physical length of the standard bat serves as a rigid distance constraint, accelerating iterative convergence.

[0039] Furthermore, the layered graph constraint includes two parts: pose graph constraint and Euclidean distance constraint. By integrating the two types of constraint information through the layered graph constraint, the accuracy of the initial pose hand-eye matrix X and robot-world matrix Y can be improved.

[0040] Furthermore, the sequential quadratic programming algorithm uniformly processes rotation and translation components on the SE manifold, avoiding parameterized projection errors.

[0041] A photogrammetric joint calibration system based on layered map constraints, characterized in that it includes:

[0042] The point cloud acquisition module uses a robot equipped with a surface structured light scanner to form a robot measurement system, which acquires point clouds of a standard sphere multiple times.

[0043] The layered graph constraint module is used to construct the relative pose relationship between nodes based on the registration of surface structured light point clouds, and to construct the Euclidean distance constraint between nodes using the standard ball-and-stick system, which is then merged into a layered graph constraint.

[0044] The error optimization module is used to simultaneously optimize the pose matrix and calibration parameters in the Lie group space using a sequential quadratic programming algorithm, minimizing algebraic and geometric errors.

[0045] The global optimization module is used to build a marker system including a target cage and a standard bat in the photogrammetry system for verification, and to introduce the optimization process into a joint optimization framework based on hierarchical graph constraints to achieve global optimization of the pose matrix and calibration parameters.

[0046] In summary, compared with the prior art, the above-described technical solutions conceived by this invention can achieve the following beneficial effects:

[0047] 1. The method of this invention involves repeatedly acquiring point clouds of a standard sphere using a robot equipped with a structured light scanner. Point cloud registration is used to construct relative pose relationships between nodes, and Euclidean distance constraints are constructed based on the known high-precision physical length of the standard ball. These two constraints are then fused to form a layered graph constraint model. A sequential quadratic programming algorithm is employed to simultaneously optimize the pose matrices A and B and the calibration parameters X and Y within the Lie group space, minimizing algebraic and geometric errors. A marker system containing the target ball cage and the standard ball is built within a photogrammetric system for verification, and the optimization process is incorporated into the HGC-JPO optimization framework. This method solves the problems of insufficient constraint information, low joint optimization accuracy, and poor convergence efficiency in existing methods. It achieves high-precision collaborative optimization of the calibration parameters of the photogrammetric system, effectively reducing the solution space, improving calibration stability, and accelerating convergence speed, thereby significantly improving the system's positional accuracy and measurement reliability.

[0048] 2. The method of this invention employs Pose Graph Constraints (PGCs) and Euclidean Distance Constraints (EDCs) to construct a hierarchical constraint mechanism. PGCs, based on sensor point cloud registration, can reflect the relative transformation relationships between different acquired poses, providing topological association information of spatial poses. Euclidean Distance Constraints (EDCs), on the other hand, utilize the known high-precision physical length of a standard baseball bat to provide rigid spatial distance constraints between nodes. These two constraints complement each other in terms of both geometric transformation and spatial distance, resulting in higher information richness.

[0049] 3. The method of the present invention uses a multi-objective fusion optimization framework of Lie group space and a sequential quadratic programming algorithm to uniformly process rotation and translation components on the SE(3) manifold, avoiding errors caused by parametric projection. At the same time, the sequential quadratic programming algorithm can efficiently solve optimization problems under complex constraints, thereby significantly improving the stability of the calibration process and making the results more reliable.

[0050] 4. The method of this invention utilizes the physical length of a standard baseball bat as a rigid distance constraint. Since the length of the standard baseball bat is a known, high-precision constant, the point clouds of its two spheres can provide strict Euclidean distance constraints between nodes. This rigid constraint can effectively reduce the solution space during the optimization process, provide a clear convergence direction for the optimization algorithm, and improve calibration accuracy. Attached Figure Description

[0051] Figure 1 This is a flowchart of the photogrammetric calibration process according to an embodiment of the present invention;

[0052] Figure 2 This is a flowchart illustrating the calculation of layered graph constraints according to an embodiment of the present invention.

[0053] Figure 3 This is a flowchart of the HGC–JPO solution based on the sequential quadratic programming algorithm in an embodiment of the present invention;

[0054] Figure 4 This is a schematic diagram of the calibration error results of the photogrammetric system according to an embodiment of the present invention;

[0055] Figure 5 This is a schematic diagram of the positional accuracy on the test set and training set in the photogrammetry system of this invention.

[0056] Figure 6(a) is a schematic diagram of the robot photogrammetry system according to an embodiment of the present invention;

[0057] Figure 6(b) is a schematic diagram of the relationship AX = YB in the robot photogrammetry system of an embodiment of the present invention. Detailed Implementation

[0058] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention. Furthermore, the technical features involved in the various embodiments of this invention described below can be combined with each other as long as they do not conflict with each other.

[0059] In the description of the embodiments of the present invention, words such as "for example" or "for instance" are used to indicate examples, illustrations, or explanations. Any embodiment or design described as "for example" or "for instance" in the embodiments of the present invention should not be construed as being more preferred or advantageous than other embodiments or designs. Rather, the use of words such as "for example" or "for instance" is intended to present the relevant concepts in a specific manner.

[0060] In the description of embodiments of the present invention, the term "multiple" means two or more. For example, multiple systems means two or more systems, and multiple screen terminals means two or more screen terminals. The terms "comprising," "including," "having," and variations thereof all mean "including but not limited to," unless otherwise specifically emphasized.

[0061] like Figure 1 As shown, this embodiment of the invention provides a photogrammetric joint calibration method based on layered map constraints, comprising the following steps:

[0062] S100: A robotic measurement system consisting of a robot equipped with a structured light scanner is used to collect point clouds of a standard sphere multiple times.

[0063] Structured light scanners, as high-precision 3D measurement devices, work by projecting structured light of a specific pattern onto a target surface, then capturing the deformation information of the reflected light through a camera, and finally calculating the target's 3D coordinates. However, limited by the imaging range of the optical system and the single projection area, a single measurement can only cover a limited spatial area. For photogrammetric systems that require global calibration, this limitation leads to the loss of feature data in some areas, making it difficult to establish complete spatial pose relationships.

[0064] To overcome this limitation, this invention employs a six-DOF robot equipped with a surface structured light scanner. The robot's multi-joint design enables flexible movement of the end effector in three-dimensional space, including translation, rotation, and other multi-DOF adjustments. Through preset paths or manual control, the robot can drive the scanner to scan a standard sphere from different positions and angles, thereby expanding the local field of view of a single measurement to global spatial coverage. This ensures that all areas of the standard sphere are effectively captured, providing a data foundation for subsequently constructing a global surface pose graph constraint.

[0065] Meanwhile, multiple data acquisitions are crucial for improving data reliability. During a single measurement, factors such as ambient light interference, scanner sensor noise, and mechanical vibration can introduce random errors, leading to deviations in the point cloud data. By repeatedly acquiring data, statistical properties can be used to offset some of these random interferences: for example, averaging multiple acquisitions of the same sphere's point cloud data can significantly reduce the impact of random errors, making characteristic parameters such as the sphere's center coordinates and surface texture closer to their true values. This redundant data not only improves the signal-to-noise ratio of the original data but also provides a sufficient and robust data source for subsequent constraint calculations and parameter optimization, reducing calibration deviations caused by insufficient data or excessive noise.

[0066] Furthermore, the choice of a standard sphere as a calibration benchmark has clear technical rationale: its geometric features are highly uniform—its surface has no complex textures and consists only of a spherical surface with constant curvature, which makes feature extraction simpler and matching accuracy higher during point cloud registration; at the same time, the spatial constraints of the sphere are stable. Under different poses and lighting conditions, the distance from its center to any point on its surface is always equal to the radius, and parameters such as the distance between the centers of multiple spheres in an array or ball-and-stick system also remain constant. This stable geometric characteristic enables it to provide a reliable reference system for robot pose calculation and sensor extrinsic parameter calibration, ensuring the accuracy of various constraint relationships during the calibration process.

[0067] S200: Based on the registration of surface structured light point cloud, the relative pose relationship between nodes is constructed, and the Euclidean distance constraint between nodes is constructed using the standard ball-and-stick system and merged into a layered graph constraint.

[0068] After data acquisition, the spatial relationships between different nodes need to be preliminarily determined through point cloud registration. However, the constraints of point cloud registration alone are easily affected by errors such as point cloud noise and weak feature matching. The physical length of the standard bat is a known, high-precision constant, and the point clouds of its two spheres can provide rigid constraints on the Euclidean distance between nodes. This rigid constraint can effectively reduce the solution space during optimization, providing a clear convergence direction for the optimization algorithm and improving calibration accuracy. Therefore, integrating the geometric transformation and spatial distance dimensions into a hierarchical graph constraint can provide complementary and more robust constraints for subsequent multi-parameter optimization.

[0069] The joint optimization constraints of this invention comprise two parts: pose graph constraints and Euclidean distance constraints. These two types of constraint information are integrated through layered graph constraints to improve the accuracy of X and Y coordinates. For example... Figure 2 As shown, step S200 specifically includes:

[0070] S210: Input pose graph constraints G pg and Euclidean graph constraint G ed and the scaled initial pose hand-eye matrix X, robot-world matrix Y, robot pose A i and sensor extrinsic matrix Bi ;

[0071] S220: Use Lie algebra to transform the pose variables into vectors in Euclidean space, and realize the first-order Taylor expansion of the error function;

[0072] S230: Applying the Lagrange multiplier method to constrain the Euclidean graph G ed Process it;

[0073] S240: Iteratively update the pose variables until the error converges or the number of iterations is satisfied, and extract the hierarchical graph constraint parameters from the solution.

[0074] To better explain the subsequent process of the algorithm, this invention first defines the relevant mathematical symbols:

[0075] Representation of special orthogonal group SO(3) and special Euclidean group SE(3):

[0076] Lie groups and Lie algebras are tools for representing rigid body transformations, and (·)^ represents the mapping process from Lie algebras to Lie groups.

[0077]

[0078] Each element T∈SE(3) and its corresponding Lie algebra ξ^ has exponential and logarithmic mappings: T=exp(ξ^) and ξ^=log(T)ξ∧=log(T). For clarity, T=Exp(ξ) and Log(T)=ξ are defined.

[0079] The symbol (·) acts on homogeneous points p∈ 4 Above, and related to the Lie algebra ξ^:

[0080]

[0081] The adjoint representation Ad(T) of a Lie group controls the transformation of Lie algebra elements under group multiplication:

[0082] TExp(ξ)=Exp(Ad(T)ξ)T (4)

[0083] The Baker-Campbel-Hausdorff formula (BCH) is used to synthesize the two matrix exponents. If one of ξ is very small, the BCH approximation can be expressed as:

[0084]

[0085] In the formula: J r J l These are the right and left Jacobian matrices, respectively.

[0086] The traditional AX=YB calibration problem can be represented using the algebraic error of the Frobenius norm.

[0087]

[0088] Where N cal There are a total of N cal Position and pose are involved in calibration, ε cal Represents N cal Each pose constitutes a set of relative pose relationships. In photogrammetry, A i Transformation matrix from photogrammetric base to sphere coordinate system, B i X is the structured light extrinsic parameter matrix, X is the structured light and sphere cage calibration matrix, and Y is the extrinsic parameter matrix from the photogrammetric tracking base to the world coordinate system. Using the calibration target sphere P... i The property of invariant spatial location, combined with point cloud matching T ij P i =P j Transform the calibration problem into spatial geometric errors

[0089]

[0090] Transforming it into a vector space of Lie algebras, the problem is defined as follows:

[0091]

[0092] in, Substituting point cloud matching further into AX = YB, the problem becomes:

[0093]

[0094] To improve the accuracy of X and Y coordinates, pose graph constraints (PGC) are introduced. pg Euclidean distance graph constraint (EDC) G ed As a constraint in the optimization problem. In G ed In the middle, G ed Each node k in the diagram corresponds to a 3D point O obtained at pose k, and each edge (k, l) represents O. k and O l The prior Euclidean distance d between them kl G pg and G ed It can be represented as

[0095]

[0096] The problem of finding the optimal solution to the objective function is transformed into

[0097]

[0098] It can also be expressed as

[0099]

[0100] Define intermediate variable α i denoted as α i =A i The constrained pose graph optimization problem is transformed into X.

[0101]

[0102] in To make the solution of formula (6) unique, the f-th intermediate α is defined. f The pose is the anchor pose, and the optimization variables of formula (6) are... Anchor point pose α f The relative poses between them are denoted as

[0103]

[0104] intermediate variables For the relative transformation between anchor pose f and pose i, the optimized variables are... As a constraint, for A i B i X and Y are jointly optimized.

[0105] S300: Employs a sequential quadratic programming algorithm to simultaneously optimize the pose matrix and calibration parameters in Lie group space, minimizing algebraic and geometric errors;

[0106] Traditional calibration methods will locate the pose A i With external parameter pose B i Treating A as a constant leads to limited calibration accuracy. This invention proposes a hierarchical graph-constrained joint optimization model, HGC-JPO, to achieve [the desired calibration accuracy]. i B i Joint optimization of X and Y parameters. For example... Figure 3 As shown, step S300 specifically includes:

[0107] S310: Input initial parameters Pose graph constraints Layered graph constraint α * Scale parameter w ρ ,w φ ;

[0108] S320: Call the scale function to normalize the scale of all homogeneous matrices (Equation 24) to obtain the normalized initial pose;

[0109] S330: Set the iteration number k = 0 and start the first iteration;

[0110] S340: Construct the objective function (Equation 21) and constraint functions (Equation 22);

[0111] S350: Use the Sequential Quadratic Programming (SQP) algorithm to solve for the pose update and achieve iterative updates;

[0112] S360: Determine if ||Ξ|| < 10 is satisfied. -9 or k < k max If the condition is not met, continue iterating; otherwise, restore the optimal pose under the normalized scale to the original scale.

[0113] The objective function of this joint optimization problem is derived from three calibration models. and The model is constructed, and to prevent overfitting during the solution process, this invention proposes a Lasso function f on the Lie group. lasso The constraints of the problem are the hierarchical graph constraints obtained in the previous section.

[0114] Linearizing the objective and constraints of formula (6), the subproblem is transformed into a quadratic constrained quadratic programming (QCQP) problem, denoted as...

[0115]

[0116] Combining the objective functions of algebraic error and geometric error, the optimization problem can be reformulated as follows:

[0117]

[0118] in It can be defined as

[0119]

[0120] f lasso The lasso function is defined as follows:

[0121]

[0122] The symbol (~) represents the initial input value.

[0123] The solution space is The joint optimization problem (16) can be simplified to

[0124]

[0125] Regarding problem (16), the objective function and constraint function are both quadratic terms and linear in form, and the perturbation problem is as follows:

[0126] minΞAΞ+ΞBs.tCΞ+D=0 (20)

[0127] in And A and B are

[0128]

[0129] in and represent and The residual vector in each term of the problem, and It is the Jacobian matrix of the residual vector.

[0130] For the coefficients in the linear constraint function and By

[0131]

[0132] Since the constraint function is a linear constraint and the objective function is a quadratic form, this problem can be solved using the sequential least squares algorithm.

[0133] The physical units of the rotating and translating components differ, resulting in a significant change in the order of magnitude of the optimization variables. This difference can cause variables of a small magnitude to deviate from their expected values. Therefore, weighting coefficients are used for the rotation and translation components, with the translation component w... ρ and rotating part w φ The weighting coefficients are derived from the initial calibration results. and definition:

[0134]

[0135] All A i B i The vector replacement of the Lie algebra is

[0136]

[0137] The pose at the normalized scale is restored to the original scale to obtain the final optimization result.

[0138] S400: A marker system including a target cage and a standard bat is built in the photogrammetry system for verification, and the optimization process is introduced into a joint optimization framework based on hierarchical graph constraints to achieve global optimization of the pose matrix and calibration parameters.

[0139] The method of this invention involves repeatedly acquiring point clouds of a standard sphere using a robot equipped with a structured light scanner. Point cloud registration is used to construct relative pose relationships between nodes, and Euclidean distance constraints are established by combining the known high-precision physical length of the standard ball. These two constraints are then fused to form a layered graph constraint model. A sequential quadratic programming algorithm is employed to simultaneously optimize the pose matrices A and B and the calibration parameters X and Y within a Lie group space, minimizing algebraic and geometric errors. The method is validated by constructing a marker system containing a target ball cage and a standard ball within a photogrammetric system, and the optimization process is incorporated into the HGC-JPO optimization framework. This approach addresses the problems of insufficient constraint information, low joint optimization accuracy, and poor convergence efficiency in existing methods. It achieves high-precision collaborative optimization of the calibration parameters of the photogrammetric system, effectively reducing the solution space, improving calibration stability, and accelerating convergence speed, thereby significantly improving the system's positional accuracy and measurement reliability.

[0140] In a photogrammetric system, a marker system consisting of a target cage and a standard bat is built to collect multi-pose point clouds and pose data. The aforementioned method is then substituted into the HGC-JPO optimization framework for joint solution, achieving global optimization of pose matrices A and B and calibration parameters X and Y. This can effectively reduce the average positioning error of the measurement system and improve position accuracy.

[0141] like Figure 4 The figure shows a comparison of error results in photogrammetric system calibration according to an embodiment of the present invention. As can be seen from the figure, the overall calibration error is significantly lower than the comparative method after adopting the joint optimization method based on hierarchical graph constraints (HGC-JPO), demonstrating the advantages of this method in suppressing noise influence and improving calibration accuracy. The method of the present invention uses pose graph constraints (PGCs) and Euclidean distance constraints (EDCs) to construct a hierarchical constraint mechanism. Pose graph constraints (PGCs), based on sensor point cloud registration, can reflect the relative transformation relationship between different acquisition poses and provide topological association information of spatial poses; Euclidean distance constraints (EDCs) utilize the known high-precision physical length of a standard ball to provide rigid spatial distance constraints between nodes. The two constraints complement each other from the two dimensions of geometric transformation and spatial distance, resulting in higher information richness.

[0142] like Figure 5The figure shows the positional accuracy performance of the embodiment of the present invention on the training and test sets. The results show that, regardless of the test or training set, the positional accuracy of the combination of the traditional method and HGC is better than that of the traditional method alone, further verifying the effectiveness of the HGC method of the present invention in improving the positional accuracy of photogrammetric systems, especially in reducing errors and stabilizing accuracy. This accuracy improvement fully demonstrates the core value of the HGC method proposed in this invention in optimizing the positional accuracy of photogrammetric systems: on the one hand, HGC provides richer and more robust constraint information for the calibration process by integrating pose graph constraints and Euclidean distance constraints, effectively compensating for the error accumulation problem caused by the single constraint in traditional methods; on the other hand, its combination with traditional methods is not a simple superposition, but rather guides the optimization direction of traditional methods through a layered graph constraint mechanism, strengthening the global error compensation capability while retaining the basic framework of traditional methods, thereby achieving stable error reduction on both the training and test sets. Especially in scenarios with complex data distribution or strong noise interference, the HGC method can improve the stability of accuracy and avoid the sudden increase or decrease in accuracy that may occur in traditional methods, providing a more reliable performance guarantee for the practical application of photogrammetry systems.

[0143] In another embodiment of the present invention, a photogrammetric joint calibration system based on layered map constraints is provided, which applies the photogrammetric joint calibration method based on layered map constraints.

[0144] As shown in Figure 6(a), the photogrammetry system includes an AGV, a six-degree-of-freedom robot, a monocular structured light scanner, a target sphere cage, and a photogrammetric positioning system. These components work together through specific connection methods and functional coordination to achieve efficient and accurate measurement tasks. The monocular structured light scanner is fixedly mounted at the end effector of the six-degree-of-freedom robot. This mounting method ensures that the scanner can flexibly adjust its position and orientation as the robot moves. Simultaneously, it remains fixedly connected to the target sphere cage, meaning there is no relative displacement between them, providing a stable physical foundation for subsequent pose tracking and data alignment. The AGV, as a mobile platform, can move the six-degree-of-freedom robot and the structured light device mounted at its end within a large spatial range. The six-degree-of-freedom robot itself can achieve multi-directional and multi-angle pose adjustment of the end effector structured light device in three-dimensional space through the coordinated movement of multiple joints. The combination of these two components allows the structured light device to overcome the limitations of a fixed measurement range, enabling comprehensive data acquisition of the target under different poses, thereby obtaining richer and more complete measurement information. The photogrammetric positioning system plays a crucial role in unifying coordinates within the entire system. By monitoring and calculating the target sphere in real time, it can accurately acquire the target sphere's pose information in space. Since the monocular structured light scanner is fixed to the target sphere, it can accurately convert and align the measurement point data collected by the structured light device at different poses to the same coordinate system based on the target sphere's spatial pose. This effectively solves the data fragmentation problem during multi-position measurements and provides a consistent and reliable data foundation for subsequent joint calibration and optimization based on layered graph constraints.

[0145] As shown in Figure 6(b), AX = YB is defined in the calibration of the photogrammetric system. EF, RB, SL, WLD, MS and PS represent the end effector, robot base, structured light, world, marker and photogrammetric system, respectively. Specifically, EF represents the coordinate system of the end effector, i.e., the robot's end-mounted tool (such as the monocular structured light scanner in this system), which directly reflects the real-time spatial position and attitude of the measuring tool; RB refers to the robot base, whose coordinate system is the reference for robot kinematics calculations, and all joint motion parameters are calculated based on this coordinate system; SL corresponds to the structured light scanner, whose coordinate system is used to describe the scanner's own measurement reference and has a fixed relative pose relationship with the EF coordinate system because the two are rigidly connected through a mechanical structure; WLD represents the world coordinate system, which serves as the global reference for the entire measurement system, and the poses of all devices and targets must ultimately be unified under this coordinate system to achieve global consistency; MS is the marking system (such as the target ball cage), whose coordinate system is fixed to the SL coordinate system and is the core target tracked by the photogrammetric positioning system; PS is the photogrammetric system, whose coordinate system is used to describe the measurement reference of the photogrammetric equipment (such as the tracking camera), and a connection with the global coordinate system is established through the observation and calculation of the MS. In the equation AX = YB, A and B represent specific pose transformation matrices: A typically represents the pose transformation from the RB coordinate system to the MS coordinate system, reflecting the spatial relationship between the robot base and the marking system; B represents the pose transformation from the SL coordinate system to the EF coordinate system, reflecting the relative pose between the structured light scanner and the end effector. X and Y serve as key parameter matrices to be calibrated. X describes the transformation relationship between the SL and MS coordinate systems, while Y characterizes the pose association between the RB and WLD coordinate systems. By establishing and solving this equation, multi-source information such as robot kinematic parameters, structured light scanner extrinsic parameters, and marking system pose can be integrated into a unified mathematical framework. This provides a core mathematical model for subsequent joint optimization based on hierarchical graph constraints (HGC-JPO), ultimately achieving high-precision collaborative calibration of each parameter and ensuring the accuracy of spatial pose calculation for the entire photogrammetric system.

[0146] Those skilled in the art will readily understand that the above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A photogrammetric joint calibration method based on layered map constraints, characterized in that, Includes the following steps: S100: A robotic measurement system consisting of a robot equipped with a structured light scanner is used to collect point clouds of a standard sphere multiple times. S200: Based on the registration of surface structured light point cloud, the relative pose relationship between nodes is constructed, and the Euclidean distance constraint between nodes is constructed using the standard ball-and-stick system and merged into a layered graph constraint. S300: Employs a sequential quadratic programming algorithm to simultaneously optimize the pose matrix and calibration parameters in Lie group space, minimizing algebraic and geometric errors; S400: A marker system including a target cage and a standard ball is built in the photogrammetry system for verification, and the optimization process is introduced into a joint optimization framework based on hierarchical graph constraints to achieve global optimization of the pose matrix and calibration parameters. S210: Input the pose graph constraints and Euclidean graph constraints, as well as the scaled initial pose hand-eye matrix X, robot-world matrix Y, robot pose and sensor extrinsic parameter matrices; S220: Use Lie algebra to transform the pose variables into vectors in Euclidean space, and realize the first-order Taylor expansion of the error function; S230: The Lagrange multiplier method is used to process the Euclidean graph constraints; S240: Iteratively update the pose variables until the error converges or the number of iterations is satisfied, and extract the hierarchical graph constraint parameters from the solution.

2. The method according to claim 1, characterized in that, Pose graph constraints and Euclidean distance constraints Represented as: ， In the formula, To calibrate the target ball, For point cloud matching, For the robot's pose matrix, For the sensor extrinsic parameter matrix, The initial pose hand-eye matrix, For the robot-world matrix, O is... Each node k in the diagram corresponds to a 3D point obtained at pose k, d kl Let O represent each edge (k, l). k and O l The prior Euclidean distance between them.

3. The method according to claim 1, characterized in that, The constrained pose graph optimization problem is transformed into: ， In the formula, , It is an intermediate variable.

4. The method according to any one of claims 1-3, characterized in that, In step S300, solving the joint optimization framework based on hierarchical graph constraints using the sequential quadratic programming algorithm includes the following steps: S310: Input initial parameters, pose graph constraints, layer graph constraints, scale parameters; S320: Call the scale function to scale all homogeneous matrices to obtain the normalized initial pose; S330: Set the iteration number k = 0, and start the first iteration; S340: Construct the objective function and constraint functions; S350: Use a sequential quadratic programming algorithm to solve for the pose update amount and achieve iterative update; S360: Determine if the condition is met. or k < k max If the condition is not met, continue iterating; otherwise, restore the optimal pose under the normalized scale to the original scale.

5. The method according to claim 4, characterized in that, The optimization problem can be restated as: ， Among them, intermediate variables For the relative transformation between anchor point pose f and pose i; , , It can be defined as ， The lasso function is defined as follows: ， The symbol ~ represents the input initial value.

6. The method according to any one of claims 1-3, characterized in that, The physical length of the standard baseball bat serves as a rigid distance constraint, accelerating iterative convergence.

7. The method according to claim 2 or 3, characterized in that, The layered graph constraint includes two parts: pose graph constraint and Euclidean distance constraint. The two types of constraint information are integrated through the layered graph constraint to improve the accuracy of the initial pose hand-eye matrix X and robot-world matrix Y.

8. The method according to any one of claims 1-3, characterized in that, The sequential quadratic programming algorithm uniformly handles rotation and translation components on the SE manifold, avoiding parameterized projection errors.

9. A photogrammetric joint calibration system based on layered map constraints, used to implement the photogrammetric joint calibration method based on layered map constraints as described in any one of claims 1-8, characterized in that, include: The point cloud acquisition module uses a robot equipped with a surface structured light scanner to form a robot measurement system, which acquires point clouds of a standard sphere multiple times. The layered graph constraint module is used to construct the relative pose relationship between nodes based on the registration of surface structured light point clouds, and to construct the Euclidean distance constraint between nodes using the standard ball-and-stick system, which is then merged into a layered graph constraint. The error optimization module is used to simultaneously optimize the pose matrix and calibration parameters in the Lie group space using a sequential quadratic programming algorithm, minimizing algebraic and geometric errors. The global optimization module is used to build a marker system including a target cage and a standard bat in the photogrammetry system for verification, and to introduce the optimization process into a joint optimization framework based on hierarchical graph constraints to achieve global optimization of the pose matrix and calibration parameters.

Images