A kinematic modeling method for a non-orthogonal axis coordinate measuring instrument

Abstract

This invention provides a non-orthogonal axis coordinate measuring instrument and proposes a kinematic modeling method for this instrument, including the following steps: (1) Parameter characterization step: The vertical rotation axis, horizontal rotation axis and measurement line of sight inside the coordinate measuring instrument are abstracted as spatial skew lines, and the spatial pose is defined by the unit direction vector of each axis and the spatial coordinates of a fixed point on the axis; (2) Screw transformation step: Based on Lie algebra theory, the geometric parameters of each axis defined in step (1) are transformed into corresponding kinematic screws; (3) Model construction step: Based on the exponential product formula, the kinematic equation of the coordinate measuring instrument as a series mechanism is constructed using the kinematic screws obtained in step (2); (4) Pose calculation step: According to the kinematic equation constructed in step (3), the real-time pose of the measurement line of sight in space is calculated, and the spatial three-dimensional coordinates of the target point are obtained by combining the distance value output by the ranging module.

Description

Technical Field

[0001] This invention relates to a method for accurate kinematic modeling of a non-orthogonal axis coordinate measuring instrument. Background Technology

[0002] In fields such as industrial manufacturing, aerospace, and precision engineering, high-precision three-dimensional coordinate measurement of large-scale objects is a crucial support for the manufacturing and research of their technical equipment. This type of measurement requires high-precision positioning and tracking of targets over a large area to meet the requirements of complex structure assembly, dynamic adjustment, and high-precision calibration. For example, during aircraft fuselage assembly, shipbuilding, or the installation of large equipment, it is necessary to measure the spatial position of target points in real time, ensuring extremely high accuracy and stability. Traditional large-scale measurements often employ laser trackers, theodolites, etc., all following the basic mode of two rotation axes driving one measurement axis to aim at the target. Such measuring equipment needs to obtain the precise spatial pose of the measurement axis after spatial rotation to achieve coordinate calculation. Therefore, the orthogonality requirement of its own structure is extremely strict; that is, the two rotation axes must be perpendicularly distributed at 90°, the line-of-sight axis and the horizontal rotation axis must be perpendicularly distributed at 90°, and all three axes must intersect at a single point. Otherwise, the final spatial pose representation of the measurement axes will be incorrect, introducing serious errors. However, current industrial processing technology struggles to meet the stringent 90° requirement between two axes, inevitably introducing errors into measuring instruments. To maintain measurement accuracy, repeated testing and calibration are necessary. In contrast, non-orthogonal axis high-precision two-axis platforms differ from traditional instruments with orthogonal physical structures. Their internal orthogonality is not required, and tilting or misalignment errors caused by installation, vibration, etc., do not affect the target point measurement. Furthermore, traditional kinematic modeling methods for orthogonal structures are completely ineffective for non-orthogonal axis systems, failing to accurately acquire the final spatial pose of the end-effector's line of sight, resulting in the loss of crucial coordinate measurement information. Therefore, it is necessary to derive and establish a universal, accurate kinematic modeling method suitable for non-orthogonal axis systems. Summary of the Invention

[0003] The purpose of this invention is to propose a general kinematic modeling method applicable to non-orthogonal axis systems by re-deriving and modeling the non-orthogonal axis system, so as to meet the need for accurate real-time pose representation of the end-effector's line of sight under the non-orthogonal axis system.

[0004] A kinematic modeling method for a non-orthogonal axis coordinate measuring instrument, characterized by comprising the following steps: (1) Parameter characterization steps: The vertical rotation axis, horizontal rotation axis and measurement line of sight inside the coordinate measuring instrument are abstracted as spatial skew lines, and their spatial pose is defined by the unit direction vector of each axis and the spatial coordinates of a fixed point on the axis. (2) Screw conversion steps: Based on Lie algebra theory, the geometric parameters of each axis defined in step (1) are converted into the corresponding kinematic screws; (3) Model construction steps: Based on the exponential product formula, the kinematic equations of the coordinate measuring instrument as a series mechanism are constructed using the motion screw obtained in step (2); (4) Pose calculation steps: Based on the kinematic equations constructed in step (3), the real-time pose of the measurement line of sight in space is calculated, and the spatial three-dimensional coordinates of the target point are calculated by combining the distance value output by the ranging module.

