A method for measuring and aligning a telescopic mechanism in a long and narrow scene

Abstract

The application discloses a method for measuring and aligning a posture of a telescopic mechanism before penetrating into a long and narrow scene. First, a coordinate system is established to convert the posture adjustment problem of the penetrating target and the telescopic mechanism into a coordinate system alignment problem. Then, the positional deviation and the angular deviation of the penetrating target coordinate system and the telescopic mechanism coordinate system are calculated. Finally, the posture of the telescopic mechanism is adjusted according to the deviation, thereby solving the problem of aligning the large-stroke telescopic mechanism and the penetrating target posture in the prior art. The application can adjust the posture of the telescopic mechanism and the penetrating target before assembly, thereby ensuring that the telescopic axis of the telescopic mechanism and the central axis of the penetrating target are coaxial before assembly.

Description

Technical Field

[0001] This invention belongs to the technical field of telescopic mechanism posture adjustment, specifically relating to a method for measuring and aligning the posture of a telescopic mechanism before it enters a narrow scene. Background Technology

[0002] When using a long-stroke telescopic mechanism to insert into a narrow, elongated part, it is essential to ensure that the telescopic axis of the mechanism is aligned with the central axis of the narrow, elongated part. If there is a significant deviation between the telescopic axis and the central axis of the narrow, elongated part, the mechanism will struggle to insert smoothly, or interference may occur between the mechanism and the part during insertion, ultimately leading to insertion failure.

[0003] In the prior art, there are also methods for assembling components without a fixed measuring length. For example, the patent application with publication number "CN118239003B" discloses a technical solution for adjusting the orientation and assembling large components during the assembly process.

[0004] The aforementioned existing technology is not applicable to scenarios where the attitude adjustment mechanism cannot cooperate to establish an attitude adjustment coordinate system in real time. Furthermore, its attitude adjustment and alignment process is implemented during the real-time assembly of the parts, rather than before the parts are aligned. During the insertion of the long-stroke telescopic mechanism and the narrow part, since there is almost no adjustment space between the telescopic mechanism and the narrow part's internal space, it is actually difficult to adjust the relative position between the long-stroke telescopic mechanism and the narrow part during the assembly process.

[0005] Therefore, in view of the fact that existing alignment methods in the assembly process are not suitable for long strokes, small internal spaces, and lack of posture adjustment conditions during assembly, this invention discloses a method for measuring and aligning the posture of a telescopic mechanism before it enters a narrow and long scene. Summary of the Invention

[0006] This invention discloses a method for measuring and aligning the attitude of a telescopic mechanism before it is inserted into a narrow scene. This method can adjust and align the telescopic mechanism with the insertion target before assembly, thereby ensuring that the telescopic axis of the telescopic mechanism and the central axis of the insertion target are coaxial before assembly.

[0007] This invention is achieved through the following technical solution:

[0008] A method for measuring and aligning the attitude of a telescopic mechanism before it enters a narrow scene includes the following steps:

[0009] Step 1: Establish a target coordinate system on the penetrating target and read the theoretical coordinates of the measured feature points of the penetrating target; establish a mechanism coordinate system on the telescopic mechanism and read the theoretical coordinates of the measured feature points of the telescopic mechanism.

[0010] Step 2: Measure the coordinates of the target measurement feature points in the world coordinate system, and measure the coordinates of the telescopic mechanism measurement feature points in the world coordinate system.

[0011] Step 3: Calculate the first transformation relationship between the target coordinate system and the world coordinate system based on the deviation between the theoretical coordinates and the measured coordinates of the target measurement feature points; calculate the second transformation relationship between the mechanism coordinate system and the world coordinate system based on the deviation between the theoretical coordinates and the measured coordinates of the telescopic mechanism measurement feature points.

[0012] Step 4: Calculate the attitude deviation characterization value between the target coordinate system and the mechanism coordinate system based on the first transformation relationship and the second transformation relationship;

