Tail end vertical attitude automatic calibration method, device, equipment, medium and product

Abstract

The invention discloses an automatic calibration method, device and equipment for a vertical posture of a tail end, a medium and a product, and relates to the technical field of robot control. The method comprises the following steps: respectively selecting three reference measuring points and three target measuring points which are not on the same straight line on a reference plane and a target plane, respectively calculating to obtain a normalized reference normal vector and a normalized target normal vector based on selection results, then constructing to obtain a rotation matrix by using a Rodrigues rotation formula based on the two normal vectors, and finally obtaining a normalized target normal vector based on the rotation matrix. According to current attitude data of a to-be-calibrated tail end, an initial rotation matrix is obtained through calculation, target attitude data are extracted from a matrix obtained by multiplying the rotation matrix by the initial rotation matrix, and the data are transmitted to an attitude adjustment controller for calibration application, so that the whole calibration process can be accurate and clear, and the calibration efficiency is improved. Numerical iterative optimization is avoided, meanwhile, calculation is efficient and stable, and then the posture of the robot end effector can be automatically calibrated to be perpendicular to the target plane quickly and accurately.

Description

TECHNICAL FIELD

[0001] The present application belongs to the technical field of robot control, and particularly relates to a method, device, equipment, medium and product for automatically calibrating a vertical posture of a robot end. BACKGROUND

[0002] With the increasing requirements for robot operation precision and reliability in application scenarios such as industrial automation, medical surgery and aerospace, the posture calibration of a robot end effector becomes a key link. However, the existing calibration techniques still have the following deficiencies in practical application:

[0003] (1) Dependence on manual operation, that is, traditional calibration techniques need manual repeated adjustment of a reference plane posture, resulting in a large time consumption and being easily affected by the experience of an operator;

[0004] (2) High sensor cost, that is, when using a three-dimensional vision sensor, a force sensor or a torque sensor array for real-time calibration, a large hardware overhead is caused, and it is not easy to promote in a cost-sensitive scenario;

[0005] (3) Difficulty in numerical iterative optimization, that is, the calibration method mainly based on a gradient or an iterative algorithm is not only computationally complex and slow in convergence speed, but also easily falls into a local optimum, making it difficult to guarantee global accuracy;

[0006] (4) Poor real-time performance, that is, on a dynamic or mobile platform, the traditional method is prone to insufficient real-time closed-loop performance and large response delay due to continuous changes in measurement points;

[0007] (5) Weak system compatibility, that is, most calibration methods need to be deeply integrated with a special controller or software platform, resulting in poor compatibility and high engineering application threshold when transplanted to robots of different brands or models.

[0008] Therefore, it is of important engineering value and application prospect to develop a robot end posture automatic calibration method without additional high-cost sensors, simple algorithm and fast real-time response capability. SUMMARY

[0009] The present application aims to provide a method, device, computer equipment, computer readable storage medium and computer program product for automatically calibrating a vertical posture of a robot end, so as to solve the problems of dependence on manual operation, high sensor cost, difficulty in numerical iterative optimization, poor real-time performance and / or weak system compatibility existing in the existing calibration techniques.

[0010] In order to achieve the above-mentioned purpose, the present application adopts the following technical solutions:

[0011] In a first aspect, a method for automatically calibrating a vertical posture of a robot end is provided, comprising:

[0012] selecting three reference measuring points not on the same straight line on a reference plane, and selecting three target measuring points not on the same straight line on a target plane, wherein the reference plane refers to a plane currently perpendicular to a to-be-calibrated end, and the target plane refers to a plane that needs to be adjusted in pose of the to-be-calibrated end so as to be perpendicular to the end in the future;

[0013] calculating a normalized reference normal vector according to point vectors of the three reference measuring points, and calculating a normalized target normal vector according to point vectors of the three target measuring points;

[0014] constructing a rotation matrix by using a Rodrigues rotation formula according to the normalized reference normal vector and the normalized target normal vector;

[0015] calculating an initial rotation matrix according to current pose data of the to-be-calibrated end;

[0016] multiplying the rotation matrix and the initial rotation matrix to obtain a target vertical pose matrix for achieving that the target plane is perpendicular to the to-be-calibrated end;

[0017] extracting target pose data from the target vertical pose matrix, and transmitting the target pose data to a pose adjustment controller of the to-be-calibrated end, so that the pose adjustment controller calibrates the pose of the to-be-calibrated end according to the target pose data.

