Multi-joint mechanical arm and base zero position calibration method, device and readable medium

Abstract

The present invention provides a method, device and readable medium for zero position calibration of a multi-joint mechanical arm and its base. The method comprises: reading first base inertial information of a base inertial sensor to obtain the installation angle between the base installation plane and the horizontal plane, wherein the base inertial sensor is arranged in the base; obtaining the real-time postures of the second joint, the third joint and the fourth joint according to the zero position information of each of the second joint, the third joint and the fourth joint; when the installation angle is within a preset parallel range, reading the first geomagnetic data of the first geomagnetic sensor and the second geomagnetic data of the second geomagnetic sensor, and obtaining the first angle of the current first joint relative to the zero position of the first joint according to the first geomagnetic data, the second geomagnetic data and the real-time postures of the second joint, the third joint and the fourth joint, wherein the first geomagnetic sensor is arranged in the base, and the second geomagnetic sensor is arranged in the forearm between the fourth joint and the end of the multi-joint mechanical arm.

Description

Technical Field

[0001] The present invention mainly relates to the field of robots, and in particular to a method, a device and a computer-readable medium for zero-position calibration of a multi-joint robotic arm and a base thereof. Background Art

[0002] The application of serial multi-joint robotic arms in various industrial fields is becoming more and more extensive. Usually, multi-joint robotic arms need to be zero-position calibrated before leaving the factory to obtain the zero-position information of each active joint. This zero-position information plays a key role in the precise motion control of the multi-joint robotic arms in later work. In actual work, the robotic arm is fixed on the installation plane through a base. After a period of use, the zero-position information of the robotic arm will change to a certain extent. For example, the robotic arm needs to be disassembled or the motor needs to be replaced after maintenance. The zero-position information of the factory does not match the actual zero-position information, and the robotic arm needs to be recalibrated. Among the current zero-position calibration methods, some require the use of professional calibration equipment, such as laser trackers, wire encoder measuring instruments, etc., while others require the use of calibration equipment designed by the robotic arm manufacturer. No matter which method is used, it depends on additional equipment and professional engineering personnel, and usually needs to be calibrated at the customer site, which consumes manpower and material resources and has many inconveniences. Summary of the invention

[0003] The technical problem to be solved by the present invention is to provide a multi-joint robotic arm and a base zero position calibration method, device and computer readable medium for convenient, accurate and automatic base zero position calibration.

[0004] To solve the above technical problems, the present invention provides a method for calibrating the base zero position of a multi-joint robotic arm, the multi-joint robotic arm comprising a base, and a first joint, a second joint, a third joint and a fourth joint sequentially arranged on the base, comprising: reading first base inertial information of a base inertial sensor, and obtaining an installation angle between a base installation plane and a horizontal plane according to the first base inertial information, wherein the base inertial sensor is arranged in the base; obtaining zero position information of each of the second joint, the third joint and the fourth joint, and obtaining the real-time postures of the second joint, the third joint and the fourth joint according to the zero position information; and when the installation angle is within a preset parallel range, reading first geomagnetic data of a first geomagnetic sensor and second geomagnetic data of a second geomagnetic sensor, and obtaining a first angle of the current first joint relative to the zero position of the first joint according to the first geomagnetic data, the second geomagnetic data and the real-time postures of the second joint, the third joint and the fourth joint, wherein the first geomagnetic sensor is arranged in the base, and the second geomagnetic sensor is arranged in a forearm between the fourth joint and the end of the multi-joint robotic arm.

[0005] In an embodiment of the present application, it also includes: controlling the first joint to rotate the first angle, and the first joint is located at the first joint zero position.

[0006] In one embodiment of the present application, when the installation angle is within a preset parallel range, it also includes: controlling the real-time postures of the second joint and the third joint so that the rotation axis of the fourth joint is parallel to the base installation plane, and obtaining the first angle of the first joint relative to the first joint zero position according to the first geomagnetic data and the second geomagnetic data.

[0007] In one embodiment of the present application, the first angle is obtained by using the following formula:

[0008]

[0009] Among them, A1 represents the first angle, q2, q3, and q4 respectively represent the rotation angles of the current positions of the second joint, the third joint, and the fourth joint relative to the second joint zero position, the third joint zero position, and the fourth joint zero position, x, y, and z represent the coordinate values ​​of the geomagnetic data sensed by the second geomagnetic sensor in the forearm coordinate system, X and Y represent the coordinate values ​​of the geomagnetic data sensed by the first geomagnetic sensor in the base coordinate system, and atan represents the inverse tangent function.

[0010] In one embodiment of the present application, it also includes: when the installation angle is outside the preset parallel range, controlling the real-time postures of the second joint and the third joint so that the rotation axis of the fourth joint is parallel to the base installation plane, reading the second base inertia information of the base inertial sensor and the second forearm inertia information of the forearm inertial sensor, obtaining the second angle between the base installation plane and the rotation axis of the fourth joint according to the second base inertia information and the second forearm inertia information, rotating the first joint so that the second angle is zero, and the first joint is at its zero position, wherein the forearm inertial sensor is arranged in the forearm.

[0011] In one embodiment of the present application, the preset parallel range is between plus or minus 30 degrees.

[0012] In one embodiment of the present application, the multi-joint robotic arm also includes a fifth joint, and a method for obtaining the zero position information of each of the second joint, the third joint and the fourth joint includes: reading the first arm inertial information of the arm inertial sensor, and obtaining a third angle between the arm and the base mounting plane according to the first arm inertial information, wherein the arm is located between the second joint and the third joint, and the arm inertial sensor is arranged inside the arm; controlling the second joint to rotate so that the third angle is equal to 90 degrees, at which time the second joint is at its zero position; reading the first arm inertial information of the forearm inertial sensor Inertia information, obtaining a fourth angle between the forearm and the upper arm according to the first forearm inertia information, rotating the third joint so that the fourth angle is equal to 90 degrees, at which time the third joint is at its zero position, wherein the forearm inertial sensor is arranged in the forearm; controlling the fourth joint to rotate, reading the second forearm inertia information of the forearm inertial sensor, calculating the fourth deflection angle of the fourth joint according to the second forearm inertia information, controlling the fourth joint to rotate the fourth deflection angle, so that the rotation axis of the fifth joint is parallel to the base mounting plane, at which time the fourth joint is at its zero position.