[0005] In step (1), the pose of the vertical rotation axis is determined by the unit direction vector. and fixed point coordinates Definition: The pose of the horizontal rotation axis is defined by the unit direction vector. and fixed point coordinates Definition: The pose of the measured line of sight is defined by a unit direction vector. and fixed point coordinates definition.

[0006] In step (2), the horizontal rotation axis Rotational curvature relative to the vertical axis of rotation It is calculated using the following formula:

[0007] .

[0008] Before step (1), there is also a step of establishing the instrument base coordinate system, specifically: taking the intersection of the horizontal rotation axis and the common perpendicular of the two rotation axes as the origin of the coordinate system, and establishing the base coordinate system according to the right-hand rule.

[0009] In step (3), the kinematic equations are expressed as:

[0010] in, The matrix exponent representing the kinetic spinor This represents the initial pose transformation matrix of the laser line-of-sight coordinate system relative to the base coordinate system when the instrument is in the zero position. g is the pose transformation matrix of the measurement line-of-sight coordinate system relative to the base coordinate system. ξ1 and ξ2 are the motion screws of the horizontal and vertical rotation axes in the base coordinate system, respectively. q1 and q2 are the corresponding rotation angles. M0 is the initial pose transformation matrix of the measurement line-of-sight coordinate system relative to the base coordinate system when the instrument is in the mechanical zero position. exp() is the matrix exponentiation operator.

[0011] Furthermore, the rotation angles q1 and q2 are provided by the rotation drive mechanism of the coordinate measuring instrument.

[0012] In step (4), the real-time pose of the measurement line of sight is obtained by applying the pose transformation matrix g to its initial pose, and is obtained through the following rigid body transformation: .

[0013] Its final pose direction vector V_LF = R V_L, whose final fixed point coordinates are P_LF = R P_L + b, where R and b are the rotation matrix component and translation vector component in the pose transformation matrix g, respectively.

[0014] In step (4), the spatial three-dimensional coordinates P of the target point are determined by the formula... The calculation yields k, where k is the distance from the fixed point on the measurement line of sight to the target point, as measured by the ranging module.

[0015] The non-orthogonal axis coordinate measuring instrument provided by the present invention includes: Fixed base; A two-axis rotary drive mechanism is mounted on the base; A non-orthogonal shaft connection mechanism connected to the rotary drive mechanism; A ranging module is installed at the end of the connecting mechanism; And a control unit configured to perform the kinematic modeling method according to any one of claims 1 to 8 to achieve accurate measurement of the coordinates of the target point.

[0016] Furthermore, the vertical rotation axis, the horizontal rotation axis, and the measurement line of sight emitted by the ranging module in the non-orthogonal axis connection mechanism do not satisfy the orthogonal geometric relationship of being perpendicular to each other and intersecting at a point; the ranging module is a laser rangefinder, and the laser beam emitted by it constitutes the measurement line of sight. Attached Figure Description

[0017] Figure 1This is a model diagram of a non-orthogonal axis coordinate measuring instrument. 1 is a rotary drive mechanism, which can be used to rotate and output precise angles; 2 is a connecting mechanism, which is a non-orthogonal axis structure; 3 is a handle, used for moving the instrument; 4 is a distance measuring module, which provides distance measuring information to the system; and 5 is a fixed base, which provides a stable fixed connection for the entire instrument.

[0018] Figure 2 A diagram is created for the coordinate system of a non-orthogonal axis coordinate measuring instrument, where o-xyz is the instrument's own base coordinate system. , , These are the coordinates of fixed points on the vertical rotation axis, horizontal rotation axis, and measurement line of sight, respectively. , and These are the unit direction vectors of the vertical rotation axis, the horizontal rotation axis, and the measurement line of sight, respectively. Detailed Implementation

