Ship block pose accurate measurement and calculation method

Abstract

The invention provides a ship block pose accurate measurement and calculation method, which comprises the following steps: step 1, carrying out measurement point location arrangement, including selection and arrangement of key measurement points, selection and arrangement of relative measurement points on the side surface of a block, and selection and arrangement of common reference points; 2, based on the measurement points in the step 1, block data measurement is carried out, and point position measurement is carried out based on the constructed coordinate system; step 3, carrying out coordinate transformation on the measurement data, and solving the relevance between two coordinate systems; and 4, calculating the pose of the block based on a three-point method and an improved ICP (Inductively Coupled Plasma) to obtain the pose information of the block. According to the method, the arrangement scheme of key measurement points, public reference points and an overall measurement system is designed in combination with block alignment characteristics, so that a high-precision measurement system is built; a mode of combining initial matching of a three-point method and an improved ICP algorithm is proposed to solve the pose of the total segment; quicker and more accurate matching is realized, and the iteration rate and the matching precision are respectively improved.

Description

Technical Field

[0001] The invention relates to a method for accurately measuring and solving the position and posture of a ship's overall section, and belongs to the technical field of shipbuilding. Background Art

[0002] With the continuous growth of the global economy and the strong demand in the shipping industry, driven by the independent research and development of new technologies by various shipbuilding powers, hull construction technology has achieved a huge leap, and the world's shipbuilding powers are forming a rapid response model for shipbuilding. At present, the block shipbuilding method is generally used in shipbuilding. The block shipbuilding method uses the ship block as the manufacturing unit, and uses cranes to move the block assembled in place A to place B to assemble it into a complete ship, so that the assembly facilities can be fully utilized to form a parallel batch production model for ship manufacturing in different locations. This model can reduce the workload of ship berth assembly and welding, and help shorten the ship construction cycle.

[0003] In this mode, the closure of the main section, as one of the key processes in the shipbuilding process, directly affects the overall efficiency and accuracy of shipbuilding. At present, the development of the main section shipbuilding method in China has not achieved the expected results. A major difficulty comes from the fact that the main section closure process is mostly based on manual visual inspection and manual operation and adjustment for alignment, which leads to low closure efficiency and poor accuracy, and it is difficult to achieve efficient and accurate closure in one go. When the traditional method is used for the alignment of the main section, the fixed main section must first be hoisted on the slipway, and then the subsequent main section must be hoisted. Then use the measuring instrument to measure the positional relationship between the main sections, and position the subsequent main sections twice to ensure the accuracy of the positioning. Under the traditional method, the alignment process requires multiple measurements and positioning to ensure the positional relationship between the main sections. The overall alignment process is cumbersome, inefficient, and has poor accuracy.

[0004] At present, the rapid development of automatic alignment technology has made the block alignment process automated and digital, greatly reducing the labor burden, reducing the alignment time, and improving the alignment accuracy. The relevant technology is particularly evident in the assembly process of aircraft and aerospace equipment, while the research and application in the field of ship block alignment is currently in its infancy.

[0005] Although my country's shipbuilding industry ranks at the forefront of global production, there is still much room for improvement in quality. In terms of key block alignment technology, compared with the world's leading level, my country still has a certain gap to make up. The low efficiency and poor precision in block alignment seriously restrict the vigorous development of my country's shipbuilding industry. Therefore, introducing advanced technical concepts such as new generation information technology and artificial intelligence technology into the shipbuilding industry and promoting efficient and accurate automated alignment of ship blocks has become an inevitable choice for my country to develop automated, intelligent, and digital shipbuilding and accelerate the construction of a shipbuilding power. Summary of the invention

[0006] Purpose of the invention: In view of the deficiencies in the prior art, the present invention provides a method for accurately measuring and solving the position and posture of a ship's entire section, so as to solve the problems mentioned in the above background technology.

[0007] Technical solution: A method for accurately measuring and solving the position and posture of a ship's entire section, comprising the following steps:

[0008] Step 1: Arrange the measurement points, including the selection and arrangement of key measurement points, the selection and arrangement of relative measurement points on the side of the total section, and the selection and arrangement of common reference points;

[0009] Step 2: Based on the measurement points in step 1, the total section data is measured, including the construction of the global coordinate system, the total station position coordinate system and the mobile total section coordinate system, and point measurement is performed based on the constructed coordinate system;

[0010] Step 3: Based on the point data measured in step 2, coordinate transformation of the measured data is performed to solve the correlation between the two coordinate systems;

[0011] Step 4: Based on the three-point method and the improved ICP, the pose information of the whole segment is obtained.

[0012] The step 1 is specifically as follows:

[0013] Step 1.1, set the key measurement points of the block alignment surface at the intersection of the strong structure of the ship block alignment surface;

[0014] Step 1.2, set the relative measuring point on the side of the total section;

[0015] Step 1.3: Arrange 6-10 common reference points on the foundation. The measuring equipment calibrates its own position through the common reference points on the foundation, and measures the coordinates of key points of the alignment surface of the fixed and mobile sections of the ship at two stations.

[0016] The step 2 is specifically as follows:

[0017] Step 2.1, construct the global coordinate system, construct the total station measurement coordinate system according to the set total station position, and construct the moving total section coordinate system;

[0018] Step 2.2, using at least two total stations arranged relatively to each other, measure the key measurement points of the alignment surfaces of the mobile total section and the fixed total section, and convert the measurement points into the global coordinate system, and realize the conversion between the global coordinate system and the coordinate system of the mobile total section by matching the coordinates of the key points of the mobile total section in the global coordinate system with the coordinates of the key measurement points in the design coordinate system;

[0019] Step 2.3: Before adjusting the posture of the entire section, measure the center of the ball joint on the top of the trolley, then perform trajectory planning, and then decompose its trajectory into the actuators in each direction. By installing a target on the top of the trolley, obtain the trajectory curve of the trolley in the three-degree-of-freedom direction during the overall process.

[0020] The step 3 is specifically as follows:

[0021] Step 3.1: Seven-parameter coordinate transformation model

[0022] By measuring the three-dimensional data of the common reference points arranged in step 1.3, the seven-parameter model is selected as the conversion tool, and the matrix calculation rule is applied to construct the equation group to calculate the conversion parameters of one coordinate system to another system;

[0023] By solving the conversion parameters, the spatial measurement data is converted from the original coordinate system to the target coordinate system to ensure that the measurement data is in a unified coordinate system; the coordinate transformation relationship includes:

[0024] Translation Transformation

