Dynamic pose calibration methods, devices, media and equipment

Abstract

This invention relates to a dynamic pose calibration method, apparatus, medium, and device. By acquiring a world coordinate set and a first device coordinate set, several feature point pairs are obtained to acquire a calibration transformation matrix. Then, it is determined whether the attitude of the acquisition device has changed. If not, calibration is completed; if so, the nth device coordinate system corresponding to the current attitude is obtained. The initial value of n is 2. The coordinates of each calibration feature point in the nth device coordinate system are then acquired to obtain the rotation axis equation of the nth device coordinate system in the world coordinate system. Based on the rotation axis equation and prior Euler angles, the transformation matrix between the nth device coordinate system and the world coordinate system is obtained, and n is incremented by 1. The process then returns to determine whether the attitude of the acquisition device has changed. This invention solves the problems of existing technologies where, due to changes in the relative attitude between the acquisition device coordinate system and the world coordinate system, separate calibration is required for different scanning attitudes of the acquisition device, leading to low calibration efficiency or even incomplete calibration.

Description

Technical Field

[0001] This invention relates to the field of computer vision technology, and in particular to a dynamic pose calibration method, apparatus, medium, and device. Background Technology

[0002] In modern industrial manufacturing, accurate dimensional measurement of large workpieces is a critical task. Traditional vision measurement methods are often limited by the camera's field of view when dealing with large workpieces, making it impossible to acquire all the workpiece's dimensional information at once. To solve this problem, a motion module is typically used to drive the camera to take pictures at different positions, achieving a complete scanning image of the workpiece surface. However, when using a motion module to drive the camera for scanning, since the module and camera each have independent coordinate systems, unifying these two coordinate systems becomes a technical challenge. Currently, a common method is to obtain the coordinates of multiple points on a calibration plate in both the camera and module coordinate systems, and then determine the extrinsic parameters between the camera and module by solving the ICP (Iterative Closest Point) problem or the PNP (Perspective-n-Point) problem, thereby achieving coordinate system unification.

[0003] However, this method has limitations in practical operation. After the robot completes hand-eye calibration, the transformation from the camera coordinate system to the robot coordinate system can be completed through the hand-eye calibration matrix and internal pose calculation. However, for the module, since its joints usually do not have well-calibrated pose relationships, when the scanning pose changes, the relative pose between the camera coordinate system and the module coordinate system will also change, leading to changes in extrinsic parameters. When scanning multiple different poses is required, traditional calibration methods require calibration for each pose individually, which is impractical in actual operation, especially when there are a large number of pose changes (e.g., hundreds of poses), making such calibration almost impossible.

[0004] Therefore, how to improve the existing technology for measuring the size of large workpieces, which suffers from insufficient flexibility and accuracy, high cost and low efficiency, is a technical problem that urgently needs to be solved in this field. Summary of the Invention

[0005] Based on this, the purpose of this application is to provide a dynamic pose calibration method, apparatus, medium and device to solve at least one of the technical problems mentioned in the background art.

[0006] Firstly, this application provides a dynamic pose calibration method, including:

[0007] S1: Obtain the coordinates of each calibration feature point in the world coordinate system and the first device coordinate system to obtain the world coordinate set and the first device coordinate set;

[0008] S2: Obtain several feature points in the first device coordinate set, and construct several feature point pairs based on the corresponding feature points in the world coordinate set, so as to obtain the first transformation matrix between the first device coordinate system and the world coordinate system based on the feature point pairs, which is the calibration transformation matrix;

[0009] S3: Determine if the attitude of the acquisition device has changed. If not, complete the calibration. If yes, obtain the coordinate system of the nth device corresponding to the current attitude. The initial value of n is 2.

[0010] S4: Obtain the coordinates of each calibration feature point in the nth device coordinate system, and select several feature points to fit several circle centers, so as to obtain the rotation axis equation of the nth device coordinate system in the world coordinate system based on the circle centers;

[0011] S5: Based on the rotation axis equation and prior Euler angles, obtain the transformation matrix between the nth device coordinate system and the (n-1)th device coordinate system. Based on the transformation matrices between each device coordinate system and the calibration transformation matrix, obtain the transformation matrix between the nth device coordinate system and the world coordinate system. Increment n by 1 and return to step S3.

[0012] Further steps for obtaining the calibration transformation matrix include:

[0013] Obtain the coordinates of each feature point in any feature point pair in the first direction in the world coordinate system and the first device coordinate system to obtain the first world feature point and the first device feature point. Based on the projection relationship between the two, obtain the yaw angle and pitch angle between the world coordinate system and the first device coordinate system.

[0014] Obtain the coordinates of each feature point in any feature point pair in the second direction in the world coordinate system and the first device coordinate system to obtain the second world feature point and the second device feature point. Based on the projection relationship between the two, obtain the roll angle between the world coordinate system and the first device coordinate system.

[0015] The rotation matrix between the world coordinate system and the first device coordinate system is obtained based on the yaw angle, pitch angle, and roll angle.

[0016] Transform any pair of feature points to the same coordinate system using the rotation matrix, and obtain the distance between the transformed feature point pairs to get the translation vector between the world coordinate system and the first device coordinate system. Based on the rotation matrix and translation vector between the two, obtain the first transformation matrix between them, which is the calibration transformation matrix.

[0017] Further steps to obtain the yaw and pitch angles include:

[0018] Obtain the coordinates of each feature point in any feature point pair in the first direction in the world coordinate system and the first device coordinate system to obtain the first world feature point and the first device feature point;

[0019] Project the three-dimensional coordinates of the first device feature point onto the world coordinate system to obtain the coordinates of the first device feature point in the world coordinate system;

[0020] Choose any two points from the first world feature point / first device feature point, and obtain the corresponding two points from the first device feature point / first world feature point. Substitute them into the angle calculation formula to obtain the yaw angle and pitch angle between the world coordinate system and the first device coordinate system.

[0021] The steps to obtain the roll angle include:

[0022] Obtain the coordinates of each feature point in any feature point pair in the second direction in the world coordinate system and the first device coordinate system to obtain the second world feature point and the second device feature point.

