A method for measuring the relative displacement and rotation angle of a rigid body

Abstract

The application discloses a kind of rigid body relative displacement and rotation angle measurement method, comprising S1: the direction vector L of laser beam sent by measurement device at initial time is obtained i =(x Li ,y Li ,z Li ) and the space coordinates (x i ,y i ,z i ) of light spot produced on position sensor by laser beam on equipment to be measured, wherein i indicates the i th laser beam, i=1,2,3,……;S2: the space coordinates (x' i ,y' i ,z' i ) of light spot produced on position sensor by laser beam at t time are obtained;S3: according to L i ,(x i ,y i ,z i ) and (x' i ,y' i ,z' i ) nonlinear equation group [x' i ,y' i ,z' i ] T -R x (α)R y (β)R z (γ)[x Li ,y Li ,z Li ] T const i -R x (α)R y (β)R z (γ)[x i ,y i ,z i ] T +[δx,δy,δz] T =0;S4: nonlinear equation group is solved, and the displacement δx,δy and δz of equipment to be measured relative to reference equipment along space coordinate axis X axis, Y axis and Z axis, and rotation angle α,β and γ are obtained.The measurement method according to the application can realize high-precision measurement of six degrees of freedom change of equipment to be measured, and the degree of automation is higher.

Description

Technical Field

[0001] The present invention relates to the fields of precision machining / assembly, vibration measurement, and in particular to a method for measuring the relative displacement and rotation angle of a rigid body. Background Art

[0002] Many industrial applications require accurate measurement of an object's spatial position or motion state, such as precision machining / assembly, robotic arm motion control, and creep / strain / vibration measurement. An object's spatial position is described by the spatial coordinates and rotation angle of a point on the object, while its motion state can be calculated by measuring changes in coordinates and rotation angles over time.

[0003] Current methods for precise position measurement require high precision in the installation of measurement equipment, making it impossible to simultaneously measure displacement and rotation angles in multiple directions. Furthermore, the measurement equipment is bulky and difficult to integrate with the system being measured. Furthermore, accurately determining an object's spatial position or motion state generally requires the use of multiple sensor groups and a measurement solution tailored to the requirements and sensor characteristics.

[0004] To this end, the present invention provides a method for measuring relative displacement and rotation angle of a rigid body, so as to at least partially solve the problems in the related art. Summary of the Invention

[0005] The Summary of the Invention introduces a series of simplified concepts that will be further described in the Detailed Description of the Invention. The Summary of the Invention is not intended to limit the key features and essential features of the claimed technical solution, nor is it intended to determine the scope of protection of the claimed technical solution.

[0006] In order to at least partially solve the above problems, the present invention provides a method for measuring relative displacement and rotation angle of a rigid body, which is characterized by comprising:

[0007] S1: Get the direction vector L of the laser beam emitted by the measuring device at the initial moment i =(x Li ,y Li , z Li ) and the spatial coordinates (x i ,y i , z i ), where i represents the i-th laser beam, i=1, 2, 3, ...;

[0008] S2: Obtain the spatial coordinates (x') of the light spot generated by the laser beam on the position sensor at time t. i , y' i , z'i );

[0009] S3: according to the L i =(x Li ,y Li ,z Li ), the (x i ,y i ,z i ) and the (x' i ,y' i ,z' i ), a nonlinear equation group is established:

[0010]

[0011] Wherein, R x (α)=[1 0 0;0 cosα -sinα;0 sinα cosα], R y (β)=[cosβ 0 sinβ;0 1 0;-sinβ 0 cosβ], R z (γ)=[cosγ -sinγ 0;sinγ cosγ 0;0 0 1], α, β and γ are respectively the rotation angles of the measured device relative to the reference device along the space coordinate axes X axis, Y axis and Z axis, δx, δy and δz are respectively the displacements of the measured device along the space coordinate axes X axis, Y axis and Z axis, const i is an unknown constant;

[0012] S4: the nonlinear equation group is solved to obtain the displacements δx, δy and δz of the measured device relative to the reference device along the space coordinate axes X axis, Y axis and Z axis, and the rotation angles α, β and γ.

[0013] According to the rigid body relative displacement and rotation angle measurement method, the direction vector of the laser beam emitted by the measurement device at the initial time and the space coordinates of the light spot generated by the laser beam on the position sensor of the measured device, and the space coordinates of the light spot generated by the laser beam on the position sensor at t time, a nonlinear equation group is established and solved, that is, the displacements δx, δy and δz of the measured device relative to the reference device along the space coordinate axes X axis, Y axis and Z axis, and the rotation angles α, β and γ are obtained, the high-precision measurement of six degrees of freedom can be realized, and the measurement precision of the space position coordinates of the measurement device at the initial time is not sensitive; In addition, in addition to measuring the space position coordinates of the measurement device at the initial time on site, subsequent measurement does not need on-site operation, only data acquisition and solving of nonlinear equation group are needed, and the degree of automation is high.

[0014] Optionally, the S1 comprises:

[0015] S101: A space coordinate system OXYZ is established, an origin O of the coordinate system being an intersection point of a photosensitive surface of the position sensor and a center line of the measured device, the space coordinate system OXYZ taking the center line of the measured device as a Z axis, taking a horizontal direction as an X axis, and taking a vertical direction as a Y axis;

[0016] S102: In the space coordinate system OXYZ, spatial coordinates (x si , y si , z si ) of an exit port of the measuring device from which the laser beam is emitted and a center of the position sensor and (x pi , y pi , z pi ) are obtained, coordinates (x oi , y oi ) of a light spot generated by the laser beam on the position sensor relative to the center of the position sensor are obtained;

[0017] S103: The L i =(x Li , y Li , z Li ) and (x i , y i , z i ) are obtained according to the (x si , y si , z si ), (x pi , y pi , z pi ) and (x oi , y oi ), wherein,

[0018] L i =(x Li , y Li , z Li )=(x pi +x oi -x si , y pi +y oi -y si , z pi -z si ) (2)

[0019] (x i , y i , z i )=(x pi +x oi , y pi +y oi , z pi ) (3).

[0020] Optionally, at time t, the spatial coordinate system OXYZ reaches the spatial coordinate system O'X'Y'Z' with the measured device, and the S2 comprises:

[0021] S201: In the spatial coordinate system O'X'Y'Z', the spatial coordinates (x pi , y pi , z pi ) of the center of the position sensor are obtained, and the coordinates (x' oi , y' oi ) of the light spot generated by the laser beam on the position sensor relative to the center of the position sensor are obtained.

[0022] S202: The (x' pi , y' pi , z' pi ) is obtained according to the (x oi , y oi , z i ) and the (x' i , y' i ), wherein,

[0023] (x' i , y' i , z' i ) = (x pi +x' oi , y pi +y' oi , z pi ) (4).

[0024] Optionally, in the S4, the non-linear equations are linearized by Newton algorithm, and the iterative solution is performed, wherein the iterative coefficient matrix J f is:

[0025]

[0026] Wherein, F i is the simplified form of the non-linear equations.

[0027] Optionally, in the S4, the convergence criteria for the iterative solution of the non-linear equations are that the root mean square residuals of δx, δy, δz, α, β and γ of two iterations are all less than 10 -4 .

[0028] Optionally, it further comprises solving the derivatives of (δx, δy, δz) and (α, β, γ) with respect to time, and obtaining the velocities (v x , v y , v z of the measured device moving along the spatial coordinate axes X, Y and Z.) and the angular velocity (ω) around the spatial coordinate axes X, Y, and Z x ,ω y ,ω z ).

[0029] Optionally, before performing S1, a collimator of the measuring device is mounted on the reference device, and a center line of the collimator forms a certain angle with a center line of the device under test.

[0030] Optionally, the collimator or the position sensor is adjusted so that the center line of the collimator passes through the center point of the photosensitive surface of the position sensor. BRIEF DESCRIPTION OF THE DRAWINGS

[0031] The following drawings of the present invention are incorporated herein as part of the present invention for understanding the present invention. The drawings show embodiments of the present invention and the description thereof is used to explain the principle of the present invention.

[0032] In the attached figure:

[0033] Figure 1 Schematic diagram of a flow chart of a method for measuring relative displacement and rotation angle of a rigid body according to a preferred embodiment of the present invention;

[0034] Figure 2 A structural intent of integrating a measuring device and a device under test for a method for measuring relative displacement and rotation angle of a rigid body according to a preferred embodiment of the present invention; and

[0035] Figure 3 Schematic diagram of the coordinate changes of the light spot generated by the laser beam on the position sensor of the device under test at the initial moment and moment t of the method for measuring the relative displacement and rotation angle of a rigid body according to a preferred embodiment of the present invention.

[0036] Description of reference numerals:

[0037] 110: Laser emitting device 111: Laser

[0038] 112: Optical Spectrometer 120: System Under Test

[0039] 121: Mounting bracket 122: Device under test

[0040] 123: Threaded guide 130: Position sensor

[0041] 131: First position sensor 132: Second position sensor

[0042] 133: Third position sensor 140: Collimator

[0043] 141: First collimator 142: Second collimator

[0044] 143: third collimator 150: optical fiber

[0045] 160: data acquisition and processing module 161: data acquisition device

[0046] 162: server DETAILED DESCRIPTION

[0047] In the following description, numerous specific details are set forth in order to provide a thorough understanding of the application. However, it will be apparent to one skilled in the art that the application can be practiced without one or more of these specific details. In other instances, well-known features have not been described in detail to avoid obscuring aspects of the application.

[0048] For a thorough understanding of the application, reference is made to the following detailed description in conjunction with the accompanying drawings. It is apparent that the application can be practiced without one or more of the specific details set forth herein. Certain terminology is used in the description and the claims below for the purpose of clarity in understanding the broadness and scope of the application. No limitation is intended to the specific details set forth in the description and claims below, which can be modified in various ways, and replaced with other details in order to implement the application.

[0049] It is to be understood that the terminology used herein is for the purpose of describing particular embodiments only and is not intended to be limiting. As used herein, the singular forms "a", "an" and "the" are intended to include the plural forms as well, unless the context clearly indicates otherwise. It will be further understood that the terms "comprises" and / or "comprising", when used in this specification, specify the presence of stated features, integers, steps, operations, elements, and / or components, but do not preclude the presence or addition of one or more other features, integers, steps, operations, elements, components, and / or groups thereof. As used herein, the terms "front", "rear", "upper", "lower", "right", "left", and similar terms are used for explanation only and not to limit.

[0050] The ordinal numbers such as "first" and "second" used in the application are merely used to identify the components and not to limit the number thereof. Also, the term "first component" does not mean that the "second component" is necessarily required, and the term "second component" does not mean that the "first component" is necessarily required.

[0051] Hereinafter, specific embodiments of the present application will be described in greater detail with reference to the accompanying drawings, which show representative embodiments of the present application and are not limiting the present application.

[0052] Reference Figure 1 and Figure 2Before the measurement method of the relative displacement and the rotation angle of the rigid body according to the present application is used, the measuring device needs to be connected to the measured system 120.

[0053] Specifically, the measuring device comprises a laser emitting device 110 and a position sensor 130. The measured system 120 comprises a reference device and a measured device 122.

[0054] The laser emitting device 110 is capable of emitting at least three laser beams. The laser emitting device 110 can comprise a laser 111, a beam splitter 112 and a collimator 140.

[0055] The laser 111 is used to generate a single laser beam. The beam splitter 112 is connected to the laser 111 to split the single laser beam generated by the laser 111. The collimator 140 is connected to the beam splitter 112 through an optical fiber 150 to emit the laser beams and adjust the emission direction of the laser beams.

[0056] It can be understood that in an embodiment not shown, a plurality of lasers 111, such as at least three lasers 111, can be arranged, and the collimator 140 is directly connected to the lasers 111 to emit a plurality of laser beams.

[0057] In the embodiment shown in Figure 2 , three collimators 140, i.e. a first collimator 141, a second collimator 142 and a third collimator 143, are provided. In this way, three laser beams can be emitted. It can be understood that in an embodiment not shown, the number of collimators 140 can be set according to the number of laser beams required, such as four, five or six, etc.

[0058] The collimator 140 is mounted on the reference device, such as can be mounted to the reference device through a mounting bracket 121 shown in Figure 2 . The mounting bracket 121 and the reference device are rigidly connected, such as the mounting bracket 121 is fixedly connected to the reference device by welding or screwing, etc.

[0059] In the embodiment shown in Figure 2 , the mounting bracket 121 comprises a threaded guide rail 123. The collimator 140 is sleeved on the threaded guide rail 123 and can slide along the threaded guide rail 123 and rotate around the threaded guide rail 123. In the process of arranging the collimator 140, the collimator 140 needs to be adjusted, such as moving or rotating the collimator 140 along the threaded guide rail 123, so that the center line of the collimator 140 and the center line of the measured device 122 form a certain angle to improve the measurement accuracy.

[0060] If the measurement accuracy of the position sensor 130 is δ, and the measurement error of the displacement of the device under test 122 along the center line direction of the device under test 122 is ε, then the angle between the center line of the collimator 140 and the center line of the device under test 122 is not less than arctan(δ / ε) to improve the measurement accuracy.

[0061] The position sensor 130 is mounted on the device under test 122 and maintains a rigid connection thereto. For example, the position sensor 130 may be fixed to the device under test 122 by welding or screwing. The position sensor 130 is configured to sense the laser beam emitted by the collimator 140. Specifically, the laser beam emitted by the collimator 140 forms a light spot on the position sensor 130. The position sensor 130 can be used to obtain the position coordinates of the light spot relative to the center of the position sensor 130.

[0062] The position sensor 130 may be a two-dimensional position sensor such as a light spot position sensor, a CCD camera, a CMOS camera, or a four-quadrant sensor.

[0063] exist Figure 2 In the illustrated embodiment, the number of position sensors 130 matches the number of collimators 140, i.e., three position sensors 130 are provided: a first position sensor 131, a second position sensor 132, and a third position sensor 133. The first position sensor 131 is used to sense the laser beam emitted by the first collimator 141, the second position sensor 132 is used to sense the laser beam emitted by the second collimator 142, and the third position sensor 133 is used to sense the laser beam emitted by the third collimator 143. In other words, there is a one-to-one correspondence between the position sensors 130 and the collimators 140.

[0064] In order to improve the measurement accuracy and ensure that the position sensor 130 can sense the laser beam emitted by the collimator 140 when the device under test 122 moves and / or rotates relative to the reference device, when arranging the collimator 140 and the position sensor 130, the collimator 140 or the position sensor 130 should be adjusted so that the intersection of the center line of the collimator 140 and the photosensitive surface of the position sensor 130 falls near the center point of the photosensitive surface, preferably so that the center line of the collimator 140 passes through the center point of the photosensitive surface of the position sensor 130.

[0065] The measuring device further includes a data acquisition and processing module 160 for acquiring and processing the electrical signal of the position sensor 130. Figure 2The data acquisition and processing module 160 comprises a data acquisition device 161 and a server 162. The data acquisition device 161 is connected with the position sensor 130 and the server 162 to acquire the electrical signals of the position sensor 130, such as the electrical signals related to the position coordinates of the light spot relative to the center of the position sensor 130, and transmit the electrical signals to the server 162 for processing. The server 162 can also be used to solve the nonlinear equation set mentioned below and output the parameters of the movement and / or rotation of the measured device 122 relative to the reference device.

[0066] The measuring device according to the present application has no special requirements for the arrangement positions of the laser 111, the beam splitter 112 and the data acquisition and processing module 160, and thus has strong compatibility with the measured system 120. Moreover, the measuring device has simple structure and small volume, and can be installed at a wide range of positions.

[0067] After the measuring device is connected with the measured system 120, a corresponding program can be started to monitor the spatial position or movement state of the measured device 122 relative to the reference device by using the rigid body relative displacement and rotation angle measuring method according to the present application.

[0068] Specifically, referring to Figure 1 , the rigid body relative displacement and rotation angle measuring method according to the present application comprises:

[0069] S1: obtaining the direction vector L of the laser beam emitted by the measuring device at the initial moment i = (x Li , y Li , z Li ) and the spatial coordinates (x i , y i , z i ) of the light spot generated by the laser beam on the position sensor 130 of the measured device 122, wherein i represents the i-th laser beam, i = 1, 2, 3, ….

[0070] Before obtaining the direction vector L i of the laser beam and the spatial coordinates (x i , y i , z i ) of the light spot, step S101 of establishing a spatial coordinate system OXYZ needs to be performed. The coordinate origin O of the spatial coordinate system OXYZ is the intersection point of the photosensitive surface of the position sensor 130 and the center line of the measured device 122, the spatial coordinate system OXYZ takes the center line of the measured device 122 as the Z axis, takes the horizontal direction as the X axis, and takes the vertical direction as the Y axis. For details, refer to Figure 2 .

[0071] After the spatial coordinate system OXYZ is established, step S102 is required, that is, the spatial coordinates (x ) of the exit port of the laser beam emitted by the measuring device, that is, the exit port of the collimator 140, are measured and obtained in the spatial coordinate system OXYZ. si ,y si , z si ), the spatial coordinates (x pi ,y pi , z pi ), and the coordinates (x oi ,y oi ).

[0072] After the above-mentioned spatial coordinates are measured and obtained, the si ,y si , z si )、(x pi ,y pi , z pi ) and (x oi ,y oi ) Get the direction vector L of the laser beam i =(x Li ,y Li , z Li ) and the spatial coordinates of the light spot (x i ,y i , z i ), that is, proceed to step S103.

[0073] L i =(x Li ,y Li , z Li )=(x pi +x oi -x si ,y pi +y oi -y si , z pi -z si ) (2)

[0074] (x i ,y i , z i )=(x pi +x oi ,y pi +y oi , z pi ) (3).

[0075] In the following, three collimators 140 and three position sensors 130 are used as an example to calculate the direction vector L of the laser beam at the initial moment.i and the spatial coordinates (x i , y i , z i ) of the light spots are described.

[0076] Referring to Figure 2 and Figure 3 , in the spatial coordinate system OXYZ, the coordinates of the exit port of the first collimator 141 are (x s1 , y s1 , z s1 ), the coordinates of the exit port of the second collimator 142 are (x s2 , y s2 , z s2 ), and the coordinates of the exit port of the third collimator 143 are (x s3 , y s3 , z s3 ); the coordinates of the center point of the first position sensor 131 are (x p1 , y p1 , z p1 ), the coordinates of the center point of the second position sensor 132 are (x p2 , y p2 , z p2 ), and the coordinates of the center point of the third position sensor 133 are (x p3 , y p3 , z p3 ); the coordinates of the first light spot generated on the first position sensor 131 relative to the center of the first position sensor 131 are (x o1 , y o1 ), the coordinates of the second light spot generated on the second position sensor 132 relative to the center of the second position sensor 132 are (x o2 , y o2 ), and the coordinates of the third light spot generated on the third position sensor 133 relative to the center of the third position sensor 133 are (x o3 , y o3 ). According to formulas (2) and (3), the direction vectors L1, L2 and L3 of the laser beams emitted by the first collimator 141, the second collimator 142 and the third collimator 143, respectively, and the spatial coordinates (x1, y1, z1), (x2, y2, z2) and (x3, y3, z3) of the first light spot, the second light spot and the third light spot in the spatial coordinate system OXYZ can be obtained, as follows:

[0077] L1 = (x L1 , y L1 , z L1 ) = (x p1 + x o1 - x s1 , yp1 + y o1 - y s1 , z p1 - z s1 ) (6)

[0078] L2 = (x L2 , y L2 , z L2 ) = (x p2 + x o2 - x s2 , y p2 + y o2 - y s2 , z p2 - z s2 ) (7)

[0079] L3 = (x L3 , y L3 , z L3 ) = (x p3 + x o3 - x s3 , y p3 + y o3 - y s3 , z p3 - z s3 ) (8)

[0080] (x1, y1, z1) = (x p1 + x o1 , y p1 + y o1 , z p1 ) (9)

[0081] (x2, y2, z2) = (x p2 + x o2 , y p2 + y o2 , z p2 ) (10)

[0082] (x3, y3, z3) = (x p3 + x o3 , y p3 + y o3 , z p3 ) (11)

[0083] At time t, the spatial coordinate system OXYZ reaches the spatial coordinate system O'X'Y'Z' with the measured device 122, with reference to Figure 3Since the position sensor 130 is rigidly connected with the measured device 122, the spatial coordinates of the center of the position sensor 130 in the spatial coordinate system O'X'Y'Z' are the same as in the spatial coordinate system OXYZ, i.e. the spatial coordinates of the center of the position sensor 130 are still (x pi , y pi , z pi ).

[0084] Therefore, when performing step S2, i.e. obtaining the spatial coordinates (x' i , y' i , z' i ) of the spot of the laser beam i on the position sensor 130 at time t, it is necessary to first obtain the coordinates (x' oi , y' oi ) of the spot of the laser beam on the position sensor 130 relative to the center of the position sensor 130 in the spatial coordinate system O'X'Y'Z', i.e. to perform step S201; and then to obtain (x' i , y' i , z' i ) according to (x pi , y pi , z pi ) and (x' oi , y' oi ), wherein

[0085] (x' i , y' i , z' i ) = (x pi +x' oi , y pi +y' oi , z pi ) (4).

[0086] Still taking the three collimators 140 and the three position sensors 130 as an example, the obtaining of the spatial coordinates (x' i , y' i , z' i ) of the spot of the laser beam i on the position sensor 130 at time t is described.

[0087] Referring to Figure 2 and Figure 3 , at time t, the first spot of the laser beam emitted by the first collimator 141 on the first position sensor 131 changes from position A to position A', at this time, the coordinates of the first spot relative to the center of the first position sensor 131 change to (x' o1 , y' o1). Correspondingly, the second light spot formed by the laser beam emitted by the second collimator 142 on the second position sensor 132 changes in coordinate position relative to the center of the second position sensor 132 as (x' o2 , y' o2 ), and the third light spot formed by the laser beam emitted by the third collimator 143 on the third position sensor 133 changes in coordinate position relative to the center of the third position sensor 133 as (x' o3 , y' o3 ). According to formula (4), the spatial coordinates (x'1, y'1, z'1), (x'2, y'2, z'2) and (x'3, y'3, z'3) of the first light spot, the second light spot and the third light spot in the spatial coordinate system O'X'Y'Z' can be obtained, which are as follows:

[0088] (x'1, y'1, z'1) = (x p1 +x' o1 , y p1 +y' o1 , z p1 ) (12)

[0089] (x'2, y'2, z'2) = (x p2 +x' o2 , y p2 +y' o2 , z p2 ) (13)

[0090] (x'3, y'3, z'3) = (x p3 +x' o3 , y p3 +y' o3 , z p3 ) (14)

[0091] After L i = (x Li , y Li , z Li ), (x i , y i , z i ) and (x' i , y' i , z' i ) are obtained, a nonlinear equation set can be established, that is, step S3 is performed. The linear equation set is as follows:

[0092]

[0093] wherein, R x (α) = [1 0 0; 0 cosα -sinα; 0 sinα cosα], R y(β) = [cosβ 0 sinβ; 0 10; -sinβ 0 cosβ], R z (γ) = [cosγ -sinγ 0; sinγ cosγ 0; 0 0 1], α, β and γ are the rotation angles of the measured device 122 relative to the reference device along the spatial coordinate axes X, Y and Z respectively, δx, δy and δz are the displacements of the measured device 122 along the spatial coordinate axes X, Y and Z respectively, const i is an unknown constant.

[0094] Still taking the three collimators 140 and the three position sensors 130 as an example, the establishment of the nonlinear equation set is described.

[0095] First, taking the first collimator 141 as an example, the direction vector of the laser beam emitted by the first collimator 141 at the initial moment in the spatial coordinate system OXYZ is L1= (x L1 , y L1 , z L1 ), and the spatial coordinates of the first light spot formed by the laser beam on the first position sensor 131 are (x1, y1, z1). At time t, the measured device 122 is translated by δx, δy and δz along the spatial coordinate axes X, Y and Z respectively, and the measured device 122 is rotated by α, β and γ along the spatial coordinate axes X, Y and Z respectively, and the spatial coordinate system OXYZ reaches O'X'Y'Z' with the measured device 122. For details, refer to Figure 3 . The spatial coordinates of the first light spot in the spatial coordinate system O'X'Y'Z' are (x'1, y'1, z'1). The above parameters have the following relationship:

[0096]

[0097] Where [x', y', z'] T , [x' L1 , y' L1 , z' L1 ] T and [x', y', z'] T are intermediate variables.

[0098] After rearranging formula (15), we get:

[0099]

[0100] Similarly, through the second collimator 142 and the second position sensor 132, and the third collimator 143 and the third position sensor 133, the following formulas can be obtained:

[0101]

[0102]

[0103] The number of equations in the above nonlinear equation set is consistent with the number of unknown variables, and thus, solving the above nonlinear equation set can obtain the displacement δx, δy and δz of the measured device 122 relative to the reference device along the spatial coordinate axes X, Y and Z, and the rotation angles α, β and γ, i.e., step S4 is performed.

[0104] Solving the above nonlinear equation set is a process of linearizing the nonlinear equation set successively based on Newton algorithm and performing iterative solution. In the process, the iterative coefficient matrix J f is as follows:

[0105]

[0106] where F i is a simplified form of the nonlinear equation set.

[0107] Still taking the three collimators 140 and the three position sensors 130 as an example, the iterative coefficient matrix J f is described.

[0108] The formulas (16)-(18) can be simplified as:

[0109]

[0110] The iterative coefficient matrix J f is as follows:

[0111]

[0112] In the process of iterative solution, the convergence criterion of the nonlinear equation set is that the root mean square residuals of δx, δy, δz, α, β and γ of two iterations are all less than 10 -4 .

[0113] After solving the displacement δx, δy and δz of the measured device 122 relative to the reference device along the spatial coordinate axes X, Y and Z, and the rotation angles α, β and γ, the velocity (v x , v y , v z ) of the measured device 122 moving along the spatial coordinate axes X, Y and Z and the angular velocity (ω x , ω y , ω z ) of the measured device 122 rotating around the spatial coordinate axes X, Y and Z can be obtained by solving the derivatives of (δx, δy, δz) and (α, β, γ) with respect to time, so as to know the motion state of the measured device 122.

[0114] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this application belongs. The terminology used herein is for the purpose of describing particular embodiments only and is not intended to be limiting of this application. As used herein, the singular forms "a", "an" and "the" are intended to include the plural forms as well, unless the context clearly indicates otherwise. It will be further understood that the terms "comprises" and / or "comprising," when used in this specification, specify the presence of stated features, integers, steps, operations, elements, and / or components, but do not preclude the presence or addition of one or more other features, integers, steps, operations, elements, components, and / or groups thereof. As used herein, the term "and / or" includes any and all combinations of one or more of the associated listed items. As used herein, the term "if' can be construed to mean "when" or "upon" or "in response to the occurrence of" in addition to "in response to the fulfillment or lapse of" unless otherwise indicated. As used herein, the term "plurality" means two or more.

[0115] While the application has been described in connection with what is presently considered to be the most practical and preferred embodiments, it is to be understood that the application is not to be limited to the disclosed embodiments, but on the contrary, is intended to cover various modifications and equivalent arrangements. Accordingly, the application is not to be limited by the embodiments described above but is only limited as required by the claims and the equivalents thereof.

Claims

1. A method for measuring relative displacement and rotation angle of a rigid body, characterized in that: include: S1: Based on the spatial coordinates of the exit port of the laser beam emitted by the measuring device at the initial moment and the center of the position sensor on the device under test, as well as the coordinates of the light spot generated by the laser beam on the position sensor relative to the center of the position sensor, obtain the direction vector L of the laser beam emitted by the measuring device at the initial moment i =(x Li ,y Li , z Li ) and the spatial coordinates (x i ,y i , z i ), where i represents the i-th laser beam, i=1, 2, 3, ...; S2: According to the spatial coordinates of the center of the position sensor at time t and the coordinates of the light spot generated by the laser beam on the position sensor relative to the center of the position sensor, obtain the spatial coordinates (x') of the light spot generated by the laser beam on the position sensor at time t i , y' i , z' i ); S3: According to the L i =(x Li ,y Li , z Li ), the (x i ,y i , z i ) and (x' i , y' i , z' i ) to establish a nonlinear system of equations: Among them, R x (α)=[1 0 0; 0cosα-sinα; 0sinαcosα], R y (β)=[cosβ0sinβ; 0 10; -sinβ0cosβ], R z (γ) = [cosγ-sinγ0; sinγcosγ0; 0 0 1], α, β, and γ are the rotation angles of the device under test relative to the reference device along the spatial coordinate axes X, Y, and Z, respectively; δx, δy, and δz are the displacements of the device under test along the spatial coordinate axes X, Y, and Z, respectively; const i is an unknown constant; S4: Solve the nonlinear equation group to obtain the displacements δx, δy and δz of the device under test relative to the reference device along the spatial coordinate axes X, Y and Z, as well as the rotation angles α, β and γ.

2. The method for measuring relative displacement and rotation angle of a rigid body according to claim 1, wherein: Said S1 comprises: S101: Establishing a spatial coordinate system OXYZ, where the origin O of the coordinate system is the intersection of the photosensitive surface of the position sensor and the center line of the device under test, and the spatial coordinate system OXYZ has the center line of the device under test as the Z axis, the horizontal direction as the X axis, and the vertical direction as the Y axis; S102: In the spatial coordinate system OXYZ, the spatial coordinates (x si ,y si , z si ) and (x pi ,y pi , z pi ), obtain the coordinates (x oi ,y oi ); S103: According to the (x si ,y si , z si )、(x pi ,y pi , z pi ) and (x oi ,y oi ) Get the L i =(x Li ,y Li , z Li ) and (x i ,y i , z i ),in, L i =(x Li ,y Li ,z Li )=(x pi +x oi -x si ,y pi +y oi -y si ,z pi -z si ) (2) (x i ,y i ,z i )=(x pi +x oi ,y pi +y oi ,z pi ) (3)。 3. The method for measuring relative displacement and rotation angle of a rigid body according to claim 2, wherein: At time t, the spatial coordinate system OXYZ arrives at the spatial coordinate system O'X'Y'Z' along with the device under test, and S2 includes: S201: Obtain the spatial coordinates (x, y, and z) of the center of the position sensor in the spatial coordinate system O'X'Y'Z'. pi ,y pi , z pi ), obtain the coordinates (x') of the light spot generated by the laser beam on the position sensor relative to the center of the position sensor oi , y' oi ); S202: According to the (x pi ,y pi , z pi ) and (x' oi , y' oi )Get the (x' i , y' i , z' i ),in, (x' i ,y' i ,z' i )=(x pi +x' oi ,y pi +y' oi ,z pi ) (4)。 4. A method for measuring relative displacement and rotation angle of a rigid body according to any one of claims 1 to 3, characterized in that: In S4, the nonlinear equations are linearized one by one using Newton's algorithm and solved iteratively, wherein the iteration coefficient matrix J f for: Among them, F i is a simplified form of the nonlinear equations.

5. The method for measuring relative displacement and rotation angle of a rigid body according to claim 4, wherein: In S4, the convergence criterion for iteratively solving the nonlinear equations is that the root mean square residuals of δx, δy, δz, α, β and γ of two iterations are all less than 10 -4 .

6. A method for measuring relative displacement and rotation angle of a rigid body according to any one of claims 1 to 3, characterized in that: The method also includes solving the time derivatives of (δx, δy, δz) and (α, β, γ) to obtain the speed (v x , v y , v z ) and the angular velocity (ω) around the spatial coordinate axes X, Y, and Z x ,ω y ,ω z ).

7. A method for measuring relative displacement and rotation angle of a rigid body according to any one of claims 1 to 3, characterized in that: Before performing S1 , the collimator of the measuring device is mounted on the reference device, and a center line of the collimator forms a certain angle with a center line of the device under test.

8. The method for measuring relative displacement and rotation angle of a rigid body according to claim 7, wherein: The collimator or the position sensor is adjusted so that the center line of the collimator passes through the center point of the photosensitive surface of the position sensor.

Images