[0025] When the axes of the two coordinate systems are in the same direction but the origins are in different positions, a translation transformation can be used to transform a point P in the coordinate system {L} to the corresponding position in the coordinate system {G}. The mathematical expression followed by this transformation is as follows:

[0026]

[0027] In the formula, the vector and Respectively represent the position of point P in the coordinate systems {G} and {L}, and the vector It represents the position of the origin of the coordinate system {B} in the global coordinate system;

[0028] Rotation Transformation

[0029] When two coordinate systems have a common origin but different orientations, the position of point P in coordinate system {L} can be transformed by rotation to obtain its equivalent position in the global coordinate system; the corresponding mathematical expression is as follows:

[0030]

[0031] In the formula, vector Respectively represent the coordinates of point P in the two coordinate systems, the matrix is the rotation transformation matrix; coordinate composite transformation

[0032] When two coordinate systems have different origins and different coordinate axis directions, a composite transformation is required to achieve the conversion of coordinate values. The mathematical expression of the composite transformation can be expressed as follows:

[0033]

[0034] In the formula, the vector and Respectively represent the position of point P in the coordinate systems {G} and {L}, and the vector represents the position of the origin of coordinate system {L} in global coordinate system {G};

[0035] On the basis of the above theory, coordinate transformation is performed on multiple points, and the mathematical expression of the composite transformation is further converted into a formula suitable for seven-parameter solution as follows:

[0036]

[0037] In the formula, the coordinate data in the coordinate system {L} is converted to the coordinate system {G}, where [△X0,△Y0,△Z0] T represents the translation vector, ε X , ε Y , ε Z The three parameters representing the rotation of the coordinate system {L} around the X, Y, and Z axes, i represents the point position, R is the matrix used for the transformation, and m represents the scale parameter. If m is equal to 0, it means that the scales of the two coordinate systems are the same;

[0038] in

[0039] R(ε)=R(ε X )R(ε Y )R(ε Z )

[0040]

[0041]

[0042] In the formula, ε X , ε Y , ε Z Three parameters representing the rotation of the coordinate system {L} around the X, Y, and Z axes.

[0043] The step 3 is specifically as follows:

[0044] Step 3.2, total station-global coordinate system conversion based on Bursa seven parameters;

[0045] When the rotation angle is small, the rotation angle is set to ε x =ε y =ε z =0, simplifying the calculation process; in the case of small-angle coordinate transformation between the total station coordinate system and the global coordinate system, the seven-parameter Bursa-Wolf model is used for coordinate system transformation. The rotation matrix in the Bursa model is:

[0046]

[0047] So we can get:

[0048]

[0049] So the Bursa model can also be expressed as:

[0050]

[0051] Then the coordinate composite transformation expression can be equivalently converted to the following formula:

[0052]

[0053] When there are n (n≥3) points that need to be transformed, the coordinates are: p1(x1, y2, z3)...p n (x n ,y n , z n ), the coordinates of the corresponding point after transformation are p1'(x i1 ,y i1 , z i1 )……p n (x in ,y in , z in ), according to the principle of matrix operation, the corresponding conversion formula is listed as follows:

[0054]

[0055] The above formula can be expressed as:

[0056]

[0057] According to the least squares principle, it can be deduced as

[0058]

[0059] When the measurement points used for coordinate transformation are all of equal weight, then:

[0060]

[0061] The seven transformation parameters of the Bursa model are obtained using the above formula, and then the measurement data is transformed from the {L} coordinate system to the {G} coordinate system.

[0062] The step 3 is specifically as follows:

[0063] Step 3.3, global-moving total segment coordinate system transformation based on Taylor expansion seven parameters;

[0064] When used for coordinate transformation from global coordinates to moving total segment coordinate system, the angle deviation is large. When constructing the seven-parameter equation, the following formula is transformed using Taylor expansion to obtain the seven-parameter model of Taylor expansion linearization;

[0065]

[0066] Thus, the rotation matrix in the Bursa model whose angle is approximated to 0 is replaced, and the replacement result is as follows:

[0067]

[0068] in

[0069]

[0070] Using the indirect adjustment method, the correction values ​​of 7 parameters are obtained; based on the principle of indirect adjustment, the following formula is obtained:

[0071] [δX0 δY0 δZ0 δμ δα δβ δγ] T =[A T PA] -1 A T Pl

[0072] Where P represents the weight matrix;

[0073] Through the iterative process, the correction accuracy of the parameters is set. In each round of iteration, the initial estimate of the parameter is composed of the sum of the previous correction value and the approximate value. When the iteration reaches the predetermined accuracy requirement, the final calculation result is output.

[0074] The step 3 is specifically as follows:

[0075] Step 4.1: Initial matching of the total segment pose based on the three-point method;

[0076] In order to ensure the effectiveness of the matching algorithm, the three-point alignment method is used for rough matching of data. First, three non-collinear points P1, P2, and P3 are selected on the initial point set of the moving total segment, and then three corresponding points Q1, Q2, and Q3 are selected on the target point set. The three pairs of points are used to construct the moving total segment measurement coordinate system and the fixed total segment measurement coordinate system respectively.

[0077] In the moving segment, take point P1 as the coordinate origin, take the direction vector from P1 to P2 as the x-axis, and construct the unit vector:

[0078]

[0079] Then determine the y-axis by the cross product of the direction vector from P1 to P3 and the x-axis direction vector, and construct the y-axis unit vector:

[0080]

[0081] Finally, the cross product of e1 and e2 is used to determine the direction of the z-axis, and its unit vector is:

[0082] e3=e1×e2

[0083] Similarly, in the fixed total segment, use Q1, Q2, and Q3 to determine the coordinate axes:

[0084]

[0086] e3′=e1′×e2′

[0087] The unit vector groups [e1, e2, e3] and [e1', e2', e3'] are the coordinate axes of the two local coordinate systems, thus forming two local coordinate systems. After coordinate transformation, the two coordinate systems should completely overlap. Then the two coordinate systems have the following relationship [e1', e2', e3'] T *k'=[e1,e2,e3] T *K, where K and k' are arbitrary vectors in the two coordinate systems. Then:

[0088] k'=([e1',e2',e3'] T ) -1 *[e1,e2,e3] T *K