[0013] Step 5: Adjust the position of the telescopic mechanism according to the attitude deviation characterization value so that the motion axis of the telescopic mechanism is aligned with the axis of the target.

[0014] To better realize the present invention, step 3 further includes:

[0015] Step 3.1: Calculate the first rotation matrix and the first translation matrix between the target coordinate system and the world coordinate system;

[0016] Step 3.2: Calculate the second rotation matrix and the second translation matrix between the world coordinate system and the mechanism coordinate system;

[0017] Step 3.3: Calculate the relative coordinates of the origin of the target coordinate system in the mechanism coordinate system, and calculate the position deviation based on the relative coordinates and the coordinates of the origin of the mechanism coordinate system;

[0018] Step 3.4: Calculate the first rotation angle of the first rotation matrix under the XYZ rotation order, calculate the second rotation angle of the second rotation matrix under the ZYX rotation order, and calculate the angle deviation based on the first rotation angle and the second rotation angle.

[0019] To better realize the present invention, step 3.3 further includes:

[0020] Step 3.3.1: Extract the origin coordinates AP0 of the target coordinate system, and transform the origin coordinates AP0 from the target coordinate system to the world coordinate system using the first rotation matrix and the first translation matrix to obtain the transformed origin coordinates AW0;

[0021] Step 3.3.2: Transform the origin coordinates AW0 from the world coordinate system to the mechanism coordinate system using the second rotation matrix and the second translation matrix to obtain the final origin coordinates AC0;

[0022] Step 3.3.3: Extract the mechanism coordinates BC0 of the center of the moving arm end of the telescopic mechanism, and calculate the positional deviation by comparing the final origin coordinates AC0 with the mechanism coordinates BC0.

[0023] To better realize the present invention, the calculation formula for the position deviation is further as follows:

[0024] △t1=AC0-BC0;

[0025] Where: △t1 represents the position deviation; AC0 represents the final origin coordinates; BC0 represents the mechanism coordinates of the center of the moving arm end of the telescopic mechanism.

[0026] To better realize the present invention, step 3.4 further includes:

[0027] Step 3.4.1: Calculate the first three-axis rotation representation of the target coordinate system around the X-axis, Y-axis, and Z-axis to the world coordinate system based on the first rotation matrix, and calculate the first rotation angle based on the first three-axis rotation representation.

[0028] Step 3.4.2: Calculate the second three-axis rotation representation quantities of the world coordinate system rotating around the X-axis, Y-axis, and Z-axis to the mechanism coordinate system based on the second rotation matrix, and calculate the second rotation angle based on the second three-axis rotation representation quantities;

[0029] Step 3.4.3: Calculate the angle deviation based on the first rotation angle and the second rotation angle.

[0030] To better realize the present invention, the calculation formula for the angle deviation is further as follows:

[0031] △t2=θ1+θ2;

[0032] Where: △t2 represents the angular deviation; θ1 represents the first rotation angle; θ2 represents the second rotation angle.

[0033] To better realize the present invention, step 3.1 further includes:

[0034] Step 3.1.1: Set several target feature points with different axes on the target and read the theoretical coordinates and measured coordinates of the target feature points;

[0035] Step 3.1.2: Calculate the theoretical centroid based on the theoretical coordinates, and calculate the measured centroid based on the measured coordinates; perform centroid removal processing on the theoretical centroid to obtain the theoretical centroid-removed coordinates, and perform centroid removal processing on the measured centroid to obtain the measured centroid-removed coordinates.

[0036] Step 3.1.3: Based on theoretical centroid coordinate removal and measurement centroid coordinate removal, establish an optimization objective function, and perform singular value decomposition on the optimization objective function;

[0037] Step 3.1.4: Calculate the first rotation matrix and the first translation matrix based on the results of singular value decomposition.

[0038] To better realize the present invention, step 3.2 further includes:

[0039] Step 3.2.1: Set several telescopic mechanism feature points with different axes on the fixed arm of the telescopic mechanism, and read the theoretical coordinates and measured coordinates of the telescopic mechanism feature points;

[0040] Step 3.2.2: Calculate the theoretical centroid based on the theoretical coordinates, and calculate the measured centroid based on the measured coordinates; perform centroid removal processing on the theoretical centroid to obtain the theoretical centroid-removed coordinates, and perform centroid removal processing on the measured centroid to obtain the measured centroid-removed coordinates.

[0041] Step 3.2.3: Based on theoretical centroid coordinate removal and measurement centroid coordinate removal, establish an optimization objective function, and perform singular value decomposition on the optimization objective function;

[0042] Step 3.2.4: Calculate the second rotation matrix and the second translation matrix based on the results of singular value decomposition.

[0043] To better realize the present invention, the optimization objective function is further defined as follows:

[0044] ;

[0045] Where: W represents the objective function; m1-m n This represents the theoretical centroid coordinates of n points; m'1-m' n This represents the centroid coordinates of n points; T represents the matrix transpose.

[0046] Compared with the prior art, the present invention has the following advantages and beneficial effects:

[0047] This invention first establishes a coordinate system to transform the problem of adjusting the posture of the penetration target and the telescopic mechanism into a problem of aligning the coordinate system; then, it calculates the positional and angular deviations between the coordinate system of the penetration target and the coordinate system of the telescopic mechanism; finally, it adjusts the posture of the telescopic mechanism according to the deviation, thus solving the problem of aligning the posture of the large-stroke telescopic mechanism with the penetration target in the prior art before the penetration assembly is performed. Attached Figure Description

[0048] Figure 1 A schematic diagram showing the arrangement of the target penetration mechanism and the telescopic mechanism;

[0049] Figure 2 This is a schematic diagram illustrating the calculation process for attitude deviation characterization values;

[0050] Figure 3This is a schematic diagram illustrating the calculation process for rotation and translation matrices.

[0051] Figure 4 A schematic diagram showing the alignment of the central axis of the penetration into the target with the telescopic axis of the telescopic mechanism;

[0052] Figure 5 This is a schematic diagram of each coordinate system. Detailed Implementation

[0053] Example 1:

[0054] This embodiment provides a method for measuring and aligning the attitude of a telescopic mechanism before it enters a narrow and elongated scene, including the following steps:

[0055] Step 1: Establish a target coordinate system on the penetrating target and read the theoretical coordinates of the measured feature points of the penetrating target; establish a mechanism coordinate system on the telescopic mechanism and read the theoretical coordinates of the measured feature points of the telescopic mechanism.

[0056] Step 2: Measure the coordinates of the target measurement feature points in the world coordinate system, and measure the coordinates of the telescopic mechanism measurement feature points in the world coordinate system.

[0057] Step 3: Based on the deviation between the theoretical coordinates and the measured coordinates of the target measurement feature points, calculate the first transformation relationship between the target coordinate system and the world coordinate system; based on the deviation between the theoretical coordinates and the measured coordinates of the telescopic mechanism measurement feature points, calculate the second transformation relationship between the mechanism coordinate system and the world coordinate system.

[0058] Step 4: Calculate the attitude deviation characterization value between the target coordinate system and the mechanism coordinate system based on the first transformation relationship and the second transformation relationship;

[0059] Step 5: Adjust the position of the telescopic mechanism according to the attitude deviation characterization value so that the motion axis of the telescopic mechanism is aligned with the axis of the target.

[0060] Furthermore, step 3 specifically includes:

[0061] Step 3.1: Calculate the first rotation matrix and the first translation matrix between the target coordinate system and the world coordinate system;

[0062] Step 3.2: Calculate the second rotation matrix and the second translation matrix between the world coordinate system and the mechanism coordinate system;

