TW202112514A - Method and device and system for calibrating position and orientation of a motion manipulator - Google Patents

Abstract

A position and gesture calibrating method comprises steps of providing a calibrating device having a plurality of calibrating bodies, each of which has at least one characteristic point, selectively arranging an image acquiring unit or the calibrating device on an end effecter on a motion manipulator with a base, capturing a plurality of images respectively corresponding to different position and gesture of the operation device in a calibration space, analyzing a transformation relation (Li) between the end effecter and the base at each position and gesture, analyzing an actual transformation relation (Pi), e.g.^S T_{W, measured} , between the measuring coordinate and an absolute coordinate of the characteristic points in each image, obtaining a hand-eye transformation relation (X) through (Li) and (Pi), calculating a transformation relation (Z) between the base and the absolute coordinate through (Li), (Pi), and (X), calculating an nominal transformation relation according to (Z), (Li) and (X), and determining a compensation information associated with arbitrary position and gesture of the motion manipulator in the calibration space according to the transformation relation Li, Pi, X, and Z.

Description

Position and posture correction system and method of motion device

The present invention relates to a position and posture correction technology, in particular to a position and posture correction system and method that uses a correction tool to assist in establishing a correction space for correcting a motion device to adjust the position accuracy of the motion device.

With the advancement of technology, the combination of network communication and sensing technology, the demand for robotic arms or automatic processing tools in the manufacturing industry has gradually increased. Because mechanical arms or automatic processing tools can reduce manpower, increase production efficiency, and can meet the accuracy requirements that cannot be achieved by ordinary manpower processing, in recent years, their related applications have received great attention.

However, the cost of high-precision robotic arms or automatic processing tools is quite high, and it is not affordable for ordinary small and medium-sized enterprises. Therefore, how to assist and improve the machining accuracy under cost-effective control, so that the general middle and low-end robotic arms can also improve their machining accuracy without changing their systems, which is an important research topic.

In conventional technologies, for example, the Republic of China Patent No. 201509617 teaches a robot arm accuracy measurement system and method, which includes a reference plate, at least two sensing probes, and a ball lens group. The reference plate can be set on the robot arm. The working platform, and each of the sensing probes is set on the reference plate, and the sensing probe is composed of two laser emitting elements and two four-quadrant light position detectors arranged in a horizontal staggered manner, and a spherical lens group It is set at the end of the robot arm, so that the ball lens group can move with the robot arm to move the X-axis, Y-axis and Z-axis, so that the ball lens group can be used in each sensing probe during the movement of the robot arm. The reciprocating measurement is used to detect the combined error of the joint axis of the robot and the error of a single joint axis in real time, and a compensation parameter is generated in real time for the robot to perform compensation actions.

In addition, the Republic of China Announcement Patent No. 468055 also teaches a method of using images to perform accuracy correction. It is a method for controlling the movement or processing of an operation object by a robot arm, which is characterized by: recording the state of the operation object and The database calculation of the ideal state after the operation is how the operation object in the ideal state after the operation is captured by the camera device, and the actual image captured by the camera device and the calculated image are superimposed on the same surface to display, and control The mechanical arm makes the actual image displayed on the display device match the calculated image.

The present invention provides a position and posture correction system and method. An image capture probe set on a specific part (for example, end effector of a robotic arm) on a motion device is moved to a position relative to a world coordinate system at the specific part. Under each different nominal position and posture, the image of the calibration device is obtained, and the feature points of the calibration body in the acquired image are calculated to obtain the calibration body on the calibration device under the measurement coordinate system of the image capture probe A relative position. Since the corresponding at least six feature points in each image can be measured in advance by other precision instruments to have a known absolute spatial position, the conversion relationship between the absolute position and the corresponding relative position and the use of hand-eye correction The obtained hand-eye conversion relationship is calculated, and the actual position and posture of the specific part on the sports device can be known. Since the nominal position and posture of a specific part of the exercise device, such as the end effector, are known, the relationship between the actual position and posture and the nominal position and posture can be obtained through calibration to obtain the specific position on the exercise device. The movement error (including the error of the six parameters of the three-axis position and the three-axis rotation attitude), and then the movement error can be compensated. In this way, the accuracy of the position and posture of the motion device can be improved, so that the position and posture control accuracy of the calibrated motion device surpasses the factory precision.

The present invention provides a position and posture correction system and method. An additional device, such as a correction device or an image capturing probe, is provided on a transfer device capable of performing at least one-dimensional movement to perform the position and posture correction of the movement device. Calibration is used for motion devices in areas with a large range of movement. By means of an image capture probe or correction device set on a specific part of the motion device (for example, the end effector of a robotic arm), the motion device is moved relative to Under various operating positions of a world coordinate system, the image of the calibration device is obtained, and the feature points of the calibration body in the obtained image are calculated to obtain the measurement coordinate system of the image capture probe. A relative position of each calibration body. Since the movement space of the movement device is usually much larger than the volume of the correction device, in order to ensure that the nominal position and posture of the movement device can be detected and corrected by the correction device, and reduce the error caused by the assembly of the correction device, the correction device can pass the Transfer device (such as XYZ three-axis right-angle coordinate displacement platform) to change the position, so as to increase the correction space to cover the entire motion space of the motion device. Since the movement dimensions of the transfer device of the present invention are orthogonal to each other, the problem of mutual superimposition of movement errors can be avoided. Generally speaking, the displacement accuracy of the transfer device after correction can be much higher than that of the robotic arm motion device. Therefore, the transfer device can provide an accurate reference spatial position and expand the space to correct the precise spatial position and posture of a specific part on the sports device. By setting a single correction device on the transfer device to correct the position and posture of the motion device in a processing environment, as there is no need to assemble the correction device, it can reduce the time spent in assembly and avoid ensuring that each correction unit is assembled and maintained Fix the required cost.

In one embodiment, the present invention provides a position and attitude correction method, which includes the following steps. First, provide a movement device with an end effector. Next, a correction space with respect to the motion range of the motion device is established, which has a plurality of correction points. Then, a calibration device is provided, which includes at least one supporting structure and a plurality of calibration bodies arranged on the at least one supporting structure, each calibration body has at least one feature point, and the feature points of the plurality of calibration bodies have at least The four are not coplanar. Then, on the end effector, choose to set up a calibration device or an image capture probe. Then, the motion device is controlled to move the end effector to each calibration point, so that at each calibration point, the arm calibration position and posture of the motion device are different, and at each calibration point, the image capture probe is used to capture Taking an arm-calibrated image related to the calibration device, each arm-calibrated image has at least six feature points. Furthermore, according to the correction position and posture of each arm of the motion device, a correction conversion relationship between the end effector and an arm of the motion device is determined for each correction point. Then, according to the position of each feature point in the calibration image of each arm relative to a measurement coordinate position in the measurement coordinate system of the image capture probe and a world coordinate position corresponding to the world coordinate system, it is determined about each The conversion relationship between a measurement of a calibration point and the world coordinate. Finally, according to the arm calibration conversion relationship of each calibration point and the conversion relationship between the measurement and the world coordinate, the compensation information of the motion device at any position and posture in the calibration space is determined.

In one embodiment, the present invention provides a position and posture correction system, which includes an image capture probe, a correction device, a movement device, and an arithmetic processing device. The calibration device includes at least one supporting structure and a plurality of calibration bodies arranged on the at least one supporting structure, each calibration body has at least one characteristic point, and the plurality of calibration bodies has at least four characteristic points. Coplanar. The movement device has an end effector thereon, wherein one of the image capturing probe or the correction device is arranged on the end effector, the movement device is located in a correction space, and the correction device There are multiple correction points in the space. The arithmetic processing device is coupled with the motion device, and the arithmetic processing device controls the motion device to move the end effector to each correction point, so that at each correction point, the arm correction position of the motion device is It is not the same as the posture, and at each calibration point, the image capturing probe is used to capture the arm calibration image of the calibration device. Each arm calibration image has at least six feature points, and the position is calibrated according to each arm of the motion device. And the posture determines the calibration conversion relationship between the end-effector for each calibration point and an arm of the motion device, and the measurement coordinate system of each feature point in the calibration image with respect to the image capturing probe is based on each arm. The position of the measurement coordinate system below and the position of the world coordinate system corresponding to the world coordinate system determine the conversion relationship between a measurement and the world coordinate for each calibration point, and the conversion relationship between the arm calibration of each calibration point and the quantity The conversion relationship between measurement and world coordinates determines the compensation information of the motion device at any position and posture in the space to be calibrated.