[0018] Based on the above invention content, a new scheme of automatically calibrating end vertical pose based on normal vector operation and Rodrigues rotation formula is provided, that is, first, three reference measuring points and three target measuring points not on the same straight line are selected on a reference plane and a target plane respectively, and then normalized reference normal vectors and normalized target normal vectors are calculated based on the selection results. Then, based on the two normal vectors, a rotation matrix is constructed by using a Rodrigues rotation formula, and an initial rotation matrix is calculated according to current pose data of the to-be-calibrated end. Finally, target pose data is extracted from the matrix obtained by multiplying the rotation matrix and the initial rotation matrix, and the data is transmitted to a pose adjustment controller for calibration application. In this way, the entire calibration process is accurate and clear, avoids numerical iterative optimization, and is efficient and stable in calculation. Therefore, the robot end effector pose can be automatically calibrated to be perpendicular to the target plane without relying on additional sensor equipment or manual calibration operation, and the robot end effector pose can be quickly and accurately calibrated to be perpendicular to the target plane, which is particularly suitable for real-time online calibration in complex dynamic environments, and is convenient for practical application and promotion.

[0019] In one possible design, the normalized reference normal vector is calculated according to the point vectors of the three reference measuring points, including:

[0020] According to the point vectors of the three reference points, first reference vector difference VB1 and second reference vector difference VB2 are calculated according to the following formula:

[0021]

[0022] In the formula, B1 represents the point vector of the first reference point among the three reference points, B2 represents the point vector of the second reference point among the three reference points, and B3 represents the point vector of the third reference point among the three reference points.

[0023] The cross product operation is performed on the first reference vector difference VB1 and the second reference vector difference VB2, and the operation result is normalized to obtain a normalized reference normal vector.

[0024] In one possible design, according to the point vectors of the three target points, a normalized target normal vector is calculated, including:

[0025] According to the point vectors of the three target points, first target vector difference VP1 and second target vector difference VP2 are calculated according to the following formula:

[0026]

[0027] In the formula, P1 represents the point vector of the first target point among the three target points, P2 represents the point vector of the second target point among the three target points, and P3 represents the point vector of the third target point among the three target points.

[0028] The cross product operation is performed on the first target vector difference VP1 and the second target vector difference VP2, and the operation result is normalized to obtain a normalized target normal vector.

[0029] In one possible design, according to the normalized reference normal vector and the normalized target normal vector, a rotation matrix is constructed by using the Rodrigues rotation formula, including:

[0030] According to the normalized reference normal vector and the normalized target normal vector, the rotation axis unit vector axis and the rotation angle angle are calculated according to the following formula: unit

[0031]

[0032] In the formula, nB represents the normalized reference normal vector, nP represents the normalized target normal vector, dot() represents a vector dot product function, and arccos() represents an inverse cosine function.

[0033] ​According to the rotation axis unit vector axis unit Given the rotation angle angle, the following rotation matrix R is constructed using the Rodrigues rotation formula. axis :

[0034] R axis =I+sin(angle)·K+(1-cos(angle))·K 2

[0035] In the formula, I represents the identity matrix, and K represents the unit vector based on the rotation axis. unit The defined antisymmetric matrix, sin() represents the sine function, and cos() represents the cosine function.

[0036] In one possible design, an initial rotation matrix is ​​calculated based on the current attitude data of the end effector to be calibrated, including:

[0037] Based on the current attitude data of the end effector to be calibrated, the first intermediate vector R is calculated according to the following formula. x The second intermediate vector R y and the third intermediate vector R z :

[0038]

[0039] In the formula, roll0 represents the roll angle in the current attitude data, pitch0 represents the pitch angle in the current attitude data, yaw0 represents the yaw angle in the current attitude data, sin() represents the sine function, and cos() represents the cosine function.

[0040] According to the first intermediate vector R x The second intermediate vector R y and the third intermediate vector R z The initial rotation matrix R is calculated. init =R z ×R y ×R x .

[0041] Secondly, an automatic end-effector vertical attitude calibration device is provided, which includes a planar measurement point acquisition unit, a normal vector calculation unit, a rotation matrix construction unit, an initial matrix acquisition unit, an attitude matrix calculation unit, and a control output interface unit.

[0042] The planar measurement point acquisition unit is used to select three reference measurement points that are not on the same straight line on the reference plane, and to select three target measurement points that are not on the same straight line on the target plane. The reference plane refers to the plane that is currently perpendicular to the end to be calibrated, and the target plane refers to the plane that needs to be adjusted so that it will be perpendicular to the end in the future.