[0013] In one embodiment of the present application, the fourth joint is controlled to rotate, the second forearm inertia information of the forearm inertial sensor is read, and the fourth deflection angle of the fourth joint is calculated according to the second forearm inertia information. The step of controlling the fourth joint to rotate the fourth deflection angle includes: controlling the fourth joint to rotate from a fourth initial position to a target position, the target position includes a first target position and a second target position, the second forearm inertia information includes a first target acceleration information and a second target acceleration information, and calculating the fourth deflection angle according to the first target acceleration information and the second target acceleration information.

[0014] In one embodiment of the present application, the first target position is a position where the fourth initial position is rotated 90 degrees along the first direction, and the second target position is a position where the fourth initial position is rotated 90 degrees along the second direction, and the first direction is opposite to the second direction. The step of calculating the fourth deflection angle according to the first target acceleration information and the second target acceleration information includes calculating the fourth deflection angle using the following formula:

[0015] α1 x sin(q 41 )-α1 y cos(q 41 )=-α2 x sin(q 41 )α2 y cos(q 41 )

[0016] Among them, q 41 represents the fourth deflection angle, x and y represent the X axis and Y axis of the forearm coordinate system where the forearm inertial sensor is located, respectively, α1 x , α1 y Respectively represent the X-axis acceleration and Y-axis acceleration in the first target acceleration information, α2 x , α2 y They respectively represent the X-axis acceleration and the Y-axis acceleration in the second target acceleration information.

[0017] In one embodiment of the present application, a fifth joint is further included, and the forearm inertial sensor is arranged between the fourth joint and the fifth joint.

[0018] In order to solve the above-mentioned technical problems, the present application also proposes a multi-joint robotic arm, comprising a base and a first joint, a second joint, a third joint and a fourth joint arranged in sequence on the base, characterized in that it also comprises a forearm and a controller, the forearm is located between the fourth joint and the end of the multi-joint robotic arm, a base inertial sensor and a first geomagnetic sensor are arranged in the base, the first installation planes of the base inertial sensor and the first geomagnetic sensor are parallel to the base installation plane, the base inertial sensor is used to sense the gravity direction information of the base installation plane, the first geomagnetic sensor is used to sense the azimuth relationship between the base installation plane and the earth's magnetic pole; a second geomagnetic sensor is arranged in the forearm, the second geomagnetic sensor is used to sense the azimuth relationship between the rotation axis of the fourth joint and the earth's magnetic pole; the controller is used to execute the base zero position calibration method as described above.

[0019] In one embodiment of the present application, a forearm inertial sensor is further provided in the forearm, and the forearm inertial sensor is used to sense acceleration information of its own installation position.

[0020] In one embodiment of the present application, a fifth joint is further included, and the forearm inertial sensor is located between the fourth joint and the fifth joint.

[0021] In one embodiment of the present application, the second geomagnetic sensor and the forearm inertial sensor are located at the same position.

[0022] In order to solve the above technical problems, the present application also proposes a base zero position calibration device for a multi-joint robotic arm, comprising: a memory for storing instructions executable by a processor; a processor for executing the instructions to implement the base zero position calibration method as described above.

[0023] In order to solve the above technical problems, the present application also proposes a computer-readable medium storing a computer program code, and the computer program code implements the base zero position calibration method as described above when executed by a processor.

[0024] The base zero position calibration method of the multi-joint robotic arm of the present application sets a base inertial sensor, a first geomagnetic sensor and a second geomagnetic sensor in the multi-joint robotic arm, and utilizes the rotation function of the multi-joint robotic arm itself. Without the need for complicated calculations, the base zero position of the multi-joint robotic arm can be conveniently, accurately, quickly and automatically calibrated. Furthermore, the zero position of the second joint, the third joint and the fourth joint can also be automatically calibrated without relying on additional calibration equipment and professional engineering personnel, thus saving the cost of zero position calibration. BRIEF DESCRIPTION OF THE DRAWINGS

[0025] The accompanying drawings are included to provide a further understanding of the present application. They are included and constitute a part of the present application. The accompanying drawings illustrate embodiments of the present application and together with the present specification serve to explain the principles of the present invention. In the accompanying drawings:

[0026] Figure 1 It is a schematic diagram of the three-dimensional structure of a multi-joint robotic arm according to an embodiment of the present application;

[0027] Figure 2 yes Figure 1 An exploded schematic diagram of a multi-jointed robotic arm is shown;

[0028] Figure 3 is an exemplary flow chart of a method for calibrating the base zero position of a multi-joint robotic arm according to an embodiment of the present application;

[0029] Figure 4 The system block diagram of the base zero position calibration device of the multi-joint robot arm according to one embodiment of the present invention. DETAILED DESCRIPTION

[0030] In order to more clearly illustrate the technical solutions of the embodiments of the present application, the following is a brief introduction to the drawings required for the description of the embodiments. Obviously, the drawings described below are only some examples or embodiments of the present application. For ordinary technicians in this field, the present application can also be applied to other similar scenarios based on these drawings without creative work. Unless it is obvious from the language environment or otherwise explained, the same reference numerals in the figures represent the same structure or operation.

[0031] As shown in this application and claims, unless the context clearly indicates an exception, the words "a", "an", "an" and / or "the" do not refer to the singular and may also include the plural. Generally speaking, the terms "include" and "comprise" only indicate the inclusion of the steps and elements that have been clearly identified, and these steps and elements do not constitute an exclusive list. The method or device may also include other steps or elements.

[0032] Unless otherwise specifically stated, the relative arrangement, numerical expressions and numerical values ​​of the parts and steps set forth in these embodiments do not limit the scope of the present application. At the same time, it should be understood that, for ease of description, the sizes of the various parts shown in the accompanying drawings are not drawn according to the actual proportional relationship. The technology, method and equipment known to those of ordinary skill in the relevant field may not be discussed in detail, but in appropriate cases, the technology, method and equipment should be considered as a part of the authorization specification. In all examples shown and discussed here, any specific value should be interpreted as being merely exemplary, rather than as a limitation. Therefore, other examples of exemplary embodiments may have different values. It should be noted that similar reference numerals and letters represent similar items in the following drawings, so that once a certain item is defined in an accompanying drawing, it does not need to be further discussed in subsequent drawings.