In another embodiment, the present invention provides a position and posture correction device, which includes an image capturing probe, a correction device, a transfer device, and an arithmetic processing device. The calibration device includes at least one supporting structure and a plurality of calibration bodies arranged on the at least one supporting structure, each calibration body has at least one characteristic point, and the plurality of calibration bodies has at least four characteristic points. Coplanar. The transfer device is used to carry the image capturing probe or the calibration device. The arithmetic processing device controls a motion device to move one of the end effectors on the motion device to a plurality of correction points in the space where the motion device is located in a plurality of different correction positions and postures, so that the image capturing probe is placed on each The correction point captures the correction image of the arm of the correction device. Each arm correction image has at least six characteristic points. The arithmetic processing device determines the end of each correction point according to each position and posture of the motion device. The effector is calibrated and converted with respect to one of the arms of the motion device, and each feature point in the image is calibrated according to the position of the measurement coordinate system under the measurement coordinate system of the image capturing probe and corresponding to the world according to each arm. The position of the world coordinate system under the coordinate system determines the conversion relationship between a measurement of each calibration point and the world coordinate, and the conversion relationship between the arm calibration of each calibration point and the conversion relationship between the measurement and the world coordinate determine the movement device Compensation information for any position and posture in the corrected space.

Hereinafter, various exemplary embodiments may be more fully described with reference to the accompanying drawings, and some exemplary embodiments are shown in the accompanying drawings. However, the inventive concept may be embodied in many different forms, and should not be construed as being limited to the exemplary embodiments set forth herein. To be precise, the provision of these exemplary embodiments makes the present invention detailed and complete, and will fully convey the scope of the concept of the present invention to those skilled in the art. Similar numbers always indicate similar components. The position and posture correction device and the position and posture correction system and method thereof will be described below with various embodiments in conjunction with drawings. However, the following embodiments are not intended to limit the present invention.

Please refer to FIG. 1, which is a three-dimensional schematic diagram of a position and attitude correction device of the present invention. The correction device 2 includes a plurality of correction units 20, which are combined with each other so that the plurality of correction units 20 form a correction space 90. Used to calibrate a sports device. Each calibration unit 20 includes a plate body 200, a plurality of supporting structures 201 and a plurality of calibration bodies 202. One end of the plurality of supporting structures 201 is connected to the board 200. In this embodiment, the number of the support structure 201 is four, but it is not limited thereto, and the number can be determined according to actual calibration requirements. The plurality of calibration bodies 202 are respectively arranged on the plurality of supporting structures 201, and each calibration body 202 corresponds to a supporting structure 201, and is connected to the corresponding supporting structure 201, so that the characteristics of the at least four calibration bodies 202 The points are not coplanar. There is no limit to one correction body on each support structure. The correction body 202 in this embodiment is a spatially symmetrical structure, such as a sphere or other symmetrical geometric structure with a centroid/gravity center as the center of symmetry.

Each calibration body has at least one characteristic point. In an embodiment, the characteristic point may be the center position of the calibration body, or a specific mark on the calibration body. In this embodiment, by virtue of the height difference of the supporting structure 201, at least four characteristic points are not coplanar. Since three non-collinear points can create a plane, and four non-coplanar points can create a three-dimensional space, in order to later combine the correction group, no matter how many correction units are used, the three-dimensional spatial coordinates can be established. For reference, at least four non-coplanar feature points are required on the correction unit to establish a reference three-dimensional space. At the same time, because the correction device may use a large number of correction units at the same time, in order to simplify the correction device, the correction unit is set on the basis of four characteristic points that can be used to establish a minimum of three-dimensional points. In addition, in another embodiment, the size of the calibration body 202 may be the same or different.

It should be noted that, in order to prevent the correction unit 20 from affecting the size of the correction unit 20 due to thermal expansion and contraction effects, the correction unit 20 can be made of materials with a low coefficient of expansion. On the other hand, it can also be considered whether the material is easy to process. characteristic. In one embodiment, the plate body 200 and the supporting structure 201 can be made of Invar 36, which has the characteristics of low expansion coefficient and easy processing. When Invar alloy is at 20℃, its expansion coefficient is 1.2~2.0 x 10 ^-6 /K. The exact value varies slightly depending on the grade of the alloy itself. The use of such a low expansion coefficient alloy can ensure that when the correction unit 20 is subjected to a certain external temperature change, its own expansion deformation can be reduced to a level that does not affect the result of measuring the spatial coordinate position of the sports device.

…(1)According to the Gong's coefficient of thermal expansion, as shown in the following formula (1):

…(1)

Among them, α is the thermal expansion coefficient, L is the length of the material, ΔL is the length increased after the temperature change, and ΔT is the temperature change. If the Invar alloy is used in a calibration unit with a length (width) of 100 mm, the temperature change is 10 degrees, and its linear thermal expansion is 2 μm. That is, when the calibration unit is at 10℃~30℃, the linear expansion change is only 2 μm at most. This change is much lower than the measurement accuracy of the probe, so it will not have too much influence on the measurement result.

As for the connecting elements 203 between the calibration units 20, they will be selected to ensure that after the calibration units 20 are connected, their mutual distance will not be greatly changed due to external factors, such as temperature, etc., which will seriously affect the accuracy of the calibration device. material. Therefore, in an embodiment, the connecting element 203 between the calibration units, such as the connecting rod, may also be made of Invar alloy.

It should be noted that the shape of the correction space 90 combined by a plurality of correction units 20 is not limited, and the user can use the correction unit 20 to combine according to the needs of the correction area. Before assembling the calibration device 2, a three-dimensional measurement is used to measure each calibration unit 20 to obtain the accurate feature point coordinate information of the calibration body 202, and a code is set to represent the position of each calibration body 202. At the same time, in order to facilitate the discrimination, a code number is also set for each calibration unit 20, so that the information of any calibration body 202 of any calibration unit 20 can be known only by knowing the code. After combining multiple correction units 20, by pre-setting the sequence of the correction units 20 and the position of the correction body 202, the code can be used to establish the feature point position model of all the correction bodies 202 in the entire correction device 2 to construct a correction Group space. As shown in FIGS. 2A and 2B, it is a schematic top view of the calibration device 2 by combining a plurality of calibration units 20. 2A and 2B show that a plurality of calibration units are connected, and a calibration group space that meets the measurement requirements is combined according to the ranges required for different measurements.

In addition, as shown in FIGS. 2C and 2D, in this embodiment, the correction device 2a or 2b is composed of a plurality of support structures 201 and a correction body 202. The difference between Fig. 2C and Fig. 2D and the aforementioned Figs. 1 or 2A~2B is that Fig. 2C or Fig. 2D is not formed by stitching multiple correction units, but a single correction formed by a combination of multiple support structures 201 and correction bodies 202 Device. The difference between FIG. 2C and FIG. 2D is that the geometric shape of the correction device is not the same. In addition, it should be noted that the correction body 202 in this embodiment is a spatially symmetrical structure, such as a sphere or other geometrical structures with symmetry with the centroid/gravity center as the center of symmetry. In the following embodiments, the characteristic points of the calibration body are taken as an example of the center position of the calibration body. In addition, the calibration body 202 may further have recognizable features, such as patterns, characters, etc., but it is not limited thereto.

Please refer to FIG. 3A, which is a schematic diagram of an embodiment of the calibration system of the present invention. In this embodiment, the calibration system 3 is constituted by the calibration device 2 shown in FIG. 1, the motion device 30, the image capturing probe 31, and the arithmetic processing device 32. In an embodiment, the movement device 30 may be a mechanical arm, or a mechanism that controls the three-dimensional position and posture of a tool in a machine tool, and can perform spatial movement including multi-axis movement or rotation. The motion device 30 in this embodiment is a robotic arm, and the motion device in the following embodiments is described as a robotic arm. The computing processing device 32 can be a computer, a notebook computer, a workstation, a server, or a cloud computing server. The arithmetic processing device 32 has a storage device, and a database of the position information of the calibration device 2 is stored in the storage device. In one embodiment, the database can be set in a remote cloud storage device via the network.

