A point cloud data acquisition and accurate docking method and device

Abstract

The application discloses a kind of point cloud data acquisition, accurate docking method and device, the point cloud data acquisition method includes the following steps;First compression amount of probe at first time, coordinates of mechanical arm in base coordinate system at second time and second compression amount of probe at third time and coordinates of mechanical arm in base coordinate system are respectively acquired;Probe compression variation is calculated according to first compression amount and second compression amount;The direction vector of mechanical arm movement is calculated according to the coordinates of mechanical arm at second time and the coordinates of mechanical arm at third time;Probe compression variation is calculated in each coordinate axis corresponding displacement component according to the direction vector of mechanical arm movement and the compression variation of probe;The coordinates of corresponding point cloud of probe needle tip are calculated when probe compression amount is first compression amount and probe is just contacted with the to-be-docked piece at third time mechanical arm coordinates and each coordinate axis corresponding displacement component are calculated.Probe compression variation is calculated in each coordinate axis corresponding displacement component according to the direction vector of mechanical arm movement and the compression variation of probe;The coordinates of corresponding point cloud of probe needle tip are calculated when probe compression amount is first compression amount and probe is just contacted with the to-be-docked piece at third time mechanical arm coordinates and each coordinate axis corresponding displacement component are calculated.Using the above method, the accuracy of point cloud data acquisition can be greatly improved.

Description

Technical Field

[0001] This invention belongs to the field of point cloud data acquisition technology, specifically relating to a point cloud data acquisition and precise docking method and device. Background Technology

[0002] The docking quality of large docking components has a direct impact on their overall performance during the docking process, especially for large compartments.

[0003] During the docking process of large docking components, point cloud data is typically collected first to determine features such as the component's axis, as illustrated in application number CN202511673641.8, "A Virtual Docking Method for Sub-sections Based on Measured Point Cloud Data." The accuracy of the point cloud data collection on the docking components directly affects the accuracy of subsequent determination of features such as the component's axis, and consequently, the overall docking accuracy.

[0004] Currently, point cloud data acquisition on docking components is achieved using 3D cameras or probes. For example, the "Hand-eye Calibration Method for Robotic Arm and Binocular Camera Based on 3D Point Cloud" (application number CN202211721133.9) uses a 3D camera to acquire point cloud data; and the "Calibration Device for Real-time Tracking of Free Bone Block Position" (application number CN201810381890.3) uses a probe to acquire point cloud data.

[0005] However, the accuracy of point cloud data acquisition using existing 3D cameras is affected by the precision of the 3D camera and the point cloud generation method, resulting in low accuracy.

[0006] Using probes to acquire point cloud data has the following problems:

[0007] The coordinates of the measurement point are determined by the contact between the probe and the measurement point on the workpiece surface. To improve the acquisition accuracy, the probe needs to be precisely controlled, that is, the probe needs to be in contact with the workpiece surface. This not only requires high control accuracy of the probe, but also results in low repeatability. Summary of the Invention

[0008] To address the problem of low accuracy in existing point cloud data acquisition, this invention proposes a point cloud data acquisition and precise docking method and device, which effectively improves the accuracy of point cloud acquisition.

[0009] The objective of this invention is achieved through the following technical solution:

[0010] The first aspect of this invention discloses a point cloud data acquisition method, comprising the following steps:

[0011] The first compression amount of the probe at the first moment, the coordinates of the robot arm in the base coordinate system at the second moment, and the second compression amount of the probe and the coordinates of the robot arm in the base coordinate system at the third moment are obtained respectively. The probe is fixed on the actuator at the end of the robot arm and can move relative to the actuator along its own axis. The axis of the probe is parallel to a coordinate axis of the tool coordinate system, which is the coordinate system corresponding to the actuator. The first moment is when the probe is not in contact with the workpiece to be docked. At the second and third moments, the probe is in contact with the workpiece to be docked. The contact position of the probe with the workpiece to be docked is the same at both moments and the probe is compressed. Between the second and third moments, the movement direction of the actuator is along the probe axis. The compression amount of the probe at the second moment is not equal to the compression amount of the probe at the third moment.

[0012] The compression change of the probe is calculated based on the first compression amount and the second compression amount;

[0013] The direction vector of the robot arm's motion is calculated based on the robot arm's coordinates at the second and third moments.

[0014] The displacement components of the probe's compression change on each coordinate axis are calculated based on the probe's compression change and the direction vector of the robotic arm's movement.

[0015] Based on the coordinates of the robotic arm at the third moment and the displacement components corresponding to each coordinate axis, the coordinates of the point cloud corresponding to the probe tip in the base coordinate system are calculated when the probe just contacts the workpiece to be docked and the probe compression is the first compression.

[0016] The second aspect of this invention discloses a precise docking method, comprising the following steps:

[0017] A point cloud data acquisition method as described in the first aspect is used to acquire a point cloud set on a workpiece to be docked. The workpiece to be docked includes a first workpiece to be docked and a second workpiece to be docked. The point cloud set includes a first point cloud set and a second point cloud set. The first point cloud set includes point clouds on the docking surface and sidewall of the first workpiece to be docked, and the second point cloud set includes point clouds on the docking surface and sidewall of the second workpiece to be docked.

[0018] The normal vector and axis coordinates of the docking surfaces of the first and second workpieces to be docked are determined based on the point cloud.

[0019] The pose adjustment parameters for completing the docking of the first and second workpieces are calculated based on the normal vector and axis coordinate of the docking surface of the first and second workpieces to be docked.

[0020] The first workpiece to be docked is controlled according to the pose adjustment parameters to complete the docking of the first workpiece to be docked and the second workpiece to be docked.

[0021] A third aspect of the present invention discloses an apparatus comprising a memory and a controller connected in sequence, the memory storing a computer program, and the controller being configured to read the computer program and execute a point cloud data acquisition method as described in the first aspect, or execute a precise docking method as described in the second aspect.

[0022] The beneficial effects of this invention are:

[0023] 1. This invention uses probe compression to compress multi-point data and perform displacement compensation on the coordinate points where the probe just contacts the surface of the workpiece to be docked, which greatly improves the accuracy of point cloud data acquisition.

[0024] 2. This invention uses probe compression to compress multi-point data, eliminating the need to determine the timing of probe contact with the workpiece surface, resulting in high sampling accuracy. Attached Figure Description

[0025] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0026] Figure 1 This is a flowchart of the point cloud data acquisition method of the present invention. Detailed Implementation

[0027] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. The components of the embodiments of the present invention described and shown in the accompanying drawings can generally be arranged and designed in various different configurations.