[0063] Step 3.3: Calculate the relative coordinates of the origin of the target coordinate system in the mechanism coordinate system, and calculate the position deviation based on the relative coordinates and the coordinates of the origin of the mechanism coordinate system;

[0064] Step 3.4: Calculate the first rotation angle of the first rotation matrix under the XYZ rotation order, calculate the second rotation angle of the second rotation matrix under the ZYX rotation order, and calculate the angle deviation based on the first rotation angle and the second rotation angle.

[0065] Furthermore, step 3.1 specifically includes:

[0066] Step 3.1.1: Set several target feature points with different axes on the target and read the theoretical coordinates and measured coordinates of the target feature points;

[0067] Step 3.1.2: Calculate the theoretical centroid based on the theoretical coordinates, and calculate the measured centroid based on the measured coordinates; perform centroid removal processing on the theoretical centroid to obtain the theoretical centroid-removed coordinates, and perform centroid removal processing on the measured centroid to obtain the measured centroid-removed coordinates.

[0068] Step 3.1.3: Based on theoretical centroid coordinate removal and measurement centroid coordinate removal, establish an optimization objective function, and perform singular value decomposition on the optimization objective function;

[0069] Step 3.1.4: Calculate the first rotation matrix and the first translation matrix based on the results of singular value decomposition.

[0070] Furthermore, step 3.2 specifically includes:

[0071] Step 3.2.1: Set several telescopic mechanism feature points with different axes on the fixed arm of the telescopic mechanism, and read the theoretical coordinates and measured coordinates of the telescopic mechanism feature points;

[0072] Step 3.2.2: Calculate the theoretical centroid based on the theoretical coordinates, and calculate the measured centroid based on the measured coordinates; perform centroid removal processing on the theoretical centroid to obtain the theoretical centroid-removed coordinates, and perform centroid removal processing on the measured centroid to obtain the measured centroid-removed coordinates.

[0073] Step 3.2.3: Based on theoretical centroid coordinate removal and measurement centroid coordinate removal, establish an optimization objective function, and perform singular value decomposition on the optimization objective function;

[0074] Step 3.2.4: Calculate the second rotation matrix and the second translation matrix based on the results of singular value decomposition.

[0075] Furthermore, the optimization objective function is:

[0076] ;

[0077] Where: W represents the objective function; m1-m n This represents the theoretical centroid coordinates of n points; m'1-m' n This represents the centroid coordinates of n points; T represents the matrix transpose.

[0078] Example 2:

[0079] This embodiment discloses a method for measuring and aligning the attitude of a telescopic mechanism before it enters a narrow scene. It is an improvement upon Embodiment 1, and step 3.3 specifically includes:

[0080] Step 3.3.1: Extract the origin coordinates AP0 of the target coordinate system, and transform the origin coordinates AP0 from the target coordinate system to the world coordinate system using the first rotation matrix and the first translation matrix to obtain the transformed origin coordinates AW0;

[0081] Step 3.3.2: Transform the origin coordinates AW0 from the world coordinate system to the mechanism coordinate system using the second rotation matrix and the second translation matrix to obtain the final origin coordinates AC0;

[0082] Step 3.3.3: Extract the mechanism coordinates BC0 of the center of the moving arm end of the telescopic mechanism, and calculate the positional deviation by comparing the final origin coordinates AC0 with the mechanism coordinates BC0.

[0083] Furthermore, the formula for calculating the positional deviation is:

[0084] △t1=AC0-BC0;

[0085] Where: △t1 represents the position deviation; AC0 represents the final origin coordinates; BC0 represents the mechanism coordinates of the center of the moving arm end of the telescopic mechanism.

[0086] The relationships between the target coordinate system, the world coordinate system, and the mechanism coordinate system are as follows: Figure 5 As shown, AP0 as a whole symbol represents the origin coordinates of the target coordinate system, AW0 as a whole symbol represents the origin coordinates of the world coordinate system, AC0 as a whole symbol represents the origin coordinates of the mechanism coordinate system, and BC0 as a whole symbol represents the coordinates of the center of the end effector of the moving arm.