[0089] Among them, [e1', e2', e3'] is an orthogonal matrix, so the initial matching rotation transformation matrix of the total segment initial point set and the target point set can be expressed as:

[0090]

[0091] According to the obtained rotation vector, determine the translation vector T:

[0092]

[0093] This results in the initial transformation matrix of the measured data.

[0094] The step 3 is specifically as follows:

[0095] Step 4.2: Accurate matching of the total segment pose based on improved ICP;

[0096] After the above initial matching of the surface measurement data of the fixed and mobile sections of the ship, the improved iterative closest point algorithm based on the K-Dtree search principle is used to accurately match the measurement point set;

[0097] Step 4.2.1. ICP algorithm

[0098] Define the centroid positions p and q of two sets of points:

[0099]

[0100] Compute the de-centroided coordinates of each point:

[0101] p i =P i -pq i =Q i -q

[0102] Define the error term for the i-th point pair:

[0103] e i =P i -(R·Q i +T)

[0104] Construct the least squares problem to minimize the sum of squared errors

[0105]

[0106] The above formula can be expressed as:

[0107]

[0108] The optimal rotation matrix R can be solved by calculating the minimum value of the left half of the above formula:

[0109]

[0110] Because R T R=I, then the above formula is:

[0111]

[0112] Defining the Matrix

[0113]

[0114] Performing singular value decomposition on the matrix W yields the following formula:

[0115] W=U∑V T

[0116] Where ∑ is a diagonal matrix composed of singular values. When W is full rank, the rotation matrix and translation vector can be obtained:

[0117]

[0118] The initial R and T obtained by the three-point matching method are substituted into the error square sum formula. If the requirements are not met, the new rotation and translation matrix is ​​calculated. The iteration ends when f(R, T) reaches the termination threshold, and the rotation matrix and translation matrix are output as the final value.

[0119] Step 4.2.2, KD tree improvement

[0120] Construct a KD tree for the initial point set and the target point set to speed up the search for the nearest neighbor points;

[0121] Starting from the root node, determine the positional relationship between the input point (i.e. the point in the initial point set) and the root node. If its coordinates are smaller than those of the root node, enter the left child node; if its coordinates are larger than those of the root node, enter the right child node, and continue to move down the node until it reaches a leaf node, which is used as the "current nearest node";

[0122] Perform a "backtracking" operation on the point. If there is a closer point on other subspace nodes on the search path, jump to the subspace node to search for the closest point, and then update the "current closest node";

[0123] Repeat the above steps until the search path is empty and the search ends to find the nearest node;

[0124] Step 4.2.3: Euler angle solution

[0125] After solving the rotation matrix, the rotation matrix can be converted into the deviation angle by the rotation angle calculation formula:

[0126]

[0127] Where: α, β, γ are the rotation angles of the total segment target posture relative to the initial posture, R 23 is the value of the 2nd row and 3rd column of the rotation matrix, R 33 is the value of the third row and third column of the rotation matrix R, R 13 is the value of the first row and third column of the rotation matrix R, R 12 is the value of the first row and second column of the rotation matrix R, R 11 is the value of the first row and first column of the rotation matrix R.

[0128] Beneficial effects: Based on the measurement technology of ship block alignment, the present invention combines the alignment characteristics of the block to design the layout of key measurement points, common reference points and the overall measurement system, thereby building a high-precision measurement system;

[0129] Based on the constructed measurement system, the Bursa seven-parameter error model and the Taylor expansion seven-parameter model are used to realize the unified conversion of the coordinate system; then, in view of the problem of large error in point set matching, a method combining the three-point initial matching method with the improved ICP algorithm is proposed to solve the overall segment pose;

[0130] The three-point initial matching and improved ICP algorithm precise matching proposed in this paper achieve faster and more accurate matching. Compared with the original ICP algorithm, the iteration rate and matching accuracy are improved. The experimental results show that the measurement system and posture solution method proposed in this application can ensure that the error of key points is within 1.5mm, which meets the measurement needs of industry. BRIEF DESCRIPTION OF THE DRAWINGS

[0131] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on the provided drawings without paying creative work.

[0132] Figure 1 This is the layout diagram of the key measurement points of the alignment surface. The left side is the main view of the fixed total section alignment surface, and the right side is the main view of the mobile total section alignment surface.

[0133] Figure 2 This is a diagram showing the relative measurement point layout.

[0134] Figure 3 This is a layout diagram of public benchmark points.

[0135] Figure 4 Create a plot for the measurement field coordinate system.

[0136] Figure 5 Arrange a location diagram for common reference points.

[0137] Figure 6 Measurement diagram for key measurement points.

[0138] Figure 7 This is the measurement diagram of the ball joint center.

[0139] Figure 8 This is the coordinate system transformation diagram.

[0140] Fig. 9 This is a diagram of the three-point matching method. DETAILED DESCRIPTION

[0141] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0142] In the description of the present invention, it is necessary to understand that the terms "up", "down", "front", "back", "left", "right", "vertical", "horizontal", "top", "bottom", "inside", "outside", etc., indicating the orientation or position relationship are based on the orientation or position relationship shown in the drawings, and are only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore cannot be understood as a limitation on the present invention.

[0143] In the present invention, unless otherwise clearly specified and limited, a first feature being "above" or "below" a second feature may include that the first and second features are in direct contact, or may include that the first and second features are not in direct contact but are in contact through another feature between them. Moreover, a first feature being "above", "above" and "above" a second feature includes that the first feature is directly above and obliquely above the second feature, or simply indicates that the first feature is higher in level than the second feature. A first feature being "below", "below" and "below" a second feature includes that the first feature is directly below and obliquely below the second feature, or simply indicates that the first feature is lower in level than the second feature.

[0144] A method for accurately measuring and solving the position and posture of a ship section comprises the following steps:

[0145] Step 1: Arrange the measurement points, including the selection and arrangement of key measurement points, the selection and arrangement of relative measurement points on the side of the total section, and the selection and arrangement of common reference points; specifically:

[0146] Step 1.1, set the key measurement points of the block alignment surface at the intersection of the strong structure of the ship block alignment surface;

[0147] The principles for selecting key measurement points on the alignment surface of the ship's overall section are as follows:

[0148] ① The endpoint or midpoint of the edge of the ship's section;

[0149] ②The intersection of the outer plate and the strength member;

[0150] ③The intersections between strong components;

[0151] ④Select management points on the curve or straight line;

[0152] ⑤The surveyor shall determine the management points.

[0153] At the same time, during the measurement process, due to the large volume of the total section itself, there may be obstruction. Therefore, this paper measures the key measurement points of the total section alignment surface by dividing the area. The part closer to the total station at station 1 is divided into area 1, and the part closer to the total station at station 2 is divided into area 2. The key measurement points in the middle part are divided into area 1.