[0023] Project the three-dimensional coordinates of the second device feature point onto the world coordinate system to obtain the coordinates of the second device feature point in the world coordinate system;

[0024] Choose any two points from the second world feature points / second device feature points, and obtain the corresponding two points from the second device feature points / second world feature points. Substitute them into the angle calculation formula to obtain the roll angle between the world coordinate system and the first device coordinate system.

[0025] Furthermore, the angle calculation formula includes:

[0026]

[0027] Where β is the pitch angle, γ is the yaw angle, α is the roll angle, and p c1 and p c2 For the selected first device feature point, the selected p w1 and p w2 p is a first-world feature point. c3 and p c4 For the selected second device feature point, p w3 and p w4 These are the selected second-world feature points.

[0028] Further steps to obtain the equation of the axis of rotation include:

[0029] Obtain several feature points in the third direction under the nth device coordinate system, and project them onto the world coordinate system to obtain the first circle center feature point;

[0030] Based on the coordinates of the first circle center feature point, several feature points at different heights are obtained by classification, and the circle center is fitted to the feature points at each height to obtain the first group of circle centers;

[0031] Obtain several feature points in the fourth direction under the nth device coordinate system, and project them to the world coordinate system to obtain the second circle center feature point;

[0032] Based on the coordinates of the second circle center feature point, several feature points at different heights are classified, and the circle center is fitted to the feature points at each height to obtain the second set of circle centers;

[0033] Obtain the center point of each circle at the same height in the first and second sets of circles, and fit the rotation axis equation of the nth device coordinate system based on several center points.

[0034] Furthermore, the step of obtaining the transformation matrix between the nth device coordinate system and the (n-1)th device coordinate system based on the rotation axis equation and prior Euler angles includes:

[0035] Determine whether the yaw, pitch, and roll angles in the prior Euler angles are all 0;

[0036] If the yaw angle is not 0, substitute the yaw angle into the transformation matrix calculation formula to obtain the yaw angle transformation matrix;

[0037] If the pitch angle is not 0, substitute the pitch angle into the transformation matrix calculation formula to obtain the pitch transformation matrix;

[0038] If the roll angle is not 0, substitute the roll angle into the transformation matrix calculation formula to obtain the roll angle transformation matrix;

[0039] By combining the yaw angle transformation matrix, pitch transformation matrix, and roll angle transformation matrix, the transformation matrix between the nth equipment coordinate system and the (n-1)th equipment coordinate system is obtained.

[0040] Furthermore, the formula for calculating the transformation matrix is:

[0041] R=I+(1-cosΘ)uu T +[sinΘ][u] × 5-1

[0042]

[0043] Where R is the transformation equation between the nth device coordinate system and the (n-1)th device coordinate system, I is a 3x3 identity matrix, [u]× is an antisymmetric matrix (also known as a cross product matrix) constructed from the rotation axis equation u, θ is the prior yaw angle, and H is the transformation matrix between the nth device coordinate system and the (n-1)th device coordinate system.

[0044] Secondly, this application provides a dynamic pose calibration device, applied to the dynamic pose calibration method described in the first aspect, comprising:

[0045] The feature point acquisition module is used to acquire the coordinates of each calibrated feature point in the world coordinate system and the first device coordinate system, and obtain the world coordinate set and the first device coordinate set.

[0046] The calibration transformation matrix calculation module is used to obtain several feature points in the first device coordinate set and construct several feature point pairs based on the corresponding feature points in the world coordinate set, so as to obtain the first transformation matrix between the first device coordinate system and the world coordinate system based on the feature point pairs, which is the calibration transformation matrix;

[0047] The device coordinate system setting module is used to determine whether the pose of the acquisition device has changed. If not, the calibration is completed; if so, the nth device coordinate system corresponding to the current pose is obtained; the initial value of n is 2.

[0048] The rotation axis equation calculation module is used to obtain the coordinates of each calibration feature point in the nth device coordinate system, and to select a number of feature points to fit a number of circle centers, so as to obtain the rotation axis equation of the nth device coordinate system in the world coordinate system based on the circle centers;

[0049] The transformation matrix calculation module is used to obtain the transformation matrix between the nth device coordinate system and the (n-1)th device coordinate system based on the rotation axis equation and prior Euler angles, increment n by 1, and return to call the device coordinate system setting module.

[0050] Thirdly, this application also provides a computer storage medium storing executable program code; the executable program code is used to execute the dynamic pose calibration method described in any one of the first aspects.

[0051] Fourthly, this application also provides a terminal device, including a memory and a processor; the memory stores program code executable by the processor; the program code is used to execute the dynamic pose calibration method described in any one of the first aspects.

[0052] This invention provides a dynamic pose calibration method, apparatus, medium, and device. It obtains the coordinates of calibration feature points in a world coordinate system and a first device coordinate system to obtain a world coordinate set and a first device coordinate set. Then, it acquires several feature points in the first device coordinate set and constructs several feature point pairs based on the corresponding feature points in the world coordinate set. A first transformation matrix between the first device coordinate system and the world coordinate system is obtained from these feature point pairs; this is the calibration transformation matrix. Next, it determines whether the attitude of the acquisition device has changed. If not, the calibration is completed; if so, the nth device coordinate system corresponding to the current attitude is obtained (initially n = 2). Then, the coordinates of each calibration feature point in the nth device coordinate system are acquired, and several feature points are randomly selected to fit several circle centers. The rotation axis equation of the nth device coordinate system in the world coordinate system is obtained from these circle centers. Finally, the rotation axis equation and the first device coordinate system are used to determine the calibration transformation matrix. The process involves verifying Euler angles to obtain the transformation matrix between the nth device coordinate system and the (n-1)th device coordinate system. Based on the transformation matrices between each device coordinate system and the calibration transformation matrix, the transformation matrix between the nth device coordinate system and the world coordinate system is obtained. Then, n is incremented by 1. The process is repeated to determine if the acquisition device's attitude has changed, continuously identifying and calibrating new acquisition device coordinate systems. By calculating the rotation axis equations and rotation angles generated when the acquisition device's attitude changes, the pairwise transformation matrices between the acquisition device coordinate systems corresponding to each attitude are calculated. This yields the transformation matrix between each acquisition device coordinate system and the world coordinate system. Therefore, calibration can be completed by calculating only the rotation axis equations, avoiding separate calibration of the acquisition device coordinate systems corresponding to different scanning attitudes, significantly reducing calibration workload and improving calibration efficiency. This solves the problem in existing technologies where separate calibration of different scanning attitudes of the acquisition device is required due to changes in the relative attitude between the acquisition device coordinate system and the world coordinate system, leading to low calibration efficiency or even incomplete calibration. Attached Figure Description