[0087] The rest of this embodiment is the same as that of Embodiment 1, so it will not be described again.

[0088] Example 3:

[0089] This embodiment discloses a method for measuring and aligning the attitude of a telescopic mechanism before it enters a narrow scene. It is an optimization based on embodiment 1 or 2. Step 3.4 specifically includes:

[0090] Step 3.4.1: Calculate the first three-axis rotation representation of the target coordinate system around the X-axis, Y-axis, and Z-axis to the world coordinate system based on the first rotation matrix, and calculate the first rotation angle based on the first three-axis rotation representation.

[0091] Step 3.4.2: Calculate the second three-axis rotation representation quantities of the world coordinate system rotating around the X-axis, Y-axis, and Z-axis to the mechanism coordinate system based on the second rotation matrix, and calculate the second rotation angle based on the second three-axis rotation representation quantities;

[0092] Step 3.4.3: Calculate the angle deviation based on the first rotation angle and the second rotation angle.

[0093] Furthermore, the formula for calculating the angular deviation is:

[0094] △t2=θ1+θ2;

[0095] Where: △t2 represents the angular deviation; θ1 represents the first rotation angle; θ2 represents the second rotation angle.

[0096] The rest of this embodiment is the same as that of embodiment 1 or 2, so it will not be described again.

[0097] Example 4:

[0098] This embodiment discloses a method for measuring and aligning the attitude of a telescopic mechanism before it enters a narrow scene. It is an optimization based on any one of embodiments 1-3, specifically as follows:

[0099] At the end of the penetration, a target coordinate system P is established with the penetration point as the origin and the penetration target axis as the Y-axis; at the end of the telescopic mechanism, when the telescopic mechanism is in the retracted state (i.e., the telescopic mechanism is in zero position), a mechanism coordinate system C is established with the center of the end of the moving arm as the origin and the moving arm direction as the Y-axis.

[0100] The extracted feature points of the penetrating target are distributed in three regions: the front, middle, and rear sections, with a maximum of three feature points. The theoretical coordinate values ​​of these feature points are read in the target coordinate system P and denoted as {AP1, AP2, ..., APn}. The feature points of the telescopic mechanism are distributed in three regions: the front, middle, and rear sections of the fixed arm, with a maximum of three feature points. When the telescopic mechanism is in the retracted state (i.e., at zero position), the theoretical coordinate values ​​of the feature points in the mechanism coordinate system C are read and denoted as {BC1, BC2, ..., BCn}. AP1-APn represent the coordinate values ​​of the first to nth feature points of the penetrating target in the target coordinate system, and BC1-BCn represent the coordinate values ​​of the first to nth feature points of the telescopic mechanism in the mechanism coordinate system.

[0101] Step 2: Measure the coordinates of the feature points.

[0102] As attached Figure 1As shown, a laser tracker is used as the measuring device. A suitable position is selected to ensure that all feature points can be measured to avoid changing stations. The laser tracker is fixed and a world coordinate system W is established with the laser tracker. The coordinates of the feature points of the penetrating target and the telescopic mechanism in the world coordinate system are measured respectively and denoted as {AW1,AW2,...,AWn} and {BW1,BW2,...,BWn}.

[0103] The above AW1-AWn represent the coordinate values ​​of the first to the nth penetration target feature points in the world coordinate system, and the above BW1-BWn represent the coordinate values ​​of the first to the nth telescopic mechanism feature points in the world coordinate system.