In one embodiment, the position information includes the number of each calibration unit 20, the center coordinate information of each calibration body 202 on each calibration unit 20, and a code is assigned to it to represent the position of each calibration body 202. For example, in Figure 3A, each calibration unit has a number A~L, and each calibration body 202 on it also has a corresponding number. Taking the calibration unit 20 with number A as an example, A1~A6 represent each calibration unit above it. For a calibration body 202, the center position of each calibration body 202 can be obtained through prior detection. For the measurement method, in one embodiment, as mentioned above, the center position of the calibration body can be obtained by measuring on a calibrated Coordinate Measuring Machine (CMM). The coordinate information of. Since the serial number of each calibration unit 20 has been measured in advance, after the calibration device 2 formed by the combination of a plurality of calibration units 20, the position of the calibration device 2 with respect to the three-dimensional space coordinates of the world coordinate system (W) can be established relationship. In this embodiment, the world coordinate system (W) is a coordinate system represented by a correction space constructed by a plurality of correction units.

The movement device 30 has a base 301, and a plurality of mutually related movement shafts 306 are coupled from the base 301, so that the movement device 30 can change one end effect on the last movement axis through multi-axis movement. The position and posture of the end effector 302. In this embodiment, the end effector 302 is used to set the image capturing probe 31. The end effector can be provided with processing tools, measuring instruments, image capturing probes, etc., but it is not limited thereto. In another embodiment, as shown in FIG. 3B, the correction device 2 may not be arranged in a manner similar to that shown in FIG. 3A. The correction device 2 may be arranged in a displacement that can perform at least three-dimensional movement (XYZ) after spatial displacement accuracy correction. On the mounting device 7 to allow the calibration device 2 to move in space. Through the method of FIG. 3B, the installation space of the calibration device can be reduced. The transfer device 7 drives the calibration device 2 to move to ensure that the image capturing probe 31 can detect the calibration device 2 at each operating position, thereby increasing the scope of the calibration space. Since the movement dimensions of the transfer device of the present invention are orthogonal to each other, the problem of superimposing movement errors can be avoided. Therefore, as long as the spatial displacement accuracy of the transfer device is significantly higher than that of the motion device, it can be more precise Correct the actual position of a specific part of the exercise device, such as the end effector.

Returning to FIG. 3A, the image capturing probe 31 has a light source for generating structured light, such as a light field with a specific stripe pattern, and an image capturing device for capturing images to capture motion The state of the plurality of correction units 20 as seen in the position and posture of the device 30, and then at least one corresponding image is captured. It should be noted that the position where the image capturing probe 31 is installed is the position to be tracked and compensated on the motion device. For example, if the moving device is a mechanical arm, the clamping structure of the cutting tool or processing tool at the end of the mechanical arm is the position where the image capturing probe 31 is installed. Therefore, the position of the image capturing probe 31 is determined according to requirements, and there is no certain limit.

In addition, there is no limit to the number of captured images here, and it can be based on how the objects in the images can be analyzed later relative to the measurement coordinate system (the spatial coordinate system with the image capturing lens as the origin). To determine the number of images, for example, if the Four-step Phase Shift Method is used, four images with different phases are captured, the Five-Step Phase Shift method is used to capture five images with different phases, or a single The single exposure or one shot phase shift law captures a single image that has different phases at the same time. No matter which phase shift method is used, it is well known to those skilled in the art and will not be repeated here. In one embodiment, the image capturing probe 31 projects structured light, and then through the phase change, the image or point cloud corresponding to the structured light corresponding to the phase change is captured.

Please refer to FIG. 3C and FIG. 3D, which are schematic diagrams of different embodiments of the image capturing probe of the present invention. In FIG. 3C, the image capturing probe includes a light source 310 and an image capturing device 311. The light source 310 is used to project structured light, which can be coded structured light, random coded spot, or sine wave. Pattern (sinusoidal pattern) and so on. In another embodiment, as shown in FIG. 3D, there are two image capturing devices 312 in this embodiment, and the surface of the calibration body 20 may be pre-coated with a spot pattern 313, such as a random number of spots. Or scattered images of light spots. The center line between the two image capturing devices 312 is used as the baseline 314, which can be used to calculate the distance between the calibration body and the baseline 314 to obtain the position of the calibration body 20 in the coordinate system of the image capturing device 311.

It should be noted that although the foregoing embodiment is the end effector 302 where the image capturing probe 31 is set on the end structure of the motion device 30, the correction device 2 is fixed at the appropriate position of the motion device 30 where the spatial correction is required. , Which can meet the required working correction range of the covering motion device shall prevail. In another embodiment, the opposite can also be set, that is, the calibration device setting 2 is set on the end effector 302 of the motion device 30, and the image capturing probe setting 31 is set at an appropriate position that can effectively detect the calibration device. , Generally can be fixed or set on the transfer device 7 of Fig. 3B to track the position of the correction device at any time, so that the image capturing probe can be calibrated to detect the spatial position and posture of the correction device to facilitate satisfaction Covers the correction range of the sports device.

Please refer to FIG. 4A, which is a schematic flowchart of an embodiment of the position and attitude correction method of the present invention. In this embodiment, the method 4 includes the following steps: first, step 40 is performed to provide a calibration device, which includes at least one supporting structure and a plurality of calibration bodies arranged on the at least one supporting structure, the plurality of calibration bodies There are at least four characteristic points that are not coplanar. The calibration device may have the structure shown in FIGS. 1 and 2A to 2D. This embodiment uses the calibration device shown in FIG. 1 for illustration, and the formed calibration system 3 has the structure shown in FIG. 3A.

Then proceed to step 41 to establish the center position of each calibration body 202 with respect to a world coordinate system. In an embodiment of this step, the probe of the three-dimensional measuring bed can be used to precisely measure the standard body on each calibration unit 202 (which may include contact probe measurement or non-contact optical measurement), and pass three times The built-in program of the Yuanliang bed obtains the coordinate position of the center of the calibration body 202. Taking the calibration body 202 as a sphere as an example, the position of the center of the sphere of the calibration body 202 is obtained. After several measurements, the coordinate position is averaged, and the average value is recorded and stored in the database. Therefore, the center position of each calibration unit 20 and the calibration body 202 on it will be recorded in the database. The world coordinate system is the world coordinate system, that is, the space coordinate system where the motion device 30 and the correction device are located.

…(2) 以圖3A為例，三次元量床從右上角的校正單元A開始量測，此時編號n為1，視為原點校正單元，存入資料庫D₁ 。以順時針的順序量測，對下一個校正單元進行量測後，編號n為2存入資料庫D₂ 。以此類推，直到量測完所有的校正單元，並對所有校正單元完成編號。每一個位置資訊D_i 代表著世界座標系(也就是世界座標系W)下每一個校正體的中心位置資訊。The form of the calibration volume database established in step 41 can be expressed as the following formula (2):

…(2) Taking Fig. 3A as an example, the three-dimensional measuring bed starts measurement from the correction unit A in the upper right corner, and the number n is 1, which is regarded as the origin correction unit and stored in the database D ₁ . Measure in a clockwise order, and after the next calibration unit is measured, the number n is 2 and it is stored in the database D ₂ . And so on, until all calibration units have been measured, and all calibration units have been numbered. Each position information _Di represents the center position information of each calibration body in the world coordinate system (that is, the world coordinate system W).

Then, step 42 is performed to define a plurality of correction points TP in a motion space 91 of the motion device, as shown in FIG. 4B. The density of the correction points TP may be determined according to the accuracy required by the motion device in the movement. The higher the density of the correction points TP, the better the effect of subsequent movement position compensation control. In this embodiment, the motion space 91 is a space of a three-dimensional cube, but it is not limited thereto. For example, the space of a sphere can also be implemented. Each calibration point TP is the position that the end effector will reach in the future.