[0033] In the description of the present application, it should be understood that the directions or positional relationships indicated by directional words such as "front, back, up, down, left, right", "lateral, vertical, perpendicular, horizontal" and "top, bottom" are usually based on the directions or positional relationships shown in the drawings, and are only for the convenience of describing the present application and simplifying the description. Unless otherwise specified, these directional words do not indicate or imply that the device or element referred to must have a specific direction or be constructed and operated in a specific direction, and therefore cannot be understood as limiting the scope of protection of the present application; the directional words "inside and outside" refer to the inside and outside relative to the contours of each component itself.

[0034] For ease of description, spatially relative terms such as "above", "above", "on the upper surface of", "above", etc. may be used here to describe the spatial positional relationship between a device or feature and other devices or features as shown in the figure. It should be understood that spatially relative terms are intended to include different orientations of the device in use or operation in addition to the orientation described in the figure. For example, if the device in the accompanying drawings is inverted, the device described as "above other devices or structures" or "above other devices or structures" will be positioned as "below other devices or structures" or "below other devices or structures". Thus, the exemplary term "above" can include both "above" and "below". The device can also be positioned in other different ways (rotated 90 degrees or in other orientations), and the spatially relative descriptions used here are interpreted accordingly.

[0035] In addition, it should be noted that the use of words such as "first" and "second" to define components is only for the convenience of distinguishing the corresponding components. If not otherwise stated, the above words have no special meaning and cannot be understood as limiting the scope of protection of this application. In addition, although the terms used in this application are selected from well-known and commonly used terms, some terms mentioned in the specification of this application may be selected by the applicant at his or her discretion, and their detailed meanings are explained in the relevant parts of the description of this article. In addition, it is required to understand this application not only by the actual terms used, but also by the meaning implied by each term.

[0036] Flowcharts are used in the present application to illustrate the operations performed by the system according to the embodiments of the present application. It should be understood that the preceding or following operations are not necessarily performed accurately in order. On the contrary, various steps may be processed in reverse order or simultaneously. At the same time, other operations may be added to these processes, or one or more operations may be removed from these processes.

[0037] The multi-joint robotic arms involved in this application include but are not limited to serial multi-joint robotic arms, including but not limited to industrial robotic arms, collaborative robotic arms, etc. This application does not limit the number of joints of the multi-joint robotic arms. The number of joints of the multi-joint robotic arms of this application is at least 4. This specification will be described by taking a 6-joint robotic arm as an example, and is not used to limit the specific number of joints.

[0038] Figure 1 Schematic diagram of the three-dimensional structure of a multi-joint robotic arm according to an embodiment of the present application. Figure 1 As shown, the multi-joint robot arm 100 of this embodiment includes 6 joints, which are respectively labeled as the first joint J1, the second joint J2, the third joint J3, the fourth joint J4, the fifth joint J5 and the sixth joint J6. Each joint can be moved along Figure 1 The multi-joint robot arm 100 rotates in the direction of the curved arrow shown in the figure, which includes a positive direction (+) and a negative direction (-). Assuming that the multi-joint robot arm 100 is set on a horizontal plane, J1 moves in the horizontal plane.

[0039] Figure 2 yes Figure 1 The exploded diagram of the multi-joint robotic arm is shown in Figure 1. Figure 2 The multi-joint robot arm includes a base 110, a rotating base 120, an upper arm 130, an elbow joint base 140, a lower arm 150 and a wrist body 160 from bottom to top. Figure 2 and Figure 1 The multi-joint robot arm 100 is mounted on a base mounting plane through the base 110, and the base mounting plane is not necessarily a horizontal plane. As required, the base 110 can be mounted at any position and on any plane.

[0040] like Figure 2 The base 110 is fixedly arranged, and the first joint J1 is located between the base 110 and the swivel 120 , that is, the connection between the base 110 and the swivel 120 . The swivel 120 is rotatably connected to the base 110 via the first joint J1 . Figure 2 The coordinate systems of the base and some joints are also shown, and the calculations during the movement of the robot arm require the use of various coordinate systems. Among them, the base coordinate system O0X0Y0Z0 is included, and Z0 represents the rotation axis of the base 110. Z1 in the first joint coordinate system O1X1Y1Z1 is the rotation axis of the first joint J1, and the rotation axis Z1 of the first joint J1 is perpendicular to the installation plane, that is, the swivel seat 120 can rotate in a plane parallel to the installation plane.

[0041] Combination Figure 1 and Figure 2 The second joint J2 is located at the connection between the swivel seat 120 and the upper arm 130. Z2 in the second joint coordinate system O2X2Y2Z2 is the rotation axis of the second joint J2, and the rotation axis Z2 of the second joint J2 is perpendicular to the mounting plane. The third joint J3 is located at the connection between the upper arm 130 and the elbow joint seat 140. Z3 in the third joint coordinate system O3X3Y3Z3 is the rotation axis of the third joint J3, and the rotation axis Z3 of the third joint J3 is parallel to the rotation axis Z2 of the second joint J2. The fourth joint J4 is located at the connection between the elbow joint seat 140 and the forearm 150. Z4 in the fourth joint coordinate system O4X4Y4Z4 is the rotation axis of the fourth joint J4, and the rotation axis Z4 of the fourth joint J4 is related to the angle of the elbow joint seat 140. The fifth joint J5 is located at the connection between the forearm 150 and the wrist body 160. The sixth joint J6 is located at the end of the wrist body 160. The fourth joint coordinate system O4X4Y4Z4 is used as the forearm coordinate system.

[0042] refer to Figure 1 The base 110 of the multi-joint robotic arm of the present application is provided with a base inertial sensor 111 and a first geomagnetic sensor 112 , and the forearm 150 is provided with a second geomagnetic sensor 152 . Figure 1 The illustration is for illustration only and is not intended to limit the specific positions and shapes of the base inertial sensor 111 , the first geomagnetic sensor 112 , and the second geomagnetic sensor 152 .

[0043] In some embodiments, the installation plane of the base inertial sensor 111 and the first geomagnetic sensor 112 in the base 110 is parallel to the base installation plane of the base 110. The inertial sensor can generally measure the spatial acceleration information of an object when it moves in space. Since the base 110 is relatively fixed after installation, the base inertial sensor 111 can obtain the gravity direction information of the base 110, and the position relationship or direction relationship between the base installation plane and the horizontal plane can be obtained according to the gravity direction information.