[0104] First, based on the theoretical and measured coordinate values ​​of the target penetration point and the telescopic mechanism's feature points, calculate the transformation relationship between the target coordinate system P and the world coordinate system W, i.e., the first rotation matrix RPW and the first translation matrix TPW. Then, calculate the transformation relationship between the world coordinate system W and the mechanism coordinate system C, i.e., the second rotation matrix RWC and the second translation matrix TWC. Next, use the transformation relationship to obtain the attitude deviation characterization values ​​between the target coordinate system P and the mechanism coordinate system C. These attitude deviation characterization values ​​include position deviation Δt1 and angle deviation Δt2. Adjust the position and attitude of the telescopic mechanism according to the attitude deviation characterization values ​​to align the motion axis of the telescopic mechanism's moving arm with the target penetration axis. The overall process for calculating the attitude deviation characterization values ​​is attached. Figure 2 As shown, the specific implementation steps are as follows:

[0105] Calculate the transformation relationship between the target coordinate system P and the world coordinate system W, i.e., calculate the first rotation matrix RPW and the first translation matrix TPW. The calculation process for the coordinate transformation matrix is ​​shown in the appendix. Figure 3 As shown, using the theoretical coordinates {AP1,AP2,...,APn} of the target feature points and the measured coordinates {AW1,AW2,...,AWn}, the first rotation matrix RPW and the first translation matrix TPW between the target coordinate system P and the world coordinate system W are calculated. The coordinates AW of any point AP with known theoretical coordinates on the target can be calculated in the world coordinate system W by AW=RPW·AP+TPW.

[0106] Calculate the transformation relationship between the world coordinate system W and the mechanism coordinate system C, i.e., calculate the second rotation matrix RWC and the second translation matrix TWC. The flowchart is attached. Figure 3 As shown, the theoretical coordinate values ​​{BC1,BC2,...,BCn} and measured coordinate values ​​{BW1,BW2,...,BWn} of the fixed arm feature points of the telescopic mechanism are used to calculate the second rotation matrix RWC and the second translation matrix TWC between the mechanism coordinate system C and the world coordinate system W. Any point DW in the known world coordinate system can have its coordinate DC calculated in the mechanism coordinate system C using DC=RWC·DW+TWC.

[0107] Calculate the coordinates AC0 of the origin A0 of the target coordinate system P in the mechanism coordinate system C. The coordinate transformation of point A0 consists of two steps: First, transform the coordinates of A0 from the target coordinate system P to the world coordinate system W using AW0 = RPW·AP0 + TPW, and denote it as AW0; Second, transform AW0 from the world coordinate system W to the mechanism coordinate system C using AC0 = RWC·AW0 + TWC.

[0108] To calculate the position deviation Δt1, the point of penetration is the origin AP0 of the target coordinate system P, and the center point of the telescopic mechanism's moving arm is the origin BC0 of the mechanism coordinate system C. The calculation of the position deviation Δt1 requires first unifying the coordinate system. The coordinates AP0 of A0 are transformed to AC0 through the above steps to achieve coordinate system unification. Let AC0 = [AC0x, AC0y, AC0z], BC0 =

[000] . Then the final position deviation is: Δt1 = AC0 - BC0 = [AC0x, AC0y, AC0z].

[0109] Calculate the first rotation angle θ1 of the first rotation matrix RPW under the XYZ rotation order. According to the definition of the rotation matrix RXYZ(α, β, γ), the first rotation angle θ1 of the target coordinate system P around the X, Y, Z coordinate axes to the world coordinate system W is calculated as follows:

[0110] βPW=arcsin(RPW[0,2]);

[0111] αPW=arccos(RPW[0,2] / cos(βPW));

[0112] γPW=arccos(RPW[0,0] / cos(βPW));

[0113] Therefore, the first rotation angle θ1 = [αPW, βPW, γPW].

[0114] Calculate the second rotation angle θ2 of the second rotation matrix RWC under the ZYX rotation order.

[0115] According to the definition of the rotation matrix RZYX(α, β, γ), the second rotation angle θ2 of the world coordinate system W around the X, Y, Z coordinate axes to the mechanism coordinate system C is calculated as follows:

[0116] βWC=arcsin(-RPW[2,0]);

[0117] αWC=arccos(RWC[0,0] / cos(βWC));

[0118] γWC=arccos(RWC[2,2] / cos(βWC));

[0119] Therefore, the second rotation angle θ2 = [αWC, βWC, γWC].

[0120] The angular deviation Δt2 is the angle by which the target coordinate system P rotates around the X, Y, and Z coordinate axes to the mechanism coordinate system C. The target coordinate system P first rotates to the world coordinate system W through the first rotation angle θ1, and then rotates to the mechanism coordinate system C through the second rotation angle θ2. Therefore, the angular deviation is the superposition of the first rotation angle and the second rotation angle, that is: Δt2 = θ1 + θ2 = [αPW + αWC, βPW + βWC, γPW + γWC].

[0121] The position and attitude of the telescopic mechanism are adjusted based on the attitude deviation values, specifically the angle deviation Δt2 and the position deviation Δt1. This ensures that the center point of the telescopic mechanism's moving arm end is aligned with the target insertion point, and the direction of the telescopic mechanism's movement is aligned with the target insertion direction. The adjusted position and attitude are shown in the attached figure. Figure 4 As shown.

[0122] The rest of this embodiment is the same as any one of embodiments 1-3, so it will not be described again.

[0123] The above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention in any way. Any simple modifications or equivalent changes made to the above embodiments based on the technical essence of the present invention shall fall within the protection scope of the present invention.

Claims

1. A method for measuring and aligning the attitude of a telescopic mechanism before it enters a narrow scene, characterized in that, Includes the following steps: Step 1: Establish a target coordinate system on the target and read the theoretical coordinates of the target's measurement feature points; Establish a coordinate system on the telescopic mechanism and read the theoretical coordinates of the measured feature points of the telescopic mechanism; Step 2: Measure the coordinates of the target measurement feature points in the world coordinate system, and measure the coordinates of the telescopic mechanism measurement feature points in the world coordinate system. Step 3: Calculate the first transformation relationship between the target coordinate system and the world coordinate system based on the deviation between the theoretical coordinates and the measured coordinates of the target measurement feature points; The second transformation relationship between the mechanism coordinate system and the world coordinate system is calculated based on the deviation between the theoretical coordinates and the measured coordinates of the feature points of the telescopic mechanism. Step 4: Calculate the attitude deviation characterization value between the target coordinate system and the mechanism coordinate system based on the first transformation relationship and the second transformation relationship; Step 5: Adjust the position of the telescopic mechanism according to the attitude deviation characterization value so that the motion axis of the telescopic mechanism is aligned with the axis of the target; Step 3 specifically includes: Step 3.1: Calculate the first rotation matrix and the first translation matrix between the target coordinate system and the world coordinate system; Step 3.2: Calculate the second rotation matrix and the second translation matrix between the world coordinate system and the mechanism coordinate system; Step 3.3: Calculate the relative coordinates of the origin of the target coordinate system in the mechanism coordinate system, and calculate the position deviation based on the relative coordinates and the coordinates of the origin of the mechanism coordinate system; Step 3.4: Calculate the first rotation angle of the first rotation matrix under the XYZ rotation order, calculate the second rotation angle of the second rotation matrix under the ZYX rotation order, and calculate the angle deviation based on the first rotation angle and the second rotation angle.

2. The method for measuring and aligning the attitude of a telescopic mechanism before insertion in a narrow scene according to claim 1, characterized in that, Step 3.3 specifically includes: Step 3.3.1: Extract the origin coordinates AP0 of the target coordinate system, and transform the origin coordinates AP0 from the target coordinate system to the world coordinate system using the first rotation matrix and the first translation matrix to obtain the transformed origin coordinates AW0; Step 3.3.2: Transform the origin coordinates AW0 from the world coordinate system to the mechanism coordinate system using the second rotation matrix and the second translation matrix to obtain the final origin coordinates AC0; Step 3.3.3: Extract the mechanism coordinates BC0 of the center of the moving arm end of the telescopic mechanism, and calculate the positional deviation by comparing the final origin coordinates AC0 with the mechanism coordinates BC0.