[0028] Therefore, the following detailed description of the embodiments of the invention provided in the accompanying drawings is not intended to limit the scope of the claimed invention, but merely to illustrate selected embodiments of the invention. All other embodiments obtained by those skilled in the art based on the embodiments of the invention without inventive effort are within the scope of protection of the invention.

[0029] It should be noted that, unless otherwise specified, the embodiments and features described in this invention can be combined with each other.

[0030] It should be noted that similar labels and letters in the following figures indicate similar items. Therefore, once an item is defined in one figure, it does not need to be further defined and explained in subsequent figures.

[0031] In the description of this invention, it should be noted that the terms "center," "upper," "lower," "left," "right," "vertical," "horizontal," "inner," and "outer," etc., indicate the orientation or positional relationship based on the orientation or positional relationship shown in the accompanying drawings, or the orientation or positional relationship commonly used when the product of this invention is in use, or the orientation or positional relationship commonly understood by those skilled in the art. They are only used for the convenience of describing this invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation, and therefore should not be construed as a limitation of this invention. In addition, the terms "first," "second," etc., are only used to distinguish descriptions and should not be construed as indicating or implying relative importance.

[0032] In the description of this invention, it should also be noted that, unless otherwise explicitly specified and limited, the terms "set," "install," "connect," and "link" should be interpreted broadly. For example, they can refer to a fixed connection, a detachable connection, or an integral connection; they can refer to a mechanical connection or an electrical connection; they can refer to a direct connection or an indirect connection through an intermediate medium; and they can refer to the internal connection of two components. Those skilled in the art can understand the specific meaning of the above terms in this invention based on the specific circumstances.

[0033] The system involved in this invention includes a six-degree-of-freedom motion platform, a measuring platform, and a control unit for controlling the workpiece to be docked.

[0034] The workpieces to be docked include a first workpiece and a second workpiece, and the surface where the first and second workpieces are docked is the docking surface. The second workpiece serves as the reference docking component, and docking with the first workpiece is achieved by controlling the corresponding pose adjustment parameters of the first workpiece.

[0035] The measurement platform includes a robotic arm, a 3D camera, a probe, and a displacement sensor. The displacement sensor is fixed to the actuator at the end of the robotic arm. The coordinate system of the measurement platform is the base coordinate system, with its origin at a fixed point on the base of the robotic arm. The probe is mounted on the sensing element of the displacement sensor and can move relative to the actuator along its own axis. The probe's axis is parallel to one axis of the tool coordinate system. The coordinate system corresponding to the actuator is the tool coordinate system, with its origin at a fixed point on the actuator, typically at the tip of the probe when it is in an uncompressed state. The displacement of the probe relative to the actuator along its own axis is collected by the displacement sensor, i.e., the compression of the probe can be collected by the displacement sensor. The 3D camera, mounted on the robotic arm, is used to collect image data. The image data collected by the 3D camera can be used not only to guide the position and direction of the probe movement by the robotic arm but also to collect point cloud data at corresponding points.

[0036] The control unit uses at least one method in this embodiment to control the motion platform and the measurement platform to work together to achieve the docking of the first workpiece to be docked and the second workpiece to be docked.

[0037] Based on the above system, the first aspect of this invention discloses a point cloud data acquisition method, which is executed by a point cloud data acquisition device, namely the aforementioned control unit. Specifically, as shown... Figure 1 As shown, the method for the control unit to acquire point cloud data includes steps S211 to S215. It should be noted that the step labels in this scheme are only for the convenience of describing the method and do not constitute a limitation on the order of steps. The order of each step is based on its verbal description and the sequential connection of each signal.

[0038] During the point cloud data acquisition process, a 3D camera is used to guide the acquisition points and control the probe to move in different directions to acquire the coordinates of each measurement point.

[0039] For each measurement point, the control unit controls the robotic arm to move three times. The first movement corresponds to the first moment, where the robotic arm moves the probe, but the probe does not contact the measurement point on the workpiece to be docked, i.e., it is in a non-compressed state. The second and third movements control the robotic arm to bring the probe into contact with the measurement point and compress or retract it. The contact point between the probe tip and the surface of the workpiece to be docked is the same at the second and third moments, representing two different moments during the probe compression or rebound process. It is necessary to ensure that the compression amount of the probe is different at the second and third moments, and the direction of the actuator's movement between the second and third moments is along the probe axis.

[0040] For example, the second moment can be the moment when the probe is compressed to a length of 1mm, 2mm, 3mm, etc., and the third moment can be the moment when the probe is compressed to a length of 4mm, 5mm, 6mm, etc.

[0041] Step S211: The control unit acquires the first compression amount of the probe at the first moment, the coordinates of the robot arm in the base coordinate system at the second moment, and the second compression amount of the probe and the coordinates of the robot arm in the base coordinate system at the third moment.

[0042] The first and second compression values ​​can be directly obtained from the displacement sensor values.

[0043] Step S212: Calculate the compression change of the probe based on the first compression amount and the second compression amount.

[0044] Since the probe is not in contact with the workpiece at the first moment, it is not compressed at this time, and the first compression amount is the initial compression value of the probe.

[0045] The amount of compression change of the probe for:

[0046] ,

[0047] in, This is the first compression amount. This is the second compression level.

[0048] Step S213: Calculate the direction vector of the robot arm's motion based on the coordinates of the robot arm at the second and third moments.

[0049] Specifically, the second and third moments correspond to different times during the probe compression process, and the direction vector of the robotic arm's movement is... for:

[0050] ( , , );

[0051] The second and third moments correspond to different times during the probe's rebound process, and the direction vector of the robotic arm's movement is... for:

[0052] =( , , ),

[0053] in, The coordinates of the robotic arm at the third moment are... =( ), This represents the X-coordinate value of the robotic arm at the third moment. The Y-coordinate value of the robotic arm at the third moment. The Z-coordinate value of the robotic arm at the third moment. The coordinates of the robotic arm at the second moment are... The direction vector of the robotic arm's movement Components on the X-axis The direction vector of the robotic arm's movement Components on the Y-axis, The direction vector of the robotic arm's movement The component on the Z-axis.

[0054] Step S214: Calculate the displacement components of the probe's compression change on each coordinate axis based on the probe's compression change and the direction vector of the robotic arm's movement.

[0055] Specifically, calculate the displacement component of the probe's compression change on the X-axis of the base coordinate system. for:

[0056] = ;

[0057] The displacement component of the probe's compression change on the Y-axis of the base coordinate system for:

[0058] = ;

[0059] The displacement component of the probe's compression change on the Z-axis of the base coordinate system for:

[0060] = .