[0053] Figure 1 This is a flowchart of the dynamic pose calibration method according to an embodiment of the present invention;

[0054] Figure 2 This is a schematic diagram of the module's movement direction according to an embodiment of the present invention;

[0055] Figure 3 This is a schematic diagram of the Euler angle calculation method according to an embodiment of the present invention;

[0056] Figure 4 This is a schematic diagram of the dynamic pose calibration device according to an embodiment of the present invention. Detailed Implementation

[0057] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative effort are within the scope of protection of the present invention.

[0058] It should be noted that if the embodiments of the present invention involve directional indications, such as up, down, left, right, front, back, etc., these directional indications are only used to explain the relative positional relationships and movement of the components in a specific posture. If the specific posture changes, the directional indications will also change accordingly. Furthermore, if the embodiments of the present invention involve descriptions such as "first," "second," "S1," "S2," "step one," "step two," etc., these descriptions are for descriptive purposes only and should not be construed as indicating or implying their relative importance, or implicitly indicating the number of technical features indicated or the order of method execution. Those skilled in the art will understand that anything that does not violate the inventive concept and is within the scope of the present invention should be included in the protection scope of the present invention.

[0059] After completing hand-eye calibration, the robot can transform from the camera coordinate system to the robot coordinate system when the scanning posture changes. However, unlike a robot, the joints of a module often do not have pre-calibrated pose relationships. When the actuator, such as the module, drives the camera to scan and image, the extrinsic parameters change due to the relative posture between the camera coordinate system and the module coordinate system under different scanning postures. Therefore, when multiple different postures are required for scanning, traditional calibration methods require separate calibration for each scanning posture. However, when there are too many scanning postures, it is practically impossible to calibrate each scanning posture individually. Therefore, such as Figure 1 As shown, this invention provides a dynamic pose calibration method. By calculating the equation of the acquisition device's axis in the world coordinate system, the transformation matrix between the first device coordinate system and the world coordinate system is dynamically changed to any scanning posture through geometric relationships. Dynamic calculation of calibration relationships under different scanning postures is then performed, thereby unifying the point cloud coordinates in the device coordinate system corresponding to all scanning postures to the same coordinate system.

[0060] S1: Obtain the coordinates of each calibration feature point in the world coordinate system and the first device coordinate system to obtain the world coordinate set and the first device coordinate set;

[0061] Specifically, but not limited to, connecting the acquisition device to the actuator, allowing the actuator to drive the acquisition device to move along a set trajectory, and setting a calibration component with several calibration feature points at a set position, so that the actuator can carry the acquisition device to acquire complete surface data of the calibration component within the range of motion, obtaining the three-dimensional coordinates of all feature points in the first device coordinate system to construct the first device coordinate set. Then, a world coordinate system is established with any point in the three-dimensional space, and the three-dimensional coordinates of all feature points in the world coordinate system are obtained according to the prior size parameters of the calibration component to construct the world coordinate set. The acquisition device may include commonly used acquisition devices such as 3D laser cameras, 3D structured light cameras, multi-line 3D laser cameras, and binocular cameras that can be used to acquire the three-dimensional coordinates of feature points.

[0062] Preferably, the actuator is a module. In the following examples, a module will be used as an example, but it is not a limitation.

[0063] S2: Obtain several feature points in the first device coordinate set, and construct several feature point pairs based on the corresponding feature points in the world coordinate set, so as to obtain the first transformation matrix between the first device coordinate system and the world coordinate system based on the feature point pairs, which is the calibration transformation matrix;

[0064] Specifically, but not limited to, several feature point pairs can be constructed based on several feature points corresponding to the world coordinate set and the first device coordinate set. The rotation matrix and translation vector between the first device coordinate system and the world coordinate system can be calculated based on the feature point pairs to obtain the first transformation matrix between the first device coordinate system and the world coordinate system, which is the calibration transformation matrix.

[0065] Preferably, the step of obtaining several feature points in the first device coordinate set and constructing several feature point pairs based on the corresponding feature points in the world coordinate set, and obtaining the first transformation matrix between the first device coordinate system and the world coordinate system based on the feature point pairs, may include:

[0066] S21: Obtain the coordinates of each feature point in any feature point pair in the first direction in the world coordinate system and the first device coordinate system, and obtain the first world feature point and the first device feature point, so as to obtain the yaw angle and pitch angle between the world coordinate system and the first device coordinate system according to the projection relationship between the two.

[0067] Specifically, optional, but not limited to, such as Figure 2As shown, a first direction is set, causing the module to drive the acquisition device to move along this direction and scan the surface of the calibration component. The feature points acquired are the first device feature points, and the feature points corresponding to these first device feature points in the world coordinate set are the first world feature points. Then, two pairs of corresponding feature points are randomly selected from both the first world feature points and the first device feature points. Based on the projections and trigonometric relationships between the selected feature points, the yaw angle and pitch angle between the world coordinate system and the first device coordinate system are calculated. More specifically, the yaw angle is the rotation angle of the first device coordinate system around the z-axis of the world coordinate system, and the pitch angle is the rotation angle of the first device coordinate system around the y-axis of the world coordinate system.

[0068] Preferably, the first direction is the y-axis direction of the world coordinate system.