Next, proceed to step 43 to provide an image capturing probe 31 to be set on the exercise device 30 capable of multi-axial dimensional motion. The moving device 30, as shown in FIG. 3A, has a plurality of moving shafts 306 connected to each other. As shown in FIG. 3A, the final motion axis 306 has an end effector 302, so the image capturing probe 31 can be set on the end effector 302. The so-called end effector refers to the end of the movement shaft 306 at the very end of the movement device, which is used to install various processing devices, detection tools, or holding tools for different purposes. In this embodiment, each movement shaft 306 has at least one degree of freedom of movement, such as rotation in different axial directions. In an embodiment, the image capturing probe 31 may be of the structure of FIG. 3C to project fringe structured light and perform phase shift modulation on the fringe structure of the structured light. In addition, the image capturing probe can also be structured as shown in FIG. 3D. Taking the architecture of FIG. 3C as an example, the image capturing probe 31 includes an image capturing device 311, which can capture one or more images and provide the images required by the phase shift calculation.

，以及運動裝置的末端效應器的位置與姿態

。其中x,y,z代表三維座標系中的座標位置，而α,β與γ則代表末端效應器的三軸位置與姿態角度資訊。此公稱位置E_nominal 是運動裝置控制系統本身認知末端效應器302理論上要移動到的位置。然而，在現實上，運動裝置30因為安裝的環境或者是本身的負載，亦或者是因為運動裝置長時間使用的磨損，實際上運動裝置將末端效應器302移動的位置和校正點之間一定會存在有誤差。如何找到這個誤差，並將其補償，使得末端效應器可以和公稱位置E_nominal (理論位置)的誤差達到最小，以下進行進一步說明。After step 43, proceed to step 44 to control the movement device 30 to move the end effector to the first correction point with an arm correction position and posture, such as TP (1) in FIG. 4B. After the end effector moves to the TP(1), proceed to step 45 to enable the image capturing probe 31 to capture the arm calibration image of the calibration device corresponding to the current arm calibration position and posture, and each arm calibration image There are at least six characteristic points of the calibration body. It should be noted that in this step, for each calibration point TP, when the end effector moves to the calibration point TP, the motion device 30 itself will have a _{nominal position E nominal corresponding to the} calibration point TP, which includes each The DH parameters of the motion axis and the position and posture of the end effector, for example: the rotation parameters and translation parameters of each motion axis 306 in different directions

, And the position and posture of the end effector of the motion device

. Among them, x, y, z represent the coordinate position in the three-dimensional coordinate system, and α, β and γ represent the three-axis position and attitude angle information of the end effector. _{The nominal} position E nominal is the position to which the motion device control system recognizes the end effector 302 theoretically. However, in reality, due to the installation environment or the load of the exercise device 30, or because of the wear and tear of the exercise device for a long time, in fact, there must be a gap between the position where the exercise device moves the end effector 302 and the calibration point. There are errors. How to find this error and compensate it so that the end effector can _{minimize the error from the nominal} position E nominal (theoretical position) is described further below.

 ……………(3)

……..( 4) 其中，本實施例以六軸的運動裝置來說明，0代表運動裝置的基座、1代表與基座相耦接的第一軸、2代表與第一軸相耦接的第二軸，然後以此類推，6代表與第五軸相接的末端效應器。式(3)表示運動裝置從基座到末端效應器之間的轉換關係，式(4)則代表第n-1運動軸到第n運動軸的轉換關係。任兩軸之間的轉換關係T可以由轉動參數與平移參數

來表示。其中θ_n 代表相鄰運動軸間的轉動角度變化，而d_n 、α_n 與γ_n 則代表相鄰運動軸間的幾何關係，其中d_n 與γ_n 為平移參數，α_n 是旋轉參數，此三個參數為定值，不會隨著各種位置與姿態而有變化。因此運動裝置移動至某一校正點TP後，讀取運動裝置內部的控制器儲存對應該運動位置與姿態的公稱位置E_nominal 所具有的各軸DH參數，再利用正向運動學，以及式(3)、(4)，可以計算出當前位置與姿態{E}(末端效應器，第6軸)相對於{B}(基座，第0軸)的手臂校正轉換關係(^E T_B )，或稱為L_i 。Next, step 46 is performed to obtain the arm correction conversion relationship of the end effector at the current correction point TP(1) according to the arm correction position and posture currently located at the correction point TP(1). Specifically, this step is to calculate the arm correction conversion relationship between the end effector 302 and a specific position on the motion device 30 according to the position and posture of the corresponding correction point and the DH parameter of each motion axis. . In this step, the specific position is the base 301 of the motion device, and the arm correction conversion relationship can be shown in the following equations (3) and (4):

……………(3)

……..( 4) Among them, this embodiment is described with a six-axis motion device, 0 represents the base of the motion device, 1 represents the first axis coupled to the base, 2 represents the second axis coupled to the first axis, and then By analogy, 6 represents the end effector connected to the fifth axis. Equation (3) represents the conversion relationship between the motion device from the base to the end effector, and Equation (4) represents the conversion relationship from the n-1th axis of motion to the nth axis of motion. The conversion relationship T between any two axes can be determined by the rotation parameter and the translation parameter

To represent. Wherein the rotation angle θ _n between the adjacent representative of changes in movement of the shaft, and d _{n, α n} and γ _n represents the geometric relationship between the movement of the shaft adjacent to, and where d _n γ _n translation parameter, α _n is a rotation parameter, These three parameters are fixed values and will not change with various positions and attitudes. Therefore, after the motion device moves to a certain calibration point TP, the controller inside the motion device stores the _{DH parameters of each axis of the nominal} position E nominal corresponding to the motion position and posture, and then uses the forward kinematics, and the formula ( 3), (4), can calculate the current position and posture {E} (end effector, 6th axis) relative to {B} (base, 0th axis) arm correction conversion relationship ( ^E T _B ), Or called _Li .

Then proceed to step 47, according to the center position of each calibration body in the arm calibration image obtained in step 45 relative to the calibration body in the measurement coordinate system of the image capturing probe and the calibration body in the world coordinate system corresponding to the calibration body The position of the center determines the conversion relationship between the measurement of the current calibration point and the world coordinate. In this step, first, according to the position and posture of the moving device under the corresponding current calibration point, the arm calibration image captured by the image capturing probe 31 is analyzed to parse out at least six calibration bodies 202 in the image. The center position of a measurement coordinate system. The measurement coordinate system, in one embodiment, is a coordinate system established with the lens of the image capturing probe 31 as the origin. In this step, the phase shift method can be used to analyze each calibration body in the arm calibration image to obtain the positional relationship between the center positions of at least six calibration bodies 202 in the image relative to the measurement coordinate system established by the image capturing probe 31 . Since the calibration unit 20 has a number, each calibration body can be identified through step 46, and the center position of the measurement coordinate system can be established for each calibration body 202.

…(5)Then, according to the position of each calibration body in the arm calibration image relative to the center of the calibration body in the measurement coordinate system of the image capture probe and the position corresponding to the center of the calibration body in the world coordinate system, the amount under the current calibration point is determined Measure the conversion relationship with world coordinates. Hereinafter, the calculation method of the conversion relationship is further explained. The spatial distribution point cloud of each calibration body 202 analyzed in the aforementioned measurement image is used to calculate its center position, and the measured calibration unit 20 is recorded at the same time. The number m. The position information obtained after the measurement of each calibration body 202 can be expressed as the following formula (5):

…(5)

Thus the space in the correcting means 2, a reference spatial coordinates through D _i Coordinate Measuring Machine measurement correcting unit 20, the scanning unit also measured by the correction amount obtained spatial coordinates measured 20 M _i. At this time, when the value of the number m and n are the same, so that M _i D _i and pair with each other, so that _{(x Di, y Di, z} Di) and _{(x Mi, y Mi, z} Mi) are representative of the real space The same ball center position. Find the center of the sphere representing the same location D _i and M _i, can successfully transform matrix calculated two different coordinate systems. When it comes to actual measurement, applying this conversion matrix can convert the measured information into position information in the world coordinate system. The advantage of this method is that the method is simple, the calibration unit can use the same style, only need to know the number to be able to identify, but after the number is completed, the number can not be changed. Once the combination of the calibration unit in the calibration device needs to be changed, the measurement and numbering of the calibration target must be redone.