[0061] Step S215: Calculate the coordinates of the point cloud corresponding to the probe tip in the base coordinate system when the probe just contacts the workpiece to be docked and the probe compression is the first compression amount, based on the coordinates of the robotic arm at the third moment and the displacement components corresponding to each coordinate axis.

[0062] Specifically, when the probe just makes contact with the workpiece and the probe compression is at the first compression amount, the coordinates Pt of the point cloud corresponding to the probe tip in the base coordinate system are:

[0063] Pt=( ) ( , , ).

[0064] Using the above method, the point cloud coordinates of the measurement point can be accurately obtained.

[0065] Based on the aforementioned system and point cloud data acquisition method, a second aspect of this invention discloses a precise docking method. This method is executed by a precise docking device, which is the aforementioned control unit. Specifically, the precise docking method includes steps S1 to S4. It should be noted that the step identifiers in this solution are only for the convenience of describing the method and do not constitute a limitation on the order of steps. The order of each step is based on its verbal description and the sequential connection of each signal.

[0066] Step S1: System calibration.

[0067] This step requires defining the coordinate system of the motion platform, i.e., the first coordinate system. The coordinate system of the measurement platform, i.e., the second coordinate system. The calibration process includes steps S11 to S13.

[0068] Step S11: Set a target on the motion platform, control the six-degree-of-freedom motion platform to translate or rotate by a preset amount in the positive and negative directions of the X, Y, and Z axes respectively at the midpoint, and use a measuring robotic arm to measure the coordinates of the target after each translation or rotation by the preset amount.

[0069] Specifically, the motion platform is controlled to translate in the positive and negative directions of the three axes after the midpoint, and the coordinates of the target after each translation are obtained to obtain the first set of points.

[0070] The translation amount of the motion platform after the midpoint can be unequal in different directions, or it can be set to the same displacement amount.

[0071] For example, moving the motion platform to the midpoint. (0, 0, 0), move a distance d1 in the negative X-axis direction and collect the first coordinate point of the target at this time. Move the motion platform to the midpoint. (0, 0, 0), move a distance d2 in the positive X-axis direction and collect the second coordinate point of the target at this time. Move the motion platform to the midpoint. (0, 0, 0), move a distance d1 in the negative Y-axis direction and collect the third coordinate point of the target at this time. Move the motion platform to the midpoint. (0, 0, 0), move a distance d2 in the positive Y-axis direction, and collect the fourth coordinate point of the target at this time. Similarly, using the above method, the coordinates of the target after moving a certain distance in the negative Z-axis and positive Z-axis directions are collected: Fifth coordinate point. Sixth coordinate point .

[0072] After the control platform reaches the midpoint, it rotates in the positive and negative directions of the three axes respectively, and the coordinates of the target are obtained after each rotation to obtain the second set of points.

[0073] The angles of rotation of the motion platform in each direction after the midpoint can be unequal or set to the same angle. Preferably, for ease of calculation in the subsequent calibration process, the angles of rotation of the motion platform in the positive and negative X-axis directions after the midpoint are equal, the angles of rotation of the motion platform in the positive and negative Y-axis directions after the midpoint are equal, and the angles of rotation of the motion platform in the positive and negative Z-axis directions after the midpoint are equal. The positive and negative directions of each axis are determined by the right-hand rule.

[0074] Furthermore, to improve calibration accuracy, the motion platform is rotated by an angle greater than or equal to 5° in each direction after the midpoint.

[0075] Furthermore, in order to improve calibration accuracy, the target points should be installed as far away from each coordinate axis as possible, with the preferred installation location being at least 400mm away from each coordinate axis.

[0076] For example, moving a six-DOF motion platform to the midpoint. (0, 0, 0), following the above method, rotate the target by the corresponding angles in the negative and positive directions of the X, Y, and Z axes respectively, and use the measuring platform to measure the coordinates of the target when the motion platform moves to the corresponding position. The resulting target coordinates are: the seventh coordinate point. Eighth coordinate point Ninth coordinate point 10th coordinate point Eleventh coordinate point 12th coordinate point .