[0069] A further preferred step, which involves obtaining the coordinates of each feature point in any feature point pair along the first direction in the world coordinate system and the first device coordinate system to obtain the first world feature point and the first device feature point, and then determining the yaw angle and pitch angle between the world coordinate system and the first device coordinate system based on the projection relationship between the two, may optionally include:

[0070] S211: Obtain the coordinates of each feature point in any feature point pair in the first direction in the world coordinate system and the first device coordinate system to obtain the first world feature point and the first device feature point;

[0071] S212: Project the three-dimensional coordinates of the first device feature point to the world coordinate system to obtain the coordinates of the first device feature point in the world coordinate system;

[0072] Specifically, the first direction can be set, but is not limited to, so that the module drives the acquisition device to move along the first direction and scan the surface of the calibration part. The feature points that can be acquired are the first device feature points. The feature points corresponding to the first device feature points in the world coordinate set are the first world feature points. Then, the first device feature points are projected according to the intrinsic parameters of the acquisition device to obtain the coordinates of the first device feature points in the world coordinate system.

[0073] For example, if the three-dimensional coordinates of a first device feature point are (1,1,1), then the three-dimensional coordinates of the feature point can be directly projected into the world coordinate system, that is, the coordinates of the feature point in the world coordinate system are (1,1,1). Since only the angular deviation between the world coordinate system and the first device coordinate system needs to be calculated, the displacement of the feature point during the projection process does not need to be considered. It is only necessary to consider that the relative positions between the feature points have not changed.

[0074] S213: Select any two points from the first world feature point / first device feature point, and obtain the corresponding two points from the first device feature point / first world feature point. Substitute them into the angle calculation formula to obtain the yaw angle and pitch angle between the world coordinate system and the first device coordinate system.

[0075] For example, such as Figure 3 As shown, let feature point p be selected from the feature points of the first device. c1 and p c2 Select feature point p from the first world feature points w1 and p w2 Optionally, the yaw and pitch angles between the world coordinate system and the first equipment coordinate system can be obtained based on angle calculation formulas, including pitch angle calculation formulas and yaw angle calculation formulas.

[0076] Preferably, the formulas for calculating the pitch angle and yaw angle can be expressed as Equations 2-1 and 2-2:

[0077]

[0078] Where β is the pitch angle, γ is the yaw angle, and p c1 and p c2 p is a selected feature point from the first device feature points. w1 and p w2 These are the feature points selected from the first-world feature points.

[0079] S22: Obtain the coordinates of each feature point in the world coordinate system and the first device coordinate system in any feature point in the second direction, and obtain the second world feature point and the second device feature point, so as to obtain the roll angle between the world coordinate system and the first device coordinate system according to the projection relationship between the two.

[0080] Specifically, optional, but not limited to, such as Figure 2 As shown, a second direction is set. According to steps S211-S213, the coordinates of the selected feature points are substituted into the roll angle calculation formula to calculate the roll angle between the world coordinate system and the first device coordinate system. More specifically, the roll angle is the rotation angle of the first device coordinate system around the x-axis of the world coordinate system.

[0081] Preferably, the second direction is the x-axis of the world coordinate system. At this time, although the posture of the acquisition device changes, only the rotation angle around the z-axis of the world coordinate system changes. Therefore, the rotation angle of the first device coordinate system around the x-axis of the world coordinate system can still be calculated.

[0082] A further preferred step, which involves obtaining the roll angle between the world coordinate system and the first device coordinate system, may optionally include:

[0083] S221: Obtain the coordinates of each feature point in any feature point pair in the second direction in the world coordinate system and the first device coordinate system to obtain the second world feature point and the second device feature point;

[0084] S222: Project the three-dimensional coordinates of the second device feature point to the world coordinate system to obtain the coordinates of the second device feature point in the world coordinate system;

[0085] S223: Select any two points from the second world feature points / second device feature points, and obtain the corresponding two points from the second device feature points / second world feature points. Substitute them into the angle calculation formula to obtain the roll angle between the world coordinate system and the first device coordinate system.

[0086] Specifically, according to steps S211-S213, the second world feature point and the second device feature point can be obtained, and the roll angle between the world coordinate system and the first device coordinate system can be calculated.

[0087] For example, suppose feature point p is selected in the feature points of the second device. c3 and p c4 Select feature point p from the second world feature points w3 and p w4 The roll angle between the world coordinate system and the first device coordinate system can be obtained by using the roll angle calculation formula.

[0088] Preferably, the formula for calculating the roll angle can be expressed as Equation 2-3:

[0089]

[0090] Where α is the roll angle, p c3 and p c4 p is a selected feature point among the feature points of the second device. w3 and p w4 These are the feature points selected from the second-world feature points.

[0091] S23: Obtain the rotation matrix between the world coordinate system and the first device coordinate system based on the yaw angle, pitch angle and roll angle;

[0092] Specifically, since steps S21-S32 yield the rotation angles (yaw, pitch, and roll) of the first device coordinate system around the x-axis, y-axis, and z-axis of the world coordinate system, the rotation matrix between the two can be calculated.

[0093] Preferably, the rotation matrix between the world coordinate system and the first device coordinate system can be expressed as Equation 2-4:

[0094]

[0095] Where H1 is the rotation matrix between the world coordinate system and the first device coordinate system.

[0096] S24: Transform any pair of feature points to the same coordinate system according to the rotation matrix, and obtain the distance between the transformed feature point pairs to obtain the translation vector between the world coordinate system and the first device coordinate system. Based on the rotation matrix and translation vector between the two, obtain the first transformation matrix between them, which is the calibration transformation matrix.

[0097] Specifically, optional but not limited to transforming the feature points in any pair of feature points to the same coordinate system and obtaining the distance between the corresponding feature points, the translation vector t between the world coordinate system and the first device coordinate system can be obtained. x t y and t z By combining the rotation matrix between the world coordinate system and the first device coordinate system obtained in step S23, the first transformation matrix between the world coordinate system and the first device coordinate system can be obtained, which is the calibration transformation matrix.

[0098] Preferably, the first transformation matrix between the world coordinate system and the first device coordinate system can be expressed as Equation 2-5:

[0099]

[0100] Among them, H new Let t be the first transformation matrix between the world coordinate system and the first device coordinate system. x t y and t z This is the translation vector between the world coordinate system and the first device coordinate system.

[0101] S3: Determine if the attitude of the acquisition device has changed. If not, complete the calibration. If yes, obtain the coordinate system of the nth device corresponding to the current pose. The initial value of n is 2.

[0102] Specifically, the system can optionally, but is not limited to, real-time detection of changes in the attitude of the acquisition device. If the attitude of the acquisition device changes, it indicates that the coordinate system corresponding to the current attitude of the acquisition device has changed, and the transformation matrix between the acquisition device and the world coordinate system needs to be recalibrated. Therefore, when the attitude of the acquisition device changes, the origin can be taken as the optical center of the current acquisition device to obtain the nth device coordinate system corresponding to the current attitude, where n is initially 2 and iterates continuously with the number of attitude changes of the acquisition device. It is worth noting that the attitude of the acquisition device is the rotation angle of the acquisition device around the world coordinate system, which can be directly read by the module sensor.

[0103] S4: Obtain the coordinates of each calibration feature point in the nth device coordinate system, and select several feature points to fit several circle centers, so as to obtain the rotation axis equation of the nth device coordinate system in the world coordinate system based on the circle centers;

[0104] Specifically, according to step S1, the coordinates of each calibration feature point of the calibration component in the nth device coordinate system are obtained to obtain the nth device coordinate set. Then, several feature points located at different heights are selected in the nth device coordinate set. Feature points at the same height are grouped together to obtain several groups of feature points. The center of a circle is fitted to each group of feature points, and the rotation axis equation of the nth device coordinate system in the world coordinate system can be calculated based on the several circle centers.

[0105] For example, taking the acquisition of feature points at two heights as an example, the steps of obtaining the coordinates of each calibrated feature point in the nth device coordinate system, and fitting several feature points to obtain several circle centers, and then obtaining the rotation axis equation of the nth device coordinate system in the world coordinate system based on the circle centers, may include:

[0106] S41: Obtain several feature points in the third direction under the nth device coordinate system, and project them to the world coordinate system to obtain the first circle center feature point;

[0107] S42: Based on the coordinates of the first circle center feature point, classify and obtain several feature points at different heights, and fit the circle center to the feature points at each height to obtain the first group of circle centers;

[0108] Taking two heights as an example, a third direction, a first height, and a second height can be optionally set, causing the module to drive the acquisition device to move along the first direction and scan the surface of the calibration part, obtaining several feature points in the third direction. The three-dimensional coordinates of each feature point are then projected onto the world coordinate system to obtain the first circle center feature point. Then, several first circle center feature points located at the first height and the second height are acquired respectively to obtain the first height feature point and the second height feature point. The circle centers are then fitted to obtain the first set of circle centers, including circle center A and circle center B. The third direction, the first height, and the second height can be arbitrarily set by those skilled in the art. It is worth noting that the number of first height feature points and second height feature points is not less than 3.

[0109] S43: Obtain several feature points in the fourth direction under the nth device coordinate system, and project them to the world coordinate system to obtain the second circle center feature point;

[0110] S44: Based on the coordinates of the second circle center feature point, classify and obtain several feature points at different heights, and fit the circle center to the feature points at each height to obtain the second set of circle centers;

[0111] Taking two heights as an example, the fourth direction can be optionally set, but is not limited to. According to steps S41-S42, the first height feature point and the second height feature point are obtained, and the center of the circle is fitted to each, resulting in a second set of circle centers, including circle center A1 and circle center B1; the fourth direction can be arbitrarily set by those skilled in the art. It is worth noting that the number of the first height feature points and the second height feature points is not less than 3.

[0112] S45: Obtain the center points of the centers of the first and second sets of circles at the same height, and fit the rotation axis equation of the nth device coordinate system based on several center points.

[0113] Taking two heights as an example, one can optionally, but not limited to, obtain the center point O1 between the center A and center A1 of the circles at the same height, and the center point O2 between the center B and center B1 of the circles at the same height. Then, calculate the direction vectors for center points O1 and O2 to obtain the rotation axis equation of the nth device coordinate system.

[0114] For example, assuming the coordinates of center point O1 transformed to the world coordinate system are (x1, y1, z1), and the coordinates of center point O2 transformed to the world coordinate system are (x2, y2, z2), then we can obtain the direction vector equation 4-1:

[0115]

[0116] Then, normalize the direction vector according to Equation 4-2:

[0117]

[0118] Thus, we obtain the equations for the axis of rotation: 4-3, 4-4, 4-5.

[0119]

[0120] S5: Based on the rotation axis equation and prior Euler angles, obtain the transformation matrix between the nth device coordinate system and the (n-1)th device coordinate system. Based on the transformation matrices between each device coordinate system and the calibration transformation matrix, obtain the transformation matrix between the nth device coordinate system and the world coordinate system. Increment n by 1 and return to step S3.

[0121] Specifically, the rotation axis equations of the nth device coordinate system obtained in step S4 can be chosen, but are not limited to, based on prior Euler angles and the equations of rotation of the nth device coordinate system. The transformation matrix between the coordinate system of the nth device and the coordinate system of the (n-1)th device is obtained. Finally, by combining the transformation matrices between all the coordinate systems of the acquisition devices and the calibration transformation matrix, the transformation matrix between the coordinate system of the nth device and the world coordinate system can be obtained, and the calibration is completed. Then, n is incremented by 1, and the process returns to step S3 to continuously calibrate the transformation matrix between the new coordinate system of the acquisition device and the world coordinate system after the attitude of the acquisition device changes. The a priori Euler angles, including yaw angle, pitch angle and roll angle, can be obtained through the module sensors.

[0122] Preferably, the step of obtaining the transformation matrix between the nth device coordinate system and the (n-1)th device coordinate system based on the rotation axis equation and prior Euler angles may include:

[0123] S51: Determine whether the yaw, pitch, and roll angles in the prior Euler angles are 0 respectively;