In another embodiment of step 47, the characteristic method may be used to establish the measurement spatial coordinates M _{i of the} calibration volume. Compared with the numbered correction method, the characteristic correction method mainly uses the characteristics of the correction unit 20 itself for data identification. In this embodiment, when the calibration body 202 is designed, the style of each calibration body 202 has different characteristics. For example, there may be spots with different density distributions or specific patterns or marks on the sphere. In this case, the image capturing probe can adopt the architecture shown in FIG. 3D to measure the spatial distribution of the center position of the calibration body 202. At this time, the number of calibration bodies 202 is not limited, and different spatial distributions composed of 5 and 6 may be used to establish different features, for example, the spatial distribution pattern of 5 calibration bodies as the feature. In the measurement phase, the spatial distribution of the measured center position is compared with each feature in the database, and the average distance of the point cloud is used to find the feature with the smallest difference. Then the feature is the current measured feature The center of the paired information. The advantage of this method is that no serial number is required, and its own characteristics can be regarded as a serial number (actually no serial number). But on the contrary, because the measured information needs to be compared with each calibration unit in the database, the amount of calculation is more than the numbered calibration method. Moreover, since each calibration body 202 must have a different style and have recognizable features, the design cost will increase as the number of calibration units 20 increases, which is not conducive to the situation where there are too many calibration units 20.

Due to the measurement phase of the previous calibration device, the spatial position of each calibration unit 20 and its calibration body 202 measured by the three-dimensional measuring bed in the world coordinate system will be recorded. In order to be able to identify the spatial position where the calibration body 202 is measured, it is necessary to compare specific information with the spatial coordinate information in the database to obtain the conversion relationship between the measurement and the world coordinate. Step 47 adopts the numbering correction method, which only stores the center position information of the calibration unit and the calibration body in the database in step 41, and the calibration unit is numbered, and this number is used to compare the center position information obtained in step 47. Calculus.

在步驟47的一實施例中，取得手臂校正影像中，相對於該影像擷取探頭之量測座標系下校正體中心的位置，其量測座標系為量測座標系{S}以及將在步驟41時，校正單元20以三次元量測所建立關於世界座標系，亦即世界座標系{W}的世界座標系下校正體中心的位置資訊與量測座標系{S}的量測座標系下校正體中心的位置資訊做比對，即能計算出當前校正點所具有的兩者的量測與世界座標轉換關係。從量測座標系{S}到世界座標系{W}的量測與世界座標轉換關係可表示為4乘4的齊次座標轉換矩陣，可以表示為下式(6)： …(6)