[0044] Geomagnetic sensors can sense the direction of the Earth's magnetic poles in real time, that is, the direction of the north and south poles in the Earth's magnetic field, and are usually used in compasses. One type of geomagnetic sensor is an anisotropic magnetoresistive sensor, which can be made of a thin film alloy. It uses the basic principle that the resistance characteristics of a current-carrying magnetic material will change when an external magnetic field exists to measure magnetic field changes. When the geomagnetic sensor is working, when there is an external magnetic field, the thin film alloy will generate an internal magnetization vector parallel to the direction of the current, and the direction of the external magnetic field can be known based on the magnetization vector.

[0045] The first geomagnetic sensor 112 and the second geomagnetic sensor 152 of the present application can sense the azimuth relationship between the installation plane and the earth's magnetic poles based on their own installation planes. Specifically, the first geomagnetic sensor 112 can sense the azimuth relationship between the base coordinate system O0X0Y0Z0 and the earth's magnetic poles, and then sense the azimuth relationship between the base installation plane and the earth's magnetic poles; the second geomagnetic sensor 152 can sense the azimuth relationship between the forearm coordinate system O4 X4Y4Z4 and the earth's magnetic poles, and then sense the azimuth relationship between the rotation axis of the fourth joint J4 and the earth's magnetic poles.

[0046] In some embodiments, the base inertial sensor 111 and the first geomagnetic sensor 112 are integrated into the same device, such as a chip. In this embodiment, the base inertial sensor 111 and the first geomagnetic sensor 112 have the same installation plane and are parallel to the base installation plane.

[0047] Figure 3 is an exemplary flow chart of a method for calibrating the base zero position of a multi-joint robotic arm according to an embodiment of the present application. Figure 1 and Figure 2 The multi-joint robot shown in the figure is used to implement Figure 3 As shown, the base zero position calibration method of this embodiment includes the following steps:

[0048] Step S310: reading first base inertia information of the base inertia sensor, and obtaining an installation angle between the base installation plane and the horizontal plane according to the first base inertia information, wherein the base inertia sensor is arranged in the base;

[0049] Step S320: obtaining zero position information of the second joint, the third joint and the fourth joint, and obtaining the real-time postures of the second joint, the third joint and the fourth joint according to the zero position information; and

[0050] Step S330: When the installation angle is within a preset parallel range, read the first geomagnetic data of the first geomagnetic sensor, the second geomagnetic data of the second geomagnetic sensor, and the real-time postures of the second joint, the third joint, and the fourth joint to obtain the first angle of the first joint relative to the zero position of the first joint, wherein the first geomagnetic sensor is arranged in the base, and the second geomagnetic sensor is arranged in the forearm between the fourth joint and the end of the multi-joint robotic arm.

[0051] The above steps S310 - S330 are described in detail below with reference to the accompanying drawings.

[0052] In some embodiments, before step S310, each joint is in an arbitrary position.

[0053] In step S310 , the first base inertial information of the base inertial sensor 111 includes gravity direction information of the base 110 , and the installation angle A_setup between the base installation plane and the horizontal plane can be calculated based on the gravity direction information.

[0054] In step S320, the present application does not limit the steps of how to obtain the zero position information of the second joint J2, the third joint J3 and the fourth joint J4. Since the focus of the present application is on how to calibrate the zero position of the base, it can also be assumed that the zero position information of the second joint J2, the third joint J3 and the fourth joint J4 is known, so as to use the zero position information in subsequent steps to obtain and control the real-time posture of the second joint J2, the third joint J3 and the fourth joint J4.

[0055] In some embodiments, a traditional calibration method may be used, such as using a professional calibration device to calibrate the zero position information of each of the second joint J2 , the third joint J3 , and the fourth joint J4 .

[0056] In some embodiments, the multi-joint robot arm 100 of the present application can automatically obtain the zero position information of the second joint J2, the third joint J3 and the fourth joint J4 by using the following steps:

[0057] Step S321: reading first boom inertia information of the boom inertia sensor, and obtaining a third angle between the boom and the base mounting plane according to the first boom inertia information, wherein the boom is located between the second joint and the third joint, and the boom inertia sensor is arranged inside the boom;

[0058] Step S322: controlling the second joint to rotate so that the third angle is equal to 90 degrees, at which time the second joint is at its zero position;

[0059] Step S323: reading the first forearm inertial information of the forearm inertial sensor, obtaining the fourth angle between the forearm and the upper arm according to the first forearm inertial information, rotating the third joint so that the rotation axis of the fourth joint is perpendicular to the upper arm, and the third joint is at its zero position, wherein the forearm inertial sensor is arranged in the forearm;

[0060] Step S324: control the real-time posture of the fourth joint, read the second forearm inertia information of the forearm inertial sensor, calculate the fourth deflection angle of the fourth joint according to the second forearm inertia information, control the fourth joint to rotate the fourth deflection angle, make the rotation axis of the fifth joint parallel to the base mounting plane, and the fourth joint is at its zero position.

[0061] The above steps S321 to S324 are described below respectively.

[0062] In step S321, Figure 1 As shown, a boom inertial sensor 131 is provided in the boom 130, which can be used to obtain first boom inertial information of the boom 130. The first boom inertial information includes three-dimensional acceleration data of the boom 130. It should be noted that when the boom 130 is stationary, the first boom inertial information at least includes gravity direction information of the boom inertial sensor 131, which can be used to characterize the gravity direction of the boom 130. Since the first base inertial information has been obtained in step S310, the third angle A3 between the boom 130 and the base installation plane can be obtained according to the first base inertial information and the first boom inertial information.

[0063] In step S322, the second joint J2 is controlled to rotate so that the third angle A3 is equal to 90 degrees, that is, the upper arm 130 is perpendicular to the base installation plane, and the second joint J2 is at its zero position. Specifically, the motor corresponding to the second joint J2 can be controlled to rotate and stop rotating when the third angle A3 is equal to 90 degrees.

[0064] In an embodiment of the present application, the encoder information of the motors of each joint of the multi-joint robotic arm is available, and the rotation information of the joints can be obtained according to the encoder information. The multi-joint robotic arm of the present application may also include a controller for interacting with each joint for data and commands. The controller may be integrated in the body of the multi-joint robotic arm or may be an independent controller. Regardless of the setting, the controller may establish a communication connection with the motors, encoders, and various sensors in the multi-joint robotic arm through a bus. The communication mode may be wired or wireless, and the present application does not limit this.