[0124] S52: If the yaw angle is not 0, substitute the yaw angle into the transformation matrix calculation formula to obtain the yaw angle transformation matrix;

[0125] S53: If the pitch angle is not 0, substitute the pitch angle into the transformation matrix calculation formula to obtain the pitch transformation matrix;

[0126] S53: If the roll angle is not 0, substitute the roll angle into the transformation matrix calculation formula to obtain the roll angle transformation matrix;

[0127] S54: By combining the yaw angle transformation matrix, pitch transformation matrix and roll angle transformation matrix, the transformation matrix between the nth equipment coordinate system and the (n-1)th equipment coordinate system is obtained;

[0128] Specifically, Euler angles include yaw, pitch, and roll, which represent the angles of rotation of the acquisition device's coordinate system around the z-axis, y-axis, and x-axis of the world coordinate system, respectively. Therefore, it is possible, but not limited to, obtaining the Euler angles of the acquisition device's coordinate system through the module sensor to obtain its rotation angles around the z-axis, y-axis, and x-axis of the world coordinate system. Then, based on the yaw, pitch, and roll angles, the transformation matrices of the acquisition device's coordinate system around the z-axis, y-axis, and x-axis of the world coordinate system are calculated respectively. The three matrices are combined to obtain the transformation matrix between the nth device coordinate system and the (n-1)th device coordinate system.

[0129] For example, suppose the pitch and roll angles in the prior Euler angles are 0, and the yaw angle is θ. Therefore, we only need to substitute the pitch angle into the transformation matrix calculation formula 5-1 to obtain the pitch transformation matrix, which is the transformation equation between the nth equipment coordinate system and the (n-1)th equipment coordinate system:

[0130] R=I+(1-cosΘ)uu T +[sinΘ][u] × 5-1

[0131]

[0132] Where R is the transformation equation between the coordinate system of the nth device and the coordinate system of the (n-1)th device, I is a 3x3 identity matrix, [u]× is an antisymmetric matrix (also known as a cross product matrix) constructed from the rotation axis equation u, and θ is the prior yaw angle.

[0133] Then, optionally, the transformation equation can be substituted into Equation 5-3 to obtain the transformation matrix between the nth device coordinate system and the (n-1)th device coordinate system:

[0134]

[0135] Where H is the transformation matrix between the coordinate system of the nth device and the coordinate system of the (n-1)th device.

[0136] After obtaining the transformation matrix between the nth device coordinate system and the (n-1)th device coordinate system, the transformation matrix between the nth device coordinate system and the world coordinate system can be obtained based on the transformation matrices between all device coordinate systems and the calibration transformation matrix between the first device coordinate system and the world coordinate system, thus completing the calibration:

[0137] S55: Based on the transformation matrices between the coordinate systems of each device and the calibration transformation matrix, obtain the transformation matrix between the coordinate system of the nth device and the world coordinate system:

[0138] Specifically, steps S51-S54 can obtain the transformation matrix between each pair of the first device coordinate system and the nth device coordinate system. By combining the transformation matrices between the coordinate systems of each acquisition device and the calibration transformation matrix between the first acquisition device coordinate system and the world coordinate system, the transformation matrix between the nth device coordinate system and the world coordinate system can be obtained, thus completing the calibration.

[0139] For example, let the coordinate system of the second acquisition device be C2, which can be represented by Equation 5-4:

[0140] C2=HC1=HH1W1 5-4

[0141] Where C2 is the coordinate system of the second acquisition device, H is the transformation matrix between the coordinate systems of the first and second acquisition devices, H1 is the calibration transformation matrix, and W1 is the world coordinate system.

[0142] Therefore, the coordinate system of the nth acquisition device can be further calculated as Equation 5-5:

[0143] C n =HC1=[H n …H3H2]H1W1,n=2,3,4…5-5

[0144] Therefore, after obtaining the transformation matrix between all acquisition coordinate systems, the point in any acquisition coordinate system can be transformed to the world coordinate system according to the transformation matrix between each acquisition device, thus completing the calibration.

[0145] More specifically, since the acquisition device is constantly moving during the scanning process, if the attitude of the acquisition device changes, resulting in a new acquisition device coordinate system, then the number of acquisition device coordinate systems n = n + 1 needs to be changed, and the process should return to step S3 to recalibrate and obtain the transformation matrix between the updated nth acquisition device coordinate system and the world coordinate system.

[0146] In this embodiment, a dynamic pose calibration method of the present invention is given. The method involves obtaining the coordinates of each calibration feature point in the world coordinate system and the first device coordinate system to obtain the world coordinate set and the first device coordinate set. Then, several feature points are obtained from the first device coordinate set, and several feature point pairs are constructed based on the corresponding feature points in the world coordinate set. A first transformation matrix between the first device coordinate system and the world coordinate system is obtained from the feature point pairs; this is the calibration transformation matrix. Next, it is determined whether the attitude of the acquisition device has changed. If not, the calibration is completed; if so, the nth device coordinate system corresponding to the current attitude is obtained (n is initially 2). The coordinates of each calibration feature point in the nth device coordinate system are then obtained, and several feature points are randomly selected to fit several circle centers. The rotation axis equation of the nth device coordinate system in the world coordinate system is obtained from the circle centers. Finally, based on the rotation axis equation and prior knowledge... Euler angles are used to obtain the transformation matrix between the nth device coordinate system and the (n-1)th device coordinate system. Based on the transformation matrices between each device coordinate system and the calibration transformation matrix, the transformation matrix between the nth device coordinate system and the world coordinate system is obtained. n is incremented by 1, and the process is repeated to determine if the acquisition device's attitude has changed. This continuously checks for new acquisition device coordinate systems and calibrates them accordingly. By calculating the rotation axis equation and rotation angle generated when the acquisition device's attitude changes, the pairwise transformation matrices between the acquisition device coordinate systems corresponding to each attitude are calculated. This yields the transformation matrix between each acquisition device coordinate system and the world coordinate system. Therefore, calibration can be completed by calculating only the rotation axis equation, avoiding separate calibration of the acquisition device coordinate systems corresponding to different scanning attitudes, greatly reducing the calibration workload and improving calibration efficiency. This solves the problem in existing technologies where, due to changes in the relative attitude between the acquisition device coordinate system and the world coordinate system, separate calibration is required for different scanning attitudes, leading to low calibration efficiency or even incomplete calibration.