[0019] The basic scheme of the general kinematic modeling method for non-orthogonal axis systems proposed in this invention will be described below. This invention mainly includes the following steps: 1. Characterization of the internal three-axis parameters of a non-orthogonal axis coordinate measuring instrument.

[0020] 2. Based on Lie algebra theory, the geometric parameters of the axis system are transformed into kinematic spinors.

[0021] 3. General kinematic expression of serial mechanisms based on exponential product form.

[0022] 4. Construction of a general kinematic model for non-orthogonal axis systems and calculation of the pose of the line of sight axis.

[0023] The following provides a detailed explanation of each part. Part 1: Characterization of the internal three-axis parameters of a non-orthogonal coordinate measuring machine: (1) The basic architecture of the non-orthogonal axis coordinate measuring instrument consists of a high-precision two-axis turntable and a high-precision ranging module. The collimated laser beam emitted by the ranging module serves as the "visual" measurement line of sight of the non-orthogonal axis coordinate measuring instrument. The two-axis turntable drives the ranging module, so that the collimated laser beam aims at the target point in space and provides the spatial azimuth angle of the current laser beam.

[0024] (2) Since the rotation center axis of the two-axis turntable of the instrument and the "visualization" measurement line of sight form a non-orthogonal axis system, when constructing its measurement model, the spatial pose parameters of the rotation center axis and the measurement line of sight are the internal structural parameters of the non-orthogonal laser theodolite, which need to be accurately characterized and precisely calibrated.

[0025] (3) Since there are no orthogonal constraints between the axes of a non-orthogonal laser theodolite, it can be abstracted into three skew lines in space, and the unit direction vector of the lines can be used. and the coordinates of a fixed point on a straight line To define the spatial pose of the three straight lines.

[0026] (4) Following the traditional naming convention for orthogonal axis systems, the rotation center axes of the two-axis turntable are still referred to as the vertical rotation axis and the horizontal rotation axis, respectively. The laser axis used for measurement at the end is called the measurement line of sight, and its representation method is shown in Table 1: Table 1

[0027] Part Two: Based on Lie algebra theory, the geometric parameters of the axis system are transformed into kinematic spinors.

[0028] (1) In mathematics, a group represents an algebraic structure that satisfies the closure condition and the associativity law, where the element g can be subjected to group operations. Group operations usually refer to the binary operations corresponding to addition and multiplication, using the operator " "Representation. A certain set" G The element g in the equation satisfies the following property: •For any g 1, g 2∈ G ,have .

[0029] •For any g 1, g 2, g 3∈ G All have .

[0030] •For any g ∈ G There exists a unit element , making Established.

[0031] •For any g ∈ G There exists an inverse. , making Established This set G can be called a group. A Lie group is a smooth manifold, and the tangent space at the identity element e is defined as the Lie algebra. Every Lie group has a corresponding Lie algebra, which describes the local properties of the Lie group.

[0032] (2) During the motion of a rigid body in space, the tool coordinate system at the end of the rigid body changes relative to the reference coordinate system. The corresponding transformation matrix can be used to describe the motion of any rigid body in space. The set of transformation matrices corresponding to all rigid body motions is as follows:

[0033] in, R This represents the rotation transformation matrix of the rigid body relative to the reference coordinate system. b This represents the translation matrix of the rigid body relative to the reference coordinate system.

[0034] (3) Establish R Mapping relationship with equivalent rotation axis and corresponding rotation angle:

[0035] in, Let be a unit vector, representing the equivalent rotation axis vector. This corresponds to the rotation angle value. Defined as The antisymmetric matrix, that is, for any Its antisymmetric matrix is:

[0036] (4) Define the equivalent kinetic spinor of the rotation axis as follows:

[0037] Will This is called the kinetic spinor. Let the motion spinor be in six-dimensional coordinate form, and define the operator. Will Convert to :

[0038] in, p It is the vector from the origin of the rotation axis to the origin of the reference coordinate system.

[0039] (5) Establish a coordinate system. The coordinate system of the non-orthogonal axis coordinate measuring instrument is a right-handed coordinate system. The intersection of the common perpendicular of the two rotation axes and the horizontal rotation axis is taken as the origin. The direction vectors of the coordinate axes are expressed as follows:

[0040]

[0041]

[0042] (6) According to (4), the rotation of the horizontal and vertical rotation axes of the non-orthogonal axis coordinate measuring instrument are respectively , It is obtained from the direction vector of the rotation axis and the coordinates of the fixed point:

[0043]

[0044] Part 3: General kinematic expression of serial mechanisms based on exponential product form.

[0045] (1) The kinematic model of a series mechanism is a kinematic model based on global coordinates. Its main advantage is that only two coordinate systems need to be established: the base coordinate system and the end-effector coordinate system. The base coordinate system is a Cartesian coordinate system fixed relative to the base of the series mechanism, and the end-effector coordinate system only needs to remain relatively stationary with respect to the end of the mechanism. In the base coordinate system, let the joint rotation of the moving joint be... As we know from the previous section, it is determined by the vector along the axis of the moving joint. ω With any point on the axis p Composition. Joint rotation. The form of unity spin needs to be satisfied:

[0046] (2) When the drive angle command is q When, the corresponding rigid body motion transformation matrix can be expressed as:

[0047] in,

[0048]

[0049] (3) When the series mechanism moves, the moving joint i With joint movement i The relative pose between -1 and 1 can be represented as:

[0050] in, g i-1,i Indicates joint i Relative to joint i -1 relative pose relationship q i Indicates the amount of motion driving force. M i Indicates joint i The zero-position initial pose transformation matrix. Indicates joint i Motion spinor in its own local coordinate system.

[0051] (4) When the degrees of freedom of the series mechanism are n At that time, its kinematic model based on the exponential product can be expressed as:

[0052] The following relationship can be obtained:

[0053] in, Represents the motion joint in the base coordinate system i The corresponding spinor, M 0 represents the pose transformation matrix of the initial zero-position end-effector coordinate system relative to the base coordinate system.

[0054] (5) Let The corresponding initial pose transformation spinor of the end-effector coordinate system relative to the base coordinate system is expressed as:

[0055] Then the final n The kinematic model of a series-connected degree-of-freedom mechanism can be rewritten as:

[0056] Part 4: Construction of a general kinematic model for non-orthogonal axis systems and calculation of the pose of the line of sight axis.

[0057] (1) When a non-orthogonal axis coordinate measuring instrument measures a target point in space, the corresponding rotation angles of the two axes are respectively , Assuming the turntable is in the zero position, the pose transformation spinor of the end-effector coordinate system relative to the base coordinate system is: During the movement of a non-orthogonal axis laser theodolite, the spatial pose transformation matrix of the laser line of sight... g The exponential product mapping can be expressed as:

[0058] (2) Set the end-effector coordinate system to coincide with the base coordinate system, that is:

[0059] Then the pose transformation matrix g The exponential product mapping form can be rewritten as:

[0060] (3) From the first part, we can obtain the direction vector. V L coordinates of a fixed point P L This represents the spatial pose of the laser line of sight at its initial position. After aiming at the target point, the direction vectors are used respectively. V LF coordinates of a fixed point P LFThis represents the final spatial pose of the laser line of sight. The above process can be represented as:

[0061] (4) After aiming at the target point in space, the ranging module outputs the distance between the fixed point on the line of sight and the target point in space. k The final measured coordinates of the target point P It can be represented as:

[0062] The present invention will now be described in further detail with reference to the accompanying drawings and specific embodiments.

[0063] The kinematic modeling method for non-orthogonal axis coordinate measuring instruments proposed in this embodiment aims to solve the problem of failure of traditional orthogonal structure kinematic models under non-orthogonal axis systems, and to achieve accurate representation of the pose of the end-effector's line of sight.

[0064] like Figure 1 As shown, the non-orthogonal axis coordinate measuring instrument in this embodiment mainly consists of a base 5, a handle 3, a drive mechanism 1, a connection mechanism 2 for the non-orthogonal axis structure, and a distance measuring module 4. The instrument drives the distance measuring module 4 through a two-axis turntable, so that the collimated laser beam (measurement line of sight) is aimed at the target point in space.