[0065] In some embodiments, after the zero position of the second joint J2 is obtained in step S322, the controller establishes and stores a correspondence between the current motor encoder information of the second joint J2 and the zero position of the second joint J2.

[0066] In step S323, if Figure 1 As shown, an arm inertial sensor 151 is also provided in the arm 150. The arm inertial sensor 151 is used to sense the acceleration information of its own installation position. When the arm 150 is stationary, the arm inertial sensor 151 is at least used to sense the gravity direction information of the arm 150, which can be used to characterize the gravity direction of the arm 150.

[0067] In some embodiments, the forearm inertial sensor 151 and the second geomagnetic sensor 152 have the same mounting plane, and the two can be separately arranged in the forearm 150 .

[0068] In some embodiments, the forearm inertial sensor 151 and the second geomagnetic sensor 152 are integrated into the same device, such as a chip, and the two naturally have the same installation plane.

[0069] refer to Figure 1 and Figure 2 In step S323, the fourth angle A4 between the upper arm 130 and the lower arm 150 can be obtained according to the inertia information of the first upper arm and the inertia information of the first lower arm. At this time, the third joint J3 is controlled to rotate so that the rotation axis Z4 of the fourth joint J4 is perpendicular to the upper arm 130. At this time, the third joint J3 is located at its zero position. Specifically, the motor corresponding to the third joint J3 can be controlled to rotate and the rotation can be stopped when the fourth angle A4 is equal to 90 degrees.

[0070] In some embodiments, after the zero position of the third joint J3 is obtained in step S323, the controller establishes and stores a correspondence between the current motor encoder information of the third joint J3 and the zero position of the third joint J3.

[0071] In the above steps S321 to S323, step S323 may be performed first, and then step S321 and step S322.

[0072] In step S324, the fourth joint J4 can be rotated by controlling the motor corresponding to the fourth joint J4 to rotate, and the second forearm inertia information of the forearm inertial sensor 151 is read at the same time. In this process, the fourth deflection angle of the fourth joint J4 is calculated according to the second forearm inertia information.

[0073] In some embodiments, step S324 specifically includes the following steps:

[0074] Step S410: control the fourth joint to rotate from a fourth initial position to a target position, the target position includes a first target position and a second target position, the second forearm inertia information includes first target acceleration information and second target acceleration information, and calculate the fourth deflection angle according to the first target acceleration information and the second target acceleration information.

[0075] The fourth initial position in step S410 is the position of the fourth joint J4 after executing steps S321 to S323.

[0076] In some embodiments, the first target position is a position where the fourth initial position is rotated 90 degrees along the first direction, and the second target position is a position where the fourth initial position is rotated 90 degrees along the second direction, the first direction and the second direction are opposite, and the step of calculating the fourth deflection angle according to the first target acceleration information and the second target acceleration information includes calculating the fourth deflection angle using the following formula:

[0077] α1 x sin(q 41 )-α1 y cos(q 41 )=-α2 x sin(q 41 )α2 y cos(q 41 ) (1)

[0078] Among them, q 41 represents the fourth deflection angle, x and y represent the X axis and Y axis of the forearm coordinate system of the forearm inertial sensor 151, respectively, and α1 x , α1 y Respectively represent the X-axis acceleration and Y-axis acceleration in the first target acceleration information, α2 x , α2 y They represent the X-axis acceleration and the Y-axis acceleration in the second target acceleration information respectively. 41 Indicates the current zero position deviation of the fourth joint J4. The forearm coordinate system is O4. Figure 2 shown.

[0079] In the above equation (1), only the fourth deflection angle q 41 is an unknown variable. By substituting the known quantity, we can solve for the fourth deflection angle q 41 This application does not limit the specific method of solving the equation.

[0080] like Figure 1 and Figure 2 As shown, in step S410, the fourth joint J4 is controlled to rotate along its rotation axis Z4, then the first direction can be Figure 1 The first direction is the positive direction (+) or negative direction (-) of the fourth joint J4, and the second direction is the negative direction (-) or positive direction (+) of the fourth joint J4 accordingly.

[0081] After step S324, the fourth joint J4 is located at its zero position. In some embodiments, the controller establishes and stores a corresponding relationship between the current motor encoder information of the fourth joint J4 and the zero position of the fourth joint J4.

[0082] The above steps S321 to S324 are Figure 3 A specific implementation of step S320 in .

[0083] exist Figure 1 In the illustrated embodiment, each joint is approximately in its zero position. In some embodiments, before executing the above steps S321 to S324, each joint can be controlled to be approximately in its zero position. For example, each joint is manually observed to be in its zero position. The zero position determined in this way has a certain error, but compared to each joint being in an arbitrary position, each joint is adjusted to approximately zero position before adopting the base zero position calibration method of the present application, and steps S321 to S324 only need to be fine-tuned to make the second joint J2, the third joint J3 and the fourth joint J4 reach their respective zero positions, thereby shortening the execution time of steps S321 to S324 and speeding up the zero position calibration as a whole.

[0084] In step S330, since the zero position information of the second joint J2, the third joint J3 and the fourth joint J4 has been obtained, the real-time postures of the second joint J2, the third joint J3 and the fourth joint J4 can be controlled according to the zero position information. Figure 1 In step S330, when the installation angle A_setup is within the preset parallel range, the first geomagnetic data of the first geomagnetic sensor 112 and the second geomagnetic data of the second geomagnetic sensor 152 are read, and according to the first geomagnetic data, the second geomagnetic data and the real-time postures of the second joint J2, the third joint J3 and the fourth joint J4, the first angle A1 of the current first joint J1 relative to the first joint zero position can be obtained.

[0085] In some embodiments, the first angle A1 can be obtained by using the following formula (2):

[0086]

[0087] Wherein, A1 represents the first angle, q2, q3, q4 represent the rotation angles of the current positions of the second joint J2, the third joint J3, and the fourth joint J4 relative to the second joint zero position, the third joint zero position, and the fourth joint zero position, respectively, x, y, z represent the coordinate values ​​of the geomagnetic data sensed by the second geomagnetic sensor 152 in the forearm coordinate system O4X4Y4Z4, X, Y represent the coordinate values ​​of the geomagnetic data sensed by the first geomagnetic sensor 112 in the base coordinate system O0X0Y0Z0, and atan represents the inverse tangent function arctan. According to these embodiments, there is no restriction on the current positions of the second joint J2, the third joint J3, and the fourth joint J4, and they can be at any position.