[0147] On the other hand, the present invention also provides a dynamic pose calibration device, such as Figure 4 The diagram shown is a structural block diagram of a dynamic pose calibration device according to an embodiment of this application. For ease of explanation, only the parts related to the embodiment of this application are shown. (Refer to...) Figure 4The dynamic pose calibration device may include a feature point acquisition module 401, a calibration transformation matrix calculation module 402, a device coordinate system setting module 403, a rotation axis equation calculation module 404, and a transformation matrix calculation module 405.

[0148] The feature point acquisition module 401 is used to acquire the coordinates of each calibrated feature point in the world coordinate system and the first device coordinate system, so as to obtain the world coordinate set and the first device coordinate set.

[0149] The calibration transformation matrix calculation module 402 is used to obtain several feature points in the first device coordinate set and construct several feature point pairs based on the corresponding feature points in the world coordinate set, so as to obtain the first transformation matrix between the first device coordinate system and the world coordinate system based on the feature point pairs, which is the calibration transformation matrix;

[0150] The device coordinate system setting module 403 is used to determine whether the pose of the acquisition device has changed. If not, the calibration is completed; if so, the nth device coordinate system corresponding to the current pose is obtained; the initial value of n is 2.

[0151] The rotation axis equation calculation module 404 is used to obtain the coordinates of each calibration feature point in the nth device coordinate system, and to select a number of feature points to fit a number of circle centers, so as to obtain the rotation axis equation of the nth device coordinate system in the world coordinate system based on the circle centers.

[0152] The transformation matrix calculation module 405 is used to obtain the transformation matrix between the nth device coordinate system and the (n-1)th device coordinate system based on the rotation axis equation and the prior Euler angles, increment n by 1, and return to call the device coordinate system setting module.

[0153] On the other hand, the present invention also provides a computer storage medium storing executable program code; the executable program code is used to execute any of the above-mentioned dynamic pose calibration methods.

[0154] On the other hand, the present invention also provides a terminal device, including a memory and a processor; the memory stores program code that can be executed by the processor; the program code is used to execute any of the above-mentioned dynamic pose calibration methods.

[0155] For example, the program code can be divided into one or more modules / units, which are stored in the memory and executed by the processor to complete the present invention. The one or more modules / units can be a series of computer program instruction segments capable of performing a specific function, which describe the execution process of the program code in the terminal device.

[0156] The terminal device can be a desktop computer, laptop, handheld computer, or cloud server, etc. The terminal device may include, but is not limited to, a processor and memory. Those skilled in the art will understand that the terminal device may also include input / output devices, network access devices, buses, etc.

[0157] The processor can be a Central Processing Unit (CPU), or other general-purpose processors, digital signal processors (DSPs), application-specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. A general-purpose processor can be a microprocessor or any conventional processor.

[0158] The memory can be an internal storage unit of the terminal device, such as a hard drive or RAM. The memory can also be an external storage device of the terminal device, such as a plug-in hard drive, SmartMedia Card (SMC), Secure Digital (SD) card, or Flash Card. Furthermore, the memory can include both internal and external storage units of the terminal device. The memory is used to store the program code and other programs and data required by the terminal device. The memory can also be used to temporarily store data that has been output or will be output.

[0159] The computer storage medium and terminal device described above are created based on the dynamic pose calibration method described above. Their technical functions and beneficial effects will not be elaborated here. The technical features of the above embodiments can be combined arbitrarily. For the sake of brevity, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0160] The embodiments described above are merely illustrative of several implementations of the present invention, and while the descriptions are relatively specific and detailed, they should not be construed as limiting the scope of the invention patent. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of the present invention, and these all fall within the protection scope of the present invention. Therefore, the protection scope of this invention patent should be determined by the appended claims.

Claims

1. A dynamic pose calibration method, characterized in that, include: S1: Obtain the coordinates of each calibration feature point in the world coordinate system and the first device coordinate system to obtain the world coordinate set and the first device coordinate set; S2: Obtain several feature points in the first device coordinate set, and construct several feature point pairs based on the corresponding feature points in the world coordinate set, so as to obtain the first transformation matrix between the first device coordinate system and the world coordinate system based on the feature point pairs, which is the calibration transformation matrix; S3: Determine if the attitude of the acquisition device has changed. If not, complete the calibration. If yes, obtain the coordinate system of the nth device corresponding to the current attitude. The initial value of n is 2. S4: Obtain the coordinates of each calibration feature point in the nth device coordinate system, and select several feature points to fit several circle centers, so as to obtain the rotation axis equation of the nth device coordinate system in the world coordinate system based on the circle centers; S5: Based on the rotation axis equation and prior Euler angles, obtain the transformation matrix between the nth device coordinate system and the (n-1)th device coordinate system. The Euler angles include yaw, pitch, and roll, which represent the angles of rotation of the acquisition device coordinate system around the z-axis, x-axis, and y-axis of the world coordinate system, respectively, and are obtained through the module sensors. Then, calculate the transformation matrices of the acquisition device coordinate system around the z-axis, x-axis, and y-axis of the world coordinate system based on the yaw, pitch, and roll angles, respectively. Combine the three matrices to obtain the transformation matrix between the nth device coordinate system and the (n-1)th device coordinate system. Based on the transformation matrices between each device coordinate system and the calibration transformation matrix, obtain the transformation matrix between the nth device coordinate system and the world coordinate system, increment n by 1, and return to step S3.