3. The method for measuring and aligning the attitude of a telescopic mechanism before insertion in a narrow scene according to claim 2, characterized in that, The formula for calculating the positional deviation is: △t1=AC0-BC0; Where: △t1 represents the position deviation; AC0 represents the final origin coordinates; BC0 represents the mechanism coordinates of the center of the moving arm end of the telescopic mechanism.

4. The method for measuring and aligning the attitude of a telescopic mechanism before insertion in a narrow scene according to claim 1, characterized in that, Step 3.4 specifically includes: Step 3.4.1: Calculate the first three-axis rotation representation of the target coordinate system around the X-axis, Y-axis, and Z-axis to the world coordinate system based on the first rotation matrix, and calculate the first rotation angle based on the first three-axis rotation representation. Step 3.4.2: Calculate the second three-axis rotation representation quantities of the world coordinate system rotating around the X-axis, Y-axis, and Z-axis to the mechanism coordinate system based on the second rotation matrix, and calculate the second rotation angle based on the second three-axis rotation representation quantities; Step 3.4.3: Calculate the angle deviation based on the first rotation angle and the second rotation angle.

5. The method for measuring and aligning the attitude of a telescopic mechanism before insertion in a narrow scene according to claim 4, characterized in that, The formula for calculating the angle deviation is: △t2=θ1+θ2; Where: △t2 represents the angular deviation; θ1 represents the first rotation angle; θ2 represents the second rotation angle.

6. A method for measuring and aligning the attitude of a telescopic mechanism before insertion in a narrow scene according to any one of claims 1-5, characterized in that, Step 3.1 specifically includes: Step 3.1.1: Set several target measurement feature points with different axes on the target, and read the theoretical coordinates and measured coordinates of the target measurement feature points; Step 3.1.2: Calculate the theoretical centroid based on the theoretical coordinates, and calculate the measured centroid based on the measured coordinates; perform centroid removal processing on the theoretical centroid to obtain the theoretical centroid-removed coordinates, and perform centroid removal processing on the measured centroid to obtain the measured centroid-removed coordinates; Step 3.1.3: Based on theoretical centroid coordinate removal and measurement centroid coordinate removal, establish an optimization objective function, and perform singular value decomposition on the optimization objective function; Step 3.1.4: Calculate the first rotation matrix and the first translation matrix based on the results of singular value decomposition.

7. The method for measuring and aligning the attitude of a telescopic mechanism before insertion in a narrow scene according to claim 6, characterized in that, Step 3.2 specifically includes: Step 3.2.1: Set several telescopic mechanism feature points with different axes on the fixed arm of the telescopic mechanism, and read the theoretical coordinates and measured coordinates of the telescopic mechanism feature points; Step 3.2.2: Calculate the theoretical centroid based on the theoretical coordinates, and calculate the measured centroid based on the measured coordinates; perform centroid removal processing on the theoretical centroid to obtain the theoretical centroid-removed coordinates, and perform centroid removal processing on the measured centroid to obtain the measured centroid-removed coordinates. Step 3.2.3: Based on theoretical centroid coordinate removal and measurement centroid coordinate removal, establish an optimization objective function, and perform singular value decomposition on the optimization objective function; Step 3.2.4: Calculate the second rotation matrix and the second translation matrix based on the results of singular value decomposition.

8. The method for measuring and aligning the attitude of a telescopic mechanism before insertion in a narrow scene according to claim 7, characterized in that, The optimization objective function is: ； Where: W represents the optimization objective function; Represents the theoretical centroid coordinates of n points; This represents the centroid coordinates of n points; T represents the matrix transpose.

Images