[0088] In some embodiments, after obtaining the first angle A1 in step S330, the process further includes controlling the first joint J1 to rotate the first angle A1, and at this time the first joint J1 is located at the first joint zero position.

[0089] In some embodiments, in step S330, when the installation angle A_setup is within the preset parallel range, the process of obtaining the first angle A1 can be simplified by the following steps: control the real-time posture of the second joint J2 and the third joint J3, so that the rotation axis Z4 of the fourth joint J4 is parallel to the base installation plane, at this time, read the first geomagnetic data of the first geomagnetic sensor 112 and the second geomagnetic data of the second geomagnetic sensor 152, obtain the first angle A1 of the first joint J1 relative to the first joint zero position according to the first geomagnetic data and the second geomagnetic data, rotate the first joint J1 so that the first angle A1 is zero, and at this time the first joint A1 is at its zero position. In this embodiment, making the rotation axis Z4 of the fourth joint J4 parallel to the base installation plane is calculated based on the zero position information of each joint. In practice, due to the inaccurate zero position of the base, the rotation axis Z4 of the fourth joint J4 and the base installation plane are not parallel, but have an angle, which is the first angle A1, which can be obtained by the rotation angle q4 of the current position of the fourth joint J4 relative to the fourth joint zero position and the first geomagnetic data and the second geomagnetic data. Combined with the above formula (2), the rotation axis Z4 of the fourth joint J4 is made parallel to the base installation plane, so that q2+q3 in formula (2)=0, and the above formula (2) is simplified to the following formula (3):

[0090]

[0091] In some embodiments, the first geomagnetic sensor 112 obtains the azimuth relationship between the base installation plane and the North Pole of the Earth's magnetic poles, and the second geomagnetic sensor 152 obtains the azimuth relationship between the rotation axis of the fourth joint J4 and the North Pole of the Earth's magnetic poles. Since the direction of the North Pole is fixed, it serves as a unified reference.

[0092] In step S330, the positional relationship between the base installation plane and the horizontal plane is determined by setting a preset parallel range. The installation angle A_setup is within the preset parallel range, indicating that the base installation plane is substantially parallel to the horizontal plane.

[0093] In some embodiments, the preset parallel range is between plus or minus 30 degrees. In other embodiments, the preset parallel range can be smaller, such as between plus or minus 10 degrees, between plus or minus 5 degrees, etc.

[0094] In some embodiments, when the installation angle A_setup is outside the preset parallel range, it means that the angle between the base installation plane and the horizontal plane is too large. At this time, the real-time postures of the second joint J2 and the third joint J3 are controlled to make the rotation axis of the fourth joint J4 parallel to the base installation plane, and the second base inertia information of the base inertial sensor 111 and the second forearm inertia information of the forearm inertial sensor 151 are read. The second angle A2 between the base installation plane and the rotation axis of the fourth joint J4 is obtained according to the second base inertia information and the second forearm inertia information, and the first joint J1 is rotated so that the second angle A2 is zero. At this time, the first joint J1 is at its zero position, wherein the forearm inertial sensor 151 is arranged in the forearm 150.

[0095] After step S330, the first joint J1 is at its zero position, that is, the base of the multi-joint robot 100 is at the zero position. In some embodiments, the controller establishes and stores the corresponding relationship between the current motor encoder information of the first joint J1 and the zero position of the first joint J1.

[0096] According to the above steps S310-S330, by setting the base inertial sensor 111, the first geomagnetic sensor 112 and the second geomagnetic sensor 152, and utilizing the functions of the multi-joint robotic arm itself, the base zero position of the multi-joint robotic arm can be conveniently, accurately, quickly and automatically calibrated without complicated calculations. Furthermore, the zero positions of the second joint, the third joint and the fourth joint can also be automatically calibrated without relying on additional calibration equipment and professional engineering personnel, thus saving the cost of zero position calibration.

[0097] like Figure 1 As shown, the present application also provides a multi-joint robot arm 100, including a base 110 and a first joint J1, a second joint J2, a third joint J3 and a fourth joint J4 arranged in sequence on the base 110, the multi-joint robot arm 100 also includes a small arm 150 and a controller, and the small arm 150 is located between the fourth joint J4 and the end of the multi-joint robot arm 100. Figure 2 The end of the multi-joint robot arm 100 may be the end of the wrist body 160. Figure 1A base inertial sensor 111 and a first geomagnetic sensor 112 are provided in the base 110, and the first installation planes of the base inertial sensor 111 and the first geomagnetic sensor 112 are parallel to the base installation plane, and the base inertial sensor 111 is used to sense the gravity direction information of the base installation plane, and the first geomagnetic sensor 112 is used to sense the azimuth relationship between the base installation plane and the earth's magnetic poles, that is, to sense the azimuth relationship between the base coordinate system O0X0Y0Z0 and the earth's magnetic poles; a second geomagnetic sensor 152 is provided in the forearm 150, and the second geomagnetic sensor 152 is used to sense the azimuth relationship between the rotation axis of the fourth joint J4 and the earth's magnetic poles, that is, to sense the azimuth relationship between the forearm coordinate system O4 X4Y4Z4 and the earth's magnetic poles; the controller is used to execute the base zero position calibration method described above. The above description can be used to illustrate the multi-joint robotic arm of the present application.

[0098] like Figure 1 In some embodiments, a forearm inertial sensor 151 is also provided in the forearm 150, and the forearm inertial sensor 151 is used to sense the acceleration information of its own installation position.

[0099] like Figure 1 In some embodiments, the multi-joint robotic arm 100 further includes a fifth joint J5, and the forearm inertial sensor 151 is located between the fourth joint J4 and the fifth joint J5.

[0100] In some embodiments, the second geomagnetic sensor 152 and the forearm inertial sensor 151 are located at the same position.

[0101] The present application also includes a base zero position calibration device for a multi-joint robotic arm, comprising a memory and a processor, wherein the memory is used to store instructions executable by the processor; the processor is used to execute the instructions to implement the base zero position calibration method for the multi-joint robotic arm described above.