[0154] Based on the above point selection principles, the key measurement points of the ship section alignment surface are distributed as follows: Figure 1 shown.

[0155] Step 1.2, set the relative measuring point on the side of the total section;

[0156] After the alignment of the total section is completed, it is necessary to verify the completion of the alignment of the total section. However, after the alignment of the ship's total section is completed, it is difficult to measure the key measurement points of the alignment surface of the ship's total section. Therefore, relative measurement points are set on the side of the total section to find the positions of the key measurement points of the alignment surface with relative position relationships, which are then converted into relative positions of the key measurement points of the alignment surface to reflect the actual situation of the total section alignment.

[0157] The principles for selecting relative measurement points on the side of the ship's overall section are as follows:

[0158] ①Relative measuring points need to be set on the right and left sides of the total section;

[0159] ② The total section is evenly distributed relative to the measurement points and is not in the same straight line;

[0160] ③The surveyor determines the management points by himself.

[0161] Based on the above point selection principles, the relative measurement points on the side of the ship section are distributed as follows: Figure 2 shown.

[0162] Step 1.3: Arrange 6-10 common reference points on the foundation. The measuring equipment calibrates its own position through the common reference points on the foundation, and measures the coordinates of key points of the alignment surface of the fixed and mobile sections of the ship at two stations.

[0163] The ship's main section is large in size, and there will usually be obstructions during on-site measurement, so multi-station measurement is required to complete the overall data collection. After the measuring equipment measures the coordinates of key points at different stations, the data needs to be converted to the same coordinate system. At this time, the measuring instrument needs to measure the common reference point (ERS) at different stations and solve the more accurate relative position between the instrument's stations to determine the precise conversion relationship between the coordinate systems of each station. Thereby, the error of transferring the measured data at each station to the unified coordinate system is smaller. Therefore, this application arranges several ERS points on the foundation, and the measuring equipment calibrates its own position through the ERS points on the foundation, and measures the key point coordinates of the alignment surface of the fixed main section and the mobile main section of the ship at two stations.

[0164] In general, the selection of public reference points is made by technical personnel, and it is sufficient to be within the range that the measuring equipment can measure. However, in actual situations, due to reasons such as foundation vibration and temperature changes, ERS points that are usually considered fixed on the ground and walls of the alignment site can still be found to have slight positional offsets under the detection of high-precision measuring instruments. In order to reduce the impact of changes in the position of public reference points on the measurement accuracy of the system transfer station and improve the reconstruction efficiency of the system model, the following general principles need to be followed when arranging the positions of ERS points:

[0165] (1) The public reference point must have a stable layout foundation and should not be easily vibrated or displaced.

[0166] The public reference point is the basis of the measurement process, and its positioning accuracy directly affects the reliability of the measurement results. Therefore, the basis for the arrangement of the points must be stable and reliable.

[0167] (2) Public reference points should avoid being distributed in a straight line

[0168] Because multiple points on the same straight line cannot determine a coordinate system, the common reference points distributed on the straight line are not conducive to the coordinate system and also reduce the reference significance of the common reference points.

[0169] (3) The number of public reference points should be appropriate

[0170] When setting up the measurement system, ERS points should be reasonably arranged in the dock according to the position of the measuring equipment. Too many ERS points will greatly increase the cost of the measurement system. In actual projects, an appropriate number of public reference points should be selected.

[0171] The present application sets 8 public reference points near station 1 to determine the conversion relationship of the coordinate system of station 1 relative to the global coordinate system. Similarly, 8 public reference points are set at station 2 to determine the position relationship of the coordinate system of station 2 relative to the global coordinate system.

[0172] Based on the above principles, the layout of public benchmark points is as follows: Figure 3shown.

[0173] Step 2: Based on the measurement points in step 1, perform total section data measurement, including the construction of the global coordinate system, the total station position coordinate system and the mobile total section coordinate system, and perform point measurement based on the constructed coordinate system; specifically:

[0174] Establishing a coordinate system can clarify the position and relative relationship of each measurement point in the entire measurement process; and, through the relative relationship between coordinate systems, the measurement data can be unified into the same coordinate system, making it easier to analyze the data and providing support for subsequent overall segment alignment. The coordinate system establishment process is as follows: Figure 4 shown.

[0175] Step 2.1, construct the global coordinate system, construct the total station measurement coordinate system according to the set total station position, and construct the moving total section coordinate system;

[0176] Construct a global coordinate system, with the intersection of the vertical center line of the rear end face of the fixed segment and the ground as the coordinate origin O, the length direction of the fixed segment as the Y axis, the direction perpendicular to the ground as the Z axis, and the X axis established by the right-hand rule to construct a global coordinate system, and record this coordinate system as O-XYZ;

[0177] Construct the measurement coordinate system of station 1: take the laser emission position of the total station at station 1 as the origin O1, the direction of the fixed total section length as the Y1 axis, the direction perpendicular to the ground as the Z1 axis, and establish the X1 axis through the right-hand rule to construct the measurement coordinate system 1, and record this coordinate system as O1-X1Y1Z1;

[0178] Construct the measurement coordinate system of station 2: take the laser emission position of the total station at station 2 as the origin O2, the direction of the fixed total section length as the Y2 axis, the direction perpendicular to the ground as the Z2 axis, and establish the X2 axis through the right-hand rule to construct the measurement coordinate system 2, and record this coordinate system as O2-X2Y2Z2;

[0179] Construct the coordinate system of the moving total section: take the center of gravity of the moving total section as the origin O3, the direction parallel to the centerline of the moving total section as the Y3 axis, the direction perpendicular to the upward direction of the moving total section deck as the Z3 axis, determine the direction of the X3 axis using the right-hand rule, establish the coordinate system of the moving total section, and record this coordinate system as O3-X3Y3Z3.

[0180] Step 2.2: The public reference points are arranged as follows: Figure 4 Some public reference points ERS1-ERS16 are set in the global coordinate system, and then these public reference points are measured by the total station. The coordinates of the public reference points in the global coordinate system and the public reference points in the total station are matched to obtain the coordinates of the total station relative to the global coordinate system, thereby achieving self-calibration.

[0181] After the calibration is completed, the coordinates measured by the total station at different locations can be uniformly converted to the global coordinate system to achieve the unity of the coordinate system of the measurement data.