或者簡化表示為下式(7)： …(7) 其中R可以表示為下式(8)，是滾動(Roll）與俯仰角 (Pitch)以及偏轉角（Yaw）等三個物件位置與姿態參數之轉換矩陣的組合，稱之為旋轉矩陣。

…(8) 而矩陣P可以表示為下式(9)所示，其代表為平移向量。

…(9) 校正的目的就是在於解出這個包含12個未知項的轉換矩陣^W T_S 。由於在旋轉矩陣R之中，每一行的三個參數互有關連，為非獨立參數，所以實際上僅需3組座標即可算出旋轉矩陣。因此量測與世界座標轉換關係^W T_S 最少僅需6組座標即可解出。

In an embodiment of step 47, in obtaining the arm calibration image, relative to the position of the center of the calibration body under the measurement coordinate system of the image capturing probe, the measurement coordinate system is the measurement coordinate system {S} and will be in step At 41 o'clock, the calibration unit 20 uses the three-dimensional measurement to establish the world coordinate system, that is, the position information of the center of the calibration body under the world coordinate system of the world coordinate system {W} and the measurement coordinate system of the measurement coordinate system {S} is calibrated. By comparing the position information of the center of the body, the conversion relationship between the two measurements and the world coordinate of the current calibration point can be calculated. The conversion relationship between measurement and world coordinates from the measurement coordinate system {S} to the world coordinate system {W} can be expressed as a 4-by-4 homogeneous coordinate transformation matrix, which can be expressed as the following formula (6): …(6)

Or simply expressed as the following equation (7): …(7) where R can be expressed as the following equation (8), which is the position and attitude parameters of three objects such as Roll, Pitch, and Yaw The combination of the conversion matrix is called the rotation matrix.

…(8) The matrix P can be expressed as the following equation (9), which is represented by the translation vector.

…(9) The purpose of the correction is to solve this conversion matrix ^W T _S which contains 12 unknown items. In the rotation matrix R, the three parameters in each row are related to each other and are non-independent parameters, so in fact, only 3 sets of coordinates are needed to calculate the rotation matrix. Therefore, the conversion relationship between the measurement and the world coordinate ^W T _S can be solved with at least 6 sets of coordinates.

…(10) 而在世界座標系{W}中的球形目標的球心位置為下式(11)所式：

…(11) 此時兩座標系下的空間位置C_si 與C_Wi 均代表真實空間中同一個球心位置。因此，這兩個空間位置的關係可表示為如下式(12)所示：

…(12) 將式C_Si 和C_Wi 代入上式，展開得到以下的方程式(13)組合：

…(13) 將C_si 與C_Wi 代入其中即可算出校正用的旋轉矩陣中的12項參數。利用至少6組以上的C_si 與C_Wi 即可解出轉換矩陣的未知參數，而得到關於運動裝置的量測與世界座標轉換關係^S T_{W, measured} (P_i )。也就是說，量測與世界座標轉換關係^S T_{W, measured} (P_i )是通過將運動裝置在當前校正點所具有的位置與姿態下所偵測到的每一個校正體的位置C_Si 與資料庫中對應校正體的位置C_Wi , 進行比較而計算出來的。The position of the center of the sphere of the spherical target in the measurement coordinate system {S} is given by the following formula (10):

…(10) And the position of the center of the sphere of the spherical target in the world coordinate system {W} is given by the following formula (11):

…(11) At this time, the spatial positions C _si and C _Wi in the two coordinate systems both represent the same sphere center position in real space. Therefore, the relationship between these two spatial positions can be expressed as the following formula (12):

…(12) _{Substituting formulas C Si} and C _Wi into the above formula, expand to obtain the following combination of formula (13):

…(13) By _{substituting C si} and C _Wi into them, the 12 parameters in the rotation matrix for correction can be calculated. At least 6 groups of C _si and C _Wi can be used to solve the unknown parameters of the conversion matrix, and the conversion relationship between the measurement of the motion device and the world coordinate ^S T _{W, measured} (P _i ) can be obtained. That is to say, the conversion relationship between measurement and world coordinates ^S T _{W, measured} (P _{i ) is the position C Si} of each calibration body detected by the motion device under the position and posture of the current calibration point. _{The position C Wi} of the corresponding calibration body in the database is calculated by comparison.

After step 47 obtains the arm calibration conversion relationship and the measurement and world coordinate conversion relationship under the current calibration point, the transformation step 47a confirms whether all the calibration points have been completed. If not, step 48 controls the motion device to move the end effector to the next correction point with another arm to correct the position and posture. Continuing the previous example, when the calibration point TP(1) is completed, change to another calibration point TP(2) through step 48, and then go back to step 45 and repeat steps 45 to 47 until the calculation The arm calibration conversion relationship and the conversion relationship between measurement and world coordinates corresponding to all calibration points TP. It should be noted that when changing the position and posture, it must be able to cover the range of the rotation angle of each motion axis. For example, if the range of the motion axis rotation angle is 180 degrees, if a total of 10 correction points are taken, then each time for the axis The angle can be changed by 18 degrees. The same is true for other motion axes.

After completing the arm calibration conversion relationship and the measurement-world coordinate conversion relationship corresponding to all the calibration points TP, proceed to step 49, according to the arm calibration conversion relationship and the measurement-world coordinate conversion relationship corresponding to each calibration point TP , Determine a position and posture compensation information corresponding to each calibration point. In step 49, as shown in FIG. 4C, it is a schematic flowchart of an embodiment of determining position and attitude compensation information according to the present invention. In this embodiment, the step 490 is further included to calculate the hand-eye conversion relationship on the end effector according to the arm correction conversion relationship corresponding to each arm correction position and posture and the measurement and world coordinate conversion relationship. In step 490, using the aforementioned different positions and postures corresponding to multiple different correction points, each position and posture will correspond to (P _i ) and (L _i ) to perform hand-eye correction. As shown in Fig. 5A, it represents the conversion relationship between _{(P i} ) and (L _{i ).} In FIG. 5A, the motion shaft 306 represents the last section of the motion device, which has an end effector, and the motion shaft 306b is coupled to the base B, and the motion shaft 306a is coupled to the motion shaft 306 and the motion shaft 306b, respectively. together. There is a conversion relationship between joints coupled with adjacent motion axes, which is shown in the aforementioned equation (4).

並使用最小平方法計算出X，此未知項X即為手眼校正中所欲求得之手眼轉換關係，亦即量測座標系{S}與機械手臂安裝該影像擷取探頭之末端座標系{E}之間的手眼轉換關係^S T_E 。For each position and posture, for example _{, the two positions and postures formed by (L 1} , P ₁ ) and (L ₂ , P ₂ ) in FIG. 5A have a hand-eye conversion relationship X between them. Based on the above concepts and calculation methods in accordance with AX = XB hand-eye correction, wherein A of the present embodiment of the embodiment of L _i, in the embodiment B was P _i of the present embodiment, a plurality of calibration points using a plurality of sets of the obtained P _i and L _i, the probe may be calculated to the end of the arm {S} {E} hand-eye conversion relationship. Theoretically, the more accurate the more sets there are P _i, L _i, the calculation result after correction. In Fig. 5A, X is the conversion relationship from the probe {S} to the end of the arm {E}, and L, P, and X are all 4-by-4 matrices representing the conversion of position and posture. Since the L and P of each calibration point TP are both It is known that the following formula can be used:

And use the least square method to calculate X. This unknown item X is the hand-eye conversion relationship desired in the hand-eye calibration, that is, the measurement coordinate system {S} and the end coordinate system of the image capture probe installed by the robotic arm {E } The hand-eye conversion relationship ^S T _E.

^{After obtaining the hand-eye conversion relationship, proceed to step 491 to calculate the coordinate conversion relationship B} T of the specific position on the motion device relative to the world coordinate system according to the arm correction conversion relationship, the hand-eye conversion relationship, and the measurement and world coordinate conversion relationship. _W. In this embodiment, the specific position is the base of the motion device. In this step, the hand-eye conversion relationship ^E T _S (X) and the arm calibration conversion relationship ( ^E T _B ), or the conversion relationship between Li and measurement and world coordinates ^S T _{W, measured} (Pi), are mainly used first to determine ^{The conversion relationship B} T _W of the specific position on the motion device relative to the world coordinate system (hereinafter referred to as Z). Because the specific position is the position of the base in this embodiment, ^B T _W is the conversion relationship between the base and the world coordinate, that is, the conversion relationship Z is obtained using the following equation (14). (Li)X=Z(Pi)………….(14)

As shown in FIG. 5B, the figure is a schematic diagram showing the conversion relationship between the base and the world coordinate Z and the arm correction conversion relationship, the hand-eye conversion relationship and the measurement and the world coordinate conversion relationship. In FIG. 5B, also because the positions of the calibration bodies of the calibration device 20 are fixed in the first coordinate system, the positions of the calibration bodies on _{the calibration device obtained through Li} , X, and P _i will be the same as those of the motion device base. The position and posture of each correction body obtained through the conversion relationship Z are the same. Based on this concept, the above equation (14) can be derived. Because L _i, X and P _i are known, the base matrix can be determined with the World coordinate conversion relationship Z.

…………(15)     Then step 492 is performed to calculate a nominal position conversion relationship ^S of each correction point according to the hand-eye conversion relationship (X), conversion relationship (Z), and the arm correction conversion relationship ^E T _B (L _{i) of each correction point} T _{W,model is} shown in the following formula (15):

…………(15)

^S T _W,model represented by equation (15) is the nominal position conversion relationship obtained by the motion device system itself for each position and posture. Finally, proceed to step 493. According to ^{the difference between S} T _{W,model of} equation (15) and the aforementioned measurement and world coordinate conversion relationship ^S T _W,measured , the actual position and attitude reached and the position and position that should be reached can be calculated. The gap between the attitudes ∆T=B-ZAX, as shown in Figure 5C. Taking the motion device 30 as an example of a robotic arm, when the clamping member 300 on the motion device 30 moves from the position R1 in FIG. 3A to the position R2 in FIG. The R2 position is the R2 position in the world coordinate system. Therefore, when the gripper 300 moves to the R2 position under the control of the arithmetic processing device 32, the actual R2' position that the gripper 300 moves to and the R2 recognized by the arithmetic processing device There will be errors between the positions due to the mechanism or the driving device. The system calibration model can be established through the aforementioned conversion relationship, and then this error can be found to facilitate the determination of the compensation amount of its position and posture.

It is noted that the foregoing relationship is calculated hand-eye conversion of X and P _i L _i is the respective correction points shown in FIG. 4B TP corresponding to P _i L _i is calculated. Not actually to correct point TP of L _i and P _i as limiting. In another embodiment, other points different from the correction points can be used to obtain the _Li and P _{i of} these points. For example, as shown in FIG. 4D, step 490a is first performed to control the motion device to correct the position and posture with one hand and eye Move to the one-handed eye correction position. Then, step 490b is performed to enable the image capturing probe to capture the hand-eye correction image of the correction device under the hand-eye correction position and posture, and each hand-eye correction image has at least six correction body feature points. The aforementioned hand-eye correction position may not be the same as the correction point TP at all, or may be partially overlapped. Then step 490c is performed to determine the arm correction conversion relationship of the end effector relative to the motion device according to the current hand-eye correction position and posture of the motion device, that is, corresponding to _Li .

Then in step 490d, according to the current hand-eye calibration position, the position of each calibration body in the corresponding hand-eye calibration image relative to the center of the calibration body in the measurement coordinate system of the image capturing probe and the calibration body corresponding to the world coordinate system The position of the center determines the conversion relationship between the measurement of the current hand-eye correction position and the world coordinate, which corresponds to P _i . Since the hand-eye conversion relationship X requires at least six sets of arm correction conversion relationships and measurement and world coordinate conversion relationships corresponding to at least six hand-eye correction positions to be solved, step 490e is used to determine whether to obtain at least six hand-eye correction positions Corresponding at least six sets of arm correction conversion relations and measurement and world coordinate conversion relations, if not, go to step 490f to control the motion device to change the hand-eye correction position and posture to another hand-eye correction position, and then repeat steps 490b~490e , Until obtaining at least six hand-eye correction positions of the arm correction conversion relationship and the measurement and world coordinate conversion relationship.

Thereafter, in step 490g The arm then corrected conversion relationship L _i and the measurement of the relationship between the world coordinate transformation P _i, and calculates the hand-eye on the end effector of the conversion relation X. And then to step 490h conversion relation based on the correction arm L _i, the hand-eye relationship X converting means calculates the movement of the base and the world coordinate conversion relation to the measurements and the relationship between the world coordinate transformation P _i Z. Finally, in step 490i L _i, the base and the Z world coordinate conversion relationship and hand-eye relationship X conversion system calculation based on the conversion relation arm out of a well-corrected for each position of the hand-eye correction of the position of said conversion relation ^S T _{W, model.} Through steps 490a to 495i, it is also possible to calculate the nominal position conversion relationship ^S T _W,model equivalent to that calculated in step 492.

則代表4個DH參數誤差

。

(16)

(17)

(18) 如圖5C所示，其表示每一個位置與姿態下，末端效應器的公稱位置E_nominal 與實際到達的位置之間誤差ΔT_i 。而在方程式(17)中，J₁ ~J_n 分別是一個ΔT_i 的雅可比矩陣，以基座加上六個運動軸來說，相鄰基座與運動軸，以及相鄰運動軸之間總共會有六個連接關節。因此對於每一個ΔT_i ，表示成[J_i1 , J_i2 , J_i3 , J_i4 , J_i5 , J_i6 ]*[ Δd, Δθ, Δα, Δγ]^Τ ，其表示每一個校正點TP上，每一個相鄰接運動軸的關節之間都會有對應的位置與姿態補償資訊[ Δd, Δθ, Δα, Δγ]。利用n個ΔT_i 計算出

後，每一個對應ΔT_i 的

為運動裝置相鄰接運動軸的關節所具有的位置與姿態補償資訊，如方程式(19)所示。補償原先的公稱DH參數，則式(4)變化為式(19)，式(3)變化為式(20)。利用已校正之^E T_B,c 即可令機械手臂移動至接近公稱位置與姿態的校正後位置與姿態。

(19)

(20) Finally, returning to FIG. 4C, step 493 determines the position and attitude compensation information according to the conversion relationship between the nominal position and the conversion relationship between the measurement and the world coordinate ^S T _{W, measured.} In this step, the position and attitude error of the end effector position and direction of the motion device can be expressed by the first derivative function of equation (4), as in equation (16), n ΔT calculated by n sets of positions and attitudes _{If i is} expressed in matrix form, then equation (17) can be obtained, and if it is simplified, equation (18) can be obtained. Where, ΔT _i representative of a correction point corresponding to each ^S T _{W, model} and ^S T _W, the _measured difference. J _i is the _{Jacobian matrix of ΔT i} , which can be calculated by the DH parameters of the current position and posture of the arm and the kinematic model, and

It represents 4 DH parameter errors

.

(16)

(17)

(18) As shown in Fig. 5C, it represents the _{error ΔT i between the nominal position E nominal of the} end effector and the actually reached position in each position and attitude. In equation (17), J ₁ ~J _n are respectively a _{Jacobian matrix of ΔT i} . In terms of the base plus six motion axes, the adjacent base and the motion axis, and the adjacent motion axis There will be six joints in total. Therefore, for each ΔT _i , it is expressed as [J _i1 , J _i2 , J _i3 , J _i4 , J _i5 , J _i6 ]*[ Δd, Δθ, Δα, Δγ] ^Τ , which represents each correction point TP, every A joint adjacent to the motion axis will have corresponding position and attitude compensation information [Δd, Δθ, Δα, Δγ]. Calculate using n ΔT _i

After that, each corresponding to ΔT _i

It is the position and posture compensation information of the joints of the motion device adjacent to the motion axis, as shown in equation (19). To compensate the original nominal DH parameters, the equation (4) changes to equation (19), and the equation (3) changes to equation (20). The calibrated ^E T _B,c can be used to move the robot arm to the corrected position and posture close to the nominal position and posture.

(19)

(20)

(21) Based on the calculation result of the above step 493, the compensation information ⁿ T _{n-1,c of} each joint of the motion device at each calibration point TP can be obtained. Therefore, when the processing is actually in progress, if the processing position of the motion device is just any of the above-mentioned TP positions, the DH parameters of the _{nominal position E nominal can be added, such as: d n} , θ _n , α _n and γ _{n The compensation information Δd n} , Δθ _n , Δα _n and Δγ _{n corresponding} to the joints of TP above are used to obtain the position and posture shown in equation (19). If the target position of the moving device is not the TP position. _{Add the compensation information Δd n} , Δα _n and Δγ _{n of} each axis to the DH parameter of the target nominal position E _nominal to obtain the formula (21). Using the formula (21) and the target position for the iterative calculation of inverse kinematics, we get θ _n parameters of each axis, then the input arm controller parameters θ _n, and then moves the arm to the target position command.

(twenty one)

…(22) 中A為本實施例中的L_i ，而B則為本實施例中的P_i 。而

=BX-ZA可以進一步表示成如下式(22)：

(23) 其中，

=

=

=

=

=

=

=

=

=

=

=

=

=

=

=

=

=

, 其中c 表示餘弦函數(cos)，s表示正弦函數(sin)。

=

…(24)

=

…(25)

=

…(26)

=

…(27) 由上述方程式(23)~(27)即可以決定運動裝置每一個運動軸的補償參數

=

。It is noted _that, ^S T _{W, model} and ^S T _W, the difference of the _measured error ΔTi representative of a correction point corresponding to each of the image capturing position of the probe is also calculated for each down movement of the shaft position and attitude error information. In another embodiment, the error ΔT _i can also be calculated using the position of the end effector to calculate the error of each motion axis, which can be expressed as the following equation (21):

... (22) A L _i of the present embodiment example, and B in this case was P _i of FIG. and

=BX-ZA can be further expressed as the following formula (22):

(23) Among them,

=

=

=

=

=

=

=

=

=

=

=

=

=

=

=

=

=

, Where c represents the cosine function (cos) and s represents the sine function (sin).

=

…(twenty four)

=

…(25)

=

…(26)

=

…(27) According to the above equations (23)~(27), the compensation parameters of each motion axis of the motion device can be determined

=

.

The above description only describes the preferred implementations or examples of the technical means adopted by the present invention in order to solve the problems, and is not used to limit the scope of implementation of the patent of the present invention. That is to say, all the equivalent changes and modifications made in accordance with the scope of the patent application of the present invention or made in accordance with the scope of the patent of the present invention are covered by the scope of the present patent.

2, 2a: Correction device 20: Correction unit 200: Board body 201: Supporting structure 202: correction body 203: connecting member 3: Correction system 30: exercise device 300: Clamping parts 301: Pedestal 302: End Effector 31: Image capture probe 310: light source 311, 312: Image capture device 313: spot pattern 314: Baseline 32: arithmetic processing device 4.4a: Process 40~47: Step 470~473: Steps

Fig. 1 is a three-dimensional schematic diagram of a position and posture correction device of the present invention. 2A and 2B are respectively a schematic top view of a different calibration device by combining a plurality of calibration units. Fig. 2C and Fig. 2D show schematic diagrams of different embodiments of the calibration device of the present invention. Fig. 3A is a schematic diagram of an embodiment of the calibration system of the present invention. FIG. 3B is a schematic diagram of another embodiment of the calibration system of the present invention. 3C and 3D are schematic diagrams of different embodiments of the image capturing probe of the present invention. 4A is a schematic flowchart of an embodiment of the position and attitude correction method of the present invention. 4B is a schematic diagram of an embodiment of the correction space and correction points of the present invention. FIG. 4C is a schematic flowchart of an embodiment of determining position and attitude compensation information according to the present invention. FIG. 4D is a schematic diagram of another embodiment of the invention for calculating the conversion relationship of the nominal position. 5A is a schematic diagram of the hand-eye position conversion relationship of the present invention. FIG. 5B is a schematic diagram showing the conversion relationship between the conversion relationship Z and the arm correction conversion relationship, the hand-eye conversion relationship and the measurement and the world coordinate conversion relationship according to the present invention. FIG. 5C is a schematic diagram of the gap between the actually reached position and posture calculated by the present invention and the position and posture that should be reached.

20: Correction unit

202: correction body

31: Image capture probe

32: arithmetic processing device

306, 306a, 306b: motion axis

L1, L2: Arm correction conversion relationship

P1, P2: Measurement and world coordinate conversion relationship

X: Hand-eye conversion relationship

{E}: (end effector, 6th axis)

{B}: (Base, 0th axis)

{W}: World Coordinate System

{S}: Measurement coordinate system

Claims (15)

A method for position and posture correction, which includes the following steps: Provide a movement device with an end effector; Establish a correction space regarding the range of motion of the exercise device, which has a plurality of correction points; A calibration device is provided, which includes at least one supporting structure and a plurality of calibration bodies arranged on the at least one supporting structure, each calibration body has at least one characteristic point, and the plurality of calibration bodies has at least four characteristic points Not coplanar On the end effector, choose to set up a calibration device or an image capture probe; Control the motion device to move the end effector to each calibration point, so that at each calibration point, the arm calibration position and posture of the motion device are different, and at each calibration point, the image capture probe captures the The arm of the correction device calibrates the image, and each arm has at least six feature points in the calibrated image; Determine the correction conversion relationship between the end-effector for each correction point and an arm of the motion device according to the correction position and posture of each arm of the motion device; According to the position of each feature point in the calibration image of each arm relative to a measurement coordinate position in the measurement coordinate system of the image capturing probe and a world coordinate position corresponding to the world coordinate system, it is determined about each calibration The conversion relationship between a measurement of a point and the world coordinate; and According to the arm calibration conversion relationship of each calibration point and the conversion relationship between the measurement and the world coordinate, the compensation information of the motion device at any position and posture in the calibrated space is determined. For the position and posture correction method described in item 1 of the scope of patent application, determining the position and posture compensation information further includes the following steps: Calculate the hand-eye conversion relationship on the end effector according to the arm correction conversion relationship corresponding to each arm correction position and posture and the conversion relationship between the measurement and the world coordinate; Calculate a standard conversion relationship based on the arm correction conversion relationship, the hand-eye conversion relationship, and the measurement and world coordinate conversion relationship; and According to the hand-eye conversion relationship, the coordinate conversion relationship, the arm correction conversion relationship of each calibration point, and the measurement and world coordinate conversion relationship, the position and posture compensation information are determined. For example, the position and posture correction method described in item 1 of the scope of patent application includes the following steps to determine the position and posture compensation information: Control the movement device to move to a plurality of different hand-eye correction positions with a plurality of different correction positions and postures, and make the image capturing probe capture the hand-eye correction image of the correction device at each correction point, each hand There are at least six feature points in the eye-corrected image; Determine the correction conversion relationship of the end effector relative to the arm of the motion device according to a plurality of different correction positions and postures of the motion device; Determine the position of each feature point in the hand-eye correction image corresponding to each hand-eye correction position relative to the measurement coordinate position of the image capturing probe under the measurement coordinate system and the world coordinate position corresponding to the world coordinate system The conversion relationship between the measurement of the correction position of each hand and the world coordinate; According to the arm calibration conversion relationship and the conversion relationship between measurement and world coordinates, the hand-eye conversion relationship on the end effector is calculated; Calculate a base-world coordinate conversion relationship based on the arm correction conversion relationship, the hand-eye conversion relationship, and the measurement-world coordinate conversion relationship; The position and posture compensation information are determined according to the arm correction conversion relationship, the base and world coordinate conversion relationship, the hand-eye conversion relationship, and the measurement and world coordinate conversion relationship. According to the position and posture correction method described in item 1 of the scope of patent application, the correction device includes a plurality of correction units which are connected to each other to form the correction device, and each correction unit further has: A board; and One end of a plurality of supporting structures is connected to the board, and each supporting structure has at least one correcting body. The position and posture correction method described in item 1 of the scope of patent application, wherein the image capturing probe is arranged on the end effector, the correction device is arranged on the transfer device, or the correction device It is arranged on the end effector, the image capturing probe is arranged on the transfer device, and the transfer device can perform displacement movement in at least one dimension. For the position and posture correction method described in item 1 of the scope of patent application, the correction device is arranged on the end effector, and the image capturing probe is arranged on a transfer device, which can perform At least one-dimensional displacement movement. The position and posture correction method described in item 1 of the scope of patent application, wherein the motion device has a base and a plurality of mutually related motion axes, the end effector is arranged on one of the motion axes, each The motion axis has at least one degree of freedom of motion. A position and attitude correction system, including: An image capture probe; A calibration device includes at least one supporting structure and a plurality of calibration bodies arranged on the at least one supporting structure, each calibration body has at least one characteristic point, and the plurality of calibration bodies has at least four characteristic points. Coplanar A motion device having an end effector thereon, wherein one of the image capturing probe or the calibration device is disposed on the end effector, the motion device is located in a calibration space, and the calibration device There are multiple calibration points in the space; and An arithmetic processing device, which is coupled with the motion device, the arithmetic processing device controls the motion device to move the end effector to each calibration point, so that at each calibration point, the arm correction position of the motion device It is not the same as the posture, and at each calibration point, the image capturing probe is used to capture the arm calibration image of the calibration device. Each arm calibration image has at least six feature points, and the position is calibrated according to each arm of the motion device. And the posture determines the calibration conversion relationship between the end-effector for each calibration point and an arm of the motion device, and the measurement coordinate system of each feature point in the calibration image with respect to the image capturing probe is based on each arm. The position of the measurement coordinate system below and the position of the world coordinate system corresponding to the world coordinate system determine the conversion relationship between a measurement and the world coordinate for each calibration point, and the conversion relationship between the arm calibration of each calibration point and the quantity The conversion relationship between measurement and world coordinates determines the compensation information of the motion device at any position and posture in the space to be calibrated. The position and posture correction system described in item 8 of the scope of patent application, wherein the arithmetic processing device calculates the end effect according to the arm correction conversion relationship corresponding to each arm correction position and posture and the conversion relationship between the measurement and the world coordinate The hand-eye conversion relationship on the device is calculated based on the arm correction conversion relationship, the hand-eye conversion relationship, and the measurement and world coordinate conversion relationship. According to the hand-eye conversion relationship, the coordinate conversion relationship, and each calibration point The arm calibration conversion relationship and the conversion relationship between the measurement and the world coordinate determine the position and posture compensation information. For example, the position and posture correction system described in item 8 of the scope of patent application, wherein the arithmetic processing device controls the movement device to move to a plurality of different hand-eye correction positions in a plurality of different positions and postures, and make the image capture The probe captures the hand-eye correction image of the correction device, and each hand-eye correction image has at least six feature points. According to a plurality of different positions and postures of the movement device, the end effector relative to the arm of the movement device is determined The calibration conversion relationship is based on the position of each feature point in the hand-eye calibration image corresponding to each hand-eye calibration position relative to the measurement coordinate system position of the image capturing probe under the measurement coordinate system and corresponding to the world coordinate system The world coordinate system position determines the conversion relationship between the measurement of each hand-eye correction position and the world coordinate, and calculates the hand-eye conversion relationship on the end effector according to the arm correction conversion relationship and the conversion relationship between the measurement and the world coordinate. Correction conversion relationship, the hand-eye conversion relationship and the measurement and world coordinate conversion relationship calculate a base-world coordinate conversion relationship, according to the arm correction conversion relationship, the base-world coordinate conversion relationship, the hand-eye conversion relationship, and the The conversion relationship between measurement and world coordinates determines the position and attitude compensation information. The position and posture correction system described in item 8 of the scope of patent application, wherein the correction device includes a plurality of correction units which are connected to each other to form the correction device, and each correction unit further has: A board; and One end of a plurality of supporting structures is connected to the board, and each supporting structure is provided with at least one correction body. The position and attitude correction system described in item 8 of the scope of patent application, wherein the image capturing probe is set on the end effector, and the correction device is set on a transfer device, which can perform At least one-dimensional displacement movement. For the position and posture correction system described in item 9 of the scope of patent application, the correction device is arranged on the end effector, and the image capturing probe is arranged on a transfer device, which can perform At least one-dimensional displacement movement. The position and attitude correction system described in item 8 of the scope of patent application, wherein the motion device has a base and a plurality of mutually related motion axes, the end effector is arranged on one of the motion axes, each The motion axis has at least one degree of freedom of motion. A position and posture correction device, including: An image capture probe; A calibration device includes at least one supporting structure and a plurality of calibration bodies arranged on the at least one supporting structure, each calibration body has at least one characteristic point, and the plurality of calibration bodies has at least four characteristic points. Coplanar A transfer device for carrying the image capturing probe or the calibration device; and An arithmetic processing device that controls a motion device to move an end effector on the motion device to a plurality of correction points in the space where the motion device is located in a plurality of different calibration positions and postures, so that the image capture probe is placed on each The correction point captures the correction image of the arm of the correction device. Each arm correction image has at least six characteristic points. The arithmetic processing device determines the end of each correction point according to each position and posture of the motion device. The effector is calibrated and converted with respect to one of the arms of the motion device, and each feature point in the image is calibrated according to the position of the measurement coordinate system under the measurement coordinate system of the image capturing probe and corresponding to the world according to each arm. The position of the world coordinate system under the coordinate system determines the conversion relationship between a measurement of each calibration point and the world coordinate, and the conversion relationship between the arm calibration of each calibration point and the conversion relationship between the measurement and the world coordinate determine the movement device Compensation information for any position and posture in the corrected space.

Images