[0102] Figure 4 1 is a system block diagram of a base zero position calibration device for a multi-joint mechanical arm according to an embodiment of the present invention. Figure 4As shown, the base zero position calibration device 400 may include an internal communication bus 401, a processor 402, a read-only memory (ROM) 403, a random access memory (RAM) 404, and a communication port 405. When applied on a personal computer, the base zero position calibration device 400 may also include a hard disk 406. The internal communication bus 401 can realize data communication between the components of the base zero position calibration device 400. The processor 402 can make judgments and issue prompts. In some embodiments, the processor 402 can be composed of one or more processors. The communication port 405 can realize data communication between the base zero position calibration device 400 and the outside. In some embodiments, the base zero position calibration device 400 can send and receive information and data from the network through the communication port 405. The base zero calibration device 400 may also include different forms of program storage units and data storage units, such as a hard disk 406, a read-only memory (ROM) 403 and a random access memory (RAM) 404, which can store various data files used for computer processing and / or communication, and possible program instructions executed by the processor 402. The processor executes these instructions to implement the main part of the method. The results of the processor processing are transmitted to the user device through the communication port and displayed on the user interface.

[0103] The above-mentioned base zero position calibration method can be implemented as a computer program, stored in the hard disk 406, and loaded into the processor 402 for execution to implement the base zero position calibration method of the present application.

[0104] The present invention also includes a computer-readable medium storing a computer program code, which, when executed by a processor, implements the base zero position calibration method described above.

[0105] When the base zero calibration method of the multi-joint mechanical arm is implemented as a computer program, it can also be stored in a computer-readable storage medium as a product. For example, a computer-readable storage medium may include, but is not limited to, a magnetic storage device (e.g., a hard disk, a floppy disk, a magnetic strip), an optical disk (e.g., a compact disk (CD), a digital versatile disk (DVD)), a smart card, and a flash memory device (e.g., an electrically erasable programmable read-only memory (EPROM), a card, a stick, a key drive). In addition, the various storage media described herein can represent one or more devices and / or other machine-readable media for storing information. The term "machine-readable medium" may include, but is not limited to, wireless channels and various other media (and / or storage media) that can store, contain and / or carry code and / or instructions and / or data.

[0106] It should be understood that the embodiments described above are only illustrative. The embodiments described herein may be implemented in hardware, software, firmware, middleware, microcode, or any combination thereof. For hardware implementation, the processor may be implemented in one or more application specific integrated circuits (ASICs), digital signal processors (DSPs), digital signal processing devices (DSPDs), programmable logic devices (PLDs), field programmable gate arrays (FPGAs), processors, controllers, microcontrollers, microprocessors, and / or other electronic units designed to perform the functions described herein, or a combination thereof.

[0107] Some aspects of the present application may be performed entirely by hardware, entirely by software (including firmware, resident software, microcode, etc.), or by a combination of hardware and software. The above hardware or software may be referred to as "data blocks", "modules", "engines", "units", "components" or "systems". The processor may be one or more application specific integrated circuits (ASICs), digital signal processors (DSPs), digital signal processing devices (DAPDs), programmable logic devices (PLDs), field programmable gate arrays (FPGAs), processors, controllers, microcontrollers, microprocessors, or combinations thereof. In addition, various aspects of the present application may be expressed as computer products located in one or more computer-readable media, which include computer-readable program codes. For example, computer-readable media may include, but are not limited to, magnetic storage devices (e.g., hard disks, floppy disks, tapes ...), optical disks (e.g., compact disks CDs, digital versatile disks DVDs ...), smart cards, and flash memory devices (e.g., cards, sticks, key drives ...).

[0108] A computer-readable medium may include a propagated data signal containing computer program code, such as in baseband or as part of a carrier wave. The propagated signal may have a variety of manifestations, including electromagnetic, optical, etc., or a suitable combination. A computer-readable medium may be any computer-readable medium other than a computer-readable storage medium, which may be connected to an instruction execution system, device or apparatus to communicate, propagate or transmit a program for use. The program code on the computer-readable medium may be propagated via any suitable medium, including radio, cable, fiber optic cable, radio frequency signal, or similar medium, or any combination of the above mediums.

[0109] The basic concepts have been described above. Obviously, for those skilled in the art, the above invention disclosure is only used as an example and does not constitute a limitation of the present application. Although not explicitly stated herein, those skilled in the art may make various modifications, improvements and amendments to the present application. Such modifications, improvements and amendments are suggested in the present application, so such modifications, improvements and amendments still belong to the spirit and scope of the exemplary embodiments of the present application.

[0110] At the same time, the present application uses specific words to describe the embodiments of the present application. For example, "one embodiment", "an embodiment", and / or "some embodiments" refer to a certain feature, structure or characteristic related to at least one embodiment of the present application. Therefore, it should be emphasized and noted that "one embodiment" or "an embodiment" or "an alternative embodiment" mentioned twice or more in different positions in this specification does not necessarily refer to the same embodiment. In addition, some features, structures or characteristics in one or more embodiments of the present application can be appropriately combined.

[0111] In some embodiments, numbers describing the number of components and attributes are used. It should be understood that such numbers used in the description of the embodiments are modified by the modifiers "about", "approximately" or "substantially" in some examples. Unless otherwise specified, "about", "approximately" or "substantially" indicate that the numbers are allowed to vary by ±20%. Accordingly, in some embodiments, the numerical parameters used in the specification and claims are approximate values, which may change according to the required features of individual embodiments. In some embodiments, the numerical parameters should take into account the specified significant digits and adopt the general method of retaining digits. Although the numerical domains and parameters used to confirm the breadth of their range in some embodiments of the present application are approximate values, in specific embodiments, the setting of such numerical values ​​is as accurate as possible within the feasible range.

Claims

1. A method for calibrating the base zero position of a multi-joint robotic arm, the multi-joint robotic arm comprising a base, and a first joint, a second joint, a third joint and a fourth joint sequentially arranged on the base, characterized in that: include: Reading first base inertia information of a base inertia sensor, and obtaining an installation angle between a base installation plane and a horizontal plane according to the first base inertia information, wherein the base inertia sensor is arranged in the base; Obtaining zero position information of each of the second joint, the third joint and the fourth joint, and obtaining real-time postures of the second joint, the third joint and the fourth joint according to the zero position information; and When the installation angle is within a preset parallel range, the first geomagnetic data of the first geomagnetic sensor and the second geomagnetic data of the second geomagnetic sensor are read, and the first angle of the first joint relative to the zero position of the first joint is obtained according to the first geomagnetic data, the second geomagnetic data and the real-time postures of the second joint, the third joint and the fourth joint, wherein the first geomagnetic sensor is arranged in the base, and the second geomagnetic sensor is arranged in the forearm between the fourth joint and the end of the multi-joint robotic arm.