[0182] The key points of the alignment surfaces of the mobile total section and the fixed total section are measured by the total station at station 1 and station 2, and the measured points are converted to the global coordinate system. Then, the coordinates of the key points of the mobile total section in the global coordinate system are matched with the coordinates in the design coordinate system to realize the transformation between the global coordinate system and the mobile total section coordinate system. The data can be further converted to the mobile total section coordinate system. Converting the data to the mobile total section coordinate system can ensure that the center of gravity position does not change during the rotation of the total section, and can improve the safety of the ship total section in the automated alignment process.

[0183] The key points close to it are measured by the total station at station 1: fixed total section key measurement points 1-20, and mobile total section key measurement points 1'-20'; similarly, the key points close to it are measured by the total station at station 2: fixed total section key measurement points 21-35, and mobile total section key measurement points 21'-35'. Figure 6 shown.

[0184] After the measurement is completed, the data will be finally converted to the moving total segment coordinate system. The conversion process is as follows: coordinates of fixed total segment 1-35, moving total segment 1'-35' in the total station coordinate system - coordinates in the global coordinate system - coordinates in the moving total segment coordinate system.

[0185] Step 2.3: Before the overall operation, the center of the ball joint on the top of the trolley needs to be measured. Because the movement of the trolley is around the center of the ball joint, the trajectory can be planned by measuring the center position of the ball joint, and then the trajectory is decomposed into the actuators in each direction. By installing a target on the top of the trolley, the trajectory curve of the trolley in the three degrees of freedom direction during the whole process can be solved. The measurement method of the ball joint on the top of the trolley is as follows: Figure 7 As shown, the center of the ball joint cannot be measured directly. It can be solved by measuring the targets set around it and converting it through the relative position relationship.

[0186] Based on the above method, the specific process of data measurement of the total section alignment measurement system is as follows:

[0187] a. Set up common reference points on the block alignment foundation; according to the ship block design model, determine the key point positions for ship block measurement. The key point positions are at the intersection of the ship block strong structure and the outer plate. 35 points are collected on each block alignment surface; set up ball head measurement points on four attitude adjustment trolleys.

[0188] b. Place two total stations at different positions, and measure the coordinates of the common reference point, key measurement point of the total section, and measurement point of the ball head of the attitude adjustment trolley on the same side, and obtain the coordinate data of the above points in the total station coordinate system.

[0189] c. Based on the data of the common reference points in the global coordinate system and the data of the common reference points measured at different positions of the total station, the coordinate transformation parameters are solved to solve the parameter relationship between the coordinate systems of position 1 and position 2 and the global coordinate system.

[0190] d. Based on the above parameter relationship and the principle of coordinate transformation, the coordinate data of key measurement points and ball head measurement points in the total station coordinate system are converted to the global coordinate system.

[0191] e. Perform coordinate transformation based on the coordinate data in the design coordinate system of the moving section and the key measurement point data of the moving section in the global coordinate system, solve the parameter relationship between the global coordinate system and the moving section coordinate system, and convert the data to the moving section coordinate system.

[0192] f. After the overall segment is aligned, since the key measurement points on the overall segment alignment surface are blocked, the relative measurement points on the overall segment are measured at this time, and the data are converted to the coordinates of the key measurement points according to the overall segment design model, so that the above method is used again to perform coordinate transformation.

[0193] Step 3: Based on the point data measured in step 2, after determining the data of the same measurement points in the two coordinate systems, it is necessary to solve the correlation between the two coordinate systems. First, use the measuring equipment to measure the three-dimensional data of the common reference points distributed in the site, and then use the matrix calculation rules to construct a set of equations to calculate the conversion parameters from one coordinate system to another. Through the conversion parameters solved above, the spatial measurement data is converted from the original coordinate system to the target coordinate system to ensure that the measurement data is in a unified coordinate system. Specifically:

[0194] Step 3.1: Seven-parameter coordinate transformation model

[0195] By measuring the three-dimensional data of the common reference points arranged in step 1.3, the seven-parameter model is selected as the conversion tool, such as Figure 8 As shown, the matrix calculation rules are applied to construct a set of equations to calculate the conversion parameters from one coordinate system to another.

[0196] By solving the conversion parameters, the spatial measurement data is converted from the original coordinate system to the target coordinate system to ensure that the measurement data is in a unified coordinate system; the coordinate transformation relationship includes:

[0197] Translation Transformation

[0198] When the axes of the two coordinate systems are in the same direction but the origins are in different positions, a translation transformation can be used to transform a point P in the coordinate system {L} to the corresponding position in the coordinate system {G}. The mathematical expression followed by this transformation is as follows:

[0199]

[0200] In the formula, the vector and Respectively represent the position of point P in the coordinate systems {G} and {L}, and the vector It represents the position of the origin of the coordinate system {B} in the global coordinate system;

[0201] Rotation Transformation

[0202] When two coordinate systems have a common origin but different orientations, the position of point P in coordinate system {L} can be transformed by rotation to obtain its equivalent position in the global coordinate system; the corresponding mathematical expression is as follows:

[0203]

[0204] In the formula, vector Respectively represent the coordinates of point P in the two coordinate systems, the matrix is the rotation transformation matrix; coordinate composite transformation

[0205] When two coordinate systems have different origins and different coordinate axis directions, a composite transformation is required to achieve the conversion of coordinate values. The mathematical expression of the composite transformation can be expressed as follows:

[0206]

[0207] In the formula, the vector and Respectively represent the position of point P in the coordinate systems {G} and {L}, and the vector represents the position of the origin of coordinate system {L} in global coordinate system {G};

[0208] On the basis of the above theory, coordinate transformation is performed on multiple points, and the mathematical expression of the composite transformation is further converted into a formula suitable for seven-parameter solution as follows:

[0209]

[0210] In the formula, the coordinate data in the coordinate system {L} is converted to the coordinate system {G}, where [△X0,△Y0,△Z0] T represents the translation vector, ε X , ε Y , ε ZThe three parameters representing the rotation of the coordinate system {L} around the X, Y, and Z axes, i represents the point, R is the matrix used for the transformation, and m represents the scale parameter. If m is equal to 0, it means that the scales of the two coordinate systems are the same;

[0211] in

[0212] R(ε)=R(ε X )R(ε Y )R(ε Z )

[0213]

[0214] In the formula, ε X , ε Y , ε Z Three parameters representing the rotation of the coordinate system {L} around the X, Y, and Z axes.

[0215] Step 3.2, total station-global coordinate system conversion based on Bursa seven parameters;

[0216] When the rotation angle is small, the rotation angle is set to ε x =ε y =ε z =0, simplifying the calculation process; in the case of small-angle coordinate transformation between the total station coordinate system and the global coordinate system, the seven-parameter Bursa-Wolf model is used for coordinate system transformation. The rotation matrix in the Bursa model is:

[0217]

[0218]

[0219] So we can get:

[0220]

[0221] So the Bursa model can also be expressed as:

[0222]

[0223] Then the coordinate composite transformation expression can be equivalently converted to the following formula:

[0224]

[0225] When there are n (n≥3) points that need to be transformed, the coordinates are: p1(x1, y2, z3)...p n (x n ,y n , z n ), the coordinates of the corresponding point after transformation are p1'(x i1 ,yi1 , z i1 )……p n (x in ,y in , z in ), according to the principle of matrix operation, the corresponding conversion formula is listed as follows:

[0226]

[0227] The above formula can be expressed as:

[0228]

[0229] According to the least squares principle, it can be deduced as

[0230]

[0231] When the measurement points used for coordinate transformation are all of equal weight, then:

[0232]

[0233] The seven transformation parameters of the Bursa model are obtained using the above formula, and then the measurement data is transformed from the {L} coordinate system to the {G} coordinate system.

[0234] Step 3.3, global-moving total segment coordinate system transformation based on Taylor expansion seven parameters;

[0235] When used for coordinate transformation from global coordinates to the moving total segment coordinate system, the angle deviation is large (greater than 0.5°). When constructing the seven-parameter equation, the following formula is transformed using Taylor expansion to obtain a seven-parameter model with Taylor expansion linearization;

[0236]

[0237] Thus, the rotation matrix in the Bursa model whose angle is approximated to 0 is replaced, and the replacement result is as follows:

[0238]

[0239] in

[0240]

[0241] Using the indirect adjustment method, the correction values ​​of 7 parameters are obtained; based on the principle of indirect adjustment, the following formula is obtained:

[0242] [δX0 δY0 δZ0 δμ δα δβ δγ] T =[A T PA] -1 A TPl

[0243] Where P represents the weight matrix;

[0244] Through the iterative process, the correction accuracy of the parameters is set. In each round of iteration, the initial estimate of the parameter is composed of the sum of the previous correction value and the approximate value. When the iteration reaches the predetermined accuracy requirement, the final calculation result is output.

[0245] Step 4: Based on the three-point method and the improved ICP, the pose information of the total segment is obtained, which is as follows:

[0246] Step 4.1: Initial matching of the total segment pose based on the three-point method;

[0247] In order to ensure the effectiveness of the matching algorithm, the three-point alignment method is used for rough matching of data. First, three non-collinear points P1, P2, and P3 are selected on the initial point set of the moving total segment. Then, three corresponding points Q1, Q2, and Q3 are selected on the target point set. The three pairs of points are used to construct the moving total segment measurement coordinate system and the fixed total segment measurement coordinate system respectively; Fig. 9 shown.

[0248] In the moving segment, take point P1 as the coordinate origin, take the direction vector from P1 to P2 as the x-axis, and construct the unit vector:

[0249]

[0250] Then determine the y-axis by the cross product of the direction vector from P1 to P3 and the x-axis direction vector, and construct the y-axis unit vector:

[0251]

[0252] Finally, the cross product of e1 and e2 is used to determine the direction of the z-axis, and its unit vector is:

[0253] e3=e1×e2

[0254] Similarly, in the fixed total segment, use Q1, Q2, and Q3 to determine the coordinate axes:

[0255]

[0256] e′3=e′1×e′2

[0257] The unit vector groups [e1, e2, e3] and [e1', e2', e3'] are the coordinate axes of the two local coordinate systems, thus forming two local coordinate systems. After coordinate transformation, the two coordinate systems should completely overlap. Then the two coordinate systems have the following relationship [e1', e2', e3'] T *k'=[e1,e2,e3] T*K, where K and k' are arbitrary vectors in the two coordinate systems. Then:

[0258] k'=([e1',e2',e3'] T ) -1 *[e1,e2,e3] T *K