[0043] The normal vector calculation unit is communicatively connected to the plane measurement point acquisition unit, and is used to calculate the normalized reference normal vector based on the point vectors of the three reference measurement points, and to calculate the normalized target normal vector based on the point vectors of the three target measurement points.

[0044] The rotation matrix construction unit is communicatively connected to the normal vector calculation unit and is used to construct a rotation matrix using the Rodrigues rotation formula based on the normalized reference normal vector and the normalized target normal vector.

[0045] The initial matrix acquisition unit is used to calculate the initial rotation matrix based on the current attitude data of the end to be calibrated.

[0046] The attitude matrix calculation unit is communicatively connected to the rotation matrix construction unit and the initial matrix acquisition unit, respectively, and is used to multiply the rotation matrix with the initial rotation matrix to obtain the target vertical attitude matrix used to achieve the perpendicularity of the target plane with the end to be calibrated;

[0047] The control output interface unit is communicatively connected to the attitude matrix calculation unit, and is used to extract target attitude data from the target vertical attitude matrix and transmit the target attitude data to the attitude adjustment controller of the end to be calibrated, so that the attitude adjustment controller can calibrate the attitude of the end to be calibrated based on the target attitude data.

[0048] In one possible design, the normal vector calculation unit is connected to the plane measurement point acquisition unit via a high-speed data bus or network interconnection; the rotation matrix construction unit is connected to the normal vector calculation unit via a high-speed data bus or network interconnection; the attitude matrix solving unit is connected to the rotation matrix construction unit and the initial matrix acquisition unit via a high-speed data bus or network interconnection respectively; and the control output interface unit is connected to the attitude matrix solving unit via a high-speed data bus or network interconnection.

[0049] Thirdly, the present invention provides a computer device comprising a storage module, a processing module, and a transceiver module connected in sequence for communication, wherein the storage module is used to store a computer program, the transceiver module is used to send and receive messages, and the processing module is used to read the computer program and execute the end-effector vertical attitude automatic calibration method as described in the first aspect or any possible design in the first aspect.

[0050] Fourthly, the present invention provides a computer-readable storage medium storing instructions that, when executed on a computer, perform the automatic end-effector vertical attitude calibration method as described in the first aspect or any possible design within the first aspect.

[0051] Fifthly, the present invention provides a computer program product, including a computer program or instructions, which, when executed by a computer, implement the automatic end-effector vertical attitude calibration method as described in the first aspect or any possible design in the first aspect.

[0052] The beneficial effects of the above scheme are:

[0053] (1) This invention creatively provides a new scheme for automatic end-effector vertical attitude calibration based on normal vector operation and Rodrigues rotation formula. First, three reference measurement points and three target measurement points that are not on the same straight line are selected on the reference plane and the target plane respectively. Based on the selection results, the normalized reference normal vector and the normalized target normal vector are calculated respectively. Then, based on the two normal vectors, the rotation matrix is ​​constructed using the Rodrigues rotation formula. The initial rotation matrix is ​​calculated according to the current attitude data of the end-effector to be calibrated. Finally, the target attitude data is extracted from the matrix obtained by multiplying the rotation matrix and the initial rotation matrix, and the data is transmitted to the attitude adjustment controller for calibration application. This makes the entire calibration process accurate and clear, avoids numerical iterative optimization, and is computationally efficient and stable. It can be done without relying on additional sensor equipment or manual calibration operations, and can quickly and accurately calibrate the robot end-effector attitude to be perpendicular to the target plane. It is particularly suitable for real-time online calibration in complex dynamic environments.

[0054] (2) High-precision attitude calibration without iterative error can be achieved by analytically calculating the normal vector and constructing the Rodrigues rotation matrix;

[0055] (3) It can be universally adapted to the plane, that is, it only requires any three coordinates to automatically perpendicularize the reference plane and the target plane at any angle, which has strong applicability;

[0056] (4) It can close the loop in real time, that is, a single matrix operation can complete the rotation calculation, with a small amount of computation, which meets the high-speed response requirements in dynamic environments.

[0057] (5) It facilitates modular system design, which makes each unit such as measurement point acquisition, normal vector calculation, rotation matrix construction and initial acquisition, attitude calculation and control output interface independent, and easy to integrate and expand;