2. The base zero position calibration method according to claim 1, characterized in that: Also includes: The first joint is controlled to rotate to the first angle, and the first joint is located at the first joint zero position.

3. The base zero position calibration method according to claim 1, characterized in that: When the installation angle is within a preset parallel range, it also includes: controlling the real-time postures of the second joint and the third joint so that the rotation axis of the fourth joint is parallel to the base installation plane, and obtaining the first angle of the first joint relative to the first joint zero position according to the first geomagnetic data and the second geomagnetic data.

4. The base zero position calibration method according to claim 1, characterized in that: The first angle is obtained using the following formula: Among them, A1 represents the first angle, q2, q3, and q4 respectively represent the rotation angles of the current positions of the second joint, the third joint, and the fourth joint relative to the second joint zero position, the third joint zero position, and the fourth joint zero position, x, y, and z represent the coordinate values ​​of the geomagnetic data sensed by the second geomagnetic sensor in the forearm coordinate system, X and Y represent the coordinate values ​​of the geomagnetic data sensed by the first geomagnetic sensor in the base coordinate system, and atan represents the inverse tangent function.

5. The base zero position calibration method according to claim 1, characterized in that: Also includes: When the installation angle is outside the preset parallel range, the real-time postures of the second joint and the third joint are controlled so that the rotation axis of the fourth joint is parallel to the base installation plane, the second base inertia information of the base inertial sensor and the second forearm inertia information of the forearm inertial sensor are read, and the second angle between the base installation plane and the rotation axis of the fourth joint is obtained according to the second base inertia information and the second forearm inertia information, and the first joint is rotated so that the second angle is zero, and the first joint is at its zero position at this time, wherein the forearm inertial sensor is arranged in the forearm.

6. The base zero position calibration method according to claim 1, characterized in that: The preset parallel range is between plus or minus 30 degrees.

7. The base zero position calibration method according to claim 1, characterized in that: The multi-joint mechanical arm further includes a fifth joint, and the method for obtaining the zero position information of each of the second joint, the third joint and the fourth joint includes: Reading first boom inertia information of the boom inertia sensor, and obtaining a third angle between the boom and the base mounting plane according to the first boom inertia information, wherein the boom is located between the second joint and the third joint, and the boom inertia sensor is arranged inside the boom; Controlling the second joint to rotate so that the third angle is equal to 90 degrees, at which time the second joint is at its zero position; Reading first forearm inertial information of a forearm inertial sensor, obtaining a fourth angle between the forearm and the upper arm according to the first forearm inertial information, rotating the third joint so that the fourth angle is equal to 90 degrees, at which time the third joint is at its zero position, wherein the forearm inertial sensor is arranged in the forearm; Control the rotation of the fourth joint, read the second forearm inertia information of the forearm inertial sensor, calculate the fourth deflection angle of the fourth joint according to the second forearm inertia information, control the fourth joint to rotate the fourth deflection angle, so that the rotation axis of the fifth joint is parallel to the base mounting plane, and the fourth joint is at its zero position.

8. The base zero position calibration method according to claim 7, characterized in that: Control the fourth joint to rotate, read the second forearm inertia information of the forearm inertial sensor, calculate the fourth deflection angle of the fourth joint according to the second forearm inertia information, and the step of controlling the fourth joint to rotate the fourth deflection angle includes: controlling the fourth joint to rotate from a fourth initial position to a target position, the target position includes a first target position and a second target position, the second forearm inertia information includes first target acceleration information and second target acceleration information, and calculating the fourth deflection angle according to the first target acceleration information and the second target acceleration information.

9. The base zero position calibration method according to claim 8, characterized in that: The first target position is a position where the fourth initial position is rotated 90 degrees along the first direction, and the second target position is a position where the fourth initial position is rotated 90 degrees along the second direction, and the first direction is opposite to the second direction. The step of calculating the fourth deflection angle according to the first target acceleration information and the second target acceleration information includes calculating the fourth deflection angle using the following formula: α1 x sin(q 41 )-α1 y cos(q 41 )=-α2 x sin(q 41 )+α2 y cos(q 41 ) Among them, q 41 represents the fourth deflection angle, x and y represent the X axis and Y axis of the forearm coordinate system where the forearm inertial sensor is located, respectively, α1 x , α1 y Respectively represent the X-axis acceleration and Y-axis acceleration in the first target acceleration information, α2 x , α2 y They respectively represent the X-axis acceleration and the Y-axis acceleration in the second target acceleration information.

10. The base zero position calibration method according to claim 5 or 7, characterized in that: It also includes a fifth joint, and the forearm inertial sensor is arranged between the fourth joint and the fifth joint.

11. A multi-joint robotic arm, comprising a base and a first joint, a second joint, a third joint and a fourth joint sequentially arranged on the base, characterized in that: It also includes a forearm and a controller, the forearm is located between the fourth joint and the end of the multi-joint robotic arm, the base is provided with a base inertial sensor and a first geomagnetic sensor, the first installation planes of the base inertial sensor and the first geomagnetic sensor are parallel to the base installation plane, the base inertial sensor is used to sense the gravity direction information of the base installation plane, the first geomagnetic sensor is used to sense the azimuth relationship between the base installation plane and the earth's magnetic pole; the forearm is provided with a second geomagnetic sensor, the second geomagnetic sensor is used to sense the azimuth relationship between the rotation axis of the fourth joint and the earth's magnetic pole; the controller is used to execute the base zero position calibration method as described in any one of claims 1-10.

12. The multi-joint mechanical arm according to claim 11, characterized in that: The forearm is also provided with a forearm inertial sensor, and the forearm inertial sensor is used to sense acceleration information of its own installation position.

13. The multi-joint robotic arm according to claim 12, characterized in that: It also includes a fifth joint, and the forearm inertial sensor is located between the fourth joint and the fifth joint.

14. The multi-joint mechanical arm according to claim 12, characterized in that: The second geomagnetic sensor and the forearm inertial sensor are located at the same position.

15. A base zero position calibration device for a multi-joint robotic arm, comprising: a memory for storing instructions executable by a processor; A processor, used to execute the instructions to implement the base zero position calibration method as described in any one of claims 1-10.

16. A computer-readable medium storing computer program code, wherein the computer program code, when executed by a processor, implements the base zero position calibration method according to any one of claims 1 to 10.

Images