[0077] Then the first set of target points can be obtained. Second point set Among them, the first set of points = ( , , , , , ), second point set = ( To improve the accuracy of coordinate point acquisition, multiple data points can be collected for each coordinate point and averaged.

[0078] Step S12: Calculate the first rotation matrix and the position coordinates of the origin of the motion platform in the measurement platform coordinate system based on the target coordinates after each translation and rotation of the motion platform by the preset amount. .

[0079] The first rotation matrix is ​​calculated based on the first set of points. Specifically, it is first calculated based on the first set of points. This yields the axial direction vector of the first coordinate system, which includes the X-axis direction vector. Y-axis direction vector Z-axis direction vector .

[0080] ,

[0081] ,

[0082] .

[0083] Again , , Orthogonalization process is performed to obtain , , ,in, The direction vector on the X-axis. The direction vector on the Y-axis. The direction vector on the Z-axis is finally determined by... , , We obtain the first rotation matrix R, R=[ , , ].

[0084] The position coordinates of the origin of the motion platform's coordinate system in the measurement platform's coordinate system The second set of points is calculated, specifically using steps S121 to S122.

[0085] Step S121: Based on the second point set Each coordinate point pair determines the chord length. Based on the projection method, the direction vector perpendicular to the corresponding chord and pointing towards the center of the corresponding rotation circle is determined. Based on the chord length, rotation angle, and direction vector perpendicular to the corresponding chord and pointing towards the center of the corresponding rotation circle for each coordinate point pair, the rotation circle corresponding to the coordinate point pair is determined. The rotation axis corresponding to each rotation circle is obtained by solving the geometric relationship of the chord length. , where the axis of rotation Including the first axis of rotation Second axis of rotation and the third axis of rotation Specifically, this step is implemented using steps S1211 to S1213.

[0086] Second point set The coordinate point pairs in the diagram include a first coordinate point pair, a second coordinate point pair, and a third coordinate point pair. The first coordinate point pair includes the coordinate points of the target after the motion platform rotates by corresponding angles in the positive and negative X-axis directions from the midpoint. , The second coordinate point pair includes the target's coordinate points after the motion platform rotates by corresponding angles in the positive and negative Y-axis directions from the midpoint, i.e., including... , The third coordinate point pair includes the target's coordinates after the motion platform rotates by corresponding angles in the positive and negative Z-axis directions from the midpoint, i.e., it includes... , Each coordinate point corresponds to a circle of revolution and an axis of revolution.

[0087] Step S1211: Determine the direction vector that is perpendicular to the corresponding chord and points to the center of the corresponding rotation circle based on the projection method.

[0088] Specifically, first set the second set of points The coordinate points in the image are projected onto a plane perpendicular to the corresponding rotation axis to obtain the set of projection points corresponding to each coordinate point pair. .

[0089] in, = ,

[0090] = ,

[0091] In the formula, Coordinates The corresponding projection point, Coordinates The corresponding projection point, Coordinates The corresponding projection point, Coordinates The corresponding projection point, Coordinates The corresponding projection point, Coordinates The corresponding projection point, Let nr or pr, and i be x, y, or z. This represents the dot product.

[0092] Then, the projection displacement vector corresponding to each coordinate point pair is calculated based on the projection point set.

[0093] Specifically, the projected displacement vector Including the first projected displacement vector corresponding to the first coordinate point pair The second projected displacement vector corresponding to the second coordinate point pair and the third projected displacement vector corresponding to the third coordinate point. .

[0094] Specifically,

[0095] First projected displacement vector for:

[0096] = ;

[0097] Second projection displacement vector for:

[0098] = ;

[0099] Third projection displacement vector for:

[0100] = .

[0101] Finally, based on the projected displacement vector and the first rotation matrix R, a direction vector perpendicular to the corresponding chord and pointing towards the corresponding rotation circle is determined.

[0102] A direction vector perpendicular to the corresponding chord and pointing towards the corresponding circle of revolution. ,

[0103] In the formula,

[0104] This represents the cross product.

[0105] Step S1212: Obtain the rotation axis corresponding to each rotating circle by solving the geometric relationship of the chord length. Specifically, this step can be implemented by steps S12121 to S12124.

[0106] Step S12121: Calculate the coordinates of the midpoint of the chord length of the rotating circle corresponding to each of the projected displacement vectors.

[0107] Specifically, calculate the displacement vector relative to the first projection vector. Second projected displacement vector and the third projected displacement vector Given the coordinates of the midpoint of the chord of the circle of revolution, then the coordinates of the midpoint are... Including the coordinates of the first midpoint Coordinates of the second midpoint and the coordinates of the third midpoint .

[0108] Due to the projected displacement vector The modulus is the chord length on the corresponding circle of revolution, and the coordinates of the first midpoint are... The first projected displacement vector The coordinates of the midpoint of the chord of the circle of revolution

[0109] ;

[0110] Coordinates of the second midpoint The second projected displacement vector The coordinates of the midpoint of the corresponding chord of the circle of revolution

[0111] ;

[0112] Coordinates of the third midpoint The third projected displacement vector The coordinates of the midpoint of the corresponding chord of the circle of revolution

[0113] .

[0114] Step S12122: Calculate the corresponding chord center distance based on the projected displacement vector and the corresponding rotation angle.

[0115] String center distance This includes the first chord center distance, the second chord center distance, and the third chord center distance. Specifically, the first chord center distance is calculated based on the first projected displacement vector and the corresponding rotation angle; the second chord center distance is calculated based on the second projected displacement vector and the corresponding rotation angle; and the third chord center distance is calculated based on the third projected displacement vector and the corresponding rotation angle.

[0116] For example, the angle of rotation of the motion platform in all directions after reaching the midpoint is α, based on the chord length and the radius of the rotation circle. Relationship || ||= available:

[0117] ,

[0118] Furthermore, the center distances of each chord can be obtained, where,

[0119] First String Distance for:

[0120] ;

[0121] Second string center distance for:

[0122] ;

[0123] The distance from the center of the third chord is:

[0124] .

[0125] It is important to explain here that, assuming the motion platform rotates by the same angle α in all directions after reaching the midpoint, the distances of the target from the X, Y, and Z axes will be different when the motion platform is at the midpoint, resulting in unequal radii of the corresponding rotation circles for each coordinate point.

[0126] Step S12123: Calculate the center coordinates of the corresponding rotating circle based on the midpoint coordinates of the chord length of the rotating circle, the direction vector perpendicular to the corresponding chord and pointing towards the corresponding rotating circle, and the distance from the center of the chord.

[0127] Coordinates of the center of the corresponding circle of revolution = - ,but:

[0128] The coordinates of the first coordinate point relative to the center of the corresponding circle of revolution for:

[0129] = ,

[0130] The coordinates of the second coordinate point relative to the center of the corresponding circle of revolution for:

[0131] = ,

[0132] The coordinates of the third coordinate point relative to the center of the corresponding circle of revolution for:

[0133] = .

[0134] Step S12124: Calculate the corresponding rotation axis based on the center coordinates of the rotating circle and the first rotation matrix R.

[0135] axis of rotation Including the first axis of rotation Second axis of rotation and the third axis of rotation (t).

[0136] Among them, the first axis of rotation for

[0137] ,

[0138] Where t is a parameter, and R represents the set of real numbers.

[0139] Second axis of rotation for:

[0140] .

[0141] Third axis of rotation for:

[0142] .

[0143] Step S122: Solve for the position coordinates of the origin of the motion platform's coordinate system in the measurement platform's coordinate system based on each rotation axis and the constructed least-squares objective function. .

[0144] The position coordinates of the origin of the motion platform's coordinate system in the measurement platform's coordinate system It should be the intersection of the three rotation axes; however, measurement errors may cause the three rotation axes to not exactly intersect. Therefore, this solution uses the least squares method to find the coordinate point.

[0145] Specifically, the least-squares objective function for constructing the sum of squared distances from a point to a line is... for:

[0146] ,

[0147] in, express The sum of the squares of the distances to each axis of rotation.

[0148] Step S13: Based on the position coordinates of the origin of the motion platform in the measurement platform coordinate system. The second coordinate system is calculated using the first rotation matrix R. With the first coordinate system The transformation relationship between them.

[0149] P OR =R ,

[0150] In the formula, For the coordinate system of the motion platform The point in the middle;

[0151] For the coordinate system of the measurement platform The point in, and Correspondingly.

[0152] Step S2: Collect and confirm the pose data of the feature points of the workpiece to be docked.

[0153] This step requires collecting feature points from both the first and second workpieces to be docked. Feature point collection can be achieved using a robotic arm controlling a 3D camera, manual operation with a 3D camera, or a coordinate measuring machine, or by employing the methods described in steps S211 to S215 above. To improve the accuracy of point cloud data acquisition, it is preferable to use the methods described in steps S211 to S215 above.

[0154] When using a robotic arm to control a probe to acquire feature points guided by a 3D camera, the first workpiece to be docked... Set on the motion platform, in an active state; the second workpiece to be docked. The system is in a fixed state. The feature points on the first and second workpieces to be docked are collected by controlling the robotic arm to move the probe. During feature point collection, only the docking surfaces and sidewalls of the first and second workpieces can be collected; if the workpieces to be docked have locating pin holes, feature points can be collected from these holes to improve docking accuracy.

[0155] When only feature points on the docking surface and sidewalls are collected, the pose data is determined using the following steps.

[0156] First, the robotic arm moves the probe to the corresponding points on the docking surfaces and sidewalls of the first and second workpieces to be docked, respectively. Steps S211 to S215 are then used to determine the point cloud data for these points, thus obtaining the first point cloud set. The first point cloud set includes the point cloud set of the first workpiece to be docked and the point cloud set of the second workpiece to be docked. The point cloud corresponding to the docking surface and sidewall of the first workpiece to be docked, and the point cloud set of the second workpiece to be docked. The point cloud corresponds to the docking surface and sidewall of the second workpiece to be docked. The point cloud set of the first workpiece to be docked. The second workpiece to be docked is clustered together The number of point clouds on the central docking surface and the sidewalls is at least 4 each.

[0157] Next, the first workpiece to be docked was clustered together. The second workpiece to be docked is clustered together The pose data of the first docking surface were obtained by using pose calculation algorithms respectively. Second docking surface pose data Among them, the pose data of the first docking surface Second docking surface pose data All include the normal vector n of the mating surface and the coordinate point C of the intersection of the axis of the parts to be mated and the mating surface, that is:

[0158] = ( , ),

[0159] = ( , ),

[0160] The coordinates of the point where the axis of the first part to be docked intersects with its docking surface are the coordinates of the axis centerline of the first part to be docked. This represents the normal vector of the mating surface of the first pair of parts to be mated;

[0161] The coordinates of the point where the axis of the second part to be docked intersects with its docking surface are the coordinates of the axis centerline of the second part to be docked. This represents the normal vector of the mating surface of the second part to be mated.

[0162] To improve the docking accuracy, pin holes can be introduced simultaneously. In this case, the feature points of the docking surface and the pin holes need to be collected.

[0163] First, the robotic arm moves the probe to the corresponding points on the docking surfaces and sidewalls of the first and second workpieces to be docked, respectively. The point cloud data of these points is determined using steps S211 to S215, thereby obtaining the point cloud set corresponding to the first workpiece to be docked. The second workpiece point cloud corresponding to the second workpiece to be docked Among them, the first batch of workpieces to be docked is clustered together. The second batch of workpieces to be docked is gathered together. There are at least 4 point clouds on the docking surface and at least 4 point clouds on the sidewalls.

[0164] This method also requires the acquisition of pin hole point cloud data, which can be achieved using a 3D camera to obtain a second point cloud set. The second point cloud set includes a first pin hole point cloud set composed of the point clouds at the pin holes of the first workpiece to be docked, and a second pin hole point cloud set composed of the point clouds at the pin holes of the second workpiece to be docked. Of course, the pin hole point cloud data can also be obtained using other existing methods.

[0165] Thus, we have obtained the first and second cloud clusters.

[0166] For the pose data of the docking surface:

[0167] The first workpiece to be docked is clustered together. The point cloud on the docking surface of the first component to be docked is fitted to obtain the normal vector of the docking surface of the first component to be docked. A point on the mating surface of the first part to be mated The first workpiece to be docked is clustered together. The center of the cylinder base where the point cloud on the side wall of the first part to be docked is located is obtained by fitting the point cloud on the side wall of the first part to be docked. radius corresponding to the center of the circle Based on the normal vector of the mating surface of the first part to be mated. One point on the mating surface of the first part to be mated The center of the cylinder base where the point cloud on the side wall of the first part to be docked is located. Project the circle corresponding to the bottom surface of the cylinder where the point cloud of the side wall of the first part to be docked is located onto the docking surface of the first part to be docked, and obtain the center of the docking surface of the first part to be docked. Therefore, based on the normal vector of the mating surface of the first pair of parts to be mated... The center of the first mating surface of the parts to be mated The radius of the mating surface of the first pair of parts to be mated The pose data of the docking surface of the first part to be docked can be obtained. The same method is used to obtain the docking surface pose data of the second workpiece to be docked, by applying the same point set to the second workpiece to be docked. .

[0168] In response,

[0169] = ( , ),

[0170] = ( , ).

[0171] Pose data for the pin hole:

[0172] The first pin hole pose data are calculated based on the first pin hole point set and the second pin hole point set, respectively. Second pin hole pose data The first pin hole pose data includes the axis coordinates of the pin hole on the first part to be docked. The normal vector of the pin hole on the first part to be mated The second pin hole pose data includes the coordinates of the axis of the pin hole on the second part to be docked. Normal vector of the pin hole on the second part to be mated .

[0173] In response,

[0174] =( , ),

[0175] =( , ).

[0176] Step S3: Calculate the pose adjustment parameters of the docking parts.

[0177] In step S2, only the feature points of the docking surface are collected to obtain the pose data of the docking surface. At this point, it is necessary to use the pose data of the docking surface of the first part to be docked. The pose data of the docking surface of the second part to be docked Determine the pose adjustment parameters, specifically including steps S311 to S313.

[0178] Step S311: Calculate the normal vector from the mating surface of the first part to be mated using quaternions. Rotate to the normal vector of the mating surface of the second part to be mated The second rotation matrix .

[0179] Step S312: According to the second rotation matrix Calculate the rotational pose data of the first docking surface after rotational transformation. = ( , ).

[0180] Step S313: Based on the pose data of the docking surface of the second part to be docked First docking surface rotation pose data = ( , Determine the first translation vector of the first part to be docked. ,in,

[0181] = .

[0182] From this, the pose adjustment parameters P of the first part to be docked can be obtained, where,

[0183] P=( , ).

[0184] Different operating environments, due to variations in temperature, humidity, and other factors, have varying impacts on the system, causing errors in various components, such as the motion platform and the measurement platform. To mitigate these environmental errors and improve the docking accuracy of the parts to be docked, the pose adjustment parameter P of the first part to be docked can be further compensated during docking control. In this method, only the first translation vector needs to be adjusted. Compensation will be provided.

[0185] Specifically, the first translation vector The compensation value is calculated using steps S3131 to S3136. The first translation vector... The compensation value can be determined during the initial docking control and used directly in subsequent module dockings; alternatively, this method can be used periodically for the first translation vector. Determining the compensation value.

[0186] Step S3131: Extract the transformation Euler angles of the motion platform coordinate system based on the first rotation matrix R. According to the pose data of the second docking surface and The centroid translation vector of the motion platform is calculated. ,in,

[0187] = ( , ),

[0188] = .

[0189] For example:

[0190] Transformation Euler angles Including ψ, And φ. Assuming the rotation is externally rotated by Euler angles and then rotated in the ZYX order, ψ is the Euler angle of rotation about the Z-axis; φ is the Euler angle of rotation about the Y-axis; φ is the Euler angle of rotation about the X-axis.

[0191] If column vectors and active rotation conventions are used, then the first rotation matrix R is:

[0192] ,

[0193] in, This is a rotation matrix with the X-axis as the rotation axis; This is a rotation matrix with the Y-axis as the rotation axis; Let Z be the rotation matrix.

[0194]

[0195] After unfolding, you get:

[0196]

[0197] That is, we get:

[0198] ,

[0199] R is a 3x3 matrix. , , , , , , , , This is the element at the corresponding position in R.

[0200] In the case of non-gimbal lock, cos ≠0, the corresponding inverse formula is:

[0201] .

[0202] Step S3132: Set the first coordinate system of the motion platform According to the transformation Euler angles Transform to the second coordinate system Parallel, and translated by the centroid. Swap the corresponding positions of the centroids.

[0203] Step S3133, from the second rotation matrix Extracting the Euler angles of rotation The coordinates Pt of the docking preparation position are determined based on the axis coordinates of the docking surfaces of the second workpiece to be docked, the preset distance, and the direction from the axis of the second workpiece to be docked surface to the docking surface of the first workpiece to be docked. Rd .

[0204] The preset distance here can be set according to the situation, such as 10mm, 20mm, 30mm or other values.

[0205] Step S3134: Based on the coordinates Pt of the docking preparation position. Rd The second translation vector t of the first part to be docked is determined by the coordinates of the axis center of the first part to be docked. rd ,in,

[0206] t rd =Pt Rd Pt Fc .

[0207] Step S3135: Control the motion platform to drive the first part to be docked to rotate at Euler angles. Rotate and by the second translation vector t rd Move it to the docking preparation position.

[0208] The control motion platform drives the first part to be docked according to the Euler angle of rotation. And according to the second translation vector t rd After translation and pose adjustment, the axes of the first and second parts to be docked are aligned.

[0209] Then by the second rotation matrix The rotational pose data of the first docking surface after the first docking component has undergone rotational changes can be obtained. = ( , ).

[0210] Step S3136: Measure the center coordinates of the first workpiece to be docked after completing step S3135. The center coordinates of the second workpiece docking surface ,according to , The compensation amount of the first translation vector is calculated. .

[0211] Therefore, the compensated translation vector is obtained. ,in, = + .

[0212] In response, the position adjustment parameters of the first docking component after compensation for:

[0213] = ( ).

[0214] In step S2, the pose data of the mating surface is obtained by collecting feature points of the mating surface and pin hole. Then, based on the pose data of the mating surface of the first part to be mated... The pose data of the docking surface of the second component to be docked First pin hole position data Second pin hole pose data Determine the pose adjustment parameters, specifically including steps S321 to S324.

[0215] Step S321: Calculate the normal vector from the mating surface of the first part to be mated using quaternions. Rotate to the normal vector of the mating surface of the second part to be mated The second rotation matrix .

[0216] Step S322: According to the second rotation matrix Calculate the rotational pose data of the first docking surface after rotational transformation. = ( , ).

[0217] Step S323: Based on the pose data of the docking surface of the second part to be docked First docking surface rotation pose data = ( , Determine the first translation vector of the first part to be docked. ,in,

[0218] = .

[0219] Step S324: Calculate the pin hole rotation matrix Specifically, this includes steps S3241 to S3247.

[0220] Step S3241: Based on the coordinates of the axis of the first part to be docked and the first pin hole position data Calculate the first vector pointing from the center of the first mating surface to the center of the pin hole. ,in,

[0221] = .

[0222] Step S3242: Calculate the first vector According to the second rotation matrix The second vector after rotation ,in,

[0223] = .

[0224] Step S3243, according to the second vector Rotational pose data of the first docking surface = ( , Calculate the pose data of the pin holes on the first part to be aligned. ,in,

[0225] = ( + , ).

[0226] Step S3244, will and Perform axial projection onto the mating surface of the first pair of parts to be mated, and obtain the first projection point. ;Will , Perform axial projection onto the mating surface of the second part to be mated, and obtain the second projection point. .

[0227] Step S3245: Based on the first projection point and Calculate the third vector pointing from the center of the mating surface of the first mating component to the center of the pin hole on the first mating component. According to the second projection point The pose data of the docking surface of the second part to be docked Calculate the fourth vector pointing from the center of the mating surface of the second part to be mated to the center of the pin hole on the second part to be mated. .

[0228] Step S3246: Calculate the third vector With the fourth vector The angle θ between them.

[0229] Step S3247: Based on the angle θ and the normal vector of the mating surface of the second part to be mated The pin hole rotation matrix is ​​calculated using the right-hand rule. .

[0230] From this, the pose adjustment parameters P of the first part to be docked can be obtained, where,

[0231] P=( , ).

[0232] Similar to the previous method, to improve docking accuracy, the pose adjustment parameter P of the first part to be docked can be compensated. During compensation, the following parameters are applied: and At least one parameter should be compensated. Prioritized, while simultaneously... and Compensation is performed. Specifically, compensation is carried out using the methods described in steps S331 to S339.

[0233] Step S331: Extract the transformation Euler angles of the motion platform coordinate system based on the first rotation matrix R. Then, based on the pose data of the second docking surface and The centroid translation vector of the motion platform is calculated. ,in,

[0234] = ( , ),

[0235] = .

[0236] Step S332: Set the first coordinate system of the motion platform According to the transformation Euler angles Transform to the second coordinate system Parallel, and translated by the centroid. Swap the corresponding positions of the centroids.

[0237] Step S333, from Extracting the Euler angles of rotation The coordinates Pt of the docking preparation position are determined based on the axis coordinates of the docking surfaces of the second workpiece to be docked, the preset distance, and the direction from the axis of the second workpiece to be docked surface to the docking surface of the first workpiece to be docked. Rd And based on the coordinates Pt of the docking preparation position. Rd The second translation vector t of the first part to be docked is determined by the coordinates of the axis center of the first part to be docked. rd ,in,

[0238] t rd =Pt Rd Pt Fc .

[0239] Step S334: Control the motion platform to drive the first part to be docked according to the Euler angle rotation. Rotate and by the second translation vector t rd Move it to the docking preparation position.

[0240] The first part to be connected is based on the Euler angle of rotation. and the second translation vector t rd After completing the pose adjustment, the axes of the first and second parts to be docked are aligned.

[0241] Step S335: Measure the coordinates of the center of the pin hole on the first part to be connected after completing step S334. The coordinates of the center of the pin hole on the second part to be connected The center coordinates of the first docking surface of the component to be docked The center coordinates of the second docking surface .

[0242] Step S336: According to the center coordinates of the first mating surface of the parts to be mated The center coordinates of the second mating surface The compensation amount of the first translation vector is calculated. .

[0243] Obtain the compensated translation vector ,in, = + .

[0244] Step S337: According to the center coordinates of the first mating surface of the parts to be mated The coordinates of the center of the pin hole on the first part to be connected The fifth vector pointing from the center of the first part to be docked to the center of the pin hole on the first part to be docked is calculated. According to the center coordinates of the docking surfaces of the second part to be docked The coordinates of the center of the pin hole on the second part to be connected The sixth vector pointing from the center of the second part to be docked to the center of the pin hole on the second part to be docked is calculated. .

[0245] Step S338, according to the fifth vector The sixth vector Calculate the angle difference between the pin holes on the first and second parts to be coupled. .

[0246] Step S339: Based on the angle difference The alignment compensation value of the pin hole is determined according to the right-hand rule based on the actual rotation direction of the first part to be docked. .

[0247] This can be addressed by adjusting the pin hole alignment compensation value. The angle θ and the normal vector of the mating surface of the second part to be mated. The compensated pin hole rotation matrix is ​​calculated. .

[0248] In response, the position adjustment parameters of the first docking component after compensation for:

[0249] = ( , ).

[0250] Step S4: Docking control.

[0251] This step can be based on the pose adjustment parameter P determined in step S3 or the pose adjustment parameter after compensation for the first docking part. The existing control method is used to adjust the position of the first docking component to achieve precise docking between the first docking component and the second docking component.

[0252] The third aspect of the present invention discloses an apparatus, as described above, which is the control unit of the system corresponding to this solution. The control unit includes a memory and a controller connected in sequence. The memory stores a computer program, and the controller is used to read the computer program and execute a point cloud data acquisition method as described in the first aspect, or execute a precise docking method as described in the second aspect.

[0253] Specifically, the operating principle of the device described in the third aspect is detailed in the second aspect and will not be repeated here.

[0254] Those skilled in the art will recognize that the embodiments described herein are intended to help the reader understand the principles of the invention, and should be understood that the scope of protection of the invention is not limited to such specific statements and embodiments. Those skilled in the art can make various other specific modifications and combinations based on the technical teachings disclosed in this invention without departing from the spirit of the invention, and these modifications and combinations are still within the scope of protection of this invention.

Claims

1. A point cloud data acquisition method, characterized in that, Includes the following steps: The first compression amount of the probe at the first moment, the coordinates of the robot arm in the base coordinate system at the second moment, and the second compression amount of the probe and the coordinates of the robot arm in the base coordinate system at the third moment are obtained respectively. The probe is fixed on the actuator at the end of the robot arm and can move relative to the actuator along its own axis. The axis of the probe is parallel to a coordinate axis of the tool coordinate system, which is the coordinate system corresponding to the actuator. The first moment is when the probe is not in contact with the workpiece to be docked. At the second and third moments, the probe is in contact with the workpiece to be docked. The contact position of the probe with the workpiece to be docked is the same at both moments and the probe is compressed. Between the second and third moments, the movement direction of the actuator is along the probe axis. The compression amount of the probe at the second moment is not equal to the compression amount of the probe at the third moment. The compression change of the probe is calculated based on the first compression amount and the second compression amount; The direction vector of the robot arm's motion is calculated based on the robot arm's coordinates at the second and third moments. The displacement components of the probe's compression change on each coordinate axis are calculated based on the probe's compression change and the direction vector of the robotic arm's movement. The coordinates of the probe tip point cloud in the base coordinate system are calculated based on the coordinates of the robotic arm at the third moment and the displacement components corresponding to each coordinate axis. When the probe just contacts the workpiece to be docked and the probe compression is the first compression, the coordinates of the point cloud corresponding to the probe tip in the base coordinate system are calculated. The direction vector of the robotic arm's motion, calculated based on the coordinates of the robotic arm at the second and third moments, is as follows: The second and third moments correspond to different times during the probe compression process, and represent the direction vector of the robotic arm's movement. for: =( ， ， )， or, The second and third moments correspond to different times during the probe's rebound process, and the direction vector of the robotic arm's movement is... for: =( ， ， )， in, The coordinates of the robotic arm at the third moment are... = ( ), This represents the X-coordinate value of the robotic arm at the third moment. The Y-coordinate value of the robotic arm at the third moment. The Z-coordinate value of the robotic arm at the third moment. The coordinates of the robotic arm at the second moment are... The direction vector of the robotic arm's movement Components on the X-axis The direction vector of the robotic arm's movement Components on the Y-axis, The direction vector of the robotic arm's movement Components on the Z-axis; The displacement components of the probe's compression change along each coordinate axis are calculated based on the probe's compression change and the direction vector of the robotic arm's movement. Calculate the displacement component of the probe's compression change on the X-axis of the base coordinate system. ,in, = ， In the formula, This represents the amount of compression change of the probe; Calculate the displacement component of the probe's compression change on the Y-axis of the base coordinate system. ,in, = ； Calculate the displacement component of the probe's compression change on the Z-axis of the base coordinate system. ,in, = ； The coordinates of the point cloud corresponding to the probe tip in the base coordinate system when the probe just contacts the workpiece and the probe compression is the first compression amount, calculated based on the coordinates of the robotic arm at the third moment and the displacement components corresponding to each coordinate axis, are as follows: Pt=( ) ( , , )。 2. A precise docking method, characterized in that, Includes the following steps: The method described in claim 1 acquires a first point cloud set on a workpiece to be docked, wherein the workpiece to be docked includes a first workpiece to be docked and a second workpiece to be docked, the first point cloud set includes a point cloud set of the first workpiece to be docked and a point cloud set of the second workpiece to be docked, the first point cloud set of the first workpiece to be docked includes point clouds on the docking surface and sidewall of the first workpiece to be docked, and the second point cloud set of the second workpiece to be docked includes point clouds on the docking surface and sidewall of the second workpiece to be docked. Based on the first point set, the normal vector and axis coordinates of the docking surface of the first and second workpieces to be docked are determined; The pose adjustment parameters for completing the docking of the first and second workpieces are calculated based on the normal vector and axis coordinate of the docking surface of the first and second workpieces to be docked. The first workpiece to be docked is controlled according to the pose adjustment parameters to complete the docking of the first workpiece to be docked and the second workpiece to be docked.

3. The precise docking method according to claim 2, characterized in that, Before acquiring the first point cloud set on the workpiece to be docked using the point cloud data acquisition method described in claim 1, the following steps are also included: The six-degree-of-freedom motion platform is controlled to translate or rotate by a preset amount in the positive and negative directions of the X, Y, and Z axes at the midpoint. A target is set on the motion platform. The target coordinates are measured by the measurement platform after each translation or rotation by the preset amount. The first rotation matrix and the position coordinates of the origin of the motion platform in the measurement platform coordinate system are calculated based on the target coordinates after each preset translation and rotation of the motion platform. ; Based on the position coordinates of the origin of the motion platform in the measurement platform coordinate system. The transformation relationship between the measurement platform coordinate system and the motion platform coordinate system is calculated using the first rotation matrix R: ， In the formula, For the coordinate system of the motion platform The point in the middle; R is the first rotation matrix; For the coordinate system of the measurement platform The point in, and Correspondingly.

4. The precise docking method according to claim 2, characterized in that, The pose adjustment parameters for completing the docking of the first and second workpieces, calculated based on the normal vector and axis coordinates of the docking surfaces of the first and second workpieces, include: The normal vector from the mating surface of the first part to be mated is calculated using quaternions. Rotate to the normal vector of the mating surface of the second part to be mated The second rotation matrix ; According to the second rotation matrix Calculate the rotational pose data of the first docking surface after rotational transformation. = ( , ); Based on the pose data of the docking surface of the second component to be docked First docking surface rotation pose data = ( , Determine the first translation vector of the first part to be docked. ,in, = ； According to the first translation vector Second rotation matrix Determine the pose adjustment parameters, or, based on the first translation vector. The compensation amount of the first translation vector Second rotation matrix Determine the pose adjustment parameters.

5. The precise docking method according to claim 4, characterized in that, Compensation amount of the first translation vector It was determined by the following method: The Euler angles of the transformation of the motion platform coordinate system are extracted based on the first rotation matrix R. ; Based on the pose data of the second docking surface The position coordinates of the origin of the motion platform's coordinate system in the measurement platform's coordinate system The centroid translation vector of the motion platform is calculated. ; The coordinate system of the motion platform is transformed using Euler angles. Transform to a coordinate system parallel to the measurement platform, and translate the vector by the centroid. Swap the corresponding positions of the centroids; From the second rotation matrix Extracting the Euler angles of rotation The coordinates Pt of the docking preparation position are determined based on the axis coordinates of the docking surfaces of the second workpiece to be docked, the preset distance, and the direction from the axis of the second workpiece to be docked surface to the docking surface of the first workpiece. Rd ; Based on the coordinates Pt of the docking preparation position Rd The second translation vector t of the first part to be docked is determined by the coordinates of the axis center of the first part to be docked. rd ; The control motion platform drives the first part to be docked to rotate at Euler angles. Rotate and by the second translation vector t rd Move to the docking preparation position; Measure the center coordinates of the mating surfaces of the first workpiece to be docked at this time. The center coordinates of the second workpiece docking surface ,according to , The compensation amount of the first translation vector is calculated. .

6. The precise docking method according to claim 4, characterized in that, Before determining the normal vector and axis coordinates of the docking surfaces of the first and second workpieces to be docked based on the first point cloud, the method further includes obtaining a second point cloud, which includes a first pin hole point cloud composed of point clouds at the pin holes of the first workpiece to be docked and a second pin hole point cloud composed of point clouds at the pin holes of the second workpiece to be docked. Based on the second point, the first pin hole position data of the first pin hole on the first workpiece to be docked is determined. The second pin hole pose data of the second pin hole on the second workpiece to be docked ; The step of calculating the pose adjustment parameters for completing the docking of the first and second workpieces based on the normal vector and axis coordinates of the docking surfaces of the first and second workpieces to be docked also includes: Based on the pose data of the docking surface of the first component to be docked The pose data of the docking surface of the second component to be docked First pin hole position data Second pin hole position data Determine the rotation matrix of the pin hole , Based on the pin hole rotation matrix Second rotation matrix and the first translation vector Determine the pose adjustment parameters, or, based on the pin hole rotation matrix. , pin hole alignment compensation value Second rotation matrix First translation vector Compensation amount of the first translation vector Determine the pose adjustment parameters.

7. The precise docking method according to claim 6, characterized in that, The pin hole alignment compensation value Compensation amount of the first translation vector It was determined by the following method: The Euler angles of the transformation of the motion platform coordinate system are extracted based on the first rotation matrix R. Then, based on the pose data of the second docking surface The position coordinates of the origin of the motion platform's coordinate system in the measurement platform's coordinate system The centroid translation vector of the motion platform is calculated. ; The first coordinate system of the motion platform According to the transformation Euler angles Transform to the second coordinate system Parallel, and translated by the centroid. Swap the corresponding positions of the centroids; from Extracting the Euler angles of rotation The coordinates Pt of the docking preparation position are determined based on the axis coordinates of the docking surfaces of the second workpiece to be docked, the preset distance, and the direction from the axis of the second workpiece to be docked surface to the docking surface of the first workpiece to be docked. Rd ; Based on the coordinates Pt of the docking preparation position Rd The second translation vector t of the first part to be docked is determined by the coordinates of the axis center of the first part to be docked. rd ; The control motion platform drives the first part to be docked to rotate at Euler angles. Rotate and by the second translation vector t rd Move to the docking preparation position; Measure the coordinates of the center of the pin hole on the first part to be connected at this time. The coordinates of the center of the pin hole on the second part to be connected The center coordinates of the first docking surface of the component to be docked The center coordinates of the second mating surface ; According to the center coordinates of the first docking surface of the component to be docked The center coordinates of the second mating surface The compensation amount of the first translation vector is calculated. ; According to the center coordinates of the first docking surface of the component to be docked The coordinates of the center of the pin hole on the first part to be connected The fifth vector pointing from the center of the first part to be docked to the center of the pin hole on the first part to be docked is calculated. According to the center coordinates of the docking surfaces of the second part to be docked The coordinates of the center of the pin hole on the second part to be connected The sixth vector pointing from the center of the second part to be docked to the center of the pin hole on the second part to be docked is calculated. ; According to the fifth vector The sixth vector Calculate the angle difference between the pin holes on the first and second parts to be coupled. ; According to the angle difference The alignment compensation value of the pin hole is determined according to the right-hand rule based on the actual rotation direction of the first part to be docked. .

8. A control device comprising a memory and a controller connected in sequence, wherein the memory stores a computer program, characterized in that: The controller is used to read the computer program and execute the point cloud data acquisition method according to claim 1, or execute the precise docking method according to any one of claims 2-7.

Images