[0259] Among them, [e1', e2', e3'] is an orthogonal matrix, so the initial matching rotation transformation matrix of the total segment initial point set and the target point set can be expressed as:

[0260]

[0261] According to the obtained rotation vector, determine the translation vector T:

[0262]

[0263] The initial transformation matrix of the measured data is thus obtained. After the initial matching, the translation and rotation deviations of the measured data around the design data are very small, and the two sets of data are already in a small error state, which provides a good foundation for the subsequent detailed matching and optimizes the search space of the iterative algorithm.

[0264] Step 4.2: Accurate matching of the total segment pose based on improved ICP;

[0265] After the above initial matching of the surface measurement data of the fixed and mobile sections of the ship, the two sets of data have basically overlapped, which is a small error state, but still has not achieved the effect of global optimal matching. Therefore, it is necessary to use advanced optimization algorithms to make further precise adjustments to the surface measurement data of the fixed and mobile sections. This process is called detailed matching of measurement data.

[0266] An improved iterative closest point algorithm based on the KD tree search principle is used to accurately match the measurement point set;

[0267] Step 4.2.1. ICP algorithm

[0268] Define the centroid positions p and q of two sets of points:

[0269]

[0270] Compute the de-centroided coordinates of each point:

[0271] p i =P i -pq i =Q i -q

[0272] Define the error term for the i-th point pair:

[0273] e i =P i -(R·Q i +T)

[0274] Construct the least squares problem to minimize the sum of squared errors

[0275]

[0276] The above formula can be expressed as:

[0277]

[0278] The optimal rotation matrix R can be solved by calculating the minimum value of the left half of the above formula:

[0279]

[0280] Because R T R=I, then the above formula is:

[0281]

[0282] Defining the Matrix

[0283]

[0284] Performing singular value decomposition on the matrix W yields the following formula:

[0285] W=U∑V T

[0286] Where ∑ is a diagonal matrix composed of singular values. When W is full rank, the rotation matrix and translation vector can be obtained:

[0287]

[0288] The initial R and T obtained by the three-point matching method are substituted into the error square sum formula. If the requirements are not met, the new rotation and translation matrix is ​​calculated. The iteration ends when f(R, T) reaches the termination threshold, and the rotation matrix and translation matrix are output as the final value.

[0289] Step 4.2.2, KD tree improvement

[0290] Construct a KD tree for the initial point set and the target point set to speed up the search for the nearest neighbor points;

[0291] Starting from the root node, determine the positional relationship between the input point (i.e. the point in the initial point set) and the root node. If its coordinates are smaller than those of the root node, enter the left child node; if its coordinates are larger than those of the root node, enter the right child node, and continue to move down the node until it reaches a leaf node, which is used as the "current nearest node";

[0292] Perform a "backtracking" operation on the point. If there is a closer point on other subspace nodes on the search path, jump to the subspace node to search for the closest point, and then update the "current closest node";

[0293] Repeat the above steps until the search path is empty and the search ends to find the nearest node;

[0294] Step 4.2.3: Euler angle solution

[0295] After solving the rotation matrix, the rotation matrix can be converted into the deviation angle by the rotation angle calculation formula:

[0296]

[0297] Where: α, β, γ are the rotation angles of the total segment target posture relative to the initial posture, R 23 is the value of the 2nd row and 3rd column of the rotation matrix, R 33 is the value of the third row and third column of the rotation matrix R, R 13 is the value of the first row and third column of the rotation matrix R, R 12 is the value of the first row and second column of the rotation matrix R, R 11 is the value of the first row and first column of the rotation matrix R.

[0298] In this specification, each embodiment is described in a progressive manner, and each embodiment focuses on the differences from other embodiments. The same or similar parts between the embodiments can be referred to each other. For the device disclosed in the embodiment, since it corresponds to the method disclosed in the embodiment, the description is relatively simple, and the relevant parts can be referred to the method part.

[0299] The above description of the disclosed embodiments enables one skilled in the art to implement or use the present invention. Various modifications to these embodiments will be apparent to one skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the present invention. Therefore, the present invention will not be limited to the embodiments shown herein, but rather to the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A method for accurately measuring and solving the position and posture of a ship section, characterized in that: The following steps are involved: Step 1: Arrange the measurement points, including the selection and arrangement of key measurement points, the selection and arrangement of relative measurement points on the side of the total section, and the selection and arrangement of common reference points; Step 2: Based on the measurement points in step 1, the total section data is measured, including the construction of the global coordinate system, the total station position coordinate system and the mobile total section coordinate system, and point measurement is performed based on the constructed coordinate system; Step 3: Based on the point data measured in step 2, coordinate transformation of the measured data is performed to solve the correlation between the two coordinate systems; Step 4: Based on the three-point method and the improved ICP, the pose information of the whole segment is obtained.

2. The method for accurately measuring and solving the position and posture of a ship section according to claim 1 is characterized in that: The step 1 is specifically as follows: Step 1.1, set the key measurement points of the block alignment surface at the intersection of the strong structure of the ship block alignment surface; Step 1.2, set the relative measuring point on the side of the total section; Step 1.3: Arrange 6-10 common reference points on the foundation. The measuring equipment calibrates its own position through the common reference points on the foundation, and measures the coordinates of key points of the alignment surface of the fixed and mobile sections of the ship at two stations.

3. The method for accurately measuring and solving the position and posture of a ship section according to claim 2 is characterized in that: The step 2 is specifically as follows: Step 2.1, construct the global coordinate system, construct the total station measurement coordinate system according to the set total station position, and construct the moving total section coordinate system; Step 2.2, using at least two total stations arranged relatively to each other, measure the key measurement points of the alignment surfaces of the mobile total section and the fixed total section, and convert the measurement points into the global coordinate system, and realize the conversion between the global coordinate system and the coordinate system of the mobile total section by matching the coordinates of the key points of the mobile total section in the global coordinate system with the coordinates of the key measurement points in the design coordinate system; Step 2.3: Before adjusting the posture of the entire section, measure the center of the ball joint on the top of the trolley, then perform trajectory planning, and then decompose its trajectory into the actuators in each direction. By installing a target on the top of the trolley, obtain the trajectory curve of the trolley in the three-degree-of-freedom direction during the overall process.

4. The method for accurately measuring and solving the position and posture of a ship section according to claim 3 is characterized in that: The step 3 is specifically as follows: Step 3.1: Seven-parameter coordinate transformation model By measuring the three-dimensional data of the common reference points arranged in step 1.3, the seven-parameter model is selected as the conversion tool, and the matrix calculation rule is applied to construct the equation group to calculate the conversion parameters of one coordinate system to another system; By solving the conversion parameters, the spatial measurement data is converted from the original coordinate system to the target coordinate system to ensure that the measurement data is in a unified coordinate system; The coordinate transformation relationships include: Translation Transformation When the axes of the two coordinate systems are in the same direction but the origins are in different positions, a translation transformation can be used to transform a point P in the coordinate system {L} to the corresponding position in the coordinate system {G}. The mathematical expression followed by this transformation is as follows: In the formula, the vector and Respectively represent the position of point P in the coordinate systems {G} and {L}, and the vector It represents the position of the origin of the coordinate system {B} in the global coordinate system; Rotation Transformation When two coordinate systems have a common origin but different orientations, the position of point P in coordinate system {L} can be transformed by rotation to obtain its equivalent position in the global coordinate system; the corresponding mathematical expression is as follows: In the formula, vector Respectively represent the coordinates of point P in the two coordinate systems, the matrix is the rotation transformation matrix; Coordinate composite transformation When two coordinate systems have different origins and different coordinate axis directions, a composite transformation is required to achieve the conversion of coordinate values. The mathematical expression of the composite transformation can be expressed as follows: In the formula, the vector and Respectively represent the position of point P in the coordinate systems {G} and {L}, and the vector represents the position of the origin of coordinate system {L} in global coordinate system {G}; On the basis of the above theory, coordinate transformation is performed on multiple points, and the mathematical expression of the composite transformation is further converted into a formula suitable for seven-parameter solution as follows: In the formula, the coordinate data in the coordinate system {L} is converted to the coordinate system {G}, where [△X0,△Y0,△Z0] T represents the translation vector, ε X , ε Y , ε Z The three parameters representing the rotation of the coordinate system {L} around the X, Y, and Z axes, i represents the point position, R is the matrix used for transformation, and m represents the scale parameter. If m is equal to 0, it means that the scales of the two coordinate systems are the same; in R(e)=R(e X )R(e Y )R(e Z ) In the formula, ε X , ε Y , ε Z Three parameters representing the rotation of the coordinate system {L} around the X, Y, and Z axes.

5. The method for accurately measuring and solving the position and posture of a ship section according to claim 1 is characterized in that: The step 3 is specifically as follows: Step 3.2, total station-global coordinate system conversion based on Bursa seven parameters; When the rotation angle is small, the rotation angle is set to ε x =ε y =ε z =0, simplifying the calculation process; in the case of small-angle coordinate transformation between the total station coordinate system and the global coordinate system, the seven-parameter Bursa-Wolf model is used for coordinate system transformation. The rotation matrix in the Bursa model is: So we can get: So the Bursa model can also be expressed as: Then the coordinate composite transformation expression can be equivalently converted to the following formula: When there are n (n≥3) points that need to be transformed, the coordinates are: p1(x1, y2, z3)...p n (x n ,y n , z n ), the coordinates of the corresponding point after transformation are p1'(x i1 ,y i1 , z i1 )……p n (x in ,y in , z in ), according to the principle of matrix operation, the corresponding conversion formula is listed as follows: The above formula can be expressed as: According to the least squares principle, it can be deduced as When the measurement points used for coordinate transformation are all of equal weight, then: The seven transformation parameters of the Bursa model are obtained using the above formula, and then the measurement data is transformed from the {L} coordinate system to the {G} coordinate system.

6. The method for accurately measuring and solving the position and posture of a ship section according to claim 1 is characterized in that: The step 3 is specifically as follows: Step 3.3, global-moving total segment coordinate system transformation based on Taylor expansion seven parameters; When used for coordinate transformation from global coordinates to moving total segment coordinate system, the angle deviation is large. When constructing the seven-parameter equation, the following formula is transformed using Taylor expansion to obtain the seven-parameter model of Taylor expansion linearization; Thus, the rotation matrix in the Bursa model whose angle is approximated to 0 is replaced, and the replacement result is as follows: in Using the indirect adjustment method, the revised values ​​of seven parameters were obtained; Based on the principle of indirect adjustment, the following formula is obtained: [δX0δY0δZ0δμδαδβδγ] T =[A T [PA] -1 A T Pl Where P represents the weight matrix; Through the iterative process, the correction accuracy of the parameters is set. In each round of iteration, the initial estimate of the parameter is composed of the sum of the previous correction value and the approximate value. When the iteration reaches the predetermined accuracy requirement, the final calculation result is output.

7. The method for accurately measuring and solving the position and posture of a ship section according to claim 1 is characterized in that: The step 3 is specifically as follows: Step 4.1: Initial matching of the total segment pose based on the three-point method; In order to ensure the effectiveness of the matching algorithm, the three-point alignment method is used for rough matching of data. First, three non-collinear points P1, P2, and P3 are selected on the initial point set of the moving total segment, and then three corresponding points Q1, Q2, and Q3 are selected on the target point set. The three pairs of points are used to construct the moving total segment measurement coordinate system and the fixed total segment measurement coordinate system respectively. In the moving segment, take point P1 as the coordinate origin, and the direction vector from P1 to P2 as the x-axis to construct the unit vector: Then determine the y-axis by the cross product of the direction vector from P1 to P3 and the x-axis direction vector, and construct the y-axis unit vector: Finally, the cross product of e1 and e2 is used to determine the direction of the z-axis, and its unit vector is: e3=e1×e2 Similarly, in the fixed total segment, use Q1, Q2, and Q3 to determine the coordinate axes: e3′=e1′×e2′ The unit vector groups [e1, e2, e3] and [e1', e2', e3'] are the coordinate axes of the two local coordinate systems, thus forming two local coordinate systems. After coordinate transformation, the two coordinate systems should completely coincide, and the two coordinate systems have the following relationship [e1', e2', e3'] T *k'=[e1,e2,e3] T *K, where K and k' are arbitrary directions in the two coordinate systems, then: <h2 style=";text-align:left;direction:ltr">k' = ([e1',e2',e3']<h2 style=";text-align:left;direction:ltr"> T <h2 style=";text-align:left;direction:ltr"> )<h2 style=";text-align:left;direction:ltr"> -1 <h2 style=";text-align:left;direction:ltr"> *[e1,e2,e3]<h2 style=";text-align:left;direction:ltr"> T <h2 style=";text-align:left;direction:ltr"> *K Among them, [e1', e2', e3'] is an orthogonal matrix, so the initial matching rotation transformation matrix of the total segment initial point set and the target point set can be expressed as: According to the obtained rotation vector, determine the translation vector T: This results in the initial transformation matrix of the measured data.

8. The method for accurately measuring and calculating the position and posture of a ship section according to claim 7 is characterized in that: The step 3 is specifically as follows: Step 4.2: Accurately match the total segment pose based on improved ICP; After the above initial matching of the surface measurement data of the fixed and mobile sections of the ship, the iterative closest point algorithm based on the KD tree search principle is used to accurately match the measurement point set; Step 4.2.

1. ICP algorithm Define the centroid positions p and q of two sets of points: Compute the de-centroided coordinates of each point: p i =P i -p q i =Q i -q Define the error term for the i-th point pair: e i =P i -(R·Q i +T) Construct the least squares problem to minimize the sum of squared errors The above formula can be expressed as: The optimal rotation matrix R can be solved by calculating the minimum value of the left half of the above formula: Because R T R=I, then the above formula is: Defining the Matrix Performing singular value decomposition on the matrix W yields the following formula: W=U∑V T Where ∑ is a diagonal matrix composed of singular values. When W is full rank, the rotation matrix and translation vector can be obtained: R=UV T The initial R and T obtained by the three-point matching method are substituted into the error square sum formula. If the requirements are not met, the new rotation and translation matrix is ​​calculated. The iteration ends when f(R, T) reaches the termination threshold, and the rotation matrix and translation matrix are output as the final value. Step 4.2.2, KD tree improvement Construct a KD tree for the initial point set and the target point set to speed up the search for the nearest neighbor points; Starting from the root node, determine the positional relationship between the input point (i.e. the point in the initial point set) and the root node. If its coordinates are smaller than those of the root node, enter the left child node; if its coordinates are larger than those of the root node, enter the right child node, and keep moving down the node until it reaches a leaf node, which is used as the "current nearest node"; Perform a "backtracking" operation on the point. If there is a closer point on other subspace nodes on the search path, jump to the subspace node to search for the closest point, and update the "current closest node"; Repeat the above steps until the search path is empty and the search ends to find the nearest node; Step 4.2.3: Euler angle solution After solving the rotation matrix, the rotation matrix can be converted into the deviation angle by the rotation angle calculation formula: Where: α, β, γ are the rotation angles of the total segment target posture relative to the initial posture, R 23 is the value of the 2nd row and 3rd column of the rotation matrix, R 33 is the value of the third row and third column of the rotation matrix R, R 13 is the value of the first row and third column of the rotation matrix R, R 12 is the value of the first row and second column of the rotation matrix R, R 11 is the value of the first row and first column of the rotation matrix R.

Images