2. The method according to claim 1, characterized in that, The steps to obtain the calibration transformation matrix include: Obtain the coordinates of each feature point in any feature point pair in the first direction in the world coordinate system and the first device coordinate system to obtain the first world feature point and the first device feature point. Based on the projection relationship between the two, obtain the yaw angle and pitch angle between the world coordinate system and the first device coordinate system. Obtain the coordinates of each feature point in any feature point pair in the second direction in the world coordinate system and the first device coordinate system to obtain the second world feature point and the second device feature point. Based on the projection relationship between the two, obtain the roll angle between the world coordinate system and the first device coordinate system. The rotation matrix between the world coordinate system and the first device coordinate system is obtained based on the yaw angle, pitch angle, and roll angle. Transform any pair of feature points to the same coordinate system using the rotation matrix, and obtain the distance between the transformed feature point pairs to get the translation vector between the world coordinate system and the first device coordinate system. Based on the rotation matrix and translation vector between the two, obtain the first transformation matrix between them, which is the calibration transformation matrix.

3. The method according to claim 2, characterized in that, The steps to obtain the yaw and pitch angles include: Obtain the coordinates of each feature point in any feature point pair in the first direction in the world coordinate system and the first device coordinate system to obtain the first world feature point and the first device feature point; Project the three-dimensional coordinates of the first device feature point onto the world coordinate system to obtain the coordinates of the first device feature point in the world coordinate system; Choose any two points from the first world feature point / first device feature point, and obtain the corresponding two points from the first device feature point / first world feature point. Substitute them into the angle calculation formula to obtain the yaw angle and pitch angle between the world coordinate system and the first device coordinate system. The steps to obtain the roll angle include: Obtain the coordinates of each feature point in any feature point pair in the second direction in the world coordinate system and the first device coordinate system to obtain the second world feature point and the second device feature point. Project the three-dimensional coordinates of the second device feature point onto the world coordinate system to obtain the coordinates of the second device feature point in the world coordinate system; Choose any two points from the second world feature points / second device feature points, and obtain the corresponding two points from the second device feature points / second world feature points. Substitute them into the angle calculation formula to obtain the roll angle between the world coordinate system and the first device coordinate system.

4. The method according to claim 3, characterized in that, Angle calculation formulas include: β=arccos 2-1 γ=arctan 2-2 α=arccos 2-3 Where β is the pitch angle, γ is the yaw angle, and α is the roll angle. For the selected first device feature point, For point The corresponding y-axis component, and For the selected first-world feature points, and For the selected second device feature point, and These are the selected second-world feature points.

5. The method according to claim 1, characterized in that, The steps to obtain the equation of the pivot shaft include: Obtain several feature points in the third direction under the nth device coordinate system, and project them onto the world coordinate system to obtain the first circle center feature point; Based on the coordinates of the first circle center feature point, several feature points at different heights are obtained by classification, and the circle center is fitted to the feature points at each height to obtain the first group of circle centers; Obtain several feature points in the fourth direction under the nth device coordinate system, and project them to the world coordinate system to obtain the second circle center feature point; Based on the coordinates of the second circle center feature point, several feature points at different heights are classified, and the circle center is fitted to the feature points at each height to obtain the second set of circle centers; Obtain the center point of each circle at the same height in the first and second sets of circles, and fit the rotation axis equation of the nth device coordinate system based on several center points.

6. The method according to claim 1, characterized in that, The steps for obtaining the transformation matrix between the nth device coordinate system and the (n-1)th device coordinate system based on the rotation axis equation and prior Euler angles include: Determine whether the yaw, pitch, and roll angles in the prior Euler angles are all 0; If the yaw angle is not 0, substitute the yaw angle into the transformation matrix calculation formula to obtain the yaw angle transformation matrix; If the pitch angle is not 0, substitute the pitch angle into the transformation matrix calculation formula to obtain the pitch transformation matrix; If the roll angle is not 0, substitute the roll angle into the transformation matrix calculation formula to obtain the roll angle transformation matrix; By combining the yaw angle transformation matrix, pitch transformation matrix, and roll angle transformation matrix, the transformation matrix between the nth equipment coordinate system and the (n-1)th equipment coordinate system is obtained.

7. The method according to claim 6, characterized in that, The formula for calculating the transformation matrix is: 5-1 5-2 5-3 Where I is a 3x3 identity matrix, θ is the yaw, pitch, or roll angle, and [u]× is an antisymmetric matrix constructed from the rotation axis direction vector u of the rotation axis equation. , , Let x, y, and z be the x, y, and z components of the rotation axis direction vector, respectively; H be the transformation matrix between the nth device coordinate system and the (n-1)th device coordinate system; and R be the rotation transformation matrix between the nth device coordinate system and the (n-1)th device coordinate system.

8. A dynamic pose calibration device, characterized in that, include: The feature point acquisition module is used to acquire the coordinates of each calibrated feature point in the world coordinate system and the first device coordinate system, and obtain the world coordinate set and the first device coordinate set. The calibration transformation matrix calculation module is used to obtain several feature points in the first device coordinate set and construct several feature point pairs based on the corresponding feature points in the world coordinate set, so as to obtain the first transformation matrix between the first device coordinate system and the world coordinate system based on the feature point pairs, which is the calibration transformation matrix; The device coordinate system setting module is used to determine whether the pose of the acquisition device has changed. If not, the calibration is completed; if so, the nth device coordinate system corresponding to the current pose is obtained; the initial value of n is 2. The rotation axis equation calculation module is used to obtain the coordinates of each calibration feature point in the nth device coordinate system, and to select a number of feature points to fit a number of circle centers, so as to obtain the rotation axis equation of the nth device coordinate system in the world coordinate system based on the circle centers; The transformation matrix calculation module is used to obtain the transformation matrix between the nth device coordinate system and the (n-1)th device coordinate system based on the rotation axis equation and prior Euler angles, increment n by 1, and return to call the device coordinate system setting module.

9. A computer storage medium, characterized in that, It stores executable program code; the executable program code is used to execute the dynamic pose calibration method according to any one of claims 1-7.

10. A terminal device, characterized in that, It includes a memory and a processor; the memory stores program code that can be executed by the processor; the program code is used to execute the dynamic pose calibration method according to any one of claims 1-7.

Images