[0065] The modeling method described in this embodiment, such as Figure 2 The coordinate system establishment diagram shown is mainly implemented in the following four steps: Step 1: Characterize the internal three-axis parameters of the non-orthogonal axis coordinate measuring machine (1) In this embodiment, the instrument’s own base coordinate system is first established. .like Figure 2 As shown, the intersection of the common perpendicular of the two rotation axes and the horizontal rotation axis is taken as the origin of the coordinate system. The coordinate axis direction vectors follow the right-hand rule.

[0066] (2) Since the rotation center axis of the two-axis turntable of the instrument and the measurement line of sight form a non-orthogonal axis system, there is no strict 90° orthogonal constraint between the axes. Therefore, in this embodiment, it is abstracted into three spatial skew lines, and the spatial pose of the three lines is uniquely defined by the "unit direction vector" and the "coordinates of a fixed point on the line".

[0067] (3) Specific parameter definitions are shown in Table 1 and Figure 2 As shown: Vertical axis of rotation: defined as the unit direction vector and fixed point coordinates .

[0068] Horizontal rotation axis: defined as the unit direction vector and fixed point coordinates .

[0069] Measurement line of sight: defined as the unit direction vector and fixed point coordinates .

[0070] These parameters constitute the internal structural parameters of the non-orthogonal laser theodolite, which are obtained through precise calibration.

[0071] Step 2: Based on Lie algebra theory, convert the geometric parameters of the axis system into kinematic spinors. (1) To describe rigid body motion, this embodiment introduces the theory of Lie groups and Lie algebras. Rigid body pose transformation group SE (3) The Lie algebra is defined as se (3), the corresponding rotation matrix group SO (3) Lie algebra is called so (3). The kinetic spinor is expressed as:

[0072] Let the motion spinor be in six-dimensional coordinate form, and define the operator. Will Convert to :

[0073] (2) In this embodiment, the rotation axis is converted into a kinematic screw using the geometric parameters determined in step one. According to screw theory, the horizontal rotation axis... Rotational curvature relative to the vertical axis of rotation It can be calculated using the following formula:

[0074]

[0075] Step 3: General kinematic expression of serial mechanisms based on exponential product form (1) In this embodiment, the kinematic model is established using the product of exponents (POE) formula based on global coordinates. The advantage of this method is that only the base coordinate system and the end tool coordinate system need to be established, without the need to establish a complex intermediate link coordinate system.

[0076] (2) For the two-axis series mechanism in this embodiment, when the driving angle command is q When (corresponding to the horizontal axis angle and the vertical axis angle), the pose transformation matrix g of the end-effector coordinate system relative to the base coordinate system is represented in the form of an exponential product mapping.

[0077] (3) According to the kinematic principle of series mechanism, the moving joint i The spinor of motion in the base coordinate system is Then the kinematic equations of the mechanism are:

[0078] in, The matrix exponent representing the kinetic spinor It represents the initial pose transformation matrix of the end-effector coordinate system (i.e., the laser line-of-sight coordinate system) relative to the base coordinate system when the instrument is in the zero position.

[0079] Step 4: Construction of a general kinematic model for non-orthogonal axis systems and calculation of the pose of the line of sight axis. (1) Combining the general expression in step three, construct a specific model for this non-orthogonal axis instrument. Assume that when the turntable is at zero position, the initial pose of the laser line of sight is given by the direction vector in step one. coordinates of a fixed point Confirmed. When the instrument moves, the rotation angles of the two axes are respectively... , At that time, the final pose (direction vector) of the laser line of sight after spatial transformation and fixed point coordinates It can be obtained through the following rigid body transformation:

[0080] (2) Finally, the ranging module obtains the distance from the fixed point on the laser line of sight to the target point as: k Based on spatial geometric relationships, the final measured three-dimensional coordinates of the target point are obtained. It can be represented as:

[0081] Through the above four steps, the present invention addresses the issue of non-orthogonal instrument axis systems (i.e.,...). , , Even without satisfying the perpendicular intersection relationship, the direction of the line of sight can still be accurately calculated using a general kinematic model, thereby achieving high-precision coordinate measurement and avoiding the stringent requirements for hardware orthogonality.