[0058] (6) Hardware portability means that no additional high-cost sensors are required. It only relies on the pose information of the robot body and general measurement point acquisition equipment, which has excellent compatibility and is convenient for practical application and promotion. Attached Figure Description

[0059] 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.

[0060] Figure 1 This is a flowchart illustrating the automatic end-effector vertical attitude calibration method provided in this application embodiment.

[0061] Figure 2 This is an example diagram illustrating the change in the vertical relationship between the robot's end-effector pose and the reference plane and the target plane, as provided in an embodiment of this application.

[0062] Figure 3 This is a schematic diagram of the structure of the automatic end-effector vertical attitude calibration device provided in the embodiments of this application.

[0063] Figure 4 A schematic diagram of the structure of a computer device provided in an embodiment of this application. Detailed Implementation

[0064] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the present invention will be briefly introduced below in conjunction with the accompanying drawings and descriptions of the embodiments or the prior art. Obviously, the following description of the structure of the accompanying drawings is only some embodiments of the present invention. For those skilled in the art, other embodiments can be obtained based on these embodiments without creative effort. It should be noted that the description of these embodiments is for the purpose of helping to understand the present invention, but does not constitute a limitation of the present invention.

[0065] It should be understood that although the terms "first" and "second", etc., may be used herein to describe various objects, these objects should not be limited by these terms. These terms are only used to distinguish one object from another. For example, the first object may be referred to as the second object, and similarly, the second object may be referred to as the first object, without departing from the scope of the exemplary embodiments of the invention.

[0066] It should be understood that the term "and / or" that may appear in this document is merely a description of the relationship between related objects, indicating that three relationships can exist. For example, A and / or B can mean: A exists alone, B exists alone, or A and B exist simultaneously. Another example is A, B and / or C, which can mean that any one of A, B, and C or any combination thereof exists. The term " / and" that may appear in this document describes another relationship between related objects, indicating that two relationships can exist. For example, A / and B can mean: A exists alone or A and B exist simultaneously. In addition, the character " / " that may appear in this document generally indicates that the related objects before and after it are in an "or" relationship.

[0067] Example

[0068] like Figures 1-2 As shown, the automatic vertical attitude calibration method for the end effector provided in the first aspect of this embodiment can be executed, but is not limited to, by a computer device with certain computing resources and which is communicatively connected to the end effector pose system and the attitude adjustment controller. For example, it can be executed by an electronic device such as a robot end effector, a server, a personal computer (PC, referring to a multi-purpose computer of a size, price, and performance suitable for personal use; desktop computers, laptops, mini-laptops, tablets, and ultrabooks are all considered personal computers), a smartphone, a personal digital assistant (PDA), or a wearable device. Figure 1 As shown, the automatic vertical attitude calibration method for the end effector includes, but is not limited to, the following steps S1 to S6.

[0069] S1. Select three reference measurement points that are not on the same straight line on the reference plane, and select three target measurement points that are not on the same straight line on the target plane. The reference plane is the plane that is currently perpendicular to the end to be calibrated, and the target plane is the plane that needs to be adjusted so that it will be perpendicular to the end in the future.