Claims

1. A kinematic modeling method for a non-orthogonal axis coordinate measuring instrument, characterized in that, Includes the following steps: (1) Parameter characterization steps: The vertical rotation axis, horizontal rotation axis and measurement line of sight inside the coordinate measuring instrument are abstracted as spatial skew lines, and their spatial pose is defined by the unit direction vector of each axis and the spatial coordinates of a fixed point on the axis. (2) Screw conversion steps: Based on Lie algebra theory, the geometric parameters of each axis defined in step (1) are converted into the corresponding kinematic screws; (3) Model construction steps: Based on the exponential product formula, the kinematic equations of the coordinate measuring instrument as a series mechanism are constructed using the motion screw obtained in step (2); (4) Pose calculation steps: Based on the kinematic equations constructed in step (3), the real-time pose of the measurement line of sight in space is calculated, and the spatial three-dimensional coordinates of the target point are calculated by combining the distance value output by the ranging module.

2. The method according to claim 1, characterized in that, In step (1), the pose of the vertical rotation axis is determined by the unit direction vector. and fixed point coordinates Definition: The pose of the horizontal rotation axis is defined by the unit direction vector. and fixed point coordinates Definition: The pose of the measured line of sight is defined by a unit direction vector. and fixed point coordinates definition.

3. The method according to claim 2, characterized in that, In step (2), the horizontal rotation axis Rotational curvature relative to the vertical axis of rotation It is calculated using the following formula: 。 4. The method according to claim 1, characterized in that, Before step (1), there is also a step of establishing the instrument base coordinate system, specifically: taking the intersection of the horizontal rotation axis and the common perpendicular of the two rotation axes as the origin of the coordinate system, and establishing the base coordinate system according to the right-hand rule.

5. The method according to claim 3, characterized in that, In step (3), the kinematic equations are expressed as: in, The matrix exponent representing the kinetic spinor This represents the initial pose transformation matrix of the laser line-of-sight coordinate system relative to the base coordinate system when the instrument is in the zero position. g is the pose transformation matrix of the measurement line-of-sight coordinate system relative to the base coordinate system. ξ1 and ξ2 are the motion screws of the horizontal and vertical rotation axes in the base coordinate system, respectively. q1 and q2 are the corresponding rotation angles. M0 is the initial pose transformation matrix of the measurement line-of-sight coordinate system relative to the base coordinate system when the instrument is in the mechanical zero position. exp() is the matrix exponentiation operator.

6. The method according to claim 5, characterized in that, The rotation angles q1 and q2 are provided by the rotation drive mechanism of the coordinate measuring instrument.

7. The method according to claim 5, characterized in that, In step (4), the real-time pose of the measurement line of sight is obtained by applying the pose transformation matrix g to its initial pose, and is obtained through the following rigid body transformation: 。 Its final pose direction vector V_LF = R V_L, whose final fixed point coordinates are P_LF = R P_L + b, where R and b are the rotation matrix component and translation vector component in the pose transformation matrix g, respectively.

8. The method according to claim 7, characterized in that, In step (4), the spatial three-dimensional coordinates P of the target point are determined by the formula... The calculation yields k, where k is the distance from the fixed point on the measurement line of sight to the target point, as measured by the ranging module.

9. A non-orthogonal axis coordinate measuring instrument, characterized in that, include: Fixed base; A two-axis rotary drive mechanism is mounted on the base; A non-orthogonal shaft connection mechanism connected to the rotary drive mechanism; A ranging module is installed at the end of the connecting mechanism; And a control unit configured to perform the kinematic modeling method according to any one of claims 1 to 8 to achieve accurate measurement of the coordinates of the target point.

10. The coordinate measuring instrument according to claim 9, characterized in that, The vertical rotation axis, horizontal rotation axis, and measurement line of sight emitted by the ranging module in the non-orthogonal axis connection mechanism do not satisfy the orthogonal geometric relationship of being perpendicular to each other and intersecting at a point; the ranging module is a laser rangefinder, and the laser beam emitted by it constitutes the measurement line of sight.

Images