[0070] In step S1, the end effector to be calibrated is the object to be automatically calibrated, which can be, for example but not limited to, a robot end effector / robot end effector; such as Figure 2 As shown, the plane specifically containing points B1, B2, and B3 is the reference plane, and the plane specifically containing points P1, P2, and P3 is the target plane. After adjusting the attitude of the end-effector to be calibrated, it needs to be made perpendicular to the target plane at a future time. Furthermore, the three reference measurement points (e.g., Figure 2 Points B1, B2, and B3 in the above) and the three target measurement points (e.g. Figure 2Points P1, P2, and P3 in the data can be obtained through conventional acquisition using general-purpose measurement point acquisition equipment, or they can be randomly and automatically selected.

[0071] S2. Based on the point vectors of the three reference measurement points, calculate the normalized reference normal vector, and based on the point vectors of the three target measurement points, calculate the normalized target normal vector.

[0072] In step S2, the two normal vectors can be calculated using vector subtraction and cross product operations. Specifically, based on the point vectors of the three reference measurement points, the normalized reference normal vectors are calculated, including but not limited to the following steps: First, based on the point vectors of the three reference measurement points, the first reference vector difference VB1 and the second reference vector difference VB2 are calculated according to the following formula:

[0073]

[0074] In the formula, B1 represents the point vector of the first reference measurement point among the three reference measurement points, B2 represents the point vector of the second reference measurement point among the three reference measurement points, and B3 represents the point vector of the third reference measurement point among the three reference measurement points. Then, the cross product operation is performed on the first reference vector difference VB1 and the second reference vector difference VB2, and the result is normalized to obtain the normalized reference normal vector. Specifically, based on the point vectors of the three target measurement points, the normalized target normal vector is calculated, including but not limited to the following steps: First, based on the point vectors of the three target measurement points, the first target vector difference VP1 and the second target vector difference VP2 are calculated according to the following formula:

[0075]

[0076] In the formula, P1 represents the point vector of the first target measurement point among the three target measurement points, P2 represents the point vector of the second target measurement point among the three target measurement points, and P3 represents the point vector of the third target measurement point among the three target measurement points. Then, a cross product operation is performed on the first target vector difference VP1 and the second target vector difference VP2, and the result is normalized to obtain the normalized target normal vector. The point vectors of the reference measurement point and the target measurement point can be two vectors in the same three-dimensional coordinate system, or two vectors in their respective planar coordinate systems. Furthermore, the specific processes of the cross product operation and the normalization process are existing technical means and will not be described in detail here.

[0077] S3. Based on the normalized reference normal vector and the normalized target normal vector, construct the rotation matrix using the Rodrigues rotation formula.

[0078] In step S3, the Rodrigues rotation formula, also known as the Rodrigues rotation formula, is a mathematical formula used to calculate the new vector obtained by rotating a vector in three-dimensional space around a specified rotation axis by a specific angle. This formula decomposes the original vector into components parallel to the rotation axis and perpendicular to it. It uses the original vector, the rotation axis, and their cross product to construct a frame, and only performs a rotation transformation on the perpendicular component to reconstruct the vector. Its matrix form consists of a linear combination of the original vector, the rotation axis, and the cross product, and can be rewritten as a standard matrix expression. Specifically, based on the normalized reference normal vector and the normalized target normal vector, the rotation matrix is ​​constructed using the Rodrigues rotation formula, including but not limited to the following steps S31 to S32.

[0079] S31. Based on the normalized reference normal vector and the normalized target normal vector, the rotation axis unit vector is calculated according to the following formula. unit and rotation angle:

[0080]

[0081] In the formula, nB represents the normalized reference normal vector, nP represents the normalized target normal vector, dot() represents the vector dot product function, and arccos() represents the inverse cosine function.

[0082] S32. Based on the unit vector of the rotation axis unit Given the rotation angle angle, the following rotation matrix R is constructed using the Rodrigues rotation formula. axis :

[0083] R axis =I+sin(angle)·K+(1-cos(angle))·K 2

[0084] In the formula, I represents the identity matrix, and K represents the unit vector based on the rotation axis. unit The defined antisymmetric matrix, sin() represents the sine function, and cos() represents the cosine function.

[0085] S4. Calculate the initial rotation matrix based on the current attitude data of the end to be calibrated.

[0086] In step S4, the current attitude data can be directly read from the end effector pose system, and specifically includes, but is not limited to, roll angle, pitch angle, and yaw angle. Specifically, based on the current attitude data of the end effector to be calibrated, an initial rotation matrix is ​​calculated, including but not limited to the following steps S41 to S42.

[0087] S41. Based on the current attitude data of the end effector to be calibrated, calculate the first intermediate vector R according to the following formula. x The second intermediate vector R y and the third intermediate vector R z :

[0088]

[0089] In the formula, roll0 represents the roll angle in the current attitude data, pitch0 represents the pitch angle in the current attitude data, yaw0 represents the yaw angle in the current attitude data, sin() represents the sine function, and cos() represents the cosine function.

[0090] S42. Based on the first intermediate vector R x The second intermediate vector R y and the third intermediate vector R z The initial rotation matrix R is calculated. init =R z ×R y ×R x .

[0091] S5. Multiply the rotation matrix by the initial rotation matrix to obtain the target vertical attitude matrix used to achieve the perpendicularity between the target plane and the end to be calibrated.

[0092] S6. Extract target attitude data from the target vertical attitude matrix and transmit the target attitude data to the attitude adjustment controller of the end to be calibrated, so that the attitude adjustment controller can calibrate the attitude of the end to be calibrated based on the target attitude data.

[0093] In step S6, the target attitude data includes, but is not limited to, roll angle, pitch angle, and yaw angle. After these data are transmitted to the attitude adjustment controller, they can be combined with conventional control methods (such as PID control algorithms) to calibrate the attitude of the end effector to be calibrated.

[0094] Therefore, based on the automatic end-effector vertical attitude calibration method described in steps S1 to S6 above, a new scheme for automatic end-effector vertical attitude calibration based on normal vector calculation and Rodrigues rotation formula is provided. First, three reference measurement points and three target measurement points not on the same straight line are selected on the reference plane and the target plane, respectively. Based on the selection results, normalized reference normal vectors and normalized target normal vectors are calculated respectively. Then, based on the two normal vectors, a rotation matrix is ​​constructed using the Rodrigues rotation formula. An initial rotation matrix is ​​calculated based on the current attitude data of the end-effector to be calibrated. Finally, the target attitude data is extracted from the matrix obtained by multiplying the rotation matrix and the initial rotation matrix, and this data is transmitted to the attitude adjustment controller for calibration application. This makes the entire calibration process accurate and clear, avoids numerical iterative optimization, and is computationally efficient and stable. Therefore, it does not rely on additional sensor equipment or manual calibration operations, and can quickly and accurately calibrate the robot end-effector attitude to be perpendicular to the target plane. It is particularly suitable for real-time online calibration in complex dynamic environments, facilitating practical application and promotion.

[0095] like Figure 3 As shown, the second aspect of this embodiment provides a virtual device for implementing the automatic end-effector vertical attitude calibration method described in the first aspect, including a planar measurement point acquisition unit, a normal vector calculation unit, a rotation matrix construction unit, an initial matrix acquisition unit, an attitude matrix calculation unit, and a control output interface unit;

[0096] The planar measurement point acquisition unit is used to select three reference measurement points that are not on the same straight line on the reference plane, and to select three target measurement points that are not on the same straight line on the target plane. The reference plane refers to the plane that is currently perpendicular to the end to be calibrated, and the target plane refers to the plane that needs to be adjusted so that it will be perpendicular to the end in the future.

[0097] The normal vector calculation unit is communicatively connected to the plane measurement point acquisition unit, and is used to calculate the normalized reference normal vector based on the point vectors of the three reference measurement points, and to calculate the normalized target normal vector based on the point vectors of the three target measurement points.

[0098] The rotation matrix construction unit is communicatively connected to the normal vector calculation unit and is used to construct a rotation matrix using the Rodrigues rotation formula based on the normalized reference normal vector and the normalized target normal vector.

[0099] The initial matrix acquisition unit is used to calculate the initial rotation matrix based on the current attitude data of the end to be calibrated.

[0100] The attitude matrix calculation unit is communicatively connected to the rotation matrix construction unit and the initial matrix acquisition unit, respectively, and is used to multiply the rotation matrix with the initial rotation matrix to obtain the target vertical attitude matrix used to achieve the perpendicularity of the target plane with the end to be calibrated;

[0101] The control output interface unit is communicatively connected to the attitude matrix calculation unit, and is used to extract target attitude data from the target vertical attitude matrix and transmit the target attitude data to the attitude adjustment controller of the end to be calibrated, so that the attitude adjustment controller can calibrate the attitude of the end to be calibrated based on the target attitude data.

[0102] In one possible design, the normal vector calculation unit is connected to the planar measurement point acquisition unit via a high-speed data bus or network interconnection. The rotation matrix construction unit is connected to the normal vector calculation unit via a high-speed data bus or network interconnection. The attitude matrix solving unit is connected to both the rotation matrix construction unit and the initial matrix acquisition unit via a high-speed data bus or network interconnection. The control output interface unit is connected to the attitude matrix solving unit via a high-speed data bus or network interconnection. This also allows the functional units to be interconnected via a high-speed data bus or network, achieving parallel computing and real-time closed-loop calibration processing.

[0103] The working process, working details and technical effects of the aforementioned device provided in the second aspect of this embodiment can be found in the automatic end-effector vertical attitude calibration method described in the first aspect, and will not be repeated here.

[0104] like Figure 4As shown, the third aspect of this embodiment provides a computer device for executing the automatic end-effector vertical attitude calibration method as described in the first aspect. The device includes a storage module, a processing module, and a transceiver module connected in sequence. The storage module stores a computer program, the transceiver module sends and receives messages, and the processing module reads the computer program and executes the automatic end-effector vertical attitude calibration method as described in the first aspect. Specifically, the storage module may include, but is not limited to, random-access memory (RAM), read-only memory (ROM), flash memory, first-in-first-out (FIFO) memory, and / or first-in-last-out (FILO) memory, etc.; the processing module may, but is not limited to, use a microprocessor of the STM32F105 series. Furthermore, the computer device may also include, but is not limited to, a power supply module, a display screen, and other necessary components.

[0105] The working process, working details and technical effects of the aforementioned computer device provided in the third aspect of this embodiment can be found in the automatic end-effector vertical attitude calibration method described in the first aspect, and will not be repeated here.

[0106] This fourth aspect of the embodiment provides a computer-readable storage medium storing instructions comprising the automatic end-effector vertical attitude calibration method as described in the first aspect. Specifically, the computer-readable storage medium stores instructions that, when executed on a computer, perform the automatic end-effector vertical attitude calibration method as described in the first aspect. The computer-readable storage medium refers to a data storage medium, and may include, but is not limited to, floppy disks, optical disks, hard disks, flash memory, USB flash drives, and / or Memory Sticks. The computer may be a general-purpose computer, a special-purpose computer, a computer network, or other programmable devices.

[0107] The working process, working details and technical effects of the aforementioned computer-readable storage medium provided in the fourth aspect of this embodiment can be found in the automatic end-effector vertical attitude calibration method described in the first aspect, and will not be repeated here.

[0108] This fifth aspect of the embodiment provides a computer program product, including a computer program or instructions, which, when executed by a computer, implement the automatic end-effector vertical attitude calibration method as described in the first aspect. The computer may be a general-purpose computer, a special-purpose computer, a computer network, or other programmable device.

[0109] Finally, it should be noted that the above description is merely a preferred embodiment of the present invention and is not intended to limit the scope of protection of the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. An automatic end-effector vertical attitude calibration method, characterized in that, include: Three reference measurement points not on the same straight line are selected on the reference plane, and three target measurement points not on the same straight line are selected on the target plane. The reference plane is the plane that is currently perpendicular to the end to be calibrated, and the target plane is the plane that needs to be adjusted so that it will be perpendicular to the end in the future. Based on the point vectors of the three reference measurement points, the normalized reference normal vector is calculated, and based on the point vectors of the three target measurement points, the normalized target normal vector is calculated. Based on the normalized reference normal vector and the normalized target normal vector, a rotation matrix is ​​constructed using the Rodrigues rotation formula; The initial rotation matrix is ​​calculated based on the current attitude data of the end to be calibrated; Multiplying the rotation matrix by the initial rotation matrix yields the target vertical attitude matrix used to achieve the perpendicularity between the target plane and the end to be calibrated; The target attitude data is extracted from the target vertical attitude matrix and transmitted to the attitude adjustment controller of the end to be calibrated, so that the attitude adjustment controller can calibrate the attitude of the end to be calibrated based on the target attitude data.

2. The automatic end-effector vertical attitude calibration method according to claim 1, characterized in that, Based on the point vectors of the three reference measurement points, the normalized reference normal vector is calculated, including: Based on the point vectors of the three reference measurement points, the first reference vector difference VB1 and the second reference vector difference VB2 are calculated according to the following formula: In the formula, B1 represents the point vector of the first reference measuring point among the three reference measuring points, B2 represents the point vector of the second reference measuring point among the three reference measuring points, and B3 represents the point vector of the third reference measuring point among the three reference measuring points. Perform a cross product operation on the first reference vector difference VB1 and the second reference vector difference VB2, and normalize the result to obtain a normalized reference normal vector.

3. The automatic end-effector vertical attitude calibration method according to claim 1, characterized in that, Based on the point vectors of the three target measurement points, the normalized target normal vector is calculated, including: Based on the point vectors of the three target measurement points, the first target vector difference VP1 and the second target vector difference VP2 are calculated according to the following formula: In the formula, P1 represents the point vector of the first target measuring point among the three target measuring points, P2 represents the point vector of the second target measuring point among the three target measuring points, and P3 represents the point vector of the third target measuring point among the three target measuring points; Perform a cross product operation on the first target vector difference VP1 and the second target vector difference VP2, and normalize the result to obtain a normalized target normal vector.

4. The automatic end-effector vertical attitude calibration method according to claim 1, characterized in that, Based on the normalized reference normal vector and the normalized target normal vector, a rotation matrix is ​​constructed using the Rodrigues rotation formula, including: Based on the normalized reference normal vector and the normalized target normal vector, the rotation axis unit vector is calculated according to the following formula. unit and rotation angle: In the formula, nB represents the normalized reference normal vector, nP represents the normalized target normal vector, dot() represents the vector dot product function, and arccos() represents the inverse cosine function. According to the rotation axis unit vector axis unit Given the rotation angle angle, the following rotation matrix R is constructed using the Rodrigues rotation formula. axis : R axis =I+sin(angle)·K+(1-cos(angle))·K 2 In the formula, I represents the identity matrix, and K represents the unit vector based on the rotation axis. unit The defined antisymmetric matrix, sin() represents the sine function, and cos() represents the cosine function.

5. The automatic end-effector vertical attitude calibration method according to claim 1, characterized in that, Based on the current attitude data of the end effector to be calibrated, an initial rotation matrix is ​​calculated, including: Based on the current attitude data of the end effector to be calibrated, the first intermediate vector R is calculated according to the following formula. x The second intermediate vector R y and the third intermediate vector R z : In the formula, roll0 represents the roll angle in the current attitude data, pitch0 represents the pitch angle in the current attitude data, yaw0 represents the yaw angle in the current attitude data, sin() represents the sine function, and cos() represents the cosine function. According to the first intermediate vector R x The second intermediate vector R y and the third intermediate vector R z The initial rotation matrix R is calculated. init =R z ×R y ×R x .

6. An automatic end-effector vertical attitude calibration device, characterized in that, It includes a planar measurement point acquisition unit, a normal vector calculation unit, a rotation matrix construction unit, an initial matrix acquisition unit, an attitude matrix calculation unit, and a control output interface unit; The planar measurement point acquisition unit is used to select three reference measurement points that are not on the same straight line on the reference plane, and to select three target measurement points that are not on the same straight line on the target plane. The reference plane refers to the plane that is currently perpendicular to the end to be calibrated, and the target plane refers to the plane that needs to be adjusted so that it will be perpendicular to the end in the future. The normal vector calculation unit is communicatively connected to the plane measurement point acquisition unit, and is used to calculate the normalized reference normal vector based on the point vectors of the three reference measurement points, and to calculate the normalized target normal vector based on the point vectors of the three target measurement points. The rotation matrix construction unit is communicatively connected to the normal vector calculation unit and is used to construct a rotation matrix using the Rodrigues rotation formula based on the normalized reference normal vector and the normalized target normal vector. The initial matrix acquisition unit is used to calculate the initial rotation matrix based on the current attitude data of the end to be calibrated. The attitude matrix calculation unit is communicatively connected to the rotation matrix construction unit and the initial matrix acquisition unit, respectively, and is used to multiply the rotation matrix with the initial rotation matrix to obtain the target vertical attitude matrix used to achieve the perpendicularity of the target plane with the end to be calibrated; The control output interface unit is communicatively connected to the attitude matrix calculation unit, and is used to extract target attitude data from the target vertical attitude matrix and transmit the target attitude data to the attitude adjustment controller of the end to be calibrated, so that the attitude adjustment controller can calibrate the attitude of the end to be calibrated based on the target attitude data.

7. The automatic end-effector vertical attitude calibration device according to claim 6, characterized in that, The normal vector calculation unit is connected to the plane measurement point acquisition unit via a high-speed data bus or network interconnection. The rotation matrix construction unit is connected to the normal vector calculation unit via a high-speed data bus or network interconnection. The attitude matrix solving unit is connected to the rotation matrix construction unit and the initial matrix acquisition unit via a high-speed data bus or network interconnection respectively. The control output interface unit is connected to the attitude matrix solving unit via a high-speed data bus or network interconnection.

8. A computer device, characterized in that, It includes a storage module, a processing module, and a transceiver module that are sequentially connected in communication. The storage module is used to store a computer program, the transceiver module is used to send and receive messages, and the processing module is used to read the computer program and execute the automatic end-effector vertical attitude calibration method as described in any one of claims 1 to 5.

9. A computer-readable storage medium, characterized in that... The computer-readable storage medium stores instructions that, when executed on a computer, perform the automatic end-effector vertical attitude calibration method as described in any one of claims 1 to 5.

10. A computer program product, comprising a computer program or instructions, characterized in that, When the computer program or the instructions are executed by the computer, they implement the automatic end-effector vertical attitude calibration method as described in any one of claims 1 to 5.

Images